From bdade7b13c8a71272dea973b8ace00d635ef7702 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Sat, 27 Aug 2016 22:50:46 -0700 Subject: [PATCH] Modularized homing and probing code --- src/homing.c | 34 +++++++++++++++++++----- src/homing.h | 2 ++ src/machine.c | 15 +++++------ src/machine.h | 33 +++-------------------- src/plan/arc.c | 6 ++--- src/plan/line.c | 65 +++++++++++++++++++++++++++------------------- src/plan/line.h | 4 +-- src/plan/planner.h | 1 + src/probing.c | 27 +++++++++++++------ src/varcb.c | 2 +- 10 files changed, 103 insertions(+), 86 deletions(-) diff --git a/src/homing.c b/src/homing.c index 809d045..30e55da 100644 --- a/src/homing.c +++ b/src/homing.c @@ -46,8 +46,18 @@ typedef void (*homing_func_t)(int8_t axis); static void _homing_axis_start(int8_t axis); +typedef enum { // applies to mach.homing_state + HOMING_NOT_HOMED, // machine is not homed + HOMING_HOMED, // machine is homed + HOMING_WAITING, // machine waiting to be homed +} homingState_t; + + /// persistent homing runtime variables struct hmHomingSingleton { + homingState_t state; // homing cycle sub-state machine + bool homed[AXES]; // individual axis homing flags + // controls for homing cycle int8_t axis; // axis currently being homed @@ -82,7 +92,9 @@ struct hmHomingSingleton { float saved_feed_rate; // F setting float saved_jerk; // saved and restored for each axis homed }; -static struct hmHomingSingleton hm; + + +static struct hmHomingSingleton hm = {0,}; // G28.2 homing cycle @@ -217,7 +229,7 @@ static void _homing_abort(int8_t axis) { static void _homing_axis_set_zero(int8_t axis) { if (hm.set_coordinates) { mach_set_position(axis, 0); - mach.homed[axis] = true; + hm.homed[axis] = true; } else // do not set axis if in G28.4 cycle mach_set_position(axis, mp_get_runtime_work_position(axis)); @@ -277,7 +289,7 @@ static void _homing_axis_start(int8_t axis) { // get the first or next axis if ((axis = _get_next_axis(axis)) < 0) { // axes are done or error if (axis == -1) { // -1 is done - mach.homing_state = HOMING_HOMED; + hm.state = HOMING_HOMED; _homing_finalize_exit(); return; @@ -286,7 +298,7 @@ static void _homing_axis_start(int8_t axis) { } // clear the homed flag for axis to move w/o triggering soft limits - mach.homed[axis] = false; + hm.homed[axis] = false; // trap axis mis-configurations if (fp_ZERO(mach.a[axis].search_velocity)) @@ -349,7 +361,17 @@ bool mach_is_homing() { void mach_set_not_homed() { // TODO save homed to EEPROM for (int axis = 0; axis < AXES; axis++) - mach.homed[axis] = false; + hm.homed[axis] = false; +} + + +bool mach_get_homed(int axis) { + return hm.homed[axis]; +} + + +void mach_set_homed(int axis, bool homed) { + hm.homed[axis] = homed; } @@ -372,7 +394,7 @@ void mach_homing_cycle_start() { hm.axis = -1; // set to retrieve initial axis hm.func = _homing_axis_start; // bind initial processing function mp_set_cycle(CYCLE_HOMING); - mach.homing_state = HOMING_NOT_HOMED; + hm.state = HOMING_NOT_HOMED; } diff --git a/src/homing.h b/src/homing.h index 33355ea..f4798f1 100644 --- a/src/homing.h +++ b/src/homing.h @@ -32,6 +32,8 @@ bool mach_is_homing(); void mach_set_not_homed(); +bool mach_get_homed(int axis); +void mach_set_homed(int axis, bool homed); void mach_homing_cycle_start(); void mach_homing_cycle_start_no_set(); void mach_homing_callback(); diff --git a/src/machine.c b/src/machine.c index 5c5c95b..2d5846a 100644 --- a/src/machine.c +++ b/src/machine.c @@ -78,6 +78,7 @@ #include "usart.h" // for serial queue flush #include "estop.h" #include "report.h" +#include "homing.h" #include "plan/planner.h" #include "plan/buffer.h" @@ -93,6 +94,9 @@ #include +#define DISABLE_SOFT_LIMIT -1000000 + + machine_t mach = { // Offsets .offset = { @@ -200,7 +204,6 @@ static void _exec_absolute_origin(float *value, float *flag); // Machine State functions uint32_t mach_get_line() {return mach.gm.line;} -machHomingState_t mach_get_homing_state() {return mach.homing_state;} machMotionMode_t mach_get_motion_mode() {return mach.gm.motion_mode;} machCoordSystem_t mach_get_coord_system() {return mach.gm.coord_system;} machUnitsMode_t mach_get_units_mode() {return mach.gm.units_mode;} @@ -493,12 +496,6 @@ void mach_calc_move_time(const float axis_length[], const float axis_square[]) { } -/// Set endpoint position from final runtime position -void mach_update_model_position_from_runtime() { - copy_vector(mach.position, mr.ms.target); -} - - /* Set target vector in GM model * * This is a core routine. It handles: @@ -578,7 +575,7 @@ void mach_set_model_target(float target[], float flag[]) { stat_t mach_test_soft_limits(float target[]) { #ifdef SOFT_LIMIT_ENABLE for (int axis = 0; axis < AXES; axis++) { - if (!mach.homed[axis]) continue; // don't test axes that are not homed + if (!mach_get_homed(axis)) continue; // don't test axes that arent homed if (fp_EQ(mach.a[axis].travel_min, mach.a[axis].travel_max)) continue; @@ -758,7 +755,7 @@ 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]); - mach.homed[axis] = true; // G28.3 is not considered homed until here + mach_set_homed(axis, true); // G28.3 is not considered homed until here } mp_set_steps_to_runtime_position(); diff --git a/src/machine.h b/src/machine.h index b8a3800..4be3355 100644 --- a/src/machine.h +++ b/src/machine.h @@ -38,23 +38,6 @@ #define TO_MILLIMETERS(a) (mach.gm.units_mode == INCHES ? (a) * MM_PER_INCH : a) -#define DISABLE_SOFT_LIMIT -1000000 - - - -typedef enum { // applies to mach.homing_state - HOMING_NOT_HOMED, // machine is not homed - HOMING_HOMED, // machine is homed - HOMING_WAITING, // machine waiting to be homed -} machHomingState_t; - - -typedef enum { // applies to mach.probe_state - PROBE_FAILED, // probe reached endpoint without triggering - PROBE_SUCCEEDED, // probe was triggered, mach.probe_results has position - PROBE_WAITING, // probe is waiting to be started -} machProbeState_t; - /* The difference between machNextAction_t and machMotionMode_ is that * machNextAction_t is used by the current block, and may carry non-modal @@ -324,14 +307,7 @@ typedef struct { // struct to manage mach globals and cycles float g28_position[AXES]; // stored machine position for G28 float g30_position[AXES]; // stored machine position for G30 - // settings for axes X,Y,Z,A B,C - AxisConfig_t a[AXES]; - - machHomingState_t homing_state; // home: homing cycle sub-state machine - bool homed[AXES]; // individual axis homing flags - - machProbeState_t probe_state; - float probe_results[AXES]; // probing results + AxisConfig_t a[AXES]; // settings for axes // Model states MoveState_t ms; @@ -346,7 +322,6 @@ extern machine_t mach; // machine controller singleton // Model state getters and setters uint32_t mach_get_line(); -machHomingState_t mach_get_homing_state(); machMotionMode_t mach_get_motion_mode(); machCoordSystem_t mach_get_coord_system(); machUnitsMode_t mach_get_units_mode(); @@ -378,7 +353,6 @@ float mach_get_work_position(uint8_t axis); // Critical helpers void mach_calc_move_time(const float axis_length[], const float axis_square[]); -void mach_update_model_position_from_runtime(); void mach_finalize_move(); stat_t mach_deferred_write_callback(); void mach_set_model_target(float target[], float flag[]); @@ -425,9 +399,8 @@ void mach_set_path_control(machPathControlMode_t mode); // Machining Functions (4.3.6) stat_t mach_straight_feed(float target[], float flags[]); -stat_t mach_arc_feed(float target[], float flags[], - float i, float j, float k, - float radius, uint8_t motion_mode); +stat_t mach_arc_feed(float target[], float flags[], float i, float j, float k, + float radius, uint8_t motion_mode); stat_t mach_dwell(float seconds); // Spindle Functions (4.3.7) see spindle.h diff --git a/src/plan/arc.c b/src/plan/arc.c index 2d8ef61..c4c6bfc 100644 --- a/src/plan/arc.c +++ b/src/plan/arc.c @@ -362,9 +362,9 @@ static stat_t _compute_arc() { * approximated by generating a large number of tiny, linear arc_segments. */ stat_t mach_arc_feed(float target[], float flags[], // arc endpoints - float i, float j, float k, // raw arc offsets - float radius, // non-zero radius implies radius mode - uint8_t motion_mode) { // defined motion mode + float i, float j, float k, // raw arc offsets + float radius, // non-zero radius implies radius mode + uint8_t motion_mode) { // defined motion mode // Set axis plane and trap arc specification errors // trap missing feed rate diff --git a/src/plan/line.c b/src/plan/line.c index 120be0e..44130ba 100644 --- a/src/plan/line.c +++ b/src/plan/line.c @@ -55,7 +55,7 @@ mpMoveMasterSingleton_t mm = {}; // context for line planning /// Set planner position for a single axis -void mp_set_planner_position(uint8_t axis, const float position) { +void mp_set_planner_position(int axis, const float position) { mm.position[axis] = position; } @@ -182,10 +182,6 @@ static float _get_junction_vmax(const float a_unit[], const float b_unit[]) { * the minimums. */ stat_t mp_aline(MoveState_t *ms) { - float exact_stop = 0; // preset this value OFF - float junction_velocity; - uint8_t mr_flag = false; - // Compute some reusable terms float axis_length[AXES]; float axis_square[AXES]; @@ -207,13 +203,15 @@ stat_t mp_aline(MoveState_t *ms) { // minimum move time get a more accurate time estimate based on // starting velocity and acceleration. The time of the move is // determined by its initial velocity (Vi) and how much acceleration - // will be occur. For this we need to look at the exit velocity of + // will occur. For this we need to look at the exit velocity of // the previous block. There are 3 possible cases: // // (1) There is no previous block. // Vi = 0 + // // (2) Previous block is optimally planned. // Vi = previous block's exit_velocity + // // (3) Previous block is not optimally planned. // Vi <= previous block's entry_velocity + delta_velocity @@ -258,19 +256,23 @@ stat_t mp_aline(MoveState_t *ms) { // // The math for time-to-decelerate T from speed S to speed E with constant // jerk J is: - // T = 2 * sqrt((S - E) / J[n]) + // + // T = 2 * sqrt((S - E) / J) // // Since E is always zero in this calculation, we can simplify: - // T = 2 * sqrt(S / J[n]) + // + // T = 2 * sqrt(S / J) // // The math for distance-to-decelerate l from speed S to speed E with // constant jerk J is: + // // l = (S + E) * sqrt((S - E) / J) // // Since E is always zero in this calculation, we can simplify: + // // l = S * sqrt(S / J) // - // The final value we want is to know which one is *longest*, + // The final value we want to know is which one is *longest*, // compared to the others, so we don't need the actual value. This // means that the scale of the value doesn't matter, so for T we // can remove the "2 *" and For L we can remove the "S*". This @@ -281,7 +283,7 @@ stat_t mp_aline(MoveState_t *ms) { // However, we *do* need to compensate for the fact that each axis // contributes differently to the move, so we will scale each // contribution C[n] by the proportion of the axis movement length - // D[n] to the total length of the move L. 0Using that, we + // D[n] to the total length of the move L. Using that, we // construct the following, with these definitions: // // J[n] = max_jerk for axis n @@ -289,23 +291,30 @@ stat_t mp_aline(MoveState_t *ms) { // L = total length of the move in all axes // C[n] = "axis contribution" of axis n // - // For each axis n: C[n] = sqrt(1 / J[n]) * (D[n] / L) + // For each axis n: // - // Keeping in mind that we only need a rank compared to the other - // axes, we can further optimize the calculations:: + // C[n] = sqrt(1 / J[n]) * (D[n] / L) // - // Square the expression to remove the square root: - // C[n]^2 = (1 / J[n]) * (D[n] / L)^2 (We don't care the C is squared, - // we'll use it that way.) + // Keeping in mind that we only need a rank compared to the other + // axes, we can further optimize the calculations: // - // Re-arranged to optimize divides for pre-calculated values, - // we create a value M that we compute once: - // M = 1 / L^2 - // Then we use it in the calculations for every axis: - // C[n] = 1 / J[n] * D[n]^2 * M + // Square the expression to remove the square root: // - // Also note that we already have 1 / J[n] calculated for each axis, - // which simplifies it further. + // C[n]^2 = (1 / J[n]) * (D[n] / L)^2 + // + // We don't care the C is squared, so we'll use it that way. + // + // Re-arranged to optimize divides for pre-calculated values, + // we create a value M that we compute once: + // + // M = 1 / L^2 + // + // Then we use it in the calculations for every axis: + // + // C[n] = 1 / J[n] * D[n]^2 * M + // + // Also note that we already have 1 / J[n] calculated for each axis, + // which simplifies it further. // // Finally, the selected jerk term needs to be scaled by the // reciprocal of the absolute value of the jerk-limit axis's unit @@ -347,21 +356,23 @@ stat_t mp_aline(MoveState_t *ms) { // finish up the current block variables // exact stop cases already zeroed + float exact_stop = 0; if (mach_get_path_control() != PATH_EXACT_STOP) { bf->replannable = true; exact_stop = 8675309; // an arbitrarily large floating point number } + float junction_velocity = _get_junction_vmax(bf->pv->unit, bf->unit); bf->cruise_vmax = bf->length / bf->ms.move_time; // target velocity requested - junction_velocity = _get_junction_vmax(bf->pv->unit, bf->unit); bf->entry_vmax = min3(bf->cruise_vmax, junction_velocity, exact_stop); bf->delta_vmax = mp_get_target_velocity(0, bf->length, bf); - bf->exit_vmax = min3(bf->cruise_vmax, (bf->entry_vmax + bf->delta_vmax), - exact_stop); + bf->exit_vmax = + min3(bf->cruise_vmax, (bf->entry_vmax + bf->delta_vmax), exact_stop); bf->braking_velocity = bf->delta_vmax; - // Note: these next lines must remain in exact order. Position must update + // NOTE: these next lines must remain in exact order. Position must update // before committing the buffer. + uint8_t mr_flag = false; mp_plan_block_list(bf, &mr_flag); // replan block list copy_vector(mm.position, bf->ms.target); // set the planner position // commit current block (must follow the position update) diff --git a/src/plan/line.h b/src/plan/line.h index 467e963..d6b008b 100644 --- a/src/plan/line.h +++ b/src/plan/line.h @@ -27,7 +27,7 @@ #pragma once -#include "buffer.h" +#include "machine.h" -void mp_set_planner_position(uint8_t axis, const float position); +void mp_set_planner_position(int axis, const float position); stat_t mp_aline(MoveState_t *ms); diff --git a/src/plan/planner.h b/src/plan/planner.h index ed895aa..fe3f37a 100644 --- a/src/plan/planner.h +++ b/src/plan/planner.h @@ -148,3 +148,4 @@ void mp_kinematics(const float travel[], float steps[]); void mp_plan_block_list(mpBuf_t *bf, uint8_t *mr_flag); 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/probing.c b/src/probing.c index 0ac1f3d..e035e39 100644 --- a/src/probing.c +++ b/src/probing.c @@ -45,7 +45,17 @@ #define MINIMUM_PROBE_TRAVEL 0.254 +typedef enum { // applies to mach.probe_state + PROBE_FAILED, // probe reached endpoint without triggering + PROBE_SUCCEEDED, // probe was triggered, pb.results has position + PROBE_WAITING, // probe is waiting to be started +} probeState_t; + + struct pbProbingSingleton { // persistent probing runtime variables + probeState_t state; + float results[AXES]; // probing results + void (*func)(); // binding for callback function state machine // state saved from gcode model @@ -59,7 +69,8 @@ struct pbProbingSingleton { // persistent probing runtime variables float flags[AXES]; }; -static struct pbProbingSingleton pb; + +static struct pbProbingSingleton pb = {0,}; /* Note: When coding a cycle (like this one) you get to perform one @@ -101,14 +112,14 @@ static void _probing_error_exit(stat_t status) { static void _probing_finish() { bool closed = switch_is_active(SW_PROBE); - mach.probe_state = closed ? PROBE_SUCCEEDED : PROBE_FAILED; + pb.state = closed ? PROBE_SUCCEEDED : PROBE_FAILED; 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)); // store the probe results - mach.probe_results[axis] = mach_get_absolute_position(axis); + pb.results[axis] = mach_get_absolute_position(axis); } _probe_restore_settings(); @@ -140,7 +151,7 @@ static void _probing_init() { // NOTE: it is *not* an error condition for the probe not to trigger. // it is an error for the limit or homing switches to fire, or for some other // configuration error. - mach.probe_state = PROBE_FAILED; + pb.state = PROBE_FAILED; mp_set_cycle(CYCLE_PROBING); // initialize the axes - save the jerk settings & switch to the jerk_homing @@ -176,7 +187,7 @@ static void _probing_init() { bool mach_is_probing() { - return mp_get_cycle() == CYCLE_PROBING || mach.probe_state == PROBE_WAITING; + return mp_get_cycle() == CYCLE_PROBING || pb.state == PROBE_WAITING; } @@ -194,11 +205,11 @@ stat_t mach_straight_probe(float target[], float flags[]) { // set probe move endpoint copy_vector(pb.target, target); // set probe move endpoint copy_vector(pb.flags, flags); // set axes involved on the move - clear_vector(mach.probe_results); // clear the old probe position. - // NOTE: relying on probe_results will not detect a probe to (0, 0, 0). + clear_vector(pb.results); // clear the old probe position. + // NOTE: relying on pb.results will not detect a probe to (0, 0, 0). // wait until planner queue empties before completing initialization - mach.probe_state = PROBE_WAITING; + pb.state = PROBE_WAITING; pb.func = _probing_init; // bind probing initialization function return STAT_OK; diff --git a/src/varcb.c b/src/varcb.c index 1725107..b26acd3 100644 --- a/src/varcb.c +++ b/src/varcb.c @@ -41,6 +41,6 @@ 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 mr.ms.line;} +int32_t get_line() {return mp_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());} -- 2.27.0