diff --git a/g2core/canonical_machine.h b/g2core/canonical_machine.h index 7a3339fd..e35e7a41 100644 --- a/g2core/canonical_machine.h +++ b/g2core/canonical_machine.h @@ -175,8 +175,8 @@ typedef enum { // applies to cm->probe_state PROBE_WAITING = 2 // probe is waiting to be started or is running } cmProbeState; - typedef enum { #ifdef ENABLE_INTERLOCK_AND_ESTOP + typedef enum { ESTOP_RELEASED = 0, // pressed/released is physical state, acked/unacked is machine control state, active/inactive is whether we're currently in estop mode ESTOP_ACKED = 0, ESTOP_INACTIVE = 0, @@ -188,8 +188,10 @@ typedef enum { // applies to cm->probe_state ESTOP_ACK_MASK = 0x2, ESTOP_PRESSED_MASK = 0x1, } cmEstopState; +#endif typedef enum { +#ifdef ENABLE_INTERLOCK_AND_ESTOP SAFETY_INTERLOCK_CLOSED = 0, SAFETY_INTERLOCK_OPEN = 0x1, @@ -201,11 +203,12 @@ typedef enum { SAFETY_INTERLOCK_MASK = 0x1, SAFETY_ESC_MASK = 0xE, -#else - SAFETY_INTERLOCK_ENGAGED = 0, // meaning the interlock input is CLOSED (low) - SAFETY_INTERLOCK_DISENGAGED #endif -} cmSafetyState; + SAFETY_INTERLOCK_ENGAGED = 0, // meaning the interlock input is CLOSED (low) + SAFETY_INTERLOCK_DISENGAGING, // meaning the interlock opened and we're dealing with it + SAFETY_INTERLOCK_DISENGAGED, + SAFETY_INTERLOCK_ENGAGING +} cmSafetyState; typedef enum { // feed override state machine MFO_OFF = 0, diff --git a/g2core/controller.cpp b/g2core/controller.cpp index 7c5dcbd9..dca32bea 100644 --- a/g2core/controller.cpp +++ b/g2core/controller.cpp @@ -115,10 +115,10 @@ gpioDigitalInputHandler _limit_input_handler { gpioDigitalInputHandler _interlock_input_handler{ [&](const bool state, const inputEdgeFlag edge, const uint8_t triggering_pin_number) { - if (edge == INPUT_EDGE_LEADING) { - cm->safety_interlock_disengaged = triggering_pin_number; + if (edge != INPUT_EDGE_LEADING) { + cm1.safety_interlock_disengaged = triggering_pin_number; } else { // edge == INPUT_EDGE_TRAILING - cm->safety_interlock_reengaged = triggering_pin_number; + cm1.safety_interlock_reengaged = triggering_pin_number; } return GPIO_HANDLED; @@ -636,22 +636,19 @@ static stat_t _interlock_estop_handler(void) #else static stat_t _interlock_handler(void) { - if (cm->safety_interlock_enable) { - // interlock broken - if (cm->safety_interlock_disengaged != 0) { - cm->safety_interlock_disengaged = 0; - cm->safety_interlock_state = SAFETY_INTERLOCK_DISENGAGED; + // NOTE: Always use cm1. directly for interlock state! + if (cm1.safety_interlock_enable) { + // interlock broken + if ((cm1.safety_interlock_disengaged != 0) && (cm1.safety_interlock_state == SAFETY_INTERLOCK_ENGAGED)) { + cm1.safety_interlock_disengaged = 0; + cm1.safety_interlock_state = SAFETY_INTERLOCK_DISENGAGING; 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 } // interlock restored - if ((cm->safety_interlock_reengaged != 0) && (mp_runtime_is_idle())) { - cm->safety_interlock_reengaged = 0; - cm->safety_interlock_state = SAFETY_INTERLOCK_ENGAGED; // interlock restored -// cm_request_exit_hold(); // use cm_request_exit_hold() instead of just ending +++++ + if ((cm1.safety_interlock_reengaged != 0) && mp_runtime_is_idle() && (cm1.safety_interlock_state == SAFETY_INTERLOCK_DISENGAGED)) { + cm1.safety_interlock_reengaged = 0; + cm1.safety_interlock_state = SAFETY_INTERLOCK_ENGAGING; // interlock restored cm_request_cycle_start(); // proper way to restart the cycle } } diff --git a/g2core/cycle_feedhold.cpp b/g2core/cycle_feedhold.cpp index 78cfbc0b..2a3f1b94 100644 --- a/g2core/cycle_feedhold.cpp +++ b/g2core/cycle_feedhold.cpp @@ -57,7 +57,8 @@ static stat_t _run_program_stop(void); static stat_t _run_program_end(void); static stat_t _run_alarm(void); static stat_t _run_shutdown(void); -static stat_t _run_interlock(void); +static stat_t _run_interlock_started(void); +static stat_t _run_interlock_ended(void); static stat_t _run_reset_position(void); /**************************************************************************************** @@ -181,7 +182,7 @@ void cm_operation_init() * A cycle_start (~) returns to p1 and exits the feedhold, performing exit actions if entry * actions were performed. Motion resumes in p1 from the held point. * - * A queue_flush (~) returns to p1 and exits the feedhold, performing exit actions if entry + * A queue_flush (%) returns to p1 and exits the feedhold, performing exit actions if entry * actions were performed. The p1 planner is flushed, and motion does not resume. The machine * executes a program_stop, and ends in the STOP state. * @@ -329,10 +330,15 @@ static stat_t _run_shutdown() { cm->machine_state = MACHINE_SHUTDOWN; return (STAT_OK); } -static stat_t _run_interlock() { - cm->machine_state = MACHINE_INTERLOCK; +static stat_t _run_interlock_started() { + cm1.safety_interlock_state = SAFETY_INTERLOCK_DISENGAGED; + // cm->machine_state = MACHINE_INTERLOCK; return (STAT_OK); } +static stat_t _run_interlock_ended() { + cm1.safety_interlock_state = SAFETY_INTERLOCK_ENGAGED; + return (_run_restart_cycle()); +} /**************************************************************************************** * cm_request_cycle_start() - set request enum only @@ -371,9 +377,7 @@ static void _start_cycle_restart() case FEEDHOLD_EXIT_FLUSH: { op.add_action(_run_queue_flush); } // no break case FEEDHOLD_EXIT_STOP: { op.add_action(_run_program_stop); break; } case FEEDHOLD_EXIT_END: { op.add_action(_run_program_end); break; } - case FEEDHOLD_EXIT_ALARM: { op.add_action(_run_alarm); break; } - case FEEDHOLD_EXIT_SHUTDOWN: { op.add_action(_run_shutdown); break; } - case FEEDHOLD_EXIT_INTERLOCK: { op.add_action(_run_interlock); break; } + case FEEDHOLD_EXIT_INTERLOCK: { op.add_action(_run_interlock_ended); break; } default: {} } } @@ -405,7 +409,7 @@ void cm_request_queue_flush() static void _start_queue_flush() { devflags_t flags = DEV_IS_DATA; - + // Don't initiate the queue until in HOLD state (this also means that runtime is idle) if ((cm1.queue_flush_state == QUEUE_FLUSH_REQUESTED) && (cm1.hold_state == FEEDHOLD_HOLD)) { xio_flush_device(flags); @@ -535,16 +539,15 @@ static void _start_job_kill() void cm_request_feedhold(cmFeedholdType type, cmFeedholdExit exit) { - // Can only initiate a feedhold if you are in a machining cycle, running, not already in a feedhold, and e-stop is not pressed - - // +++++ This needs to be extended to allow HOLDs to be requested when motion has stopped - if ((cm1.hold_state == FEEDHOLD_OFF) && + // Can only initiate a feedhold if not already in a feedhold + if ((cm1.hold_state == FEEDHOLD_OFF) #ifdef ENABLE_INTERLOCK_AND_ESTOP - (cm1.machine_state == MACHINE_CYCLE) && (cm1.motion_state == MOTION_RUN) && - (cm1.estop_state == 0)) { -#else - (cm1.machine_state == MACHINE_CYCLE) && (cm1.motion_state == MOTION_RUN)) { + && (cm1.estop_state == 0) #endif + ) { + // OLD: Can only initiate a feedhold if you are in a machining cycle, running, and not already in a feedhold + // OLD: && (cm1.machine_state == MACHINE_CYCLE) && (cm1.motion_state == MOTION_RUN) + // NEW: Will run the operations even if not moving, they will each need to handle cycle/running state correctly cm1.hold_type = type; cm1.hold_exit = exit; @@ -563,7 +566,7 @@ void cm_request_feedhold(cmFeedholdType type, cmFeedholdExit exit) case FEEDHOLD_EXIT_END: { op.add_action(_run_program_end); break; } case FEEDHOLD_EXIT_ALARM: { op.add_action(_run_alarm); break; } case FEEDHOLD_EXIT_SHUTDOWN: { op.add_action(_run_shutdown); break; } - case FEEDHOLD_EXIT_INTERLOCK: { op.add_action(_run_interlock); break; } + case FEEDHOLD_EXIT_INTERLOCK: { op.add_action(_run_interlock_started); break; } case FEEDHOLD_EXIT_RESET_POSITION: { op.add_action(_run_reset_position); break; } default: {} } @@ -653,9 +656,12 @@ static void _check_motion_stopped() // Motion has stopped, so we can rely on positions and other values to be stable // If SKIP type, discard the remainder of the block and position to the next block - if (cm->hold_type == FEEDHOLD_TYPE_SKIP) { + // OR if the buffer is empty then there's nothing to discard, don't modify the buffer either + if ((cm->hold_type == FEEDHOLD_TYPE_SKIP) || (bf->buffer_state == MP_BUFFER_EMPTY)) { copy_vector(mp->position, mr->position); // update planner position to the final runtime position - mp_free_run_buffer(); // advance to next block, discarding the rest of the move + if (mp_get_run_buffer()) { + mp_free_run_buffer(); // advance to next block, discarding the rest of the move + } } else { // Otherwise setup the block to complete motion (regardless of how hold will ultimately be exited) bf->length = get_axis_vector_length(mr->position, mr->target); // update bf w/remaining length in move bf->block_state = BLOCK_INITIAL_ACTION; // tell _exec to re-use the bf buffer @@ -671,6 +677,11 @@ static void _check_motion_stopped() static stat_t _feedhold_skip() { + // check for actual motion to stop + if (cm1.machine_state != MACHINE_CYCLE) { + return (STAT_OK); + } + if (cm1.hold_state == FEEDHOLD_OFF) { // if entered while OFF start a feedhold cm1.hold_type = FEEDHOLD_TYPE_SKIP; cm1.hold_state = FEEDHOLD_SYNC; // ...FLUSH can be overridden by setting hold_exit after this function @@ -726,6 +737,7 @@ static stat_t _feedhold_with_actions() // Execute Case (5) if (cm1.motion_state == MOTION_STOP) { // if motion has already stopped declare that you are in a feedhold _check_motion_stopped(); cm1.hold_state = FEEDHOLD_HOLD; + cm1.hold_type = FEEDHOLD_TYPE_HOLD; // no actions will be performed, don't try to undo them } else { cm1.hold_state = FEEDHOLD_SYNC; // ... STOP can be overridden by setting hold_exit after this function return (STAT_EAGAIN); @@ -757,8 +769,8 @@ static stat_t _feedhold_with_actions() // Execute Case (5) return (STAT_EAGAIN); } - // finalize feedhold entry after callback (this is needed so we can return STAT_OK) - if (cm1.hold_state == FEEDHOLD_HOLD_ACTIONS_COMPLETE) { + // finalize feedhold entry after callback OR skipping actions (this is needed so we can return STAT_OK) + if ((cm1.hold_state == FEEDHOLD_HOLD_ACTIONS_COMPLETE) || (cm1.hold_state == FEEDHOLD_HOLD)) { cm1.hold_state = FEEDHOLD_HOLD; return (STAT_OK); }