From 2da256bb5b573571af9304c868d2d4aa5f29535b Mon Sep 17 00:00:00 2001 From: Patrick Felixberger Date: Thu, 14 Jun 2018 19:42:02 +0200 Subject: [PATCH] Bugfixes for coolant and spindle --- grbl/CoolantControl.c | 40 ++++++++++++++++++++++++---------------- grbl/GCode.c | 12 ++++-------- grbl/Planner.h | 1 + grbl/Protocol.c | 12 +++++++----- grbl/Report.c | 3 +++ 5 files changed, 39 insertions(+), 29 deletions(-) diff --git a/grbl/CoolantControl.c b/grbl/CoolantControl.c index 1d30a90..d220d43 100644 --- a/grbl/CoolantControl.c +++ b/grbl/CoolantControl.c @@ -90,28 +90,36 @@ void Coolant_SetState(uint8_t mode) return; } - if(mode == COOLANT_DISABLE) { - Coolant_Stop(); - } - else { - if(mode & COOLANT_FLOOD_ENABLE) { + if (mode & COOLANT_FLOOD_ENABLE) { #ifdef INVERT_COOLANT_FLOOD_PIN - GPIO_ResetBits(GPIO_COOL_FLOOD_PORT, GPIO_COOL_FLOOD_PIN); + GPIO_ResetBits(GPIO_COOL_FLOOD_PORT, GPIO_COOL_FLOOD_PIN); #else - GPIO_SetBits(GPIO_COOL_FLOOD_PORT, GPIO_COOL_FLOOD_PIN); + GPIO_SetBits(GPIO_COOL_FLOOD_PORT, GPIO_COOL_FLOOD_PIN); #endif - } + } + else { +#ifdef INVERT_COOLANT_FLOOD_PIN + GPIO_SetBits(GPIO_COOL_FLOOD_PORT, GPIO_COOL_FLOOD_PIN); +#else + GPIO_ResetBits(GPIO_COOL_FLOOD_PORT, GPIO_COOL_FLOOD_PIN); +#endif + } #ifdef ENABLE_M7 - if(mode & COOLANT_MIST_ENABLE) { - #ifdef INVERT_COOLANT_MIST_PIN - GPIO_ResetBits(GPIO_COOL_MIST_PORT, GPIO_COOL_MIST_PIN); - #else - GPIO_SetBits(GPIO_COOL_MIST_PORT, GPIO_COOL_MIST_PIN); - #endif - } + if (mode & COOLANT_MIST_ENABLE) { +#ifdef INVERT_COOLANT_MIST_PIN + GPIO_ResetBits(GPIO_COOL_MIST_PORT, GPIO_COOL_MIST_PIN); +#else + GPIO_SetBits(GPIO_COOL_MIST_PORT, GPIO_COOL_MIST_PIN); +#endif + } else { +#ifdef INVERT_COOLANT_MIST_PIN + GPIO_SetBits(GPIO_COOL_MIST_PORT, GPIO_COOL_MIST_PIN); +#else + GPIO_ResetBits(GPIO_COOL_MIST_PORT, GPIO_COOL_MIST_PIN); +#endif + } #endif - } sys.report_ovr_counter = 0; // Set to report change immediately } diff --git a/grbl/GCode.c b/grbl/GCode.c index 0133783..a1e5258 100644 --- a/grbl/GCode.c +++ b/grbl/GCode.c @@ -373,14 +373,15 @@ uint8_t GC_ExecuteLine(char *line) { #ifdef ENABLE_M7 case 7: - gc_block.modal.coolant = COOLANT_MIST_ENABLE; + gc_block.modal.coolant |= COOLANT_MIST_ENABLE; break; #endif case 8: - gc_block.modal.coolant = COOLANT_FLOOD_ENABLE; + gc_block.modal.coolant |= COOLANT_FLOOD_ENABLE; break; case 9: + // M9: Disable both gc_block.modal.coolant = COOLANT_DISABLE; break; } @@ -1245,12 +1246,7 @@ uint8_t GC_ExecuteLine(char *line) // NOTE: Coolant M-codes are modal. Only one command per line is allowed. But, multiple states // can exist at the same time, while coolant disable clears all states. Coolant_Sync(gc_block.modal.coolant); - if(gc_block.modal.coolant == COOLANT_DISABLE) { - gc_state.modal.coolant = COOLANT_DISABLE; - } - else { - gc_state.modal.coolant |= gc_block.modal.coolant; - } + gc_state.modal.coolant = gc_block.modal.coolant; } pl_data->condition |= gc_state.modal.coolant; // Set condition flag for planner use. diff --git a/grbl/Planner.h b/grbl/Planner.h index 9e411ad..4d01ad4 100644 --- a/grbl/Planner.h +++ b/grbl/Planner.h @@ -40,6 +40,7 @@ #define PL_COND_FLAG_COOLANT_FLOOD BIT(6) #define PL_COND_FLAG_COOLANT_MIST BIT(7) #define PL_COND_MOTION_MASK (PL_COND_FLAG_RAPID_MOTION|PL_COND_FLAG_SYSTEM_MOTION|PL_COND_FLAG_NO_FEED_OVERRIDE) +#define PL_COND_SPINDLE_MASK (PL_COND_FLAG_SPINDLE_CW|PL_COND_FLAG_SPINDLE_CCW) #define PL_COND_ACCESSORY_MASK (PL_COND_FLAG_SPINDLE_CW|PL_COND_FLAG_SPINDLE_CCW|PL_COND_FLAG_COOLANT_FLOOD|PL_COND_FLAG_COOLANT_MIST) diff --git a/grbl/Protocol.c b/grbl/Protocol.c index b171c82..365c6c7 100644 --- a/grbl/Protocol.c +++ b/grbl/Protocol.c @@ -557,8 +557,9 @@ void Protocol_ExecRtSystem(void) last_s_override = max(last_s_override,MIN_SPINDLE_SPEED_OVERRIDE); if(last_s_override != sys.spindle_speed_ovr) { - BIT_TRUE(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_PWM); - sys.spindle_speed_ovr = last_s_override; + // NOTE: Spindle speed overrides during HOLD state are taken care of by suspend function. + if (sys.state == STATE_IDLE) { Spindle_SetState(gc_state.modal.spindle, gc_state.spindle_speed); } + else { BIT_TRUE(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_PWM); } sys.report_ovr_counter = 0; // Set to report change immediately } @@ -577,8 +578,9 @@ void Protocol_ExecRtSystem(void) // NOTE: Since coolant state always performs a planner sync whenever it changes, the current // run state can be determined by checking the parser state. + // NOTE: Coolant overrides only operate during IDLE, CYCLE, HOLD, and JOG states. Ignored otherwise. if(rt_exec & (EXEC_COOLANT_FLOOD_OVR_TOGGLE | EXEC_COOLANT_MIST_OVR_TOGGLE)) { - if((sys.state == STATE_IDLE) || (sys.state & (STATE_CYCLE | STATE_HOLD))) { + if((sys.state == STATE_IDLE) || (sys.state & (STATE_CYCLE | STATE_HOLD | STATE_JOG))) { uint8_t coolant_state = gc_state.modal.coolant; #ifdef ENABLE_M7 if(rt_exec & EXEC_COOLANT_MIST_OVR_TOGGLE) { @@ -656,7 +658,7 @@ static void Protocol_ExecRtSuspend(void) restore_spindle_speed = gc_state.spindle_speed; } else { - restore_condition = block->condition; + restore_condition = (block->condition & PL_COND_SPINDLE_MASK) | Coolant_GetState(); restore_spindle_speed = block->spindle_speed; } #ifdef DISABLE_LASER_DURING_HOLD @@ -810,7 +812,7 @@ static void Protocol_ExecRtSuspend(void) // Block if safety door re-opened during prior restore actions. if(BIT_IS_FALSE(sys.suspend, SUSPEND_RESTART_RETRACT)) { // NOTE: Laser mode will honor this delay. An exhaust system is often controlled by this pin. - Coolant_SetState((restore_condition & (PL_COND_FLAG_COOLANT_FLOOD | PL_COND_FLAG_COOLANT_FLOOD))); + Coolant_SetState((restore_condition & (PL_COND_FLAG_COOLANT_FLOOD | PL_COND_FLAG_COOLANT_MIST))); Delay_sec(SAFETY_DOOR_COOLANT_DELAY, DELAY_MODE_SYS_SUSPEND); } } diff --git a/grbl/Report.c b/grbl/Report.c index c0eb5f4..ff1c1ef 100644 --- a/grbl/Report.c +++ b/grbl/Report.c @@ -528,6 +528,9 @@ void Report_BuildInfo(char *line) #ifndef HOMING_INIT_LOCK Putc('L'); #endif +#ifdef ENABLE_SAFETY_DOOR_INPUT_PIN + Putc('+'); +#endif // NOTE: Compiled values, like override increments/max/min values, may be added at some point later. Putc(',');