From 0923a24f2f38ac4cc55d63c6a62909b5f75d3cda Mon Sep 17 00:00:00 2001 From: Matt Staniszewski Date: Wed, 30 Oct 2019 20:05:45 -0400 Subject: [PATCH] Added Bantam Tools E-Stop and Interlock Handler, Fixes #7 --- g2core/alarm.cpp | 5 +++ g2core/canonical_machine.cpp | 56 ++++++++++++++++++++++- g2core/canonical_machine.h | 54 +++++++++++++++++++++- g2core/config_app.cpp | 45 ++++++++++--------- g2core/controller.cpp | 86 ++++++++++++++++++++++++++++++++++++ g2core/cycle_feedhold.cpp | 18 +++++++- g2core/spindle.cpp | 21 +++++++++ 7 files changed, 261 insertions(+), 24 deletions(-) diff --git a/g2core/alarm.cpp b/g2core/alarm.cpp index 8c9bd395..d0e9cb56 100644 --- a/g2core/alarm.cpp +++ b/g2core/alarm.cpp @@ -86,11 +86,16 @@ void cm_clear() } else if (cm->machine_state == MACHINE_SHUTDOWN) { cm->machine_state = MACHINE_READY; } + } void cm_parse_clear(const char *s) { +#ifdef ENABLE_INTERLOCK_AND_ESTOP + if (cm->machine_state == MACHINE_ALARM || (cm->machine_state == MACHINE_SHUTDOWN && cm->estop_state != 0)) { +#else if (cm->machine_state == MACHINE_ALARM) { +#endif if (toupper(s[0]) == 'M') { if (( (s[1]=='3') && (s[2]=='0') && (s[3]==0)) || ((s[1]=='2') && (s[2]==0) )) { cm_clear(); diff --git a/g2core/canonical_machine.cpp b/g2core/canonical_machine.cpp index 8d1dd56f..0aa1bf72 100644 --- a/g2core/canonical_machine.cpp +++ b/g2core/canonical_machine.cpp @@ -304,6 +304,12 @@ void canonical_machine_reset(cmMachine_t *_cm) _cm->gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE; // never start in a motion mode _cm->machine_state = MACHINE_READY; +#ifdef ENABLE_INTERLOCK_AND_ESTOP + _cm->safety_state = _cm->estop_state = 0; + _cm->esc_boot_timer = Motate::SysTickTimer.getValue(); + _cm->safety_state = SAFETY_ESC_REBOOTING; +#endif + cm_operation_init(); // reset operations runner canonical_machine_reset_rotation(_cm); @@ -1691,7 +1697,7 @@ static void _exec_program_finalize(float *value, bool *flag) // Allow update in the alarm state, to accommodate queue flush (RAS) if ((cm->cycle_type == CYCLE_MACHINING || cm->cycle_type == CYCLE_NONE) && -// (cm->machine_state != MACHINE_ALARM) && // omitted by OMC (RAS) + (cm->machine_state != MACHINE_ALARM) && (cm->machine_state != MACHINE_SHUTDOWN)) { cm->machine_state = machine_state; // don't update macs/cycs if we're in the middle of a canned cycle, cm->cycle_type = CYCLE_NONE; // or if we're in machine alarm/shutdown mode @@ -1763,6 +1769,16 @@ void cm_program_end() mp_queue_command(_exec_program_finalize, value, nullptr); } +#ifdef ENABLE_INTERLOCK_AND_ESTOP +stat_t cm_ack_estop(nvObj_t *nv) +{ + cm->estop_state &= ~ESTOP_UNACKED; + nv->value_flt = (float)cm->estop_state; + nv->valuetype = TYPE_FLOAT; + return (STAT_OK); +} +#endif + /**************************************************************************************** **** Additional Functions ************************************************************** ****************************************************************************************/ @@ -2075,6 +2091,21 @@ static const char msg_g94[] = "G94 - units-per-minute mode (i.e. feedrate mode)" static const char msg_g95[] = "G95 - units-per-revolution mode"; static const char *const msg_frmo[] = { msg_g93, msg_g94, msg_g95 }; +#ifdef ENABLE_INTERLOCK_AND_ESTOP +static const char msg_safe0[] = "Interlock Circuit Closed/ESC nominal"; +static const char msg_safe1[] = "Interlock Circuit Broken/ESC nominal"; +static const char msg_safe2[] = "Interlock Circuit Closed/ESC rebooting"; +static const char msg_safe3[] = "Interlock Circuit Broken/ESC rebooting"; +static const char *const msg_safe[] = { msg_safe0, msg_safe1, msg_safe2, msg_safe3 }; + +static const char msg_estp0[] = "E-Stop Circuit Closed"; +static const char msg_estp1[] = "E-Stop Circuit Closed but unacked"; +static const char msg_estp2[] = "E-Stop Circuit Broken and acked"; +static const char msg_estp3[] = "E-Stop Circuit Broken and unacked"; +// Don't worry about indicating the "Active" state +static const char *const msg_estp[] = { msg_estp0, msg_estp1, msg_estp2, msg_estp3 }; +#endif + #else #define msg_units NULL @@ -2092,6 +2123,10 @@ static const char *const msg_frmo[] = { msg_g93, msg_g94, msg_g95 }; #define msg_dist NULL #define msg_admo NULL #define msg_frmo NULL +#ifdef ENABLE_INTERLOCK_AND_ESTOP +#define msg_safe NULL +#define msg_estp NULL +#endif #define msg_am NULL #endif // __TEXT_MODE @@ -2120,6 +2155,17 @@ stat_t cm_get_dist(nvObj_t *nv) { return(_get_msg_helper(nv, msg_dist, cm_get_di stat_t cm_get_admo(nvObj_t *nv) { return(_get_msg_helper(nv, msg_admo, cm_get_arc_distance_mode(ACTIVE_MODEL)));} stat_t cm_get_frmo(nvObj_t *nv) { return(_get_msg_helper(nv, msg_frmo, cm_get_feed_rate_mode(ACTIVE_MODEL)));} +#ifdef ENABLE_INTERLOCK_AND_ESTOP +stat_t cm_get_safe(nvObj_t *nv) { + uint8_t safe = 0; + if((cm->safety_state & SAFETY_INTERLOCK_MASK) != 0) + safe |= 0x1; + if((cm->safety_state & SAFETY_ESC_MASK) != 0) + safe |= 0x2; + return(_get_msg_helper(nv, msg_safe, safe)); } +stat_t cm_get_estp(nvObj_t *nv) { return(_get_msg_helper(nv, msg_estp, (cm->estop_state & 0x3))); } +#endif + stat_t cm_get_toolv(nvObj_t *nv) { return(get_integer(nv, cm_get_tool(ACTIVE_MODEL))); } stat_t cm_get_mline(nvObj_t *nv) { return(get_integer(nv, cm_get_linenum(MODEL))); } stat_t cm_get_line(nvObj_t *nv) { return(get_integer(nv, cm_get_linenum(ACTIVE_MODEL))); } @@ -2480,6 +2526,10 @@ static const char fmt_dist[] = "Distance mode: %s\n"; static const char fmt_admo[] = "Arc Distance mode: %s\n"; static const char fmt_frmo[] = "Feed rate mode: %s\n"; static const char fmt_tool[] = "Tool number %d\n"; +#ifdef ENABLE_INTERLOCK_AND_ESTOP +static const char fmt_safe[] = "Safety System Flags: %s\n"; +static const char fmt_estp[] = "Emergency Stop: %s\n"; +#endif static const char fmt_g92e[] = "G92 enabled %d\n"; void cm_print_vel(nvObj_t *nv) { text_print_flt_units(nv, fmt_vel, GET_UNITS(ACTIVE_MODEL));} @@ -2501,6 +2551,10 @@ void cm_print_path(nvObj_t *nv) { text_print_str(nv, fmt_path);} void cm_print_dist(nvObj_t *nv) { text_print_str(nv, fmt_dist);} void cm_print_admo(nvObj_t *nv) { text_print_str(nv, fmt_admo);} void cm_print_frmo(nvObj_t *nv) { text_print_str(nv, fmt_frmo);} +#ifdef ENABLE_INTERLOCK_AND_ESTOP +void cm_print_safe(nvObj_t *nv) { text_print_str(nv, fmt_safe);} +void cm_print_estp(nvObj_t *nv) { text_print_str(nv, fmt_estp);} +#endif static const char fmt_gpl[] = "[gpl] default gcode plane%10d [0=G17,1=G18,2=G19]\n"; static const char fmt_gun[] = "[gun] default gcode units mode%5d [0=G20,1=G21]\n"; diff --git a/g2core/canonical_machine.h b/g2core/canonical_machine.h index 1f733aba..7a3339fd 100644 --- a/g2core/canonical_machine.h +++ b/g2core/canonical_machine.h @@ -175,10 +175,37 @@ 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 + 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, + ESTOP_PRESSED = 0x1, + ESTOP_UNACKED = 0x2, + ESTOP_ACTIVE = 0x4, + + ESTOP_ACTIVE_MASK = 0x4, + ESTOP_ACK_MASK = 0x2, + ESTOP_PRESSED_MASK = 0x1, +} cmEstopState; + typedef enum { + SAFETY_INTERLOCK_CLOSED = 0, + SAFETY_INTERLOCK_OPEN = 0x1, + + SAFETY_ESC_ONLINE = 0, + SAFETY_ESC_OFFLINE = 0x2, + SAFETY_ESC_LOCKOUT = 0x4, + SAFETY_ESC_REBOOTING = 0x8, + SAFETY_ESC_LOCKOUT_AND_REBOOTING = 0xC, + + SAFETY_INTERLOCK_MASK = 0x1, + SAFETY_ESC_MASK = 0xE, +#else SAFETY_INTERLOCK_ENGAGED = 0, // meaning the interlock input is CLOSED (low) SAFETY_INTERLOCK_DISENGAGED -} cmSafetyState; +#endif +} cmSafetyState; typedef enum { // feed override state machine MFO_OFF = 0, @@ -329,6 +356,14 @@ typedef struct cmMachine { // struct to manage canonical machin void *mp; // linked mpPlanner_t - use a void pointer to avoid circular header files cmArc_t arc; // arc parameters GCodeState_t *am; // active Gcode model is maintained by state management + +#ifdef ENABLE_INTERLOCK_AND_ESTOP + uint8_t safety_state; // Tracks whether interlock has been triggered, whether esc is rebooting, etc + uint8_t estop_state; // Whether estop has been triggered + uint32_t esc_boot_timer; // When the ESC last booted up + uint32_t esc_lockout_timer; // When the ESC lockout last triggered +#endif + GCodeState_t gm; // core gcode model state GCodeStateX_t gmx; // extended gcode model state @@ -483,6 +518,11 @@ void cm_program_stop(void); // M0 void cm_optional_program_stop(void); // M1 void cm_program_end(void); // M2 +// E-Stop +#ifdef ENABLE_INTERLOCK_AND_ESTOP +stat_t cm_ack_estop(nvObj_t *nv); +#endif + stat_t cm_json_command(char *json_string); // M100 stat_t cm_json_command_immediate(char *json_string); // M100.1 stat_t cm_json_wait(char *json_string); // M102 @@ -576,6 +616,10 @@ stat_t cm_get_mpo(nvObj_t *nv); // get runtime machine position stat_t cm_get_ofs(nvObj_t *nv); // get runtime work offset stat_t cm_get_coord(nvObj_t *nv); // get coordinate offset stat_t cm_set_coord(nvObj_t *nv); // set coordinate offset +#ifdef ENABLE_INTERLOCK_AND_ESTOP +stat_t cm_get_safe(nvObj_t *nv); // get interlock state +stat_t cm_get_estp(nvObj_t *nv); // get E-stop state +#endif stat_t cm_get_g92e(nvObj_t *nv); // get g92 enable state stat_t cm_get_g92(nvObj_t *nv); // get g92 offset @@ -695,6 +739,10 @@ stat_t cm_set_gdi(nvObj_t *nv); // set gcode default distance mode void cm_print_admo(nvObj_t *nv); void cm_print_frmo(nvObj_t *nv); void cm_print_tool(nvObj_t *nv); +#ifdef ENABLE_INTERLOCK_AND_ESTOP + void cm_print_safe(nvObj_t *nv); + void cm_print_estp(nvObj_t *nv); +#endif void cm_print_g92e(nvObj_t *nv); void cm_print_gpl(nvObj_t *nv); // Gcode defaults @@ -764,6 +812,10 @@ stat_t cm_set_gdi(nvObj_t *nv); // set gcode default distance mode #define cm_print_admo tx_print_stub #define cm_print_frmo tx_print_stub #define cm_print_tool tx_print_stub +#ifdef ENABLE_INTERLOCK_AND_ESTOP + #define cm_print_safe tx_print_stub + #define cm_print_estp tx_print_stub +#endif #define cm_print_g92e tx_print_stub #define cm_print_gpl tx_print_stub // Gcode defaults diff --git a/g2core/config_app.cpp b/g2core/config_app.cpp index ebea8b2e..e288db73 100644 --- a/g2core/config_app.cpp +++ b/g2core/config_app.cpp @@ -115,26 +115,31 @@ const cfgItem_t cfgArray[] = { { "sys", "id", _sn, 0, hw_print_id, hw_get_id, set_ro, nullptr, 0 }, // device ID (ASCII signature) // dynamic model attributes for reporting purposes (up front for speed) - { "", "stat",_i0, 0, cm_print_stat, cm_get_stat, set_ro, nullptr, 0 }, // combined machine state - { "","stat2",_i0, 0, cm_print_stat, cm_get_stat2,set_ro, nullptr, 0 }, // combined machine state - { "", "n", _ii, 0, cm_print_line, cm_get_mline,set_noop,nullptr,0 }, // Model line number - { "", "line",_ii, 0, cm_print_line, cm_get_line, set_ro, nullptr, 0 }, // Active line number - model or runtime line number - { "", "vel", _f0, 2, cm_print_vel, cm_get_vel, set_ro, nullptr, 0 }, // current velocity - { "", "feed",_f0, 2, cm_print_feed, cm_get_feed, set_ro, nullptr, 0 }, // feed rate - { "", "macs",_i0, 0, cm_print_macs, cm_get_macs, set_ro, nullptr, 0 }, // raw machine state - { "", "cycs",_i0, 0, cm_print_cycs, cm_get_cycs, set_ro, nullptr, 0 }, // cycle state - { "", "mots",_i0, 0, cm_print_mots, cm_get_mots, set_ro, nullptr, 0 }, // motion state - { "", "hold",_i0, 0, cm_print_hold, cm_get_hold, set_ro, nullptr, 0 }, // feedhold state - { "", "unit",_i0, 0, cm_print_unit, cm_get_unit, set_ro, nullptr, 0 }, // units mode - { "", "coor",_i0, 0, cm_print_coor, cm_get_coor, set_ro, nullptr, 0 }, // coordinate system - { "", "momo",_i0, 0, cm_print_momo, cm_get_momo, set_ro, nullptr, 0 }, // motion mode - { "", "plan",_i0, 0, cm_print_plan, cm_get_plan, set_ro, nullptr, 0 }, // plane select - { "", "path",_i0, 0, cm_print_path, cm_get_path, set_ro, nullptr, 0 }, // path control mode - { "", "dist",_i0, 0, cm_print_dist, cm_get_dist, set_ro, nullptr, 0 }, // distance mode - { "", "admo",_i0, 0, cm_print_admo, cm_get_admo, set_ro, nullptr, 0 }, // arc distance mode - { "", "frmo",_i0, 0, cm_print_frmo, cm_get_frmo, set_ro, nullptr, 0 }, // feed rate mode - { "", "tool",_i0, 0, cm_print_tool, cm_get_toolv,set_ro, nullptr, 0 }, // active tool - { "", "g92e",_i0, 0, cm_print_g92e, cm_get_g92e, set_ro, nullptr, 0 }, // G92 enable state + { "", "stat", _i0, 0, cm_print_stat, cm_get_stat, set_ro, nullptr, 0 }, // combined machine state + { "","stat2", _i0, 0, cm_print_stat, cm_get_stat2, set_ro, nullptr, 0 }, // combined machine state + { "", "n", _ii, 0, cm_print_line, cm_get_mline, set_noop, nullptr, 0 }, // Model line number + { "", "line", _ii, 0, cm_print_line, cm_get_line, set_ro, nullptr, 0 }, // Active line number - model or runtime line number + { "", "vel", _f0, 2, cm_print_vel, cm_get_vel, set_ro, nullptr, 0 }, // current velocity + { "", "feed", _f0, 2, cm_print_feed, cm_get_feed, set_ro, nullptr, 0 }, // feed rate + { "", "macs", _i0, 0, cm_print_macs, cm_get_macs, set_ro, nullptr, 0 }, // raw machine state + { "", "cycs", _i0, 0, cm_print_cycs, cm_get_cycs, set_ro, nullptr, 0 }, // cycle state + { "", "mots", _i0, 0, cm_print_mots, cm_get_mots, set_ro, nullptr, 0 }, // motion state + { "", "hold", _i0, 0, cm_print_hold, cm_get_hold, set_ro, nullptr, 0 }, // feedhold state + { "", "unit", _i0, 0, cm_print_unit, cm_get_unit, set_ro, nullptr, 0 }, // units mode + { "", "coor", _i0, 0, cm_print_coor, cm_get_coor, set_ro, nullptr, 0 }, // coordinate system + { "", "momo", _i0, 0, cm_print_momo, cm_get_momo, set_ro, nullptr, 0 }, // motion mode + { "", "plan", _i0, 0, cm_print_plan, cm_get_plan, set_ro, nullptr, 0 }, // plane select + { "", "path", _i0, 0, cm_print_path, cm_get_path, set_ro, nullptr, 0 }, // path control mode + { "", "dist", _i0, 0, cm_print_dist, cm_get_dist, set_ro, nullptr, 0 }, // distance mode + { "", "admo", _i0, 0, cm_print_admo, cm_get_admo, set_ro, nullptr, 0 }, // arc distance mode + { "", "frmo", _i0, 0, cm_print_frmo, cm_get_frmo, set_ro, nullptr, 0 }, // feed rate mode + { "", "tool", _i0, 0, cm_print_tool, cm_get_toolv, set_ro, nullptr, 0 }, // active tool +#ifdef ENABLE_INTERLOCK_AND_ESTOP + { "", "safe", _i0, 0, cm_print_safe, cm_get_safe, set_ro, nullptr, 0 }, // interlock status + { "", "estp", _i0, 0, cm_print_estp, cm_get_estp, cm_ack_estop, nullptr, 0 }, // E-stop status (SET to ack) + { "", "estpc",_i0, 0, cm_print_estp, cm_ack_estop, cm_ack_estop, nullptr, 0 }, // E-stop status clear (GET to ack) +#endif + { "", "g92e", _i0, 0, cm_print_g92e, cm_get_g92e, set_ro, nullptr, 0 }, // G92 enable state #ifdef TEMPORARY_HAS_LEDS { "", "_leds",_i0, 0, tx_print_nul, _get_leds,_set_leds, nullptr, 0 }, // TEMPORARY - change LEDs #endif diff --git a/g2core/controller.cpp b/g2core/controller.cpp index 714f7292..0e77639e 100644 --- a/g2core/controller.cpp +++ b/g2core/controller.cpp @@ -45,6 +45,9 @@ #include "util.h" #include "xio.h" #include "settings.h" +#ifdef ENABLE_INTERLOCK_AND_ESTOP +#include "spindle.h" +#endif #include "MotatePower.h" @@ -65,7 +68,11 @@ controller_t cs; // controller state structure static void _controller_HSM(void); static stat_t _led_indicator(void); // twiddle the LED indicator static stat_t _shutdown_handler(void); // new (replaces _interlock_estop_handler) +#ifdef ENABLE_INTERLOCK_AND_ESTOP +static stat_t _interlock_estop_handler(void); +#else static stat_t _interlock_handler(void); // new (replaces _interlock_estop_handler) +#endif static stat_t _limit_switch_handler(void); // revised for new GPIO code static void _init_assertions(void); @@ -194,7 +201,11 @@ static void _controller_HSM() DISPATCH(hardware_periodic()); // give the hardware a chance to do stuff DISPATCH(_led_indicator()); // blink LEDs at the current rate DISPATCH(_shutdown_handler()); // invoke shutdown +#ifdef ENABLE_INTERLOCK_AND_ESTOP + DISPATCH(_interlock_estop_handler()); // interlock or estop have been thrown +#else DISPATCH(_interlock_handler()); // invoke / remove safety interlock +#endif DISPATCH(temperature_callback()); // makes sure temperatures are under control DISPATCH(_limit_switch_handler()); // invoke limit switch DISPATCH(_controller_state()); // controller state management @@ -254,7 +265,11 @@ static stat_t _dispatch_control() static stat_t _dispatch_command() { +#ifdef ENABLE_INTERLOCK_AND_ESTOP + if (cs.controller_state != CONTROLLER_PAUSED && cm->estop_state == 0) { +#else if (cs.controller_state != CONTROLLER_PAUSED) { +#endif devflags_t flags = DEV_IS_BOTH | DEV_IS_MUTED; // expressly state we'll handle muted devices if ((!mp_planner_is_full(mp)) && (cs.bufp = xio_readline(flags, cs.linelen)) != NULL) { _dispatch_kernel(flags); @@ -547,6 +562,76 @@ static stat_t _limit_switch_handler(void) return (STAT_OK); } +#ifdef ENABLE_INTERLOCK_AND_ESTOP +static stat_t _interlock_estop_handler(void) +{ + bool report = false; + //Process E-Stop and Interlock signals + if((cm->safety_state & SAFETY_INTERLOCK_MASK) == SAFETY_INTERLOCK_CLOSED && gpio_read_input(INTERLOCK_SWITCH_INPUT) == INPUT_ACTIVE) { + cm->safety_state |= SAFETY_INTERLOCK_OPEN; + if(spindle.state != SPINDLE_OFF) { + if(mp_get_run_buffer() != NULL) + cm_request_feedhold(FEEDHOLD_TYPE_ACTIONS, FEEDHOLD_EXIT_INTERLOCK); // may have already requested STOP as INPUT_ACTION + else { + cm_cycle_start(); + spindle_control_immediate(spindle.state); + } + } + //If we just entered interlock and we're not off, start the lockout timer + if((cm->safety_state & SAFETY_ESC_MASK) == SAFETY_ESC_ONLINE || (cm->safety_state & SAFETY_ESC_MASK) == SAFETY_ESC_REBOOTING) { + cm->esc_lockout_timer = Motate::SysTickTimer.getValue(); + cm->safety_state |= SAFETY_ESC_LOCKOUT; + } + report = true; + } else if((cm->safety_state & SAFETY_INTERLOCK_MASK) == SAFETY_INTERLOCK_OPEN && gpio_read_input(INTERLOCK_SWITCH_INPUT) == INPUT_INACTIVE) { + cm->safety_state &= ~SAFETY_INTERLOCK_OPEN; + //If we just left interlock, stop the lockout timer + if((cm->safety_state & SAFETY_ESC_LOCKOUT) == SAFETY_ESC_LOCKOUT) + cm->safety_state &= ~SAFETY_ESC_LOCKOUT; + report = true; + } + if((cm->estop_state & ESTOP_PRESSED_MASK) == ESTOP_RELEASED && gpio_read_input(ESTOP_SWITCH_INPUT) == INPUT_ACTIVE) { + cm->estop_state = ESTOP_PRESSED | ESTOP_UNACKED | ESTOP_ACTIVE; + cm_shutdown(STAT_SHUTDOWN, "e-stop pressed"); + //E-stop always sets the ESC to off + cm->safety_state &= ~SAFETY_ESC_MASK; + cm->safety_state |= SAFETY_ESC_OFFLINE; + report = true; + } else if((cm->estop_state & ESTOP_PRESSED_MASK) == ESTOP_PRESSED && gpio_read_input(ESTOP_SWITCH_INPUT) == INPUT_INACTIVE) { + cm->estop_state &= ~ESTOP_PRESSED; + report = true; + } + + //if E-Stop and Interlock are both 0, and we're off, go into "ESC Reboot" + if((cm->safety_state & SAFETY_ESC_MASK) == SAFETY_ESC_OFFLINE && (cm->estop_state & ESTOP_PRESSED) == 0 && (cm->safety_state & SAFETY_INTERLOCK_OPEN) == 0) { + cm->safety_state &= ~SAFETY_ESC_MASK; + cm->safety_state |= SAFETY_ESC_REBOOTING; + cm->esc_boot_timer = Motate::SysTickTimer.getValue(); + report = true; + } + + //Check if ESC lockout timer or reboot timer have expired + uint32_t now = Motate::SysTickTimer.getValue(); + if((cm->safety_state & SAFETY_ESC_LOCKOUT) != 0 && (now - cm->esc_lockout_timer) > ESC_LOCKOUT_TIME) { + cm->safety_state &= ~SAFETY_ESC_MASK; + cm->safety_state |= SAFETY_ESC_OFFLINE; + report = true; + } + if((cm->safety_state & SAFETY_ESC_MASK) == SAFETY_ESC_REBOOTING && (now - cm->esc_boot_timer) > ESC_BOOT_TIME) { + cm->safety_state &= ~SAFETY_ESC_MASK; + report = true; + } + + //If we've successfully ended all the ESTOP conditions, then end ESTOP + if(cm->estop_state == ESTOP_ACTIVE) { + cm->estop_state = 0; + report = true; + } + if(report) + sr_request_status_report(SR_REQUEST_IMMEDIATE); + return (STAT_OK); +} +#else static stat_t _interlock_handler(void) { if (cm->safety_interlock_enable) { @@ -570,6 +655,7 @@ static stat_t _interlock_handler(void) } return(STAT_OK); } +#endif /**************************************************************************************** * _init_assertions() - initialize controller memory integrity assertions diff --git a/g2core/cycle_feedhold.cpp b/g2core/cycle_feedhold.cpp index f33366c5..58dc16e9 100644 --- a/g2core/cycle_feedhold.cpp +++ b/g2core/cycle_feedhold.cpp @@ -381,8 +381,12 @@ static void _start_cycle_restart() void cm_request_queue_flush() { - // Can only initiate a queue flush if in a feedhold + // Can only initiate a queue flush if in a feedhold and e-stop not pressed +#ifdef ENABLE_INTERLOCK_AND_ESTOP + if ((cm1.hold_state != FEEDHOLD_OFF) && (cm1.estop_state == 0)) { +#else if (cm1.hold_state != FEEDHOLD_OFF) { +#endif cm1.queue_flush_state = QUEUE_FLUSH_REQUESTED; } else { cm1.queue_flush_state = QUEUE_FLUSH_OFF; @@ -462,6 +466,11 @@ static stat_t _run_job_kill() _run_queue_flush(); +#ifdef ENABLE_INTERLOCK_AND_ESTOP + if((cm->safety_state & (SAFETY_ESC_MASK | SAFETY_INTERLOCK_MASK)) != 0 && spindle.state != SPINDLE_OFF) + return STAT_EAGAIN; +#endif + coolant_control_immediate(COOLANT_OFF, COOLANT_BOTH); // stop coolant spindle_control_immediate(SPINDLE_OFF); // stop spindle @@ -514,11 +523,16 @@ 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, and not already in a feedhold + // 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) && +#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)) { +#endif cm1.hold_type = type; cm1.hold_exit = exit; diff --git a/g2core/spindle.cpp b/g2core/spindle.cpp index a15ff6c2..2f6e360c 100644 --- a/g2core/spindle.cpp +++ b/g2core/spindle.cpp @@ -193,6 +193,22 @@ static void _exec_spindle_control(float *value, bool *) int8_t dir_bit = -1; // -1 will skip setting the direction. 0 & 1 are valid values bool do_spinup_delay = false; +#ifdef ENABLE_INTERLOCK_AND_ESTOP + if(cm->estop_state != 0) // In E-stop, don't process any spindle commands + action = SPINDLE_OFF; + + // If we're paused or in interlock, or the esc is rebooting, send the spindle an "OFF" command (invisible to cm->gm), + // and issue a hold if necessary + else if(action == SPINDLE_PAUSE || cm->safety_state != 0) { + if(action != SPINDLE_PAUSE) { + action = SPINDLE_PAUSE; + cm_set_motion_state(MOTION_STOP); + cm_request_feedhold(FEEDHOLD_TYPE_ACTIONS, FEEDHOLD_EXIT_INTERLOCK); + sr_request_status_report(SR_REQUEST_IMMEDIATE); + } + } +#endif + switch (action) { case SPINDLE_NOP: { return; } @@ -282,6 +298,11 @@ static void _exec_spindle_speed(float *value, bool *flag) { float previous_speed = spindle.speed; +#ifdef ENABLE_INTERLOCK_AND_ESTOP + if(cm->estop_state != 0 || cm->safety_state != 0 || spindle.state == SPINDLE_PAUSE) + spindle_control_immediate(SPINDLE_OFF); +#endif + spindle.speed = value[0]; pwm_set_duty(PWM_1, _get_spindle_pwm(spindle, pwm));