From fec6c336121295e17072ed67a9e431f1ebe1abb9 Mon Sep 17 00:00:00 2001 From: Alden Hart Date: Sun, 5 Feb 2017 10:39:28 -0500 Subject: [PATCH] Work on secondary feedholds --- g2core/alarm.cpp | 8 ++-- g2core/canonical_machine.cpp | 2 - g2core/canonical_machine.h | 14 ++----- g2core/cycle_feedhold.cpp | 76 +++++++++++++++++++++++++++--------- g2core/plan_exec.cpp | 18 +++++---- 5 files changed, 76 insertions(+), 42 deletions(-) diff --git a/g2core/alarm.cpp b/g2core/alarm.cpp index dbbdfeab..b98e1ff2 100644 --- a/g2core/alarm.cpp +++ b/g2core/alarm.cpp @@ -206,14 +206,14 @@ stat_t cm_shutdown(const stat_t status, const char *msg) spindle_reset(); // stop spindle immediately and set speed to 0 RPM coolant_reset(); // stop coolant immediately temperature_reset(); // turn off heaters and fans - cm_queue_flush(); // flush all queues and reset positions + cm_queue_flush(&cm1); // flush all queues and reset positions for (uint8_t i = 0; i < HOMING_AXES; i++) { // unhome axes and the machine cm->homed[i] = false; } cm->homing_state = HOMING_NOT_HOMED; - cm->machine_state = MACHINE_SHUTDOWN; // do this after all other activity + cm->machine_state = MACHINE_SHUTDOWN; // do this after all other activity rpt_exception(status, msg); // send exception report return (status); } @@ -239,9 +239,9 @@ stat_t cm_panic(const stat_t status, const char *msg) spindle_reset(); // stop spindle immediately and set speed to 0 RPM coolant_reset(); // stop coolant immediately temperature_reset(); // turn off heaters and fans - cm_queue_flush(); // flush all queues and reset positions + cm_queue_flush(&cm1); // flush all queues and reset positions - cm->machine_state = MACHINE_PANIC; // don't reset anything. Panics are not recoverable + cm->machine_state = MACHINE_PANIC; // don't reset anything. Panics are not recoverable rpt_exception(status, msg); // send panic report return (status); } diff --git a/g2core/canonical_machine.cpp b/g2core/canonical_machine.cpp index b01a00b0..a4fd9334 100644 --- a/g2core/canonical_machine.cpp +++ b/g2core/canonical_machine.cpp @@ -114,7 +114,6 @@ **** CM GLOBALS & STRUCTURE ALLOCATIONS ******************************************* ***********************************************************************************/ -cmMachineSelect cm_select; // CM_PRIMARY, CM_SECONDARY, CM_SECONDARY_RETURN cmMachine_t *cm; // pointer to active canonical machine cmMachine_t cm1; // canonical machine primary machine cmMachine_t cm2; // canonical machine secondary machine @@ -156,7 +155,6 @@ void canonical_machine_inits() cm = &cm1; // set global canonical machine pointer to primary machine mp = &mp1; // set global pointer to the primary planner mr = &mr1; // and primary runtime - cm_select = CM_PRIMARY; } void canonical_machine_init(cmMachine_t *_cm, void *_mp) diff --git a/g2core/canonical_machine.h b/g2core/canonical_machine.h index 102b054e..ee21aeb8 100644 --- a/g2core/canonical_machine.h +++ b/g2core/canonical_machine.h @@ -108,6 +108,7 @@ typedef enum { } cmMotionState; typedef enum { // feedhold state machine +// FEEDHOLD_FLUSH = -2, // set when p2 feedhold is ready to flush p2 queue 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 @@ -150,12 +151,6 @@ typedef enum { // queue flush state machine FLUSH_WAS_RUN // transient state to note that a queue flush has been run } cmQueueFlushState; -typedef enum { - CM_NOT_INIT = 0, // planners need initialization - CM_PRIMARY, // in primary machine/planner - CM_SECONDARY, // in secondary machine/planner -} cmMachineSelect; - /***************************************************************************** * CANONICAL MACHINE STRUCTURES */ @@ -291,7 +286,6 @@ typedef struct cmToolTable { // struct to keep a global tool tabl /**** Externs - See canonical_machine.cpp for allocation ****/ -extern cmMachineSelect cm_select; // CM_PRIMARY, CM_SECONDARY, CM_SECONDARY_RETURN extern cmMachine_t *cm; // pointer to active canonical machine extern cmMachine_t cm1; // canonical machine primary machine extern cmMachine_t cm2; // canonical machine secondary machine @@ -441,9 +435,9 @@ void cm_request_end_hold(void); void cm_request_queue_flush(void); stat_t cm_feedhold_sequencing_callback(void); // process feedhold, cycle start and queue flush requests -bool cm_has_hold(void); -void cm_start_hold(void); -void cm_queue_flush(void); // flush serial and planner queues with coordinate resets +bool cm_has_hold(void); // has hold in primary planner +void cm_start_hold(void); // starts hold in primary planner +void cm_queue_flush(cmMachine_t *_cm); // queue flush in either planner // Homing cycles (cycle_homing.cpp) stat_t cm_homing_cycle_start(const float axes[], const bool flags[]); // G28.2 diff --git a/g2core/cycle_feedhold.cpp b/g2core/cycle_feedhold.cpp index 3212d146..d294066e 100644 --- a/g2core/cycle_feedhold.cpp +++ b/g2core/cycle_feedhold.cpp @@ -143,21 +143,41 @@ void cm_start_hold() * cm_request_feedhold() * cm_request_end_hold() * cm_request_queue_flush() + * + * p1 is the primary planner, p2 is the secondary planner, which is active if the + * primary planner is in hold. IOW p2 can only be in a hold if p1 is already in one. + * Request_feedhold, request_end_hold, and request_queue_flush are contextual: + * + * request_feedhold: + * - If p1 is not in HOLD & is in motion, request_feedhold requests a p1 hold + * - If p1 is in HOLD & p2 is in motion, request_feedhold requests a p2 hold + * - If both p1 and p2 are in HOLD, request_feedhold is ignored + * + * request_end_hold: + * - If p1 is not in HOLD, request_end_hold is ignored + * - If p1 is in HOLD request_end_hold will end p1 hold & resume motion. + * Pre-defined exit actions (coolant, spindle, Z move) are completed first + * Any executing or pending "in-hold" moves are stopped prior to the exit actions + * + * request_queue_flush: + * - If p1 is not in HOLD, request_queue_flush is ignored + * - If p1 is in HOLD request_queue_flush will end p1 hold & queue flush (stop motion). + * Pre-defined exit actions (coolant, spindle, Z move) are completed first + * Any executing or pending "in-hold" moves are stopped prior to the exit actions */ void cm_request_feedhold(void) { - // cannot generate a feedhold request from the secondary context - if (cm_select != CM_PRIMARY) { - return; - } - // only generate request if not already in a feedhold and the machine is in motion + // Only generate request if not already in a feedhold and the machine is in motion if ((cm1.hold_state == FEEDHOLD_OFF) && (cm1.motion_state != MOTION_STOP)) { cm1.hold_state = FEEDHOLD_REQUESTED; + } else + if ((cm2.hold_state == FEEDHOLD_OFF) && (cm2.motion_state != MOTION_STOP)) { + cm2.hold_state = FEEDHOLD_REQUESTED; } } -void cm_request_end_hold(void) // This is usually requested from the secondary context +void cm_request_end_hold(void) { if (cm1.hold_state != FEEDHOLD_OFF) { cm1.end_hold_requested = true; @@ -166,11 +186,10 @@ void cm_request_end_hold(void) // This is usually requested from the secondary void cm_request_queue_flush() { + // NOTE: this function used to flush input buffers, but this is handled in xio *prior* to queue flush now if ((cm1.hold_state != FEEDHOLD_OFF) && // don't honor request unless you are in a feedhold (cm1.queue_flush_state == FLUSH_OFF)) { // ...and only once cm1.queue_flush_state = FLUSH_REQUESTED; // request planner flush once motion has stopped - // NOTE: this function used to flush the input buffers, - // but this is handled in xio *prior* to queue flush now } } @@ -193,24 +212,38 @@ void cm_request_queue_flush() stat_t cm_feedhold_sequencing_callback() { - // invoking a feedhold is a 2 step process - get to the stop, then execute the hold actions + // invoking a p1 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 + if (mp_has_runnable_buffer(&mp1)) { // bypass cm_start_hold() to start from here cm_set_motion_state(MOTION_HOLD); - cm->hold_state = FEEDHOLD_SYNC; // invokes hold from aline execution + cm1.hold_state = FEEDHOLD_SYNC; // invokes hold from aline execution } } if (cm1.hold_state == FEEDHOLD_ACTIONS_START) { // perform Z lift, spindle & coolant actions _run_p1_hold_entry_actions(); } + if (cm2.hold_state == FEEDHOLD_REQUESTED) { + if (mp_has_runnable_buffer(&mp2)) { + cm_set_motion_state(MOTION_HOLD); + cm2.hold_state = FEEDHOLD_SYNC; + } + } + // queue flush won't run until the hold is complete and all (subsequent) motion has stopped +/* + if ((cm2.queue_flush_state == FLUSH_REQUESTED) && (mp_runtime_is_idle())) { + cm_queue_flush(&cm2); + return (STAT_OK); + } +*/ if ((cm1.queue_flush_state == FLUSH_REQUESTED) && (cm1.hold_state == FEEDHOLD_HOLD) && // only flush once hold is actually holding (mp_runtime_is_idle())) { // don't flush planner during movement - cm_queue_flush(); - } // queue flush always ends hold, so it drops through - + cm_queue_flush(&cm1); + cm1.end_hold_requested = true; // p1 queue flush always ends the hold + } + // exit_hold runs for both ~ and % feedhold ends if (cm1.end_hold_requested) { @@ -225,7 +258,7 @@ stat_t cm_feedhold_sequencing_callback() } if (cm1.hold_state == FEEDHOLD_EXIT) { return(_finalize_p1_hold_exit()); // run multiple times until actions are complete - } + } return (STAT_OK); } @@ -267,7 +300,6 @@ static stat_t _run_p1_hold_entry_actions() cm = &cm2; mp = (mpPlanner_t *)cm->mp; // mp is a void pointer mr = mp->mr; - cm_select = CM_SECONDARY; // set motion state and ACTIVE_MODEL. This must be performed after cm is set to cm2 cm_set_g30_position(); @@ -345,7 +377,6 @@ static stat_t _finalize_p1_hold_exit() cm = &cm1; mp = (mpPlanner_t *)cm->mp; // cm->mp is a void pointer mr = mp->mr; - cm_select = CM_PRIMARY; // execute this block if a queue flush was performed // adjust primary planner positions to runtime positions @@ -408,18 +439,25 @@ static stat_t _finalize_p1_hold_exit() */ /*********************************************************************************** - * cm_queue_flush() - Flush primary planner queue + * cm_queue_flush() - Flush planner queue * * This function assumes that the feedhold sequencing callback has resolved all * state and timing issues and it's OK to call this now. Do not call this function * directly. Always use the feedhold sequencing callback. */ -void cm_queue_flush() +void cm_queue_flush(cmMachine_t *_cm) { + cm_abort_arc(_cm); // kill arcs so they don't just create more alines + planner_reset((mpPlanner_t *)_cm->mp); // reset primary planner. also resets the mr under the planner + _cm->queue_flush_state = FLUSH_WAS_RUN; + qr_request_queue_report(0); // request a queue report, since we've changed the number of buffers available + +/* cm_abort_arc(&cm1); // kill arcs so they don't just create more alines planner_reset((mpPlanner_t *)cm1.mp); // reset primary planner. also resets the mr under the planner cm1.queue_flush_state = FLUSH_WAS_RUN; cm1.end_hold_requested = true; // queue flush always ends the hold qr_request_queue_report(0); // request a queue report, since we've changed the number of buffers available +*/ } diff --git a/g2core/plan_exec.cpp b/g2core/plan_exec.cpp index 816ce870..65394f47 100644 --- a/g2core/plan_exec.cpp +++ b/g2core/plan_exec.cpp @@ -527,24 +527,28 @@ stat_t mp_exec_aline(mpBuf_t *bf) // (4) - We have decelerated a block to some velocity > zero (needs continuation in next block) // (5) - We have decelerated a block to zero velocity // (6) - We have finished all the runtime work now we have to wait for the steppers to stop - // (7) - The steppers have stopped. No motion should occur. ALlows hold finalization to commence + // (6a) - It's a homing or probing feedhold - ditch the remaining buffer & go directly to OFF + // (6b) - It's a p2 feedhold - ditch the remaining buffer & signal we want a p2 queue flush + // (6c) - It's a normal feedhold - signal we want the entry action + // (7) - The steppers have stopped. No motion should occur. Allows hold actions to complete // (8) - We are removing the hold state and there is queued motion (handled outside this routine) // (9) - We are removing the hold state and there is no queued motion (also handled outside this routine) if (cm->motion_state == MOTION_HOLD) { // Case (7) - All motion has ceased - // 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 + if (cm->hold_state >= FEEDHOLD_ACTIONS_START) { // FEEDHOLD_ACTIONS_START, FEEDHOLD_ACTIONS_WAIT or FEEDHOLD HOLD + return (STAT_NOOP); // VERY IMPORTANT to exit as a NOOP. No more movement } // Case (6) - Wait for the steppers to stop if (cm->hold_state == FEEDHOLD_STOPPING) { - if (mp_runtime_is_idle()) { // wait for the steppers to actually clear out + if (mp_runtime_is_idle()) { // wait for steppers to actually finish + // when homing or probing don't stay in HOLD or execute entry actions if ((cm->cycle_state == CYCLE_HOMING) || (cm->cycle_state == CYCLE_PROBE)) { - // when homing or probing we don't want to stay in HOLD or execute finalizations cm->hold_state = FEEDHOLD_OFF; - } else { + } else if (cm == &cm2) { // if in p2 hold set up a flush + cm->queue_flush_state = FLUSH_REQUESTED; + } else { cm->hold_state = FEEDHOLD_ACTIONS_START; // perform Z-lift, spindle, coolant actions } mp_zero_segment_velocity(); // for reporting purposes