Mirrors_Framework/g2
1
0
Fork 0
mirror of https://github.com/synthetos/g2.git synced 2026-02-06 11:11:57 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Correct handling of interlock pin changes

Browse Source
This commit is contained in:
Rob Giseburt
2019-11-01 10:18:01 -05:00
parent 83fdc1d62c
commit be0efcc175
3 changed files with 53 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
g2core/canonical_machine.h
View File

@@ -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,

27
g2core/controller.cpp
View File

@@ -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
}
}

54
g2core/cycle_feedhold.cpp
View File

@@ -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);
}