From b17a5d19634715cbdf3f3742f72782feeee30ab9 Mon Sep 17 00:00:00 2001 From: Alden Hart Date: Sun, 5 Feb 2017 06:48:33 -0500 Subject: [PATCH] Better isolation of feedhold state --- g2core/canonical_machine.h | 10 ++--- g2core/controller.cpp | 1 - g2core/cycle_feedhold.cpp | 76 ++++++++++++++++++++++---------------- g2core/plan_exec.cpp | 6 +-- 4 files changed, 51 insertions(+), 42 deletions(-) diff --git a/g2core/canonical_machine.h b/g2core/canonical_machine.h index 9f491a88..efad66b7 100644 --- a/g2core/canonical_machine.h +++ b/g2core/canonical_machine.h @@ -108,6 +108,7 @@ typedef enum { } cmMotionState; typedef enum { // feedhold state machine + FEEDHOLD_EXIT = -1, // set when feedhold is due to exit FEEDHOLD_OFF = 0, // no feedhold in effect FEEDHOLD_REQUESTED, // feedhold has been requested but not started yet FEEDHOLD_SYNC, // start hold - sync to latest aline segment @@ -117,7 +118,7 @@ typedef enum { // feedhold state machine FEEDHOLD_STOPPING, // waiting to complete deceleration once planner motion stops FEEDHOLD_ACTIONS_START, // enter secondary planner and perform feedhold actions (once) FEEDHOLD_ACTIONS_WAIT, // wait for feedhold actions to complete - FEEDHOLD_HOLD // holding + FEEDHOLD_HOLD // holding (steady state) Must be last state } cmFeedholdState; typedef enum { // applies to cm->homing_state @@ -153,7 +154,6 @@ typedef enum { CM_NOT_INIT = 0, // planners need initialization CM_PRIMARY, // in primary machine/planner CM_SECONDARY, // in secondary machine/planner - CM_SECONDARY_RETURN // in return move from secondary planner } cmMachineSelect; /***************************************************************************** @@ -260,7 +260,7 @@ typedef struct cmMachine { // struct to manage canonical machin cmSafetyState safety_interlock_state; // safety interlock state uint32_t esc_boot_timer; // timer for Electronic Speed Control (Spindle electronics) to boot - bool waiting_for_exit_hold; // used by cm_exit_hold_planner() to tell when secondary planner is done +// bool waiting_for_exit_hold; // used by cm_exit_hold_planner() to tell when secondary planner is done bool end_hold_requested; // request restart after feedhold bool deferred_write_flag; // G10 data has changed (e.g. offsets) - flag to persist them uint8_t limit_requested; // set non-zero to request limit switch processing (value is input number) @@ -446,10 +446,6 @@ bool cm_has_hold(void); void cm_start_hold(void); void cm_queue_flush(void); // flush serial and planner queues with coordinate resets -stat_t cm_enter_hold_planner(void); // move to secondary planner for in-hold actions -stat_t cm_exit_hold_planner(void); // return to primary planner -stat_t cm_exit_hold_finalize(void); // main loop callback for synchronization - // Homing cycles (cycle_homing.cpp) stat_t cm_homing_cycle_start(const float axes[], const bool flags[]); // G28.2 stat_t cm_homing_cycle_start_no_set(const float axes[], const bool flags[]); // G28.4 diff --git a/g2core/controller.cpp b/g2core/controller.cpp index d1003b8f..4d701bb5 100644 --- a/g2core/controller.cpp +++ b/g2core/controller.cpp @@ -162,7 +162,6 @@ static void _controller_HSM() DISPATCH(cm_feedhold_sequencing_callback());// feedhold state machine runner DISPATCH(mp_planner_callback()); // motion planner DISPATCH(cm_arc_callback(cm)); // arc generation runs as a cycle above lines - DISPATCH(cm_exit_hold_finalize()); // finalization for return from secondary planner DISPATCH(cm_homing_cycle_callback()); // homing cycle operation (G28.2) DISPATCH(cm_probing_cycle_callback()); // probing cycle operation (G38.2) DISPATCH(cm_jogging_cycle_callback()); // jog cycle operation diff --git a/g2core/cycle_feedhold.cpp b/g2core/cycle_feedhold.cpp index f57dd962..76d61117 100644 --- a/g2core/cycle_feedhold.cpp +++ b/g2core/cycle_feedhold.cpp @@ -37,6 +37,13 @@ #include "coolant.h" #include "util.h" +static stat_t _run_p1_hold_entry_actions(void); +static void _sync_to_p1_hold_entry_actions_done(float* vect, bool* flag); + +static stat_t _run_p1_hold_exit_actions(void); +static void _sync_to_p1_hold_exit_actions_done(float* vect, bool* flag); +static stat_t _finalize_p1_hold_exit(void); + /*********************************************************************************** **** Feedholds ******************************************************************** ***********************************************************************************/ @@ -186,7 +193,7 @@ void cm_request_queue_flush() stat_t cm_feedhold_sequencing_callback() { - // invoking a feedhold is a 2 step process, invoke it, then do the hold moves + // invoking a feedhold is a 2 step process - get to the stop, then execute the hold actions if (cm1.hold_state == FEEDHOLD_REQUESTED) { if (mp_has_runnable_buffer(mp)) { // bypass cm_start_hold() to start from here cm_set_motion_state(MOTION_HOLD); @@ -194,7 +201,7 @@ stat_t cm_feedhold_sequencing_callback() } } if (cm1.hold_state == FEEDHOLD_ACTIONS_START) { // perform Z lift, spindle & coolant actions - cm_enter_hold_planner(); + _run_p1_hold_entry_actions(); } // queue flush won't run until the hold is complete and all (subsequent) motion has stopped @@ -213,9 +220,12 @@ stat_t cm_feedhold_sequencing_callback() } if (cm1.hold_state == FEEDHOLD_HOLD) { // don't run end_hold until fully into a hold cm1.end_hold_requested = false; - cm_exit_hold_planner(); + _run_p1_hold_exit_actions(); // runs once only } } + if (cm1.hold_state == FEEDHOLD_EXIT) { + return(_finalize_p1_hold_exit()); // run multiple times until actions are complete + } return (STAT_OK); } @@ -230,13 +240,17 @@ stat_t cm_feedhold_sequencing_callback() * directly. Always use the feedhold sequencing callback. */ -// Callback to run at when the secondary planner moves are finished -static void _enter_hold_finalize(float* vect, bool* flag) +// Callback to run when the ACTIONS from feedhold in planner 1 are finished. +// This function hits cm1 directly (no pointers) as ACTIONS for a feedhold +// in planner 1 actually run in the secondary planner. Feedholds from planner 2 +// do not run actions, so this function is never called for planner 2 feedholds. +// It's called from an interrupt, so it only sets a flag. +static void _sync_to_p1_hold_entry_actions_done(float* vect, bool* flag) { cm1.hold_state = FEEDHOLD_HOLD; } -stat_t cm_enter_hold_planner() +static stat_t _run_p1_hold_entry_actions() { cm->hold_state = FEEDHOLD_ACTIONS_WAIT; // last state before transitioning to HOLD @@ -280,15 +294,15 @@ stat_t cm_enter_hold_planner() } spindle_control_sync(SPINDLE_PAUSE); // optional spindle pause coolant_control_sync(COOLANT_PAUSE, COOLANT_BOTH); // optional coolant pause - mp_queue_command(_enter_hold_finalize, nullptr, nullptr); + mp_queue_command(_sync_to_p1_hold_entry_actions_done, nullptr, nullptr); return (STAT_OK); } /*********************************************************************************** - * cm_exit_hold_planner() - initiate return from feedhold planner - * cm_exit_hold_finalize() - main loop callback to finsh return once moves are done - * _planner_done_callback() - callback to sync to end of planner operations + * _run_p1_hold_exit_actions() - initiate return from feedhold planner + * _sync_to_p1_hold_exit_actions_done() - callback to sync to end of planner operations + * _finalize_p1_hold_exit() - callback to finsh return once moves are done * * Moving between planners is only safe when the machine is completely stopped * either during a feedhold or when idle. @@ -302,13 +316,7 @@ stat_t cm_enter_hold_planner() * directly. Always use the feedhold sequencing callback. */ -// Callback to run at when the G30 return move is finished -static void _exit_hold_finalize(float* vect, bool* flag) -{ - cm2.waiting_for_exit_hold = false; -} - -stat_t cm_exit_hold_planner() // LATER: if value == true return with offset corrections +static stat_t _run_p1_hold_exit_actions() // LATER: if value == true return with offset corrections { // perform end-hold actions --- while still in secondary machine spindle_control_sync(SPINDLE_RESUME); // resume spindle if paused @@ -318,24 +326,30 @@ stat_t cm_exit_hold_planner() // LATER: if value == true return with offset float target[] = { 0,0,0,0,0,0 }; // LATER: Make this move return through XY, then Z bool flags[] = { 0,0,0,0,0,0 }; cm_goto_g30_position(target, flags); // initiate a return move - cm2.waiting_for_exit_hold = true; // indicates running the final G30 move in the secondary - mp_queue_command(_exit_hold_finalize, nullptr, nullptr); - cm_select = CM_SECONDARY_RETURN; + // cm2.waiting_for_exit_hold = true; // indicates running the final G30 move in the secondary + mp_queue_command(_sync_to_p1_hold_exit_actions_done, nullptr, nullptr); return (STAT_OK); - -// cm_exit_hold_planner() completes in cm_exit_hold_finalize() after the above moves are done } -stat_t cm_exit_hold_finalize() +// Callback to run at when the G30 return move is finished. This function is +// only ever called by the secondary planner, and only when exiting a feedhold +// from planner 1. It's called from an interrupt, so it only sets a flag. +static void _sync_to_p1_hold_exit_actions_done(float* vect, bool* flag) { - if (cm_select != CM_SECONDARY_RETURN) { // exit if not in secondary planner +// cm2.waiting_for_exit_hold = false; + cm1.hold_state = FEEDHOLD_EXIT; // last state before transitioning out of HOLD +} + +static stat_t _finalize_p1_hold_exit() +{ + if (cm1.hold_state != FEEDHOLD_EXIT) { // skip out if not ready to finalize the exit return (STAT_NOOP); } - if (cm2.waiting_for_exit_hold) { // sync to planner move ends (via _return_move_callback) - return (STAT_EAGAIN); - } +// if (cm2.waiting_for_exit_hold) { // sync to planner move ends (via _return_move_callback) +// return (STAT_EAGAIN); +// } - // return to primary machine + // return to primary planner (p1) cm = &cm1; mp = (mpPlanner_t *)cm->mp; // cm->mp is a void pointer mr = mp->mr; @@ -350,9 +364,9 @@ stat_t cm_exit_hold_finalize() cm1.queue_flush_state = FLUSH_OFF; } - // resume motion from primary planner or end cycle if now moves - cm->hold_state = FEEDHOLD_OFF; - if (mp_has_runnable_buffer(mp)) { //+++++ Should MP be passed or global? + // resume motion from primary planner or end cycle if no moves in planner + cm1.hold_state = FEEDHOLD_OFF; + if (mp_has_runnable_buffer(&mp1)) { cm_set_motion_state(MOTION_RUN); cm_cycle_start(); st_request_exec_move(); diff --git a/g2core/plan_exec.cpp b/g2core/plan_exec.cpp index a082dba5..816ce870 100644 --- a/g2core/plan_exec.cpp +++ b/g2core/plan_exec.cpp @@ -533,7 +533,8 @@ stat_t mp_exec_aline(mpBuf_t *bf) if (cm->motion_state == MOTION_HOLD) { // Case (7) - All motion has ceased - if (cm->hold_state >= FEEDHOLD_ACTIONS_START) { // FEEDHOLD_ACTIONS or later + // FEEDHOLD_ACTIONS_START, FEEDHOLD_ACTIONS_WAIT or FEEDHOLD HOLD + if (cm->hold_state >= FEEDHOLD_ACTIONS_START) { return (STAT_NOOP); // VERY IMPORTANT to exit as a NOOP. No more movement } @@ -554,12 +555,11 @@ stat_t mp_exec_aline(mpBuf_t *bf) } // Case (5) - Decelerated to zero - // Update the run buffer then force a replan of the whole planner queue + // Update the run buffer then force a replan of the whole planner queue. Replans from 0 velocity if (cm->hold_state == FEEDHOLD_DECEL_END) { mr->block_state = BLOCK_INACTIVE; // invalidate mr buffer to reset the new move bf->block_state = BLOCK_INITIAL_ACTION; // tell _exec to re-use the bf buffer bf->length = get_axis_vector_length(mr->target, mr->position);// reset length - //bf->entry_vmax = 0; // set bp+0 as hold point cm->hold_state = FEEDHOLD_STOPPING; // No point bothering with the rest of this move if homing or probing