From b3c3eb9e7cbddd0e291fe15746363f0c9bb192a4 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Thu, 1 Sep 2016 14:14:41 -0700 Subject: [PATCH] Move code and doc cleanup, simplified plan/exec.c --- MoveLifecycle.md | 31 ++++++++++---------- src/command.c | 33 ++++++++++++---------- src/machine.c | 46 +++++++++++++++--------------- src/motor.c | 2 +- src/plan/buffer.c | 4 +-- src/plan/buffer.h | 15 ++++------ src/plan/command.c | 1 - src/plan/exec.c | 69 +++++++++++++++++++++------------------------ src/plan/feedhold.c | 26 ++++++++--------- src/plan/line.c | 62 ++++++++++++++++++++-------------------- src/plan/planner.c | 23 ++++++++------- src/plan/planner.h | 6 ++-- 12 files changed, 152 insertions(+), 166 deletions(-) diff --git a/MoveLifecycle.md b/MoveLifecycle.md index f9bdef1..f6d781b 100644 --- a/MoveLifecycle.md +++ b/MoveLifecycle.md @@ -62,19 +62,20 @@ returns ``STAT_EAGAIN``. On the first call to ``mp_exec_aline()`` a call is made to ``_exec_aline_init()``. This function may stop processing the move if a feedhold is in effect. It may also skip a move if it has zero length. -Otherwise, it initializes the move runtime state (``mr``) by copying the move -state from the planner buffer and initializing variables. In addition, it -computes waypoints at the ends of each trapezoid section. Waypoints are used -later for position correction which adjust position for rounding errors. +Otherwise, it initializes the move runtime state (``mr``) copying over the +variables set in the planner buffer. In addition, it computes waypoints at +the ends of each trapezoid section. Waypoints are used later to correct +position for rounding errors. ### Move execution After move initialization ``mp_exec_aline()`` calls ``_exec_aline_head()``, -``_exec_aline_body()`` and ``exec_aline_tail()`` on successive callbacks. Each -of these functions are called repeatedly until the section finishes. If any +``_exec_aline_body()`` and ``exec_aline_tail()`` on successive callbacks. +These functions are called repeatedly until each section finishes. If any sections have zero length they are skipped and execution is passed immediately -to the next section. During each section forward differencing is used to map +to the next section. During each section, forward differencing is used to map the trapezoid computed during the planning stage to a fifth-degree Bezier -polynomial S-curve. The curve is used to find the next target position. +polynomial S-curve. The curve is used to find the appropriate velocity at the +next target position. ``_exec_aline_segment()`` is called for each non-zero section to convert the computed target position to target steps by calling ``mp_kinematics()``. The @@ -83,13 +84,13 @@ move fragment is then passed to the stepper driver by a call to returns ``STAT_OK`` indicating the next segment should be loaded. When all non-zero segments have been executed, the move is complete. -## Stepper driver -Calls to ``st_prep_line()`` prepare short (~5ms) moves for execution by the -stepper driver. The move time in clock ticks is computed from travel in steps -and the move duration. Then ``motor_prep_move()`` is called for each motor. -``motor_prep_move()`` may perform step correction, enables the motors if needed. -It then computes the optimal step clock divisor, clock ticks and sets the move -direction, taking the motor's configuration in to account. +## Pulse generation +Calls to ``st_prep_line()`` prepare short (~5ms) move fragements for pluse +generation by the stepper driver. Move time in clock ticks is computed from +travel in steps and move duration. Then ``motor_prep_move()`` is called for +each motor. ``motor_prep_move()`` may perform step correction and enable the +motor. It then computes the optimal step clock divisor, clock ticks and sets +the move direction, taking the motor's configuration in to account. The stepper timer ISR, after ending the previous move, calls ``motor_load_move()`` on each motor. This sets up and starts the motor clocks, diff --git a/src/command.c b/src/command.c index 9fe4ef1..841b78e 100644 --- a/src/command.c +++ b/src/command.c @@ -55,25 +55,28 @@ static char *_cmd = 0; -static void _estop() {estop_trigger(ESTOP_USER);} -static void _clear() {estop_clear();} -static void _pause() {mp_request_hold();} -static void _run() {mp_request_start();} -static void _step() {} // TODO -static void _report() {report_request_full();} -static void _reboot() {hw_request_hard_reset();} +static void _estop() {estop_trigger(ESTOP_USER);} +static void _clear() {estop_clear();} +static void _pause() {mp_request_hold();} +static void _opt_pause() {} // TODO +static void _run() {mp_request_start();} +static void _step() {} // TODO +static void _report() {report_request_full();} +static void _home() {} // TODO +static void _reboot() {hw_request_hard_reset();} static void command_i2c_cb(i2c_cmd_t cmd, uint8_t *data, uint8_t length) { switch (cmd) { - case I2C_ESTOP: _estop(); break; - case I2C_CLEAR: _clear(); break; - case I2C_PAUSE: _pause(); break; - case I2C_RUN: _run(); break; - case I2C_STEP: _step(); break; - case I2C_REPORT: _report(); break; - case I2C_HOME: break; - case I2C_REBOOT: _reboot(); break; + case I2C_ESTOP: _estop(); break; + case I2C_CLEAR: _clear(); break; + case I2C_PAUSE: _pause(); break; + case I2C_OPTIONAL_PAUSE: _opt_pause(); break; + case I2C_RUN: _run(); break; + case I2C_STEP: _step(); break; + case I2C_REPORT: _report(); break; + case I2C_HOME: _home(); break; + case I2C_REBOOT: _reboot(); break; default: break; } } diff --git a/src/machine.c b/src/machine.c index c84bf68..5df90de 100644 --- a/src/machine.c +++ b/src/machine.c @@ -35,35 +35,35 @@ * executes commands - passing the stateless commands to the motion * planning layer. * - * Synchronizing command execution + * Synchronizing command execution: * - * "Synchronous commands" are commands that affect the runtime need - * to be synchronized with movement. Examples include G4 dwells, - * program stops and ends, and most M commands. These are queued - * into the planner queue and execute from the queue. Synchronous - * commands work like this: + * "Synchronous commands" are commands that affect the runtime need + * to be synchronized with movement. Examples include G4 dwells, + * program stops and ends, and most M commands. These are queued + * into the planner queue and execute from the queue. Synchronous + * commands work like this: * - * - Call the mach_xxx_xxx() function which will do any input - * validation and return an error if it detects one. + * - Call the mach_xxx_xxx() function which will do any input + * validation and return an error if it detects one. * - * - The mach_ function calls mp_queue_command(). Arguments are a - * callback to the _exec_...() function, which is the runtime - * execution routine, and any arguments that are needed by the - * runtime. See typedef for *exec in planner.h for details + * - The mach_ function calls mp_queue_command(). Arguments are a + * callback to the _exec_...() function, which is the runtime + * execution routine, and any arguments that are needed by the + * runtime. See typedef for *exec in planner.h for details * - * - mp_queue_command() stores the callback and the args in a - planner buffer. + * - mp_queue_command() stores the callback and the args in a + * planner buffer. * - * - When planner execution reaches the buffer it executes the - * callback w/ the args. Take careful note that the callback - * executes under an interrupt, so beware of variables that may - * need to be volatile. + * - When planner execution reaches the buffer it executes the + * callback w/ the args. Take careful note that the callback + * executes under an interrupt, so beware of variables that may + * need to be volatile. * - * Note: - The synchronous command execution mechanism uses 2 - * vectors in the bf buffer to store and return values for the - * callback. It's obvious, but impractical to pass the entire bf - * buffer to the callback as some of these commands are actually - * executed locally and have no buffer. + * Note: The synchronous command execution mechanism uses 2 + * vectors in the bf buffer to store and return values for the + * callback. It's obvious, but impractical to pass the entire bf + * buffer to the callback as some of these commands are actually + * executed locally and have no buffer. */ #include "machine.h" diff --git a/src/motor.c b/src/motor.c index ecaa703..3e91b0d 100644 --- a/src/motor.c +++ b/src/motor.c @@ -402,7 +402,7 @@ void motor_prep_move(int motor, uint32_t seg_clocks, float travel_steps, // Fall through case MOTOR_ALWAYS_POWERED: case MOTOR_POWERED_IN_CYCLE: - _energize(motor); + _energize(motor); // TODO is ~5ms enough time to enable the motor? break; case MOTOR_POWER_MODE_MAX_VALUE: break; // Shouldn't get here diff --git a/src/plan/buffer.c b/src/plan/buffer.c index 3950c8a..ecd7da2 100644 --- a/src/plan/buffer.c +++ b/src/plan/buffer.c @@ -138,11 +138,11 @@ mpBuf_t *mp_get_write_buffer() { * buffer once it has been queued. Action may start on the buffer immediately, * invalidating its contents */ -void mp_commit_write_buffer(uint32_t line, moveType_t move_type) { +void mp_commit_write_buffer(uint32_t line, moveType_t type) { mp_state_running(); mb.q->ms.line = line; - mb.q->move_type = move_type; + mb.q->move_type = type; mb.q->move_state = MOVE_NEW; mb.q->buffer_state = MP_BUFFER_QUEUED; mb.q = mb.q->nx; // advance the queued buffer pointer diff --git a/src/plan/buffer.h b/src/plan/buffer.h index 5198fbe..0765dd7 100644 --- a/src/plan/buffer.h +++ b/src/plan/buffer.h @@ -33,7 +33,7 @@ #include -typedef enum { // bf->move_type values +typedef enum { MOVE_TYPE_NULL, // null move - does a no-op MOVE_TYPE_ALINE, // acceleration planned line MOVE_TYPE_DWELL, // delay with no movement @@ -55,7 +55,7 @@ typedef enum { // bf->buffer_state values MP_BUFFER_QUEUED, // in queue MP_BUFFER_PENDING, // marked as the next buffer to run MP_BUFFER_RUNNING // current running buffer -} mpBufferState_t; +} bufferState_t; // Callbacks @@ -71,11 +71,8 @@ typedef struct mpBuffer { // See Planning Velocity Notes bf_func_t bf_func; // callback to buffer exec function mach_exec_t mach_func; // callback to machine - float naive_move_time; - - mpBufferState_t buffer_state; // used to manage queuing/dequeuing + bufferState_t buffer_state; // used to manage queuing/dequeuing moveType_t move_type; // used to dispatch to run routine - uint8_t move_code; // byte used by used exec functions moveState_t move_state; // move state machine sequence bool replannable; // true if move can be re-planned @@ -85,18 +82,18 @@ typedef struct mpBuffer { // See Planning Velocity Notes float head_length; float body_length; float tail_length; + // See notes on these variables, in aline() float entry_velocity; // entry velocity requested for the move float cruise_velocity; // cruise velocity requested & achieved float exit_velocity; // exit velocity requested for the move + float braking_velocity; // current value for braking velocity float entry_vmax; // max junction velocity at entry of this move float cruise_vmax; // max cruise velocity requested for move float exit_vmax; // max exit velocity possible (redundant) float delta_vmax; // max velocity difference for this move - float braking_velocity; // current value for braking velocity - uint8_t jerk_axis; // rate limiting axis used to compute jerk float jerk; // maximum linear jerk term for this move float recip_jerk; // 1/Jm used for planning (computed & cached) float cbrt_jerk; // cube root of Jm (computed & cached) @@ -109,7 +106,7 @@ uint8_t mp_get_planner_buffer_room(); void mp_wait_for_buffer(); void mp_init_buffers(); mpBuf_t *mp_get_write_buffer(); -void mp_commit_write_buffer(uint32_t line, moveType_t move_type); +void mp_commit_write_buffer(uint32_t line, moveType_t type); mpBuf_t *mp_get_run_buffer(); void mp_free_run_buffer(); mpBuf_t *mp_get_last_buffer(); diff --git a/src/plan/command.c b/src/plan/command.c index 71d2397..89c8a6a 100644 --- a/src/plan/command.c +++ b/src/plan/command.c @@ -68,7 +68,6 @@ void mp_queue_command(mach_exec_t mach_exec, float *value, float *flag) { return; // Shouldn't happen, buffer availability was checked upstream. } - bf->move_type = MOVE_TYPE_COMMAND; bf->bf_func = _exec_command; // callback to planner queue exec function bf->mach_func = mach_exec; // callback to machine exec function diff --git a/src/plan/exec.c b/src/plan/exec.c index ba9f401..539ba97 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -39,6 +39,7 @@ #include #include #include +#include /*** Segment runner @@ -231,7 +232,7 @@ static stat_t _exec_aline_segment() { * * Note that with our current control points, D and E are actually 0. */ -static void _init_forward_diffs(float Vi, float Vt) { +static float _init_forward_diffs(float Vi, float Vt, float segments) { float A = -6.0 * Vi + 6.0 * Vt; float B = 15.0 * Vi - 15.0 * Vt; float C = -10.0 * Vi + 10.0 * Vt; @@ -239,7 +240,7 @@ static void _init_forward_diffs(float Vi, float Vt) { // E = 0 // F = Vi - float h = 1 / mr.segments; + float h = 1 / segments; float Ah_5 = A * h * h * h * h * h; float Bh_4 = B * h * h * h * h; @@ -257,23 +258,23 @@ static void _init_forward_diffs(float Vi, float Vt) { float half_Bh_4 = B * half_h * half_h * half_h * half_h; float half_Ah_5 = C * half_h * half_h * half_h * half_h * half_h; - mr.segment_velocity = half_Ah_5 + half_Bh_4 + half_Ch_3 + Vi; + return half_Ah_5 + half_Bh_4 + half_Ch_3 + Vi; } /// Common code for head and tail sections -static stat_t _exec_aline_ends(float length, float entry_velocity, - float exit_velocity) { +static stat_t _exec_aline_section(float length, float vin, float vout) { if (mr.section_new) { if (fp_ZERO(length)) return STAT_OK; // end the move // len / avg. velocity - mr.ms.move_time = 2 * length / (entry_velocity + exit_velocity); + mr.ms.move_time = 2 * length / (vin + vout); mr.segments = ceil(uSec(mr.ms.move_time) / NOM_SEGMENT_USEC); mr.segment_time = mr.ms.move_time / mr.segments; mr.segment_count = (uint32_t)mr.segments; - _init_forward_diffs(entry_velocity, exit_velocity); + if (vin == vout) mr.segment_velocity = vin; + else mr.segment_velocity = _init_forward_diffs(vin, vout, mr.segments); if (mr.segment_time < MIN_SEGMENT_TIME) return STAT_MINIMUM_TIME_MOVE; // exit /wo advancing position @@ -286,11 +287,12 @@ static stat_t _exec_aline_ends(float length, float entry_velocity, mr.section_new = false; - } else { - mr.segment_velocity += mr.forward_diff[4]; + } else { // Do subsequent segments + if (vin != vout) mr.segment_velocity += mr.forward_diff[4]; if (_exec_aline_segment() == STAT_OK) return STAT_OK; - else { + + if (vin != vout) { mr.forward_diff[4] += mr.forward_diff[3]; mr.forward_diff[3] += mr.forward_diff[2]; mr.forward_diff[2] += mr.forward_diff[1]; @@ -305,37 +307,28 @@ static stat_t _exec_aline_ends(float length, float entry_velocity, /// Callback for tail section static stat_t _exec_aline_tail() { mr.section = SECTION_TAIL; - return _exec_aline_ends(mr.tail_length, mr.cruise_velocity, mr.exit_velocity); + return + _exec_aline_section(mr.tail_length, mr.cruise_velocity, mr.exit_velocity); } -/// Callback for cruise section +/// Callback for body section static stat_t _exec_aline_body() { - if (mr.section_new) { - if (fp_ZERO(mr.body_length)) { - mr.section = SECTION_TAIL; - return _exec_aline_tail(); // skip to tail - } + mr.section = SECTION_BODY; - mr.ms.move_time = mr.body_length / mr.cruise_velocity; - mr.segments = ceil(uSec(mr.ms.move_time) / NOM_SEGMENT_USEC); - mr.segment_time = mr.ms.move_time / mr.segments; - mr.segment_velocity = mr.cruise_velocity; - mr.segment_count = mr.segments; - - if (mr.segment_time < MIN_SEGMENT_TIME) - return STAT_MINIMUM_TIME_MOVE; // exit without advancing position + stat_t status = + _exec_aline_section(mr.body_length, mr.cruise_velocity, mr.cruise_velocity); - mr.section = SECTION_BODY; - mr.section_new = false; - } + if (status == STAT_OK) { + if (mr.section_new) return _exec_aline_tail(); - if (_exec_aline_segment() == STAT_OK) { // OK means this section is done mr.section = SECTION_TAIL; mr.section_new = true; + + return STAT_EAGAIN; } - return STAT_EAGAIN; + return status; } @@ -343,12 +336,15 @@ static stat_t _exec_aline_body() { static stat_t _exec_aline_head() { mr.section = SECTION_HEAD; stat_t status = - _exec_aline_ends(mr.head_length, mr.entry_velocity, mr.cruise_velocity); + _exec_aline_section(mr.head_length, mr.entry_velocity, mr.cruise_velocity); if (status == STAT_OK) { - mr.section = SECTION_BODY; if (mr.section_new) return _exec_aline_body(); + + mr.section = SECTION_BODY; mr.section_new = true; + + return STAT_EAGAIN; } return status; @@ -380,16 +376,16 @@ static stat_t _exec_aline_init(mpBuf_t *bf) { mr.section = SECTION_HEAD; mr.section_new = true; - mr.jerk = bf->jerk; + copy_vector(mr.unit, bf->unit); + copy_vector(mr.final_target, bf->ms.target); + mr.head_length = bf->head_length; mr.body_length = bf->body_length; mr.tail_length = bf->tail_length; mr.entry_velocity = bf->entry_velocity; mr.cruise_velocity = bf->cruise_velocity; mr.exit_velocity = bf->exit_velocity; - - copy_vector(mr.unit, bf->unit); - copy_vector(mr.final_target, bf->ms.target); + mr.jerk = bf->jerk; // Generate waypoints for position correction at section ends for (int axis = 0; axis < AXES; axis++) { @@ -489,7 +485,6 @@ stat_t mp_exec_aline(mpBuf_t *bf) { case SECTION_HEAD: status = _exec_aline_head(); break; case SECTION_BODY: status = _exec_aline_body(); break; case SECTION_TAIL: status = _exec_aline_tail(); break; - default: return CM_ALARM(STAT_INTERNAL_ERROR); // never supposed to get here } mp_state_hold_callback(status == STAT_OK); diff --git a/src/plan/feedhold.c b/src/plan/feedhold.c index 73ae7b1..b9b52ad 100644 --- a/src/plan/feedhold.c +++ b/src/plan/feedhold.c @@ -64,21 +64,18 @@ * before or after motion stops. * * Terms used: - * - mr is the runtime buffer. It was initially loaded from the bf - * buffer + * - mr is the runtime buffer. It was initially loaded from the bf buffer * - bp + 0 is the "companion" bf buffer to the mr buffer. * - bp + 1 is the bf buffer following bp+0. This runs through bp + N - * - bp (by itself) just refers to the current buffer being - * adjusted / replanned + * - bp (by itself) refers to the current buffer being adjusted / replanned * * Details: * Planning re-uses bp + 0 as an "extra" buffer. Normally bp + 0 * is returned to the buffer pool as it is redundant once mr is - * loaded. Use the extra buffer to split the move in two where the - * hold decelerates to zero. Use one buffer to go to zero, the other - * to replan up from zero. All buffers past that point are - * unaffected other than that they need to be replanned for - * velocity. + * loaded. Use the extra buffer to split the move in two where the + * hold decelerates to zero. Use one buffer to go to zero, the other + * to replan up from zero. All buffers past that point are + * unaffected other than that they need to be replanned for velocity. * * Note: * There are multiple opportunities for more efficient @@ -128,8 +125,7 @@ void mp_plan_hold_callback() { // simply be the mr.segment_velocity as this is the velocity of the // last segment, not the one that's going to be executed next. The // braking_velocity needs to be the velocity of the next segment - // that has not yet been computed. In the mean time, this hack will - // work. + // that has not yet been computed. In the mean time, this hack will work. if (mr_available_length < braking_length && fp_ZERO(bp->exit_velocity)) braking_length = mr_available_length; @@ -168,7 +164,7 @@ void mp_plan_hold_callback() { braking_velocity = mr.exit_velocity; // adjust braking velocity downward bp->move_state = MOVE_NEW; // tell _exec to re-use buffer - // a safety to avoid wraparound + // A safety to avoid wraparound for (int i = 0; i < PLANNER_BUFFER_POOL_SIZE; i++) { mp_copy_buffer(bp, bp->nx); // copy bp+1 into bp+0, and onward @@ -197,13 +193,13 @@ void mp_plan_hold_callback() { bp->length = braking_length; bp->exit_vmax = 0; - bp = mp_get_next_buffer(bp); // point to the acceleration buffer + bp = mp_get_next_buffer(bp); // point to the acceleration buffer bp->entry_vmax = 0; - bp->length -= braking_length; // identical buffers and hence their lengths + bp->length -= braking_length; // identical buffers, hence their lengths bp->delta_vmax = mp_get_target_velocity(0, bp->length, bp); bp->exit_vmax = bp->delta_vmax; - _reset_replannable_list(); // replan all the blocks + _reset_replannable_list(); // replan all the blocks mp_plan_block_list(mp_get_last_buffer(), true); mp_set_hold_state(FEEDHOLD_DECEL); // set state to decelerate } diff --git a/src/plan/line.c b/src/plan/line.c index 1aba97b..945fc84 100644 --- a/src/plan/line.c +++ b/src/plan/line.c @@ -42,16 +42,16 @@ #include /// common variables for planning (move master) -typedef struct mpMoveMasterSingleton { +typedef struct { float position[AXES]; // final move position for planning purposes float jerk; // jerk values cached from previous block float recip_jerk; float cbrt_jerk; -} mpMoveMasterSingleton_t; +} mpMoveMaster_t; -mpMoveMasterSingleton_t mm = {{0}}; // context for line planning +mpMoveMaster_t mm = {{0}}; // context for line planning /// Set planner position for a single axis @@ -88,17 +88,22 @@ void mp_set_planner_position(int axis, const float position) { * approximation term, not the TinyG jerk terms) * * If you do the geometry in terms of the known variables, you get: - * sin(theta/2) = R/(R+delta) + * + * sin(theta/2) = R / (R + delta) + * * Re-arranging in terms of circle radius (R) - * R = delta*sin(theta/2)/(1-sin(theta/2). + * + * R = delta * sin(theta/2) / (1 - sin(theta/2)) * * Theta is the angle between line segments given by: - * cos(theta) = dot(a,b)/(norm(a)*norm(b)). + * + * cos(theta) = dot(a, b) / (norm(a) * norm(b)). * * Most of these calculations are already done in the planner. * To remove the acos() and sin() computations, use the trig half * angle identity: - * sin(theta/2) = +/- sqrt((1-cos(theta))/2). + * + * sin(theta/2) = +/-sqrt((1 - cos(theta)) / 2) * * For our applications, this should always be positive. Now just * plug the equations into the centripetal acceleration equation: @@ -106,8 +111,9 @@ void mp_set_planner_position(int axis, const float position) { * computations and no sine/cosines." * * How to compute the radius using brute-force trig: + * * float theta = acos(costheta); - * float radius = delta * sin(theta/2)/(1-sin(theta/2)); + * float radius = delta * sin(theta/2) / (1 - sin(theta/2)); * * This version extends Chamnit's algorithm by computing a value for * delta that takes the contributions of the individual axes in the @@ -125,34 +131,24 @@ void mp_set_planner_position(int axis, const float position) { * * U[i] Unit sum of i'th axis fabs(unit_a[i]) + fabs(unit_b[i]) * Usum Length of sums Ux + Uy - * d Delta of sums (Dx*Ux+DY*UY)/Usum + * d Delta of sums (Dx * Ux + DY * UY) / Usum */ static float _get_junction_vmax(const float a_unit[], const float b_unit[]) { - float costheta = - -a_unit[AXIS_X] * b_unit[AXIS_X] - - a_unit[AXIS_Y] * b_unit[AXIS_Y] - - a_unit[AXIS_Z] * b_unit[AXIS_Z] - - a_unit[AXIS_A] * b_unit[AXIS_A] - - a_unit[AXIS_B] * b_unit[AXIS_B] - - a_unit[AXIS_C] * b_unit[AXIS_C]; + float costheta = 0; + for (int axis = 0; axis < AXES; axis++) + costheta -= a_unit[axis] * b_unit[axis]; if (costheta < -0.99) return 10000000; // straight line cases if (costheta > 0.99) return 0; // reversal cases // Fuse the junction deviations into a vector sum - float a_delta = square(a_unit[AXIS_X] * mach.a[AXIS_X].junction_dev); - a_delta += square(a_unit[AXIS_Y] * mach.a[AXIS_Y].junction_dev); - a_delta += square(a_unit[AXIS_Z] * mach.a[AXIS_Z].junction_dev); - a_delta += square(a_unit[AXIS_A] * mach.a[AXIS_A].junction_dev); - a_delta += square(a_unit[AXIS_B] * mach.a[AXIS_B].junction_dev); - a_delta += square(a_unit[AXIS_C] * mach.a[AXIS_C].junction_dev); - - float b_delta = square(b_unit[AXIS_X] * mach.a[AXIS_X].junction_dev); - b_delta += square(b_unit[AXIS_Y] * mach.a[AXIS_Y].junction_dev); - b_delta += square(b_unit[AXIS_Z] * mach.a[AXIS_Z].junction_dev); - b_delta += square(b_unit[AXIS_A] * mach.a[AXIS_A].junction_dev); - b_delta += square(b_unit[AXIS_B] * mach.a[AXIS_B].junction_dev); - b_delta += square(b_unit[AXIS_C] * mach.a[AXIS_C].junction_dev); + float a_delta = 0; + float b_delta = 0; + + for (int axis = 0; axis < AXES; axis++) { + a_delta += square(a_unit[axis] * mach.a[axis].junction_dev); + b_delta += square(b_unit[axis] * mach.a[axis].junction_dev); + } float delta = (sqrt(a_delta) + sqrt(b_delta)) / 2; float sintheta_over2 = sqrt((1 - costheta) / 2); @@ -326,6 +322,7 @@ stat_t mp_aline(MoveState_t *ms) { float C; // contribution term. C = T * a float maxC = 0; float recip_L2 = 1 / length_square; + int jerk_axis = 0; for (int axis = 0; axis < AXES; axis++) // You cannot use the fp_XXX comparisons here! @@ -337,13 +334,13 @@ stat_t mp_aline(MoveState_t *ms) { if (C > maxC) { maxC = C; - bf->jerk_axis = axis; // also needed for junction vmax calculation + jerk_axis = axis; // also needed for junction vmax calculation } } // set up and pre-compute the jerk terms needed for this round of planning - bf->jerk = mach.a[bf->jerk_axis].jerk_max * JERK_MULTIPLIER / - fabs(bf->unit[bf->jerk_axis]); // scale jerk + bf->jerk = mach.a[jerk_axis].jerk_max * JERK_MULTIPLIER / + fabs(bf->unit[jerk_axis]); // scale jerk // specialized comparison for tolerance of delta if (fabs(bf->jerk - mm.jerk) > JERK_MATCH_PRECISION) { @@ -375,6 +372,7 @@ stat_t mp_aline(MoveState_t *ms) { // before committing the buffer. mp_plan_block_list(bf, false); // replan block list copy_vector(mm.position, bf->ms.target); // set the planner position + // commit current block (must follow the position update) mp_commit_write_buffer(ms->line, MOVE_TYPE_ALINE); diff --git a/src/plan/planner.c b/src/plan/planner.c index 29a8057..fd19618 100644 --- a/src/plan/planner.c +++ b/src/plan/planner.c @@ -320,10 +320,10 @@ void mp_calculate_trapezoid(mpBuf_t *bf) { // velocity you can get given the delta_vmax (maximum velocity slew) // supportable. - bf->naive_move_time = + float naive_move_time = 2 * bf->length / (bf->entry_velocity + bf->exit_velocity); // average - if (bf->naive_move_time < MIN_SEGMENT_TIME_PLUS_MARGIN) { + if (naive_move_time < MIN_SEGMENT_TIME_PLUS_MARGIN) { bf->cruise_velocity = bf->length / MIN_SEGMENT_TIME_PLUS_MARGIN; bf->exit_velocity = max(0.0, min(bf->cruise_velocity, (bf->entry_velocity - bf->delta_vmax))); @@ -337,7 +337,7 @@ void mp_calculate_trapezoid(mpBuf_t *bf) { } // B" case: Block is short, but fits into a single body segment - if (bf->naive_move_time <= NOM_SEGMENT_TIME) { + if (naive_move_time <= NOM_SEGMENT_TIME) { bf->entry_velocity = bf->pv->exit_velocity; if (fp_NOT_ZERO(bf->entry_velocity)) { @@ -532,7 +532,7 @@ void mp_calculate_trapezoid(mpBuf_t *bf) { * planned becomes the effective first block. * * mp_plan_block_list() plans all blocks between and including the - * (effective) first block and the bf. It sets entry, exit and + * (effective) first block and the bf. It sets entry, exit and * cruise v's from vmax's then calls trapezoid generation. * * Variables that must be provided in the mpBuffers that will be processed: @@ -595,21 +595,20 @@ void mp_plan_block_list(mpBuf_t *bf, bool mr_flag) { mpBuf_t *bp = bf; // Backward planning pass. Find first block and update braking velocities. - // At the end *bp points to the buffer before the first block. + // By the end bp points to the buffer before the first block. while ((bp = mp_get_prev_buffer(bp)) != bf) { if (!bp->replannable) break; bp->braking_velocity = min(bp->nx->entry_vmax, bp->nx->braking_velocity) + bp->delta_vmax; } - // Forward planning pass - recomputes trapezoids in the list from the first - // block to the bf block. + // Forward planning pass. Recompute trapezoids from the first block to bf. while ((bp = mp_get_next_buffer(bp)) != bf) { if (bp->pv == bf || mr_flag) { - bp->entry_velocity = bp->entry_vmax; // first block in the list + bp->entry_velocity = bp->entry_vmax; // first block mr_flag = false; - } else bp->entry_velocity = bp->pv->exit_velocity; // other blocks in list + } else bp->entry_velocity = bp->pv->exit_velocity; // other blocks bp->cruise_velocity = bp->cruise_vmax; bp->exit_velocity = min4(bp->exit_vmax, bp->nx->entry_vmax, @@ -618,8 +617,7 @@ void mp_plan_block_list(mpBuf_t *bf, bool mr_flag) { mp_calculate_trapezoid(bp); - // test for optimally planned trapezoids - only need to check various exit - // conditions + // Test for optimally planned trapezoids by checking exit conditions if ((fp_EQ(bp->exit_velocity, bp->exit_vmax) || fp_EQ(bp->exit_velocity, bp->nx->entry_vmax)) || (!bp->pv->replannable && @@ -627,10 +625,11 @@ void mp_plan_block_list(mpBuf_t *bf, bool mr_flag) { bp->replannable = false; } - // finish up the last block move + // Finish last block bp->entry_velocity = bp->pv->exit_velocity; bp->cruise_velocity = bp->cruise_vmax; bp->exit_velocity = 0; + mp_calculate_trapezoid(bp); } diff --git a/src/plan/planner.h b/src/plan/planner.h index 1d76946..e6ae33d 100644 --- a/src/plan/planner.h +++ b/src/plan/planner.h @@ -70,7 +70,6 @@ typedef enum { SECTION_HEAD, // acceleration SECTION_BODY, // cruise SECTION_TAIL, // deceleration - SECTIONS // section count } moveSection_t; @@ -86,7 +85,7 @@ typedef struct mpMoveRuntimeSingleton { // persistent runtime variables /// current move position float position[AXES]; /// head/body/tail endpoints for correction - float waypoint[SECTIONS][AXES]; + float waypoint[3][AXES]; /// current MR target (absolute target as steps) float target_steps[MOTORS]; /// current MR position (target from previous segment) @@ -102,16 +101,15 @@ typedef struct mpMoveRuntimeSingleton { // persistent runtime variables float head_length; float body_length; float tail_length; - float entry_velocity; float cruise_velocity; float exit_velocity; + float jerk; // max linear jerk float segments; // number of segments in line or arc uint32_t segment_count; // count of running segments float segment_velocity; // computed velocity for aline segment float segment_time; // actual time increment per aline segment - float jerk; // max linear jerk float forward_diff[5]; // forward difference levels MoveState_t ms; -- 2.27.0