From 2986e914c14e02376d92488bfabf0d8d0cde1fb9 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Sun, 4 Sep 2016 20:08:25 -0700 Subject: [PATCH] Function renaming, planner reorg and runtime state encapsulation. --- src/homing.c | 7 +- src/machine.c | 18 +- src/motor.c | 5 +- src/plan/command.c | 6 +- src/plan/command.h | 2 +- src/plan/jog.c | 9 +- src/plan/line.c | 4 +- src/plan/planner.c | 93 +-------- src/plan/planner.h | 56 ------ src/plan/{exec.c => runtime.c} | 348 ++++++++++++++++++++++----------- src/plan/{exec.h => runtime.h} | 15 +- src/plan/state.c | 14 +- src/plan/state.h | 1 + src/probing.c | 7 +- src/stepper.c | 8 +- src/stepper.h | 2 +- src/varcb.c | 7 +- 17 files changed, 300 insertions(+), 302 deletions(-) rename src/plan/{exec.c => runtime.c} (64%) rename src/plan/{exec.h => runtime.h} (74%) diff --git a/src/homing.c b/src/homing.c index 5c26baf..d49ebb8 100644 --- a/src/homing.c +++ b/src/homing.c @@ -32,6 +32,7 @@ #include "report.h" #include "plan/planner.h" +#include "plan/runtime.h" #include "plan/state.h" #include @@ -148,7 +149,7 @@ static struct hmHomingSingleton hm = {0}; * the cycle to be done. Otherwise there is a nasty race condition * that will accept the next command before the position of * the final move has been recorded in the Gcode model. That's what the call - * to mach_isbusy() is about. + * to mach_is_busy() is about. */ @@ -232,7 +233,7 @@ static void _homing_axis_set_zero(int8_t axis) { hm.homed[axis] = true; } else // do not set axis if in G28.4 cycle - mach_set_position(axis, mp_get_runtime_work_position(axis)); + mach_set_position(axis, mp_runtime_get_work_position(axis)); mach_set_axis_jerk(axis, hm.saved_jerk); // restore the max jerk value @@ -406,6 +407,6 @@ void mach_homing_cycle_start_no_set() { /// Main loop callback for running the homing cycle void mach_homing_callback() { - if (mp_get_cycle() != CYCLE_HOMING || mp_get_runtime_busy()) return; + if (mp_get_cycle() != CYCLE_HOMING || mp_runtime_is_busy()) return; hm.func(hm.axis); // execute the current homing move } diff --git a/src/machine.c b/src/machine.c index 6fa9bcd..e8787bd 100644 --- a/src/machine.c +++ b/src/machine.c @@ -80,6 +80,7 @@ #include "homing.h" #include "plan/planner.h" +#include "plan/runtime.h" #include "plan/dwell.h" #include "plan/command.h" #include "plan/arc.h" @@ -698,7 +699,7 @@ static void _exec_offset(float *value, float *flag) { offsets[axis] = mach.offset[coord_system][axis] + mach.origin_offset[axis] * (mach.origin_offset_enable ? 1 : 0); - mp_set_runtime_work_offset(offsets); + mp_runtime_set_work_offset(offsets); mach_set_work_offsets(); // set work offsets in the Gcode model } @@ -717,19 +718,16 @@ static void _exec_offset(float *value, float *flag) { * frame than the one you are now going to set. These functions should * only be called during initialization sequences and during cycles * (such as homing cycles) when you know there are no more moves in - * the planner and that all motion has stopped. Use - * mp_get_runtime_busy() to be sure the system is quiescent. + * the planner and that all motion has stopped. */ void mach_set_position(int axis, float position) { - if ((mp_get_state() != STATE_HOLDING && mp_get_state() != STATE_READY) || - mp_get_runtime_busy()) - CM_ALARM(STAT_INTERNAL_ERROR); + if (!mp_is_quiescent()) CM_ALARM(STAT_INTERNAL_ERROR); mach.position[axis] = position; mach.ms.target[axis] = position; mp_set_planner_position(axis, position); - mp_set_runtime_position(axis, position); - mp_set_steps_to_runtime_position(); + mp_runtime_set_position(axis, position); + mp_runtime_set_steps_to_position(); } @@ -765,11 +763,11 @@ void mach_set_absolute_origin(float origin[], float flag[]) { static void _exec_absolute_origin(float *value, float *flag) { for (int axis = 0; axis < AXES; axis++) if (fp_TRUE(flag[axis])) { - mp_set_runtime_position(axis, value[axis]); + mp_runtime_set_position(axis, value[axis]); mach_set_homed(axis, true); // G28.3 is not considered homed until here } - mp_set_steps_to_runtime_position(); + mp_runtime_set_steps_to_position(); } diff --git a/src/motor.c b/src/motor.c index 3e91b0d..97a7e56 100644 --- a/src/motor.c +++ b/src/motor.c @@ -34,8 +34,9 @@ #include "stepper.h" #include "tmc2660.h" #include "estop.h" +#include "util.h" -#include "plan/planner.h" +#include "plan/runtime.h" #include "plan/calibrate.h" #include @@ -158,7 +159,7 @@ ISR(TCE1_CCA_vect) { void motor_init() { // Reset position - mp_set_steps_to_runtime_position(); + mp_runtime_set_steps_to_position(); // Enable DMA DMA.CTRL = DMA_RESET_bm; diff --git a/src/plan/command.c b/src/plan/command.c index 89c8a6a..b043887 100644 --- a/src/plan/command.c +++ b/src/plan/command.c @@ -36,9 +36,9 @@ * - When the planning queue gets to the function it calls _exec_command() * which loads a pointer to the bf buffer in stepper.c's next move. * - When the runtime gets to the end of the current activity (sending steps, - * counting a dwell) it executes mp_runtime_command which uses the callback + * counting a dwell) it executes mp_command_callback which uses the callback * function in the bf and the saved parameters in the vectors. - * - To finish up mp_runtime_command() frees the bf buffer. + * - To finish up mp_command_callback() frees the bf buffer. * * Doing it this way instead of synchronizing on queue empty simplifies the * handling of feedholds, feed overrides, buffer flushes, and thread blocking, @@ -82,7 +82,7 @@ void mp_queue_command(mach_exec_t mach_exec, float *value, float *flag) { } -void mp_runtime_command(mpBuf_t *bf) { +void mp_command_callback(mpBuf_t *bf) { // Use values & flags stored in mp_queue_command() bf->mach_func(bf->ms.target, bf->unit); diff --git a/src/plan/command.h b/src/plan/command.h index 599bf11..a337a30 100644 --- a/src/plan/command.h +++ b/src/plan/command.h @@ -30,4 +30,4 @@ #include "plan/buffer.h" void mp_queue_command(mach_exec_t mach_exec, float *value, float *flag); -void mp_runtime_command(mpBuf_t *bf); +void mp_command_callback(mpBuf_t *bf); diff --git a/src/plan/jog.c b/src/plan/jog.c index 408f8f2..fa3b4b4 100644 --- a/src/plan/jog.c +++ b/src/plan/jog.c @@ -81,7 +81,7 @@ static stat_t _exec_jog(mpBuf_t *bf) { // Compute travel from velocity travel[axis] = time * jr.current_velocity[axis]; - if (travel[axis]) done = false; + if (!fp_ZERO(travel[axis])) done = false; } // Check if we are done @@ -138,10 +138,7 @@ uint8_t command_jog(int argc, char *argv[]) { if (!mp_jog_busy()) { // Should always be at least one free buffer mpBuf_t *bf = mp_get_write_buffer(); - if (!bf) { - CM_ALARM(STAT_BUFFER_FULL_FATAL); - return 0; - } + if (!bf) return STAT_BUFFER_FULL_FATAL; // Start mp_set_cycle(CYCLE_JOGGING); @@ -149,5 +146,5 @@ uint8_t command_jog(int argc, char *argv[]) { mp_commit_write_buffer(-1, MOVE_TYPE_COMMAND); } - return 0; + return STAT_OK; } diff --git a/src/plan/line.c b/src/plan/line.c index e710e73..d80585b 100644 --- a/src/plan/line.c +++ b/src/plan/line.c @@ -30,7 +30,7 @@ #include "line.h" #include "planner.h" -#include "exec.h" +#include "runtime.h" #include "buffer.h" #include "machine.h" #include "stepper.h" @@ -237,7 +237,7 @@ stat_t mp_aline(MoveState_t *ms) { if (!bf) return CM_ALARM(STAT_BUFFER_FULL_FATAL); // never fails // Register callback to exec function - bf->bf_func = mp_exec_aline; + bf->bf_func = mp_runtime_exec_aline; bf->length = length; // Copy model state into planner buffer diff --git a/src/plan/planner.c b/src/plan/planner.c index 4a20248..2fd1cb6 100644 --- a/src/plan/planner.c +++ b/src/plan/planner.c @@ -41,7 +41,7 @@ * * One important concept is isolation of the three layers of the * data model - the Gcode model (gm), planner model (bf queue & mm), - * and runtime model (mr). + * and runtime model (exec.c). * * The Gcode model is owned by the machine and should only be * accessed by mach_xxxx() functions. Data from the Gcode model is @@ -69,9 +69,6 @@ #include -mpMoveRuntimeSingleton_t mr = {0}; // context for line runtime - - void planner_init() { mp_init_buffers(); } @@ -79,7 +76,7 @@ void planner_init() { /*** Flush all moves in the planner * - * Does not affect the move currently running in mr. Does not affect + * Does not affect the move currently running. Does not affect * mm or gm model positions. This function is designed to be called * during a hold to reset the planner. This function should not usually * be directly called. Call mp_request_flush() instead. @@ -89,86 +86,6 @@ void mp_flush_planner() { } -/* Since steps are in motor space you have to run the position vector - * through inverse kinematics to get the right numbers. This means - * that in a non-Cartesian robot changing any position can result in - * changes to multiple step values. So this operation is provided as a - * single function and always uses the new position vector as an - * input. - * - * Keeping track of position is complicated by the fact that moves - * exist in several reference frames. The scheme to keep this - * straight is: - * - * - mm.position - start and end position for planning - * - mr.position - current position of runtime segment - * - mr.ms.target - target position of runtime segment - * - mr.endpoint - final target position of runtime segment - * - * Note that position is set immediately when called and may not be - * not an accurate representation of the tool position. The motors - * are still processing the action and the real tool position is - * still close to the starting point. - */ - - -/// Set runtime position for a single axis -void mp_set_runtime_position(uint8_t axis, const float position) { - mr.position[axis] = position; -} - - -/// Set encoder counts to the runtime position -void mp_set_steps_to_runtime_position() { - float step_position[MOTORS]; - - // convert lengths to steps in floating point - mp_kinematics(mr.position, step_position); - - for (int motor = 0; motor < MOTORS; motor++) { - mr.target_steps[motor] = step_position[motor]; - mr.position_steps[motor] = step_position[motor]; - mr.commanded_steps[motor] = step_position[motor]; - - // write steps to encoder register - motor_set_encoder(motor, step_position[motor]); - - // must be zero - mr.following_error[motor] = 0; - } -} - - -/// Returns current velocity (aggregate) -float mp_get_runtime_velocity() {return mr.segment_velocity;} - - -/// Returns current axis position in machine coordinates -float mp_get_runtime_absolute_position(uint8_t axis) {return mr.position[axis];} - - -/// Set offsets in the MR struct -void mp_set_runtime_work_offset(float offset[]) { - copy_vector(mr.ms.work_offset, offset); -} - - -/// Returns current axis position in work coordinates -/// that were in effect at move planning time -float mp_get_runtime_work_position(uint8_t axis) { - return mr.position[axis] - mr.ms.work_offset[axis]; -} - - -/// Return TRUE if motion control busy (i.e. robot is moving) -/// Use this function to sync to the queue. If you wait until it returns -/// FALSE you know the queue is empty and the motors have stopped. -uint8_t mp_get_runtime_busy() { - if (mp_get_state() == STATE_ESTOPPED) return false; - return st_runtime_isbusy() || mr.active; -} - - /* Performs axis mapping & conversion of length units to steps (and deals * with inhibited axes) * @@ -563,7 +480,7 @@ void mp_calculate_trapezoid(mpBuf_t *bf) { * bf->ms.target[] - block target position * bf->unit[] - block unit vector * bf->time - gets set later - * bf->jerk - source of the other jerk variables. Used in mr. + * bf->jerk - source of the other jerk variables. * * Notes: * @@ -574,8 +491,8 @@ void mp_calculate_trapezoid(mpBuf_t *bf) { * * In normal operation the first block (currently running * block) is not replanned, but may be for feedholds and feed - * overrides. In these cases the prep routines modify the - * contents of the mr buffer and re-shuffle the block list, + * overrides. In these cases the prep routines modify the + * contents of the (ex) buffer and re-shuffle the block list, * re-enlisting the current bf buffer with new parameters. * These routines also set all blocks in the list to be * replannable so the list can be recomputed regardless of diff --git a/src/plan/planner.h b/src/plan/planner.h index 4f91da2..e64308b 100644 --- a/src/plan/planner.h +++ b/src/plan/planner.h @@ -73,66 +73,10 @@ typedef enum { } moveSection_t; -typedef struct mpMoveRuntimeSingleton { // persistent runtime variables - bool active; // True if a move is running - moveSection_t section; // what section is the move in? - bool section_new; // true if it's a new section - - /// unit vector for axis scaling & planning - float unit[AXES]; - /// final target for bf (used to correct rounding errors) - float final_target[AXES]; - /// current move position - float position[AXES]; - /// head/body/tail endpoints for correction - float waypoint[3][AXES]; - /// current MR target (absolute target as steps) - float target_steps[MOTORS]; - /// current MR position (target from previous segment) - float position_steps[MOTORS]; - /// will align with next encoder sample (target 2nd previous segment) - float commanded_steps[MOTORS]; - /// encoder position in steps - ideally the same as commanded_steps - float encoder_steps[MOTORS]; - /// difference between encoder & commanded steps - float following_error[MOTORS]; - - /// copies of bf variables of same name - 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 forward_diff[5]; // forward difference levels - bool hold_planned; // true when a feedhold has been planned - - MoveState_t ms; -} mpMoveRuntimeSingleton_t; - - -// Reference global scope structures -extern mpMoveRuntimeSingleton_t mr; // context for line runtime - - void planner_init(); void mp_flush_planner(); -void mp_set_runtime_position(uint8_t axis, const float position); -void mp_set_steps_to_runtime_position(); -float mp_get_runtime_velocity(); -float mp_get_runtime_work_position(uint8_t axis); -float mp_get_runtime_absolute_position(uint8_t axis); -void mp_set_runtime_work_offset(float offset[]); -uint8_t mp_get_runtime_busy(); void mp_kinematics(const float travel[], float steps[]); void mp_plan_block_list(mpBuf_t *bf); void mp_replan_blocks(); float mp_get_target_length(const float Vi, const float Vf, const mpBuf_t *bf); float mp_get_target_velocity(const float Vi, const float L, const mpBuf_t *bf); -inline int32_t mp_get_line() {return mr.ms.line;} diff --git a/src/plan/exec.c b/src/plan/runtime.c similarity index 64% rename from src/plan/exec.c rename to src/plan/runtime.c index 9a424d8..8459d15 100644 --- a/src/plan/exec.c +++ b/src/plan/runtime.c @@ -42,6 +42,132 @@ #include +typedef struct { + bool active; // True if a move is running + moveSection_t section; // what section is the move in? + bool section_new; // true if it's a new section + + /// unit vector for axis scaling & planning + float unit[AXES]; + /// final target for bf (used to correct rounding errors) + float final_target[AXES]; + /// head/body/tail endpoints for correction + float waypoint[3][AXES]; + /// current target (absolute target as steps) + float target_steps[MOTORS]; + /// current position (target from previous segment) + float position_steps[MOTORS]; + /// will align with next encoder sample (target 2nd previous segment) + float commanded_steps[MOTORS]; + /// encoder position in steps - ideally the same as commanded_steps + float encoder_steps[MOTORS]; + /// difference between encoder & commanded steps + float following_error[MOTORS]; + + /// copies of bf variables of same name + float head_length; + float body_length; + float tail_length; + float entry_velocity; + float cruise_velocity; + float exit_velocity; + + 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 forward_diff[5]; // forward difference levels + bool hold_planned; // true when a feedhold has been planned + + float position[AXES]; // Current move position + MoveState_t ms; // Current move state +} mp_exec_t; + + +mp_exec_t ex = {0}; + + +/// Return TRUE if motion control busy (i.e. robot is moving) +/// Use this function to sync to the queue. If you wait until it returns +/// FALSE you know the queue is empty and the motors have stopped. +bool mp_runtime_is_busy() { + return mp_get_state() != STATE_ESTOPPED && (st_is_busy() || ex.active); +} + + +int32_t mp_runtime_get_line() {return ex.ms.line;} + + +/// Returns current segment velocity +float mp_runtime_get_velocity() {return ex.segment_velocity;} + + +/// Set encoder counts to the runtime position +void mp_runtime_set_steps_to_position() { + float step_position[MOTORS]; + + // Convert lengths to steps in floating point + mp_kinematics(ex.position, step_position); + + for (int motor = 0; motor < MOTORS; motor++) { + ex.target_steps[motor] = ex.position_steps[motor] = + ex.commanded_steps[motor] = step_position[motor]; + + // Write steps to encoder register + motor_set_encoder(motor, step_position[motor]); + + // Must be zero + ex.following_error[motor] = 0; + } +} + + +/* Since steps are in motor space you have to run the position vector + * through inverse kinematics to get the right numbers. This means + * that in a non-Cartesian robot changing any position can result in + * changes to multiple step values. So this operation is provided as a + * single function and always uses the new position vector as an + * input. + * + * Keeping track of position is complicated by the fact that moves + * exist in several reference frames. The scheme to keep this + * straight is: + * + * - mm.position - start and end position for planning + * - ex.position - current position of runtime segment + * - ex.ms.target - target position of runtime segment + * - ex.*_steps + * + * Note that position is set immediately when called and may not be + * not an accurate representation of the tool position. The motors + * are still processing the action and the real tool position is + * still close to the starting point. + */ + + +/// Set runtime position for a single axis +void mp_runtime_set_position(uint8_t axis, const float position) { + ex.position[axis] = position; +} + + +/// Returns current axis position in machine coordinates +float mp_runtime_get_absolute_position(uint8_t axis) {return ex.position[axis];} + + +/// Set offsets +void mp_runtime_set_work_offset(float offset[]) { + copy_vector(ex.ms.work_offset, offset); +} + + +/// Returns current axis position in work coordinates +/// that were in effect at move planning time +float mp_runtime_get_work_position(uint8_t axis) { + return ex.position[axis] - ex.ms.work_offset[axis]; +} + + /*** Segment runner * * Notes on step error correction: @@ -63,55 +189,52 @@ * -100 -90 -10 encoder is 10 steps behind commanded steps */ static stat_t _exec_aline_segment() { - float travel_steps[MOTORS]; - - // Set target position for the segment - // If the segment ends on a section waypoint, synchronize to the - // head, body or tail end. Otherwise, if not at a section waypoint - // compute target from segment time and velocity. Don't do waypoint - // correction if you are going into a hold. - if (!--mr.segment_count && !mr.section_new && mp_get_state() == STATE_RUNNING) - copy_vector(mr.ms.target, mr.waypoint[mr.section]); + // Set target position for the segment. If the segment ends on a section + // waypoint, synchronize to the head, body or tail end. Otherwise, if not at + // a section waypoint compute target from segment time and velocity. Don't + // do waypoint correction if you are going into a hold. + if (!--ex.segment_count && !ex.section_new && mp_get_state() == STATE_RUNNING) + copy_vector(ex.ms.target, ex.waypoint[ex.section]); else { - float segment_length = mr.segment_velocity * mr.segment_time; + float segment_length = ex.segment_velocity * ex.segment_time; for (int i = 0; i < AXES; i++) - mr.ms.target[i] = mr.position[i] + mr.unit[i] * segment_length; + ex.ms.target[i] = ex.position[i] + ex.unit[i] * segment_length; } // Convert target position to steps. Bucket-brigade the old target - // down the chain before getting the new target from kinematics - // - // The direct manipulation of steps to compute travel_steps only - // works for Cartesian kinematics. Other kinematics may require - // transforming travel distance as opposed to simply subtracting - // steps. + // down the chain before getting the new target from kinematics. for (int i = 0; i < MOTORS; i++) { // previous segment's position, delayed by 1 segment - mr.commanded_steps[i] = mr.position_steps[i]; + ex.commanded_steps[i] = ex.position_steps[i]; // previous segment's target becomes position - mr.position_steps[i] = mr.target_steps[i]; + ex.position_steps[i] = ex.target_steps[i]; // current encoder position (aligns to commanded_steps) - mr.encoder_steps[i] = motor_get_encoder(i); - mr.following_error[i] = mr.encoder_steps[i] - mr.commanded_steps[i]; + ex.encoder_steps[i] = motor_get_encoder(i); + ex.following_error[i] = ex.encoder_steps[i] - ex.commanded_steps[i]; } - // now determine the target steps - mp_kinematics(mr.ms.target, mr.target_steps); + // Determine the target steps. + mp_kinematics(ex.ms.target, ex.target_steps); - // and compute the distances, in steps, to be traveled + // Compute distances in steps to be traveled. + // + // The direct manipulation of steps to compute travel_steps only + // works for Cartesian kinematics. Other kinematics may require + // transforming travel distance as opposed to simply subtracting steps. + float travel_steps[MOTORS]; for (int i = 0; i < MOTORS; i++) - travel_steps[i] = mr.target_steps[i] - mr.position_steps[i]; + travel_steps[i] = ex.target_steps[i] - ex.position_steps[i]; // Call the stepper prep function - RITORNO(st_prep_line(travel_steps, mr.following_error, mr.segment_time)); + RITORNO(st_prep_line(travel_steps, ex.following_error, ex.segment_time)); - // update position from target - copy_vector(mr.position, mr.ms.target); + // Update position from target + copy_vector(ex.position, ex.ms.target); - if (!mr.segment_count) return STAT_OK; // this section has run all segments - return STAT_EAGAIN; // this section still has more segments to run + // Return EAGAIN to continue or OK if this segment is done + return ex.segment_count ? STAT_EAGAIN : STAT_OK; } @@ -246,11 +369,11 @@ static float _init_forward_diffs(float Vi, float Vt, float segments) { float Bh_4 = B * h * h * h * h; float Ch_3 = C * h * h * h; - mr.forward_diff[4] = 121.0 / 16.0 * Ah_5 + 5.0 * Bh_4 + 13.0 / 4.0 * Ch_3; - mr.forward_diff[3] = 165.0 / 2.0 * Ah_5 + 29.0 * Bh_4 + 9.0 * Ch_3; - mr.forward_diff[2] = 255.0 * Ah_5 + 48.0 * Bh_4 + 6.0 * Ch_3; - mr.forward_diff[1] = 300.0 * Ah_5 + 24.0 * Bh_4; - mr.forward_diff[0] = 120.0 * Ah_5; + ex.forward_diff[4] = 121.0 / 16.0 * Ah_5 + 5.0 * Bh_4 + 13.0 / 4.0 * Ch_3; + ex.forward_diff[3] = 165.0 / 2.0 * Ah_5 + 29.0 * Bh_4 + 9.0 * Ch_3; + ex.forward_diff[2] = 255.0 * Ah_5 + 48.0 * Bh_4 + 6.0 * Ch_3; + ex.forward_diff[1] = 300.0 * Ah_5 + 24.0 * Bh_4; + ex.forward_diff[0] = 120.0 * Ah_5; // Calculate the initial velocity by calculating V(h / 2) float half_h = h / 2.0; @@ -264,39 +387,39 @@ static float _init_forward_diffs(float Vi, float Vt, float segments) { /// Common code for head and tail sections static stat_t _exec_aline_section(float length, float vin, float vout) { - if (mr.section_new) { + if (ex.section_new) { if (fp_ZERO(length)) return STAT_OK; // end the move // len / avg. 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; + ex.ms.move_time = 2 * length / (vin + vout); + ex.segments = ceil(uSec(ex.ms.move_time) / NOM_SEGMENT_USEC); + ex.segment_time = ex.ms.move_time / ex.segments; + ex.segment_count = (uint32_t)ex.segments; - if (vin == vout) mr.segment_velocity = vin; - else mr.segment_velocity = _init_forward_diffs(vin, vout, mr.segments); + if (vin == vout) ex.segment_velocity = vin; + else ex.segment_velocity = _init_forward_diffs(vin, vout, ex.segments); - if (mr.segment_time < MIN_SEGMENT_TIME) + if (ex.segment_time < MIN_SEGMENT_TIME) return STAT_MINIMUM_TIME_MOVE; // exit /wo advancing position // Do first segment if (_exec_aline_segment() == STAT_OK) { - mr.section_new = false; + ex.section_new = false; return STAT_OK; } - mr.section_new = false; + ex.section_new = false; } else { // Do subsequent segments - if (vin != vout) mr.segment_velocity += mr.forward_diff[4]; + if (vin != vout) ex.segment_velocity += ex.forward_diff[4]; if (_exec_aline_segment() == STAT_OK) return STAT_OK; 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]; - mr.forward_diff[1] += mr.forward_diff[0]; + ex.forward_diff[4] += ex.forward_diff[3]; + ex.forward_diff[3] += ex.forward_diff[2]; + ex.forward_diff[2] += ex.forward_diff[1]; + ex.forward_diff[1] += ex.forward_diff[0]; } } @@ -306,24 +429,24 @@ static stat_t _exec_aline_section(float length, float vin, float vout) { /// Callback for tail section static stat_t _exec_aline_tail() { - mr.section = SECTION_TAIL; + ex.section = SECTION_TAIL; return - _exec_aline_section(mr.tail_length, mr.cruise_velocity, mr.exit_velocity); + _exec_aline_section(ex.tail_length, ex.cruise_velocity, ex.exit_velocity); } /// Callback for body section static stat_t _exec_aline_body() { - mr.section = SECTION_BODY; + ex.section = SECTION_BODY; stat_t status = - _exec_aline_section(mr.body_length, mr.cruise_velocity, mr.cruise_velocity); + _exec_aline_section(ex.body_length, ex.cruise_velocity, ex.cruise_velocity); if (status == STAT_OK) { - if (mr.section_new) return _exec_aline_tail(); + if (ex.section_new) return _exec_aline_tail(); - mr.section = SECTION_TAIL; - mr.section_new = true; + ex.section = SECTION_TAIL; + ex.section_new = true; return STAT_EAGAIN; } @@ -334,15 +457,15 @@ static stat_t _exec_aline_body() { /// Callback for head section static stat_t _exec_aline_head() { - mr.section = SECTION_HEAD; + ex.section = SECTION_HEAD; stat_t status = - _exec_aline_section(mr.head_length, mr.entry_velocity, mr.cruise_velocity); + _exec_aline_section(ex.head_length, ex.entry_velocity, ex.cruise_velocity); if (status == STAT_OK) { - if (mr.section_new) return _exec_aline_body(); + if (ex.section_new) return _exec_aline_body(); - mr.section = SECTION_BODY; - mr.section_new = true; + ex.section = SECTION_BODY; + ex.section_new = true; return STAT_EAGAIN; } @@ -352,8 +475,8 @@ static stat_t _exec_aline_head() { static float _compute_next_segment_velocity() { - if (mr.section == SECTION_BODY) return mr.segment_velocity; - return mr.segment_velocity + mr.forward_diff[4]; + if (ex.section == SECTION_BODY) return ex.segment_velocity; + return ex.segment_velocity + ex.forward_diff[4]; } @@ -373,8 +496,8 @@ static void _plan_hold() { mpBuf_t *bp = mp_get_run_buffer(); // working buffer pointer if (!bp) return; // Oops! nothing's running - // Examine and process mr buffer and compute length left for decel - float available_length = get_axis_vector_length(mr.final_target, mr.position); + // Examine and process current buffer and compute length left for decel + float available_length = get_axis_vector_length(ex.final_target, ex.position); // Compute next_segment velocity, velocity left to shed to brake to zero float braking_velocity = _compute_next_segment_velocity(); // Distance to brake to zero from braking_velocity, bp is OK to use here @@ -382,24 +505,24 @@ static void _plan_hold() { // Hack to prevent Case 2 moves for perfect-fit decels. Happens in // homing situations. The real fix: The braking velocity cannot - // simply be the mr.segment_velocity as this is the velocity of the + // simply be the ex.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. if (available_length < braking_length && fp_ZERO(bp->exit_velocity)) braking_length = available_length; - // Replan mr to decelerate - mr.section = SECTION_TAIL; - mr.section_new = true; - mr.cruise_velocity = braking_velocity; - mr.hold_planned = true; + // Replan to decelerate + ex.section = SECTION_TAIL; + ex.section_new = true; + ex.cruise_velocity = braking_velocity; + ex.hold_planned = true; - // Case 1: deceleration fits entirely into the length remaining in mr buffer + // Case 1: deceleration fits entirely into the length remaining in buffer if (braking_length <= available_length) { - // set mr to a tail to perform the deceleration - mr.exit_velocity = 0; - mr.tail_length = braking_length; + // Set to a tail to perform the deceleration + ex.exit_velocity = 0; + ex.tail_length = braking_length; // Re-use bp+0 to be the hold point and to run the remaining block length bp->length = available_length - braking_length; @@ -407,10 +530,10 @@ static void _plan_hold() { bp->entry_vmax = 0; // set bp+0 as hold point bp->move_state = MOVE_NEW; // tell _exec to re-use the bf buffer - } else { // Case 2: deceleration exceeds length remaining in mr buffer - // Replan mr to minimum (but non-zero) exit velocity - mr.tail_length = available_length; - mr.exit_velocity = + } else { // Case 2: deceleration exceeds length remaining in buffer + // Replan to minimum (but non-zero) exit velocity + ex.tail_length = available_length; + ex.exit_velocity = braking_velocity - mp_get_target_velocity(0, available_length, bp); } } @@ -419,13 +542,13 @@ static void _plan_hold() { /// Initializes move runtime with a new planner buffer static stat_t _exec_aline_init(mpBuf_t *bf) { // copy in the gcode model state - memcpy(&mr.ms, &bf->ms, sizeof(MoveState_t)); + memcpy(&ex.ms, &bf->ms, sizeof(MoveState_t)); bf->replannable = false; report_request(); // Executing line number has changed // Remove zero length lines. Short lines have already been removed. if (fp_ZERO(bf->length)) { - mr.active = false; // reset mr buffer + ex.active = false; // reset buffer bf->nx->replannable = false; // prevent overplanning (Note 2) mp_free_run_buffer(); // free buffer @@ -434,32 +557,31 @@ static stat_t _exec_aline_init(mpBuf_t *bf) { // Initialize move runtime bf->move_state = MOVE_RUN; - mr.active = true; - mr.section = SECTION_HEAD; - mr.section_new = true; - mr.hold_planned = false; - - 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; - mr.jerk = bf->jerk; + ex.active = true; + ex.section = SECTION_HEAD; + ex.section_new = true; + ex.hold_planned = false; + + copy_vector(ex.unit, bf->unit); + copy_vector(ex.final_target, bf->ms.target); + + ex.head_length = bf->head_length; + ex.body_length = bf->body_length; + ex.tail_length = bf->tail_length; + ex.entry_velocity = bf->entry_velocity; + ex.cruise_velocity = bf->cruise_velocity; + ex.exit_velocity = bf->exit_velocity; // Generate waypoints for position correction at section ends. This helps // negate floating point errors in the forward differencing code. for (int axis = 0; axis < AXES; axis++) { - mr.waypoint[SECTION_HEAD][axis] = - mr.position[axis] + mr.unit[axis] * mr.head_length; + ex.waypoint[SECTION_HEAD][axis] = + ex.position[axis] + ex.unit[axis] * ex.head_length; - mr.waypoint[SECTION_BODY][axis] = - mr.position[axis] + mr.unit[axis] * (mr.head_length + mr.body_length); + ex.waypoint[SECTION_BODY][axis] = + ex.position[axis] + ex.unit[axis] * (ex.head_length + ex.body_length); - mr.waypoint[SECTION_TAIL][axis] = mr.final_target[axis]; + ex.waypoint[SECTION_TAIL][axis] = ex.final_target[axis]; } return STAT_OK; @@ -530,20 +652,20 @@ static stat_t _exec_aline_init(mpBuf_t *bf) { * from _NEW to _RUN on first call (sub_state set to _OFF) * from _RUN to _OFF on final call or just remain _OFF */ -stat_t mp_exec_aline(mpBuf_t *bf) { +stat_t mp_runtime_exec_aline(mpBuf_t *bf) { if (bf->move_state == MOVE_OFF) return STAT_NOOP; // No more moves stat_t status = STAT_OK; // Start a new move - if (!mr.active) { + if (!ex.active) { status = _exec_aline_init(bf); if (status != STAT_OK) return status; } // Main segment dispatch. From this point on the contents of the bf buffer // do not affect execution. - switch (mr.section) { + switch (ex.section) { case SECTION_HEAD: status = _exec_aline_head(); break; case SECTION_BODY: status = _exec_aline_body(); break; case SECTION_TAIL: status = _exec_aline_tail(); break; @@ -553,14 +675,14 @@ stat_t mp_exec_aline(mpBuf_t *bf) { // // status bf->move_state Description // ----------- -------------- ---------------------------------- - // STAT_EAGAIN mr buffer has more segments to run - // STAT_OK MOVE_RUN mr and bf buffers are done - // STAT_OK MOVE_NEW mr done; bf must be run again + // STAT_EAGAIN buffer has more segments to run + // STAT_OK MOVE_RUN ex and bf buffers are done + // STAT_OK MOVE_NEW ex done; bf must be run again // (it's been reused) if (status != STAT_EAGAIN) { - mr.active = false; // reset mr buffer + ex.active = false; // reset buffer bf->nx->replannable = false; // prevent overplanning (Note 2) - if (fp_ZERO(mr.exit_velocity)) mr.segment_velocity = 0; + if (fp_ZERO(ex.exit_velocity)) ex.segment_velocity = 0; // Note, _plan_hold() may change bf->move_state to reuse this buffer so it // can plan the deceleration. if (bf->move_state == MOVE_RUN) mp_free_run_buffer(); @@ -568,8 +690,8 @@ stat_t mp_exec_aline(mpBuf_t *bf) { if (mp_get_state() == STATE_STOPPING && (status == STAT_OK || status == STAT_EAGAIN)) { - if (!mr.active && !mr.segment_velocity) mp_state_holding(); - else if (mr.active && !mr.hold_planned) _plan_hold(); + if (!ex.active && !ex.segment_velocity) mp_state_holding(); + else if (ex.active && !ex.hold_planned) _plan_hold(); } return status; @@ -577,7 +699,7 @@ stat_t mp_exec_aline(mpBuf_t *bf) { /// Dequeues buffer and executes move callback -stat_t mp_exec_move() { +stat_t mp_runtime_exec_move() { if (mp_get_state() == STATE_ESTOPPED) return STAT_MACHINE_ALARMED; if (mp_get_state() == STATE_HOLDING) return STAT_NOOP; diff --git a/src/plan/exec.h b/src/plan/runtime.h similarity index 74% rename from src/plan/exec.h rename to src/plan/runtime.h index c73bb1c..f726dc1 100644 --- a/src/plan/exec.h +++ b/src/plan/runtime.h @@ -29,5 +29,16 @@ #include "buffer.h" -stat_t mp_exec_move(); -stat_t mp_exec_aline(mpBuf_t *bf); +#include + + +bool mp_runtime_is_busy(); +int32_t mp_runtime_get_line(); +float mp_runtime_get_velocity(); +void mp_runtime_set_steps_to_position(); +void mp_runtime_set_position(uint8_t axis, const float position); +float mp_runtime_get_work_position(uint8_t axis); +float mp_runtime_get_absolute_position(uint8_t axis); +void mp_runtime_set_work_offset(float offset[]); +stat_t mp_runtime_exec_move(); +stat_t mp_runtime_exec_aline(mpBuf_t *bf); diff --git a/src/plan/state.c b/src/plan/state.c index 0b5252a..003f298 100644 --- a/src/plan/state.c +++ b/src/plan/state.c @@ -30,6 +30,7 @@ #include "state.h" #include "machine.h" #include "planner.h" +#include "runtime.h" #include "buffer.h" #include "arc.h" @@ -117,6 +118,12 @@ bool mp_is_flushing() {return ps.flush_requested && !ps.resume_requested;} bool mp_is_resuming() {return ps.resume_requested;} +bool mp_is_quiescent() { + return (mp_get_state() == STATE_READY || mp_get_state() == STATE_HOLDING) && + !mp_runtime_is_busy(); +} + + void mp_state_holding() {_set_state(STATE_HOLDING);} @@ -168,10 +175,7 @@ void mp_state_callback() { } // Only flush queue when idle or holding. - if (ps.flush_requested && - (mp_get_state() == STATE_READY || mp_get_state() == STATE_HOLDING) && - !mp_get_runtime_busy()) { - + if (ps.flush_requested && mp_is_quiescent()) { mach_abort_arc(); if (!mp_queue_empty()) { @@ -181,7 +185,7 @@ void mp_state_callback() { // Reset to actual machine position. Otherwise machine is set to the // position of the last queued move. for (int axis = 0; axis < AXES; axis++) - mach_set_position(axis, mp_get_runtime_absolute_position(axis)); + mach_set_position(axis, mp_runtime_get_absolute_position(axis)); } // Resume diff --git a/src/plan/state.h b/src/plan/state.h index f5e8cf6..a8ca4a9 100644 --- a/src/plan/state.h +++ b/src/plan/state.h @@ -62,6 +62,7 @@ PGM_P mp_get_cycle_pgmstr(plannerCycle_t cycle); bool mp_is_flushing(); bool mp_is_resuming(); +bool mp_is_quiescent(); void mp_state_holding(); void mp_state_running(); diff --git a/src/probing.c b/src/probing.c index f372155..3fe2bcd 100644 --- a/src/probing.c +++ b/src/probing.c @@ -32,6 +32,7 @@ #include "util.h" #include "plan/planner.h" +#include "plan/runtime.h" #include "plan/state.h" #include @@ -81,7 +82,7 @@ static struct pbProbingSingleton pb = {0}; * before declaring the cycle to be done. Otherwise there is a nasty * race condition in the tg_controller() that will accept the next * command before the position of the final move has been recorded in - * the Gcode model. That's what the call to mp_get_runtime_busy() is + * the Gcode model. That's what the call to mp_runtime_is_busy() is * about. */ @@ -116,7 +117,7 @@ static void _probing_finish() { for (int axis = 0; axis < AXES; axis++) { // if we got here because of a feed hold keep the model position correct - mach_set_position(axis, mp_get_runtime_work_position(axis)); + mach_set_position(axis, mp_runtime_get_work_position(axis)); // store the probe results pb.results[axis] = mach_get_absolute_position(axis); @@ -219,7 +220,7 @@ stat_t mach_straight_probe(float target[], float flags[]) { /// main loop callback for running the homing cycle void mach_probe_callback() { // sync to planner move ends - if (!mach_is_probing() || mp_get_runtime_busy()) return; + if (!mach_is_probing() || mp_runtime_is_busy()) return; pb.func(); // execute the current homing move } diff --git a/src/stepper.c b/src/stepper.c index 8518ea0..a773939 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -31,7 +31,7 @@ #include "config.h" #include "machine.h" -#include "plan/exec.h" +#include "plan/runtime.h" #include "plan/command.h" #include "motor.h" #include "hardware.h" @@ -80,13 +80,13 @@ void st_shutdown() { /// Return true if motors or dwell are running -uint8_t st_runtime_isbusy() {return st.busy;} +uint8_t st_is_busy() {return st.busy;} /// Interrupt handler for calling move exec function. /// ADC channel 0 triggered by load ISR as a "software" interrupt. ISR(ADCB_CH0_vect) { - mp_exec_move(); + mp_runtime_exec_move(); ADCB_CH0_INTCTRL = 0; st.requesting = false; } @@ -142,7 +142,7 @@ ISR(STEP_TIMER_ISR) { st.dwell = st.prep_dwell; } else if (st.move_type == MOVE_TYPE_COMMAND) - mp_runtime_command(st.bf); // Execute command + mp_command_callback(st.bf); // Execute command // We are done with this move st.move_type = MOVE_TYPE_NULL; diff --git a/src/stepper.h b/src/stepper.h index a77ca07..ec9c37b 100644 --- a/src/stepper.h +++ b/src/stepper.h @@ -37,7 +37,7 @@ void stepper_init(); void st_shutdown(); -uint8_t st_runtime_isbusy(); +uint8_t st_is_busy(); stat_t st_prep_line(float travel_steps[], float following_error[], float segment_time); void st_prep_command(mpBuf_t *bf); diff --git a/src/varcb.c b/src/varcb.c index 4d4a136..8876c90 100644 --- a/src/varcb.c +++ b/src/varcb.c @@ -28,20 +28,21 @@ #include "usart.h" #include "machine.h" +#include "plan/runtime.h" #include "plan/planner.h" #include "plan/state.h" #include "plan/buffer.h" -float get_position(int index) {return mp_get_runtime_absolute_position(index);} -float get_velocity() {return mp_get_runtime_velocity();} +float get_position(int index) {return mp_runtime_get_absolute_position(index);} +float get_velocity() {return mp_runtime_get_velocity();} float get_speed() {return mach_get_spindle_speed();} float get_feed() {return mach_get_feed_rate();} uint8_t get_tool() {return mach_get_tool();} bool get_echo() {return usart_is_set(USART_ECHO);} void set_echo(bool value) {return usart_set(USART_ECHO, value);} uint16_t get_queue() {return mp_get_planner_buffer_room();} -int32_t get_line() {return mp_get_line();} +int32_t get_line() {return mp_runtime_get_line();} PGM_P get_state() {return mp_get_state_pgmstr(mp_get_state());} PGM_P get_cycle() {return mp_get_cycle_pgmstr(mp_get_cycle());} PGM_P get_unit() {return mp_get_units_mode_pgmstr(mach_get_units_mode());} -- 2.27.0