Mirrors_Framework/g2
1
0
Fork 0
mirror of https://github.com/synthetos/g2.git synced 2026-02-06 11:11:57 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Highly experimental segment linear-velocity-interpolation

Browse Source
This commit is contained in:
Rob Giseburt
2017-07-15 22:35:45 -05:00
parent 5609c8740d
commit 2de08704cd
4 changed files with 274 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files

153
g2core/plan_exec.cpp
View File

@@ -50,17 +50,17 @@ static void _init_forward_diffs(float v_0, float v_1);
* mp_forward_plan() - plan commands and moves ahead of exec; call ramping for moves
*
**** WARNING ****
* This function should NOT be called directly!
* Instead call st_request_forward_plan(), which mediates access.
* This function should NOT be called directly!
* Instead call st_request_forward_plan(), which mediates access.
*
* mp_forward_plan() performs just-in-time forward planning immediately before
* lines and commands are queued to the move execution runtime (exec).
* mp_forward_plan() performs just-in-time forward planning immediately before
* lines and commands are queued to the move execution runtime (exec).
* Unlike backward planning, buffers are only forward planned once.
*
* mp_forward_plan() is called aggressively via st_request_forward_plan().
* It has a relatively low interrupt level to call its own.
* See also: Planner Overview notes in planner.h
*
*
* It examines the currently running buffer and its adjacent buffers to:
*
* - Stop the system from re-planning or planning something that's not prepped
@@ -68,43 +68,43 @@ static void _init_forward_diffs(float v_0, float v_1);
* - Skip past/ or pre-plan COMMAND blocks while labeling them as PLANNED
*
* Returns:
* - STAT_OK if exec should be called to kickstart (or continue) movement
* - STAT_OK if exec should be called to kickstart (or continue) movement
* - STAT_NOOP to exit with no action taken (do not call exec)
*/
/*
* --- Forward Planning Processing and Cases ---
*
* These cases describe all possible sequences of buffers in the planner queue starting
*
* These cases describe all possible sequences of buffers in the planner queue starting
* with the currently executing (or about to execute) Run buffer, looking forward
* to more recently arrived buffers. In most cases only one or two buffers need to
* be examined, but contiguous groups of commands may need to be processed.
* to more recently arrived buffers. In most cases only one or two buffers need to
* be examined, but contiguous groups of commands may need to be processed.
*
* 'Running' cases are where the run buffer state is RUNNING. Bootstrap handles all other cases.
* 'Bootstrap' occurs during the startup phase where moves are collected before starting movement.
* Conditions that are impossible based on this definition are not listed in the tables below.
*
* See planner.h / bufferState enum for shorthand used in the descriptions.
* See planner.h / bufferState enum for shorthand used in the descriptions.
* All cases assume a mix of moves and commands, as noted in the shorthand.
* All cases assume 2 'blocks' - Run block & Plan block. Cases will need to be
* revisited and generalized if more blocks are used in the future (deeper
* All cases assume 2 'blocks' - Run block & Plan block. Cases will need to be
* revisited and generalized if more blocks are used in the future (deeper
* forward planning).
*
* 'NOT_PREPPED' refers to any preliminary state below PREPPED, i.e. < PREPPED.
* ' NOT_PREPPED' can be either a move or command, we don't care so it's not specified.
*
* 'COMMAND' or 'COMMAND(s)' refers to one command or a contiguous group of command buffers
* that may be in PREPPED or PLANNED states. Processing is always the same. Plan all
* that may be in PREPPED or PLANNED states. Processing is always the same. Plan all
* PREPPED commands and skip past all PLANNED commands.
*
* Note 1: For MOVEs use the exit velocity of the Run block (mr.r->exit_velocity) as the
* Note 1: For MOVEs use the exit velocity of the Run block (mr.r->exit_velocity) as the
* entry velocity of the next adjacent move.
*
* Note 1a: In this special COMMAND case we trust mr.r->exit_velocity because the
* Note 1a: In this special COMMAND case we trust mr.r->exit_velocity because the
* backplanner has already handled this case for us.
*
* Note 2: For COMMANDs use the entry velocity of the current runtime (mr.entry_velocity)
* as the entry velocity for the next adjacent move. mr.entry_velocity is almost always 0,
* but could be non-0 in a race condition.
* as the entry velocity for the next adjacent move. mr.entry_velocity is almost always 0,
* but could be non-0 in a race condition.
* FYI: mr.entry_velocity is set at the end of the last running block in mp_exec_aline().
*
*
@@ -197,7 +197,7 @@ stat_t mp_forward_plan()
{
mpBuf_t *bf = mp_get_run_buffer();
float entry_velocity;
// Case 0: Examine current running buffer for early exit conditions
if (bf == NULL) { // case 0a: NULL means nothing is running - this is OK
st_prep_null();
@@ -221,11 +221,11 @@ stat_t mp_forward_plan()
// bf now points to the first non-command buffer past the command(s)
if ((bf->block_type == BLOCK_TYPE_ALINE) && (bf->buffer_state > MP_BUFFER_PREPPED )) { // case 1i
entry_velocity = mr.r->exit_velocity; // set entry_velocity for Note 1a
}
}
}
}
// bf will always be on a non-command at this point - either a move or empty buffer
// process move
// process move
if (bf->block_type == BLOCK_TYPE_ALINE) { // do cases 1a - 1e; finish cases 1f - 1k
if (bf->buffer_state == MP_BUFFER_PREPPED) {// do 1a; finish 1f, 1j, 2d, 2i
return (_plan_move(bf, entry_velocity));
@@ -322,12 +322,12 @@ stat_t mp_exec_move()
* STAT_NOOP cause no operation from the steppers - do not load the move
* STAT_xxxxx fatal error. Ends the move and frees the bf buffer
*
* This routine is called from the (LO) interrupt level. The interrupt sequencing
* relies on the behaviors of the routines being exactly correct. Each call to
* _exec_aline() must execute and prep *one and only one* segment. If the segment
* is the not the last segment in the bf buffer the _aline() must return STAT_EAGAIN.
* If it's the last segment it must return STAT_OK. If it encounters a fatal error
* that would terminate the move it should return a valid error code. Failure to
* This routine is called from the (LO) interrupt level. The interrupt sequencing
* relies on the behaviors of the routines being exactly correct. Each call to
* _exec_aline() must execute and prep *one and only one* segment. If the segment
* is the not the last segment in the bf buffer the _aline() must return STAT_EAGAIN.
* If it's the last segment it must return STAT_OK. If it encounters a fatal error
* that would terminate the move it should return a valid error code. Failure to
* obey this will introduce subtle and very difficult to diagnose bugs (trust us on this).
*
* Note 1: Returning STAT_OK ends the move and frees the bf buffer.
@@ -358,26 +358,26 @@ stat_t mp_exec_move()
*/
/* Synchronization of run BUFFER and run BLOCK
*
* Note first: mp_exec_aline() makes a huge assumption: When it comes time to get a
* new run block (mr.r) it assumes the planner block (mr.p) has been fully planned
* Note first: mp_exec_aline() makes a huge assumption: When it comes time to get a
* new run block (mr.r) it assumes the planner block (mr.p) has been fully planned
* via the JIT forward planning and is ready for use as the new run block.
*
* The runtime uses 2 structures for the current move or commend, the run BUFFER
* from the planner queue (mb.r, aka bf), and the run BLOCK from the runtime
* singleton (mr.r). These structures are synchronized implicitly, but not
* explicitly referenced, as pointers can lead to race conditions.
* The runtime uses 2 structures for the current move or commend, the run BUFFER
* from the planner queue (mb.r, aka bf), and the run BLOCK from the runtime
* singleton (mr.r). These structures are synchronized implicitly, but not
* explicitly referenced, as pointers can lead to race conditions.
* See plan_zoid.cpp / mp_calculate_ramps() for more details
*
* When mp_exec_aline() needs to grab a new planner buffer for a new move or command
* When mp_exec_aline() needs to grab a new planner buffer for a new move or command
* (i.e. block state is inactive) it swaps (rolls) the run and planner BLOCKS so that
* mr.p (planner block) is now the mr.r (run block), and the old mr.r block becomes
* mr.p (planner block) is now the mr.r (run block), and the old mr.r block becomes
* available for planning; it becomes mr.p block.
*
* At the same time, it's when finished with its current run buffer (mb.r), it has already
* At the same time, it's when finished with its current run buffer (mb.r), it has already
* advanced to the next buffer. mp_exec_move() does this at the end of previous move.
* Or in the bootstrap case, there never was a previous mb.r, so the current one is OK.
*
* As if by magic, the new mb.r aligns with the run block that was just moved in from
*
* As if by magic, the new mb.r aligns with the run block that was just moved in from
* the planning block.
*/
@@ -786,6 +786,16 @@ void mp_exit_hold_state()
* F_1 = 120Ah^5
*
* Note that with our current control points, D and E are actually 0.
*
* Further expansion, if we can do linear extroplation during a segment, we actually do want to start
* at t = 0.
*
* F_5(h)-F_5(0) = Ah^5 + Bh^4 + Ch^3 + Dh^2 + Eh
* F_4(h)-F_4(0) = 30Ah^5 + 14Bh^4 + 6Ch^3 + 2Dh^2
* F_3(h)-F_3(0) = 150Ah^5 + 36Bh^4 + 6Ch^3
* F_2(h)-F_2(0) = 240Ah^5 + 24Bh^4
* F_1(h)-F_1(0) = 120Ah^5
*
*/
// Total time: 147us
@@ -829,6 +839,7 @@ static void _init_forward_diffs(const float v_0, const float v_1)
const float Bh_4 = B * h_4;
const float Ch_3 = C * h_3;
#if !defined(NEW_FWD_DIFF) || (NEW_FWD_DIFF==0)
const float const1 = 7.5625; // (121.0/16.0)
const float const2 = 3.25; // ( 13.0/ 4.0)
const float const3 = 82.5; // (165.0/ 2.0)
@@ -858,6 +869,28 @@ static void _init_forward_diffs(const float v_0, const float v_1)
const float half_Ah_5 = A * half_h_5;
mr.segment_velocity = half_Ah_5 + half_Bh_4 + half_Ch_3 + v_0;
#else
// NEW_FWD_DIFF == 1
/*
* F_5 = A h^5 + B h^4 + C h^3 + D h^2 + E h
* F_4 = 30 A h^5 + 14 B h^4 + 6 C h^3 + 2 D h^2
* F_3 = 150 A h^5 + 36 B h^4 + 6 C h^3
* F_2 = 240 A h^5 + 24 B h^4
* F_1 = 120 A h^5
*/
mr.forward_diff_5 = Ah_5 + Bh_4 + Ch_3;
mr.forward_diff_4 = 30.0*Ah_5 + 14.0*Bh_4 + 6.0*Ch_3;
mr.forward_diff_3 = 150.0*Ah_5 + 36.0*Bh_4 + 6.0*Ch_3;
mr.forward_diff_2 = 240.0*Ah_5 + 24.0*Bh_4;
mr.forward_diff_1 = 120.0*Ah_5;
mr.segment_velocity = v_0;
mr.target_velocity = v_0 + mr.forward_diff_5;
#endif
}
/*********************************************************************************************
@@ -868,7 +901,9 @@ static stat_t _exec_aline_head(mpBuf_t *bf)
{
bool first_pass = false;
if (mr.section_state == SECTION_NEW) { // INITIALIZATION
#if !defined(NEW_FWD_DIFF) || (NEW_FWD_DIFF==0)
first_pass = true;
#endif
if (fp_ZERO(mr.r->head_length)) {
mr.section = SECTION_BODY;
return(_exec_aline_body(bf)); // skip ahead to the body generator
@@ -879,7 +914,13 @@ static stat_t _exec_aline_head(mpBuf_t *bf)
if (mr.segment_count == 1) {
// We will only have one segment, simply average the velocities
#if !defined(NEW_FWD_DIFF) || (NEW_FWD_DIFF==0)
mr.segment_velocity = mr.r->head_length / mr.segment_time;
#else
mr.segment_velocity = mr.entry_velocity;
mr.target_velocity = mr.r->cruise_velocity;
#endif
} else {
_init_forward_diffs(mr.entry_velocity, mr.r->cruise_velocity); // <-- sets inital segment_velocity
}
@@ -890,7 +931,12 @@ static stat_t _exec_aline_head(mpBuf_t *bf)
mr.section = SECTION_HEAD;
mr.section_state = SECTION_RUNNING;
} else {
#if !defined(NEW_FWD_DIFF) || (NEW_FWD_DIFF==0)
mr.segment_velocity += mr.forward_diff_5;
#else
mr.segment_velocity = mr.target_velocity;
mr.target_velocity += mr.forward_diff_5;
#endif
}
if (_exec_aline_segment() == STAT_OK) { // set up for second half
@@ -927,6 +973,7 @@ static stat_t _exec_aline_body(mpBuf_t *bf)
mr.segments = ceil(uSec(body_time) / NOM_SEGMENT_USEC);
mr.segment_time = body_time / mr.segments;
mr.segment_velocity = mr.r->cruise_velocity;
mr.target_velocity = mr.segment_velocity;
mr.segment_count = (uint32_t)mr.segments;
if (mr.segment_time < MIN_SEGMENT_TIME) {
_debug_trap("mr.segment_time < MIN_SEGMENT_TIME");
@@ -954,7 +1001,9 @@ static stat_t _exec_aline_tail(mpBuf_t *bf)
{
bool first_pass = false;
if (mr.section_state == SECTION_NEW) { // INITIALIZATION
#if !defined(NEW_FWD_DIFF) || (NEW_FWD_DIFF==0)
first_pass = true;
#endif
// Mark the block as unplannable
bf->plannable = false;
@@ -965,7 +1014,12 @@ static stat_t _exec_aline_tail(mpBuf_t *bf)
mr.segment_time = mr.r->tail_time / mr.segments; // time to advance for each segment
if (mr.segment_count == 1) {
#if !defined(NEW_FWD_DIFF) || (NEW_FWD_DIFF==0)
mr.segment_velocity = mr.r->tail_length / mr.segment_time;
#else
mr.segment_velocity = mr.r->cruise_velocity;
mr.target_velocity = mr.r->exit_velocity;
#endif
} else {
_init_forward_diffs(mr.r->cruise_velocity, mr.r->exit_velocity); // <-- sets inital segment_velocity
}
@@ -977,7 +1031,12 @@ static stat_t _exec_aline_tail(mpBuf_t *bf)
mr.section = SECTION_TAIL;
mr.section_state = SECTION_RUNNING;
} else {
#if !defined(NEW_FWD_DIFF) || (NEW_FWD_DIFF==0)
mr.segment_velocity += mr.forward_diff_5;
#else
mr.segment_velocity = mr.target_velocity;
mr.target_velocity += mr.forward_diff_5;
#endif
}
if (_exec_aline_segment() == STAT_OK) {
@@ -1023,18 +1082,21 @@ static stat_t _exec_aline_segment()
if ((--mr.segment_count == 0) && (cm.motion_state != MOTION_HOLD)) {
copy_vector(mr.gm.target, mr.waypoint[mr.section]);
} else {
#if !defined(NEW_FWD_DIFF) || (NEW_FWD_DIFF==0)
float segment_length = mr.segment_velocity * mr.segment_time;
#else
float segment_length = (mr.segment_velocity+mr.target_velocity) * 0.5 * mr.segment_time;
#endif
// see https://en.wikipedia.org/wiki/Kahan_summation_algorithm
// for the summation compensation description
for (uint8_t a=0; a<AXES; a++) {
#if 1
// The following is equivalent to:
// mr.gm.target[a] = mr.position[a] + (mr.unit[a] * segment_length);
float to_add = (mr.unit[a] * segment_length) - mr.gm.target_comp[a];
float target = mr.position[a] + to_add;
mr.gm.target_comp[a] = (target - mr.position[a]) - to_add;
mr.gm.target[a] = target;
#else
mr.gm.target[a] = mr.position[a] + (mr.unit[a] * segment_length);
#endif
}
}
@@ -1063,7 +1125,12 @@ static stat_t _exec_aline_segment()
}
// Call the stepper prep function
#if !defined(NEW_FWD_DIFF) || (NEW_FWD_DIFF==0)
ritorno(st_prep_line(travel_steps, mr.following_error, mr.segment_time));
#else
ritorno(st_prep_line(mr.segment_velocity, mr.target_velocity, travel_steps, mr.following_error, mr.segment_time));
#endif
copy_vector(mr.position, mr.gm.target); // update position from target
if (mr.segment_count == 0) {
return (STAT_OK); // this section has run all its segments

5
g2core/planner.h
View File

@@ -149,6 +149,8 @@
#ifndef PLANNER_H_ONCE
#define PLANNER_H_ONCE
#define NEW_FWD_DIFF 1
#include "canonical_machine.h" // used for GCodeState_t
#include "hardware.h" // for MIN_SEGMENT_MS
@@ -506,7 +508,8 @@ typedef struct mpMotionRuntimeSingleton { // persistent runtime variables
float segments; // number of segments in line (also used by arc generation)
uint32_t segment_count; // count of running segments
float segment_velocity; // computed velocity for aline segment
float segment_velocity; // computed start velocity for aline segment
float target_velocity; // computed end velocity for aline segment
float segment_time; // actual time increment per aline segment
float forward_diff_1; // forward difference level 1

184
g2core/stepper.cpp
View File

@@ -302,67 +302,85 @@ void dda_timer_type::interrupt()
// }
// process DDAs for each motor
if ((st_run.mot[MOTOR_1].substep_accumulator += st_run.mot[MOTOR_1].substep_increment) > 0) {
motor_1.stepStart(); // turn step bit on
if ((st_run.mot[MOTOR_1].substep_accumulator += st_run.mot[MOTOR_1].substep_increment) > 0) {
motor_1.stepStart(); // turn step bit on
#if NEW_DDA == 1
st_run.mot[MOTOR_1].substep_accumulator -= DDA_SUBSTEPS; //st_run.dda_ticks_X_substeps;
st_run.mot[MOTOR_1].substep_accumulator -= DDA_SUBSTEPS; //st_run.dda_ticks_X_substeps;
#else
st_run.mot[MOTOR_1].substep_accumulator -= st_run.dda_ticks_X_substeps;
st_run.mot[MOTOR_1].substep_accumulator -= st_run.dda_ticks_X_substeps;
#endif
INCREMENT_ENCODER(MOTOR_1);
}
if ((st_run.mot[MOTOR_2].substep_accumulator += st_run.mot[MOTOR_2].substep_increment) > 0) {
motor_2.stepStart(); // turn step bit on
INCREMENT_ENCODER(MOTOR_1);
}
#if defined(NEW_FWD_DIFF) && (NEW_FWD_DIFF==1)
st_run.mot[MOTOR_1].substep_increment += st_run.mot[MOTOR_1].substep_increment_increment;
#endif
if ((st_run.mot[MOTOR_2].substep_accumulator += st_run.mot[MOTOR_2].substep_increment) > 0) {
motor_2.stepStart(); // turn step bit on
#if NEW_DDA == 1
st_run.mot[MOTOR_2].substep_accumulator -= DDA_SUBSTEPS; //st_run.dda_ticks_X_substeps;
st_run.mot[MOTOR_2].substep_accumulator -= DDA_SUBSTEPS; //st_run.dda_ticks_X_substeps;
#else
st_run.mot[MOTOR_2].substep_accumulator -= st_run.dda_ticks_X_substeps;
st_run.mot[MOTOR_2].substep_accumulator -= st_run.dda_ticks_X_substeps;
#endif
INCREMENT_ENCODER(MOTOR_2);
}
#if defined(NEW_FWD_DIFF) && (NEW_FWD_DIFF==1)
st_run.mot[MOTOR_2].substep_increment += st_run.mot[MOTOR_2].substep_increment_increment;
#endif
INCREMENT_ENCODER(MOTOR_2);
}
#if MOTORS > 2
if ((st_run.mot[MOTOR_3].substep_accumulator += st_run.mot[MOTOR_3].substep_increment) > 0) {
motor_3.stepStart(); // turn step bit on
if ((st_run.mot[MOTOR_3].substep_accumulator += st_run.mot[MOTOR_3].substep_increment) > 0) {
motor_3.stepStart(); // turn step bit on
#if NEW_DDA == 1
st_run.mot[MOTOR_3].substep_accumulator -= DDA_SUBSTEPS; //st_run.dda_ticks_X_substeps;
st_run.mot[MOTOR_3].substep_accumulator -= DDA_SUBSTEPS; //st_run.dda_ticks_X_substeps;
#else
st_run.mot[MOTOR_3].substep_accumulator -= st_run.dda_ticks_X_substeps;
st_run.mot[MOTOR_3].substep_accumulator -= st_run.dda_ticks_X_substeps;
#endif
INCREMENT_ENCODER(MOTOR_3);
}
#if defined(NEW_FWD_DIFF) && (NEW_FWD_DIFF==1)
st_run.mot[MOTOR_3].substep_increment += st_run.mot[MOTOR_3].substep_increment_increment;
#endif
INCREMENT_ENCODER(MOTOR_3);
}
#endif
#if MOTORS > 3
if ((st_run.mot[MOTOR_4].substep_accumulator += st_run.mot[MOTOR_4].substep_increment) > 0) {
motor_4.stepStart(); // turn step bit on
if ((st_run.mot[MOTOR_4].substep_accumulator += st_run.mot[MOTOR_4].substep_increment) > 0) {
motor_4.stepStart(); // turn step bit on
#if NEW_DDA == 1
st_run.mot[MOTOR_4].substep_accumulator -= DDA_SUBSTEPS; //st_run.dda_ticks_X_substeps;
st_run.mot[MOTOR_4].substep_accumulator -= DDA_SUBSTEPS; //st_run.dda_ticks_X_substeps;
#else
st_run.mot[MOTOR_4].substep_accumulator -= st_run.dda_ticks_X_substeps;
st_run.mot[MOTOR_4].substep_accumulator -= st_run.dda_ticks_X_substeps;
#endif
INCREMENT_ENCODER(MOTOR_4);
}
#if defined(NEW_FWD_DIFF) && (NEW_FWD_DIFF==1)
st_run.mot[MOTOR_4].substep_increment += st_run.mot[MOTOR_4].substep_increment_increment;
#endif
INCREMENT_ENCODER(MOTOR_4);
}
#endif
#if MOTORS > 4
if ((st_run.mot[MOTOR_5].substep_accumulator += st_run.mot[MOTOR_5].substep_increment) > 0) {
motor_5.stepStart(); // turn step bit on
if ((st_run.mot[MOTOR_5].substep_accumulator += st_run.mot[MOTOR_5].substep_increment) > 0) {
motor_5.stepStart(); // turn step bit on
#if NEW_DDA == 1
st_run.mot[MOTOR_5].substep_accumulator -= DDA_SUBSTEPS; //st_run.dda_ticks_X_substeps;
st_run.mot[MOTOR_5].substep_accumulator -= DDA_SUBSTEPS; //st_run.dda_ticks_X_substeps;
#else
st_run.mot[MOTOR_5].substep_accumulator -= st_run.dda_ticks_X_substeps;
st_run.mot[MOTOR_5].substep_accumulator -= st_run.dda_ticks_X_substeps;
#endif
INCREMENT_ENCODER(MOTOR_5);
}
INCREMENT_ENCODER(MOTOR_5);
#if defined(NEW_FWD_DIFF) && (NEW_FWD_DIFF==1)
st_run.mot[MOTOR_5].substep_increment += st_run.mot[MOTOR_5].substep_increment_increment;
#endif
}
#endif
#if MOTORS > 5
if ((st_run.mot[MOTOR_6].substep_accumulator += st_run.mot[MOTOR_6].substep_increment) > 0) {
motor_6.stepStart(); // turn step bit on
if ((st_run.mot[MOTOR_6].substep_accumulator += st_run.mot[MOTOR_6].substep_increment) > 0) {
motor_6.stepStart(); // turn step bit on
#if NEW_DDA == 1
st_run.mot[MOTOR_6].substep_accumulator -= DDA_SUBSTEPS; //st_run.dda_ticks_X_substeps;
st_run.mot[MOTOR_6].substep_accumulator -= DDA_SUBSTEPS; //st_run.dda_ticks_X_substeps;
#else
st_run.mot[MOTOR_6].substep_accumulator -= st_run.dda_ticks_X_substeps;
st_run.mot[MOTOR_6].substep_accumulator -= st_run.dda_ticks_X_substeps;
#endif
INCREMENT_ENCODER(MOTOR_6);
}
#if defined(NEW_FWD_DIFF) && (NEW_FWD_DIFF==1)
st_run.mot[MOTOR_6].substep_increment += st_run.mot[MOTOR_6].substep_increment_increment;
#endif
INCREMENT_ENCODER(MOTOR_6);
}
#endif
// Process end of segment.
@@ -527,13 +545,15 @@ static void _load_move()
// the following if() statement sets the runtime substep increment value or zeroes it
if ((st_run.mot[MOTOR_1].substep_increment = st_pre.mot[MOTOR_1].substep_increment) != 0) {
// NB: If motor has 0 steps the following is all skipped. This ensures that state comparisons
// always operate on the last segment actually run by this motor, regardless of how many
// segments it may have been inactive in between.
// Apply accumulator correction if the time base has changed since previous segment
#if NEW_DDA == 1
#if defined(NEW_FWD_DIFF) && (NEW_FWD_DIFF==1)
st_run.mot[MOTOR_1].substep_increment_increment = st_pre.mot[MOTOR_1].substep_increment_increment;
#endif
#else
if (st_pre.mot[MOTOR_1].accumulator_correction_flag == true) {
st_pre.mot[MOTOR_1].accumulator_correction_flag = false;
@@ -560,6 +580,9 @@ static void _load_move()
SET_ENCODER_STEP_SIGN(MOTOR_1, st_pre.mot[MOTOR_1].step_sign);
} else { // Motor has 0 steps; might need to energize motor for power mode processing
#if defined(NEW_FWD_DIFF) && (NEW_FWD_DIFF==1)
st_run.mot[MOTOR_1].substep_increment_increment = 0;
#endif
motor_1.motionStopped();
}
// accumulate counted steps to the step position and zero out counted steps for the segment currently being loaded
@@ -568,6 +591,9 @@ static void _load_move()
#if (MOTORS >= 2)
if ((st_run.mot[MOTOR_2].substep_increment = st_pre.mot[MOTOR_2].substep_increment) != 0) {
#if NEW_DDA == 1
#if defined(NEW_FWD_DIFF) && (NEW_FWD_DIFF==1)
st_run.mot[MOTOR_2].substep_increment_increment = st_pre.mot[MOTOR_2].substep_increment_increment;
#endif
#else
if (st_pre.mot[MOTOR_2].accumulator_correction_flag == true) {
st_pre.mot[MOTOR_2].accumulator_correction_flag = false;
@@ -586,6 +612,9 @@ static void _load_move()
motor_2.enable();
SET_ENCODER_STEP_SIGN(MOTOR_2, st_pre.mot[MOTOR_2].step_sign);
} else {
#if defined(NEW_FWD_DIFF) && (NEW_FWD_DIFF==1)
st_run.mot[MOTOR_2].substep_increment_increment = 0;
#endif
motor_2.motionStopped();
}
ACCUMULATE_ENCODER(MOTOR_2);
@@ -593,6 +622,9 @@ static void _load_move()
#if (MOTORS >= 3)
if ((st_run.mot[MOTOR_3].substep_increment = st_pre.mot[MOTOR_3].substep_increment) != 0) {
#if NEW_DDA == 1
#if defined(NEW_FWD_DIFF) && (NEW_FWD_DIFF==1)
st_run.mot[MOTOR_3].substep_increment_increment = st_pre.mot[MOTOR_3].substep_increment_increment;
#endif
#else
if (st_pre.mot[MOTOR_3].accumulator_correction_flag == true) {
st_pre.mot[MOTOR_3].accumulator_correction_flag = false;
@@ -611,6 +643,9 @@ static void _load_move()
motor_3.enable();
SET_ENCODER_STEP_SIGN(MOTOR_3, st_pre.mot[MOTOR_3].step_sign);
} else {
#if defined(NEW_FWD_DIFF) && (NEW_FWD_DIFF==1)
st_run.mot[MOTOR_3].substep_increment_increment = 0;
#endif
motor_3.motionStopped();
}
ACCUMULATE_ENCODER(MOTOR_3);
@@ -618,6 +653,9 @@ static void _load_move()
#if (MOTORS >= 4)
if ((st_run.mot[MOTOR_4].substep_increment = st_pre.mot[MOTOR_4].substep_increment) != 0) {
#if NEW_DDA == 1
#if defined(NEW_FWD_DIFF) && (NEW_FWD_DIFF==1)
st_run.mot[MOTOR_4].substep_increment_increment = st_pre.mot[MOTOR_4].substep_increment_increment;
#endif
#else
if (st_pre.mot[MOTOR_4].accumulator_correction_flag == true) {
st_pre.mot[MOTOR_4].accumulator_correction_flag = false;
@@ -636,6 +674,9 @@ static void _load_move()
motor_4.enable();
SET_ENCODER_STEP_SIGN(MOTOR_4, st_pre.mot[MOTOR_4].step_sign);
} else {
#if defined(NEW_FWD_DIFF) && (NEW_FWD_DIFF==1)
st_run.mot[MOTOR_4].substep_increment_increment = 0;
#endif
motor_4.motionStopped();
}
ACCUMULATE_ENCODER(MOTOR_4);
@@ -643,6 +684,9 @@ static void _load_move()
#if (MOTORS >= 5)
if ((st_run.mot[MOTOR_5].substep_increment = st_pre.mot[MOTOR_5].substep_increment) != 0) {
#if NEW_DDA == 1
#if defined(NEW_FWD_DIFF) && (NEW_FWD_DIFF==1)
st_run.mot[MOTOR_5].substep_increment_increment = st_pre.mot[MOTOR_5].substep_increment_increment;
#endif
#else
if (st_pre.mot[MOTOR_5].accumulator_correction_flag == true) {
st_pre.mot[MOTOR_5].accumulator_correction_flag = false;
@@ -661,6 +705,9 @@ static void _load_move()
motor_5.enable();
SET_ENCODER_STEP_SIGN(MOTOR_5, st_pre.mot[MOTOR_5].step_sign);
} else {
#if defined(NEW_FWD_DIFF) && (NEW_FWD_DIFF==1)
st_run.mot[MOTOR_5].substep_increment_increment = 0;
#endif
motor_5.motionStopped();
}
ACCUMULATE_ENCODER(MOTOR_5);
@@ -668,6 +715,9 @@ static void _load_move()
#if (MOTORS >= 6)
if ((st_run.mot[MOTOR_6].substep_increment = st_pre.mot[MOTOR_6].substep_increment) != 0) {
#if NEW_DDA == 1
#if defined(NEW_FWD_DIFF) && (NEW_FWD_DIFF==1)
st_run.mot[MOTOR_6].substep_increment_increment = st_pre.mot[MOTOR_6].substep_increment_increment;
#endif
#else
if (st_pre.mot[MOTOR_6].accumulator_correction_flag == true) {
st_pre.mot[MOTOR_6].accumulator_correction_flag = false;
@@ -686,6 +736,9 @@ static void _load_move()
motor_6.enable();
SET_ENCODER_STEP_SIGN(MOTOR_6, st_pre.mot[MOTOR_6].step_sign);
} else {
#if defined(NEW_FWD_DIFF) && (NEW_FWD_DIFF==1)
st_run.mot[MOTOR_6].substep_increment_increment = 0;
#endif
motor_6.motionStopped();
}
ACCUMULATE_ENCODER(MOTOR_6);
@@ -738,7 +791,11 @@ static void _load_move()
* dda_ticks_X_substeps = (int32_t)((microseconds/1000000) * f_dda * dda_substeps);
*/
#if !defined(NEW_FWD_DIFF) || (NEW_FWD_DIFF==0)
stat_t st_prep_line(float travel_steps[], float following_error[], float segment_time)
#else
stat_t st_prep_line(float start_velocity, float end_velocity, float travel_steps[], float following_error[], float segment_time)
#endif
{
stepper_debug("😶");
// trap assertion failures and other conditions that would prevent queuing the line
@@ -763,6 +820,10 @@ stat_t st_prep_line(float travel_steps[], float following_error[], float segment
#endif
// setup motor parameters
#if defined(NEW_FWD_DIFF) && (NEW_FWD_DIFF==1)
// this is explained later
float t_v0_v1 = (float)st_pre.dda_ticks * (start_velocity + end_velocity);
#endif
float correction_steps;
for (uint8_t motor=0; motor<MOTORS; motor++) { // remind us that this is motors, not axes
@@ -822,7 +883,56 @@ stat_t st_prep_line(float travel_steps[], float following_error[], float segment
// that results in long-term negative drift. (fabs/round order doesn't matter)
#if NEW_DDA == 1
#if !defined(NEW_FWD_DIFF) || (NEW_FWD_DIFF==0)
// t = v n s
// m = v n
// t = m s
// m = t/s
// Needed is steps/tick
// 1/m = s/t
st_pre.mot[motor].substep_increment = round(fabs(travel_steps[motor] / (float)st_pre.dda_ticks) * (float)DDA_SUBSTEPS);
#else
// t is ticks duration of the move
// T is time duration of the move in minutes
// f is dda frequency, ticks/sec
// s is steps for the move
// n is unknown scale factor
// whatever the kinematics end up with to convert mm to steps for this motor and segment
// v_0 and v_1 are the start and end velocity (in mm/min)
//
// t = T 60 f
// Note: conversion from minutes to seconds cancels out in n
// n = (s/(T 60))/(((v_0/60)+(v_1/60))/2) = (2 s)/(T(v_0 + v_1))
//
// Needed is steps/tick
// 1/m_0 = (n (v_0/60))/f
// 1/m_1 = (n (v_1/60))/f
//
// Substitute n:
// 1/m_0 = ((2 s)/(T(v_0 + v_1)) (v_0/60))/f = (s v_0)/(T 30 f (v_0 + v_1)) = (2 s v_0)/(t (v_0 + v_1))
// 1/m_1 = ((2 s)/(T(v_0 + v_1)) (v_1/60))/f = (s v_1)/(T 30 f (v_0 + v_1)) = (2 s v_1)/(t (v_0 + v_1))
// d = (1/m_1-1/m_0)/(t-1) = (2 s (v_1 - v_0))/((t - 1) t (v_0 + v_1))
// Some common terms:
// a = t (v_0 + v_1)
// b = 2 s
// c = 1/m_0 = (b v_0)/a
// option 1:
// d = ((b v_1)/a - c)/(t-1)
// option 2:
// d = (b (v_1 - v_0))/((t-1) a)
float s_double = fabs(travel_steps[motor] * 2.0);
// 1/m_0 = (2 s v_0)/(t (v_0 + v_1))
st_pre.mot[motor].substep_increment = round(((s_double * start_velocity)/(t_v0_v1)) * (float)DDA_SUBSTEPS);
// option 1:
// d = ((b v_1)/a - c)/(t-1)
// option 2:
// d = (b (v_1 - v_0))/((t-1) a)
st_pre.mot[motor].substep_increment_increment = round(((s_double*(end_velocity-start_velocity))/(((float)st_pre.dda_ticks-1.0)*t_v0_v1)) * (float)DDA_SUBSTEPS);
#warning Using new increment style!
#endif
#else
st_pre.mot[motor].substep_increment = round(fabs(travel_steps[motor] * DDA_SUBSTEPS));
#endif

19
g2core/stepper.h
View File

@@ -372,7 +372,8 @@ typedef struct stConfig { // stepper configs
// Motor runtime structure. Used exclusively by step generation ISR (HI)
typedef struct stRunMotor { // one per controlled motor
uint32_t substep_increment; // total steps in axis times substeps factor
uint32_t substep_increment; // partial steps to increment substep_accumulator per tick
uint32_t substep_increment_increment; // partial steps to increment substep_increment per tick
int32_t substep_accumulator; // DDA phase angle accumulator
bool motor_flag; // true if motor is participating in this move
uint32_t power_systick; // sys_tick for next motor power state transition
@@ -396,7 +397,8 @@ typedef struct stRunSingleton { // Stepper static values and axis pa
// Must be careful about volatiles in this one
typedef struct stPrepMotor {
uint32_t substep_increment; // total steps in axis times substep factor
uint32_t substep_increment; // partial steps to increment substep_accumulator per tick
uint32_t substep_increment_increment; // partial steps to increment substep_increment per tick
bool motor_flag; // true if motor is participating in this move
// direction and direction change
@@ -492,7 +494,7 @@ struct Stepper {
// turn on motor in all cases unless it's disabled
// this version is called from the loader, and explicitly does NOT have floating point computations
// HOT - called from the DDA interrupt
void enable() HOT_FUNC
void enable() //HOT_FUNC
{
if (_power_mode == MOTOR_DISABLED) {
return;
@@ -505,7 +507,7 @@ struct Stepper {
// turn on motor in all cases unless it's disabled
// this version has the timeout computed here, as provided
void enable(float timeout) HOT_FUNC
void enable(float timeout) //HOT_FUNC
{
if (_power_mode == MOTOR_DISABLED) {
return;
@@ -523,7 +525,7 @@ struct Stepper {
// turn off motor in all cases unless it's permanently enabled
// HOT - called from the DDA interrupt
void disable() HOT_FUNC
void disable() //HOT_FUNC
{
if (this->getPowerMode() == MOTOR_ALWAYS_POWERED) {
return;
@@ -535,7 +537,8 @@ struct Stepper {
// turn off motor is only powered when moving
// HOT - called from the DDA interrupt
void motionStopped() HOT_FUNC {
void motionStopped() //HOT_FUNC
{
if (_power_mode == MOTOR_POWERED_IN_CYCLE) {
this->enable();
_power_state = MOTOR_POWER_TIMEOUT_START;
@@ -607,7 +610,11 @@ void st_prep_null(void);
void st_prep_command(void *bf); // use a void pointer since we don't know about mpBuf_t yet)
void st_prep_dwell(float milliseconds);
void st_request_out_of_band_dwell(float microseconds);
#if !defined(NEW_FWD_DIFF) || (NEW_FWD_DIFF==0)
stat_t st_prep_line(float travel_steps[], float following_error[], float segment_time) HOT_FUNC;
#else
stat_t st_prep_line(float start_velocity, float end_velocity, float travel_steps[], float following_error[], float segment_time) HOT_FUNC;
#endif
stat_t st_set_ma(nvObj_t *nv);
stat_t st_set_sa(nvObj_t *nv);