From 6811849aa30a38f6f1b0da7a170acbf88cb829b6 Mon Sep 17 00:00:00 2001 From: Alden Hart Date: Tue, 14 Mar 2017 07:07:03 -0400 Subject: [PATCH] Milestone: First time it makes it all the way through a !~ cycle with actions --- g2core/alarm.cpp | 4 +- g2core/canonical_machine.h | 169 +++++++++++++------------- g2core/controller.cpp | 5 +- g2core/cycle_feedhold.cpp | 238 ++++++++++++++++++------------------- g2core/gpio.cpp | 12 +- g2core/plan_exec.cpp | 19 +-- 6 files changed, 216 insertions(+), 231 deletions(-) diff --git a/g2core/alarm.cpp b/g2core/alarm.cpp index f38ffd4c..1e6f885b 100644 --- a/g2core/alarm.cpp +++ b/g2core/alarm.cpp @@ -166,7 +166,7 @@ stat_t cm_alarm(const stat_t status, const char *msg) (cm->machine_state == MACHINE_PANIC)) { return (STAT_OK); // don't alarm if already in an alarm state } - cm_request_feedhold(FEEDHOLD_TYPE_SCRAM, FEEDHOLD_FINAL_ALARM); // fast stop and alarm + cm_request_feedhold(FEEDHOLD_TYPE_SCRAM, FEEDHOLD_EXIT_ALARM); // fast stop and alarm rpt_exception(status, msg); // send alarm message sr_request_status_report(SR_REQUEST_TIMED); return (status); @@ -194,7 +194,7 @@ stat_t cm_shutdown(const stat_t status, const char *msg) if ((cm->machine_state == MACHINE_SHUTDOWN) || (cm->machine_state == MACHINE_PANIC)) { return (STAT_OK); // don't shutdown if shutdown or panic'd } - cm_request_feedhold(FEEDHOLD_TYPE_SCRAM, FEEDHOLD_FINAL_SHUTDOWN); // fast stop and shutdown + cm_request_feedhold(FEEDHOLD_TYPE_SCRAM, FEEDHOLD_EXIT_SHUTDOWN); // fast stop and shutdown // cm_halt_motion(); // halt motors (may have already been done from GPIO) // spindle_reset(); // stop spindle immediately and set speed to 0 RPM diff --git a/g2core/canonical_machine.h b/g2core/canonical_machine.h index 5b801da7..86e9eff7 100644 --- a/g2core/canonical_machine.h +++ b/g2core/canonical_machine.h @@ -63,131 +63,124 @@ // ### LAYER 8 CRITICAL REGION ### // ### DO NOT CHANGE THESE ENUMERATIONS WITHOUT COMMUNITY INPUT ### -typedef enum { // check alignment with messages in config.c / msg_stat strings - COMBINED_INITIALIZING = 0, // [0] machine is initializing - COMBINED_READY, // [1] machine is ready for use - COMBINED_ALARM, // [2] machine in alarm state - COMBINED_PROGRAM_STOP, // [3] program stop/no more blocks - COMBINED_PROGRAM_END, // [4] program end - COMBINED_RUN, // [5] machine is running - COMBINED_HOLD, // [6] machine is holding - COMBINED_PROBE, // [7] probe cycle activ - COMBINED_CYCLE, // [8] reserved for canned cycles - COMBINED_HOMING, // [9] homing cycle active - COMBINED_JOG, // [10] jogging cycle active - COMBINED_INTERLOCK, // [11] machine in safety interlock hold - COMBINED_SHUTDOWN, // [12] machine in shutdown state - COMBINED_PANIC // [13] machine in panic state +typedef enum { // check alignment with messages in config.c / msg_stat strings + COMBINED_INITIALIZING = 0, // [0] machine is initializing + COMBINED_READY, // [1] machine is ready for use + COMBINED_ALARM, // [2] machine in alarm state + COMBINED_PROGRAM_STOP, // [3] program stop/no more blocks + COMBINED_PROGRAM_END, // [4] program end + COMBINED_RUN, // [5] machine is running + COMBINED_HOLD, // [6] machine is holding + COMBINED_PROBE, // [7] probe cycle activ + COMBINED_CYCLE, // [8] reserved for canned cycles + COMBINED_HOMING, // [9] homing cycle active + COMBINED_JOG, // [10] jogging cycle active + COMBINED_INTERLOCK, // [11] machine in safety interlock hold + COMBINED_SHUTDOWN, // [12] machine in shutdown state + COMBINED_PANIC // [13] machine in panic state } cmCombinedState; //### END CRITICAL REGION ### -typedef enum { // Note: MachineState signals if the machine is in cycle (5) or some other non-cycle state - MACHINE_INITIALIZING = 0, // machine is initializing - MACHINE_READY, // machine is ready for use but idle - MACHINE_ALARM, // machine is in alarm state - MACHINE_PROGRAM_STOP, // no blocks to run; like PROGRAM_END but without the M2 to reset gcode state - MACHINE_PROGRAM_END, // program end (same as MACHINE_READY, really...) - MACHINE_CYCLE, // machine is in cycle, running; blocks still to run, or steppers are busy - MACHINE_INTERLOCK, // machine is in interlock state - MACHINE_SHUTDOWN, // machine is in shutdown state - MACHINE_PANIC // machine is in panic state +typedef enum { // Note: MachineState signals if the machine is in cycle (5) or some other non-cycle state + MACHINE_INITIALIZING = 0, // machine is initializing + MACHINE_READY, // machine is ready for use but idle + MACHINE_ALARM, // machine is in alarm state + MACHINE_PROGRAM_STOP, // no blocks to run; like PROGRAM_END but without the M2 to reset gcode state + MACHINE_PROGRAM_END, // program end (same as MACHINE_READY, really...) + MACHINE_CYCLE, // machine is in cycle, running; blocks still to run, or steppers are busy + MACHINE_INTERLOCK, // machine is in interlock state + MACHINE_SHUTDOWN, // machine is in shutdown state + MACHINE_PANIC // machine is in panic state } cmMachineState; typedef enum { - MOTION_STOP = 0, // motion has stopped: set when the steppers reach the end of the planner queue - MOTION_RUN // machine is in motion: set when the steppers execute an ALINE segment + MOTION_STOP = 0, // motion has stopped: set when the steppers reach the end of the planner queue + MOTION_RUN // machine is in motion: set when the steppers execute an ALINE segment } cmMotionState; -typedef enum { // state machine for cycle start - CYCLE_START_NONE = 0, // not in a cycle +typedef enum { // state machine for cycle start + CYCLE_START_OFF = 0, // not requested CYCLE_START_REQUESTED, CYCLE_START_COMPLETE } cmCycleState; typedef enum { - CYCLE_NONE = 0, // not in a cycle - CYCLE_MACHINING, // in normal machining cycle - CYCLE_HOMING, // in homing cycle - CYCLE_PROBE, // in probe cycle - CYCLE_JOG // in jogging cycle -// CYCLE_G81 // illustration of canned cycles + CYCLE_NONE = 0, // not in a cycle + CYCLE_MACHINING, // in normal machining cycle + CYCLE_HOMING, // in homing cycle + CYCLE_PROBE, // in probe cycle + CYCLE_JOG // in jogging cycle +// CYCLE_G81 // illustration of canned cycles // ... } cmCycleType; -typedef enum { // feedhold type parameter - FEEDHOLD_TYPE_ACTIONS = 0, // feedhold at max jerk with actions - FEEDHOLD_TYPE_NO_ACTIONS, // feedhold at max jerk with no actions - FEEDHOLD_TYPE_SYNC, // feedhold at max jerk with queue flush and sync command - FEEDHOLD_TYPE_FAST, // feedhold at high jerk with no actions. Can resume - FEEDHOLD_TYPE_SCRAM // feedhold at high jerk and stop all active devices +typedef enum { // feedhold type parameter + FEEDHOLD_TYPE_ACTIONS = 0, // feedhold at max jerk with actions + FEEDHOLD_TYPE_NO_ACTIONS, // feedhold at max jerk with no actions + FEEDHOLD_TYPE_SYNC, // feedhold at max jerk with queue flush and sync command + FEEDHOLD_TYPE_FAST, // feedhold at high jerk with no actions. Can resume + FEEDHOLD_TYPE_SCRAM // feedhold at high jerk and stop all active devices } cmFeedholdType; -typedef enum { // feedhold final operation - FEEDHOLD_FINAL_CYCLE = 0, // normal final state - HOLD or STOP, depending on type - FEEDHOLD_FINAL_STOP, // perform program stop - FEEDHOLD_FINAL_END, // perform program end - FEEDHOLD_FINAL_ALARM, // perform alarm - FEEDHOLD_FINAL_SHUTDOWN, // perform shutdown - FEEDHOLD_FINAL_INTERLOCK // report as interlock +typedef enum { // feedhold final operation + FEEDHOLD_EXIT_CYCLE = 0, // exit feedhold with cycle restart - HOLD or STOP, depending on type + FEEDHOLD_EXIT_FLUSH, // exit feedhold with flush + FEEDHOLD_EXIT_STOP, // perform program stop + FEEDHOLD_EXIT_END, // perform program end + FEEDHOLD_EXIT_ALARM, // perform alarm + FEEDHOLD_EXIT_SHUTDOWN, // perform shutdown + FEEDHOLD_EXIT_INTERLOCK // report as interlock } cmFeedholdFinal; -typedef enum { // feedhold state machine -// FEEDHOLD_P1_EXIT = -1, // set when p1 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 - FEEDHOLD_DECEL_CONTINUE, // in deceleration that will not end at zero - FEEDHOLD_DECEL_TO_ZERO, // in deceleration that will go to zero - FEEDHOLD_DECEL_COMPLETE, // feedhold deceleration has completed, but motors may not have stopped yet - FEEDHOLD_MOTION_STOPPING, // waiting for motors to have stopped at hold point (motion stop) - FEEDHOLD_MOTION_STOPPED, // motion has stopped at hold point -// FEEDHOLD_P2_START, // enter secondary planner and perform feedhold actions (once) -// FEEDHOLD_P2_WAIT, // wait for feedhold actions to complete - - FEEDHOLD_HOLD_ACTION_START, - FEEDHOLD_HOLD_PENDING, // wait for feedhold actions to complete - FEEDHOLD_HOLD_DONE, // - - FEEDHOLD_HOLD, // holding (steady state) Must be last state - - FEEDHOLD_HOLD_EXIT_PENDING, // performing exit actions - FEEDHOLD_HOLD_EXIT_DONE, // completed exit actions - -// FEEDHOLD_P2_EXIT // set when p2 feedhold is finishing +typedef enum { // feedhold state machine + 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 + FEEDHOLD_DECEL_CONTINUE, // in deceleration that will not end at zero + FEEDHOLD_DECEL_TO_ZERO, // in deceleration that will go to zero + FEEDHOLD_DECEL_COMPLETE, // feedhold deceleration has completed, but motors may not have stopped yet + FEEDHOLD_MOTION_STOPPING, // waiting for motors to have stopped at hold point (motion stop) + FEEDHOLD_MOTION_STOPPED, // motion has stopped at hold point + FEEDHOLD_HOLD_ACTIONS_START, + FEEDHOLD_HOLD_ACTIONS_PENDING, // wait for feedhold actions to complete + FEEDHOLD_HOLD_ACTIONS_COMPLETE, // + FEEDHOLD_HOLD, // HOLDING (steady state) + FEEDHOLD_EXIT_ACTIONS_PENDING, // performing exit actions + FEEDHOLD_EXIT_ACTIONS_COMPLETE // completed exit actions } cmFeedholdState; -typedef enum { // applies to cm->homing_state - HOMING_NOT_HOMED = 0, // machine is not homed (0=false) - HOMING_HOMED = 1, // machine is homed (1=true) - HOMING_WAITING // machine waiting to be homed +typedef enum { // applies to cm->homing_state + HOMING_NOT_HOMED = 0, // machine is not homed (0=false) + HOMING_HOMED = 1, // machine is homed (1=true) + HOMING_WAITING // machine waiting to be homed } cmHomingState; -typedef enum { // applies to cm->probe_state - PROBE_FAILED = 0, // probe reached endpoint without triggering - PROBE_SUCCEEDED = 1, // probe was triggered, cm.probe_results has position - PROBE_WAITING = 2 // probe is waiting to be started or is running +typedef enum { // applies to cm->probe_state + PROBE_FAILED = 0, // probe reached endpoint without triggering + PROBE_SUCCEEDED = 1, // probe was triggered, cm.probe_results has position + PROBE_WAITING = 2 // probe is waiting to be started or is running } cmProbeState; typedef enum { - SAFETY_INTERLOCK_ENGAGED = 0, // meaning the interlock input is CLOSED (low) + SAFETY_INTERLOCK_ENGAGED = 0, // meaning the interlock input is CLOSED (low) SAFETY_INTERLOCK_DISENGAGED } cmSafetyState; -typedef enum { // feed override state machine +typedef enum { // feed override state machine MFO_OFF = 0, MFO_REQUESTED, MFO_SYNC } cmOverrideState; -typedef enum { // queue flush state machine - FLUSH_OFF = 0, // no queue flush in effect - FLUSH_REQUESTED, // flush has been requested but not started yet - FLUSH_WAS_RUN // transient state to note that a queue flush has been run +typedef enum { // queue flush state machine + FLUSH_OFF = 0, // no queue flush in effect + FLUSH_REQUESTED, // flush has been requested but not started yet + FLUSH_WAS_RUN // transient state to note that a queue flush has been run } cmFlushState; -typedef enum { // Motion profiles - PROFILE_NORMAL = 0, // Normal jerk in effect - PROFILE_FAST // High speed jerk in effect +typedef enum { // Motion profiles + PROFILE_NORMAL = 0, // Normal jerk in effect + PROFILE_FAST // High speed jerk in effect } cmMotion_profile; /***************************************************************************** diff --git a/g2core/controller.cpp b/g2core/controller.cpp index 24d1d6da..882ebede 100644 --- a/g2core/controller.cpp +++ b/g2core/controller.cpp @@ -238,9 +238,8 @@ static void _dispatch_kernel(const devflags_t flags) } // trap single character commands - if (*cs.bufp == '!') { cm_request_feedhold(FEEDHOLD_TYPE_ACTIONS, FEEDHOLD_FINAL_CYCLE); } + if (*cs.bufp == '!') { cm_request_feedhold(FEEDHOLD_TYPE_ACTIONS, FEEDHOLD_EXIT_CYCLE); } else if (*cs.bufp == '%') { cm_request_queue_flush(); xio_flush_to_command(); } -// else if (*cs.bufp == '~') { cm_request_exit_hold(); } +++++ else if (*cs.bufp == '~') { cm_request_cycle_start(); } else if (*cs.bufp == EOT) { cm_job_kill(); } else if (*cs.bufp == ENQ) { controller_request_enquiry(); } @@ -435,7 +434,7 @@ static stat_t _interlock_handler(void) if (cm->safety_interlock_disengaged != 0) { cm->safety_interlock_disengaged = 0; cm->safety_interlock_state = SAFETY_INTERLOCK_DISENGAGED; - cm_request_feedhold(FEEDHOLD_TYPE_ACTIONS, FEEDHOLD_FINAL_INTERLOCK); // may have already requested STOP as INPUT_ACTION + cm_request_feedhold(FEEDHOLD_TYPE_ACTIONS, FEEDHOLD_EXIT_INTERLOCK); // may have already requested STOP as INPUT_ACTION // feedhold was initiated by input action in gpio // pause spindle // pause coolant diff --git a/g2core/cycle_feedhold.cpp b/g2core/cycle_feedhold.cpp index 70b552b9..51986600 100644 --- a/g2core/cycle_feedhold.cpp +++ b/g2core/cycle_feedhold.cpp @@ -42,13 +42,16 @@ static void _initiate_feedhold(void); static void _initiate_cycle_start(void); static void _initiate_queue_flush(void); +// Feedhold actions static stat_t _feedhold_with_actions(float *param); static stat_t _feedhold_with_no_actions(float *param); static stat_t _feedhold_with_sync(float *param); -static stat_t _feedhold_exit_with_actions(float *param); -static stat_t _feedhold_exit_with_no_actions(float *param); +static stat_t _feedhold_restart_with_actions(float *param); +static stat_t _feedhold_restart_with_no_actions(float *param); -static stat_t _cycle_exit(float *param); +// Feedhold exits (finalization) +static stat_t _restart_cycle(float *param); +static stat_t _restart_flush(float *param); static stat_t _program_stop(float *param); static stat_t _program_end(float *param); static stat_t _alarm(float *param); @@ -71,11 +74,15 @@ static stat_t _interlock(float *param); * STAT_OK - operation has completed successfully * STAT_EAGAIN - operation needs to be re-entered to complete (via operation callback) * STAT_XXXXX - any other status is an error that quits the operation + * + * Constraints: + * - Operations run to completion. They are not preemptable (at this point) + * - Actions cannot be added to an operation once it is being run */ /*** Object Definitions ***/ -#define PARAM_MAX 4 // maximum number of parameters that can be passed in param +#define PARAM_MAX 2 // maximum number of parameters that can be passed in param #define ACTION_MAX 12 // maximum actions that can be queued for an operation typedef stat_t (*action_exec_t)(float *); // callback to action execution function @@ -123,9 +130,7 @@ typedef struct cmOperation { // operation runner object }; stat_t run_operation(void) { - if (run->func == NULL) { - return (STAT_NOOP); - } // not an error. This is normal. + if (run->func == NULL) { return (STAT_NOOP); } // not an error. This is normal. in_operation = true; // disable add_action during operations stat_t status; @@ -136,9 +141,7 @@ typedef struct cmOperation { // operation runner object return (STAT_OK); } } - if (status == STAT_EAGAIN) { - return (STAT_EAGAIN); - } + if (status == STAT_EAGAIN) { return (STAT_EAGAIN); } reset(); // reset operation if action threw an error return (status); // return error code }; @@ -244,15 +247,7 @@ bool cm_has_hold() { return (cm1.hold_state != FEEDHOLD_OFF); } -/* -void cm_start_hold() -{ - // Can only request a feedhold if the machine is in motion and there not one is not already in progress - if ((cm1.hold_state == FEEDHOLD_OFF) && (mp_has_runnable_buffer(mp))) { - cm1.hold_state = FEEDHOLD_SYNC; // invokes hold from aline execution - } -} -*/ + stat_t cm_feedhold_command_blocker() { if (cm1.hold_state != FEEDHOLD_OFF) { @@ -261,14 +256,9 @@ stat_t cm_feedhold_command_blocker() return (STAT_OK); } -void cm_request_alarm() -{ - -} - /**************************************************************************************** - * request_cycle_start() - set request flag only - * start_cycle_start() - run the cycle start + * cm_request_cycle_start() - set request flag only + * _initiate_cycle_start() - run the cycle start */ void cm_request_cycle_start() @@ -280,6 +270,7 @@ static void _initiate_cycle_start() { // Normal cycle start - not in a feedhold if (cm1.hold_state == FEEDHOLD_OFF) { + cm1.cycle_state = CYCLE_START_OFF; cm_cycle_start(); // execute cycle start directly st_request_exec_move(); return; @@ -287,83 +278,23 @@ static void _initiate_cycle_start() // Feedhold cycle starts run an operation to complete multiple actions if (cm1.hold_state == FEEDHOLD_HOLD) { + cm1.cycle_state = CYCLE_START_OFF; switch (cm1.hold_type) { - case FEEDHOLD_TYPE_ACTIONS: { op.add_action(_feedhold_exit_with_actions, nullptr); break; } - case FEEDHOLD_TYPE_NO_ACTIONS: { op.add_action(_feedhold_exit_with_no_actions, nullptr); break; } + case FEEDHOLD_TYPE_ACTIONS: { op.add_action(_feedhold_restart_with_actions, nullptr); break; } + case FEEDHOLD_TYPE_NO_ACTIONS: { op.add_action(_feedhold_restart_with_no_actions, nullptr); break; } default: {} } switch (cm1.hold_final) { - case FEEDHOLD_FINAL_CYCLE: { op.add_action(_cycle_exit, nullptr); break; } - case FEEDHOLD_FINAL_STOP: { op.add_action(_program_stop, nullptr); break; } - case FEEDHOLD_FINAL_END: { op.add_action(_program_end, nullptr); break; } - case FEEDHOLD_FINAL_ALARM: { op.add_action(_alarm, nullptr); break; } - case FEEDHOLD_FINAL_SHUTDOWN: { op.add_action(_shutdown, nullptr); break; } - case FEEDHOLD_FINAL_INTERLOCK: { op.add_action(_interlock, nullptr); break; } + case FEEDHOLD_EXIT_CYCLE: { op.add_action(_restart_cycle, nullptr); break; } + case FEEDHOLD_EXIT_FLUSH: { op.add_action(_restart_flush, nullptr); break; } + case FEEDHOLD_EXIT_STOP: { op.add_action(_program_stop, nullptr); break; } + case FEEDHOLD_EXIT_END: { op.add_action(_program_end, nullptr); break; } + case FEEDHOLD_EXIT_ALARM: { op.add_action(_alarm, nullptr); break; } + case FEEDHOLD_EXIT_SHUTDOWN: { op.add_action(_shutdown, nullptr); break; } + case FEEDHOLD_EXIT_INTERLOCK: { op.add_action(_interlock, nullptr); break; } default: {} } } - op.run_operation(); -} - -/**************************************************************************************** - * cm_request_feedhold() - request a feedhold - d0 not run it yet - * _initiate_feedhold() - start feedhold of correct type and finalization - * _feedhold_sync() - planner callback to reach sync point - * _feedhold_with_actions() - perform hold entry actions - */ - -void cm_request_feedhold(cmFeedholdType type, cmFeedholdFinal final) -{ - cm->hold_type = type; - cm->hold_final = final; - cm->hold_state = FEEDHOLD_REQUESTED; - _initiate_feedhold(); // attempt to run it immediately -} - -static void _initiate_feedhold() -{ - // This function is "safe" and will not initiate a feedhold unless it's OK to. - - if ((cm1.hold_state == FEEDHOLD_REQUESTED) && (cm1.motion_state == MOTION_RUN)) { - switch (cm1.hold_type) { - case FEEDHOLD_TYPE_ACTIONS: { op.add_action(_feedhold_with_actions, nullptr); break; } - case FEEDHOLD_TYPE_NO_ACTIONS: { op.add_action(_feedhold_with_no_actions, nullptr); break; } - case FEEDHOLD_TYPE_SYNC: { op.add_action(_feedhold_with_sync, nullptr); break; } - default: { } - } - switch (cm1.hold_final) { - case FEEDHOLD_FINAL_STOP: { op.add_action(_program_stop, nullptr); break; } - case FEEDHOLD_FINAL_END: { op.add_action(_program_end, nullptr); break; } - case FEEDHOLD_FINAL_ALARM: { op.add_action(_alarm, nullptr); break; } - case FEEDHOLD_FINAL_SHUTDOWN: { op.add_action(_shutdown, nullptr); break; } - case FEEDHOLD_FINAL_INTERLOCK: { op.add_action(_interlock, nullptr); break; } - default: { } - } - cm1.hold_state = FEEDHOLD_SYNC; // start feedhold state machine in aline exec - return; - } - - // P2 feedholds only allow feedhold sync types - if ((cm2.hold_state == FEEDHOLD_REQUESTED) && (cm2.motion_state == MOTION_RUN)) { - op.add_action(_feedhold_with_sync, nullptr); - cm2.hold_state = FEEDHOLD_SYNC; - } -} - -static void _feedhold_sync_to_planner(float* vect, bool* flag) -{ - cm1.hold_state = FEEDHOLD_HOLD_DONE; // penultimate state before transitioning to FEEDHOLD_HOLD - sr_request_status_report(SR_REQUEST_IMMEDIATE); -} - -static stat_t _feedhold_with_no_actions(float *param) -{ - return (STAT_OK); -} - -static stat_t _feedhold_with_sync(float *param) -{ - return (STAT_OK); } static stat_t _program_stop(float *param) @@ -391,12 +322,74 @@ static stat_t _interlock(float *param) return (STAT_OK); } +/**************************************************************************************** + * cm_request_feedhold() - request a feedhold - d0 not run it yet + * _initiate_feedhold() - start feedhold of correct type and finalization + * _feedhold_sync_to_planner() - planner callback to reach sync point + * _feedhold_with_sync() + * _feedhold_with_no_actions() + * _feedhold_with_actions() - perform hold entry actions + */ + +void cm_request_feedhold(cmFeedholdType type, cmFeedholdFinal final) +{ + cm->hold_type = type; + cm->hold_final = final; + cm->hold_state = FEEDHOLD_REQUESTED; + _initiate_feedhold(); // attempt to run it immediately +} + +static void _initiate_feedhold() +{ + // This function is "safe" and will not initiate a feedhold unless it's OK to. + + if ((cm1.hold_state == FEEDHOLD_REQUESTED) && (cm1.motion_state == MOTION_RUN)) { + switch (cm1.hold_type) { + case FEEDHOLD_TYPE_ACTIONS: { op.add_action(_feedhold_with_actions, nullptr); break; } + case FEEDHOLD_TYPE_NO_ACTIONS: { op.add_action(_feedhold_with_no_actions, nullptr); break; } + case FEEDHOLD_TYPE_SYNC: { op.add_action(_feedhold_with_sync, nullptr); break; } + default: { } + } + switch (cm1.hold_final) { + case FEEDHOLD_EXIT_STOP: { op.add_action(_program_stop, nullptr); break; } + case FEEDHOLD_EXIT_END: { op.add_action(_program_end, nullptr); break; } + case FEEDHOLD_EXIT_ALARM: { op.add_action(_alarm, nullptr); break; } + case FEEDHOLD_EXIT_SHUTDOWN: { op.add_action(_shutdown, nullptr); break; } + case FEEDHOLD_EXIT_INTERLOCK: { op.add_action(_interlock, nullptr); break; } + default: { } + } + cm1.hold_state = FEEDHOLD_SYNC; // start feedhold state machine in aline exec + return; + } + + // P2 feedholds only allow feedhold sync types + if ((cm2.hold_state == FEEDHOLD_REQUESTED) && (cm2.motion_state == MOTION_RUN)) { + op.add_action(_feedhold_with_sync, nullptr); + cm2.hold_state = FEEDHOLD_SYNC; + } +} + +static void _feedhold_sync_to_planner(float* vect, bool* flag) +{ + cm1.hold_state = FEEDHOLD_HOLD_ACTIONS_COMPLETE; // penultimate state before transitioning to FEEDHOLD_HOLD + sr_request_status_report(SR_REQUEST_IMMEDIATE); +} + +static stat_t _feedhold_with_no_actions(float *param) +{ + return (STAT_OK); +} + +static stat_t _feedhold_with_sync(float *param) +{ + return (STAT_OK); +} static stat_t _feedhold_with_actions(float *param) // Execute Case (5) { // Check to run first-time code - if (cm1.hold_state == FEEDHOLD_HOLD_ACTION_START) { - cm->hold_state = FEEDHOLD_HOLD_PENDING; // next state + if (cm1.hold_state == FEEDHOLD_HOLD_ACTIONS_START) { + cm->hold_state = FEEDHOLD_HOLD_ACTIONS_PENDING; // next state // copy the primary canonical machine to the secondary, // fix the planner pointer, and reset the secondary planner @@ -442,12 +435,12 @@ static stat_t _feedhold_with_actions(float *param) // Execute Case (5) } // wait for hold actions to complete - if (cm1.hold_state == FEEDHOLD_HOLD_PENDING) { + if (cm1.hold_state == FEEDHOLD_HOLD_ACTIONS_PENDING) { return (STAT_EAGAIN); } - // finalize feedhold exit - if (cm1.hold_state == FEEDHOLD_HOLD_DONE) { + // finalize feedhold entry after callback (this is needed so we can return STAT_OK) + if (cm1.hold_state == FEEDHOLD_HOLD_ACTIONS_COMPLETE) { cm1.hold_state = FEEDHOLD_HOLD; return (STAT_OK); } @@ -455,44 +448,45 @@ static stat_t _feedhold_with_actions(float *param) // Execute Case (5) } /**************************************************************************************** - * _feedhold_exit_sync() - planner callback to reach sync point - * _feedhold_exit_with_actions() - perform hold exit actions + * _feedhold_restart_sync_to_planner() - planner callback to reach sync point + * _feedhold_restart_with_no_actions() - perform hold restart with no actions + * _feedhold_restart_with_actions() - perform hold restart with actions */ -static void _feedhold_exit_sync_to_planner(float* vect, bool* flag) +static void _feedhold_restart_sync_to_planner(float* vect, bool* flag) { - cm1.hold_state = FEEDHOLD_HOLD_EXIT_DONE; // penultimate state before transitioning to FEEDHOLD_OFF + cm1.hold_state = FEEDHOLD_EXIT_ACTIONS_COMPLETE; // penultimate state before transitioning to FEEDHOLD_OFF sr_request_status_report(SR_REQUEST_IMMEDIATE); } -static stat_t _feedhold_exit_with_no_actions(float *param) +static stat_t _feedhold_restart_with_no_actions(float *param) { return (STAT_OK); } -static stat_t _feedhold_exit_with_actions(float *param) // Execute Cases (6) and (7) +static stat_t _feedhold_restart_with_actions(float *param) // Execute Cases (6) and (7) { // Check to run first-time code if (cm1.hold_state == FEEDHOLD_HOLD) { // perform end-hold actions --- while still in secondary machine coolant_control_sync(COOLANT_RESUME, COOLANT_BOTH); // resume coolant if paused spindle_control_sync(SPINDLE_RESUME); // resume spindle if paused - + // do return move though an intermediate point; queue a wait cm2.return_flags[AXIS_Z] = false; cm_goto_g30_position(cm2.gmx.g30_position, cm2.return_flags); - mp_queue_command(_feedhold_exit_sync_to_planner, nullptr, nullptr); - cm1.hold_state = FEEDHOLD_HOLD_EXIT_PENDING; + mp_queue_command(_feedhold_restart_sync_to_planner, nullptr, nullptr); + cm1.hold_state = FEEDHOLD_EXIT_ACTIONS_PENDING; return (STAT_EAGAIN); } // wait for exit actions to complete - if (cm1.hold_state == FEEDHOLD_HOLD_EXIT_PENDING) { + if (cm1.hold_state == FEEDHOLD_EXIT_ACTIONS_PENDING) { return (STAT_EAGAIN); } // finalize feedhold exit - if (cm1.hold_state == FEEDHOLD_HOLD_EXIT_DONE) { + if (cm1.hold_state == FEEDHOLD_EXIT_ACTIONS_COMPLETE) { // return to primary planner (p1) cm = &cm1; @@ -505,29 +499,25 @@ static stat_t _feedhold_exit_with_actions(float *param) // Execute Cases (6) a cm_reset_position_to_absolute_position(cm); cm1.flush_state = FLUSH_OFF; } -/* - // resume motion from primary planner or end cycle if no moves in planner - if (mp_has_runnable_buffer(&mp1)) { - cm_cycle_start(); - st_request_exec_move(); - } else { - cm_cycle_end(); - } - cm1.hold_state = FEEDHOLD_OFF; -*/ return (STAT_OK); } return (STAT_EAGAIN); } -static stat_t _cycle_exit(float *param) +static stat_t _restart_cycle(float *param) { + cm1.hold_state = FEEDHOLD_OFF; // must precede st_request_exec_move() if (mp_has_runnable_buffer(&mp1)) { cm_cycle_start(); st_request_exec_move(); } else { cm_cycle_end(); } + return (STAT_OK); +} + +static stat_t _restart_flush(float *param) +{ cm1.hold_state = FEEDHOLD_OFF; return (STAT_OK); } @@ -607,6 +597,8 @@ void cm_queue_flush(cmMachine_t *_cm) + + //static stat_t _run_p1_hold_entry_actions(void); //static void _sync_to_p1_hold_entry_actions_done(float* vect, bool* flag); diff --git a/g2core/gpio.cpp b/g2core/gpio.cpp index c7d0ef02..00cb39e6 100644 --- a/g2core/gpio.cpp +++ b/g2core/gpio.cpp @@ -143,8 +143,7 @@ struct ioDigitalInputExt { if (in->homing_mode) { if (in->edge == INPUT_EDGE_LEADING) { // we only want the leading edge to fire en_take_encoder_snapshot(); -//+++++ cm_start_hold(); - cm_request_feedhold(FEEDHOLD_TYPE_SYNC, FEEDHOLD_FINAL_STOP); + cm_request_feedhold(FEEDHOLD_TYPE_SYNC, FEEDHOLD_EXIT_STOP); } return; } @@ -155,8 +154,7 @@ struct ioDigitalInputExt { // Probing tests the start condition for the correct direction ahead of time. // If we see any edge, it's the right one. en_take_encoder_snapshot(); -//+++++ cm_start_hold(); - cm_request_feedhold(FEEDHOLD_TYPE_SYNC, FEEDHOLD_FINAL_STOP); + cm_request_feedhold(FEEDHOLD_TYPE_SYNC, FEEDHOLD_EXIT_STOP); return; } @@ -165,12 +163,10 @@ struct ioDigitalInputExt { // trigger the action on leading edges if (in->edge == INPUT_EDGE_LEADING) { if (in->action == INPUT_ACTION_STOP) { -//+++++ cm_start_hold(); - cm_request_feedhold(FEEDHOLD_TYPE_SYNC, FEEDHOLD_FINAL_STOP); + cm_request_feedhold(FEEDHOLD_TYPE_SYNC, FEEDHOLD_EXIT_STOP); } if (in->action == INPUT_ACTION_FAST_STOP) { -//+++++ cm_start_hold(); // for now is same as STOP - cm_request_feedhold(FEEDHOLD_TYPE_SYNC, FEEDHOLD_FINAL_STOP); + cm_request_feedhold(FEEDHOLD_TYPE_SYNC, FEEDHOLD_EXIT_STOP); } if (in->action == INPUT_ACTION_HALT) { cm_halt(); // hard stop, including spindle, coolant and heaters diff --git a/g2core/plan_exec.cpp b/g2core/plan_exec.cpp index ffb09ccb..1fe32a0d 100644 --- a/g2core/plan_exec.cpp +++ b/g2core/plan_exec.cpp @@ -484,9 +484,8 @@ stat_t mp_exec_aline(mpBuf_t *bf) // Feedhold Processing - We need to handle the following cases (listed in rough sequence order): if (cm->hold_state != FEEDHOLD_OFF) { - // if FEEDHOLD_P2_START, FEEDHOLD_P2_WAIT, FEEDHOLD HOLD or FEEDHOLD_P2_EXIT -// if (cm->hold_state >= FEEDHOLD_P2_START) { // handles _exec_aline_feedhold_processing case (7) - if (cm->hold_state >= FEEDHOLD_HOLD_ACTION_START) { // handles _exec_aline_feedhold_processing case (7) + // if running actions, or in HOLD state, or exiting with actions + if (cm->hold_state >= FEEDHOLD_HOLD_ACTIONS_START) { // handles _exec_aline_feedhold_processing case (7) return (STAT_NOOP); // VERY IMPORTANT to exit as a NOOP. Do not load another move } // STAT_OK terminates aline execution for this move @@ -1032,18 +1031,20 @@ static stat_t _exec_aline_feedhold(mpBuf_t *bf) // If in a p2 hold, exit the p2 hold immediately set up a flush of the p2 planner queue if (cm == &cm2) { - cm->hold_state = FEEDHOLD_HOLD_EXIT_PENDING; +// cm->hold_type = FEEDHOLD_TYPE_SYNC; // force to a sync hold + cm->hold_state = FEEDHOLD_HOLD; return (STAT_OK); // will end this exec_aline() with no more movement } // At this point we know we are in a p1 hold - + // If probing or homing, exit the move and advance to the _motion_end_callback()'s. // Stop the runtime, clear the run buffer and do not transition to p2 planner. else if ((cm->cycle_type == CYCLE_HOMING) || (cm->cycle_type == CYCLE_PROBE)) { +// else if (cm->hold_type == FEEDHOLD_TYPE_SYNC) { mr->block_state = BLOCK_INACTIVE; // disable the rest of the runtime movement mp_free_run_buffer(); // free buffer and enable finalization move to get loaded cm->hold_state = FEEDHOLD_OFF; - } + } // In a regular p1 hold. Motion has stopped, so we can rely on positions and other values to be stable else { @@ -1057,7 +1058,11 @@ static stat_t _exec_aline_feedhold(mpBuf_t *bf) st_request_forward_plan(); // replan the current bf buffer // Set state to enable transition to p2 and perform entry actions in the p2 planner - cm->hold_state = FEEDHOLD_HOLD_ACTION_START; // signal operations runner to start actions + if (cm->hold_type == FEEDHOLD_TYPE_ACTIONS) { + cm->hold_state = FEEDHOLD_HOLD_ACTIONS_START; // signal to start entry actions + } else { + cm->hold_state = FEEDHOLD_HOLD; // achieved hold state + } } sr_request_status_report(SR_REQUEST_IMMEDIATE);