diff --git a/g2core/alarm.cpp b/g2core/alarm.cpp index 1949e6b5..93eeece8 100644 --- a/g2core/alarm.cpp +++ b/g2core/alarm.cpp @@ -30,6 +30,7 @@ #include "config.h" // #2 #include "gcode.h" // #3 #include "canonical_machine.h" +#include "safety_manager.h" #include "planner.h" #include "report.h" #include "spindle.h" @@ -95,11 +96,7 @@ void cm_clear() void cm_parse_clear(const char *s) { -// #ifdef ENABLE_INTERLOCK_AND_ESTOP -// if (cm->machine_state == MACHINE_ALARM || (cm->machine_state == MACHINE_SHUTDOWN && cm1.estop_state != 0)) { -// #else - if ((cm->machine_state == MACHINE_ALARM) || (cm->machine_state == MACHINE_SHUTDOWN)) { -// #endif + if (safety_manager->can_clear()) { if (toupper(s[0]) == 'M') { if (( (s[1]=='3') && (s[2]=='0') && (s[3]==0)) || ((s[1]=='2') && (s[2]==0) )) { cm_clear(); @@ -114,13 +111,7 @@ void cm_parse_clear(const char *s) stat_t cm_is_alarmed() { - if (cm->machine_state == MACHINE_ALARM) { return (STAT_COMMAND_REJECTED_BY_ALARM); } -#ifdef ENABLE_INTERLOCK_AND_ESTOP - if (cm1.estop_state != 0) { return (STAT_COMMAND_REJECTED_BY_SHUTDOWN); } -#endif - if (cm->machine_state == MACHINE_SHUTDOWN) { return (STAT_COMMAND_REJECTED_BY_SHUTDOWN); } - if (cm->machine_state == MACHINE_PANIC) { return (STAT_COMMAND_REJECTED_BY_PANIC); } - return (STAT_OK); + return safety_manager->is_system_alarmed(); } /**************************************************************************************** @@ -135,7 +126,7 @@ stat_t cm_is_alarmed() void cm_halt(void) { cm_halt_motion(); - spindle_control_immediate(SPINDLE_OFF); + spindle_stop(); coolant_control_immediate(COOLANT_OFF, COOLANT_BOTH); temperature_init(); } diff --git a/g2core/board/G2v9.mk b/g2core/board/G2v9.mk index 6b79a9a0..f3b06abf 100755 --- a/g2core/board/G2v9.mk +++ b/g2core/board/G2v9.mk @@ -40,7 +40,7 @@ ifeq ("$(BASE_BOARD)","g2v9") CHIP_LOWERCASE = sam3x8c BOARD_PATH = ./board/G2v9 - SOURCE_DIRS += ${BOARD_PATH} device/step_dir_driver + SOURCE_DIRS += ${BOARD_PATH} device/step_dir_driver device/esc_spindle PLATFORM_BASE = ${MOTATE_PATH}/platform/atmel_sam @@ -48,7 +48,7 @@ ifeq ("$(BASE_BOARD)","g2v9") ifeq ("$(BANTAM)", "1") DEVICE_DEFINES += BANTAM=1 - SOURCE_DIRS += device/sd_card + SOURCE_DIRS += device/sd_card device/bantam_safety_manager CUSTOM_LINKER_SCRIPT := board/G2v9/bantam-checksum-flash.ld ALL_ADDITIONS += $(OUTPUT_BIN)-summed.bin define ADDITIONAL_RECIPES diff --git a/g2core/board/G2v9/0_hardware.cpp b/g2core/board/G2v9/0_hardware.cpp index 09c1fd1b..6105020a 100644 --- a/g2core/board/G2v9/0_hardware.cpp +++ b/g2core/board/G2v9/0_hardware.cpp @@ -40,6 +40,26 @@ #include "board_spi.h" #include "sd_persistence.h" +#include "board_gpio.h" + +#ifndef SPINDLE_ENABLE_OUTPUT_NUMBER +#warning SPINDLE_ENABLE_OUTPUT_NUMBER is defaulted to 4! +#warning SPINDLE_ENABLE_OUTPUT_NUMBER should be defined in settings or a board file! +#define SPINDLE_ENABLE_OUTPUT_NUMBER 4 +#endif + +#ifndef SPINDLE_DIRECTION_OUTPUT_NUMBER +#warning SPINDLE_DIRECTION_OUTPUT_NUMBER is defaulted to 5! +#warning SPINDLE_DIRECTION_OUTPUT_NUMBER should be defined in settings or a board file! +#define SPINDLE_DIRECTION_OUTPUT_NUMBER 5 +#endif + +#ifndef SPINDLE_PWM_NUMBER +#warning SPINDLE_PWM_NUMBER is defaulted to 6! +#warning SPINDLE_PWM_NUMBER should be defined in settings or a board file! +#define SPINDLE_PWM_NUMBER 6 +#endif + /* * hardware_init() - lowest level hardware init */ @@ -49,6 +69,37 @@ SPIBus_used_t spiBus; Motate::SPIChipSelectPin sdcs{}; SDCard_used_t sd_card{spiBus, sdcs}; +#ifdef BANTAM + +#include "bantam_safety_manager.h" + +BantamSafetyManager sm{}; +SafetyManager *safety_manager = &sm; + +#include "esc_spindle.h" +ESCSpindle esc_spindle {SPINDLE_PWM_NUMBER, SPINDLE_ENABLE_OUTPUT_NUMBER, SPINDLE_DIRECTION_OUTPUT_NUMBER, SPINDLE_SPEED_CHANGE_PER_MS}; + +ToolHead *toolhead_for_tool(uint8_t tool) { + return &esc_spindle; +} + +#else + +SafetyManager sm{}; +SafetyManager *safety_manager = &sm; + +constexpr cfgItem_t sys_config_items_3[] = {}; +constexpr cfgSubtableFromStaticArray sys_config_3{sys_config_items_3}; +const configSubtable * const getSysConfig_3() { return &sys_config_3; } + +#error No toolhead setup yet +ToolHead *toolhead_for_tool(uint8_t tool) { + return nullptr; +} + +#endif + + void hardware_init() { diff --git a/g2core/canonical_machine.cpp b/g2core/canonical_machine.cpp index 477294a7..c7e18402 100644 --- a/g2core/canonical_machine.cpp +++ b/g2core/canonical_machine.cpp @@ -289,11 +289,6 @@ void canonical_machine_reset(cmMachine_t *_cm) _cm->cycle_start_state = CYCLE_START_OFF; _cm->job_kill_state = JOB_KILL_OFF; _cm->limit_requested = 0; // resets switch closures that occurred during initialization - _cm->safety_interlock_disengaged = 0; // ditto - _cm->safety_interlock_reengaged = 0; // ditto - _cm->shutdown_requested = 0; // ditto - _cm->request_interlock = false; - _cm->request_interlock_exit = false; // set initial state and signal that the machine is ready for action _cm->cycle_type = CYCLE_NONE; @@ -303,12 +298,6 @@ 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 - cm1.safety_state = cm1.estop_state = 0; - cm1.esc_boot_timer.set(ESC_BOOT_TIME); - cm1.safety_state = SAFETY_ESC_REBOOTING; -#endif - cm_operation_init(); // reset operations runner canonical_machine_reset_rotation(_cm); @@ -1677,7 +1666,7 @@ stat_t cm_tro_control(const float P_word, const bool P_flag) // M50.1 static void _exec_program_finalize(float* value, bool* flag) { // perform the following resets if it's a program END if (cm->machine_state == MACHINE_PROGRAM_END) { - spindle_control_immediate(SPINDLE_OFF); // immediate M5 + spindle_stop(); // immediate M5 coolant_control_immediate(COOLANT_OFF,COOLANT_BOTH);// immediate M9 temperature_reset(); // turn off all heaters and fans } @@ -1761,16 +1750,6 @@ void cm_program_end() _exec_program_stop_end(MACHINE_PROGRAM_END); } -#ifdef ENABLE_INTERLOCK_AND_ESTOP -stat_t cm_ack_estop(nvObj_t *nv) -{ - cm1.estop_state &= ~ESTOP_UNACKED; - nv->value_flt = (float)cm1.estop_state; - nv->valuetype = TYPE_FLOAT; - return (STAT_OK); -} -#endif - /**************************************************************************************** **** Additional Functions ************************************************************** ****************************************************************************************/ @@ -2092,21 +2071,6 @@ 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 @@ -2124,10 +2088,6 @@ static const char *const msg_estp[] = { msg_estp0, msg_estp1, msg_estp2, msg_est #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 @@ -2156,20 +2116,6 @@ 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 ((cm1.safety_state & SAFETY_INTERLOCK_MASK) != 0) { - safe |= 0x1; - } - if ((cm1.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, (cm1.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))); } @@ -2441,9 +2387,6 @@ stat_t cm_set_sl(nvObj_t *nv) { return(set_integer(nv, (uint8_t &)cm->soft_limit stat_t cm_get_lim(nvObj_t *nv) { return(get_integer(nv, cm->limit_enable)); } stat_t cm_set_lim(nvObj_t *nv) { return(set_integer(nv, (uint8_t &)cm->limit_enable, 0, 1)); } -stat_t cm_get_saf(nvObj_t *nv) { return(get_integer(nv, cm->safety_interlock_enable)); } -stat_t cm_set_saf(nvObj_t *nv) { return(set_integer(nv, (uint8_t &)cm->safety_interlock_enable, 0, 1)); } - stat_t cm_get_m48(nvObj_t *nv) { return(get_integer(nv, cm->gmx.m48_enable)); } stat_t cm_set_m48(nvObj_t *nv) { return(set_integer(nv, (uint8_t &)cm->gmx.m48_enable, 0, 1)); } @@ -2556,11 +2499,6 @@ constexpr cfgItem_t cm_config_items_1[] = { {"", "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 @@ -2917,10 +2855,6 @@ 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));} @@ -2942,10 +2876,6 @@ 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 5535395e..2a792dbb 100644 --- a/g2core/canonical_machine.h +++ b/g2core/canonical_machine.h @@ -175,42 +175,6 @@ typedef enum { // applies to cm->probe_state PROBE_WAITING = 2 // probe is waiting to be started or is running } cmProbeState; -#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, - ESTOP_PRESSED = 0x1, - ESTOP_UNACKED = 0x2, - ESTOP_ACTIVE = 0x4, - - ESTOP_ACTIVE_MASK = 0x4, - 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, - - 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_DISENGAGING, // meaning the interlock opened and we're dealing with it - SAFETY_INTERLOCK_DISENGAGED, - SAFETY_INTERLOCK_ENGAGING -#endif -} cmSafetyState; - typedef enum { // feed override state machine MFO_OFF = 0, MFO_REQUESTED, @@ -332,16 +296,8 @@ typedef struct cmMachine { // struct to manage canonical machin bool return_flags[AXES]; // flags for recording which axes moved - used in feedhold exit move uint8_t limit_requested; // set non-zero to request limit switch processing (value is input number) - uint8_t shutdown_requested; // set non-zero to request shutdown in support of external estop (value is input number) bool deferred_write_flag; // G10 data has changed (e.g. offsets) - flag to persist them - bool safety_interlock_enable; // true to enable safety interlock system - bool request_interlock; // enter interlock - bool request_interlock_exit; // exit interlock - uint8_t safety_interlock_disengaged; // set non-zero to start interlock processing (value is input number) - uint8_t safety_interlock_reengaged; // set non-zero to end interlock processing (value is input number) - cmSafetyState safety_interlock_state; // safety interlock state - cmHomingState homing_state; // home: homing cycle sub-state machine uint8_t homed[AXES]; // individual axis homing flags @@ -360,13 +316,6 @@ typedef struct cmMachine { // struct to manage canonical machin 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 - Motate::Timeout esc_boot_timer; // When the ESC last booted up - Motate::Timeout esc_lockout_timer; // When the ESC lockout last triggered -#endif - GCodeState_t gm; // core gcode model state GCodeStateX_t gmx; // extended gcode model state @@ -521,11 +470,6 @@ 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 @@ -631,10 +575,6 @@ 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 @@ -696,8 +636,6 @@ stat_t cm_get_sl(nvObj_t *nv); // get soft limit enable stat_t cm_set_sl(nvObj_t *nv); // set soft limit enable stat_t cm_get_lim(nvObj_t *nv); // get hard limit enable stat_t cm_set_lim(nvObj_t *nv); // set hard limit enable -stat_t cm_get_saf(nvObj_t *nv); // get safety interlock enable -stat_t cm_set_saf(nvObj_t *nv); // set safety interlock enable stat_t cm_get_m48(nvObj_t *nv); // get M48 value (enable/disable overrides) stat_t cm_set_m48(nvObj_t *nv); // set M48 value (enable/disable overrides) @@ -754,10 +692,6 @@ 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 @@ -827,10 +761,6 @@ 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 61d6e79d..9b742e2d 100644 --- a/g2core/config_app.cpp +++ b/g2core/config_app.cpp @@ -48,6 +48,7 @@ #include "help.h" #include "xio.h" #include "kinematics.h" +#include "safety_manager.h" /*** structures ***/ @@ -750,21 +751,7 @@ constexpr cfgItem_t ain_config_items_1[] = { constexpr cfgSubtableFromStaticArray ain_config_1 {ain_config_items_1}; constexpr const configSubtable * const getAINConfig_1() { return &ain_config_1; } -constexpr cfgItem_t p1_config_items_1[] = { - // PWM settings - { "p1","p1frq",_fip, 0, pwm_print_p1frq, get_flt, pwm_set_pwm,&pwm.c[PWM_1].frequency, P1_PWM_FREQUENCY }, - { "p1","p1csl",_fip, 0, pwm_print_p1csl, get_flt, pwm_set_pwm,&pwm.c[PWM_1].cw_speed_lo, P1_CW_SPEED_LO }, - { "p1","p1csh",_fip, 0, pwm_print_p1csh, get_flt, pwm_set_pwm,&pwm.c[PWM_1].cw_speed_hi, P1_CW_SPEED_HI }, - { "p1","p1cpl",_fip, 3, pwm_print_p1cpl, get_flt, pwm_set_pwm,&pwm.c[PWM_1].cw_phase_lo, P1_CW_PHASE_LO }, - { "p1","p1cph",_fip, 3, pwm_print_p1cph, get_flt, pwm_set_pwm,&pwm.c[PWM_1].cw_phase_hi, P1_CW_PHASE_HI }, - { "p1","p1wsl",_fip, 0, pwm_print_p1wsl, get_flt, pwm_set_pwm,&pwm.c[PWM_1].ccw_speed_lo, P1_CCW_SPEED_LO }, - { "p1","p1wsh",_fip, 0, pwm_print_p1wsh, get_flt, pwm_set_pwm,&pwm.c[PWM_1].ccw_speed_hi, P1_CCW_SPEED_HI }, - { "p1","p1wpl",_fip, 3, pwm_print_p1wpl, get_flt, pwm_set_pwm,&pwm.c[PWM_1].ccw_phase_lo, P1_CCW_PHASE_LO }, - { "p1","p1wph",_fip, 3, pwm_print_p1wph, get_flt, pwm_set_pwm,&pwm.c[PWM_1].ccw_phase_hi, P1_CCW_PHASE_HI }, - { "p1","p1pof",_fip, 3, pwm_print_p1pof, get_flt, pwm_set_pwm,&pwm.c[PWM_1].phase_off, P1_PWM_PHASE_OFF }, -}; -constexpr cfgSubtableFromStaticArray p1_config_1 {p1_config_items_1}; -constexpr const configSubtable * const getP1Config_1() { return &p1_config_1; } +// p1_config_1 constexpr cfgItem_t pid_config_items_1[] = { // temperature configs - pid active values (read-only) @@ -1834,6 +1821,7 @@ constexpr cfgItem_t uber_groups_config_items_1[] = { }; constexpr cfgSubtableFromStaticArray uber_groups_config_1 {uber_groups_config_items_1}; constexpr const configSubtable * const getUberGroupsConfig_1() { return &uber_groups_config_1; } + auto nodes = makeSubtableNodes( 0, getSysConfig_1(), getCmConfig_1(), getMpoConfig_1(), getPosConfig_1(), getOfsConfig_1(), getHomConfig_1(), getPrbConfig_1(), getJogConfig_1(), getPwrConfig_1(), getMotorConfig_1(), getAxisConfig_1(), getDIConfig_1(), diff --git a/g2core/controller.cpp b/g2core/controller.cpp index cb74b826..1d3e14a1 100644 --- a/g2core/controller.cpp +++ b/g2core/controller.cpp @@ -45,10 +45,8 @@ #include "util.h" #include "xio.h" #include "settings.h" -#ifdef ENABLE_INTERLOCK_AND_ESTOP -#include "spindle.h" -#endif #include "persistence.h" +#include "safety_manager.h" #include "MotatePower.h" @@ -68,12 +66,7 @@ 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 _safety_handler(void); // new (replaces _interlock_estop_handler) static stat_t _limit_switch_handler(void); // revised for new GPIO code static void _init_assertions(void); @@ -89,18 +82,6 @@ static stat_t _controller_state(void); // manage controller state trans static Motate::OutputPin safe_pin; -gpioDigitalInputHandler _shutdown_input_handler { - [&](const bool state, const inputEdgeFlag edge, const uint8_t triggering_pin_number) { - if (edge != INPUT_EDGE_LEADING) { return GPIO_NOT_HANDLED; } - - cm->shutdown_requested = triggering_pin_number; - - return GPIO_HANDLED; - }, - 5, // priority - nullptr // next - nullptr to start with -}; - gpioDigitalInputHandler _limit_input_handler { [&](const bool state, const inputEdgeFlag edge, const uint8_t triggering_pin_number) { if (edge != INPUT_EDGE_LEADING) { return GPIO_NOT_HANDLED; } @@ -113,19 +94,6 @@ gpioDigitalInputHandler _limit_input_handler { nullptr // next - nullptr to start with }; -gpioDigitalInputHandler _interlock_input_handler{ - [&](const bool state, const inputEdgeFlag edge, const uint8_t triggering_pin_number) { - if (edge != INPUT_EDGE_LEADING) { - cm1.safety_interlock_disengaged = triggering_pin_number; - } else { // edge == INPUT_EDGE_TRAILING - cm1.safety_interlock_reengaged = triggering_pin_number; - } - - return GPIO_HANDLED; - }, - 5, // priority - nullptr // next - nullptr to start with -}; /*********************************************************************************** **** CODE ************************************************************************* @@ -152,15 +120,9 @@ void controller_init() } // IndicatorLed.setFrequency(100000); - din_handlers[INPUT_ACTION_SHUTDOWN].registerHandler(&_shutdown_input_handler); din_handlers[INPUT_ACTION_LIMIT].registerHandler(&_limit_input_handler); - din_handlers[INPUT_ACTION_INTERLOCK].registerHandler(&_interlock_input_handler); -#ifdef ENABLE_INTERLOCK_AND_ESTOP - if (gpio_read_input(INTERLOCK_SWITCH_INPUT) == INPUT_ACTIVE) { - // cm1.safety_interlock_disengaged = INTERLOCK_SWITCH_INPUT; - cm1.safety_state |= SAFETY_INTERLOCK_OPEN; - } -#endif + + safety_manager->init(); } void controller_request_enquiry() @@ -207,12 +169,7 @@ 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(_safety_handler()); // invoke shutdown DISPATCH(temperature_callback()); // makes sure temperatures are under control DISPATCH(_limit_switch_handler()); // invoke limit switch DISPATCH(_controller_state()); // controller state management @@ -530,9 +487,8 @@ static stat_t _sync_to_planner() /**************************************************************************************** * ALARM STATE HANDLERS * - * _shutdown_handler() - put system into shutdown state + * _safety_handler() - handle safety stuff like shutdown and interlock * _limit_switch_handler() - shut down system if limit switch fired - * _interlock_handler() - feedhold and resume depending on edge * * Some handlers return EAGAIN causing the control loop to never advance beyond that point. * @@ -541,14 +497,9 @@ static stat_t _sync_to_planner() * - safety_interlock_requested == INPUT_EDGE_LEADING is interlock onset * - safety_interlock_requested == INPUT_EDGE_TRAILING is interlock offset */ -static stat_t _shutdown_handler(void) +static stat_t _safety_handler(void) { - if (cm->shutdown_requested != 0) { // request may contain the (non-zero) input number - char msg[10]; - sprintf(msg, "input %d", (int)cm->shutdown_requested); - cm->shutdown_requested = false; // clear limit request used here ^ - cm_shutdown(STAT_SHUTDOWN, msg); - } + safety_manager->periodic_handler(); return(STAT_OK); } @@ -569,112 +520,6 @@ 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 - - // Door opened and was closed - if ((cm1.safety_state & SAFETY_INTERLOCK_MASK) == SAFETY_INTERLOCK_CLOSED && (gpio_read_input(INTERLOCK_SWITCH_INPUT) == INPUT_ACTIVE)) { - cm1.safety_state |= SAFETY_INTERLOCK_OPEN; - - // Check if the spindle is on - if (is_spindle_on_or_paused()) { - if (cm1.machine_state == MACHINE_CYCLE) { - cm_request_feedhold(FEEDHOLD_TYPE_ACTIONS, FEEDHOLD_EXIT_CYCLE); - } else { - spindle_control_immediate(SPINDLE_OFF); - } - } - - // If we just entered interlock and we're not off, start the lockout timer - if ((cm1.safety_state & SAFETY_ESC_MASK) == SAFETY_ESC_ONLINE || - (cm1.safety_state & SAFETY_ESC_MASK) == SAFETY_ESC_REBOOTING) { - cm->esc_lockout_timer.set(ESC_LOCKOUT_TIME); - cm1.safety_state |= SAFETY_ESC_LOCKOUT; - } - report = true; - - // Door closed and was open - } else if ((cm1.safety_state & SAFETY_INTERLOCK_MASK) == SAFETY_INTERLOCK_OPEN && (gpio_read_input(INTERLOCK_SWITCH_INPUT) == INPUT_INACTIVE)) { - cm1.safety_state &= ~SAFETY_INTERLOCK_OPEN; - // If we just left interlock, stop the lockout timer - if ((cm1.safety_state & SAFETY_ESC_LOCKOUT) == SAFETY_ESC_LOCKOUT) { - cm1.safety_state &= ~SAFETY_ESC_LOCKOUT; - cm->esc_lockout_timer.clear(); - } - report = true; - } - - // EStop was pressed - if ((cm1.estop_state & ESTOP_PRESSED_MASK) == ESTOP_RELEASED && gpio_read_input(ESTOP_SWITCH_INPUT) == INPUT_ACTIVE) { - cm1.estop_state = ESTOP_PRESSED | ESTOP_UNACKED | ESTOP_ACTIVE; - cm_shutdown(STAT_SHUTDOWN, "e-stop pressed"); - - // E-stop always sets the ESC to off - cm1.safety_state &= ~SAFETY_ESC_MASK; - cm1.safety_state |= SAFETY_ESC_OFFLINE; - report = true; - - // EStop was released - } else if ((cm1.estop_state & ESTOP_PRESSED_MASK) == ESTOP_PRESSED && gpio_read_input(ESTOP_SWITCH_INPUT) == INPUT_INACTIVE) { - cm1.estop_state &= ~ESTOP_PRESSED; - report = true; - } - - // if E-Stop and Interlock are both 0, and we're off, go into "ESC Reboot" - if ((cm1.safety_state & SAFETY_ESC_MASK) == SAFETY_ESC_OFFLINE && (cm1.estop_state & ESTOP_PRESSED) == 0 && (cm1.safety_state & SAFETY_INTERLOCK_OPEN) == 0) { - cm1.safety_state &= ~SAFETY_ESC_MASK; - cm1.safety_state |= SAFETY_ESC_REBOOTING; - cm->esc_boot_timer.set(ESC_BOOT_TIME); - report = true; - } - - // Check if ESC lockout timer or reboot timer have expired - if ((cm1.safety_state & SAFETY_ESC_LOCKOUT) != 0 && cm->esc_lockout_timer.isPast()) { - cm1.safety_state &= ~SAFETY_ESC_MASK; - cm1.safety_state |= SAFETY_ESC_OFFLINE; - report = true; - } - if ((cm1.safety_state & SAFETY_ESC_MASK) == SAFETY_ESC_REBOOTING && cm->esc_boot_timer.isPast()) { - cm1.safety_state &= ~SAFETY_ESC_MASK; - report = true; - } - - // If we've successfully ended all the ESTOP conditions, then end ESTOP - if (cm1.estop_state == ESTOP_ACTIVE) { - cm1.estop_state = 0; - report = true; - } - - if (report) { - sr_request_status_report(SR_REQUEST_IMMEDIATE); - } - return (STAT_OK); -} -#else -static stat_t _interlock_handler(void) -{ - // 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 - } - - // interlock restored - 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 - } - } - return(STAT_OK); -} -#endif - /**************************************************************************************** * _init_assertions() - initialize controller memory integrity assertions * _test_assertions() - check controller memory integrity assertions diff --git a/g2core/cycle_feedhold.cpp b/g2core/cycle_feedhold.cpp index e89d8fa7..89411846 100644 --- a/g2core/cycle_feedhold.cpp +++ b/g2core/cycle_feedhold.cpp @@ -30,6 +30,7 @@ #include "config.h" // #2 #include "gcode.h" // #3 #include "canonical_machine.h" +#include "safety_manager.h" #include "planner.h" #include "plan_arc.h" #include "stepper.h" @@ -374,9 +375,9 @@ stat_t _run_shutdown() { cm1.machine_state = MACHINE_SHUTDOWN; return (STAT_OK); } -#ifdef ENABLE_INTERLOCK_AND_ESTOP stat_t _run_interlock_started() { cm1.machine_state = MACHINE_INTERLOCK; + safety_manager->start_interlock_after_feedhold(); return (STAT_OK); } stat_t _run_interlock_ended() { @@ -385,24 +386,10 @@ stat_t _run_interlock_ended() { } else { cm1.machine_state = MACHINE_PROGRAM_END; } + safety_manager->end_interlock_after_feedhold(); return (_run_restart_cycle()); } -#else -stat_t _run_interlock_started() { - cm1.safety_interlock_state = SAFETY_INTERLOCK_DISENGAGED; - cm1.machine_state = MACHINE_INTERLOCK; - return (STAT_OK); -} -stat_t _run_interlock_ended() { - cm1.safety_interlock_state = SAFETY_INTERLOCK_ENGAGED; - if (cm1.cycle_type != CYCLE_NONE) { - cm1.machine_state = MACHINE_CYCLE; - } else { - cm1.machine_state = MACHINE_PROGRAM_END; - } - return (_run_restart_cycle()); -} -#endif + /**************************************************************************************** * cm_request_cycle_start() - set request enum only * _start_cycle_start() - run the cycle start @@ -458,11 +445,7 @@ void _start_cycle_restart() void cm_request_queue_flush() { // 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 + if ((cm1.hold_state != FEEDHOLD_OFF) && safety_manager->can_queue_flush()) { cm1.queue_flush_state = QUEUE_FLUSH_REQUESTED; } else { cm1.queue_flush_state = QUEUE_FLUSH_OFF; @@ -548,7 +531,7 @@ stat_t _run_job_kill() _run_queue_flush(); coolant_control_immediate(COOLANT_OFF, COOLANT_BOTH); // stop coolant - spindle_control_immediate(SPINDLE_OFF); // stop spindle + spindle_stop(); // stop spindle cm_set_motion_state(MOTION_STOP); // set to stop and set the active model cm->hold_state = FEEDHOLD_OFF; @@ -839,7 +822,7 @@ stat_t _feedhold_with_actions() // Execute Case (5) cm_set_distance_mode(cm1.gm.distance_mode); // restore distance mode to p1 setting } } - spindle_control_sync(SPINDLE_PAUSE); // optional spindle pause + spindle_pause(); // optional spindle pause coolant_control_sync(COOLANT_PAUSE, COOLANT_BOTH); // optional coolant pause mp_queue_command(_feedhold_actions_done_callback, nullptr, nullptr); return (STAT_EAGAIN); @@ -896,7 +879,7 @@ stat_t _feedhold_restart_with_actions() // Execute Cases (6) and (7) // 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 + spindle_resume(); // resume spindle if paused // do return move though an intermediate point; queue a wait cm2.return_flags[AXIS_Z] = false; diff --git a/g2core/device/bantam_safety_manager/bantam_safety_manager.cpp b/g2core/device/bantam_safety_manager/bantam_safety_manager.cpp new file mode 100644 index 00000000..1a3f63e7 --- /dev/null +++ b/g2core/device/bantam_safety_manager/bantam_safety_manager.cpp @@ -0,0 +1,101 @@ +/* + * bantam_safety_manager.cpp - The safety manager handles interlock and spindle safety controls + * This file is part of the g2core project + * + * Copyright (c) 2019 Rob Giseburt + * + * This code is a loose implementation of Kramer, Proctor and Messina's + * canonical machining functions as described in the NIST RS274/NGC v3 + */ +/* This file ("the software") is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License, version 2 as published by the + * Free Software Foundation. You should have received a copy of the GNU General Public + * License, version 2 along with the software. If not, see . + * + * As a special exception, you may use this file as part of a software library without + * restriction. Specifically, if other files instantiate templates or use macros or + * inline functions from this file, or you compile this file and link it with other + * files to produce an executable, this file does not by itself cause the resulting + * executable to be covered by the GNU General Public License. This exception does not + * however invalidate any other reasons why the executable file might be covered by the + * GNU General Public License. + * + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY + * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES + * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT + * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF + * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +#include "bantam_safety_manager.h" + +BantamSafetyManager *bsm; + +stat_t _bt_get_msg_helper(nvObj_t *nv, const char *const msg_array[], int32_t value) +{ + nv->value_int = value; + nv->valuetype = TYPE_INTEGER; + return(nv_copy_string(nv, (const char *)GET_TEXT_ITEM(msg_array, value))); +} + + +#ifdef __TEXT_MODE + +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 }; + +#else + +#define msg_safe NULL +#define msg_estp NULL + +#endif // __TEXT_MODE + +stat_t cm_ack_estop(nvObj_t *nv) +{ + if (!bsm) { + return (STAT_OK); + } + bsm->ack_estop(); + nv->value_flt = (float)bsm->get_estop_state(); + nv->valuetype = TYPE_FLOAT; + return (STAT_OK); +} + +stat_t cm_get_safe(nvObj_t *nv) { + + uint8_t safe = 0; + if ((bsm) && (bsm->get_interlock_safety_state())) { + safe |= 0x1; + } + if ((bsm) && (bsm->get_esc_safety_state())) { + safe |= 0x2; + } + return (_bt_get_msg_helper(nv, msg_safe, safe)); +} +stat_t cm_get_estp(nvObj_t *nv) { return (_bt_get_msg_helper(nv, msg_estp, bsm ? (bsm->get_estop_state() & 0x3) : 0)); } + +constexpr cfgItem_t sys_config_items_3[] = { + {"", "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) +}; +constexpr cfgSubtableFromStaticArray sys_config_3{sys_config_items_3}; +const configSubtable * const getSysConfig_3() { return &sys_config_3; } + +static const char fmt_safe[] = "Safety System Flags: %s\n"; +static const char fmt_estp[] = "Emergency Stop: %s\n"; + +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);} diff --git a/g2core/device/bantam_safety_manager/bantam_safety_manager.h b/g2core/device/bantam_safety_manager/bantam_safety_manager.h new file mode 100644 index 00000000..55cdd3fe --- /dev/null +++ b/g2core/device/bantam_safety_manager/bantam_safety_manager.h @@ -0,0 +1,229 @@ +/* + * safety_manager.h - The safety manager handles interlock and spindle safety controls + * This file is part of the g2core project + * + * Copyright (c) 2019 Rob Giseburt + * + * This code is a loose implementation of Kramer, Proctor and Messina's + * canonical machining functions as described in the NIST RS274/NGC v3 + */ +/* This file ("the software") is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License, version 2 as published by the + * Free Software Foundation. You should have received a copy of the GNU General Public + * License, version 2 along with the software. If not, see . + * + * As a special exception, you may use this file as part of a software library without + * restriction. Specifically, if other files instantiate templates or use macros or + * inline functions from this file, or you compile this file and link it with other + * files to produce an executable, this file does not by itself cause the resulting + * executable to be covered by the GNU General Public License. This exception does not + * however invalidate any other reasons why the executable file might be covered by the + * GNU General Public License. + * + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY + * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES + * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT + * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF + * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +#ifndef BANTAM_SAFETY_MANAGER_H_ONCE +#define BANTAM_SAFETY_MANAGER_H_ONCE + +#include "safety_manager.h" +#include "hardware.h" + +#include "text_parser.h" + +class BantamSafetyManager; +extern BantamSafetyManager *bsm; + +class BantamSafetyManager : public SafetyManager { + 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, + 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, + } cmBantamSafetyState; + + uint8_t safety_state; // Tracks whether interlock has been triggered, whether esc is rebooting, etc + uint8_t estop_state; // Whether estop has been triggered + Motate::Timeout esc_boot_timer; // When the ESC last booted up + Motate::Timeout esc_lockout_timer; // When the ESC lockout last triggered + + public: + void init() override { + SafetyManager::init(); + + safety_state = estop_state = 0; + esc_boot_timer.set(ESC_BOOT_TIME); + safety_state = SAFETY_ESC_REBOOTING; + + // if (gpio_read_input(INTERLOCK_SWITCH_INPUT) == INPUT_ACTIVE) { + // // safety_interlock_disengaged = INTERLOCK_SWITCH_INPUT; + // safety_state |= SAFETY_INTERLOCK_OPEN; + // } + + bsm = this; + } + + bool ok_to_spindle() override { + if ((estop_state != 0) || (safety_state != 0)) { + return false; + } + + // otherwise safe + return SafetyManager::ok_to_spindle(); + } + + bool can_clear() override { + if ((estop_state != 0) || SafetyManager::can_clear()) { + return true; + } + return false; + } + + bool can_queue_flush() override { + return (estop_state == 0); + } + + stat_t is_system_alarmed() override { + if (estop_state != 0) { return (STAT_COMMAND_REJECTED_BY_SHUTDOWN); } + return SafetyManager::is_system_alarmed(); + } + + stat_t handle_interlock() override { + bool report = false; + + // Process E-Stop and Interlock signals + + // Door opened and was closed + if ((safety_state & SAFETY_INTERLOCK_MASK) == SAFETY_INTERLOCK_CLOSED && (gpio_read_input(INTERLOCK_SWITCH_INPUT) == INPUT_ACTIVE)) { + safety_state |= SAFETY_INTERLOCK_OPEN; + + // Check if the spindle is on + if (is_spindle_on_or_paused()) { + if (cm1.machine_state == MACHINE_CYCLE) { + cm_request_feedhold(FEEDHOLD_TYPE_ACTIONS, FEEDHOLD_EXIT_CYCLE); + } else { + spindle_stop(); + } + } + + // If we just entered interlock and we're not off, start the lockout timer + if ((safety_state & SAFETY_ESC_MASK) == SAFETY_ESC_ONLINE || + (safety_state & SAFETY_ESC_MASK) == SAFETY_ESC_REBOOTING) { + esc_lockout_timer.set(ESC_LOCKOUT_TIME); + safety_state |= SAFETY_ESC_LOCKOUT; + } + report = true; + + // Door closed and was open + } else if ((safety_state & SAFETY_INTERLOCK_MASK) == SAFETY_INTERLOCK_OPEN && (gpio_read_input(INTERLOCK_SWITCH_INPUT) == INPUT_INACTIVE)) { + safety_state &= ~SAFETY_INTERLOCK_OPEN; + // If we just left interlock, stop the lockout timer + if ((safety_state & SAFETY_ESC_LOCKOUT) == SAFETY_ESC_LOCKOUT) { + safety_state &= ~SAFETY_ESC_LOCKOUT; + esc_lockout_timer.clear(); + } + report = true; + } + + // EStop was pressed + if ((estop_state & ESTOP_PRESSED_MASK) == ESTOP_RELEASED && gpio_read_input(ESTOP_SWITCH_INPUT) == INPUT_ACTIVE) { + estop_state = ESTOP_PRESSED | ESTOP_UNACKED | ESTOP_ACTIVE; + cm_shutdown(STAT_SHUTDOWN, "e-stop pressed"); + + // E-stop always sets the ESC to off + safety_state &= ~SAFETY_ESC_MASK; + safety_state |= SAFETY_ESC_OFFLINE; + report = true; + + // EStop was released + } else if ((estop_state & ESTOP_PRESSED_MASK) == ESTOP_PRESSED && gpio_read_input(ESTOP_SWITCH_INPUT) == INPUT_INACTIVE) { + estop_state &= ~ESTOP_PRESSED; + report = true; + } + + // if E-Stop and Interlock are both 0, and we're off, go into "ESC Reboot" + if ((safety_state & SAFETY_ESC_MASK) == SAFETY_ESC_OFFLINE && (estop_state & ESTOP_PRESSED) == 0 && (safety_state & SAFETY_INTERLOCK_OPEN) == 0) { + safety_state &= ~SAFETY_ESC_MASK; + safety_state |= SAFETY_ESC_REBOOTING; + esc_boot_timer.set(ESC_BOOT_TIME); + report = true; + } + + // Check if ESC lockout timer or reboot timer have expired + if ((safety_state & SAFETY_ESC_LOCKOUT) != 0 && esc_lockout_timer.isPast()) { + safety_state &= ~SAFETY_ESC_MASK; + safety_state |= SAFETY_ESC_OFFLINE; + report = true; + } + if ((safety_state & SAFETY_ESC_MASK) == SAFETY_ESC_REBOOTING && esc_boot_timer.isPast()) { + safety_state &= ~SAFETY_ESC_MASK; + report = true; + } + + // If we've successfully ended all the ESTOP conditions, then end ESTOP + if (estop_state == ESTOP_ACTIVE) { + estop_state = 0; + report = true; + } + + if (report) { + sr_request_status_report(SR_REQUEST_IMMEDIATE); + } + return (STAT_OK); + } + + uint8_t get_estop_state() { return estop_state; } + void ack_estop() { + estop_state &= ~BantamSafetyManager::ESTOP_UNACKED; + } + + uint8_t get_interlock_safety_state() { return (safety_state & BantamSafetyManager::SAFETY_INTERLOCK_MASK) != 0; } + uint8_t get_esc_safety_state() { return (safety_state & BantamSafetyManager::SAFETY_ESC_MASK) != 0; } +}; + +stat_t cm_get_safe(nvObj_t *nv); // get interlock state +stat_t cm_get_estp(nvObj_t *nv); // get E-stop state + +#ifdef __TEXT_MODE + +void cm_print_safe(nvObj_t *nv); +void cm_print_estp(nvObj_t *nv); + +#else + +#define cm_print_safe tx_print_stub +#define cm_print_estp tx_print_stub + +#endif // __TEXT_MODE + +// E-Stop +stat_t cm_ack_estop(nvObj_t *nv); +const configSubtable *const getSysConfig_3(); + +#endif //BANTAM_SAFETY_MANAGER_H_ONCE diff --git a/g2core/device/esc_spindle/esc_spindle.cpp b/g2core/device/esc_spindle/esc_spindle.cpp new file mode 100644 index 00000000..e7541344 --- /dev/null +++ b/g2core/device/esc_spindle/esc_spindle.cpp @@ -0,0 +1,694 @@ +/* + * esc_spindle.cpp - toolhead driver for a ESC-driven brushless spindle + * This file is part of the g2core project + * + * Copyright (c) 2019 Robert Giseburt + * Copyright (c) 2019 Alden S. Hart, Jr. + * + * This file ("the software") is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License, version 2 as published by the + * Free Software Foundation. You should have received a copy of the GNU General Public + * License, version 2 along with the software. If not, see . + * + * As a special exception, you may use this file as part of a software library without + * restriction. Specifically, if other files instantiate templates or use macros or + * inline functions from this file, or you compile this file and link it with other + * files to produce an executable, this file does not by itself cause the resulting + * executable to be covered by the GNU General Public License. This exception does not + * however invalidate any other reasons why the executable file might be covered by the + * GNU General Public License. + * + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY + * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES + * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT + * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF + * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +#if 0 + +#include "g2core.h" // #1 dependency order +#include "config.h" // #2 +#include "canonical_machine.h" // #3 +#include "text_parser.h" // #4 + +#include "gpio.h" +#include "esc_spindle.h" +#include "planner.h" +#include "hardware.h" +#include "pwm.h" +#include "util.h" + +/**** Allocate structures ****/ + +#define SPINDLE_OVERRIDE_ENABLE false +#define SPINDLE_OVERRIDE_FACTOR 1.00 +#define SPINDLE_OVERRIDE_MIN 0.05 // 5% +#define SPINDLE_OVERRIDE_MAX 2.00 // 200% +#define SPINDLE_OVERRIDE_RAMP_TIME 1 // change sped in seconds + +enum spMode { + SPINDLE_DISABLED = 0, // spindle will not operate + SPINDLE_PLAN_TO_STOP, // spindle operating, plans to stop + SPINDLE_CONTINUOUS, // spindle operating, does not plan to stop +}; +#define SPINDLE_MODE_MAX SPINDLE_CONTINUOUS + +// *** NOTE: The spindle polarity active hi/low values currently agree with ioMode in gpio.h +// These will all need to be changed to ACTIVE_HIGH = 0, ACTIVE_LOW = 1 +// See: https://github.com/synthetos/g2_private/wiki/GPIO-Design-Discussion#settings-common-to-all-io-types + +enum spPolarity { // Note: These values agree with + SPINDLE_ACTIVE_LOW = 0, // Will set output to 0 to enable the spindle or CW direction + SPINDLE_ACTIVE_HIGH = 1, // Will set output to 1 to enable the spindle or CW direction +}; + +enum ESCState { // electronic speed controller for some spindles + ESC_ONLINE = 0, + ESC_OFFLINE, + ESC_LOCKOUT, + ESC_REBOOTING, + ESC_LOCKOUT_AND_REBOOTING, +}; + +/* + * Spindle control structure + */ + +enum spState { // how spindle states are represented internally + SPINDLE_STATE_OFF = 0, // OFF - startup condition + SPINDLE_STATE_PAUSED, // Paused - was on, is still holding properties for when it's resumed + SPINDLE_STATE_SPINUP, // Spinning up - in the process of going to RUN + SPINDLE_STATE_RUN, // Running - all parameters are as requested + SPINDLE_STATE_SPINDOWN, // Spining down - on the way to paused +}; + +struct spSpindle_t { + spControl state; // {spc:} OFF, ON, PAUSE, RESUME, WAIT + spDirection direction; // 1=CW, 2=CCW (subset of above state) + + float speed; // {sps:} S in RPM + float speed_min; // {spsn:} minimum settable spindle speed + float speed_max; // {spsm:} maximum settable spindle speed + float speed_actual; // hidden internal value used in speed ramping + float speed_change_per_tick; // hidden internal value used in speed ramping + + spPolarity enable_polarity; // {spep:} 0=active low, 1=active high + spPolarity dir_polarity; // {spdp:} 0=clockwise low, 1=clockwise high + bool pause_enable; // {spph:} pause on feedhold + float spinup_delay; // {spde:} optional delay on spindle start (set to 0 to disable) + + bool override_enable; // {spoe:} TRUE = spindle speed override enabled (see also m48_enable in canonical machine) + float override_factor; // {spo:} 1.0000 x S spindle speed. Go up or down from there +}; + +spSpindle_t spindle; + + +gpioDigitalOutput *spindle_enable_output = nullptr; +gpioDigitalOutput *spindle_direction_output = nullptr; + +#ifndef SPINDLE_ENABLE_OUTPUT_NUMBER +#warning SPINDLE_ENABLE_OUTPUT_NUMBER is defaulted to 4! +#warning SPINDLE_ENABLE_OUTPUT_NUMBER should be defined in settings or a board file! +#define SPINDLE_ENABLE_OUTPUT_NUMBER 4 +#endif + +#ifndef SPINDLE_DIRECTION_OUTPUT_NUMBER +#warning SPINDLE_DIRECTION_OUTPUT_NUMBER is defaulted to 5! +#warning SPINDLE_DIRECTION_OUTPUT_NUMBER should be defined in settings or a board file! +#define SPINDLE_DIRECTION_OUTPUT_NUMBER 5 +#endif + +/**** Static functions ****/ + +static float _get_spindle_pwm (spSpindle_t &_spindle, pwmControl_t &_pwm); + +#define SPINDLE_DIRECTION_ASSERT \ + if ((spindle.direction < SPINDLE_CW) || (spindle.direction > SPINDLE_CCW)) { \ + spindle.direction = SPINDLE_CW; \ + } + +#ifndef SPINDLE_SPEED_CHANGE_PER_MS +#define SPINDLE_SPEED_CHANGE_PER_MS 0 +#endif + +/**************************************************************************************** + * spindle_init() + * spindle_reset() - stop spindle, set speed to zero, and reset values + */ +void spindle_init() +{ + SPINDLE_DIRECTION_ASSERT // spindle needs an initial direction + + if (SPINDLE_ENABLE_OUTPUT_NUMBER > 0) { + spindle_enable_output = d_out[SPINDLE_ENABLE_OUTPUT_NUMBER-1]; + spindle_enable_output->setEnabled(IO_ENABLED); + spindle_enable_output->setPolarity((ioPolarity)SPINDLE_ENABLE_POLARITY); + } + if (SPINDLE_DIRECTION_OUTPUT_NUMBER > 0) { + spindle_direction_output = d_out[SPINDLE_DIRECTION_OUTPUT_NUMBER-1]; + spindle_direction_output->setEnabled(IO_ENABLED); + spindle_direction_output->setPolarity((ioPolarity)SPINDLE_DIR_POLARITY); + } + + if( pwm.c[PWM_1].frequency < 0 ) { + pwm.c[PWM_1].frequency = 0; + } + pwm_set_freq(PWM_1, pwm.c[PWM_1].frequency); + pwm_set_duty(PWM_1, pwm.c[PWM_1].phase_off); + + spindle.speed_change_per_tick = SPINDLE_SPEED_CHANGE_PER_MS; +} + +void spindle_reset() +{ + spindle_set_speed(0); + spindle_stop(); +} + +// to be used blow, assumes spindle.speed (etc) are already setup +void _actually_set_spindle_speed() { + float speed_lo, speed_hi; + bool clamp_speeds = false; + if (spindle.state == SPINDLE_CW) { + speed_lo = pwm.c[PWM_1].cw_speed_lo; + speed_hi = pwm.c[PWM_1].cw_speed_hi; + clamp_speeds = true; + } else if (spindle.state == SPINDLE_CCW ) { + speed_lo = pwm.c[PWM_1].ccw_speed_lo; + speed_hi = pwm.c[PWM_1].ccw_speed_hi; + clamp_speeds = true; + } else { + // off/disabled/paused + spindle.speed_actual = 0; + } + + if (clamp_speeds) { + // clamp spindle speed to lo/hi range + if (spindle.speed < speed_lo) { + spindle.speed = speed_lo; + } + + // allow spindle.speed_actual to start at 0 to match physical spinup + + if (spindle.speed > speed_hi) { + spindle.speed = speed_hi; + } + + if (spindle.speed_actual > speed_hi) { + spindle.speed_actual = speed_hi; + } + } else { + pwm_set_duty(PWM_1, _get_spindle_pwm(spindle, pwm)); + return; + } + + if (fp_ZERO(spindle.speed_change_per_tick)) { // || (spindle.speed <= spindle.speed_actual) + spindle.speed_actual = spindle.speed; + } + pwm_set_duty(PWM_1, _get_spindle_pwm(spindle, pwm)); + + if (fp_NE(spindle.speed_actual, spindle.speed)) { // && (spindle.speed > spindle.speed_actual) + // use the larger of: spindup_delay setting, or the time it'll take to ramp to the new speed, converted to seconds + if (fp_NOT_ZERO(spindle.speed_change_per_tick)) { + mp_request_out_of_band_dwell(spindle.spinup_delay + 0.001*std::abs(spindle.speed-spindle.speed_actual)/spindle.speed_change_per_tick); + } else { + mp_request_out_of_band_dwell(spindle.spinup_delay); + } + } +} + +/**************************************************************************************** + * _exec_spindle_control() - actually execute the spindle command + * spindle_control_immediate() - execute spindle control immediately + * spindle_control_sync() - queue a spindle control to the planner buffer + * + * Basic operation: Spindle function is executed by _exec_spindle_control(). + * Spindle_control_immediate() performs the control as soon as it's received. + * Spindle_control_sync() inserts spindle move into the planner, and handles spinups. + * + * Valid inputs to Spindle_control_immediate() and Spindle_control_sync() are: + * + * - SPINDLE_OFF turns off spindle and sets spindle state to SPINDLE_OFF. + * This will also re-load enable and direction polarity to the pins if they have changed. + * The spindle.direction value is not affected (although this doesn't really matter). + * + * - SPINDLE_CW or SPINDLE_CCW turns sets direction accordingly and spindle on. + * In spindle_control_sync() a non-zero spinup delay runs a dwell immediately + * following the spindle change, but only if the planner had planned the spindle + * operation to zero. (I.e. if the spindle controls / S words do not plan to zero + * the delay is not run). Spindle_control_immediate() has no spinup delay or + * dwell behavior. + * + * - SPINDLE_PAUSE is only applicable to CW and CCW states. It forces the spindle OFF and + * sets spindle.state to PAUSE. A PAUSE received when not in CW or CCW state is ignored. + * + * - SPINDLE_RESUME, if in a PAUSE state, reverts to previous SPINDLE_CW or SPINDLE_CCW. + * The SPEED is not changed, and if it were changed in the interim the "new" speed + * is used. If RESUME is received from spindle_control_sync() the usual spinup delay + * behavior occurs. If RESUME is received when not in a PAUSED state it is ignored. + * This recognizes that the main reason an immediate command would be issued - either + * manually by the user or by an alarm or some other program function - is to stop + * a spindle. So the Resume should be ignored for safety. + */ +/* Notes: + * - Since it's possible to queue a sync'd control, and then set any spindle state + * with an immediate() before the queued command is reached, _exec_spindle_control() + * must gracefully handle any arbitrary state transition (not just the "legal" ones). + * + * - The spinup and spindown rows are present, but are not implemented unless we + * find we need them. It's easy enough to set these flags using the bit vector + * passed from sync(),but unsetting them once the delay is complete would take + * some more work. + * + * Q: Do we need a spin-down for direction reversal? + * Q: Should the JSON be able to pause and resume? For test purposes only? + */ +/* State/Control matrix. Read "If you are in state X and get control Y do action Z" + + Control: OFF CW CCW PAUSE RESUME + State: |-----------|-----------|-----------|-----------|-----------| + OFF | OFF | CW | CCW | NOP | NOP | + |-----------|-----------|-----------|-----------|-----------| + CW | OFF | NOP | REVERSE | PAUSE | NOP | + |-----------|-----------|-----------|-----------|-----------| + CCW | OFF | REVERSE | NOP | PAUSE | NOP | + |-----------|-----------|-----------|-----------|-----------| + PAUSE | OFF | CW | CCW | NOP | RESUME | + |-----------|-----------|-----------|-----------|-----------| + RESUME | invalid | invalid | invalid | invalid | invalid | + |-----------|-----------|-----------|-----------|-----------| + + Actions: + - OFF Turn spindle off. Even if it's already off (reloads polarities) + - CW Turn spindle on clockwise + - CCW Turn spindle on counterclockwise + - PAUSE Turn off spindle, enter PAUSE state + - RESUME Turn spindle on CW or CCW as before + - NOP No operation, ignore + - REVERSE Reverse spindle direction (Q: need a cycle to spin down then back up again?) + */ + +static void _exec_spindle_control(float *value, bool *) +{ + spControl control = (spControl)value[0]; + if (control > SPINDLE_ACTION_MAX) { + return; + } + spControl state = spindle.state; + if (state >= SPINDLE_ACTION_MAX) { +// rpt_exception(STAT_SPINDLE_ASSERTION_FAILURE, "illegal spindle state"); + return; + } + constexpr spControl matrix[20] = { + SPINDLE_OFF, SPINDLE_CW, SPINDLE_CCW, SPINDLE_NOP, SPINDLE_NOP, + SPINDLE_OFF, SPINDLE_NOP, SPINDLE_REV, SPINDLE_PAUSE, SPINDLE_NOP, + SPINDLE_OFF, SPINDLE_REV, SPINDLE_NOP, SPINDLE_PAUSE, SPINDLE_NOP, + SPINDLE_OFF, SPINDLE_CW, SPINDLE_CCW, SPINDLE_NOP, SPINDLE_RESUME + }; + spControl action = matrix[(state*5)+control]; + + SPINDLE_DIRECTION_ASSERT; // ensure that the spindle direction is sane + int8_t enable_bit = 0; // default to 0=off + int8_t dir_bit = -1; // -1 will skip setting the direction. 0 & 1 are valid values + +// #ifdef ENABLE_INTERLOCK_AND_ESTOP +// if (!spindle_ready_to_resume()) { // 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 || cm1.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; } + + case SPINDLE_OFF: { // enable_bit already set for this case + dir_bit = spindle.direction-1; // spindle direction was stored as '1' & '2' + spindle.state = SPINDLE_OFF; // the control might have been something other than SPINDLE_OFF + break; + } + case SPINDLE_CW: case SPINDLE_CCW: case SPINDLE_REV: { // REV is handled same as CW or CCW for now + enable_bit = 1; + dir_bit = control-1; // adjust direction to be used as a bitmask + spindle.direction = control; + spindle.state = control; + break; + } + case SPINDLE_PAUSE : { + spindle.state = SPINDLE_PAUSE; + break; // enable bit is already set up to stop the move + } + case SPINDLE_RESUME: { + enable_bit = 1; + dir_bit = spindle.direction-1; // spindle direction was stored as '1' & '2' + spindle.state = spindle.direction; + break; + } + default: {} // reversals not handled yet + } + + // Apply the enable and direction bits and adjust the PWM as required + + // set the direction first + if (dir_bit >= 0) { + if (spindle_direction_output != nullptr) { + spindle_direction_output->setValue(dir_bit); + } + } + + // set spindle enable + if (spindle_enable_output != nullptr) { + spindle_enable_output->setValue(enable_bit); + } + + _actually_set_spindle_speed(); +} + +/* + * spindle_control_immediate() - execute spindle control immediately + * spindle_control_sync() - queue a spindle control to the planner buffer + */ + +stat_t spindle_set_direction(spDirection direction) +{ + cm->gm.spindle_direction = direction; + return(STAT_OK); +} + + +// stat_t spindle_control_immediate(spControl control) +// { +// float value[] = { (float)control }; +// _exec_spindle_control(value, nullptr); +// return(STAT_OK); +// } + +// stat_t spindle_control_sync(spControl control) // uses spControl arg: OFF, CW, CCW +// { +// // skip the PAUSE operation if pause-enable is not enabled (pause-on-hold) +// if ((control == SPINDLE_PAUSE) && (!spindle.pause_enable)) { +// return (STAT_OK); +// } + +// // ignore pause and resume if the spindle isn't even on +// if ((spindle.state == SPINDLE_OFF) && (control == SPINDLE_PAUSE || control == SPINDLE_RESUME)) { +// return (STAT_OK); +// } + +// if (spindle.speed > 0.0 && !is_spindle_ready_to_resume()) { +// // request a feedhold immediately +// cm_request_feedhold(FEEDHOLD_TYPE_ACTIONS, FEEDHOLD_EXIT_CYCLE); +// } + +// // queue the spindle control +// float value[] = { (float)control }; +// mp_queue_command(_exec_spindle_control, value, nullptr); +// return(STAT_OK); +// } + +/**************************************************************************************** + * _exec_spindle_speed() - actually execute the spindle speed command + * spindle_speed_immediate() - execute spindle speed change immediately + * spindle_speed_sync() - queue a spindle speed change to the planner buffer + * + * Setting S0 is considered as turning spindle off. Setting S to non-zero from S0 + * will enable a spinup delay if spinups are npn-zero. + */ + +static void _exec_spindle_speed(float *value, bool *flag) +{ + spindle.speed = value[0]; + + _actually_set_spindle_speed(); +} + +static stat_t _casey_jones(float speed) +{ + if (speed < spindle.speed_min) { return (STAT_SPINDLE_SPEED_BELOW_MINIMUM); } + if (speed > spindle.speed_max) { return (STAT_SPINDLE_SPEED_MAX_EXCEEDED); } + return (STAT_OK); +} + +// stat_t spindle_speed_immediate(float speed) +// { +// ritorno(_casey_jones(speed)); +// float value[] = { speed }; +// _exec_spindle_speed(value, nullptr); +// return (STAT_OK); +// } + +stat_t spindle_set_speed(float speed) +{ + ritorno(_casey_jones(speed)); + cm->gm.spindle_speed = speed; + + // float value[] = { speed }; + // mp_queue_command(_exec_spindle_speed, value, nullptr); + + return (STAT_OK); +} + +bool is_spindle_ready_to_resume() { +#ifdef ENABLE_INTERLOCK_AND_ESTOP + if ((cm1.estop_state != 0) || (cm1.safety_state != 0)) { + return false; + } +#endif + return true; +} + +bool is_spindle_on_or_paused() { + if (spindle.state != SPINDLE_OFF) { + return true; + } + return false; +} + +// returns if it's done +bool do_spindle_speed_ramp_from_systick() { +#ifdef ENABLE_INTERLOCK_AND_ESTOP + bool done = false; + if ((cm1.estop_state == 0) && (cm1.safety_state == 0)) { + if (fp_EQ(spindle.speed_actual, spindle.speed)) { + return true; + } else if (spindle.speed_actual < spindle.speed) { + spindle.speed_actual += spindle.speed_change_per_tick; + if (spindle.speed_actual > spindle.speed) { + spindle.speed_actual = spindle.speed; + done = true; + } + } + else { + spindle.speed_actual -= spindle.speed_change_per_tick; + if (spindle.speed_actual < spindle.speed) { + spindle.speed_actual = spindle.speed; + done = true; + } + } + pwm_set_duty(PWM_1, _get_spindle_pwm(spindle, pwm)); + } else { + spindle.speed_actual = 0; + spindle.state = SPINDLE_PAUSE; + pwm_set_duty(PWM_1, _get_spindle_pwm(spindle, pwm)); + done = (cm1.hold_state != FEEDHOLD_OFF); + } + return done; +#else + return true; +#endif +} + +/**************************************************************************************** + * _get_spindle_pwm() - return PWM phase (duty cycle) for dir and speed + */ + +static float _get_spindle_pwm (spSpindle_t &_spindle, pwmControl_t &_pwm) +{ + float speed_lo, speed_hi, phase_lo, phase_hi; + if (_spindle.direction == SPINDLE_CW ) { + speed_lo = _pwm.c[PWM_1].cw_speed_lo; + speed_hi = _pwm.c[PWM_1].cw_speed_hi; + phase_lo = _pwm.c[PWM_1].cw_phase_lo; + phase_hi = _pwm.c[PWM_1].cw_phase_hi; + } else { // if (direction == SPINDLE_CCW ) { + speed_lo = _pwm.c[PWM_1].ccw_speed_lo; + speed_hi = _pwm.c[PWM_1].ccw_speed_hi; + phase_lo = _pwm.c[PWM_1].ccw_phase_lo; + phase_hi = _pwm.c[PWM_1].ccw_phase_hi; + } + + if ((_spindle.state == SPINDLE_CW) || (_spindle.state == SPINDLE_CCW)) { + // clamp spindle speed to lo/hi range + // if (_spindle.speed_actual < speed_lo) { + // _spindle.speed_actual = speed_lo; + // } + if (_spindle.speed_actual > speed_hi) { + _spindle.speed_actual = speed_hi; + } + // normalize speed to [0..1] + float speed = std::max(0.0f, (_spindle.speed_actual - speed_lo)) / (speed_hi - speed_lo); + return (speed * (phase_hi - phase_lo)) + phase_lo; + } else { + return (_pwm.c[PWM_1].phase_off); + } +} + +/**************************************************************************************** + * spindle_override_control() + * spindle_start_override() + * spindle_end_override() + */ + +stat_t spindle_override_control(const float P_word, const bool P_flag) // M51 +{ + bool new_enable = true; + bool new_override = false; + if (P_flag) { // if parameter is present in Gcode block + if (fp_ZERO(P_word)) { + new_enable = false; // P0 disables override + } else { + if (P_word < SPINDLE_OVERRIDE_MIN) { + return (STAT_INPUT_LESS_THAN_MIN_VALUE); + } + if (P_word > SPINDLE_OVERRIDE_MAX) { + return (STAT_INPUT_EXCEEDS_MAX_VALUE); + } + spindle.override_factor = P_word; // P word is valid, store it. + new_override = true; + } + } + if (cm->gmx.m48_enable) { // if master enable is ON + if (new_enable && (new_override || !spindle.override_enable)) { // 3 cases to start a ramp + spindle_start_override(SPINDLE_OVERRIDE_RAMP_TIME, spindle.override_factor); + } else if (spindle.override_enable && !new_enable) { // case to turn off the ramp + spindle_end_override(SPINDLE_OVERRIDE_RAMP_TIME); + } + } + spindle.override_enable = new_enable; // always update the enable state + return (STAT_OK); +} + +void spindle_start_override(const float ramp_time, const float override_factor) +{ + return; +} + +void spindle_end_override(const float ramp_time) +{ + return; +} + +/**************************** + * END OF SPINDLE FUNCTIONS * + ****************************/ + +/**************************************************************************************** + * CONFIGURATION AND INTERFACE FUNCTIONS + * Functions to get and set variables from the cfgArray table + ****************************************************************************************/ + +/**************************************************************************************** + **** Spindle Settings ****************************************************************** + ****************************************************************************************/ + +stat_t sp_get_spep(nvObj_t *nv) { return(get_integer(nv, spindle.enable_polarity)); } +stat_t sp_set_spep(nvObj_t *nv) { + stat_t status = set_integer(nv, (uint8_t &)spindle.enable_polarity, 0, 1); + spindle_enable_output->setPolarity((ioPolarity)spindle.enable_polarity); + spindle_stop(); // stop spindle and apply new settings + return (status); +} + +stat_t sp_get_spdp(nvObj_t *nv) { return(get_integer(nv, spindle.dir_polarity)); } +stat_t sp_set_spdp(nvObj_t *nv) { + stat_t status = set_integer(nv, (uint8_t &)spindle.dir_polarity, 0, 1); + spindle_direction_output->setPolarity((ioPolarity)spindle.dir_polarity); + spindle_stop(); // stop spindle and apply new settings + return (status); +} + +stat_t sp_get_spph(nvObj_t *nv) { return(get_integer(nv, spindle.pause_enable)); } +stat_t sp_set_spph(nvObj_t *nv) { return(set_integer(nv, (uint8_t &)spindle.pause_enable, 0, 1)); } +stat_t sp_get_spde(nvObj_t *nv) { return(get_float(nv, spindle.spinup_delay)); } +stat_t sp_set_spde(nvObj_t *nv) { return(set_float_range(nv, spindle.spinup_delay, 0, SPINDLE_DWELL_MAX)); } + +stat_t sp_get_spsn(nvObj_t *nv) { return(get_float(nv, spindle.speed_min)); } +stat_t sp_set_spsn(nvObj_t *nv) { return(set_float_range(nv, spindle.speed_min, SPINDLE_SPEED_MIN, SPINDLE_SPEED_MAX)); } +stat_t sp_get_spsm(nvObj_t *nv) { return(get_float(nv, spindle.speed_max)); } +stat_t sp_set_spsm(nvObj_t *nv) { return(set_float_range(nv, spindle.speed_max, SPINDLE_SPEED_MIN, SPINDLE_SPEED_MAX)); } + +stat_t sp_get_spoe(nvObj_t *nv) { return(get_integer(nv, spindle.override_enable)); } +stat_t sp_set_spoe(nvObj_t *nv) { return(set_integer(nv, (uint8_t &)spindle.override_enable, 0, 1)); } +stat_t sp_get_spo(nvObj_t *nv) { return(get_float(nv, spindle.override_factor)); } +stat_t sp_set_spo(nvObj_t *nv) { return(set_float_range(nv, spindle.override_factor, SPINDLE_OVERRIDE_MIN, SPINDLE_OVERRIDE_MAX)); } + +// These are provided as a way to view and control spindles without using M commands +stat_t sp_get_spc(nvObj_t *nv) { return(get_integer(nv, spindle.state)); } +stat_t sp_set_spc(nvObj_t *nv) { return(spindle_control_immediate((spControl)nv->value_int)); } +stat_t sp_get_sps(nvObj_t *nv) { return(get_float(nv, spindle.speed)); } +stat_t sp_set_sps(nvObj_t *nv) { return(spindle_speed_immediate(nv->value_flt)); } + +/**************************************************************************************** + * TEXT MODE SUPPORT + * Functions to print variables from the cfgArray table + ****************************************************************************************/ + +#ifdef __TEXT_MODE + +const char fmt_spc[] = "[spc] spindle control:%12d [0=OFF,1=CW,2=CCW]\n"; +const char fmt_sps[] = "[sps] spindle speed:%14.0f rpm\n"; +const char fmt_spmo[] = "[spmo] spindle mode%16d [0=disabled,1=plan-to-stop,2=continuous]\n"; +const char fmt_spep[] = "[spep] spindle enable polarity%5d [0=active_low,1=active_high]\n"; +const char fmt_spdp[] = "[spdp] spindle direction polarity%2d [0=CW_low,1=CW_high]\n"; +const char fmt_spph[] = "[spph] spindle pause on hold%7d [0=no,1=pause_on_hold]\n"; +const char fmt_spde[] = "[spde] spindle spinup delay%10.1f seconds\n"; +const char fmt_spsn[] = "[spsn] spindle speed min%14.2f rpm\n"; +const char fmt_spsm[] = "[spsm] spindle speed max%14.2f rpm\n"; +const char fmt_spoe[] = "[spoe] spindle speed override ena%2d [0=disable,1=enable]\n"; +const char fmt_spo[] = "[spo] spindle speed override%10.3f [0.050 < spo < 2.000]\n"; + +void sp_print_spc(nvObj_t *nv) { text_print(nv, fmt_spc);} // TYPE_INT +void sp_print_sps(nvObj_t *nv) { text_print(nv, fmt_sps);} // TYPE_FLOAT +void sp_print_spmo(nvObj_t *nv) { text_print(nv, fmt_spmo);} // TYPE_INT +void sp_print_spep(nvObj_t *nv) { text_print(nv, fmt_spep);} // TYPE_INT +void sp_print_spdp(nvObj_t *nv) { text_print(nv, fmt_spdp);} // TYPE_INT +void sp_print_spph(nvObj_t *nv) { text_print(nv, fmt_spph);} // TYPE_INT +void sp_print_spde(nvObj_t *nv) { text_print(nv, fmt_spde);} // TYPE_FLOAT +void sp_print_spsn(nvObj_t *nv) { text_print(nv, fmt_spsn);} // TYPE_FLOAT +void sp_print_spsm(nvObj_t *nv) { text_print(nv, fmt_spsm);} // TYPE_FLOAT +void sp_print_spoe(nvObj_t *nv) { text_print(nv, fmt_spoe);} // TYPE INT +void sp_print_spo(nvObj_t *nv) { text_print(nv, fmt_spo);} // TYPE FLOAT + +#endif // __TEXT_MODE + + +constexpr cfgItem_t spindle_config_items_1[] = { + // Spindle functions + { "sp","spmo", _i0, 0, sp_print_spmo, get_nul, set_nul, nullptr, 0 }, // keeping this key around, but it returns null and does nothing + { "sp","spph", _bip, 0, sp_print_spph, sp_get_spph, sp_set_spph, nullptr, SPINDLE_PAUSE_ON_HOLD }, + { "sp","spde", _fip, 2, sp_print_spde, sp_get_spde, sp_set_spde, nullptr, SPINDLE_SPINUP_DELAY }, + { "sp","spsn", _fip, 2, sp_print_spsn, sp_get_spsn, sp_set_spsn, nullptr, SPINDLE_SPEED_MIN}, + { "sp","spsm", _fip, 2, sp_print_spsm, sp_get_spsm, sp_set_spsm, nullptr, SPINDLE_SPEED_MAX}, + { "sp","spep", _iip, 0, sp_print_spep, sp_get_spep, sp_set_spep, nullptr, SPINDLE_ENABLE_POLARITY }, + { "sp","spdp", _iip, 0, sp_print_spdp, sp_get_spdp, sp_set_spdp, nullptr, SPINDLE_DIR_POLARITY }, + { "sp","spoe", _bip, 0, sp_print_spoe, sp_get_spoe, sp_set_spoe, nullptr, SPINDLE_OVERRIDE_ENABLE}, + { "sp","spo", _fip, 3, sp_print_spo, sp_get_spo, sp_set_spo, nullptr, SPINDLE_OVERRIDE_FACTOR}, + { "sp","spc", _i0, 0, sp_print_spc, sp_get_spc, sp_set_spc, nullptr, 0 }, // spindle state + { "sp","sps", _f0, 0, sp_print_sps, sp_get_sps, sp_set_sps, nullptr, 0 }, // spindle speed +}; +constexpr cfgSubtableFromStaticArray spindle_config_1 {spindle_config_items_1}; +const configSubtable * const getSpindleConfig_1() { return &spindle_config_1; } + +#endif // 0 diff --git a/g2core/device/esc_spindle/esc_spindle.h b/g2core/device/esc_spindle/esc_spindle.h new file mode 100644 index 00000000..aaa28a03 --- /dev/null +++ b/g2core/device/esc_spindle/esc_spindle.h @@ -0,0 +1,351 @@ +/* + * esc_spindle.h - toolhead driver for a ESC-driven brushless spindle + * This file is part of the g2core project + * + * Copyright (c) 2019 Robert Giseburt + * Copyright (c) 2019 Alden S. Hart, Jr. + * + * This file ("the software") is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License, version 2 as published by the + * Free Software Foundation. You should have received a copy of the GNU General Public + * License, version 2 along with the software. If not, see . + * + * As a special exception, you may use this file as part of a software library without + * restriction. Specifically, if other files instantiate templates or use macros or + * inline functions from this file, or you compile this file and link it with other + * files to produce an executable, this file does not by itself cause the resulting + * executable to be covered by the GNU General Public License. This exception does not + * however invalidate any other reasons why the executable file might be covered by the + * GNU General Public License. + * + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY + * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES + * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT + * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF + * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +#ifndef ESC_SPINDLE_H_ONCE +#define ESC_SPINDLE_H_ONCE + +#include "spindle.h" +#include "stepper.h" // for st_request_load_move +#include "util.h" // for fp_NE +#include "safety_manager.h" // for safety_manager + +/* A few notes: + * + * This is primarily for an ESC(electronic speed controller)-based spindle, where a brushless motor + * is used as the spindle. + * + * Generally, the ESC does not get direction and enable signals, and ONLY honors the PWM output. + * Also note that many ESCs can NOT reverse, and will only go one direction. + * + * We still handle those pins here for the sake of compatibility and status display (LEDs on those pins) and debugging. + * + */ + + +// class declaration +// note implementation is after +class ESCSpindle : public ToolHead { + spDirection direction; // direction + float speed; // S in RPM + float speed_actual; // actual speed (during speed ramping) + + float speed_min; // minimum settable spindle speed + float speed_max; // maximum settable spindle speed + + bool paused; // true if paused, false is not + + float speed_change_per_tick; // speed ramping rate per tick (ms) + float spinup_delay; // optional delay on spindle start (set to 0 to disable) + + struct speedToPhase { + float speed_lo; // minimum spindle speed [0..N] + float speed_hi; // maximum spindle speed + + float phase_lo; // pwm phase at minimum spindle speed, clamped [0..1] + float phase_hi; // pwm phase at maximum spindle speed, clamped [0..1] + + // convert a speed value in the range of (speed_lo .. speed_hi) + // to a value in the range of (phase_lo .. phase_hi) + float speed_to_phase(float speed) { + speed = (std::max(speed_lo, std::min(speed_hi, speed)) - speed_lo) / (speed_hi - speed_lo); + return (speed * (phase_hi - phase_lo)) + phase_lo; + } + }; + + speedToPhase cw; // clockwise speed and phase settings + speedToPhase ccw; // counter-clockwise speed and phase settings + + float phase_off; // pwm phase when spindle is disabled + + uint8_t pwm_output_num; + gpioDigitalOutput *pwm_output = nullptr; + uint8_t enable_output_num; + gpioDigitalOutput *enable_output = nullptr; + uint8_t direction_output_num; + gpioDigitalOutput *direction_output = nullptr; + + Motate::SysTickEvent spindle_systick_event{[&] { + bool done = false; + if (paused) { + // paused may have changed since this handler was registered + speed_actual = 0; // just in case there was a race condition + done = true; + } else if (fp_NE(speed, speed_actual)) { + speed_actual += speed_change_per_tick; + if (speed_actual > speed) { + speed_actual = speed; + done = true; + } + } else { + done = true; + } + set_pwm_value(); + if (done) { + SysTickTimer.unregisterEvent(&spindle_systick_event); + st_request_load_move(); // request to load the next move + } + }, + nullptr}; + + void set_pwm_value(); // using all of the settings, set the value fo the pwm pin + void complete_change(); // after an engage or resume, handle the rest + + public: + // constructor - provide it with the default output pins - 0 means no pin + ESCSpindle(const uint8_t pwm_pin_number, const uint8_t enable_pin_number, const uint8_t direction_pin_number, const float change_per_tick); + + // ToolHead overrides + void init() override; + + stat_t pause() override; // soft-stop the toolhead (usually for a feedhold) - retain all state for resume + stat_t resume() override; // resume from the pause - return STAT_EAGAIN if it's not yet ready + bool ready_to_resume() override; // return true if paused and resume would not result in an error + bool busy() override; // return true if motion should continue waiting for this toolhead + + // the result of an S word + // DON'T override set_speed - use engage instead + float get_speed() override; + + // the result of an M3/M4/M5 + // DON'T override set_direction - use engage instead + spDirection get_direction() override; + + // called from the loader right before a move, with the gcode model to use + void engage(const GCodeState_t &gm) override; + + bool is_on() override; // return if the current direction is anything but OFF, **even if paused** + + void set_pwm_output(const uint8_t pwm_pin_number) override; + void set_enable_output(const uint8_t enable_pin_number) override; + void set_direction_output(const uint8_t direction_pin_number) override; + + void set_frequency(float new_frequency)override; + float get_frequency() override; + + // trivial getters and setters - inlined + void set_speed_min(float new_speed_min) override { speed_min = new_speed_min; } + float get_speed_min() override { return speed_min; } + void set_speed_max(float new_speed_max) override { speed_max = new_speed_max; } + float get_speed_max() override { return speed_max; } + void set_speed_change_per_tick(float new_speed_change_per_tick) override { speed_change_per_tick = new_speed_change_per_tick; } + float get_speed_change_per_tick() override { return speed_change_per_tick; } + void set_spinup_delay(float new_spinup_delay) override { spinup_delay = new_spinup_delay; } + float get_spinup_delay() override { return spinup_delay; } + + void set_cw_speed_lo(float new_speed_lo) override { cw.speed_lo = new_speed_lo; } + float get_cw_speed_lo() override { return cw.speed_lo; } + void set_cw_speed_hi(float new_speed_hi) override { cw.speed_hi = new_speed_hi; } + float get_cw_speed_hi() override { return cw.speed_hi; } + void set_cw_phase_lo(float new_phase_lo) override { cw.phase_lo = new_phase_lo; } + float get_cw_phase_lo() override { return cw.phase_lo; } + void set_cw_phase_hi(float new_phase_hi) override { cw.phase_hi = new_phase_hi; } + float get_cw_phase_hi() override { return cw.phase_hi; } + + void set_ccw_speed_lo(float new_speed_lo) override { ccw.speed_lo = new_speed_lo; } + float get_ccw_speed_lo() override { return ccw.speed_lo; } + void set_ccw_speed_hi(float new_speed_hi) override { ccw.speed_hi = new_speed_hi; } + float get_ccw_speed_hi() override { return ccw.speed_hi; } + void set_ccw_phase_lo(float new_phase_lo) override { ccw.phase_lo = new_phase_lo; } + float get_ccw_phase_lo() override { return ccw.phase_lo; } + void set_ccw_phase_hi(float new_phase_hi) override { ccw.phase_hi = new_phase_hi; } + float get_ccw_phase_hi() override { return ccw.phase_hi; } + + void set_phase_off(float new_phase_off) override { phase_off = new_phase_off; } + float get_phase_off() override { return phase_off; } +}; + +// method implementations follow + +ESCSpindle::ESCSpindle(const uint8_t pwm_pin_number, const uint8_t enable_pin_number, + const uint8_t direction_pin_number, const float change_per_tick) + : speed_change_per_tick{change_per_tick}, + pwm_output_num{pwm_pin_number}, + enable_output_num{enable_pin_number}, + direction_output_num{direction_pin_number} {} + +void ESCSpindle::init() +{ + // TODO - ensure outputs are withing range + set_pwm_output(pwm_output_num); + set_enable_output(enable_output_num); + set_direction_output(direction_output_num); +} + +stat_t ESCSpindle::pause() { + if (paused) { + return (STAT_OK); + } + + paused = true; + this->complete_change(); + + return (STAT_OK); +} + +stat_t ESCSpindle::resume() { + if (!paused) { + return (STAT_OK); + } + + paused = false; + this->complete_change(); + + return (STAT_OK); +} + +bool ESCSpindle::ready_to_resume() { return paused && safety_manager->ok_to_spindle(); } +bool ESCSpindle::busy() { + // return true when not paused, on, and ramping up to speed + if (paused || (direction == SPINDLE_OFF) || fp_EQ(speed, speed_actual)) { + return false; + } + return true; +} + +// DON'T override set_speed - use engage instead +float ESCSpindle::get_speed() { return speed_actual; } + +// DON'T override set_direction - use engage instead +spDirection ESCSpindle::get_direction() { return direction; } + +// called from a command that was queued when the default set_speed and set_direction returned STAT_EAGAIN +// ALSO called from the loader right before a move +// we are handed the gcode model to use +void ESCSpindle::engage(const GCodeState_t &gm) { + if ((direction == gm.spindle_direction) && fp_EQ(speed, gm.spindle_speed)) { + // nothing changed + return; + } + + // special handling for reversals - we set the speed to zero and ramp up + if ((gm.spindle_direction != direction) && (direction != SPINDLE_OFF) && (gm.spindle_direction != SPINDLE_OFF)) { + speed_actual = 0; + } + + speed = gm.spindle_speed; + direction = gm.spindle_direction; + + // handle the rest + this->complete_change(); +} + +bool ESCSpindle::is_on() { return (direction != SPINDLE_OFF); } + +// ESCSpindle-specific functions +void ESCSpindle::set_pwm_output(const uint8_t pwm_pin_number) { + if (pwm_pin_number == 0) { + pwm_output = nullptr; + } else { + pwm_output = d_out[pwm_pin_number - 1]; + pwm_output->setEnabled(IO_ENABLED); + // set the frequency on the output -- not here + // set the polarity on the output -- not here + } +} +void ESCSpindle::set_enable_output(const uint8_t enable_pin_number) { + if (enable_pin_number == 0) { + enable_output = nullptr; + } else { + enable_output = d_out[enable_pin_number - 1]; + enable_output->setEnabled(IO_ENABLED); + // set the polarity on the output -- not here + } +} +void ESCSpindle::set_direction_output(const uint8_t direction_pin_number) { + if (direction_pin_number == 0) { + direction_output = nullptr; + } else { + direction_output = d_out[direction_pin_number - 1]; + direction_output->setEnabled(IO_ENABLED); + // set the polarity on the output -- not here + } +} + +void ESCSpindle::set_frequency(float new_frequency) +{ + if (pwm_output) { + pwm_output->setFrequency(new_frequency); + } +} +float ESCSpindle::get_frequency() +{ + if (pwm_output) { + return pwm_output->getFrequency(); + } + return 0.0; +} + +// Private functions + +void ESCSpindle::set_pwm_value() { + if (pwm_output == nullptr) { + return; + } + float value = phase_off; + if (paused) { + // nothing - leave it at phase_off + } else if (direction == SPINDLE_CW) { + value = cw.speed_to_phase(speed_actual); + } else if (direction == SPINDLE_CCW) { + value = ccw.speed_to_phase(speed_actual); + } + pwm_output->setValue(value); +} + +void ESCSpindle::complete_change() { + // if the spindle is not on (or paused), make sure we stop it + if (paused || direction == SPINDLE_OFF) { + speed_actual = 0; + set_pwm_value(); + + if (enable_output != nullptr) { + enable_output->setValue(false); + } + + return; + } else if (direction == SPINDLE_CW) { + if (enable_output != nullptr) { + enable_output->setValue(false); + } + if (direction_output != nullptr) { + direction_output->setValue(true); + } + } else if (direction == SPINDLE_CCW) { + if (enable_output != nullptr) { + enable_output->setValue(false); + } + if (direction_output != nullptr) { + direction_output->setValue(false); + } + } + + // setup for the rest to happen during systick + SysTickTimer.registerEvent(&spindle_systick_event); +} + +#endif // End of include guard: ESC_SPINDLE_H_ONCE diff --git a/g2core/gcode.h b/g2core/gcode.h index 4854bcff..1c0ac1f0 100644 --- a/g2core/gcode.h +++ b/g2core/gcode.h @@ -21,6 +21,7 @@ #define GCODE_H_ONCE #include "hardware.h" +#include "spindle.h" /**** Gcode-specific definitions ****/ @@ -158,7 +159,7 @@ typedef enum { // axis modes (ordered: see _cm_get_feed_time()) * (which may have changed). */ -typedef struct GCodeState { // Gcode model state - used by model, planning and runtime +struct GCodeState_t { // Gcode model state - used by model, planning and runtime int32_t linenum; // Gcode block line number cmMotionMode motion_mode; // Group1: G0, G1, G2, G3, G38.2, G80, G81, G82 // G83, G84, G85, G86, G87, G88, G89 @@ -181,6 +182,9 @@ typedef struct GCodeState { // Gcode model state - used by model, pl uint8_t tool; // G // M6 tool change - moves "tool_select" to "tool" uint8_t tool_select; // G // T value - T sets this value + float spindle_speed; // S - spindle "speed" in arbitrary units, often RPM + spDirection spindle_direction; // M3/M4/M5 - spindle on CW, on CCW, off setting + void reset() { linenum = 0; motion_mode = MOTION_MODE_STRAIGHT_TRAVERSE; @@ -205,7 +209,7 @@ typedef struct GCodeState { // Gcode model state - used by model, pl tool_select = 0; }; -} GCodeState_t; +}; typedef struct GCodeStateExtended { // Gcode dynamic state extensions - used by model and arcs uint16_t magic_start; // magic number to test memory integrity diff --git a/g2core/gcode_parser.cpp b/g2core/gcode_parser.cpp index 7f8440a6..5a8e64c6 100644 --- a/g2core/gcode_parser.cpp +++ b/g2core/gcode_parser.cpp @@ -145,7 +145,7 @@ typedef struct GCodeInputValue { // Gcode inputs - meaning depends on context uint8_t coolant_mist; // TRUE = mist on (M7) uint8_t coolant_flood; // TRUE = flood on (M8) uint8_t coolant_off; // TRUE = turn off all coolants (M9) - uint8_t spindle_control; // 0=OFF (M5), 1=CW (M3), 2=CCW (M4) + spDirection spindle_control; // 0=OFF (M5), 1=CW (M3), 2=CCW (M4) bool m48_enable; // M48/M49 input (enables for feed and spindle) bool fro_control; // M50 feedrate override control @@ -958,12 +958,12 @@ stat_t _execute_gcode_block(char *active_comment) ritorno(cm_check_linenum()); } - EXEC_FUNC(spindle_speed_sync, S_word); // S + EXEC_FUNC(spindle_set_speed, S_word); // S EXEC_FUNC(cm_select_tool, tool_select); // T - tool_select is where it's written EXEC_FUNC(cm_change_tool, tool_change); // M6 - is where it's effected if (gf.spindle_control) { // M3, M4, M5 (spindle OFF, CW, CCW) - ritorno(spindle_control_sync((spControl)gv.spindle_control)); + ritorno(spindle_set_direction(gv.spindle_control)); } if (gf.coolant_mist) { ritorno(coolant_control_sync((coControl)gv.coolant_mist, COOLANT_MIST)); // M7 diff --git a/g2core/kinematics.cpp b/g2core/kinematics.cpp index 5856f2f1..bbe502d4 100644 --- a/g2core/kinematics.cpp +++ b/g2core/kinematics.cpp @@ -127,7 +127,7 @@ struct CartesianKinematics : KinematicsBase { needs_sync_encoders = true; } - void inverse_kinematics(const float target[axes], const float position[axes], const float start_velocity, + void inverse_kinematics(const GCodeState_t &gm, const float target[axes], const float position[axes], const float start_velocity, const float end_velocity, const float segment_time, float steps[motors]) override { // joint == axis in cartesian kinematics @@ -206,7 +206,7 @@ struct CoreXYKinematics final : CartesianKinematics { // 7 = V (maybe) // 8 = W (maybe) - void inverse_kinematics(const float target[axes], const float position[axes], const float start_velocity, + void inverse_kinematics(const GCodeState_t &gm, const float target[axes], const float position[axes], const float start_velocity, const float end_velocity, const float segment_time, float steps[motors]) override { // need to have a place to store the adjusted COREXY A and B @@ -222,7 +222,7 @@ struct CoreXYKinematics final : CartesianKinematics { } // just use the cartesian method from here on - parent::inverse_kinematics(axes_target, position, start_velocity, end_velocity, segment_time, steps); + parent::inverse_kinematics(gm, axes_target, position, start_velocity, end_velocity, segment_time, steps); } void forward_kinematics(const float steps[motors], float position[axes]) override @@ -558,7 +558,7 @@ struct FourCableKinematics : KinematicsBase { double prev_cable_vel[4]; double prev_cable_accel[4]; - void inverse_kinematics(const float target[axes], const float position[axes], const float start_velocity, + void inverse_kinematics(const GCodeState_t &gm, const float target[axes], const float position[axes], const float start_velocity, const float end_velocity, const float segment_time, float steps[motors]) override { // read_sensors() also calls compute_encoder_error() which adjusts cable_position() incorporating the error @@ -679,20 +679,20 @@ struct FourCableKinematics : KinematicsBase { last_segment_was_idle = false; } - void inverse_kinematics(const float target[axes], float steps[motors]) override - { - compute_cable_position(target); + // void inverse_kinematics(const float target[axes], float steps[motors]) override + // { + // compute_cable_position(target); - for (uint8_t joint = 0; joint < 4; joint++) { - cable_vel[joint] = 0.0; - cable_accel[joint] = 0.0; - cable_jerk[joint] = 0.0; - } + // for (uint8_t joint = 0; joint < 4; joint++) { + // cable_vel[joint] = 0.0; + // cable_accel[joint] = 0.0; + // cable_jerk[joint] = 0.0; + // } - cables_to_steps(steps); + // cables_to_steps(steps); - last_segment_was_idle = false; - } + // last_segment_was_idle = false; + // } float best_steps_per_unit[joints]; diff --git a/g2core/kinematics.h b/g2core/kinematics.h index 0bb85317..ab6dbe4d 100644 --- a/g2core/kinematics.h +++ b/g2core/kinematics.h @@ -49,7 +49,9 @@ struct KinematicsBase { // must be as fast as possible while retaining precision // the other information is for the sake of tracking and intelligent error correction // the derivatives (acceleration, jerk) or other considerations - virtual void inverse_kinematics(const float target[axes], const float position[axes], const float start_velocity, const float end_velocity, const float segment_time, float steps[motors]) { + // the gcode model is passed in for additional context, and may be ignored + // the target is in the gcode model, but may be modified, so it's passed separately + virtual void inverse_kinematics(const GCodeState_t &gm, const float target[axes], const float position[axes], const float start_velocity, const float end_velocity, const float segment_time, float steps[motors]) { } // if the planner buffer is empty, the idel_task will be given the opportunity to drive the runtime diff --git a/g2core/main.cpp b/g2core/main.cpp index 2bed0f80..129e73c4 100644 --- a/g2core/main.cpp +++ b/g2core/main.cpp @@ -104,7 +104,6 @@ void application_init_machine(void) stepper_init(); // stepper subsystem encoder_init(); // virtual encoders gpio_init(); // inputs and outputs - pwm_init(); // pulse width modulation drivers canonical_machine_inits(); // combined inits for CMs and planner } diff --git a/g2core/plan_exec.cpp b/g2core/plan_exec.cpp index 3ae8e494..f4381398 100644 --- a/g2core/plan_exec.cpp +++ b/g2core/plan_exec.cpp @@ -248,11 +248,11 @@ stat_t mp_exec_move() // Run an out of band dwell. It was probably set in the previous st_load_move() // TODO: Find a better place for this - we shouldn't be concerned with dwells or othermove types here, // MORE: Dwells shouldn't hold planning hostage. - if (mr->out_of_band_dwell_flag) { - mr->out_of_band_dwell_flag = false; - st_prep_out_of_band_dwell(mr->out_of_band_dwell_seconds * 1000); - return (STAT_OK); - } + // if (mr->out_of_band_dwell_flag) { + // mr->out_of_band_dwell_flag = false; + // st_prep_out_of_band_dwell(mr->out_of_band_dwell_seconds * 1000); + // return (STAT_OK); + // } // NULL means nothing's running - this is OK // If something is MP_BUFFER_BACK_PLANNED, we don't want to idle or prep_null() @@ -913,7 +913,7 @@ static stat_t _exec_aline_segment() } // Convert target position to steps - kn->inverse_kinematics(mr->gm.target, mr->position, mr->segment_velocity, mr->target_velocity, mr->segment_time, exec_target_steps); + kn->inverse_kinematics(mr->gm, mr->gm.target, mr->position, mr->segment_velocity, mr->target_velocity, mr->segment_time, exec_target_steps); // Update the mb->run_time_remaining -- we know it's missing the current segment's time before it's loaded, that's ok. mp->run_time_remaining -= mr->segment_time; diff --git a/g2core/planner.cpp b/g2core/planner.cpp index dee2d3b1..fd989f52 100644 --- a/g2core/planner.cpp +++ b/g2core/planner.cpp @@ -358,7 +358,7 @@ stat_t mp_set_target_steps(const float target_steps[MOTORS], const float start_v * and makes keeping the queue full much easier - therefore avoiding Q starvation */ -void mp_queue_command(void(*cm_exec)(float *, bool *), float *value, bool *flag) +void mp_queue_command(cm_exec_t cm_exec, float *value, bool *flag) { mpBuf_t *bf; @@ -370,6 +370,7 @@ void mp_queue_command(void(*cm_exec)(float *, bool *), float *value, bool *flag) bf->block_type = BLOCK_TYPE_COMMAND; bf->bf_func = _exec_command; // callback to planner queue exec function bf->cm_func = cm_exec; // callback to canonical machine exec function + memcpy(&bf->gm, &cm->gm, sizeof(GCodeState_t)); // snapshot the active gcode state for (uint8_t axis = AXIS_X; axis < AXES; axis++) { bf->unit[axis] = value[axis]; // use the unit vector to store command values diff --git a/g2core/pwm.cpp b/g2core/pwm.cpp index f310ce40..89159667 100644 --- a/g2core/pwm.cpp +++ b/g2core/pwm.cpp @@ -25,6 +25,7 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +#if 0 #include "g2core.h" // #1 #include "config.h" // #2 #include "hardware.h" @@ -184,3 +185,4 @@ void pwm_print_p1wph(nvObj_t *nv) { text_print(nv, fmt_p1wph);} void pwm_print_p1pof(nvObj_t *nv) { text_print(nv, fmt_p1pof);} #endif //__TEXT_MODE +#endif // 0 diff --git a/g2core/pwm.h b/g2core/pwm.h index d367a873..0d0196b7 100644 --- a/g2core/pwm.h +++ b/g2core/pwm.h @@ -27,7 +27,7 @@ #ifndef PWM_H_ONCE #define PWM_H_ONCE - +#if 0 typedef struct pwmConfigChannel { float frequency; // base frequency for PWM driver, in Hz float cw_speed_lo; // minimum clockwise spindle speed [0..N] @@ -87,5 +87,5 @@ stat_t pwm_set_pwm(nvObj_t *nv); #define pwm_print_p1pof tx_print_stub #endif // __TEXT_MODE - +#endif // 0 #endif // End of include guard: PWM_H_ONCE diff --git a/g2core/safety_manager.cpp b/g2core/safety_manager.cpp new file mode 100644 index 00000000..9a76bfc1 --- /dev/null +++ b/g2core/safety_manager.cpp @@ -0,0 +1,37 @@ +/* + * safety_manager.cpp - The safety manager handles interlock and spindle safety controls + * This file is part of the g2core project + * + * Copyright (c) 2019 Rob Giseburt + * + * This code is a loose implementation of Kramer, Proctor and Messina's + * canonical machining functions as described in the NIST RS274/NGC v3 + */ +/* This file ("the software") is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License, version 2 as published by the + * Free Software Foundation. You should have received a copy of the GNU General Public + * License, version 2 along with the software. If not, see . + * + * As a special exception, you may use this file as part of a software library without + * restriction. Specifically, if other files instantiate templates or use macros or + * inline functions from this file, or you compile this file and link it with other + * files to produce an executable, this file does not by itself cause the resulting + * executable to be covered by the GNU General Public License. This exception does not + * however invalidate any other reasons why the executable file might be covered by the + * GNU General Public License. + * + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY + * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES + * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT + * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF + * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +#include "safety_manager.h" + +stat_t cm_get_saf(nvObj_t *nv) { return(get_integer(nv, safety_manager->get_interlock_enable())); } +stat_t cm_set_saf(nvObj_t *nv) { + safety_manager->set_interlock_enable((bool)nv->value_int); + return (STAT_OK); +} diff --git a/g2core/safety_manager.h b/g2core/safety_manager.h new file mode 100644 index 00000000..f851b022 --- /dev/null +++ b/g2core/safety_manager.h @@ -0,0 +1,186 @@ +/* + * safety_manager.h - The safety manager handles interlock and spindle safety controls + * This file is part of the g2core project + * + * Copyright (c) 2019 Rob Giseburt + * + * This code is a loose implementation of Kramer, Proctor and Messina's + * canonical machining functions as described in the NIST RS274/NGC v3 + */ +/* This file ("the software") is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License, version 2 as published by the + * Free Software Foundation. You should have received a copy of the GNU General Public + * License, version 2 along with the software. If not, see . + * + * As a special exception, you may use this file as part of a software library without + * restriction. Specifically, if other files instantiate templates or use macros or + * inline functions from this file, or you compile this file and link it with other + * files to produce an executable, this file does not by itself cause the resulting + * executable to be covered by the GNU General Public License. This exception does not + * however invalidate any other reasons why the executable file might be covered by the + * GNU General Public License. + * + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY + * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES + * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT + * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF + * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +#ifndef SAFETY_MANAGER_H_ONCE +#define SAFETY_MANAGER_H_ONCE + +#include "canonical_machine.h" +#include "planner.h" +#include "gpio.h" + +class SafetyManager; // forward declaration +extern SafetyManager *safety_manager; // setup in hardware.cpp + +// class to optionally override functions for managing safety functions +// default implementation handles interlock and alarm/shutdown states +class SafetyManager { + typedef enum { + 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; + + uint8_t shutdown_requested; // set non-zero to request shutdown in support of external estop (value is input number) + bool safety_interlock_enable; // true to enable safety interlock system + bool request_interlock; // enter interlock + bool request_interlock_exit; // exit interlock + uint8_t safety_interlock_disengaged; // set non-zero to start interlock processing (value is input number) + uint8_t safety_interlock_reengaged; // set non-zero to end interlock processing (value is input number) + cmSafetyState safety_interlock_state; // safety interlock state + + gpioDigitalInputHandler _shutdown_input_handler { + [&](const bool state, const inputEdgeFlag edge, const uint8_t triggering_pin_number) { + if (edge != INPUT_EDGE_LEADING) { return GPIO_NOT_HANDLED; } + + safety_manager->shutdown_requested = triggering_pin_number; + + return GPIO_HANDLED; + }, + 5, // priority + nullptr // next - nullptr to start with + }; + gpioDigitalInputHandler _interlock_input_handler { + [&](const bool state, const inputEdgeFlag edge, const uint8_t triggering_pin_number) { + if (edge != INPUT_EDGE_LEADING) { + safety_manager->safety_interlock_disengaged = triggering_pin_number; + } else { // edge == INPUT_EDGE_TRAILING + safety_manager->safety_interlock_reengaged = triggering_pin_number; + } + + return GPIO_HANDLED; + }, + 5, // priority + nullptr // next - nullptr to start with + }; + + public: + virtual void init() { + safety_interlock_disengaged = 0; // ditto + safety_interlock_reengaged = 0; // ditto + shutdown_requested = 0; // ditto + request_interlock = false; + request_interlock_exit = false; + + din_handlers[INPUT_ACTION_SHUTDOWN].registerHandler(&_shutdown_input_handler); + din_handlers[INPUT_ACTION_INTERLOCK].registerHandler(&_interlock_input_handler); + } + + virtual bool ok_to_spindle() { + // default, disable the spindle in interlock, alarm, shutdown, and panic states + if ((cm1.machine_state == MACHINE_INTERLOCK) || (cm1.machine_state == MACHINE_ALARM) || + (cm1.machine_state == MACHINE_SHUTDOWN) || (cm1.machine_state == MACHINE_PANIC)) { + return false; + } + + // otherwise safe + return true; + } + + virtual bool ok_to_coolant() { + // default, disable the coolant if the spindle isn't allowed + return this->ok_to_spindle(); + } + + virtual bool can_clear() { + if ((cm->machine_state == MACHINE_ALARM) || (cm->machine_state == MACHINE_SHUTDOWN)) { + return true; + } + return false; + } + + virtual bool can_queue_flush() { + return true; + } + + virtual stat_t is_system_alarmed() { + if (cm->machine_state == MACHINE_ALARM) { return (STAT_COMMAND_REJECTED_BY_ALARM); } + if (cm->machine_state == MACHINE_SHUTDOWN) { return (STAT_COMMAND_REJECTED_BY_SHUTDOWN); } + if (cm->machine_state == MACHINE_PANIC) { return (STAT_COMMAND_REJECTED_BY_PANIC); } + return (STAT_OK); + } + + virtual stat_t handle_shutdown () { + // called from periodic handler - useful to partially override it + + // SHUTDOWN handling + if (shutdown_requested != 0) { // request may contain the (non-zero) input number + char msg[10]; + sprintf(msg, "input %d", (int)shutdown_requested); + shutdown_requested = false; // clear limit request used here + cm_shutdown(STAT_SHUTDOWN, msg); + } + return(STAT_OK); + } + + virtual stat_t handle_interlock() { + // called from periodic handler - useful to partially override it + + // INTERLOCK handling + if (safety_interlock_enable) { + // interlock broken + if ((safety_interlock_disengaged != 0) && (safety_interlock_state == SAFETY_INTERLOCK_ENGAGED)) { + safety_interlock_disengaged = 0; + safety_interlock_state = SAFETY_INTERLOCK_DISENGAGING; + cm_request_feedhold(FEEDHOLD_TYPE_ACTIONS, FEEDHOLD_EXIT_INTERLOCK); // may have already requested STOP as INPUT_ACTION + } + + // interlock restored + if ((safety_interlock_reengaged != 0) && mp_runtime_is_idle() && (safety_interlock_state == SAFETY_INTERLOCK_DISENGAGED)) { + safety_interlock_reengaged = 0; + safety_interlock_state = SAFETY_INTERLOCK_ENGAGING; // interlock restored + cm_request_cycle_start(); // proper way to restart the cycle + } + } + return(STAT_OK); + } + + virtual stat_t periodic_handler() { + ritorno(this->handle_shutdown()); + ritorno(this->handle_interlock()); + return(STAT_OK); + } + + virtual void start_interlock_after_feedhold() { + safety_interlock_state = SAFETY_INTERLOCK_DISENGAGED; + } + + virtual void end_interlock_after_feedhold() { + safety_interlock_state = SAFETY_INTERLOCK_ENGAGED; + } + + virtual bool get_interlock_enable() { return safety_interlock_enable; } + virtual void set_interlock_enable(bool enable) { safety_interlock_enable = enable; } +}; + +stat_t cm_get_saf(nvObj_t *nv); // get safety interlock enable +stat_t cm_set_saf(nvObj_t *nv); // set safety interlock enable + +#endif // SAFETY_MANAGER_H_ONCE diff --git a/g2core/spindle.cpp b/g2core/spindle.cpp index 1e64ee9e..81634c31 100644 --- a/g2core/spindle.cpp +++ b/g2core/spindle.cpp @@ -30,502 +30,118 @@ #include "canonical_machine.h" // #3 #include "text_parser.h" // #4 -#include "gpio.h" +#include "planner.h" // for mp_queue_command() #include "spindle.h" -#include "planner.h" -#include "hardware.h" -#include "pwm.h" -#include "util.h" -/**** Allocate structures ****/ - -#define SPINDLE_OVERRIDE_ENABLE false -#define SPINDLE_OVERRIDE_FACTOR 1.00 -#define SPINDLE_OVERRIDE_MIN 0.05 // 5% -#define SPINDLE_OVERRIDE_MAX 2.00 // 200% -#define SPINDLE_OVERRIDE_RAMP_TIME 1 // change sped in seconds - -enum spMode { - SPINDLE_DISABLED = 0, // spindle will not operate - SPINDLE_PLAN_TO_STOP, // spindle operating, plans to stop - SPINDLE_CONTINUOUS, // spindle operating, does not plan to stop -}; -#define SPINDLE_MODE_MAX SPINDLE_CONTINUOUS - -// *** NOTE: The spindle polarity active hi/low values currently agree with ioMode in gpio.h -// These will all need to be changed to ACTIVE_HIGH = 0, ACTIVE_LOW = 1 -// See: https://github.com/synthetos/g2_private/wiki/GPIO-Design-Discussion#settings-common-to-all-io-types - -enum spPolarity { // Note: These values agree with - SPINDLE_ACTIVE_LOW = 0, // Will set output to 0 to enable the spindle or CW direction - SPINDLE_ACTIVE_HIGH = 1, // Will set output to 1 to enable the spindle or CW direction -}; - -enum ESCState { // electronic speed controller for some spindles - ESC_ONLINE = 0, - ESC_OFFLINE, - ESC_LOCKOUT, - ESC_REBOOTING, - ESC_LOCKOUT_AND_REBOOTING, -}; - -/* - * Spindle control structure - */ - -struct spSpindle_t { - - spControl state; // {spc:} OFF, ON, PAUSE, RESUME, WAIT - spControl direction; // 1=CW, 2=CCW (subset of above state) - - float speed; // {sps:} S in RPM - float speed_min; // {spsn:} minimum settable spindle speed - float speed_max; // {spsm:} maximum settable spindle speed - float speed_actual; // hidden internal value used in speed ramping - float speed_change_per_tick; // hidden internal value used in speed ramping - - spPolarity enable_polarity; // {spep:} 0=active low, 1=active high - spPolarity dir_polarity; // {spdp:} 0=clockwise low, 1=clockwise high - bool pause_enable; // {spph:} pause on feedhold - float spinup_delay; // {spde:} optional delay on spindle start (set to 0 to disable) -// float spindown_delay; // {spds:} optional delay on spindle stop (set to 0 to disable) - - bool override_enable; // {spoe:} TRUE = spindle speed override enabled (see also m48_enable in canonical machine) - float override_factor; // {spo:} 1.0000 x S spindle speed. Go up or down from there -}; - -spSpindle_t spindle; - - -gpioDigitalOutput *spindle_enable_output = nullptr; -gpioDigitalOutput *spindle_direction_output = nullptr; - -#ifndef SPINDLE_ENABLE_OUTPUT_NUMBER -#warning SPINDLE_ENABLE_OUTPUT_NUMBER is defaulted to 4! -#warning SPINDLE_ENABLE_OUTPUT_NUMBER should be defined in settings or a board file! -#define SPINDLE_ENABLE_OUTPUT_NUMBER 4 -#endif - -#ifndef SPINDLE_DIRECTION_OUTPUT_NUMBER -#warning SPINDLE_DIRECTION_OUTPUT_NUMBER is defaulted to 5! -#warning SPINDLE_DIRECTION_OUTPUT_NUMBER should be defined in settings or a board file! -#define SPINDLE_DIRECTION_OUTPUT_NUMBER 5 -#endif - -/**** Static functions ****/ - -static float _get_spindle_pwm (spSpindle_t &_spindle, pwmControl_t &_pwm); - -#define SPINDLE_DIRECTION_ASSERT \ - if ((spindle.direction < SPINDLE_CW) || (spindle.direction > SPINDLE_CCW)) { \ - spindle.direction = SPINDLE_CW; \ - } - -#ifndef SPINDLE_SPEED_CHANGE_PER_MS -#define SPINDLE_SPEED_CHANGE_PER_MS 0 -#endif +ToolHead *active_toolhead = nullptr; +bool spindle_pause_enabled = true; /**************************************************************************************** - * spindle_init() + * toolhead_for_tool(uint8_t tool) - return the correct toolhead for the tool number + * spindle_init() - init the spindle / toolhead subsystem + * spindle_set_toolhead(ToolHead *toolhead) - set the active toolhead * spindle_reset() - stop spindle, set speed to zero, and reset values */ -void spindle_init() -{ - SPINDLE_DIRECTION_ASSERT // spindle needs an initial direction - if (SPINDLE_ENABLE_OUTPUT_NUMBER > 0) { - spindle_enable_output = d_out[SPINDLE_ENABLE_OUTPUT_NUMBER-1]; - spindle_enable_output->setEnabled(IO_ENABLED); - spindle_enable_output->setPolarity((ioPolarity)SPINDLE_ENABLE_POLARITY); - } - if (SPINDLE_DIRECTION_OUTPUT_NUMBER > 0) { - spindle_direction_output = d_out[SPINDLE_DIRECTION_OUTPUT_NUMBER-1]; - spindle_direction_output->setEnabled(IO_ENABLED); - spindle_direction_output->setPolarity((ioPolarity)SPINDLE_DIR_POLARITY); - } - - if( pwm.c[PWM_1].frequency < 0 ) { - pwm.c[PWM_1].frequency = 0; - } - pwm_set_freq(PWM_1, pwm.c[PWM_1].frequency); - pwm_set_duty(PWM_1, pwm.c[PWM_1].phase_off); - - spindle.speed_change_per_tick = SPINDLE_SPEED_CHANGE_PER_MS; +void spindle_init() { + // active_toolhead->init(); } -void spindle_reset() -{ - spindle_speed_immediate(0); - spindle_control_immediate(SPINDLE_OFF); +void spindle_set_toolhead(ToolHead *toolhead) { + active_toolhead = toolhead; + active_toolhead->reset(); } -// to be used blow, assumes spindle.speed (etc) are already setup -void _actually_set_spindle_speed() { - float speed_lo, speed_hi; - bool clamp_speeds = false; - if (spindle.state == SPINDLE_CW) { - speed_lo = pwm.c[PWM_1].cw_speed_lo; - speed_hi = pwm.c[PWM_1].cw_speed_hi; - clamp_speeds = true; - } else if (spindle.state == SPINDLE_CCW ) { - speed_lo = pwm.c[PWM_1].ccw_speed_lo; - speed_hi = pwm.c[PWM_1].ccw_speed_hi; - clamp_speeds = true; - } else { - // off/disabled/paused - spindle.speed_actual = 0; - } - - if (clamp_speeds) { - // clamp spindle speed to lo/hi range - if (spindle.speed < speed_lo) { - spindle.speed = speed_lo; - } - - // allow spindle.speed_actual to start at 0 to match physical spinup - - if (spindle.speed > speed_hi) { - spindle.speed = speed_hi; - } - - if (spindle.speed_actual > speed_hi) { - spindle.speed_actual = speed_hi; - } - } else { - pwm_set_duty(PWM_1, _get_spindle_pwm(spindle, pwm)); - return; - } - - if (fp_ZERO(spindle.speed_change_per_tick)) { // || (spindle.speed <= spindle.speed_actual) - spindle.speed_actual = spindle.speed; - } - pwm_set_duty(PWM_1, _get_spindle_pwm(spindle, pwm)); - - if (fp_NE(spindle.speed_actual, spindle.speed)) { // && (spindle.speed > spindle.speed_actual) - // use the larger of: spindup_delay setting, or the time it'll take to ramp to the new speed, converted to seconds - if (fp_NOT_ZERO(spindle.speed_change_per_tick)) { - mp_request_out_of_band_dwell(spindle.spinup_delay + 0.001*std::abs(spindle.speed-spindle.speed_actual)/spindle.speed_change_per_tick); - } else { - mp_request_out_of_band_dwell(spindle.spinup_delay); - } - } -} - -/**************************************************************************************** - * _exec_spindle_control() - actually execute the spindle command - * spindle_control_immediate() - execute spindle control immediately - * spindle_control_sync() - queue a spindle control to the planner buffer - * - * Basic operation: Spindle function is executed by _exec_spindle_control(). - * Spindle_control_immediate() performs the control as soon as it's received. - * Spindle_control_sync() inserts spindle move into the planner, and handles spinups. - * - * Valid inputs to Spindle_control_immediate() and Spindle_control_sync() are: - * - * - SPINDLE_OFF turns off spindle and sets spindle state to SPINDLE_OFF. - * This will also re-load enable and direction polarity to the pins if they have changed. - * The spindle.direction value is not affected (although this doesn't really matter). - * - * - SPINDLE_CW or SPINDLE_CCW turns sets direction accordingly and spindle on. - * In spindle_control_sync() a non-zero spinup delay runs a dwell immediately - * following the spindle change, but only if the planner had planned the spindle - * operation to zero. (I.e. if the spindle controls / S words do not plan to zero - * the delay is not run). Spindle_control_immediate() has no spinup delay or - * dwell behavior. - * - * - SPINDLE_PAUSE is only applicable to CW and CCW states. It forces the spindle OFF and - * sets spindle.state to PAUSE. A PAUSE received when not in CW or CCW state is ignored. - * - * - SPINDLE_RESUME, if in a PAUSE state, reverts to previous SPINDLE_CW or SPINDLE_CCW. - * The SPEED is not changed, and if it were changed in the interim the "new" speed - * is used. If RESUME is received from spindle_control_sync() the usual spinup delay - * behavior occurs. If RESUME is received when not in a PAUSED state it is ignored. - * This recognizes that the main reason an immediate command would be issued - either - * manually by the user or by an alarm or some other program function - is to stop - * a spindle. So the Resume should be ignored for safety. - */ -/* Notes: - * - Since it's possible to queue a sync'd control, and then set any spindle state - * with an immediate() before the queued command is reached, _exec_spindle_control() - * must gracefully handle any arbitrary state transition (not just the "legal" ones). - * - * - The spinup and spindown rows are present, but are not implemented unless we - * find we need them. It's easy enough to set these flags using the bit vector - * passed from sync(),but unsetting them once the delay is complete would take - * some more work. - * - * Q: Do we need a spin-down for direction reversal? - * Q: Should the JSON be able to pause and resume? For test purposes only? - */ -/* State/Control matrix. Read "If you are in state X and get control Y do action Z" - - Control: OFF CW CCW PAUSE RESUME - State: |-----------|-----------|-----------|-----------|-----------| - OFF | OFF | CW | CCW | NOP | NOP | - |-----------|-----------|-----------|-----------|-----------| - CW | OFF | NOP | REVERSE | PAUSE | NOP | - |-----------|-----------|-----------|-----------|-----------| - CCW | OFF | REVERSE | NOP | PAUSE | NOP | - |-----------|-----------|-----------|-----------|-----------| - PAUSE | OFF | CW | CCW | NOP | RESUME | - |-----------|-----------|-----------|-----------|-----------| - RESUME | invalid | invalid | invalid | invalid | invalid | - |-----------|-----------|-----------|-----------|-----------| - - Actions: - - OFF Turn spindle off. Even if it's already off (reloads polarities) - - CW Turn spindle on clockwise - - CCW Turn spindle on counterclockwise - - PAUSE Turn off spindle, enter PAUSE state - - RESUME Turn spindle on CW or CCW as before - - NOP No operation, ignore - - REVERSE Reverse spindle direction (Q: need a cycle to spin down then back up again?) - */ - -static void _exec_spindle_control(float *value, bool *) -{ - spControl control = (spControl)value[0]; - if (control > SPINDLE_ACTION_MAX) { - return; - } - spControl state = spindle.state; - if (state >= SPINDLE_ACTION_MAX) { -// rpt_exception(STAT_SPINDLE_ASSERTION_FAILURE, "illegal spindle state"); - return; - } - constexpr spControl matrix[20] = { - SPINDLE_OFF, SPINDLE_CW, SPINDLE_CCW, SPINDLE_NOP, SPINDLE_NOP, - SPINDLE_OFF, SPINDLE_NOP, SPINDLE_REV, SPINDLE_PAUSE, SPINDLE_NOP, - SPINDLE_OFF, SPINDLE_REV, SPINDLE_NOP, SPINDLE_PAUSE, SPINDLE_NOP, - SPINDLE_OFF, SPINDLE_CW, SPINDLE_CCW, SPINDLE_NOP, SPINDLE_RESUME - }; - spControl action = matrix[(state*5)+control]; - - SPINDLE_DIRECTION_ASSERT; // ensure that the spindle direction is sane - int8_t enable_bit = 0; // default to 0=off - int8_t dir_bit = -1; // -1 will skip setting the direction. 0 & 1 are valid values - -// #ifdef ENABLE_INTERLOCK_AND_ESTOP -// if (!spindle_ready_to_resume()) { // 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 || cm1.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; } - - case SPINDLE_OFF: { // enable_bit already set for this case - dir_bit = spindle.direction-1; // spindle direction was stored as '1' & '2' - spindle.state = SPINDLE_OFF; // the control might have been something other than SPINDLE_OFF - break; - } - case SPINDLE_CW: case SPINDLE_CCW: case SPINDLE_REV: { // REV is handled same as CW or CCW for now - enable_bit = 1; - dir_bit = control-1; // adjust direction to be used as a bitmask - spindle.direction = control; - spindle.state = control; - break; - } - case SPINDLE_PAUSE : { - spindle.state = SPINDLE_PAUSE; - break; // enable bit is already set up to stop the move - } - case SPINDLE_RESUME: { - enable_bit = 1; - dir_bit = spindle.direction-1; // spindle direction was stored as '1' & '2' - spindle.state = spindle.direction; - break; - } - default: {} // reversals not handled yet - } - - // Apply the enable and direction bits and adjust the PWM as required - - // set the direction first - if (dir_bit >= 0) { - if (spindle_direction_output != nullptr) { - spindle_direction_output->setValue(dir_bit); - } - } - - // set spindle enable - if (spindle_enable_output != nullptr) { - spindle_enable_output->setValue(enable_bit); - } - - _actually_set_spindle_speed(); +void spindle_reset() { + active_toolhead->reset(); } /* - * spindle_control_immediate() - execute spindle control immediately - * spindle_control_sync() - queue a spindle control to the planner buffer - */ + * spindle_stop(); + * spindle_pause(); + * spindle_resume(); + * spindle_set_speed(float speed); // S parameter - returns STAT_EAGAIN if a command should be queued + * float spindle_get_speed(); // return current speed - in the same units as the S parameter + * spindle_set_direction(spDirection direction); // M3/M4/M5 - - returns STAT_EAGAIN if a command should be queued + * spDirection spindle_get_direction(); // return if any fo M3/M4/M5 are active (actual, not gcode model) + * bool is_spindle_ready_to_resume(); // if the spindle can resume at this time, return true + * bool is_spindle_on_or_paused(); // returns if the spindle is on or paused - IOW would it try to resume from feedhold +*/ -stat_t spindle_control_immediate(spControl control) -{ - float value[] = { (float)control }; - _exec_spindle_control(value, nullptr); - return(STAT_OK); -} - -stat_t spindle_control_sync(spControl control) // uses spControl arg: OFF, CW, CCW -{ - // skip the PAUSE operation if pause-enable is not enabled (pause-on-hold) - if ((control == SPINDLE_PAUSE) && (!spindle.pause_enable)) { - return (STAT_OK); +stat_t spindle_stop() { + cm->gm.spindle_direction = SPINDLE_OFF; + if (active_toolhead) { + return active_toolhead->stop(); } - - // ignore pause and resume if the spindle isn't even on - if ((spindle.state == SPINDLE_OFF) && (control == SPINDLE_PAUSE || control == SPINDLE_RESUME)) { - return (STAT_OK); - } - - if (spindle.speed > 0.0 && !is_spindle_ready_to_resume()) { - // request a feedhold immediately - cm_request_feedhold(FEEDHOLD_TYPE_ACTIONS, FEEDHOLD_EXIT_CYCLE); - } - - // queue the spindle control - float value[] = { (float)control }; - mp_queue_command(_exec_spindle_control, value, nullptr); - return(STAT_OK); + return (STAT_OK); } - -/**************************************************************************************** - * _exec_spindle_speed() - actually execute the spindle speed command - * spindle_speed_immediate() - execute spindle speed change immediately - * spindle_speed_sync() - queue a spindle speed change to the planner buffer - * - * Setting S0 is considered as turning spindle off. Setting S to non-zero from S0 - * will enable a spinup delay if spinups are npn-zero. - */ - -static void _exec_spindle_speed(float *value, bool *flag) -{ - spindle.speed = value[0]; - - _actually_set_spindle_speed(); +stat_t spindle_pause() { + if (spindle_pause_enabled && active_toolhead) { + return active_toolhead->pause(); + } + return (STAT_OK); } - -static stat_t _casey_jones(float speed) -{ - if (speed < spindle.speed_min) { return (STAT_SPINDLE_SPEED_BELOW_MINIMUM); } - if (speed > spindle.speed_max) { return (STAT_SPINDLE_SPEED_MAX_EXCEEDED); } +stat_t spindle_resume() { + if (spindle_pause_enabled && active_toolhead) { + return active_toolhead->resume(); + } return (STAT_OK); } -stat_t spindle_speed_immediate(float speed) -{ - ritorno(_casey_jones(speed)); - float value[] = { speed }; - _exec_spindle_speed(value, nullptr); - return (STAT_OK); +// A command for placing in the queue, which forces a PTS (plan-to-stop) as well as calls active_toolhead->engage() +static void _exec_spindle_control(float *, bool *) { + // not really anything to do here - engage() should have just been called } -stat_t spindle_speed_sync(float speed) -{ - ritorno(_casey_jones(speed)); - float value[] = { speed }; - mp_queue_command(_exec_spindle_speed, value, nullptr); +stat_t spindle_set_speed(float speed) { + cm->gm.spindle_speed = speed; + + if (active_toolhead && active_toolhead->set_speed(speed) == STAT_EAGAIN) { + mp_queue_command(_exec_spindle_control, nullptr, nullptr); + } + return (STAT_OK); } +float spindle_get_speed() { + if (active_toolhead) { return active_toolhead->get_speed(); } + return cm->gm.spindle_speed; // if there's not active toolhead, return what the gcode model has +} + +stat_t spindle_set_direction(spDirection direction) +{ + cm->gm.spindle_direction = direction; + + if (active_toolhead && active_toolhead->set_direction(direction) == STAT_EAGAIN) { + mp_queue_command(_exec_spindle_control, nullptr, nullptr); + } + + return (STAT_OK); +} +spDirection spindle_get_direction() { + if (active_toolhead) { return active_toolhead->get_direction(); } + return cm->gm.spindle_direction; // if there's not active toolhead, return what the gcode model has +} + +void spindle_engage(const GCodeState_t &gm) { + if (active_toolhead) { active_toolhead->engage(gm); } +} bool is_spindle_ready_to_resume() { -#ifdef ENABLE_INTERLOCK_AND_ESTOP - if ((cm1.estop_state != 0) || (cm1.safety_state != 0)) { - return false; - } -#endif + if (active_toolhead) { return active_toolhead->ready_to_resume(); } return true; } - bool is_spindle_on_or_paused() { - if (spindle.state != SPINDLE_OFF) { - return true; + if (active_toolhead) { return active_toolhead->is_on(); } + return cm->gm.spindle_direction != SPINDLE_OFF; +} +bool is_a_toolhead_busy() { + // TODO: look at more than just one toolhead + if (active_toolhead) { + return active_toolhead->busy(); } return false; } -// returns if it's done -bool do_spindle_speed_ramp_from_systick() { -#ifdef ENABLE_INTERLOCK_AND_ESTOP - bool done = false; - if ((cm1.estop_state == 0) && (cm1.safety_state == 0)) { - if (fp_EQ(spindle.speed_actual, spindle.speed)) { - return true; - } else if (spindle.speed_actual < spindle.speed) { - spindle.speed_actual += spindle.speed_change_per_tick; - if (spindle.speed_actual > spindle.speed) { - spindle.speed_actual = spindle.speed; - done = true; - } - } - else { - spindle.speed_actual -= spindle.speed_change_per_tick; - if (spindle.speed_actual < spindle.speed) { - spindle.speed_actual = spindle.speed; - done = true; - } - } - pwm_set_duty(PWM_1, _get_spindle_pwm(spindle, pwm)); - } else { - spindle.speed_actual = 0; - spindle.state = SPINDLE_PAUSE; - pwm_set_duty(PWM_1, _get_spindle_pwm(spindle, pwm)); - done = (cm1.hold_state != FEEDHOLD_OFF); - } - return done; -#else - return true; -#endif -} - -/**************************************************************************************** - * _get_spindle_pwm() - return PWM phase (duty cycle) for dir and speed - */ - -static float _get_spindle_pwm (spSpindle_t &_spindle, pwmControl_t &_pwm) -{ - float speed_lo, speed_hi, phase_lo, phase_hi; - if (_spindle.direction == SPINDLE_CW ) { - speed_lo = _pwm.c[PWM_1].cw_speed_lo; - speed_hi = _pwm.c[PWM_1].cw_speed_hi; - phase_lo = _pwm.c[PWM_1].cw_phase_lo; - phase_hi = _pwm.c[PWM_1].cw_phase_hi; - } else { // if (direction == SPINDLE_CCW ) { - speed_lo = _pwm.c[PWM_1].ccw_speed_lo; - speed_hi = _pwm.c[PWM_1].ccw_speed_hi; - phase_lo = _pwm.c[PWM_1].ccw_phase_lo; - phase_hi = _pwm.c[PWM_1].ccw_phase_hi; - } - - if ((_spindle.state == SPINDLE_CW) || (_spindle.state == SPINDLE_CCW)) { - // clamp spindle speed to lo/hi range - // if (_spindle.speed_actual < speed_lo) { - // _spindle.speed_actual = speed_lo; - // } - if (_spindle.speed_actual > speed_hi) { - _spindle.speed_actual = speed_hi; - } - // normalize speed to [0..1] - float speed = std::max(0.0f, (_spindle.speed_actual - speed_lo)) / (speed_hi - speed_lo); - return (speed * (phase_hi - phase_lo)) + phase_lo; - } else { - return (_pwm.c[PWM_1].phase_off); - } -} - /**************************************************************************************** * spindle_override_control() * spindle_start_override() @@ -534,30 +150,30 @@ static float _get_spindle_pwm (spSpindle_t &_spindle, pwmControl_t &_pwm) stat_t spindle_override_control(const float P_word, const bool P_flag) // M51 { - bool new_enable = true; - bool new_override = false; - if (P_flag) { // if parameter is present in Gcode block - if (fp_ZERO(P_word)) { - new_enable = false; // P0 disables override - } else { - if (P_word < SPINDLE_OVERRIDE_MIN) { - return (STAT_INPUT_LESS_THAN_MIN_VALUE); - } - if (P_word > SPINDLE_OVERRIDE_MAX) { - return (STAT_INPUT_EXCEEDS_MAX_VALUE); - } - spindle.override_factor = P_word; // P word is valid, store it. - new_override = true; - } - } - if (cm->gmx.m48_enable) { // if master enable is ON - if (new_enable && (new_override || !spindle.override_enable)) { // 3 cases to start a ramp - spindle_start_override(SPINDLE_OVERRIDE_RAMP_TIME, spindle.override_factor); - } else if (spindle.override_enable && !new_enable) { // case to turn off the ramp - spindle_end_override(SPINDLE_OVERRIDE_RAMP_TIME); - } - } - spindle.override_enable = new_enable; // always update the enable state + // bool new_enable = true; + // bool new_override = false; + // if (P_flag) { // if parameter is present in Gcode block + // if (fp_ZERO(P_word)) { + // new_enable = false; // P0 disables override + // } else { + // if (P_word < SPINDLE_OVERRIDE_MIN) { + // return (STAT_INPUT_LESS_THAN_MIN_VALUE); + // } + // if (P_word > SPINDLE_OVERRIDE_MAX) { + // return (STAT_INPUT_EXCEEDS_MAX_VALUE); + // } + // spindle.override_factor = P_word; // P word is valid, store it. + // new_override = true; + // } + // } + // if (cm->gmx.m48_enable) { // if master enable is ON + // if (new_enable && (new_override || !spindle.override_enable)) { // 3 cases to start a ramp + // spindle_start_override(SPINDLE_OVERRIDE_RAMP_TIME, spindle.override_factor); + // } else if (spindle.override_enable && !new_enable) { // case to turn off the ramp + // spindle_end_override(SPINDLE_OVERRIDE_RAMP_TIME); + // } + // } + // spindle.override_enable = new_enable; // always update the enable state return (STAT_OK); } @@ -584,24 +200,26 @@ void spindle_end_override(const float ramp_time) **** Spindle Settings ****************************************************************** ****************************************************************************************/ -stat_t sp_get_spep(nvObj_t *nv) { return(get_integer(nv, spindle.enable_polarity)); } -stat_t sp_set_spep(nvObj_t *nv) { - stat_t status = set_integer(nv, (uint8_t &)spindle.enable_polarity, 0, 1); - spindle_enable_output->setPolarity((ioPolarity)spindle.enable_polarity); - spindle_control_immediate(SPINDLE_OFF); // stop spindle and apply new settings - return (status); +stat_t sp_get_spep(nvObj_t *nv) { return(get_integer(nv, -1)); } // moved to gpio controls +stat_t sp_set_spep(nvObj_t *nv) { // moved to gpio controls + // stat_t status = set_integer(nv, (uint8_t &)spindle.enable_polarity, 0, 1); + // spindle_enable_output->setPolarity((ioPolarity)spindle.enable_polarity); + // spindle_stop(); // stop spindle and apply new settings + return (STAT_OK); } -stat_t sp_get_spdp(nvObj_t *nv) { return(get_integer(nv, spindle.dir_polarity)); } -stat_t sp_set_spdp(nvObj_t *nv) { - stat_t status = set_integer(nv, (uint8_t &)spindle.dir_polarity, 0, 1); - spindle_direction_output->setPolarity((ioPolarity)spindle.dir_polarity); - spindle_control_immediate(SPINDLE_OFF); // stop spindle and apply new settings - return (status); +stat_t sp_get_spdp(nvObj_t *nv) { return(get_integer(nv, -1)); } // moved to gpio controls +stat_t sp_set_spdp(nvObj_t *nv) { // moved to gpio controls + // stat_t status = set_integer(nv, (uint8_t &)spindle.dir_polarity, 0, 1); + // spindle_direction_output->setPolarity((ioPolarity)spindle.dir_polarity); + // spindle_stop(); // stop spindle and apply new settings + return (STAT_OK); } -stat_t sp_get_spph(nvObj_t *nv) { return(get_integer(nv, spindle.pause_enable)); } -stat_t sp_set_spph(nvObj_t *nv) { return(set_integer(nv, (uint8_t &)spindle.pause_enable, 0, 1)); } +stat_t sp_get_spph(nvObj_t *nv) { return(get_integer(nv, spindle_pause_enabled)); } +stat_t sp_set_spph(nvObj_t *nv) { return(set_integer(nv, (uint8_t &)spindle_pause_enabled, 0, 1)); } + +/* stat_t sp_get_spde(nvObj_t *nv) { return(get_float(nv, spindle.spinup_delay)); } stat_t sp_set_spde(nvObj_t *nv) { return(set_float_range(nv, spindle.spinup_delay, 0, SPINDLE_DWELL_MAX)); } @@ -620,6 +238,118 @@ stat_t sp_get_spc(nvObj_t *nv) { return(get_integer(nv, spindle.state)); } stat_t sp_set_spc(nvObj_t *nv) { return(spindle_control_immediate((spControl)nv->value_int)); } stat_t sp_get_sps(nvObj_t *nv) { return(get_float(nv, spindle.speed)); } stat_t sp_set_sps(nvObj_t *nv) { return(spindle_speed_immediate(nv->value_flt)); } +*/ + +stat_t sp_get_spde(nvObj_t *nv) { return(get_float(nv, 0)); } +stat_t sp_set_spde(nvObj_t *nv) { return(STAT_OK); } + +stat_t sp_get_spsn(nvObj_t *nv) { return(get_float(nv, SPINDLE_SPEED_MIN)); } +stat_t sp_set_spsn(nvObj_t *nv) { return(STAT_OK); } +stat_t sp_get_spsm(nvObj_t *nv) { return(get_float(nv, SPINDLE_SPEED_MAX)); } +stat_t sp_set_spsm(nvObj_t *nv) { return(STAT_OK); } + +stat_t sp_get_spoe(nvObj_t *nv) { return(get_integer(nv, 0)); } +stat_t sp_set_spoe(nvObj_t *nv) { return(STAT_OK); } +stat_t sp_get_spo(nvObj_t *nv) { return(get_float(nv, 1.0)); } +stat_t sp_set_spo(nvObj_t *nv) { return(STAT_OK); } + +// These are provided as a way to view and control spindles without using M commands +stat_t sp_get_spc(nvObj_t *nv) { return(get_integer(nv, spindle_get_direction())); } +stat_t sp_set_spc(nvObj_t *nv) { return(spindle_set_direction((spDirection)nv->value_int)); } +stat_t sp_get_sps(nvObj_t *nv) { return(get_float(nv, spindle_get_speed())); } +stat_t sp_set_sps(nvObj_t *nv) { return(spindle_set_speed(nv->value_flt)); } + + +/*********************************************************************************** + * CONFIGURATION AND INTERFACE FUNCTIONS + * Functions to get and set variables from the cfgArray table + ***********************************************************************************/ +/* + * pwm_get_*() - get generic PWM parameter and reset PWM channels + * pwm_set_*() - set generic PWM parameter and reset PWM channels + * + */ + +stat_t pwm_get_p1frq(nvObj_t *nv) { + if (active_toolhead) { return active_toolhead->get_frequency(); } + else { return (STAT_OK); } +} +stat_t pwm_set_p1frq(nvObj_t *nv) { + if (active_toolhead) { active_toolhead->set_frequency(nv->value_flt); } + return (STAT_OK); +} +stat_t pwm_get_p1csl(nvObj_t *nv) { + if (active_toolhead) { return active_toolhead->get_cw_speed_lo(); } + else { return (STAT_OK); } +} +stat_t pwm_set_p1csl(nvObj_t *nv) { + if (active_toolhead) { active_toolhead->set_cw_speed_lo(nv->value_flt); } + return (STAT_OK); +} +stat_t pwm_get_p1csh(nvObj_t *nv) { + if (active_toolhead) { return active_toolhead->get_cw_speed_hi(); } + else { return (STAT_OK); } +} +stat_t pwm_set_p1csh(nvObj_t *nv) { + if (active_toolhead) { active_toolhead->set_cw_speed_hi(nv->value_flt); } + return (STAT_OK); +} +stat_t pwm_get_p1cpl(nvObj_t *nv) { + if (active_toolhead) { return active_toolhead->get_cw_phase_lo(); } + else { return (STAT_OK); } +} +stat_t pwm_set_p1cpl(nvObj_t *nv) { + if (active_toolhead) { active_toolhead->set_cw_phase_lo(nv->value_flt); } + return (STAT_OK); +} +stat_t pwm_get_p1cph(nvObj_t *nv) { + if (active_toolhead) { return active_toolhead->get_cw_phase_hi(); } + else { return (STAT_OK); } +} +stat_t pwm_set_p1cph(nvObj_t *nv) { + if (active_toolhead) { active_toolhead->set_cw_phase_hi(nv->value_flt); } + return (STAT_OK); +} +stat_t pwm_get_p1wsl(nvObj_t *nv) { + if (active_toolhead) { return active_toolhead->get_ccw_speed_lo(); } + else { return (STAT_OK); } +} +stat_t pwm_set_p1wsl(nvObj_t *nv) { + if (active_toolhead) { active_toolhead->set_ccw_speed_lo(nv->value_flt); } + return (STAT_OK); +} +stat_t pwm_get_p1wsh(nvObj_t *nv) { + if (active_toolhead) { return active_toolhead->get_ccw_speed_hi(); } + else { return (STAT_OK); } +} +stat_t pwm_set_p1wsh(nvObj_t *nv) { + if (active_toolhead) { active_toolhead->set_ccw_speed_hi(nv->value_flt); } + return (STAT_OK); +} +stat_t pwm_get_p1wpl(nvObj_t *nv) { + if (active_toolhead) { return active_toolhead->get_ccw_phase_lo(); } + else { return (STAT_OK); } +} +stat_t pwm_set_p1wpl(nvObj_t *nv) { + if (active_toolhead) { active_toolhead->set_ccw_phase_lo(nv->value_flt); } + return (STAT_OK); +} +stat_t pwm_get_p1wph(nvObj_t *nv) { + if (active_toolhead) { return active_toolhead->get_ccw_phase_hi(); } + else { return (STAT_OK); } +} +stat_t pwm_set_p1wph(nvObj_t *nv) { + if (active_toolhead) { active_toolhead->set_ccw_phase_hi(nv->value_flt); } + return (STAT_OK); +} +stat_t pwm_get_p1pof(nvObj_t *nv) { + if (active_toolhead) { return active_toolhead->get_phase_off(); } + else { return (STAT_OK); } +} +stat_t pwm_set_p1pof(nvObj_t *nv) { + if (active_toolhead) { active_toolhead->set_phase_off(nv->value_flt); } + return (STAT_OK); +} /**************************************************************************************** * TEXT MODE SUPPORT @@ -652,8 +382,31 @@ void sp_print_spsm(nvObj_t *nv) { text_print(nv, fmt_spsm);} // TYPE_FLOAT void sp_print_spoe(nvObj_t *nv) { text_print(nv, fmt_spoe);} // TYPE INT void sp_print_spo(nvObj_t *nv) { text_print(nv, fmt_spo);} // TYPE FLOAT +static const char fmt_p1frq[] = "[p1frq] pwm frequency%18.0f Hz\n"; +static const char fmt_p1csl[] = "[p1csl] pwm cw speed lo%16.0f RPM\n"; +static const char fmt_p1csh[] = "[p1csh] pwm cw speed hi%16.0f RPM\n"; +static const char fmt_p1cpl[] = "[p1cpl] pwm cw phase lo%16.3f [0..1]\n"; +static const char fmt_p1cph[] = "[p1cph] pwm cw phase hi%16.3f [0..1]\n"; +static const char fmt_p1wsl[] = "[p1wsl] pwm ccw speed lo%15.0f RPM\n"; +static const char fmt_p1wsh[] = "[p1wsh] pwm ccw speed hi%15.0f RPM\n"; +static const char fmt_p1wpl[] = "[p1wpl] pwm ccw phase lo%15.3f [0..1]\n"; +static const char fmt_p1wph[] = "[p1wph] pwm ccw phase hi%15.3f [0..1]\n"; +static const char fmt_p1pof[] = "[p1pof] pwm phase off%18.3f [0..1]\n"; + +void pwm_print_p1frq(nvObj_t *nv) { text_print(nv, fmt_p1frq);} // all TYPE_FLOAT +void pwm_print_p1csl(nvObj_t *nv) { text_print(nv, fmt_p1csl);} +void pwm_print_p1csh(nvObj_t *nv) { text_print(nv, fmt_p1csh);} +void pwm_print_p1cpl(nvObj_t *nv) { text_print(nv, fmt_p1cpl);} +void pwm_print_p1cph(nvObj_t *nv) { text_print(nv, fmt_p1cph);} +void pwm_print_p1wsl(nvObj_t *nv) { text_print(nv, fmt_p1wsl);} +void pwm_print_p1wsh(nvObj_t *nv) { text_print(nv, fmt_p1wsh);} +void pwm_print_p1wpl(nvObj_t *nv) { text_print(nv, fmt_p1wpl);} +void pwm_print_p1wph(nvObj_t *nv) { text_print(nv, fmt_p1wph);} +void pwm_print_p1pof(nvObj_t *nv) { text_print(nv, fmt_p1pof);} + #endif // __TEXT_MODE +#include "settings.h" constexpr cfgItem_t spindle_config_items_1[] = { // Spindle functions @@ -664,10 +417,26 @@ constexpr cfgItem_t spindle_config_items_1[] = { { "sp","spsm", _fip, 2, sp_print_spsm, sp_get_spsm, sp_set_spsm, nullptr, SPINDLE_SPEED_MAX}, { "sp","spep", _iip, 0, sp_print_spep, sp_get_spep, sp_set_spep, nullptr, SPINDLE_ENABLE_POLARITY }, { "sp","spdp", _iip, 0, sp_print_spdp, sp_get_spdp, sp_set_spdp, nullptr, SPINDLE_DIR_POLARITY }, - { "sp","spoe", _bip, 0, sp_print_spoe, sp_get_spoe, sp_set_spoe, nullptr, SPINDLE_OVERRIDE_ENABLE}, - { "sp","spo", _fip, 3, sp_print_spo, sp_get_spo, sp_set_spo, nullptr, SPINDLE_OVERRIDE_FACTOR}, + { "sp","spoe", _bip, 0, sp_print_spoe, sp_get_spoe, sp_set_spoe, nullptr, 0}, // SPINDLE_OVERRIDE_ENABLE + { "sp","spo", _fip, 3, sp_print_spo, sp_get_spo, sp_set_spo, nullptr, 1.0}, // SPINDLE_OVERRIDE_FACTOR { "sp","spc", _i0, 0, sp_print_spc, sp_get_spc, sp_set_spc, nullptr, 0 }, // spindle state { "sp","sps", _f0, 0, sp_print_sps, sp_get_sps, sp_set_sps, nullptr, 0 }, // spindle speed }; constexpr cfgSubtableFromStaticArray spindle_config_1 {spindle_config_items_1}; const configSubtable * const getSpindleConfig_1() { return &spindle_config_1; } + +constexpr cfgItem_t p1_config_items_1[] = { + // PWM settings + { "p1","p1frq",_fip, 0, pwm_print_p1frq, pwm_get_p1frq, pwm_set_p1frq, nullptr, P1_PWM_FREQUENCY }, + { "p1","p1csl",_fip, 0, pwm_print_p1csl, pwm_get_p1csl, pwm_set_p1csl, nullptr, P1_CW_SPEED_LO }, + { "p1","p1csh",_fip, 0, pwm_print_p1csh, pwm_get_p1csh, pwm_set_p1csh, nullptr, P1_CW_SPEED_HI }, + { "p1","p1cpl",_fip, 3, pwm_print_p1cpl, pwm_get_p1cpl, pwm_set_p1cpl, nullptr, P1_CW_PHASE_LO }, + { "p1","p1cph",_fip, 3, pwm_print_p1cph, pwm_get_p1cph, pwm_set_p1cph, nullptr, P1_CW_PHASE_HI }, + { "p1","p1wsl",_fip, 0, pwm_print_p1wsl, pwm_get_p1wsl, pwm_set_p1wsl, nullptr, P1_CCW_SPEED_LO }, + { "p1","p1wsh",_fip, 0, pwm_print_p1wsh, pwm_get_p1wsh, pwm_set_p1wsh, nullptr, P1_CCW_SPEED_HI }, + { "p1","p1wpl",_fip, 3, pwm_print_p1wpl, pwm_get_p1wpl, pwm_set_p1wpl, nullptr, P1_CCW_PHASE_LO }, + { "p1","p1wph",_fip, 3, pwm_print_p1wph, pwm_get_p1wph, pwm_set_p1wph, nullptr, P1_CCW_PHASE_HI }, + { "p1","p1pof",_fip, 3, pwm_print_p1pof, pwm_get_p1pof, pwm_set_p1pof, nullptr, P1_PWM_PHASE_OFF }, +}; +constexpr cfgSubtableFromStaticArray p1_config_1 {p1_config_items_1}; +const configSubtable * const getP1Config_1() { return &p1_config_1; } diff --git a/g2core/spindle.h b/g2core/spindle.h index 3f706c29..6fa402ab 100644 --- a/g2core/spindle.h +++ b/g2core/spindle.h @@ -28,42 +28,122 @@ #ifndef SPINDLE_H_ONCE #define SPINDLE_H_ONCE -// spControl enum is used for multiple purposes: -// - request a spindle action (OFF, CW, CCW, PAUSE, RESUME) -// - keep current spindle state in spindle.state -// - store as current direction (CW/CCW) in spindle.direction -// - enumerate internal actions such as NOP, REV that are neither states nor controls -enum spControl { // how spindle controls are presented by the Gcode parser +enum spDirection { // how spindle controls are presented by the Gcode parser SPINDLE_OFF = 0, // M5 SPINDLE_CW = 1, // M3 and store CW to spindle.direction SPINDLE_CCW = 2, // M4 and store CCW to spsindle.direction - SPINDLE_PAUSE = 3, // request PAUSE and store PAUSED state to spindle.state - SPINDLE_RESUME = 4, // request RESUME and revert spindle.state to CW, CCW - SPINDLE_NOP, // no operation - SPINDLE_REV // operation to reverse spindle direction }; -#define SPINDLE_ACTION_MAX SPINDLE_RESUME + +class GCodeState_t; + +class ToolHead // TODO: Move to a toolhead file +{ + public: + virtual void init(); + virtual void reset() { + // default to stop + stop(); + } + + virtual stat_t stop() // stop - loses state - called when a job ends or is killed, even if outside a "job" + { + // default to setting the spindle to OFF and speed to 0 + set_direction(SPINDLE_OFF); + set_speed(0); + + return (STAT_OK); + } + + virtual stat_t pause(); // soft-stop the toolhead (usually for a feedhold) - retain all state for resume + virtual stat_t resume(); // resume from the pause - return STAT_EAGAIN if it's not yet ready + virtual bool ready_to_resume() { return true; } // return true if paused and resume would not result in an error + virtual bool busy() { return false; } // return true if motion should continue waiting for this toolhead + + // the result of an S word + // return STAT_EAGAIN if a command (and plan-to-stop) is needed, and STAT_OK if not + virtual stat_t set_speed(float speed) { return (STAT_EAGAIN); } + virtual float get_speed(); + + // the result of an M3/M4/M5 + // return STAT_EAGAIN if a command (and plan-to-stop) is needed, and STAT_OK if not + virtual stat_t set_direction(spDirection direction) { return (STAT_EAGAIN); } + virtual spDirection get_direction(); + + // called from the loader right before a move, with the gcode model to use + virtual void engage(const GCodeState_t &gm); + + virtual bool is_on(); // return if the current direction is anything but OFF, **even if paused** + + // support for legacy interfaces, overriding is optional + + virtual void set_pwm_output(const uint8_t pwm_pin_number) { /* do nothing by default */ }; + virtual void set_enable_output(const uint8_t enable_pin_number) { /* do nothing by default */ }; + virtual void set_direction_output(const uint8_t direction_pin_number) { /* do nothing by default */ }; + + // getters and setters for optional stuff - to support legacy JSON + virtual void set_frequency(float new_frequency) { /* do nothing */ } + virtual float get_frequency() { return 0.0; } + + virtual void set_speed_min(float new_speed_min) { /* do nothing */ } + virtual float get_speed_min() { return 0.0; } + virtual void set_speed_max(float new_speed_max) { /* do nothing */ } + virtual float get_speed_max() { return 0.0; } + virtual void set_speed_change_per_tick(float new_speed_change_per_tick) { /* do nothing */ } + virtual float get_speed_change_per_tick() { return 0.0; } + virtual void set_spinup_delay(float new_spinup_delay) { /* do nothing */ } + virtual float get_spinup_delay() { return 0.0; } + + virtual void set_cw_speed_lo(float new_speed_lo) { /* do nothing */ } + virtual float get_cw_speed_lo() { return 0.0; } + virtual void set_cw_speed_hi(float new_speed_hi) { /* do nothing */ } + virtual float get_cw_speed_hi() { return 0.0; } + virtual void set_cw_phase_lo(float new_phase_lo) { /* do nothing */ } + virtual float get_cw_phase_lo() { return 0.0; } + virtual void set_cw_phase_hi(float new_phase_hi) { /* do nothing */ } + virtual float get_cw_phase_hi() { return 0.0; } + + virtual void set_ccw_speed_lo(float new_speed_lo) { /* do nothing */ } + virtual float get_ccw_speed_lo() { return 0.0; } + virtual void set_ccw_speed_hi(float new_speed_hi) { /* do nothing */ } + virtual float get_ccw_speed_hi() { return 0.0; } + virtual void set_ccw_phase_lo(float new_phase_lo) { /* do nothing */ } + virtual float get_ccw_phase_lo() { return 0.0; } + virtual void set_ccw_phase_hi(float new_phase_hi) { /* do nothing */ } + virtual float get_ccw_phase_hi() { return 0.0; } + + virtual void set_phase_off(float new_phase_off) { /* do nothing */ } + virtual float get_phase_off() { return 0.0; } +}; /* * Global Scope Functions */ -void spindle_init(); -void spindle_reset(); +ToolHead *toolhead_for_tool(uint8_t tool); // return the correct toolhead for this given tool number - MAY retrun nullptr! -stat_t spindle_control_immediate(spControl control); -stat_t spindle_control_sync(spControl control); -stat_t spindle_speed_immediate(float speed); // S parameter -stat_t spindle_speed_sync(float speed); // S parameter +void spindle_init(); // init all known toolheads +void spindle_set_toolhead(ToolHead *toolhead); // set the active toolhead +void spindle_reset(); // reset the current toolhead -bool is_spindle_ready_to_resume(); // if the spindle can resume at this time, return true -bool is_spindle_on_or_paused(); // returns if the stepper is on or paused - IOW would it try to resume from feedhold -bool do_spindle_speed_ramp_from_systick(); // used only in systick call from stepper.cpp +stat_t spindle_stop(); +stat_t spindle_pause(); +stat_t spindle_resume(); +stat_t spindle_set_speed(float speed); // S parameter - returns STAT_EAGAIN if a command should be queued +float spindle_get_speed(); // return current speed - in the same units as the S parameter +stat_t spindle_set_direction(spDirection direction); // M3/M4/M5 - returns STAT_EAGAIN if a command should be queued +spDirection spindle_get_direction(); // return if any fo M3/M4/M5 are active (actual, not gcode model) + +void spindle_engage(const GCodeState_t &gm); // called from the loader right before a move, with the gcode model to use + +bool is_spindle_ready_to_resume(); // if the spindle can resume at this time, return true +bool is_spindle_on_or_paused(); // returns if the spindle is on or paused - IOW would it try to resume from feedhold +bool is_a_toolhead_busy(); // returns true if motion should continue to hold for ANY toolhead stat_t spindle_override_control(const float P_word, const bool P_flag); // M51 void spindle_start_override(const float ramp_time, const float override_factor); void spindle_end_override(const float ramp_time); const configSubtable * const getSpindleConfig_1(); +const configSubtable *const getP1Config_1(); #endif // End of include guard: SPINDLE_H_ONCE diff --git a/g2core/stepper.cpp b/g2core/stepper.cpp index db91a8e0..81853449 100644 --- a/g2core/stepper.cpp +++ b/g2core/stepper.cpp @@ -83,7 +83,7 @@ Motate::SysTickEvent dwell_systick_event{ st_run.dwell_ticks_downcount = 1; // this'll decerement to zero shortly cm->hold_state = FEEDHOLD_MOTION_STOPPED; } - if (do_spindle_speed_ramp_from_systick() && (--st_run.dwell_ticks_downcount == 0)) { + if ((--st_run.dwell_ticks_downcount == 0)) { // do_spindle_speed_ramp_from_systick() && st_run.dwell_ticks_downcount = 0; // in the case of stop==true, this is needed SysTickTimer.unregisterEvent(&dwell_systick_event); _load_move(); // load the next move at the current interrupt level @@ -216,11 +216,13 @@ stat_t stepper_test_assertions() * Busy conditions: * - motors are running * - dwell is running + * - an toolhead is busy in a way that should prevent motion (spinup, etc.) */ bool st_runtime_isbusy() { - return (st_run.dda_ticks_downcount || st_run.dwell_ticks_downcount); // returns false if down count is zero + // note: zero is false, anything else is true + return (st_run.dda_ticks_downcount || st_run.dwell_ticks_downcount || is_a_toolhead_busy()); } /* @@ -489,6 +491,11 @@ static void _load_move() return; } // if (st_pre.buffer_state != PREP_BUFFER_OWNED_BY_LOADER) + // give the toolhead a chance to react to the upcoming move + if (st_pre.bf) { + spindle_engage(st_pre.bf->gm); + } + // handle aline loads first (most common case) if (st_pre.block_type == BLOCK_TYPE_ALINE) { @@ -719,7 +726,7 @@ stat_t st_prep_line(const float start_velocity, const float end_velocity, const steps -= correction_steps; } - // Compute substeb increment. The accumulator must be *exactly* the incoming + // Compute substep increment. The accumulator must be *exactly* the incoming // fractional steps times the substep multiplier or positional drift will occur. // Rounding is performed to eliminate a negative bias in the uint32 conversion // that results in long-term negative drift. (std::abs/round order doesn't matter)