diff --git a/grbl/config.h b/grbl/config.h index f48d958..7287eb6 100644 --- a/grbl/config.h +++ b/grbl/config.h @@ -29,7 +29,6 @@ #define config_h #include "grbl.h" // For Arduino IDE compatibility. - // Define CPU pin map and default settings. // NOTE: OEMs can avoid the need to maintain/update the defaults.h and cpu_map.h files and use only // one configuration file by placing their specific defaults and pin map at the bottom of this file. @@ -48,10 +47,10 @@ // g-code programs, maybe selected for interface programs. // NOTE: If changed, manually update help message in report.c. -#define CMD_RESET 0x18 // ctrl-x. +#define CMD_RESET 0x18 // ctrl-x. #define CMD_STATUS_REPORT '?' -#define CMD_CYCLE_START '~' -#define CMD_FEED_HOLD '!' +#define CMD_CYCLE_START '~' +#define CMD_FEED_HOLD '!' // NOTE: All override realtime commands must be in the extended ASCII character set, starting // at character value 128 (0x80) and up to 255 (0xFF). If the normal set of realtime commands, @@ -61,26 +60,26 @@ // #define CMD_STATUS_REPORT 0x81 // #define CMD_CYCLE_START 0x82 // #define CMD_FEED_HOLD 0x83 -#define CMD_SAFETY_DOOR 0x84 -#define CMD_JOG_CANCEL 0x85 -#define CMD_DEBUG_REPORT 0x86 // Only when DEBUG enabled, sends debug report in '{}' braces. -#define CMD_FEED_OVR_RESET 0x90 // Restores feed override value to 100%. -#define CMD_FEED_OVR_COARSE_PLUS 0x91 +#define CMD_SAFETY_DOOR 0x84 +#define CMD_JOG_CANCEL 0x85 +#define CMD_DEBUG_REPORT 0x86 // Only when DEBUG enabled, sends debug report in '{}' braces. +#define CMD_FEED_OVR_RESET 0x90 // Restores feed override value to 100%. +#define CMD_FEED_OVR_COARSE_PLUS 0x91 #define CMD_FEED_OVR_COARSE_MINUS 0x92 -#define CMD_FEED_OVR_FINE_PLUS 0x93 -#define CMD_FEED_OVR_FINE_MINUS 0x94 -#define CMD_RAPID_OVR_RESET 0x95 // Restores rapid override value to 100%. -#define CMD_RAPID_OVR_MEDIUM 0x96 -#define CMD_RAPID_OVR_LOW 0x97 +#define CMD_FEED_OVR_FINE_PLUS 0x93 +#define CMD_FEED_OVR_FINE_MINUS 0x94 +#define CMD_RAPID_OVR_RESET 0x95 // Restores rapid override value to 100%. +#define CMD_RAPID_OVR_MEDIUM 0x96 +#define CMD_RAPID_OVR_LOW 0x97 // #define CMD_RAPID_OVR_EXTRA_LOW 0x98 // *NOT SUPPORTED* -#define CMD_SPINDLE_OVR_RESET 0x99 // Restores spindle override value to 100%. -#define CMD_SPINDLE_OVR_COARSE_PLUS 0x9A +#define CMD_SPINDLE_OVR_RESET 0x99 // Restores spindle override value to 100%. +#define CMD_SPINDLE_OVR_COARSE_PLUS 0x9A #define CMD_SPINDLE_OVR_COARSE_MINUS 0x9B -#define CMD_SPINDLE_OVR_FINE_PLUS 0x9C -#define CMD_SPINDLE_OVR_FINE_MINUS 0x9D -#define CMD_SPINDLE_OVR_STOP 0x9E +#define CMD_SPINDLE_OVR_FINE_PLUS 0x9C +#define CMD_SPINDLE_OVR_FINE_MINUS 0x9D +#define CMD_SPINDLE_OVR_STOP 0x9E #define CMD_COOLANT_FLOOD_OVR_TOGGLE 0xA0 -#define CMD_COOLANT_MIST_OVR_TOGGLE 0xA1 +#define CMD_COOLANT_MIST_OVR_TOGGLE 0xA1 // If homing is enabled, homing init lock sets Grbl into an alarm state upon power up. This forces // the user to perform the homing cycle (or override the locks) before doing anything else. This is @@ -102,12 +101,12 @@ // on separate pin, but homed in one cycle. Also, it should be noted that the function of hard limits // will not be affected by pin sharing. // NOTE: Defaults are set for a traditional 3-axis CNC machine. Z-axis first to clear, followed by X & Y. -#define HOMING_CYCLE_0 (1< RPM_MAX will be limited to RPM_MAX. -#define RPM_MIN 202.5 // Min RPM of model. $31 < RPM_MIN will be limited to RPM_MIN. -#define RPM_POINT12 6145.4 // Used N_PIECES >=2. Junction point between lines 1 and 2. -#define RPM_POINT23 9627.8 // Used N_PIECES >=3. Junction point between lines 2 and 3. -#define RPM_POINT34 10813.9 // Used N_PIECES = 4. Junction point between lines 3 and 4. -#define RPM_LINE_A1 3.197101e-03 // Used N_PIECES >=1. A and B constants of line 1. -#define RPM_LINE_B1 -3.526076e-1 -#define RPM_LINE_A2 1.722950e-2 // Used N_PIECES >=2. A and B constants of line 2. -#define RPM_LINE_B2 8.588176e+01 -#define RPM_LINE_A3 5.901518e-02 // Used N_PIECES >=3. A and B constants of line 3. -#define RPM_LINE_B3 4.881851e+02 -#define RPM_LINE_A4 1.203413e-01 // Used N_PIECES = 4. A and B constants of line 4. -#define RPM_LINE_B4 1.151360e+03 +#define N_PIECES 4 // Integer (1-4). Number of piecewise lines used in script solution. +#define RPM_MAX 11686.4 // Max RPM of model. $30 > RPM_MAX will be limited to RPM_MAX. +#define RPM_MIN 202.5 // Min RPM of model. $31 < RPM_MIN will be limited to RPM_MIN. +#define RPM_POINT12 6145.4 // Used N_PIECES >=2. Junction point between lines 1 and 2. +#define RPM_POINT23 9627.8 // Used N_PIECES >=3. Junction point between lines 2 and 3. +#define RPM_POINT34 10813.9 // Used N_PIECES = 4. Junction point between lines 3 and 4. +#define RPM_LINE_A1 3.197101e-03 // Used N_PIECES >=1. A and B constants of line 1. +#define RPM_LINE_B1 -3.526076e-1 +#define RPM_LINE_A2 1.722950e-2 // Used N_PIECES >=2. A and B constants of line 2. +#define RPM_LINE_B2 8.588176e+01 +#define RPM_LINE_A3 5.901518e-02 // Used N_PIECES >=3. A and B constants of line 3. +#define RPM_LINE_B3 4.881851e+02 +#define RPM_LINE_A4 1.203413e-01 // Used N_PIECES = 4. A and B constants of line 4. +#define RPM_LINE_B4 1.151360e+03 -/* --------------------------------------------------------------------------------------- - This optional dual axis feature is primarily for the homing cycle to locate two sides of +/* --------------------------------------------------------------------------------------- + This optional dual axis feature is primarily for the homing cycle to locate two sides of a dual-motor gantry independently, i.e. self-squaring. This requires an additional limit switch for the cloned motor. To self square, both limit switches on the cloned axis must - be physically positioned to trigger when the gantry is square. Highly recommend keeping + be physically positioned to trigger when the gantry is square. Highly recommend keeping the motors always enabled to ensure the gantry stays square with the $1=255 setting. - For Grbl on the Arduino Uno, the cloned axis limit switch must to be shared with and + For Grbl on the Arduino Uno, the cloned axis limit switch must to be shared with and wired with z-axis limit pin due to the lack of available pins. The homing cycle must home the z-axis and cloned axis in different cycles, which is already the default config. The dual axis feature works by cloning an axis step output onto another pair of step - and direction pins. The step pulse and direction of the cloned motor can be set + and direction pins. The step pulse and direction of the cloned motor can be set independently of the main axis motor. However to save precious flash and memory, this - dual axis feature must share the same settings (step/mm, max speed, acceleration) as the + dual axis feature must share the same settings (step/mm, max speed, acceleration) as the parent motor. This is NOT a feature for an independent fourth axis. Only a motor clone. WARNING: Make sure to test the directions of your dual axis motors! They must be setup to move the same direction BEFORE running your first homing cycle or any long motion! - Motors moving in opposite directions can cause serious damage to your machine! Use this + Motors moving in opposite directions can cause serious damage to your machine! Use this dual axis feature at your own risk. */ // NOTE: This feature requires approximately 400 bytes of flash. Certain configurations can @@ -641,40 +642,39 @@ // #define ENABLE_DUAL_AXIS // Default disabled. Uncomment to enable. // Select the one axis to mirror another motor. Only X and Y axis is supported at this time. -#define DUAL_AXIS_SELECT X_AXIS // Must be either X_AXIS or Y_AXIS +#define DUAL_AXIS_SELECT X_AXIS // Must be either X_AXIS or Y_AXIS // To prevent the homing cycle from racking the dual axis, when one limit triggers before the -// other due to switch failure or noise, the homing cycle will automatically abort if the second -// motor's limit switch does not trigger within the three distance parameters defined below. +// other due to switch failure or noise, the homing cycle will automatically abort if the second +// motor's limit switch does not trigger within the three distance parameters defined below. // Axis length percent will automatically compute a fail distance as a percentage of the max -// travel of the other non-dual axis, i.e. if dual axis select is X_AXIS at 5.0%, then the fail -// distance will be computed as 5.0% of y-axis max travel. Fail distance max and min are the +// travel of the other non-dual axis, i.e. if dual axis select is X_AXIS at 5.0%, then the fail +// distance will be computed as 5.0% of y-axis max travel. Fail distance max and min are the // limits of how far or little a valid fail distance is. -#define DUAL_AXIS_HOMING_FAIL_AXIS_LENGTH_PERCENT 5.0 // Float (percent) -#define DUAL_AXIS_HOMING_FAIL_DISTANCE_MAX 25.0 // Float (mm) -#define DUAL_AXIS_HOMING_FAIL_DISTANCE_MIN 2.5 // Float (mm) +#define DUAL_AXIS_HOMING_FAIL_AXIS_LENGTH_PERCENT 5.0 // Float (percent) +#define DUAL_AXIS_HOMING_FAIL_DISTANCE_MAX 25.0 // Float (mm) +#define DUAL_AXIS_HOMING_FAIL_DISTANCE_MIN 2.5 // Float (mm) // Dual axis pin configuration currently supports two shields. Uncomment the shield you want, // and comment out the other one(s). // NOTE: Protoneer CNC Shield v3.51 has A.STP and A.DIR wired to pins A4 and A3 respectively. // The variable spindle (i.e. laser mode) build option works and may be enabled or disabled. // Coolant pin A3 is moved to D13, replacing spindle direction. -#define DUAL_AXIS_CONFIG_PROTONEER_V3_51 // Uncomment to select. Comment other configs. +#define DUAL_AXIS_CONFIG_PROTONEER_V3_51 // Uncomment to select. Comment other configs. -// NOTE: Arduino CNC Shield Clone (Originally Protoneer v3.0) has A.STP and A.DIR wired to +// NOTE: Arduino CNC Shield Clone (Originally Protoneer v3.0) has A.STP and A.DIR wired to // D12 and D13, respectively. With the limit pins and stepper enable pin on this same port, // the spindle enable pin had to be moved and spindle direction pin deleted. The spindle // enable pin now resides on A3, replacing coolant enable. Coolant enable is bumped over to -// pin A4. Spindle enable is used far more and this pin setup helps facilitate users to -// integrate this feature without arguably too much work. +// pin A4. Spindle enable is used far more and this pin setup helps facilitate users to +// integrate this feature without arguably too much work. // Variable spindle (i.e. laser mode) does NOT work with this shield as configured. While // variable spindle technically can work with this shield, it requires too many changes for // most user setups to accomodate. It would best be implemented by sharing all limit switches -// on pins D9/D10 (as [X1,Z]/[X2,Y] or [X,Y2]/[Y1,Z]), home each axis independently, and +// on pins D9/D10 (as [X1,Z]/[X2,Y] or [X,Y2]/[Y1,Z]), home each axis independently, and // updating lots of code to ensure everything is running correctly. // #define DUAL_AXIS_CONFIG_CNC_SHIELD_CLONE // Uncomment to select. Comment other configs. - /* --------------------------------------------------------------------------------------- OEM Single File Configuration Option @@ -688,5 +688,4 @@ // Paste default settings definitions here. - #endif diff --git a/grbl/coolant_control.c b/grbl/coolant_control.c index 1eebfc0..deeab7f 100644 --- a/grbl/coolant_control.c +++ b/grbl/coolant_control.c @@ -20,107 +20,101 @@ #include "grbl.h" - -void coolant_init() -{ - COOLANT_FLOOD_DDR |= (1 << COOLANT_FLOOD_BIT); // Configure as output pin - #ifdef ENABLE_M7 +void coolant_init() { + COOLANT_FLOOD_DDR |= (1 << COOLANT_FLOOD_BIT); // Configure as output pin +#ifdef ENABLE_M7 COOLANT_MIST_DDR |= (1 << COOLANT_MIST_BIT); - #endif - coolant_stop(); +#endif + coolant_stop(); } - // Returns current coolant output state. Overrides may alter it from programmed state. -uint8_t coolant_get_state() -{ - uint8_t cl_state = COOLANT_STATE_DISABLE; - #ifdef INVERT_COOLANT_FLOOD_PIN - if (bit_isfalse(COOLANT_FLOOD_PORT,(1 << COOLANT_FLOOD_BIT))) { - #else - if (bit_istrue(COOLANT_FLOOD_PORT,(1 << COOLANT_FLOOD_BIT))) { - #endif - cl_state |= COOLANT_STATE_FLOOD; - } - #ifdef ENABLE_M7 - #ifdef INVERT_COOLANT_MIST_PIN - if (bit_isfalse(COOLANT_MIST_PORT,(1 << COOLANT_MIST_BIT))) { - #else - if (bit_istrue(COOLANT_MIST_PORT,(1 << COOLANT_MIST_BIT))) { - #endif - cl_state |= COOLANT_STATE_MIST; +uint8_t coolant_get_state() { + uint8_t cl_state = COOLANT_STATE_DISABLE; +#ifdef INVERT_COOLANT_FLOOD_PIN + if (bit_isfalse(COOLANT_FLOOD_PORT, (1 << COOLANT_FLOOD_BIT))) { +#else + if (bit_istrue(COOLANT_FLOOD_PORT, (1 << COOLANT_FLOOD_BIT))) { +#endif + cl_state |= COOLANT_STATE_FLOOD; } - #endif - return(cl_state); +#ifdef ENABLE_M7 +#ifdef INVERT_COOLANT_MIST_PIN + if (bit_isfalse(COOLANT_MIST_PORT, (1 << COOLANT_MIST_BIT))) { +#else + if (bit_istrue(COOLANT_MIST_PORT, (1 << COOLANT_MIST_BIT))) { +#endif + cl_state |= COOLANT_STATE_MIST; + } +#endif + return (cl_state); } - // Directly called by coolant_init(), coolant_set_state(), and mc_reset(), which can be at // an interrupt-level. No report flag set, but only called by routines that don't need it. -void coolant_stop() -{ - #ifdef INVERT_COOLANT_FLOOD_PIN +void coolant_stop() { +#ifdef INVERT_COOLANT_FLOOD_PIN COOLANT_FLOOD_PORT |= (1 << COOLANT_FLOOD_BIT); - #else +#else COOLANT_FLOOD_PORT &= ~(1 << COOLANT_FLOOD_BIT); - #endif - #ifdef ENABLE_M7 - #ifdef INVERT_COOLANT_MIST_PIN - COOLANT_MIST_PORT |= (1 << COOLANT_MIST_BIT); - #else - COOLANT_MIST_PORT &= ~(1 << COOLANT_MIST_BIT); - #endif - #endif +#endif +#ifdef ENABLE_M7 +#ifdef INVERT_COOLANT_MIST_PIN + COOLANT_MIST_PORT |= (1 << COOLANT_MIST_BIT); +#else + COOLANT_MIST_PORT &= ~(1 << COOLANT_MIST_BIT); +#endif +#endif } - -// Main program only. Immediately sets flood coolant running state and also mist coolant, +// Main program only. Immediately sets flood coolant running state and also mist coolant, // if enabled. Also sets a flag to report an update to a coolant state. // Called by coolant toggle override, parking restore, parking retract, sleep mode, g-code // parser program end, and g-code parser coolant_sync(). -void coolant_set_state(uint8_t mode) -{ - if (sys.abort) { return; } // Block during abort. - - if (mode & COOLANT_FLOOD_ENABLE) { - #ifdef INVERT_COOLANT_FLOOD_PIN - COOLANT_FLOOD_PORT &= ~(1 << COOLANT_FLOOD_BIT); - #else - COOLANT_FLOOD_PORT |= (1 << COOLANT_FLOOD_BIT); - #endif - } else { - #ifdef INVERT_COOLANT_FLOOD_PIN - COOLANT_FLOOD_PORT |= (1 << COOLANT_FLOOD_BIT); - #else - COOLANT_FLOOD_PORT &= ~(1 << COOLANT_FLOOD_BIT); - #endif - } - - #ifdef ENABLE_M7 - if (mode & COOLANT_MIST_ENABLE) { - #ifdef INVERT_COOLANT_MIST_PIN - COOLANT_MIST_PORT &= ~(1 << COOLANT_MIST_BIT); - #else - COOLANT_MIST_PORT |= (1 << COOLANT_MIST_BIT); - #endif - } else { - #ifdef INVERT_COOLANT_MIST_PIN - COOLANT_MIST_PORT |= (1 << COOLANT_MIST_BIT); - #else - COOLANT_MIST_PORT &= ~(1 << COOLANT_MIST_BIT); - #endif - } - #endif - - sys.report_ovr_counter = 0; // Set to report change immediately +void coolant_set_state(uint8_t mode) { + if (sys.abort) { + return; + } // Block during abort. + + if (mode & COOLANT_FLOOD_ENABLE) { +#ifdef INVERT_COOLANT_FLOOD_PIN + COOLANT_FLOOD_PORT &= ~(1 << COOLANT_FLOOD_BIT); +#else + COOLANT_FLOOD_PORT |= (1 << COOLANT_FLOOD_BIT); +#endif + } else { +#ifdef INVERT_COOLANT_FLOOD_PIN + COOLANT_FLOOD_PORT |= (1 << COOLANT_FLOOD_BIT); +#else + COOLANT_FLOOD_PORT &= ~(1 << COOLANT_FLOOD_BIT); +#endif + } + +#ifdef ENABLE_M7 + if (mode & COOLANT_MIST_ENABLE) { +#ifdef INVERT_COOLANT_MIST_PIN + COOLANT_MIST_PORT &= ~(1 << COOLANT_MIST_BIT); +#else + COOLANT_MIST_PORT |= (1 << COOLANT_MIST_BIT); +#endif + } else { +#ifdef INVERT_COOLANT_MIST_PIN + COOLANT_MIST_PORT |= (1 << COOLANT_MIST_BIT); +#else + COOLANT_MIST_PORT &= ~(1 << COOLANT_MIST_BIT); +#endif + } +#endif + + sys.report_ovr_counter = 0; // Set to report change immediately } - -// G-code parser entry-point for setting coolant state. Forces a planner buffer sync and bails +// G-code parser entry-point for setting coolant state. Forces a planner buffer sync and bails // if an abort or check-mode is active. -void coolant_sync(uint8_t mode) -{ - if (sys.state == STATE_CHECK_MODE) { return; } - protocol_buffer_synchronize(); // Ensure coolant turns on when specified in program. - coolant_set_state(mode); +void coolant_sync(uint8_t mode) { + if (sys.state == STATE_CHECK_MODE) { + return; + } + protocol_buffer_synchronize(); // Ensure coolant turns on when specified in program. + coolant_set_state(mode); } diff --git a/grbl/coolant_control.h b/grbl/coolant_control.h index 49b85f0..d966d05 100644 --- a/grbl/coolant_control.h +++ b/grbl/coolant_control.h @@ -21,13 +21,12 @@ #ifndef coolant_control_h #define coolant_control_h -#define COOLANT_NO_SYNC false -#define COOLANT_FORCE_SYNC true - -#define COOLANT_STATE_DISABLE 0 // Must be zero -#define COOLANT_STATE_FLOOD PL_COND_FLAG_COOLANT_FLOOD -#define COOLANT_STATE_MIST PL_COND_FLAG_COOLANT_MIST +#define COOLANT_NO_SYNC false +#define COOLANT_FORCE_SYNC true +#define COOLANT_STATE_DISABLE 0 // Must be zero +#define COOLANT_STATE_FLOOD PL_COND_FLAG_COOLANT_FLOOD +#define COOLANT_STATE_MIST PL_COND_FLAG_COOLANT_MIST // Initializes coolant control pins. void coolant_init(); diff --git a/grbl/cpu_map.h b/grbl/cpu_map.h index 9f43a3f..05857c8 100644 --- a/grbl/cpu_map.h +++ b/grbl/cpu_map.h @@ -22,230 +22,231 @@ processor types or alternative pin layouts. This version of Grbl officially supports only the Arduino Mega328p. */ - #ifndef cpu_map_h #define cpu_map_h - #ifdef CPU_MAP_ATMEGA328P // (Arduino Uno) Officially supported by Grbl. - // Define serial port pins and interrupt vectors. - #define SERIAL_RX USART_RX_vect - #define SERIAL_UDRE USART_UDRE_vect +// Define serial port pins and interrupt vectors. +#define SERIAL_RX USART_RX_vect +#define SERIAL_UDRE USART_UDRE_vect - // Define step pulse output pins. NOTE: All step bit pins must be on the same port. - #define STEP_DDR DDRD - #define STEP_PORT PORTD - #define X_STEP_BIT 2 // Uno Digital Pin 2 - #define Y_STEP_BIT 3 // Uno Digital Pin 3 - #define Z_STEP_BIT 4 // Uno Digital Pin 4 - #define STEP_MASK ((1< 62.5kHz - // #define SPINDLE_TCCRB_INIT_MASK (1< 7.8kHz (Used in v0.9) - // #define SPINDLE_TCCRB_INIT_MASK ((1< 1.96kHz - #define SPINDLE_TCCRB_INIT_MASK (1< 0.98kHz (J-tech laser) +// Prescaled, 8-bit Fast PWM mode. +#define SPINDLE_TCCRA_INIT_MASK ((1 << WGM20) | (1 << WGM21)) // Configures fast PWM mode. +// #define SPINDLE_TCCRB_INIT_MASK (1< 62.5kHz +// #define SPINDLE_TCCRB_INIT_MASK (1< 7.8kHz (Used in v0.9) +// #define SPINDLE_TCCRB_INIT_MASK ((1< 1.96kHz +#define SPINDLE_TCCRB_INIT_MASK (1 << CS22) // 1/64 prescaler -> 0.98kHz (J-tech laser) - // NOTE: On the 328p, these must be the same as the SPINDLE_ENABLE settings. - #define SPINDLE_PWM_DDR DDRB - #define SPINDLE_PWM_PORT PORTB - #define SPINDLE_PWM_BIT 3 // Uno Digital Pin 11 - - #else +// NOTE: On the 328p, these must be the same as the SPINDLE_ENABLE settings. +#define SPINDLE_PWM_DDR DDRB +#define SPINDLE_PWM_PORT PORTB +#define SPINDLE_PWM_BIT 3 // Uno Digital Pin 11 - // Dual axis feature requires an independent step pulse pin to operate. The independent direction pin is not - // absolutely necessary but facilitates easy direction inverting with a Grbl $$ setting. These pins replace - // the spindle direction and optional coolant mist pins. +#else - #ifdef DUAL_AXIS_CONFIG_PROTONEER_V3_51 - // NOTE: Step pulse and direction pins may be on any port and output pin. - #define STEP_DDR_DUAL DDRC - #define STEP_PORT_DUAL PORTC - #define DUAL_STEP_BIT 4 // Uno Analog Pin 4 - #define STEP_MASK_DUAL ((1< 62.5kHz - // #define SPINDLE_TCCRB_INIT_MASK (1< 7.8kHz (Used in v0.9) - // #define SPINDLE_TCCRB_INIT_MASK ((1< 1.96kHz - #define SPINDLE_TCCRB_INIT_MASK (1< 0.98kHz (J-tech laser) +// Variable spindle configuration below. Do not change unless you know what you are doing. +// NOTE: Only used when variable spindle is enabled. +#define SPINDLE_PWM_MAX_VALUE 255 // Don't change. 328p fast PWM mode fixes top value as 255. +#ifndef SPINDLE_PWM_MIN_VALUE +#define SPINDLE_PWM_MIN_VALUE 1 // Must be greater than zero. +#endif +#define SPINDLE_PWM_OFF_VALUE 0 +#define SPINDLE_PWM_RANGE (SPINDLE_PWM_MAX_VALUE - SPINDLE_PWM_MIN_VALUE) +#define SPINDLE_TCCRA_REGISTER TCCR2A +#define SPINDLE_TCCRB_REGISTER TCCR2B +#define SPINDLE_OCR_REGISTER OCR2A +#define SPINDLE_COMB_BIT COM2A1 - // NOTE: On the 328p, these must be the same as the SPINDLE_ENABLE settings. - #define SPINDLE_PWM_DDR DDRB - #define SPINDLE_PWM_PORT PORTB - #define SPINDLE_PWM_BIT 3 // Uno Digital Pin 11 - #endif +// Prescaled, 8-bit Fast PWM mode. +#define SPINDLE_TCCRA_INIT_MASK ((1 << WGM20) | (1 << WGM21)) // Configures fast PWM mode. +// #define SPINDLE_TCCRB_INIT_MASK (1< 62.5kHz +// #define SPINDLE_TCCRB_INIT_MASK (1< 7.8kHz (Used in v0.9) +// #define SPINDLE_TCCRB_INIT_MASK ((1< 1.96kHz +#define SPINDLE_TCCRB_INIT_MASK (1 << CS22) // 1/64 prescaler -> 0.98kHz (J-tech laser) - // NOTE: Variable spindle not supported with this shield. - #ifdef DUAL_AXIS_CONFIG_CNC_SHIELD_CLONE - // NOTE: Step pulse and direction pins may be on any port and output pin. - #define STEP_DDR_DUAL DDRB - #define STEP_PORT_DUAL PORTB - #define DUAL_STEP_BIT 4 // Uno Digital Pin 12 - #define STEP_MASK_DUAL ((1< + * + * Atmel Corporation + * + * \li File: eeprom.c + * \li Compiler: IAR EWAAVR 3.10c + * \li Support mail: avr@atmel.com + * + * \li Supported devices: All devices with split EEPROM erase/write + * capabilities can be used. + * The example is written for ATmega48. + * + * \li AppNote: AVR103 - Using the EEPROM Programming Modes. + * + * \li Description: Example on how to use the split EEPROM erase/write + * capabilities in e.g. ATmega48. All EEPROM + * programming modes are tested, i.e. Erase+Write, + * Erase-only and Write-only. + * + * $Revision: 1.6 $ + * $Date: Friday, February 11, 2005 07:16:44 UTC $ + ****************************************************************************/ #include +#include /* These EEPROM bits have different names on different devices. */ #ifndef EEPE - #define EEPE EEWE //!< EEPROM program/write enable. - #define EEMPE EEMWE //!< EEPROM master program/write enable. +#define EEPE EEWE //!< EEPROM program/write enable. +#define EEMPE EEMWE //!< EEPROM master program/write enable. #endif /* These two are unfortunately not defined in the device include files. */ @@ -46,12 +46,12 @@ * \param addr EEPROM address to read from. * \return The byte read from the EEPROM address. */ -unsigned char eeprom_get_char( unsigned int addr ) -{ - do {} while( EECR & (1< 0; size--) { - checksum = ((checksum << 1) != 0) || (checksum >> 7); - checksum += *source; - eeprom_put_char(destination++, *(source++)); - } - eeprom_put_char(destination, checksum); + unsigned char checksum = 0; + for (; size > 0; size--) { + checksum = ((checksum << 1) != 0) || (checksum >> 7); + checksum += *source; + eeprom_put_char(destination++, *(source++)); + } + eeprom_put_char(destination, checksum); } int memcpy_from_eeprom_with_checksum(char *destination, unsigned int source, unsigned int size) { - unsigned char data, checksum = 0; - for(; size > 0; size--) { - data = eeprom_get_char(source++); - checksum = ((checksum << 1) != 0) || (checksum >> 7); - checksum += data; - *(destination++) = data; - } - return(checksum == eeprom_get_char(source)); + unsigned char data, checksum = 0; + for (; size > 0; size--) { + data = eeprom_get_char(source++); + checksum = ((checksum << 1) != 0) || (checksum >> 7); + checksum += data; + *(destination++) = data; + } + return (checksum == eeprom_get_char(source)); } // end of file diff --git a/grbl/eeprom.h b/grbl/eeprom.h index c9718a2..7142194 100644 --- a/grbl/eeprom.h +++ b/grbl/eeprom.h @@ -22,8 +22,8 @@ #define eeprom_h unsigned char eeprom_get_char(unsigned int addr); -void eeprom_put_char(unsigned int addr, unsigned char new_value); -void memcpy_to_eeprom_with_checksum(unsigned int destination, char *source, unsigned int size); -int memcpy_from_eeprom_with_checksum(char *destination, unsigned int source, unsigned int size); +void eeprom_put_char(unsigned int addr, unsigned char new_value); +void memcpy_to_eeprom_with_checksum(unsigned int destination, char *source, unsigned int size); +int memcpy_from_eeprom_with_checksum(char *destination, unsigned int source, unsigned int size); #endif diff --git a/grbl/gcode.c b/grbl/gcode.c index 57e31e5..d90f9f5 100644 --- a/grbl/gcode.c +++ b/grbl/gcode.c @@ -27,1112 +27,1331 @@ #define MAX_LINE_NUMBER 10000000 #define MAX_TOOL_NUMBER 255 // Limited by max unsigned 8-bit value -#define AXIS_COMMAND_NONE 0 -#define AXIS_COMMAND_NON_MODAL 1 -#define AXIS_COMMAND_MOTION_MODE 2 +#define AXIS_COMMAND_NONE 0 +#define AXIS_COMMAND_NON_MODAL 1 +#define AXIS_COMMAND_MOTION_MODE 2 #define AXIS_COMMAND_TOOL_LENGTH_OFFSET 3 // *Undefined but required // Declare gc extern struct parser_state_t gc_state; parser_block_t gc_block; -#define FAIL(status) return(status); +#define FAIL(status) return (status); +void gc_init() { + memset(&gc_state, 0, sizeof(parser_state_t)); -void gc_init() -{ - memset(&gc_state, 0, sizeof(parser_state_t)); - - // Load default G54 coordinate system. - if (!(settings_read_coord_data(gc_state.modal.coord_select,gc_state.coord_system))) { - report_status_message(STATUS_SETTING_READ_FAIL); - } + // Load default G54 coordinate system. + if (!(settings_read_coord_data(gc_state.modal.coord_select, gc_state.coord_system))) { + report_status_message(STATUS_SETTING_READ_FAIL); + } } - // Sets g-code parser position in mm. Input in steps. Called by the system abort and hard // limit pull-off routines. -void gc_sync_position() -{ - system_convert_array_steps_to_mpos(gc_state.position,sys_position); +void gc_sync_position() { + system_convert_array_steps_to_mpos(gc_state.position, sys_position); } - // Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase // characters and signed floating point values (no whitespace). Comments and block delete // characters have been removed. In this function, all units and positions are converted and // exported to grbl's internal functions in terms of (mm, mm/min) and absolute machine // coordinates, respectively. -uint8_t gc_execute_line(char *line) -{ - /* ------------------------------------------------------------------------------------- - STEP 1: Initialize parser block struct and copy current g-code state modes. The parser - updates these modes and commands as the block line is parser and will only be used and - executed after successful error-checking. The parser block struct also contains a block - values struct, word tracking variables, and a non-modal commands tracker for the new - block. This struct contains all of the necessary information to execute the block. */ +uint8_t gc_execute_line(char *line) { + /* ------------------------------------------------------------------------------------- + STEP 1: Initialize parser block struct and copy current g-code state modes. The parser + updates these modes and commands as the block line is parser and will only be used and + executed after successful error-checking. The parser block struct also contains a block + values struct, word tracking variables, and a non-modal commands tracker for the new + block. This struct contains all of the necessary information to execute the block. */ - memset(&gc_block, 0, sizeof(parser_block_t)); // Initialize the parser block struct. - memcpy(&gc_block.modal,&gc_state.modal,sizeof(gc_modal_t)); // Copy current modes + memset(&gc_block, 0, sizeof(parser_block_t)); // Initialize the parser block struct. + memcpy(&gc_block.modal, &gc_state.modal, sizeof(gc_modal_t)); // Copy current modes - uint8_t axis_command = AXIS_COMMAND_NONE; - uint8_t axis_0, axis_1, axis_linear; - uint8_t coord_select = 0; // Tracks G10 P coordinate selection for execution + uint8_t axis_command = AXIS_COMMAND_NONE; + uint8_t axis_0, axis_1, axis_linear; + uint8_t coord_select = 0; // Tracks G10 P coordinate selection for execution - // Initialize bitflag tracking variables for axis indices compatible operations. - uint8_t axis_words = 0; // XYZ tracking - uint8_t ijk_words = 0; // IJK tracking + // Initialize bitflag tracking variables for axis indices compatible operations. + uint8_t axis_words = 0; // XYZ tracking + uint8_t ijk_words = 0; // IJK tracking - // Initialize command and value words and parser flags variables. - uint16_t command_words = 0; // Tracks G and M command words. Also used for modal group violations. - uint16_t value_words = 0; // Tracks value words. - uint8_t gc_parser_flags = GC_PARSER_NONE; + // Initialize command and value words and parser flags variables. + uint16_t command_words = 0; // Tracks G and M command words. Also used for modal group violations. + uint16_t value_words = 0; // Tracks value words. + uint8_t gc_parser_flags = GC_PARSER_NONE; - // Determine if the line is a jogging motion or a normal g-code block. - if (line[0] == '$') { // NOTE: `$J=` already parsed when passed to this function. - // Set G1 and G94 enforced modes to ensure accurate error checks. - gc_parser_flags |= GC_PARSER_JOG_MOTION; - gc_block.modal.motion = MOTION_MODE_LINEAR; - gc_block.modal.feed_rate = FEED_RATE_MODE_UNITS_PER_MIN; - #ifdef USE_LINE_NUMBERS - gc_block.values.n = JOG_LINE_NUMBER; // Initialize default line number reported during jog. - #endif - } + // Determine if the line is a jogging motion or a normal g-code block. + if (line[0] == '$') { // NOTE: `$J=` already parsed when passed to this function. + // Set G1 and G94 enforced modes to ensure accurate error checks. + gc_parser_flags |= GC_PARSER_JOG_MOTION; + gc_block.modal.motion = MOTION_MODE_LINEAR; + gc_block.modal.feed_rate = FEED_RATE_MODE_UNITS_PER_MIN; +#ifdef USE_LINE_NUMBERS + gc_block.values.n = JOG_LINE_NUMBER; // Initialize default line number reported during jog. +#endif + } - /* ------------------------------------------------------------------------------------- - STEP 2: Import all g-code words in the block line. A g-code word is a letter followed by - a number, which can either be a 'G'/'M' command or sets/assigns a command value. Also, - perform initial error-checks for command word modal group violations, for any repeated - words, and for negative values set for the value words F, N, P, T, and S. */ + /* ------------------------------------------------------------------------------------- + STEP 2: Import all g-code words in the block line. A g-code word is a letter followed by + a number, which can either be a 'G'/'M' command or sets/assigns a command value. Also, + perform initial error-checks for command word modal group violations, for any repeated + words, and for negative values set for the value words F, N, P, T, and S. */ - uint8_t word_bit; // Bit-value for assigning tracking variables - uint8_t char_counter; - char letter; - float value; - uint8_t int_value = 0; - uint16_t mantissa = 0; - if (gc_parser_flags & GC_PARSER_JOG_MOTION) { char_counter = 3; } // Start parsing after `$J=` - else { char_counter = 0; } + uint8_t word_bit; // Bit-value for assigning tracking variables + uint8_t char_counter; + char letter; + float value; + uint8_t int_value = 0; + uint16_t mantissa = 0; + if (gc_parser_flags & GC_PARSER_JOG_MOTION) { + char_counter = 3; + } // Start parsing after `$J=` + else { + char_counter = 0; + } - while (line[char_counter] != 0) { // Loop until no more g-code words in line. + while (line[char_counter] != 0) { // Loop until no more g-code words in line. - // Import the next g-code word, expecting a letter followed by a value. Otherwise, error out. - letter = line[char_counter]; - if((letter < 'A') || (letter > 'Z')) { FAIL(STATUS_EXPECTED_COMMAND_LETTER); } // [Expected word letter] - char_counter++; - if (!read_float(line, &char_counter, &value)) { FAIL(STATUS_BAD_NUMBER_FORMAT); } // [Expected word value] + // Import the next g-code word, expecting a letter followed by a value. Otherwise, error out. + letter = line[char_counter]; + if ((letter < 'A') || (letter > 'Z')) { + FAIL(STATUS_EXPECTED_COMMAND_LETTER); + } // [Expected word letter] + char_counter++; + if (!read_float(line, &char_counter, &value)) { + FAIL(STATUS_BAD_NUMBER_FORMAT); + } // [Expected word value] - // Convert values to smaller uint8 significand and mantissa values for parsing this word. - // NOTE: Mantissa is multiplied by 100 to catch non-integer command values. This is more - // accurate than the NIST gcode requirement of x10 when used for commands, but not quite - // accurate enough for value words that require integers to within 0.0001. This should be - // a good enough comprimise and catch most all non-integer errors. To make it compliant, - // we would simply need to change the mantissa to int16, but this add compiled flash space. - // Maybe update this later. - int_value = trunc(value); - mantissa = round(100*(value - int_value)); // Compute mantissa for Gxx.x commands. - // NOTE: Rounding must be used to catch small floating point errors. + // Convert values to smaller uint8 significand and mantissa values for parsing this word. + // NOTE: Mantissa is multiplied by 100 to catch non-integer command values. This is more + // accurate than the NIST gcode requirement of x10 when used for commands, but not quite + // accurate enough for value words that require integers to within 0.0001. This should be + // a good enough comprimise and catch most all non-integer errors. To make it compliant, + // we would simply need to change the mantissa to int16, but this add compiled flash space. + // Maybe update this later. + int_value = trunc(value); + mantissa = round(100 * (value - int_value)); // Compute mantissa for Gxx.x commands. + // NOTE: Rounding must be used to catch small floating point errors. - // Check if the g-code word is supported or errors due to modal group violations or has - // been repeated in the g-code block. If ok, update the command or record its value. - switch(letter) { + // Check if the g-code word is supported or errors due to modal group violations or has + // been repeated in the g-code block. If ok, update the command or record its value. + switch (letter) { - /* 'G' and 'M' Command Words: Parse commands and check for modal group violations. - NOTE: Modal group numbers are defined in Table 4 of NIST RS274-NGC v3, pg.20 */ + /* 'G' and 'M' Command Words: Parse commands and check for modal group violations. + NOTE: Modal group numbers are defined in Table 4 of NIST RS274-NGC v3, pg.20 */ - case 'G': - // Determine 'G' command and its modal group - switch(int_value) { - case 10: case 28: case 30: case 92: - // Check for G10/28/30/92 being called with G0/1/2/3/38 on same block. - // * G43.1 is also an axis command but is not explicitly defined this way. - if (mantissa == 0) { // Ignore G28.1, G30.1, and G92.1 - if (axis_command) { FAIL(STATUS_GCODE_AXIS_COMMAND_CONFLICT); } // [Axis word/command conflict] - axis_command = AXIS_COMMAND_NON_MODAL; - } - // No break. Continues to next line. - case 4: case 53: - word_bit = MODAL_GROUP_G0; - gc_block.non_modal_command = int_value; - if ((int_value == 28) || (int_value == 30) || (int_value == 92)) { - if (!((mantissa == 0) || (mantissa == 10))) { FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); } - gc_block.non_modal_command += mantissa; - mantissa = 0; // Set to zero to indicate valid non-integer G command. - } - break; - case 0: case 1: case 2: case 3: case 38: - // Check for G0/1/2/3/38 being called with G10/28/30/92 on same block. - // * G43.1 is also an axis command but is not explicitly defined this way. - if (axis_command) { FAIL(STATUS_GCODE_AXIS_COMMAND_CONFLICT); } // [Axis word/command conflict] - axis_command = AXIS_COMMAND_MOTION_MODE; - // No break. Continues to next line. - case 80: - word_bit = MODAL_GROUP_G1; - gc_block.modal.motion = int_value; - if (int_value == 38){ - if (!((mantissa == 20) || (mantissa == 30) || (mantissa == 40) || (mantissa == 50))) { - FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); // [Unsupported G38.x command] - } - gc_block.modal.motion += (mantissa/10)+100; - mantissa = 0; // Set to zero to indicate valid non-integer G command. - } - break; - case 17: case 18: case 19: - word_bit = MODAL_GROUP_G2; - gc_block.modal.plane_select = int_value - 17; - break; - case 90: case 91: - if (mantissa == 0) { - word_bit = MODAL_GROUP_G3; - gc_block.modal.distance = int_value - 90; - } else { - word_bit = MODAL_GROUP_G4; - if ((mantissa != 10) || (int_value == 90)) { FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); } // [G90.1 not supported] - mantissa = 0; // Set to zero to indicate valid non-integer G command. - // Otherwise, arc IJK incremental mode is default. G91.1 does nothing. - } - break; - case 93: case 94: - word_bit = MODAL_GROUP_G5; - gc_block.modal.feed_rate = 94 - int_value; - break; - case 20: case 21: - word_bit = MODAL_GROUP_G6; - gc_block.modal.units = 21 - int_value; - break; - case 40: - word_bit = MODAL_GROUP_G7; - // NOTE: Not required since cutter radius compensation is always disabled. Only here - // to support G40 commands that often appear in g-code program headers to setup defaults. - // gc_block.modal.cutter_comp = CUTTER_COMP_DISABLE; // G40 - break; - case 43: case 49: - word_bit = MODAL_GROUP_G8; - // NOTE: The NIST g-code standard vaguely states that when a tool length offset is changed, - // there cannot be any axis motion or coordinate offsets updated. Meaning G43, G43.1, and G49 - // all are explicit axis commands, regardless if they require axis words or not. - if (axis_command) { FAIL(STATUS_GCODE_AXIS_COMMAND_CONFLICT); } // [Axis word/command conflict] } - axis_command = AXIS_COMMAND_TOOL_LENGTH_OFFSET; - if (int_value == 49) { // G49 - gc_block.modal.tool_length = TOOL_LENGTH_OFFSET_CANCEL; - } else if (mantissa == 10) { // G43.1 - gc_block.modal.tool_length = TOOL_LENGTH_OFFSET_ENABLE_DYNAMIC; - } else { FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); } // [Unsupported G43.x command] - mantissa = 0; // Set to zero to indicate valid non-integer G command. - break; - case 54: case 55: case 56: case 57: case 58: case 59: - // NOTE: G59.x are not supported. (But their int_values would be 60, 61, and 62.) - word_bit = MODAL_GROUP_G12; - gc_block.modal.coord_select = int_value - 54; // Shift to array indexing. - break; - case 61: - word_bit = MODAL_GROUP_G13; - if (mantissa != 0) { FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); } // [G61.1 not supported] - // gc_block.modal.control = CONTROL_MODE_EXACT_PATH; // G61 - break; - default: FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); // [Unsupported G command] - } - if (mantissa > 0) { FAIL(STATUS_GCODE_COMMAND_VALUE_NOT_INTEGER); } // [Unsupported or invalid Gxx.x command] - // Check for more than one command per modal group violations in the current block - // NOTE: Variable 'word_bit' is always assigned, if the command is valid. - if ( bit_istrue(command_words,bit(word_bit)) ) { FAIL(STATUS_GCODE_MODAL_GROUP_VIOLATION); } - command_words |= bit(word_bit); - break; - - case 'M': - - // Determine 'M' command and its modal group - if (mantissa > 0) { FAIL(STATUS_GCODE_COMMAND_VALUE_NOT_INTEGER); } // [No Mxx.x commands] - switch(int_value) { - case 0: case 1: case 2: case 30: - word_bit = MODAL_GROUP_M4; - switch(int_value) { - case 0: gc_block.modal.program_flow = PROGRAM_FLOW_PAUSED; break; // Program pause - case 1: break; // Optional stop not supported. Ignore. - default: gc_block.modal.program_flow = int_value; // Program end and reset - } - break; - case 3: case 4: case 5: - word_bit = MODAL_GROUP_M7; - switch(int_value) { - case 3: gc_block.modal.spindle = SPINDLE_ENABLE_CW; break; - case 4: gc_block.modal.spindle = SPINDLE_ENABLE_CCW; break; - case 5: gc_block.modal.spindle = SPINDLE_DISABLE; break; - } - break; - #ifdef ENABLE_M7 - case 7: case 8: case 9: - #else - case 8: case 9: - #endif - word_bit = MODAL_GROUP_M8; - switch(int_value) { - #ifdef ENABLE_M7 - case 7: gc_block.modal.coolant |= COOLANT_MIST_ENABLE; break; - #endif - case 8: gc_block.modal.coolant |= COOLANT_FLOOD_ENABLE; break; - case 9: gc_block.modal.coolant = COOLANT_DISABLE; break; // M9 disables both M7 and M8. - } - break; - #ifdef ENABLE_PARKING_OVERRIDE_CONTROL + case 'G': + // Determine 'G' command and its modal group + switch (int_value) { + case 10: + case 28: + case 30: + case 92: + // Check for G10/28/30/92 being called with G0/1/2/3/38 on same block. + // * G43.1 is also an axis command but is not explicitly defined this way. + if (mantissa == 0) { // Ignore G28.1, G30.1, and G92.1 + if (axis_command) { + FAIL(STATUS_GCODE_AXIS_COMMAND_CONFLICT); + } // [Axis word/command conflict] + axis_command = AXIS_COMMAND_NON_MODAL; + } + // No break. Continues to next line. + case 4: + case 53: + word_bit = MODAL_GROUP_G0; + gc_block.non_modal_command = int_value; + if ((int_value == 28) || (int_value == 30) || (int_value == 92)) { + if (!((mantissa == 0) || (mantissa == 10))) { + FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); + } + gc_block.non_modal_command += mantissa; + mantissa = 0; // Set to zero to indicate valid non-integer G command. + } + break; + case 0: + case 1: + case 2: + case 3: + case 38: + // Check for G0/1/2/3/38 being called with G10/28/30/92 on same block. + // * G43.1 is also an axis command but is not explicitly defined this way. + if (axis_command) { + FAIL(STATUS_GCODE_AXIS_COMMAND_CONFLICT); + } // [Axis word/command conflict] + axis_command = AXIS_COMMAND_MOTION_MODE; + // No break. Continues to next line. + case 80: + word_bit = MODAL_GROUP_G1; + gc_block.modal.motion = int_value; + if (int_value == 38) { + if (!((mantissa == 20) || (mantissa == 30) || (mantissa == 40) || (mantissa == 50))) { + FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); // [Unsupported G38.x command] + } + gc_block.modal.motion += (mantissa / 10) + 100; + mantissa = 0; // Set to zero to indicate valid non-integer G command. + } + break; + case 17: + case 18: + case 19: + word_bit = MODAL_GROUP_G2; + gc_block.modal.plane_select = int_value - 17; + break; + case 90: + case 91: + if (mantissa == 0) { + word_bit = MODAL_GROUP_G3; + gc_block.modal.distance = int_value - 90; + } else { + word_bit = MODAL_GROUP_G4; + if ((mantissa != 10) || (int_value == 90)) { + FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); + } // [G90.1 not supported] + mantissa = 0; // Set to zero to indicate valid non-integer G command. + // Otherwise, arc IJK incremental mode is default. G91.1 does nothing. + } + break; + case 93: + case 94: + word_bit = MODAL_GROUP_G5; + gc_block.modal.feed_rate = 94 - int_value; + break; + case 20: + case 21: + word_bit = MODAL_GROUP_G6; + gc_block.modal.units = 21 - int_value; + break; + case 40: + word_bit = MODAL_GROUP_G7; + // NOTE: Not required since cutter radius compensation is always disabled. Only here + // to support G40 commands that often appear in g-code program headers to setup defaults. + // gc_block.modal.cutter_comp = CUTTER_COMP_DISABLE; // G40 + break; + case 43: + case 49: + word_bit = MODAL_GROUP_G8; + // NOTE: The NIST g-code standard vaguely states that when a tool length offset is changed, + // there cannot be any axis motion or coordinate offsets updated. Meaning G43, G43.1, and G49 + // all are explicit axis commands, regardless if they require axis words or not. + if (axis_command) { + FAIL(STATUS_GCODE_AXIS_COMMAND_CONFLICT); + } // [Axis word/command conflict] } + axis_command = AXIS_COMMAND_TOOL_LENGTH_OFFSET; + if (int_value == 49) { // G49 + gc_block.modal.tool_length = TOOL_LENGTH_OFFSET_CANCEL; + } else if (mantissa == 10) { // G43.1 + gc_block.modal.tool_length = TOOL_LENGTH_OFFSET_ENABLE_DYNAMIC; + } else { + FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); + } // [Unsupported G43.x command] + mantissa = 0; // Set to zero to indicate valid non-integer G command. + break; + case 54: + case 55: case 56: - word_bit = MODAL_GROUP_M9; - gc_block.modal.override = OVERRIDE_PARKING_MOTION; - break; - #endif - default: FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); // [Unsupported M command] + case 57: + case 58: + case 59: + // NOTE: G59.x are not supported. (But their int_values would be 60, 61, and 62.) + word_bit = MODAL_GROUP_G12; + gc_block.modal.coord_select = int_value - 54; // Shift to array indexing. + break; + case 61: + word_bit = MODAL_GROUP_G13; + if (mantissa != 0) { + FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); + } // [G61.1 not supported] + // gc_block.modal.control = CONTROL_MODE_EXACT_PATH; // G61 + break; + default: FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); // [Unsupported G command] + } + if (mantissa > 0) { + FAIL(STATUS_GCODE_COMMAND_VALUE_NOT_INTEGER); + } // [Unsupported or invalid Gxx.x command] + // Check for more than one command per modal group violations in the current block + // NOTE: Variable 'word_bit' is always assigned, if the command is valid. + if (bit_istrue(command_words, bit(word_bit))) { + FAIL(STATUS_GCODE_MODAL_GROUP_VIOLATION); + } + command_words |= bit(word_bit); + break; + + case 'M': + + // Determine 'M' command and its modal group + if (mantissa > 0) { + FAIL(STATUS_GCODE_COMMAND_VALUE_NOT_INTEGER); + } // [No Mxx.x commands] + switch (int_value) { + case 0: + case 1: + case 2: + case 30: + word_bit = MODAL_GROUP_M4; + switch (int_value) { + case 0: gc_block.modal.program_flow = PROGRAM_FLOW_PAUSED; break; // Program pause + case 1: break; // Optional stop not supported. Ignore. + default: gc_block.modal.program_flow = int_value; // Program end and reset + } + break; + case 3: + case 4: + case 5: + word_bit = MODAL_GROUP_M7; + switch (int_value) { + case 3: gc_block.modal.spindle = SPINDLE_ENABLE_CW; break; + case 4: gc_block.modal.spindle = SPINDLE_ENABLE_CCW; break; + case 5: gc_block.modal.spindle = SPINDLE_DISABLE; break; + } + break; +#ifdef ENABLE_M7 + case 7: + case 8: + case 9: +#else + case 8: + case 9: +#endif + word_bit = MODAL_GROUP_M8; + switch (int_value) { +#ifdef ENABLE_M7 + case 7: gc_block.modal.coolant |= COOLANT_MIST_ENABLE; break; +#endif + case 8: gc_block.modal.coolant |= COOLANT_FLOOD_ENABLE; break; + case 9: gc_block.modal.coolant = COOLANT_DISABLE; break; // M9 disables both M7 and M8. + } + break; +#ifdef ENABLE_PARKING_OVERRIDE_CONTROL + case 56: + word_bit = MODAL_GROUP_M9; + gc_block.modal.override = OVERRIDE_PARKING_MOTION; + break; +#endif + default: FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); // [Unsupported M command] + } + + // Check for more than one command per modal group violations in the current block + // NOTE: Variable 'word_bit' is always assigned, if the command is valid. + if (bit_istrue(command_words, bit(word_bit))) { + FAIL(STATUS_GCODE_MODAL_GROUP_VIOLATION); + } + command_words |= bit(word_bit); + break; + + // NOTE: All remaining letters assign values. + default: + + /* Non-Command Words: This initial parsing phase only checks for repeats of the remaining + legal g-code words and stores their value. Error-checking is performed later since some + words (I,J,K,L,P,R) have multiple connotations and/or depend on the issued commands. */ + switch (letter) { + // case 'A': // Not supported + // case 'B': // Not supported + // case 'C': // Not supported + // case 'D': // Not supported + case 'F': + word_bit = WORD_F; + gc_block.values.f = value; + break; + // case 'H': // Not supported + case 'I': + word_bit = WORD_I; + gc_block.values.ijk[X_AXIS] = value; + ijk_words |= (1 << X_AXIS); + break; + case 'J': + word_bit = WORD_J; + gc_block.values.ijk[Y_AXIS] = value; + ijk_words |= (1 << Y_AXIS); + break; + case 'K': + word_bit = WORD_K; + gc_block.values.ijk[Z_AXIS] = value; + ijk_words |= (1 << Z_AXIS); + break; + case 'L': + word_bit = WORD_L; + gc_block.values.l = int_value; + break; + case 'N': + word_bit = WORD_N; + gc_block.values.n = trunc(value); + break; + case 'P': + word_bit = WORD_P; + gc_block.values.p = value; + break; + // NOTE: For certain commands, P value must be an integer, but none of these commands are supported. + // case 'Q': // Not supported + case 'R': + word_bit = WORD_R; + gc_block.values.r = value; + break; + case 'S': + word_bit = WORD_S; + gc_block.values.s = value; + break; + case 'T': + word_bit = WORD_T; + if (value > MAX_TOOL_NUMBER) { + FAIL(STATUS_GCODE_MAX_VALUE_EXCEEDED); + } + gc_block.values.t = int_value; + break; + case 'X': + word_bit = WORD_X; + gc_block.values.xyz[X_AXIS] = value; + axis_words |= (1 << X_AXIS); + break; + case 'Y': + word_bit = WORD_Y; + gc_block.values.xyz[Y_AXIS] = value; + axis_words |= (1 << Y_AXIS); + break; + case 'Z': + word_bit = WORD_Z; + gc_block.values.xyz[Z_AXIS] = value; + axis_words |= (1 << Z_AXIS); + break; + default: FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); + } + + // NOTE: Variable 'word_bit' is always assigned, if the non-command letter is valid. + if (bit_istrue(value_words, bit(word_bit))) { + FAIL(STATUS_GCODE_WORD_REPEATED); + } // [Word repeated] + // Check for invalid negative values for words F, N, P, T, and S. + // NOTE: Negative value check is done here simply for code-efficiency. + if (bit(word_bit) & (bit(WORD_F) | bit(WORD_N) | bit(WORD_P) | bit(WORD_T) | bit(WORD_S))) { + if (value < 0.0) { + FAIL(STATUS_NEGATIVE_VALUE); + } // [Word value cannot be negative] + } + value_words |= bit(word_bit); // Flag to indicate parameter assigned. } - - // Check for more than one command per modal group violations in the current block - // NOTE: Variable 'word_bit' is always assigned, if the command is valid. - if ( bit_istrue(command_words,bit(word_bit)) ) { FAIL(STATUS_GCODE_MODAL_GROUP_VIOLATION); } - command_words |= bit(word_bit); - break; - - // NOTE: All remaining letters assign values. - default: - - /* Non-Command Words: This initial parsing phase only checks for repeats of the remaining - legal g-code words and stores their value. Error-checking is performed later since some - words (I,J,K,L,P,R) have multiple connotations and/or depend on the issued commands. */ - switch(letter){ - // case 'A': // Not supported - // case 'B': // Not supported - // case 'C': // Not supported - // case 'D': // Not supported - case 'F': word_bit = WORD_F; gc_block.values.f = value; break; - // case 'H': // Not supported - case 'I': word_bit = WORD_I; gc_block.values.ijk[X_AXIS] = value; ijk_words |= (1< MAX_TOOL_NUMBER) { FAIL(STATUS_GCODE_MAX_VALUE_EXCEEDED); } - gc_block.values.t = int_value; - break; - case 'X': word_bit = WORD_X; gc_block.values.xyz[X_AXIS] = value; axis_words |= (1< MAX_LINE_NUMBER) { FAIL(STATUS_GCODE_INVALID_LINE_NUMBER); } // [Exceeds max line number] - } - // bit_false(value_words,bit(WORD_N)); // NOTE: Single-meaning value word. Set at end of error-checking. - - // Track for unused words at the end of error-checking. - // NOTE: Single-meaning value words are removed all at once at the end of error-checking, because - // they are always used when present. This was done to save a few bytes of flash. For clarity, the - // single-meaning value words may be removed as they are used. Also, axis words are treated in the - // same way. If there is an explicit/implicit axis command, XYZ words are always used and are - // are removed at the end of error-checking. - - // [1. Comments ]: MSG's NOT SUPPORTED. Comment handling performed by protocol. - - // [2. Set feed rate mode ]: G93 F word missing with G1,G2/3 active, implicitly or explicitly. Feed rate - // is not defined after switching to G94 from G93. - // NOTE: For jogging, ignore prior feed rate mode. Enforce G94 and check for required F word. - if (gc_parser_flags & GC_PARSER_JOG_MOTION) { - if (bit_isfalse(value_words,bit(WORD_F))) { FAIL(STATUS_GCODE_UNDEFINED_FEED_RATE); } - if (gc_block.modal.units == UNITS_MODE_INCHES) { gc_block.values.f *= MM_PER_INCH; } - } else { - if (gc_block.modal.feed_rate == FEED_RATE_MODE_INVERSE_TIME) { // = G93 - // NOTE: G38 can also operate in inverse time, but is undefined as an error. Missing F word check added here. - if (axis_command == AXIS_COMMAND_MOTION_MODE) { - if ((gc_block.modal.motion != MOTION_MODE_NONE) && (gc_block.modal.motion != MOTION_MODE_SEEK)) { - if (bit_isfalse(value_words,bit(WORD_F))) { FAIL(STATUS_GCODE_UNDEFINED_FEED_RATE); } // [F word missing] - } - } - // NOTE: It seems redundant to check for an F word to be passed after switching from G94 to G93. We would - // accomplish the exact same thing if the feed rate value is always reset to zero and undefined after each - // inverse time block, since the commands that use this value already perform undefined checks. This would - // also allow other commands, following this switch, to execute and not error out needlessly. This code is - // combined with the above feed rate mode and the below set feed rate error-checking. - - // [3. Set feed rate ]: F is negative (done.) - // - In inverse time mode: Always implicitly zero the feed rate value before and after block completion. - // NOTE: If in G93 mode or switched into it from G94, just keep F value as initialized zero or passed F word - // value in the block. If no F word is passed with a motion command that requires a feed rate, this will error - // out in the motion modes error-checking. However, if no F word is passed with NO motion command that requires - // a feed rate, we simply move on and the state feed rate value gets updated to zero and remains undefined. - } else { // = G94 - // - In units per mm mode: If F word passed, ensure value is in mm/min, otherwise push last state value. - if (gc_state.modal.feed_rate == FEED_RATE_MODE_UNITS_PER_MIN) { // Last state is also G94 - if (bit_istrue(value_words,bit(WORD_F))) { - if (gc_block.modal.units == UNITS_MODE_INCHES) { gc_block.values.f *= MM_PER_INCH; } - } else { - gc_block.values.f = gc_state.feed_rate; // Push last state feed rate - } - } // Else, switching to G94 from G93, so don't push last state feed rate. Its undefined or the passed F word value. + // Determine implicit axis command conditions. Axis words have been passed, but no explicit axis + // command has been sent. If so, set axis command to current motion mode. + if (axis_words) { + if (!axis_command) { + axis_command = AXIS_COMMAND_MOTION_MODE; + } // Assign implicit motion-mode } - } - // bit_false(value_words,bit(WORD_F)); // NOTE: Single-meaning value word. Set at end of error-checking. - // [4. Set spindle speed ]: S is negative (done.) - if (bit_isfalse(value_words,bit(WORD_S))) { gc_block.values.s = gc_state.spindle_speed; } - // bit_false(value_words,bit(WORD_S)); // NOTE: Single-meaning value word. Set at end of error-checking. - - // [5. Select tool ]: NOT SUPPORTED. Only tracks value. T is negative (done.) Not an integer. Greater than max tool value. - // bit_false(value_words,bit(WORD_T)); // NOTE: Single-meaning value word. Set at end of error-checking. - - // [6. Change tool ]: N/A - // [7. Spindle control ]: N/A - // [8. Coolant control ]: N/A - // [9. Override control ]: Not supported except for a Grbl-only parking motion override control. - #ifdef ENABLE_PARKING_OVERRIDE_CONTROL - if (bit_istrue(command_words,bit(MODAL_GROUP_M9))) { // Already set as enabled in parser. - if (bit_istrue(value_words,bit(WORD_P))) { - if (gc_block.values.p == 0.0) { gc_block.modal.override = OVERRIDE_DISABLED; } - bit_false(value_words,bit(WORD_P)); - } + // Check for valid line number N value. + if (bit_istrue(value_words, bit(WORD_N))) { + // Line number value cannot be less than zero (done) or greater than max line number. + if (gc_block.values.n > MAX_LINE_NUMBER) { + FAIL(STATUS_GCODE_INVALID_LINE_NUMBER); + } // [Exceeds max line number] } - #endif + // bit_false(value_words,bit(WORD_N)); // NOTE: Single-meaning value word. Set at end of error-checking. - // [10. Dwell ]: P value missing. P is negative (done.) NOTE: See below. - if (gc_block.non_modal_command == NON_MODAL_DWELL) { - if (bit_isfalse(value_words,bit(WORD_P))) { FAIL(STATUS_GCODE_VALUE_WORD_MISSING); } // [P word missing] - bit_false(value_words,bit(WORD_P)); - } + // Track for unused words at the end of error-checking. + // NOTE: Single-meaning value words are removed all at once at the end of error-checking, because + // they are always used when present. This was done to save a few bytes of flash. For clarity, the + // single-meaning value words may be removed as they are used. Also, axis words are treated in the + // same way. If there is an explicit/implicit axis command, XYZ words are always used and are + // are removed at the end of error-checking. - // [11. Set active plane ]: N/A - switch (gc_block.modal.plane_select) { + // [1. Comments ]: MSG's NOT SUPPORTED. Comment handling performed by protocol. + + // [2. Set feed rate mode ]: G93 F word missing with G1,G2/3 active, implicitly or explicitly. Feed rate + // is not defined after switching to G94 from G93. + // NOTE: For jogging, ignore prior feed rate mode. Enforce G94 and check for required F word. + if (gc_parser_flags & GC_PARSER_JOG_MOTION) { + if (bit_isfalse(value_words, bit(WORD_F))) { + FAIL(STATUS_GCODE_UNDEFINED_FEED_RATE); + } + if (gc_block.modal.units == UNITS_MODE_INCHES) { + gc_block.values.f *= MM_PER_INCH; + } + } else { + if (gc_block.modal.feed_rate == FEED_RATE_MODE_INVERSE_TIME) { // = G93 + // NOTE: G38 can also operate in inverse time, but is undefined as an error. Missing F word check added + // here. + if (axis_command == AXIS_COMMAND_MOTION_MODE) { + if ((gc_block.modal.motion != MOTION_MODE_NONE) && (gc_block.modal.motion != MOTION_MODE_SEEK)) { + if (bit_isfalse(value_words, bit(WORD_F))) { + FAIL(STATUS_GCODE_UNDEFINED_FEED_RATE); + } // [F word missing] + } + } + // NOTE: It seems redundant to check for an F word to be passed after switching from G94 to G93. We would + // accomplish the exact same thing if the feed rate value is always reset to zero and undefined after each + // inverse time block, since the commands that use this value already perform undefined checks. This would + // also allow other commands, following this switch, to execute and not error out needlessly. This code is + // combined with the above feed rate mode and the below set feed rate error-checking. + + // [3. Set feed rate ]: F is negative (done.) + // - In inverse time mode: Always implicitly zero the feed rate value before and after block completion. + // NOTE: If in G93 mode or switched into it from G94, just keep F value as initialized zero or passed F word + // value in the block. If no F word is passed with a motion command that requires a feed rate, this will + // error out in the motion modes error-checking. However, if no F word is passed with NO motion command that + // requires a feed rate, we simply move on and the state feed rate value gets updated to zero and remains + // undefined. + } else { // = G94 + // - In units per mm mode: If F word passed, ensure value is in mm/min, otherwise push last state value. + if (gc_state.modal.feed_rate == FEED_RATE_MODE_UNITS_PER_MIN) { // Last state is also G94 + if (bit_istrue(value_words, bit(WORD_F))) { + if (gc_block.modal.units == UNITS_MODE_INCHES) { + gc_block.values.f *= MM_PER_INCH; + } + } else { + gc_block.values.f = gc_state.feed_rate; // Push last state feed rate + } + } // Else, switching to G94 from G93, so don't push last state feed rate. Its undefined or the passed F word + // value. + } + } + // bit_false(value_words,bit(WORD_F)); // NOTE: Single-meaning value word. Set at end of error-checking. + + // [4. Set spindle speed ]: S is negative (done.) + if (bit_isfalse(value_words, bit(WORD_S))) { + gc_block.values.s = gc_state.spindle_speed; + } +// bit_false(value_words,bit(WORD_S)); // NOTE: Single-meaning value word. Set at end of error-checking. + +// [5. Select tool ]: NOT SUPPORTED. Only tracks value. T is negative (done.) Not an integer. Greater than max tool +// value. bit_false(value_words,bit(WORD_T)); // NOTE: Single-meaning value word. Set at end of error-checking. + +// [6. Change tool ]: N/A +// [7. Spindle control ]: N/A +// [8. Coolant control ]: N/A +// [9. Override control ]: Not supported except for a Grbl-only parking motion override control. +#ifdef ENABLE_PARKING_OVERRIDE_CONTROL + if (bit_istrue(command_words, bit(MODAL_GROUP_M9))) { // Already set as enabled in parser. + if (bit_istrue(value_words, bit(WORD_P))) { + if (gc_block.values.p == 0.0) { + gc_block.modal.override = OVERRIDE_DISABLED; + } + bit_false(value_words, bit(WORD_P)); + } + } +#endif + + // [10. Dwell ]: P value missing. P is negative (done.) NOTE: See below. + if (gc_block.non_modal_command == NON_MODAL_DWELL) { + if (bit_isfalse(value_words, bit(WORD_P))) { + FAIL(STATUS_GCODE_VALUE_WORD_MISSING); + } // [P word missing] + bit_false(value_words, bit(WORD_P)); + } + + // [11. Set active plane ]: N/A + switch (gc_block.modal.plane_select) { case PLANE_SELECT_XY: - axis_0 = X_AXIS; - axis_1 = Y_AXIS; - axis_linear = Z_AXIS; - break; + axis_0 = X_AXIS; + axis_1 = Y_AXIS; + axis_linear = Z_AXIS; + break; case PLANE_SELECT_ZX: - axis_0 = Z_AXIS; - axis_1 = X_AXIS; - axis_linear = Y_AXIS; - break; + axis_0 = Z_AXIS; + axis_1 = X_AXIS; + axis_linear = Y_AXIS; + break; default: // case PLANE_SELECT_YZ: - axis_0 = Y_AXIS; - axis_1 = Z_AXIS; - axis_linear = X_AXIS; - } - - // [12. Set length units ]: N/A - // Pre-convert XYZ coordinate values to millimeters, if applicable. - uint8_t idx; - if (gc_block.modal.units == UNITS_MODE_INCHES) { - for (idx=0; idx N_COORDINATE_SYSTEM) { FAIL(STATUS_GCODE_UNSUPPORTED_COORD_SYS); } // [Greater than N sys] - if (gc_state.modal.coord_select != gc_block.modal.coord_select) { - if (!(settings_read_coord_data(gc_block.modal.coord_select,block_coord_system))) { FAIL(STATUS_SETTING_READ_FAIL); } - } - } - - // [16. Set path control mode ]: N/A. Only G61. G61.1 and G64 NOT SUPPORTED. - // [17. Set distance mode ]: N/A. Only G91.1. G90.1 NOT SUPPORTED. - // [18. Set retract mode ]: NOT SUPPORTED. - - // [19. Remaining non-modal actions ]: Check go to predefined position, set G10, or set axis offsets. - // NOTE: We need to separate the non-modal commands that are axis word-using (G10/G28/G30/G92), as these - // commands all treat axis words differently. G10 as absolute offsets or computes current position as - // the axis value, G92 similarly to G10 L20, and G28/30 as an intermediate target position that observes - // all the current coordinate system and G92 offsets. - switch (gc_block.non_modal_command) { - case NON_MODAL_SET_COORDINATE_DATA: - // [G10 Errors]: L missing and is not 2 or 20. P word missing. (Negative P value done.) - // [G10 L2 Errors]: R word NOT SUPPORTED. P value not 0 to nCoordSys(max 9). Axis words missing. - // [G10 L20 Errors]: P must be 0 to nCoordSys(max 9). Axis words missing. - if (!axis_words) { FAIL(STATUS_GCODE_NO_AXIS_WORDS) }; // [No axis words] - if (bit_isfalse(value_words,((1< N_COORDINATE_SYSTEM) { FAIL(STATUS_GCODE_UNSUPPORTED_COORD_SYS); } // [Greater than N sys] - if (gc_block.values.l != 20) { - if (gc_block.values.l == 2) { - if (bit_istrue(value_words,bit(WORD_R))) { FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); } // [G10 L2 R not supported] - } else { FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); } // [Unsupported L] - } - bit_false(value_words,(bit(WORD_L)|bit(WORD_P))); - - // Determine coordinate system to change and try to load from EEPROM. - if (coord_select > 0) { coord_select--; } // Adjust P1-P6 index to EEPROM coordinate data indexing. - else { coord_select = gc_block.modal.coord_select; } // Index P0 as the active coordinate system - - // NOTE: Store parameter data in IJK values. By rule, they are not in use with this command. - if (!settings_read_coord_data(coord_select,gc_block.values.ijk)) { FAIL(STATUS_SETTING_READ_FAIL); } // [EEPROM read fail] - - // Pre-calculate the coordinate data changes. - for (idx=0; idx WCS = MPos - G92 - TLO - WPos - gc_block.values.ijk[idx] = gc_state.position[idx]-gc_state.coord_offset[idx]-gc_block.values.xyz[idx]; - if (idx == TOOL_LENGTH_OFFSET_AXIS) { gc_block.values.ijk[idx] -= gc_state.tool_length_offset; } - } else { - // L2: Update coordinate system axis to programmed value. - gc_block.values.ijk[idx] = gc_block.values.xyz[idx]; - } - } // Else, keep current stored value. - } - break; - case NON_MODAL_SET_COORDINATE_OFFSET: - // [G92 Errors]: No axis words. - if (!axis_words) { FAIL(STATUS_GCODE_NO_AXIS_WORDS); } // [No axis words] - - // Update axes defined only in block. Offsets current system to defined value. Does not update when - // active coordinate system is selected, but is still active unless G92.1 disables it. - for (idx=0; idx G92 = MPos - WCS - TLO - WPos - gc_block.values.xyz[idx] = gc_state.position[idx]-block_coord_system[idx]-gc_block.values.xyz[idx]; - if (idx == TOOL_LENGTH_OFFSET_AXIS) { gc_block.values.xyz[idx] -= gc_state.tool_length_offset; } - } else { - gc_block.values.xyz[idx] = gc_state.coord_offset[idx]; + // [12. Set length units ]: N/A + // Pre-convert XYZ coordinate values to millimeters, if applicable. + uint8_t idx; + if (gc_block.modal.units == UNITS_MODE_INCHES) { + for (idx = 0; idx < N_AXIS; idx++) { // Axes indices are consistent, so loop may be used. + if (bit_istrue(axis_words, bit(idx))) { + gc_block.values.xyz[idx] *= MM_PER_INCH; + } } - } - break; + } + + // [13. Cutter radius compensation ]: G41/42 NOT SUPPORTED. Error, if enabled while G53 is active. + // [G40 Errors]: G2/3 arc is programmed after a G40. The linear move after disabling is less than tool diameter. + // NOTE: Since cutter radius compensation is never enabled, these G40 errors don't apply. Grbl supports G40 + // only for the purpose to not error when G40 is sent with a g-code program header to setup the default modes. + + // [14. Cutter length compensation ]: G43 NOT SUPPORTED, but G43.1 and G49 are. + // [G43.1 Errors]: Motion command in same line. + // NOTE: Although not explicitly stated so, G43.1 should be applied to only one valid + // axis that is configured (in config.h). There should be an error if the configured axis + // is absent or if any of the other axis words are present. + if (axis_command == AXIS_COMMAND_TOOL_LENGTH_OFFSET) { // Indicates called in block. + if (gc_block.modal.tool_length == TOOL_LENGTH_OFFSET_ENABLE_DYNAMIC) { + if (axis_words ^ (1 << TOOL_LENGTH_OFFSET_AXIS)) { + FAIL(STATUS_GCODE_G43_DYNAMIC_AXIS_ERROR); + } + } + } + + // [15. Coordinate system selection ]: *N/A. Error, if cutter radius comp is active. + // TODO: An EEPROM read of the coordinate data may require a buffer sync when the cycle + // is active. The read pauses the processor temporarily and may cause a rare crash. For + // future versions on processors with enough memory, all coordinate data should be stored + // in memory and written to EEPROM only when there is not a cycle active. + float block_coord_system[N_AXIS]; + memcpy(block_coord_system, gc_state.coord_system, sizeof(gc_state.coord_system)); + if (bit_istrue(command_words, bit(MODAL_GROUP_G12))) { // Check if called in block + if (gc_block.modal.coord_select > N_COORDINATE_SYSTEM) { + FAIL(STATUS_GCODE_UNSUPPORTED_COORD_SYS); + } // [Greater than N sys] + if (gc_state.modal.coord_select != gc_block.modal.coord_select) { + if (!(settings_read_coord_data(gc_block.modal.coord_select, block_coord_system))) { + FAIL(STATUS_SETTING_READ_FAIL); + } + } + } + + // [16. Set path control mode ]: N/A. Only G61. G61.1 and G64 NOT SUPPORTED. + // [17. Set distance mode ]: N/A. Only G91.1. G90.1 NOT SUPPORTED. + // [18. Set retract mode ]: NOT SUPPORTED. + + // [19. Remaining non-modal actions ]: Check go to predefined position, set G10, or set axis offsets. + // NOTE: We need to separate the non-modal commands that are axis word-using (G10/G28/G30/G92), as these + // commands all treat axis words differently. G10 as absolute offsets or computes current position as + // the axis value, G92 similarly to G10 L20, and G28/30 as an intermediate target position that observes + // all the current coordinate system and G92 offsets. + switch (gc_block.non_modal_command) { + case NON_MODAL_SET_COORDINATE_DATA: + // [G10 Errors]: L missing and is not 2 or 20. P word missing. (Negative P value done.) + // [G10 L2 Errors]: R word NOT SUPPORTED. P value not 0 to nCoordSys(max 9). Axis words missing. + // [G10 L20 Errors]: P must be 0 to nCoordSys(max 9). Axis words missing. + if (!axis_words) { + FAIL(STATUS_GCODE_NO_AXIS_WORDS) + }; // [No axis words] + if (bit_isfalse(value_words, ((1 << WORD_P) | (1 << WORD_L)))) { + FAIL(STATUS_GCODE_VALUE_WORD_MISSING); + } // [P/L word missing] + coord_select = trunc(gc_block.values.p); // Convert p value to int. + if (coord_select > N_COORDINATE_SYSTEM) { + FAIL(STATUS_GCODE_UNSUPPORTED_COORD_SYS); + } // [Greater than N sys] + if (gc_block.values.l != 20) { + if (gc_block.values.l == 2) { + if (bit_istrue(value_words, bit(WORD_R))) { + FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); + } // [G10 L2 R not supported] + } else { + FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND); + } // [Unsupported L] + } + bit_false(value_words, (bit(WORD_L) | bit(WORD_P))); + + // Determine coordinate system to change and try to load from EEPROM. + if (coord_select > 0) { + coord_select--; + } // Adjust P1-P6 index to EEPROM coordinate data indexing. + else { + coord_select = gc_block.modal.coord_select; + } // Index P0 as the active coordinate system + + // NOTE: Store parameter data in IJK values. By rule, they are not in use with this command. + if (!settings_read_coord_data(coord_select, gc_block.values.ijk)) { + FAIL(STATUS_SETTING_READ_FAIL); + } // [EEPROM read fail] + + // Pre-calculate the coordinate data changes. + for (idx = 0; idx < N_AXIS; idx++) { // Axes indices are consistent, so loop may be used. + // Update axes defined only in block. Always in machine coordinates. Can change non-active system. + if (bit_istrue(axis_words, bit(idx))) { + if (gc_block.values.l == 20) { + // L20: Update coordinate system axis at current position (with modifiers) with programmed value + // WPos = MPos - WCS - G92 - TLO -> WCS = MPos - G92 - TLO - WPos + gc_block.values.ijk[idx] = + gc_state.position[idx] - gc_state.coord_offset[idx] - gc_block.values.xyz[idx]; + if (idx == TOOL_LENGTH_OFFSET_AXIS) { + gc_block.values.ijk[idx] -= gc_state.tool_length_offset; + } + } else { + // L2: Update coordinate system axis to programmed value. + gc_block.values.ijk[idx] = gc_block.values.xyz[idx]; + } + } // Else, keep current stored value. + } + break; + case NON_MODAL_SET_COORDINATE_OFFSET: + // [G92 Errors]: No axis words. + if (!axis_words) { + FAIL(STATUS_GCODE_NO_AXIS_WORDS); + } // [No axis words] + + // Update axes defined only in block. Offsets current system to defined value. Does not update when + // active coordinate system is selected, but is still active unless G92.1 disables it. + for (idx = 0; idx < N_AXIS; idx++) { // Axes indices are consistent, so loop may be used. + if (bit_istrue(axis_words, bit(idx))) { + // WPos = MPos - WCS - G92 - TLO -> G92 = MPos - WCS - TLO - WPos + gc_block.values.xyz[idx] = gc_state.position[idx] - block_coord_system[idx] - gc_block.values.xyz[idx]; + if (idx == TOOL_LENGTH_OFFSET_AXIS) { + gc_block.values.xyz[idx] -= gc_state.tool_length_offset; + } + } else { + gc_block.values.xyz[idx] = gc_state.coord_offset[idx]; + } + } + break; default: - // At this point, the rest of the explicit axis commands treat the axis values as the traditional - // target position with the coordinate system offsets, G92 offsets, absolute override, and distance - // modes applied. This includes the motion mode commands. We can now pre-compute the target position. - // NOTE: Tool offsets may be appended to these conversions when/if this feature is added. - if (axis_command != AXIS_COMMAND_TOOL_LENGTH_OFFSET ) { // TLO block any axis command. - if (axis_words) { - for (idx=0; idx C -----------------+--------------- T <- [x,y] - | <------ d/2 ---->| - - C - Current position - T - Target position - O - center of circle that pass through both C and T - d - distance from C to T - r - designated radius - h - distance from center of CT to O - - Expanding the equations: - - d -> sqrt(x^2 + y^2) - h -> sqrt(4 * r^2 - x^2 - y^2)/2 - i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 - j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 - - Which can be written: - - i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 - j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 - - Which we for size and speed reasons optimize to: - - h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2) - i = (x - (y * h_x2_div_d))/2 - j = (y + (x * h_x2_div_d))/2 - */ - - // First, use h_x2_div_d to compute 4*h^2 to check if it is negative or r is smaller - // than d. If so, the sqrt of a negative number is complex and error out. - float h_x2_div_d = 4.0 * gc_block.values.r*gc_block.values.r - x*x - y*y; - - if (h_x2_div_d < 0) { FAIL(STATUS_GCODE_ARC_RADIUS_ERROR); } // [Arc radius error] - - // Finish computing h_x2_div_d. - h_x2_div_d = -sqrt(h_x2_div_d)/hypot_f(x,y); // == -(h * 2 / d) - // Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below) - if (gc_block.modal.motion == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; } - - /* The counter clockwise circle lies to the left of the target direction. When offset is positive, - the left hand circle will be generated - when it is negative the right hand circle is generated. - - T <-- Target position - - ^ - Clockwise circles with this center | Clockwise circles with this center will have - will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing! - \ | / - center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative - | - | - - C <-- Current position - */ - // Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go figure!), - // even though it is advised against ever generating such circles in a single line of g-code. By - // inverting the sign of h_x2_div_d the center of the circles is placed on the opposite side of the line of - // travel and thus we get the unadvisably long arcs as prescribed. - if (gc_block.values.r < 0) { - h_x2_div_d = -h_x2_div_d; - gc_block.values.r = -gc_block.values.r; // Finished with r. Set to positive for mc_arc + // [G53 Errors]: G0 and G1 are not active. Cutter compensation is enabled. + // NOTE: All explicit axis word commands are in this modal group. So no implicit check necessary. + if (!(gc_block.modal.motion == MOTION_MODE_SEEK || gc_block.modal.motion == MOTION_MODE_LINEAR)) { + FAIL(STATUS_GCODE_G53_INVALID_MOTION_MODE); // [G53 G0/1 not active] } - // Complete the operation by calculating the actual center of the arc - gc_block.values.ijk[axis_0] = 0.5*(x-(y*h_x2_div_d)); - gc_block.values.ijk[axis_1] = 0.5*(y+(x*h_x2_div_d)); - - } else { // Arc Center Format Offset Mode - if (!(ijk_words & (bit(axis_0)|bit(axis_1)))) { FAIL(STATUS_GCODE_NO_OFFSETS_IN_PLANE); } // [No offsets in plane] - bit_false(value_words,(bit(WORD_I)|bit(WORD_J)|bit(WORD_K))); - - // Convert IJK values to proper units. - if (gc_block.modal.units == UNITS_MODE_INCHES) { - for (idx=0; idx 0.005) { - if (delta_r > 0.5) { FAIL(STATUS_GCODE_INVALID_TARGET); } // [Arc definition error] > 0.5mm - if (delta_r > (0.001*gc_block.values.r)) { FAIL(STATUS_GCODE_INVALID_TARGET); } // [Arc definition error] > 0.005mm AND 0.1% radius - } - } - break; - case MOTION_MODE_PROBE_TOWARD_NO_ERROR: case MOTION_MODE_PROBE_AWAY_NO_ERROR: - gc_parser_flags |= GC_PARSER_PROBE_IS_NO_ERROR; // No break intentional. - case MOTION_MODE_PROBE_TOWARD: case MOTION_MODE_PROBE_AWAY: - if ((gc_block.modal.motion == MOTION_MODE_PROBE_AWAY) || - (gc_block.modal.motion == MOTION_MODE_PROBE_AWAY_NO_ERROR)) { gc_parser_flags |= GC_PARSER_PROBE_IS_AWAY; } - // [G38 Errors]: Target is same current. No axis words. Cutter compensation is enabled. Feed rate - // is undefined. Probe is triggered. NOTE: Probe check moved to probe cycle. Instead of returning - // an error, it issues an alarm to prevent further motion to the probe. It's also done there to - // allow the planner buffer to empty and move off the probe trigger before another probing cycle. - if (!axis_words) { FAIL(STATUS_GCODE_NO_AXIS_WORDS); } // [No axis words] - if (isequal_position_vector(gc_state.position, gc_block.values.xyz)) { FAIL(STATUS_GCODE_INVALID_TARGET); } // [Invalid target] - break; - } - } - } - - // [21. Program flow ]: No error checks required. - - // [0. Non-specific error-checks]: Complete unused value words check, i.e. IJK used when in arc - // radius mode, or axis words that aren't used in the block. - if (gc_parser_flags & GC_PARSER_JOG_MOTION) { - // Jogging only uses the F feed rate and XYZ value words. N is valid, but S and T are invalid. - bit_false(value_words,(bit(WORD_N)|bit(WORD_F))); - } else { - bit_false(value_words,(bit(WORD_N)|bit(WORD_F)|bit(WORD_S)|bit(WORD_T))); // Remove single-meaning value words. - } - if (axis_command) { bit_false(value_words,(bit(WORD_X)|bit(WORD_Y)|bit(WORD_Z))); } // Remove axis words. - if (value_words) { FAIL(STATUS_GCODE_UNUSED_WORDS); } // [Unused words] - - /* ------------------------------------------------------------------------------------- - STEP 4: EXECUTE!! - Assumes that all error-checking has been completed and no failure modes exist. We just - need to update the state and execute the block according to the order-of-execution. - */ - - // Initialize planner data struct for motion blocks. - plan_line_data_t plan_data; - plan_line_data_t *pl_data = &plan_data; - memset(pl_data,0,sizeof(plan_line_data_t)); // Zero pl_data struct - - // Intercept jog commands and complete error checking for valid jog commands and execute. - // NOTE: G-code parser state is not updated, except the position to ensure sequential jog - // targets are computed correctly. The final parser position after a jog is updated in - // protocol_execute_realtime() when jogging completes or is canceled. - if (gc_parser_flags & GC_PARSER_JOG_MOTION) { - // Only distance and unit modal commands and G53 absolute override command are allowed. - // NOTE: Feed rate word and axis word checks have already been performed in STEP 3. - if (command_words & ~(bit(MODAL_GROUP_G3) | bit(MODAL_GROUP_G6) | bit(MODAL_GROUP_G0)) ) { FAIL(STATUS_INVALID_JOG_COMMAND) }; - if (!(gc_block.non_modal_command == NON_MODAL_ABSOLUTE_OVERRIDE || gc_block.non_modal_command == NON_MODAL_NO_ACTION)) { FAIL(STATUS_INVALID_JOG_COMMAND); } - - // Initialize planner data to current spindle and coolant modal state. - pl_data->spindle_speed = gc_state.spindle_speed; - plan_data.condition = (gc_state.modal.spindle | gc_state.modal.coolant); - - uint8_t status = jog_execute(&plan_data, &gc_block); - if (status == STATUS_OK) { memcpy(gc_state.position, gc_block.values.xyz, sizeof(gc_block.values.xyz)); } - return(status); - } - - // If in laser mode, setup laser power based on current and past parser conditions. - if (bit_istrue(settings.flags,BITFLAG_LASER_MODE)) { - if ( !((gc_block.modal.motion == MOTION_MODE_LINEAR) || (gc_block.modal.motion == MOTION_MODE_CW_ARC) - || (gc_block.modal.motion == MOTION_MODE_CCW_ARC)) ) { - gc_parser_flags |= GC_PARSER_LASER_DISABLE; - } - - // Any motion mode with axis words is allowed to be passed from a spindle speed update. - // NOTE: G1 and G0 without axis words sets axis_command to none. G28/30 are intentionally omitted. - // TODO: Check sync conditions for M3 enabled motions that don't enter the planner. (zero length). - if (axis_words && (axis_command == AXIS_COMMAND_MOTION_MODE)) { - gc_parser_flags |= GC_PARSER_LASER_ISMOTION; - } else { - // M3 constant power laser requires planner syncs to update the laser when changing between - // a G1/2/3 motion mode state and vice versa when there is no motion in the line. - if (gc_state.modal.spindle == SPINDLE_ENABLE_CW) { - if ((gc_state.modal.motion == MOTION_MODE_LINEAR) || (gc_state.modal.motion == MOTION_MODE_CW_ARC) - || (gc_state.modal.motion == MOTION_MODE_CCW_ARC)) { - if (bit_istrue(gc_parser_flags,GC_PARSER_LASER_DISABLE)) { - gc_parser_flags |= GC_PARSER_LASER_FORCE_SYNC; // Change from G1/2/3 motion mode. - } - } else { - // When changing to a G1 motion mode without axis words from a non-G1/2/3 motion mode. - if (bit_isfalse(gc_parser_flags,GC_PARSER_LASER_DISABLE)) { - gc_parser_flags |= GC_PARSER_LASER_FORCE_SYNC; - } - } - } - } - } - - // [0. Non-specific/common error-checks and miscellaneous setup]: - // NOTE: If no line number is present, the value is zero. - gc_state.line_number = gc_block.values.n; - #ifdef USE_LINE_NUMBERS - pl_data->line_number = gc_state.line_number; // Record data for planner use. - #endif - - // [1. Comments feedback ]: NOT SUPPORTED - - // [2. Set feed rate mode ]: - gc_state.modal.feed_rate = gc_block.modal.feed_rate; - if (gc_state.modal.feed_rate) { pl_data->condition |= PL_COND_FLAG_INVERSE_TIME; } // Set condition flag for planner use. - - // [3. Set feed rate ]: - gc_state.feed_rate = gc_block.values.f; // Always copy this value. See feed rate error-checking. - pl_data->feed_rate = gc_state.feed_rate; // Record data for planner use. - - // [4. Set spindle speed ]: - if ((gc_state.spindle_speed != gc_block.values.s) || bit_istrue(gc_parser_flags,GC_PARSER_LASER_FORCE_SYNC)) { - if (gc_state.modal.spindle != SPINDLE_DISABLE) { - #ifdef VARIABLE_SPINDLE - if (bit_isfalse(gc_parser_flags,GC_PARSER_LASER_ISMOTION)) { - if (bit_istrue(gc_parser_flags,GC_PARSER_LASER_DISABLE)) { - spindle_sync(gc_state.modal.spindle, 0.0); - } else { spindle_sync(gc_state.modal.spindle, gc_block.values.s); } + break; } - #else - spindle_sync(gc_state.modal.spindle, 0.0); - #endif } - gc_state.spindle_speed = gc_block.values.s; // Update spindle speed state. - } - // NOTE: Pass zero spindle speed for all restricted laser motions. - if (bit_isfalse(gc_parser_flags,GC_PARSER_LASER_DISABLE)) { - pl_data->spindle_speed = gc_state.spindle_speed; // Record data for planner use. - } // else { pl_data->spindle_speed = 0.0; } // Initialized as zero already. - - // [5. Select tool ]: NOT SUPPORTED. Only tracks tool value. - gc_state.tool = gc_block.values.t; - // [6. Change tool ]: NOT SUPPORTED + // [20. Motion modes ]: + if (gc_block.modal.motion == MOTION_MODE_NONE) { + // [G80 Errors]: Axis word are programmed while G80 is active. + // NOTE: Even non-modal commands or TLO that use axis words will throw this strict error. + if (axis_words) { + FAIL(STATUS_GCODE_AXIS_WORDS_EXIST); + } // [No axis words allowed] - // [7. Spindle control ]: - if (gc_state.modal.spindle != gc_block.modal.spindle) { - // Update spindle control and apply spindle speed when enabling it in this block. - // NOTE: All spindle state changes are synced, even in laser mode. Also, pl_data, - // rather than gc_state, is used to manage laser state for non-laser motions. - spindle_sync(gc_block.modal.spindle, pl_data->spindle_speed); - gc_state.modal.spindle = gc_block.modal.spindle; - } - pl_data->condition |= gc_state.modal.spindle; // Set condition flag for planner use. + // Check remaining motion modes, if axis word are implicit (exist and not used by G10/28/30/92), or + // was explicitly commanded in the g-code block. + } else if (axis_command == AXIS_COMMAND_MOTION_MODE) { - // [8. Coolant control ]: - if (gc_state.modal.coolant != gc_block.modal.coolant) { - // 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); - gc_state.modal.coolant = gc_block.modal.coolant; - } - pl_data->condition |= gc_state.modal.coolant; // Set condition flag for planner use. + if (gc_block.modal.motion == MOTION_MODE_SEEK) { + // [G0 Errors]: Axis letter not configured or without real value (done.) + // Axis words are optional. If missing, set axis command flag to ignore execution. + if (!axis_words) { + axis_command = AXIS_COMMAND_NONE; + } - // [9. Override control ]: NOT SUPPORTED. Always enabled. Except for a Grbl-only parking control. - #ifdef ENABLE_PARKING_OVERRIDE_CONTROL + // All remaining motion modes (all but G0 and G80), require a valid feed rate value. In units per mm mode, + // the value must be positive. In inverse time mode, a positive value must be passed with each block. + } else { + // Check if feed rate is defined for the motion modes that require it. + if (gc_block.values.f == 0.0) { + FAIL(STATUS_GCODE_UNDEFINED_FEED_RATE); + } // [Feed rate undefined] + + switch (gc_block.modal.motion) { + case MOTION_MODE_LINEAR: + // [G1 Errors]: Feed rate undefined. Axis letter not configured or without real value. + // Axis words are optional. If missing, set axis command flag to ignore execution. + if (!axis_words) { + axis_command = AXIS_COMMAND_NONE; + } + break; + case MOTION_MODE_CW_ARC: gc_parser_flags |= GC_PARSER_ARC_IS_CLOCKWISE; // No break intentional. + case MOTION_MODE_CCW_ARC: + // [G2/3 Errors All-Modes]: Feed rate undefined. + // [G2/3 Radius-Mode Errors]: No axis words in selected plane. Target point is same as current. + // [G2/3 Offset-Mode Errors]: No axis words and/or offsets in selected plane. The radius to the current + // point and the radius to the target point differs more than 0.002mm (EMC def. 0.5mm OR 0.005mm and + // 0.1% radius). + // [G2/3 Full-Circle-Mode Errors]: NOT SUPPORTED. Axis words exist. No offsets programmed. P must be an + // integer. NOTE: Both radius and offsets are required for arc tracing and are pre-computed with the + // error-checking. + + if (!axis_words) { + FAIL(STATUS_GCODE_NO_AXIS_WORDS); + } // [No axis words] + if (!(axis_words & (bit(axis_0) | bit(axis_1)))) { + FAIL(STATUS_GCODE_NO_AXIS_WORDS_IN_PLANE); + } // [No axis words in plane] + + // Calculate the change in position along each selected axis + float x, y; + x = gc_block.values.xyz[axis_0] - + gc_state.position[axis_0]; // Delta x between current position and target + y = gc_block.values.xyz[axis_1] - + gc_state.position[axis_1]; // Delta y between current position and target + + if (value_words & bit(WORD_R)) { // Arc Radius Mode + bit_false(value_words, bit(WORD_R)); + if (isequal_position_vector(gc_state.position, gc_block.values.xyz)) { + FAIL(STATUS_GCODE_INVALID_TARGET); + } // [Invalid target] + + // Convert radius value to proper units. + if (gc_block.modal.units == UNITS_MODE_INCHES) { + gc_block.values.r *= MM_PER_INCH; + } + /* We need to calculate the center of the circle that has the designated radius and passes + through both the current position and the target position. This method calculates the following + set of equations where [x,y] is the vector from current to target position, d == magnitude of + that vector, h == hypotenuse of the triangle formed by the radius of the circle, the distance to + the center of the travel vector. A vector perpendicular to the travel vector [-y,x] is scaled to + the length of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] to form + the new point [i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the center of our arc. + + d^2 == x^2 + y^2 + h^2 == r^2 - (d/2)^2 + i == x/2 - y/d*h + j == y/2 + x/d*h + + O <- [i,j] + - | + r - | + - | + - | h + - | + [0,0] -> C -----------------+--------------- T <- [x,y] + | <------ d/2 ---->| + + C - Current position + T - Target position + O - center of circle that pass through both C and T + d - distance from C to T + r - designated radius + h - distance from center of CT to O + + Expanding the equations: + + d -> sqrt(x^2 + y^2) + h -> sqrt(4 * r^2 - x^2 - y^2)/2 + i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 + j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 + + Which can be written: + + i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 + j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 + + Which we for size and speed reasons optimize to: + + h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2) + i = (x - (y * h_x2_div_d))/2 + j = (y + (x * h_x2_div_d))/2 + */ + + // First, use h_x2_div_d to compute 4*h^2 to check if it is negative or r is smaller + // than d. If so, the sqrt of a negative number is complex and error out. + float h_x2_div_d = 4.0 * gc_block.values.r * gc_block.values.r - x * x - y * y; + + if (h_x2_div_d < 0) { + FAIL(STATUS_GCODE_ARC_RADIUS_ERROR); + } // [Arc radius error] + + // Finish computing h_x2_div_d. + h_x2_div_d = -sqrt(h_x2_div_d) / hypot_f(x, y); // == -(h * 2 / d) + // Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below) + if (gc_block.modal.motion == MOTION_MODE_CCW_ARC) { + h_x2_div_d = -h_x2_div_d; + } + + /* The counter clockwise circle lies to the left of the target direction. When offset is positive, + the left hand circle will be generated - when it is negative the right hand circle is generated. + + T <-- Target position + + ^ + Clockwise circles with this center | Clockwise circles with this center + will have will have > 180 deg of angular travel | < 180 deg of angular travel, which + is a good thing! + \ | / + center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d + is negative + | + | + + C <-- Current position + */ + // Negative R is g-code-alese for "I want a circle with more than 180 degrees of travel" (go + // figure!), even though it is advised against ever generating such circles in a single line of + // g-code. By inverting the sign of h_x2_div_d the center of the circles is placed on the opposite + // side of the line of travel and thus we get the unadvisably long arcs as prescribed. + if (gc_block.values.r < 0) { + h_x2_div_d = -h_x2_div_d; + gc_block.values.r = -gc_block.values.r; // Finished with r. Set to positive for mc_arc + } + // Complete the operation by calculating the actual center of the arc + gc_block.values.ijk[axis_0] = 0.5 * (x - (y * h_x2_div_d)); + gc_block.values.ijk[axis_1] = 0.5 * (y + (x * h_x2_div_d)); + + } else { // Arc Center Format Offset Mode + if (!(ijk_words & (bit(axis_0) | bit(axis_1)))) { + FAIL(STATUS_GCODE_NO_OFFSETS_IN_PLANE); + } // [No offsets in plane] + bit_false(value_words, (bit(WORD_I) | bit(WORD_J) | bit(WORD_K))); + + // Convert IJK values to proper units. + if (gc_block.modal.units == UNITS_MODE_INCHES) { + for (idx = 0; idx < N_AXIS; + idx++) { // Axes indices are consistent, so loop may be used to save flash space. + if (ijk_words & bit(idx)) { + gc_block.values.ijk[idx] *= MM_PER_INCH; + } + } + } + + // Arc radius from center to target + x -= gc_block.values.ijk[axis_0]; // Delta x between circle center and target + y -= gc_block.values.ijk[axis_1]; // Delta y between circle center and target + float target_r = hypot_f(x, y); + + // Compute arc radius for mc_arc. Defined from current location to center. + gc_block.values.r = hypot_f(gc_block.values.ijk[axis_0], gc_block.values.ijk[axis_1]); + + // Compute difference between current location and target radii for final error-checks. + float delta_r = fabs(target_r - gc_block.values.r); + if (delta_r > 0.005) { + if (delta_r > 0.5) { + FAIL(STATUS_GCODE_INVALID_TARGET); + } // [Arc definition error] > 0.5mm + if (delta_r > (0.001 * gc_block.values.r)) { + FAIL(STATUS_GCODE_INVALID_TARGET); + } // [Arc definition error] > 0.005mm AND 0.1% radius + } + } + break; + case MOTION_MODE_PROBE_TOWARD_NO_ERROR: + case MOTION_MODE_PROBE_AWAY_NO_ERROR: + gc_parser_flags |= GC_PARSER_PROBE_IS_NO_ERROR; // No break intentional. + case MOTION_MODE_PROBE_TOWARD: + case MOTION_MODE_PROBE_AWAY: + if ((gc_block.modal.motion == MOTION_MODE_PROBE_AWAY) || + (gc_block.modal.motion == MOTION_MODE_PROBE_AWAY_NO_ERROR)) { + gc_parser_flags |= GC_PARSER_PROBE_IS_AWAY; + } + // [G38 Errors]: Target is same current. No axis words. Cutter compensation is enabled. Feed rate + // is undefined. Probe is triggered. NOTE: Probe check moved to probe cycle. Instead of returning + // an error, it issues an alarm to prevent further motion to the probe. It's also done there to + // allow the planner buffer to empty and move off the probe trigger before another probing cycle. + if (!axis_words) { + FAIL(STATUS_GCODE_NO_AXIS_WORDS); + } // [No axis words] + if (isequal_position_vector(gc_state.position, gc_block.values.xyz)) { + FAIL(STATUS_GCODE_INVALID_TARGET); + } // [Invalid target] + break; + } + } + } + + // [21. Program flow ]: No error checks required. + + // [0. Non-specific error-checks]: Complete unused value words check, i.e. IJK used when in arc + // radius mode, or axis words that aren't used in the block. + if (gc_parser_flags & GC_PARSER_JOG_MOTION) { + // Jogging only uses the F feed rate and XYZ value words. N is valid, but S and T are invalid. + bit_false(value_words, (bit(WORD_N) | bit(WORD_F))); + } else { + bit_false(value_words, + (bit(WORD_N) | bit(WORD_F) | bit(WORD_S) | bit(WORD_T))); // Remove single-meaning value words. + } + if (axis_command) { + bit_false(value_words, (bit(WORD_X) | bit(WORD_Y) | bit(WORD_Z))); + } // Remove axis words. + if (value_words) { + FAIL(STATUS_GCODE_UNUSED_WORDS); + } // [Unused words] + + /* ------------------------------------------------------------------------------------- + STEP 4: EXECUTE!! + Assumes that all error-checking has been completed and no failure modes exist. We just + need to update the state and execute the block according to the order-of-execution. + */ + + // Initialize planner data struct for motion blocks. + plan_line_data_t plan_data; + plan_line_data_t *pl_data = &plan_data; + memset(pl_data, 0, sizeof(plan_line_data_t)); // Zero pl_data struct + + // Intercept jog commands and complete error checking for valid jog commands and execute. + // NOTE: G-code parser state is not updated, except the position to ensure sequential jog + // targets are computed correctly. The final parser position after a jog is updated in + // protocol_execute_realtime() when jogging completes or is canceled. + if (gc_parser_flags & GC_PARSER_JOG_MOTION) { + // Only distance and unit modal commands and G53 absolute override command are allowed. + // NOTE: Feed rate word and axis word checks have already been performed in STEP 3. + if (command_words & ~(bit(MODAL_GROUP_G3) | bit(MODAL_GROUP_G6) | bit(MODAL_GROUP_G0))) { + FAIL(STATUS_INVALID_JOG_COMMAND) + }; + if (!(gc_block.non_modal_command == NON_MODAL_ABSOLUTE_OVERRIDE || + gc_block.non_modal_command == NON_MODAL_NO_ACTION)) { + FAIL(STATUS_INVALID_JOG_COMMAND); + } + + // Initialize planner data to current spindle and coolant modal state. + pl_data->spindle_speed = gc_state.spindle_speed; + plan_data.condition = (gc_state.modal.spindle | gc_state.modal.coolant); + + uint8_t status = jog_execute(&plan_data, &gc_block); + if (status == STATUS_OK) { + memcpy(gc_state.position, gc_block.values.xyz, sizeof(gc_block.values.xyz)); + } + return (status); + } + + // If in laser mode, setup laser power based on current and past parser conditions. + if (bit_istrue(settings.flags, BITFLAG_LASER_MODE)) { + if (!((gc_block.modal.motion == MOTION_MODE_LINEAR) || (gc_block.modal.motion == MOTION_MODE_CW_ARC) || + (gc_block.modal.motion == MOTION_MODE_CCW_ARC))) { + gc_parser_flags |= GC_PARSER_LASER_DISABLE; + } + + // Any motion mode with axis words is allowed to be passed from a spindle speed update. + // NOTE: G1 and G0 without axis words sets axis_command to none. G28/30 are intentionally omitted. + // TODO: Check sync conditions for M3 enabled motions that don't enter the planner. (zero length). + if (axis_words && (axis_command == AXIS_COMMAND_MOTION_MODE)) { + gc_parser_flags |= GC_PARSER_LASER_ISMOTION; + } else { + // M3 constant power laser requires planner syncs to update the laser when changing between + // a G1/2/3 motion mode state and vice versa when there is no motion in the line. + if (gc_state.modal.spindle == SPINDLE_ENABLE_CW) { + if ((gc_state.modal.motion == MOTION_MODE_LINEAR) || (gc_state.modal.motion == MOTION_MODE_CW_ARC) || + (gc_state.modal.motion == MOTION_MODE_CCW_ARC)) { + if (bit_istrue(gc_parser_flags, GC_PARSER_LASER_DISABLE)) { + gc_parser_flags |= GC_PARSER_LASER_FORCE_SYNC; // Change from G1/2/3 motion mode. + } + } else { + // When changing to a G1 motion mode without axis words from a non-G1/2/3 motion mode. + if (bit_isfalse(gc_parser_flags, GC_PARSER_LASER_DISABLE)) { + gc_parser_flags |= GC_PARSER_LASER_FORCE_SYNC; + } + } + } + } + } + + // [0. Non-specific/common error-checks and miscellaneous setup]: + // NOTE: If no line number is present, the value is zero. + gc_state.line_number = gc_block.values.n; +#ifdef USE_LINE_NUMBERS + pl_data->line_number = gc_state.line_number; // Record data for planner use. +#endif + + // [1. Comments feedback ]: NOT SUPPORTED + + // [2. Set feed rate mode ]: + gc_state.modal.feed_rate = gc_block.modal.feed_rate; + if (gc_state.modal.feed_rate) { + pl_data->condition |= PL_COND_FLAG_INVERSE_TIME; + } // Set condition flag for planner use. + + // [3. Set feed rate ]: + gc_state.feed_rate = gc_block.values.f; // Always copy this value. See feed rate error-checking. + pl_data->feed_rate = gc_state.feed_rate; // Record data for planner use. + + // [4. Set spindle speed ]: + if ((gc_state.spindle_speed != gc_block.values.s) || bit_istrue(gc_parser_flags, GC_PARSER_LASER_FORCE_SYNC)) { + if (gc_state.modal.spindle != SPINDLE_DISABLE) { +#ifdef VARIABLE_SPINDLE + if (bit_isfalse(gc_parser_flags, GC_PARSER_LASER_ISMOTION)) { + if (bit_istrue(gc_parser_flags, GC_PARSER_LASER_DISABLE)) { + spindle_sync(gc_state.modal.spindle, 0.0); + } else { + spindle_sync(gc_state.modal.spindle, gc_block.values.s); + } + } +#else + spindle_sync(gc_state.modal.spindle, 0.0); +#endif + } + gc_state.spindle_speed = gc_block.values.s; // Update spindle speed state. + } + // NOTE: Pass zero spindle speed for all restricted laser motions. + if (bit_isfalse(gc_parser_flags, GC_PARSER_LASER_DISABLE)) { + pl_data->spindle_speed = gc_state.spindle_speed; // Record data for planner use. + } // else { pl_data->spindle_speed = 0.0; } // Initialized as zero already. + + // [5. Select tool ]: NOT SUPPORTED. Only tracks tool value. + gc_state.tool = gc_block.values.t; + + // [6. Change tool ]: NOT SUPPORTED + + // [7. Spindle control ]: + if (gc_state.modal.spindle != gc_block.modal.spindle) { + // Update spindle control and apply spindle speed when enabling it in this block. + // NOTE: All spindle state changes are synced, even in laser mode. Also, pl_data, + // rather than gc_state, is used to manage laser state for non-laser motions. + spindle_sync(gc_block.modal.spindle, pl_data->spindle_speed); + gc_state.modal.spindle = gc_block.modal.spindle; + } + pl_data->condition |= gc_state.modal.spindle; // Set condition flag for planner use. + + // [8. Coolant control ]: + if (gc_state.modal.coolant != gc_block.modal.coolant) { + // 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); + gc_state.modal.coolant = gc_block.modal.coolant; + } + pl_data->condition |= gc_state.modal.coolant; // Set condition flag for planner use. + +// [9. Override control ]: NOT SUPPORTED. Always enabled. Except for a Grbl-only parking control. +#ifdef ENABLE_PARKING_OVERRIDE_CONTROL if (gc_state.modal.override != gc_block.modal.override) { - gc_state.modal.override = gc_block.modal.override; - mc_override_ctrl_update(gc_state.modal.override); + gc_state.modal.override = gc_block.modal.override; + mc_override_ctrl_update(gc_state.modal.override); } - #endif +#endif - // [10. Dwell ]: - if (gc_block.non_modal_command == NON_MODAL_DWELL) { mc_dwell(gc_block.values.p); } - - // [11. Set active plane ]: - gc_state.modal.plane_select = gc_block.modal.plane_select; - - // [12. Set length units ]: - gc_state.modal.units = gc_block.modal.units; - - // [13. Cutter radius compensation ]: G41/42 NOT SUPPORTED - // gc_state.modal.cutter_comp = gc_block.modal.cutter_comp; // NOTE: Not needed since always disabled. - - // [14. Cutter length compensation ]: G43.1 and G49 supported. G43 NOT SUPPORTED. - // NOTE: If G43 were supported, its operation wouldn't be any different from G43.1 in terms - // of execution. The error-checking step would simply load the offset value into the correct - // axis of the block XYZ value array. - if (axis_command == AXIS_COMMAND_TOOL_LENGTH_OFFSET ) { // Indicates a change. - gc_state.modal.tool_length = gc_block.modal.tool_length; - if (gc_state.modal.tool_length == TOOL_LENGTH_OFFSET_CANCEL) { // G49 - gc_block.values.xyz[TOOL_LENGTH_OFFSET_AXIS] = 0.0; - } // else G43.1 - if ( gc_state.tool_length_offset != gc_block.values.xyz[TOOL_LENGTH_OFFSET_AXIS] ) { - gc_state.tool_length_offset = gc_block.values.xyz[TOOL_LENGTH_OFFSET_AXIS]; - system_flag_wco_change(); + // [10. Dwell ]: + if (gc_block.non_modal_command == NON_MODAL_DWELL) { + mc_dwell(gc_block.values.p); } - } - // [15. Coordinate system selection ]: - if (gc_state.modal.coord_select != gc_block.modal.coord_select) { - gc_state.modal.coord_select = gc_block.modal.coord_select; - memcpy(gc_state.coord_system,block_coord_system,N_AXIS*sizeof(float)); - system_flag_wco_change(); - } + // [11. Set active plane ]: + gc_state.modal.plane_select = gc_block.modal.plane_select; - // [16. Set path control mode ]: G61.1/G64 NOT SUPPORTED - // gc_state.modal.control = gc_block.modal.control; // NOTE: Always default. + // [12. Set length units ]: + gc_state.modal.units = gc_block.modal.units; - // [17. Set distance mode ]: - gc_state.modal.distance = gc_block.modal.distance; + // [13. Cutter radius compensation ]: G41/42 NOT SUPPORTED + // gc_state.modal.cutter_comp = gc_block.modal.cutter_comp; // NOTE: Not needed since always disabled. - // [18. Set retract mode ]: NOT SUPPORTED + // [14. Cutter length compensation ]: G43.1 and G49 supported. G43 NOT SUPPORTED. + // NOTE: If G43 were supported, its operation wouldn't be any different from G43.1 in terms + // of execution. The error-checking step would simply load the offset value into the correct + // axis of the block XYZ value array. + if (axis_command == AXIS_COMMAND_TOOL_LENGTH_OFFSET) { // Indicates a change. + gc_state.modal.tool_length = gc_block.modal.tool_length; + if (gc_state.modal.tool_length == TOOL_LENGTH_OFFSET_CANCEL) { // G49 + gc_block.values.xyz[TOOL_LENGTH_OFFSET_AXIS] = 0.0; + } // else G43.1 + if (gc_state.tool_length_offset != gc_block.values.xyz[TOOL_LENGTH_OFFSET_AXIS]) { + gc_state.tool_length_offset = gc_block.values.xyz[TOOL_LENGTH_OFFSET_AXIS]; + system_flag_wco_change(); + } + } - // [19. Go to predefined position, Set G10, or Set axis offsets ]: - switch(gc_block.non_modal_command) { - case NON_MODAL_SET_COORDINATE_DATA: - settings_write_coord_data(coord_select,gc_block.values.ijk); - // Update system coordinate system if currently active. - if (gc_state.modal.coord_select == coord_select) { - memcpy(gc_state.coord_system,gc_block.values.ijk,N_AXIS*sizeof(float)); + // [15. Coordinate system selection ]: + if (gc_state.modal.coord_select != gc_block.modal.coord_select) { + gc_state.modal.coord_select = gc_block.modal.coord_select; + memcpy(gc_state.coord_system, block_coord_system, N_AXIS * sizeof(float)); system_flag_wco_change(); - } - break; - case NON_MODAL_GO_HOME_0: case NON_MODAL_GO_HOME_1: - // Move to intermediate position before going home. Obeys current coordinate system and offsets - // and absolute and incremental modes. - pl_data->condition |= PL_COND_FLAG_RAPID_MOTION; // Set rapid motion condition flag. - if (axis_command) { mc_line(gc_block.values.xyz, pl_data); } - mc_line(gc_block.values.ijk, pl_data); - memcpy(gc_state.position, gc_block.values.ijk, N_AXIS*sizeof(float)); - break; - case NON_MODAL_SET_HOME_0: - settings_write_coord_data(SETTING_INDEX_G28,gc_state.position); - break; - case NON_MODAL_SET_HOME_1: - settings_write_coord_data(SETTING_INDEX_G30,gc_state.position); - break; - case NON_MODAL_SET_COORDINATE_OFFSET: - memcpy(gc_state.coord_offset,gc_block.values.xyz,sizeof(gc_block.values.xyz)); - system_flag_wco_change(); - break; - case NON_MODAL_RESET_COORDINATE_OFFSET: - clear_vector(gc_state.coord_offset); // Disable G92 offsets by zeroing offset vector. - system_flag_wco_change(); - break; - } - - - // [20. Motion modes ]: - // NOTE: Commands G10,G28,G30,G92 lock out and prevent axis words from use in motion modes. - // Enter motion modes only if there are axis words or a motion mode command word in the block. - gc_state.modal.motion = gc_block.modal.motion; - if (gc_state.modal.motion != MOTION_MODE_NONE) { - if (axis_command == AXIS_COMMAND_MOTION_MODE) { - uint8_t gc_update_pos = GC_UPDATE_POS_TARGET; - if (gc_state.modal.motion == MOTION_MODE_LINEAR) { - mc_line(gc_block.values.xyz, pl_data); - } else if (gc_state.modal.motion == MOTION_MODE_SEEK) { - pl_data->condition |= PL_COND_FLAG_RAPID_MOTION; // Set rapid motion condition flag. - mc_line(gc_block.values.xyz, pl_data); - } else if ((gc_state.modal.motion == MOTION_MODE_CW_ARC) || (gc_state.modal.motion == MOTION_MODE_CCW_ARC)) { - mc_arc(gc_block.values.xyz, pl_data, gc_state.position, gc_block.values.ijk, gc_block.values.r, - axis_0, axis_1, axis_linear, bit_istrue(gc_parser_flags,GC_PARSER_ARC_IS_CLOCKWISE)); - } else { - // NOTE: gc_block.values.xyz is returned from mc_probe_cycle with the updated position value. So - // upon a successful probing cycle, the machine position and the returned value should be the same. - #ifndef ALLOW_FEED_OVERRIDE_DURING_PROBE_CYCLES - pl_data->condition |= PL_COND_FLAG_NO_FEED_OVERRIDE; - #endif - gc_update_pos = mc_probe_cycle(gc_block.values.xyz, pl_data, gc_parser_flags); - } - - // As far as the parser is concerned, the position is now == target. In reality the - // motion control system might still be processing the action and the real tool position - // in any intermediate location. - if (gc_update_pos == GC_UPDATE_POS_TARGET) { - memcpy(gc_state.position, gc_block.values.xyz, sizeof(gc_block.values.xyz)); // gc_state.position[] = gc_block.values.xyz[] - } else if (gc_update_pos == GC_UPDATE_POS_SYSTEM) { - gc_sync_position(); // gc_state.position[] = sys_position - } // == GC_UPDATE_POS_NONE - } - } - - // [21. Program flow ]: - // M0,M1,M2,M30: Perform non-running program flow actions. During a program pause, the buffer may - // refill and can only be resumed by the cycle start run-time command. - gc_state.modal.program_flow = gc_block.modal.program_flow; - if (gc_state.modal.program_flow) { - protocol_buffer_synchronize(); // Sync and finish all remaining buffered motions before moving on. - if (gc_state.modal.program_flow == PROGRAM_FLOW_PAUSED) { - if (sys.state != STATE_CHECK_MODE) { - system_set_exec_state_flag(EXEC_FEED_HOLD); // Use feed hold for program pause. - protocol_execute_realtime(); // Execute suspend. - } - } else { // == PROGRAM_FLOW_COMPLETED - // Upon program complete, only a subset of g-codes reset to certain defaults, according to - // LinuxCNC's program end descriptions and testing. Only modal groups [G-code 1,2,3,5,7,12] - // and [M-code 7,8,9] reset to [G1,G17,G90,G94,G40,G54,M5,M9,M48]. The remaining modal groups - // [G-code 4,6,8,10,13,14,15] and [M-code 4,5,6] and the modal words [F,S,T,H] do not reset. - gc_state.modal.motion = MOTION_MODE_LINEAR; - gc_state.modal.plane_select = PLANE_SELECT_XY; - gc_state.modal.distance = DISTANCE_MODE_ABSOLUTE; - gc_state.modal.feed_rate = FEED_RATE_MODE_UNITS_PER_MIN; - // gc_state.modal.cutter_comp = CUTTER_COMP_DISABLE; // Not supported. - gc_state.modal.coord_select = 0; // G54 - gc_state.modal.spindle = SPINDLE_DISABLE; - gc_state.modal.coolant = COOLANT_DISABLE; - #ifdef ENABLE_PARKING_OVERRIDE_CONTROL - #ifdef DEACTIVATE_PARKING_UPON_INIT - gc_state.modal.override = OVERRIDE_DISABLED; - #else - gc_state.modal.override = OVERRIDE_PARKING_MOTION; - #endif - #endif - - #ifdef RESTORE_OVERRIDES_AFTER_PROGRAM_END - sys.f_override = DEFAULT_FEED_OVERRIDE; - sys.r_override = DEFAULT_RAPID_OVERRIDE; - sys.spindle_speed_ovr = DEFAULT_SPINDLE_SPEED_OVERRIDE; - #endif - - // Execute coordinate change and spindle/coolant stop. - if (sys.state != STATE_CHECK_MODE) { - if (!(settings_read_coord_data(gc_state.modal.coord_select,gc_state.coord_system))) { FAIL(STATUS_SETTING_READ_FAIL); } - system_flag_wco_change(); // Set to refresh immediately just in case something altered. - spindle_set_state(SPINDLE_DISABLE,0.0); - coolant_set_state(COOLANT_DISABLE); - } - report_feedback_message(MESSAGE_PROGRAM_END); } - gc_state.modal.program_flow = PROGRAM_FLOW_RUNNING; // Reset program flow. - } - // TODO: % to denote start of program. + // [16. Set path control mode ]: G61.1/G64 NOT SUPPORTED + // gc_state.modal.control = gc_block.modal.control; // NOTE: Always default. - return(STATUS_OK); + // [17. Set distance mode ]: + gc_state.modal.distance = gc_block.modal.distance; + + // [18. Set retract mode ]: NOT SUPPORTED + + // [19. Go to predefined position, Set G10, or Set axis offsets ]: + switch (gc_block.non_modal_command) { + case NON_MODAL_SET_COORDINATE_DATA: + settings_write_coord_data(coord_select, gc_block.values.ijk); + // Update system coordinate system if currently active. + if (gc_state.modal.coord_select == coord_select) { + memcpy(gc_state.coord_system, gc_block.values.ijk, N_AXIS * sizeof(float)); + system_flag_wco_change(); + } + break; + case NON_MODAL_GO_HOME_0: + case NON_MODAL_GO_HOME_1: + // Move to intermediate position before going home. Obeys current coordinate system and offsets + // and absolute and incremental modes. + pl_data->condition |= PL_COND_FLAG_RAPID_MOTION; // Set rapid motion condition flag. + if (axis_command) { + mc_line(gc_block.values.xyz, pl_data); + } + mc_line(gc_block.values.ijk, pl_data); + memcpy(gc_state.position, gc_block.values.ijk, N_AXIS * sizeof(float)); + break; + case NON_MODAL_SET_HOME_0: settings_write_coord_data(SETTING_INDEX_G28, gc_state.position); break; + case NON_MODAL_SET_HOME_1: settings_write_coord_data(SETTING_INDEX_G30, gc_state.position); break; + case NON_MODAL_SET_COORDINATE_OFFSET: + memcpy(gc_state.coord_offset, gc_block.values.xyz, sizeof(gc_block.values.xyz)); + system_flag_wco_change(); + break; + case NON_MODAL_RESET_COORDINATE_OFFSET: + clear_vector(gc_state.coord_offset); // Disable G92 offsets by zeroing offset vector. + system_flag_wco_change(); + break; + } + + // [20. Motion modes ]: + // NOTE: Commands G10,G28,G30,G92 lock out and prevent axis words from use in motion modes. + // Enter motion modes only if there are axis words or a motion mode command word in the block. + gc_state.modal.motion = gc_block.modal.motion; + if (gc_state.modal.motion != MOTION_MODE_NONE) { + if (axis_command == AXIS_COMMAND_MOTION_MODE) { + uint8_t gc_update_pos = GC_UPDATE_POS_TARGET; + if (gc_state.modal.motion == MOTION_MODE_LINEAR) { + mc_line(gc_block.values.xyz, pl_data); + } else if (gc_state.modal.motion == MOTION_MODE_SEEK) { + pl_data->condition |= PL_COND_FLAG_RAPID_MOTION; // Set rapid motion condition flag. + mc_line(gc_block.values.xyz, pl_data); + } else if ((gc_state.modal.motion == MOTION_MODE_CW_ARC) || + (gc_state.modal.motion == MOTION_MODE_CCW_ARC)) { + mc_arc(gc_block.values.xyz, pl_data, gc_state.position, gc_block.values.ijk, gc_block.values.r, axis_0, + axis_1, axis_linear, bit_istrue(gc_parser_flags, GC_PARSER_ARC_IS_CLOCKWISE)); + } else { +// NOTE: gc_block.values.xyz is returned from mc_probe_cycle with the updated position value. So +// upon a successful probing cycle, the machine position and the returned value should be the same. +#ifndef ALLOW_FEED_OVERRIDE_DURING_PROBE_CYCLES + pl_data->condition |= PL_COND_FLAG_NO_FEED_OVERRIDE; +#endif + gc_update_pos = mc_probe_cycle(gc_block.values.xyz, pl_data, gc_parser_flags); + } + + // As far as the parser is concerned, the position is now == target. In reality the + // motion control system might still be processing the action and the real tool position + // in any intermediate location. + if (gc_update_pos == GC_UPDATE_POS_TARGET) { + memcpy(gc_state.position, gc_block.values.xyz, + sizeof(gc_block.values.xyz)); // gc_state.position[] = gc_block.values.xyz[] + } else if (gc_update_pos == GC_UPDATE_POS_SYSTEM) { + gc_sync_position(); // gc_state.position[] = sys_position + } // == GC_UPDATE_POS_NONE + } + } + + // [21. Program flow ]: + // M0,M1,M2,M30: Perform non-running program flow actions. During a program pause, the buffer may + // refill and can only be resumed by the cycle start run-time command. + gc_state.modal.program_flow = gc_block.modal.program_flow; + if (gc_state.modal.program_flow) { + protocol_buffer_synchronize(); // Sync and finish all remaining buffered motions before moving on. + if (gc_state.modal.program_flow == PROGRAM_FLOW_PAUSED) { + if (sys.state != STATE_CHECK_MODE) { + system_set_exec_state_flag(EXEC_FEED_HOLD); // Use feed hold for program pause. + protocol_execute_realtime(); // Execute suspend. + } + } else { // == PROGRAM_FLOW_COMPLETED + // Upon program complete, only a subset of g-codes reset to certain defaults, according to + // LinuxCNC's program end descriptions and testing. Only modal groups [G-code 1,2,3,5,7,12] + // and [M-code 7,8,9] reset to [G1,G17,G90,G94,G40,G54,M5,M9,M48]. The remaining modal groups + // [G-code 4,6,8,10,13,14,15] and [M-code 4,5,6] and the modal words [F,S,T,H] do not reset. + gc_state.modal.motion = MOTION_MODE_LINEAR; + gc_state.modal.plane_select = PLANE_SELECT_XY; + gc_state.modal.distance = DISTANCE_MODE_ABSOLUTE; + gc_state.modal.feed_rate = FEED_RATE_MODE_UNITS_PER_MIN; + // gc_state.modal.cutter_comp = CUTTER_COMP_DISABLE; // Not supported. + gc_state.modal.coord_select = 0; // G54 + gc_state.modal.spindle = SPINDLE_DISABLE; + gc_state.modal.coolant = COOLANT_DISABLE; +#ifdef ENABLE_PARKING_OVERRIDE_CONTROL +#ifdef DEACTIVATE_PARKING_UPON_INIT + gc_state.modal.override = OVERRIDE_DISABLED; +#else + gc_state.modal.override = OVERRIDE_PARKING_MOTION; +#endif +#endif + +#ifdef RESTORE_OVERRIDES_AFTER_PROGRAM_END + sys.f_override = DEFAULT_FEED_OVERRIDE; + sys.r_override = DEFAULT_RAPID_OVERRIDE; + sys.spindle_speed_ovr = DEFAULT_SPINDLE_SPEED_OVERRIDE; +#endif + + // Execute coordinate change and spindle/coolant stop. + if (sys.state != STATE_CHECK_MODE) { + if (!(settings_read_coord_data(gc_state.modal.coord_select, gc_state.coord_system))) { + FAIL(STATUS_SETTING_READ_FAIL); + } + system_flag_wco_change(); // Set to refresh immediately just in case something altered. + spindle_set_state(SPINDLE_DISABLE, 0.0); + coolant_set_state(COOLANT_DISABLE); + } + report_feedback_message(MESSAGE_PROGRAM_END); + } + gc_state.modal.program_flow = PROGRAM_FLOW_RUNNING; // Reset program flow. + } + + // TODO: % to denote start of program. + + return (STATUS_OK); } - /* Not supported: diff --git a/grbl/gcode.h b/grbl/gcode.h index 6cdc61b..00b1347 100644 --- a/grbl/gcode.h +++ b/grbl/gcode.h @@ -22,59 +22,58 @@ #ifndef gcode_h #define gcode_h - // Define modal group internal numbers for checking multiple command violations and tracking the // type of command that is called in the block. A modal group is a group of g-code commands that are // mutually exclusive, or cannot exist on the same line, because they each toggle a state or execute // a unique motion. These are defined in the NIST RS274-NGC v3 g-code standard, available online, // and are similar/identical to other g-code interpreters by manufacturers (Haas,Fanuc,Mazak,etc). // NOTE: Modal group define values must be sequential and starting from zero. -#define MODAL_GROUP_G0 0 // [G4,G10,G28,G28.1,G30,G30.1,G53,G92,G92.1] Non-modal -#define MODAL_GROUP_G1 1 // [G0,G1,G2,G3,G38.2,G38.3,G38.4,G38.5,G80] Motion -#define MODAL_GROUP_G2 2 // [G17,G18,G19] Plane selection -#define MODAL_GROUP_G3 3 // [G90,G91] Distance mode -#define MODAL_GROUP_G4 4 // [G91.1] Arc IJK distance mode -#define MODAL_GROUP_G5 5 // [G93,G94] Feed rate mode -#define MODAL_GROUP_G6 6 // [G20,G21] Units -#define MODAL_GROUP_G7 7 // [G40] Cutter radius compensation mode. G41/42 NOT SUPPORTED. -#define MODAL_GROUP_G8 8 // [G43.1,G49] Tool length offset -#define MODAL_GROUP_G12 9 // [G54,G55,G56,G57,G58,G59] Coordinate system selection +#define MODAL_GROUP_G0 0 // [G4,G10,G28,G28.1,G30,G30.1,G53,G92,G92.1] Non-modal +#define MODAL_GROUP_G1 1 // [G0,G1,G2,G3,G38.2,G38.3,G38.4,G38.5,G80] Motion +#define MODAL_GROUP_G2 2 // [G17,G18,G19] Plane selection +#define MODAL_GROUP_G3 3 // [G90,G91] Distance mode +#define MODAL_GROUP_G4 4 // [G91.1] Arc IJK distance mode +#define MODAL_GROUP_G5 5 // [G93,G94] Feed rate mode +#define MODAL_GROUP_G6 6 // [G20,G21] Units +#define MODAL_GROUP_G7 7 // [G40] Cutter radius compensation mode. G41/42 NOT SUPPORTED. +#define MODAL_GROUP_G8 8 // [G43.1,G49] Tool length offset +#define MODAL_GROUP_G12 9 // [G54,G55,G56,G57,G58,G59] Coordinate system selection #define MODAL_GROUP_G13 10 // [G61] Control mode -#define MODAL_GROUP_M4 11 // [M0,M1,M2,M30] Stopping +#define MODAL_GROUP_M4 11 // [M0,M1,M2,M30] Stopping #define MODAL_GROUP_M7 12 // [M3,M4,M5] Spindle turning #define MODAL_GROUP_M8 13 // [M7,M8,M9] Coolant control #define MODAL_GROUP_M9 14 // [M56] Override control // Define command actions for within execution-type modal groups (motion, stopping, non-modal). Used // internally by the parser to know which command to execute. -// NOTE: Some macro values are assigned specific values to make g-code state reporting and parsing +// NOTE: Some macro values are assigned specific values to make g-code state reporting and parsing // compile a litte smaller. Necessary due to being completely out of flash on the 328p. Although not -// ideal, just be careful with values that state 'do not alter' and check both report.c and gcode.c +// ideal, just be careful with values that state 'do not alter' and check both report.c and gcode.c // to see how they are used, if you need to alter them. // Modal Group G0: Non-modal actions -#define NON_MODAL_NO_ACTION 0 // (Default: Must be zero) -#define NON_MODAL_DWELL 4 // G4 (Do not alter value) -#define NON_MODAL_SET_COORDINATE_DATA 10 // G10 (Do not alter value) -#define NON_MODAL_GO_HOME_0 28 // G28 (Do not alter value) -#define NON_MODAL_SET_HOME_0 38 // G28.1 (Do not alter value) -#define NON_MODAL_GO_HOME_1 30 // G30 (Do not alter value) -#define NON_MODAL_SET_HOME_1 40 // G30.1 (Do not alter value) -#define NON_MODAL_ABSOLUTE_OVERRIDE 53 // G53 (Do not alter value) -#define NON_MODAL_SET_COORDINATE_OFFSET 92 // G92 (Do not alter value) -#define NON_MODAL_RESET_COORDINATE_OFFSET 102 //G92.1 (Do not alter value) +#define NON_MODAL_NO_ACTION 0 // (Default: Must be zero) +#define NON_MODAL_DWELL 4 // G4 (Do not alter value) +#define NON_MODAL_SET_COORDINATE_DATA 10 // G10 (Do not alter value) +#define NON_MODAL_GO_HOME_0 28 // G28 (Do not alter value) +#define NON_MODAL_SET_HOME_0 38 // G28.1 (Do not alter value) +#define NON_MODAL_GO_HOME_1 30 // G30 (Do not alter value) +#define NON_MODAL_SET_HOME_1 40 // G30.1 (Do not alter value) +#define NON_MODAL_ABSOLUTE_OVERRIDE 53 // G53 (Do not alter value) +#define NON_MODAL_SET_COORDINATE_OFFSET 92 // G92 (Do not alter value) +#define NON_MODAL_RESET_COORDINATE_OFFSET 102 // G92.1 (Do not alter value) // Modal Group G1: Motion modes -#define MOTION_MODE_SEEK 0 // G0 (Default: Must be zero) -#define MOTION_MODE_LINEAR 1 // G1 (Do not alter value) -#define MOTION_MODE_CW_ARC 2 // G2 (Do not alter value) -#define MOTION_MODE_CCW_ARC 3 // G3 (Do not alter value) -#define MOTION_MODE_PROBE_TOWARD 140 // G38.2 (Do not alter value) +#define MOTION_MODE_SEEK 0 // G0 (Default: Must be zero) +#define MOTION_MODE_LINEAR 1 // G1 (Do not alter value) +#define MOTION_MODE_CW_ARC 2 // G2 (Do not alter value) +#define MOTION_MODE_CCW_ARC 3 // G3 (Do not alter value) +#define MOTION_MODE_PROBE_TOWARD 140 // G38.2 (Do not alter value) #define MOTION_MODE_PROBE_TOWARD_NO_ERROR 141 // G38.3 (Do not alter value) -#define MOTION_MODE_PROBE_AWAY 142 // G38.4 (Do not alter value) -#define MOTION_MODE_PROBE_AWAY_NO_ERROR 143 // G38.5 (Do not alter value) -#define MOTION_MODE_NONE 80 // G80 (Do not alter value) +#define MOTION_MODE_PROBE_AWAY 142 // G38.4 (Do not alter value) +#define MOTION_MODE_PROBE_AWAY_NO_ERROR 143 // G38.5 (Do not alter value) +#define MOTION_MODE_NONE 80 // G80 (Do not alter value) // Modal Group G2: Plane select #define PLANE_SELECT_XY 0 // G17 (Default: Must be zero) @@ -82,25 +81,25 @@ #define PLANE_SELECT_YZ 2 // G19 (Do not alter value) // Modal Group G3: Distance mode -#define DISTANCE_MODE_ABSOLUTE 0 // G90 (Default: Must be zero) +#define DISTANCE_MODE_ABSOLUTE 0 // G90 (Default: Must be zero) #define DISTANCE_MODE_INCREMENTAL 1 // G91 (Do not alter value) // Modal Group G4: Arc IJK distance mode #define DISTANCE_ARC_MODE_INCREMENTAL 0 // G91.1 (Default: Must be zero) // Modal Group M4: Program flow -#define PROGRAM_FLOW_RUNNING 0 // (Default: Must be zero) -#define PROGRAM_FLOW_PAUSED 3 // M0 -#define PROGRAM_FLOW_OPTIONAL_STOP 1 // M1 NOTE: Not supported, but valid and ignored. -#define PROGRAM_FLOW_COMPLETED_M2 2 // M2 (Do not alter value) +#define PROGRAM_FLOW_RUNNING 0 // (Default: Must be zero) +#define PROGRAM_FLOW_PAUSED 3 // M0 +#define PROGRAM_FLOW_OPTIONAL_STOP 1 // M1 NOTE: Not supported, but valid and ignored. +#define PROGRAM_FLOW_COMPLETED_M2 2 // M2 (Do not alter value) #define PROGRAM_FLOW_COMPLETED_M30 30 // M30 (Do not alter value) // Modal Group G5: Feed rate mode -#define FEED_RATE_MODE_UNITS_PER_MIN 0 // G94 (Default: Must be zero) -#define FEED_RATE_MODE_INVERSE_TIME 1 // G93 (Do not alter value) +#define FEED_RATE_MODE_UNITS_PER_MIN 0 // G94 (Default: Must be zero) +#define FEED_RATE_MODE_INVERSE_TIME 1 // G93 (Do not alter value) // Modal Group G6: Units mode -#define UNITS_MODE_MM 0 // G21 (Default: Must be zero) +#define UNITS_MODE_MM 0 // G21 (Default: Must be zero) #define UNITS_MODE_INCHES 1 // G20 (Do not alter value) // Modal Group G7: Cutter radius compensation mode @@ -110,132 +109,129 @@ #define CONTROL_MODE_EXACT_PATH 0 // G61 (Default: Must be zero) // Modal Group M7: Spindle control -#define SPINDLE_DISABLE 0 // M5 (Default: Must be zero) -#define SPINDLE_ENABLE_CW PL_COND_FLAG_SPINDLE_CW // M3 (NOTE: Uses planner condition bit flag) -#define SPINDLE_ENABLE_CCW PL_COND_FLAG_SPINDLE_CCW // M4 (NOTE: Uses planner condition bit flag) +#define SPINDLE_DISABLE 0 // M5 (Default: Must be zero) +#define SPINDLE_ENABLE_CW PL_COND_FLAG_SPINDLE_CW // M3 (NOTE: Uses planner condition bit flag) +#define SPINDLE_ENABLE_CCW PL_COND_FLAG_SPINDLE_CCW // M4 (NOTE: Uses planner condition bit flag) // Modal Group M8: Coolant control -#define COOLANT_DISABLE 0 // M9 (Default: Must be zero) -#define COOLANT_FLOOD_ENABLE PL_COND_FLAG_COOLANT_FLOOD // M8 (NOTE: Uses planner condition bit flag) -#define COOLANT_MIST_ENABLE PL_COND_FLAG_COOLANT_MIST // M7 (NOTE: Uses planner condition bit flag) +#define COOLANT_DISABLE 0 // M9 (Default: Must be zero) +#define COOLANT_FLOOD_ENABLE PL_COND_FLAG_COOLANT_FLOOD // M8 (NOTE: Uses planner condition bit flag) +#define COOLANT_MIST_ENABLE PL_COND_FLAG_COOLANT_MIST // M7 (NOTE: Uses planner condition bit flag) // Modal Group G8: Tool length offset -#define TOOL_LENGTH_OFFSET_CANCEL 0 // G49 (Default: Must be zero) +#define TOOL_LENGTH_OFFSET_CANCEL 0 // G49 (Default: Must be zero) #define TOOL_LENGTH_OFFSET_ENABLE_DYNAMIC 1 // G43.1 // Modal Group M9: Override control #ifdef DEACTIVATE_PARKING_UPON_INIT - #define OVERRIDE_DISABLED 0 // (Default: Must be zero) - #define OVERRIDE_PARKING_MOTION 1 // M56 +#define OVERRIDE_DISABLED 0 // (Default: Must be zero) +#define OVERRIDE_PARKING_MOTION 1 // M56 #else - #define OVERRIDE_PARKING_MOTION 0 // M56 (Default: Must be zero) - #define OVERRIDE_DISABLED 1 // Parking disabled. +#define OVERRIDE_PARKING_MOTION 0 // M56 (Default: Must be zero) +#define OVERRIDE_DISABLED 1 // Parking disabled. #endif // Modal Group G12: Active work coordinate system // N/A: Stores coordinate system value (54-59) to change to. // Define parameter word mapping. -#define WORD_F 0 -#define WORD_I 1 -#define WORD_J 2 -#define WORD_K 3 -#define WORD_L 4 -#define WORD_N 5 -#define WORD_P 6 -#define WORD_R 7 -#define WORD_S 8 -#define WORD_T 9 -#define WORD_X 10 -#define WORD_Y 11 -#define WORD_Z 12 +#define WORD_F 0 +#define WORD_I 1 +#define WORD_J 2 +#define WORD_K 3 +#define WORD_L 4 +#define WORD_N 5 +#define WORD_P 6 +#define WORD_R 7 +#define WORD_S 8 +#define WORD_T 9 +#define WORD_X 10 +#define WORD_Y 11 +#define WORD_Z 12 // Define g-code parser position updating flags -#define GC_UPDATE_POS_TARGET 0 // Must be zero -#define GC_UPDATE_POS_SYSTEM 1 -#define GC_UPDATE_POS_NONE 2 +#define GC_UPDATE_POS_TARGET 0 // Must be zero +#define GC_UPDATE_POS_SYSTEM 1 +#define GC_UPDATE_POS_NONE 2 // Define probe cycle exit states and assign proper position updating. -#define GC_PROBE_FOUND GC_UPDATE_POS_SYSTEM -#define GC_PROBE_ABORT GC_UPDATE_POS_NONE -#define GC_PROBE_FAIL_INIT GC_UPDATE_POS_NONE -#define GC_PROBE_FAIL_END GC_UPDATE_POS_TARGET +#define GC_PROBE_FOUND GC_UPDATE_POS_SYSTEM +#define GC_PROBE_ABORT GC_UPDATE_POS_NONE +#define GC_PROBE_FAIL_INIT GC_UPDATE_POS_NONE +#define GC_PROBE_FAIL_END GC_UPDATE_POS_TARGET #ifdef SET_CHECK_MODE_PROBE_TO_START - #define GC_PROBE_CHECK_MODE GC_UPDATE_POS_NONE +#define GC_PROBE_CHECK_MODE GC_UPDATE_POS_NONE #else - #define GC_PROBE_CHECK_MODE GC_UPDATE_POS_TARGET +#define GC_PROBE_CHECK_MODE GC_UPDATE_POS_TARGET #endif // Define gcode parser flags for handling special cases. -#define GC_PARSER_NONE 0 // Must be zero. -#define GC_PARSER_JOG_MOTION bit(0) -#define GC_PARSER_CHECK_MANTISSA bit(1) -#define GC_PARSER_ARC_IS_CLOCKWISE bit(2) -#define GC_PARSER_PROBE_IS_AWAY bit(3) -#define GC_PARSER_PROBE_IS_NO_ERROR bit(4) -#define GC_PARSER_LASER_FORCE_SYNC bit(5) -#define GC_PARSER_LASER_DISABLE bit(6) -#define GC_PARSER_LASER_ISMOTION bit(7) - +#define GC_PARSER_NONE 0 // Must be zero. +#define GC_PARSER_JOG_MOTION bit(0) +#define GC_PARSER_CHECK_MANTISSA bit(1) +#define GC_PARSER_ARC_IS_CLOCKWISE bit(2) +#define GC_PARSER_PROBE_IS_AWAY bit(3) +#define GC_PARSER_PROBE_IS_NO_ERROR bit(4) +#define GC_PARSER_LASER_FORCE_SYNC bit(5) +#define GC_PARSER_LASER_DISABLE bit(6) +#define GC_PARSER_LASER_ISMOTION bit(7) // NOTE: When this struct is zeroed, the above defines set the defaults for the system. typedef struct { - uint8_t motion; // {G0,G1,G2,G3,G38.2,G80} - uint8_t feed_rate; // {G93,G94} - uint8_t units; // {G20,G21} - uint8_t distance; // {G90,G91} - // uint8_t distance_arc; // {G91.1} NOTE: Don't track. Only default supported. - uint8_t plane_select; // {G17,G18,G19} - // uint8_t cutter_comp; // {G40} NOTE: Don't track. Only default supported. - uint8_t tool_length; // {G43.1,G49} - uint8_t coord_select; // {G54,G55,G56,G57,G58,G59} - // uint8_t control; // {G61} NOTE: Don't track. Only default supported. - uint8_t program_flow; // {M0,M1,M2,M30} - uint8_t coolant; // {M7,M8,M9} - uint8_t spindle; // {M3,M4,M5} - uint8_t override; // {M56} + uint8_t motion; // {G0,G1,G2,G3,G38.2,G80} + uint8_t feed_rate; // {G93,G94} + uint8_t units; // {G20,G21} + uint8_t distance; // {G90,G91} + // uint8_t distance_arc; // {G91.1} NOTE: Don't track. Only default supported. + uint8_t plane_select; // {G17,G18,G19} + // uint8_t cutter_comp; // {G40} NOTE: Don't track. Only default supported. + uint8_t tool_length; // {G43.1,G49} + uint8_t coord_select; // {G54,G55,G56,G57,G58,G59} + // uint8_t control; // {G61} NOTE: Don't track. Only default supported. + uint8_t program_flow; // {M0,M1,M2,M30} + uint8_t coolant; // {M7,M8,M9} + uint8_t spindle; // {M3,M4,M5} + uint8_t override; // {M56} } gc_modal_t; typedef struct { - float f; // Feed - float ijk[3]; // I,J,K Axis arc offsets - uint8_t l; // G10 or canned cycles parameters - int32_t n; // Line number - float p; // G10 or dwell parameters - // float q; // G82 peck drilling - float r; // Arc radius - float s; // Spindle speed - uint8_t t; // Tool selection - float xyz[3]; // X,Y,Z Translational axes + float f; // Feed + float ijk[3]; // I,J,K Axis arc offsets + uint8_t l; // G10 or canned cycles parameters + int32_t n; // Line number + float p; // G10 or dwell parameters + // float q; // G82 peck drilling + float r; // Arc radius + float s; // Spindle speed + uint8_t t; // Tool selection + float xyz[3]; // X,Y,Z Translational axes } gc_values_t; - typedef struct { - gc_modal_t modal; + gc_modal_t modal; - float spindle_speed; // RPM - float feed_rate; // Millimeters/min - uint8_t tool; // Tracks tool number. NOT USED. - int32_t line_number; // Last line number sent + float spindle_speed; // RPM + float feed_rate; // Millimeters/min + uint8_t tool; // Tracks tool number. NOT USED. + int32_t line_number; // Last line number sent - float position[N_AXIS]; // Where the interpreter considers the tool to be at this point in the code + float position[N_AXIS]; // Where the interpreter considers the tool to be at this point in the code - float coord_system[N_AXIS]; // Current work coordinate system (G54+). Stores offset from absolute machine - // position in mm. Loaded from EEPROM when called. - float coord_offset[N_AXIS]; // Retains the G92 coordinate offset (work coordinates) relative to - // machine zero in mm. Non-persistent. Cleared upon reset and boot. - float tool_length_offset; // Tracks tool length offset value when enabled. + float coord_system[N_AXIS]; // Current work coordinate system (G54+). Stores offset from absolute machine + // position in mm. Loaded from EEPROM when called. + float coord_offset[N_AXIS]; // Retains the G92 coordinate offset (work coordinates) relative to + // machine zero in mm. Non-persistent. Cleared upon reset and boot. + float tool_length_offset; // Tracks tool length offset value when enabled. } parser_state_t; + extern parser_state_t gc_state; - typedef struct { - uint8_t non_modal_command; - gc_modal_t modal; - gc_values_t values; + uint8_t non_modal_command; + gc_modal_t modal; + gc_values_t values; } parser_block_t; - // Initialize the parser void gc_init(); diff --git a/grbl/grbl.h b/grbl/grbl.h index dd8e781..c65c983 100644 --- a/grbl/grbl.h +++ b/grbl/grbl.h @@ -22,117 +22,116 @@ #define grbl_h // Grbl versioning system -#define GRBL_VERSION "1.1h" +#define GRBL_VERSION "1.1h" #define GRBL_VERSION_BUILD "20190830" // Define standard libraries used by Grbl. +#include #include #include -#include #include -#include -#include #include -#include -#include -#include +#include #include +#include +#include +#include +#include // Define the Grbl system include files. NOTE: Do not alter organization. #include "config.h" -#include "nuts_bolts.h" -#include "settings.h" -#include "system.h" -#include "defaults.h" -#include "cpu_map.h" -#include "planner.h" #include "coolant_control.h" +#include "cpu_map.h" +#include "defaults.h" #include "eeprom.h" #include "gcode.h" +#include "jog.h" #include "limits.h" #include "motion_control.h" +#include "nuts_bolts.h" #include "planner.h" #include "print.h" #include "probe.h" #include "protocol.h" #include "report.h" #include "serial.h" +#include "settings.h" #include "spindle_control.h" #include "stepper.h" -#include "jog.h" +#include "system.h" // --------------------------------------------------------------------------------------- // COMPILE-TIME ERROR CHECKING OF DEFINE VALUES: #ifndef HOMING_CYCLE_0 - #error "Required HOMING_CYCLE_0 not defined." +#error "Required HOMING_CYCLE_0 not defined." #endif #if defined(USE_SPINDLE_DIR_AS_ENABLE_PIN) && !defined(VARIABLE_SPINDLE) - #error "USE_SPINDLE_DIR_AS_ENABLE_PIN may only be used with VARIABLE_SPINDLE enabled" +#error "USE_SPINDLE_DIR_AS_ENABLE_PIN may only be used with VARIABLE_SPINDLE enabled" #endif #if defined(USE_SPINDLE_DIR_AS_ENABLE_PIN) && !defined(CPU_MAP_ATMEGA328P) - #error "USE_SPINDLE_DIR_AS_ENABLE_PIN may only be used with a 328p processor" +#error "USE_SPINDLE_DIR_AS_ENABLE_PIN may only be used with a 328p processor" #endif #if !defined(USE_SPINDLE_DIR_AS_ENABLE_PIN) && defined(SPINDLE_ENABLE_OFF_WITH_ZERO_SPEED) - #error "SPINDLE_ENABLE_OFF_WITH_ZERO_SPEED may only be used with USE_SPINDLE_DIR_AS_ENABLE_PIN enabled" +#error "SPINDLE_ENABLE_OFF_WITH_ZERO_SPEED may only be used with USE_SPINDLE_DIR_AS_ENABLE_PIN enabled" #endif #if defined(PARKING_ENABLE) - #if defined(HOMING_FORCE_SET_ORIGIN) - #error "HOMING_FORCE_SET_ORIGIN is not supported with PARKING_ENABLE at this time." - #endif +#if defined(HOMING_FORCE_SET_ORIGIN) +#error "HOMING_FORCE_SET_ORIGIN is not supported with PARKING_ENABLE at this time." +#endif #endif #if defined(ENABLE_PARKING_OVERRIDE_CONTROL) - #if !defined(PARKING_ENABLE) - #error "ENABLE_PARKING_OVERRIDE_CONTROL must be enabled with PARKING_ENABLE." - #endif +#if !defined(PARKING_ENABLE) +#error "ENABLE_PARKING_OVERRIDE_CONTROL must be enabled with PARKING_ENABLE." +#endif #endif #if defined(SPINDLE_PWM_MIN_VALUE) - #if !(SPINDLE_PWM_MIN_VALUE > 0) - #error "SPINDLE_PWM_MIN_VALUE must be greater than zero." - #endif +#if !(SPINDLE_PWM_MIN_VALUE > 0) +#error "SPINDLE_PWM_MIN_VALUE must be greater than zero." +#endif #endif #if (REPORT_WCO_REFRESH_BUSY_COUNT < REPORT_WCO_REFRESH_IDLE_COUNT) - #error "WCO busy refresh is less than idle refresh." +#error "WCO busy refresh is less than idle refresh." #endif #if (REPORT_OVR_REFRESH_BUSY_COUNT < REPORT_OVR_REFRESH_IDLE_COUNT) - #error "Override busy refresh is less than idle refresh." +#error "Override busy refresh is less than idle refresh." #endif #if (REPORT_WCO_REFRESH_IDLE_COUNT < 2) - #error "WCO refresh must be greater than one." +#error "WCO refresh must be greater than one." #endif #if (REPORT_OVR_REFRESH_IDLE_COUNT < 1) - #error "Override refresh must be greater than zero." +#error "Override refresh must be greater than zero." #endif #if defined(ENABLE_DUAL_AXIS) - #if !((DUAL_AXIS_SELECT == X_AXIS) || (DUAL_AXIS_SELECT == Y_AXIS)) - #error "Dual axis currently supports X or Y axes only." - #endif - #if defined(DUAL_AXIS_CONFIG_CNC_SHIELD_CLONE) && defined(VARIABLE_SPINDLE) - #error "VARIABLE_SPINDLE not supported with DUAL_AXIS_CNC_SHIELD_CLONE." - #endif - #if defined(DUAL_AXIS_CONFIG_CNC_SHIELD_CLONE) && defined(DUAL_AXIS_CONFIG_PROTONEER_V3_51) - #error "More than one dual axis configuration found. Select one." - #endif - #if !defined(DUAL_AXIS_CONFIG_CNC_SHIELD_CLONE) && !defined(DUAL_AXIS_CONFIG_PROTONEER_V3_51) - #error "No supported dual axis configuration found. Select one." - #endif - #if defined(COREXY) - #error "CORE XY not supported with dual axis feature." - #endif - #if defined(USE_SPINDLE_DIR_AS_ENABLE_PIN) - #error "USE_SPINDLE_DIR_AS_ENABLE_PIN not supported with dual axis feature." - #endif - #if defined(ENABLE_M7) - #error "ENABLE_M7 not supported with dual axis feature." - #endif +#if !((DUAL_AXIS_SELECT == X_AXIS) || (DUAL_AXIS_SELECT == Y_AXIS)) +#error "Dual axis currently supports X or Y axes only." +#endif +#if defined(DUAL_AXIS_CONFIG_CNC_SHIELD_CLONE) && defined(VARIABLE_SPINDLE) +#error "VARIABLE_SPINDLE not supported with DUAL_AXIS_CNC_SHIELD_CLONE." +#endif +#if defined(DUAL_AXIS_CONFIG_CNC_SHIELD_CLONE) && defined(DUAL_AXIS_CONFIG_PROTONEER_V3_51) +#error "More than one dual axis configuration found. Select one." +#endif +#if !defined(DUAL_AXIS_CONFIG_CNC_SHIELD_CLONE) && !defined(DUAL_AXIS_CONFIG_PROTONEER_V3_51) +#error "No supported dual axis configuration found. Select one." +#endif +#if defined(COREXY) +#error "CORE XY not supported with dual axis feature." +#endif +#if defined(USE_SPINDLE_DIR_AS_ENABLE_PIN) +#error "USE_SPINDLE_DIR_AS_ENABLE_PIN not supported with dual axis feature." +#endif +#if defined(ENABLE_M7) +#error "ENABLE_M7 not supported with dual axis feature." +#endif #endif // --------------------------------------------------------------------------------------- diff --git a/grbl/jog.c b/grbl/jog.c index 553af77..cb5c46d 100644 --- a/grbl/jog.c +++ b/grbl/jog.c @@ -20,31 +20,31 @@ #include "grbl.h" - // Sets up valid jog motion received from g-code parser, checks for soft-limits, and executes the jog. -uint8_t jog_execute(plan_line_data_t *pl_data, parser_block_t *gc_block) -{ - // Initialize planner data struct for jogging motions. - // NOTE: Spindle and coolant are allowed to fully function with overrides during a jog. - pl_data->feed_rate = gc_block->values.f; - pl_data->condition |= PL_COND_FLAG_NO_FEED_OVERRIDE; - #ifdef USE_LINE_NUMBERS +uint8_t jog_execute(plan_line_data_t *pl_data, parser_block_t *gc_block) { + // Initialize planner data struct for jogging motions. + // NOTE: Spindle and coolant are allowed to fully function with overrides during a jog. + pl_data->feed_rate = gc_block->values.f; + pl_data->condition |= PL_COND_FLAG_NO_FEED_OVERRIDE; +#ifdef USE_LINE_NUMBERS pl_data->line_number = gc_block->values.n; - #endif +#endif - if (bit_istrue(settings.flags,BITFLAG_SOFT_LIMIT_ENABLE)) { - if (system_check_travel_limits(gc_block->values.xyz)) { return(STATUS_TRAVEL_EXCEEDED); } - } - - // Valid jog command. Plan, set state, and execute. - mc_line(gc_block->values.xyz,pl_data); - if (sys.state == STATE_IDLE) { - if (plan_get_current_block() != NULL) { // Check if there is a block to execute. - sys.state = STATE_JOG; - st_prep_buffer(); - st_wake_up(); // NOTE: Manual start. No state machine required. + if (bit_istrue(settings.flags, BITFLAG_SOFT_LIMIT_ENABLE)) { + if (system_check_travel_limits(gc_block->values.xyz)) { + return (STATUS_TRAVEL_EXCEEDED); + } } - } - return(STATUS_OK); + // Valid jog command. Plan, set state, and execute. + mc_line(gc_block->values.xyz, pl_data); + if (sys.state == STATE_IDLE) { + if (plan_get_current_block() != NULL) { // Check if there is a block to execute. + sys.state = STATE_JOG; + st_prep_buffer(); + st_wake_up(); // NOTE: Manual start. No state machine required. + } + } + + return (STATUS_OK); } diff --git a/grbl/limits.c b/grbl/limits.c index 1348c14..fcd7259 100644 --- a/grbl/limits.c +++ b/grbl/limits.c @@ -21,130 +21,134 @@ #include "grbl.h" - // Homing axis search distance multiplier. Computed by this value times the cycle travel. #ifndef HOMING_AXIS_SEARCH_SCALAR - #define HOMING_AXIS_SEARCH_SCALAR 1.5 // Must be > 1 to ensure limit switch will be engaged. +#define HOMING_AXIS_SEARCH_SCALAR 1.5 // Must be > 1 to ensure limit switch will be engaged. #endif #ifndef HOMING_AXIS_LOCATE_SCALAR - #define HOMING_AXIS_LOCATE_SCALAR 5.0 // Must be > 1 to ensure limit switch is cleared. +#define HOMING_AXIS_LOCATE_SCALAR 5.0 // Must be > 1 to ensure limit switch is cleared. #endif #ifdef ENABLE_DUAL_AXIS - // Flags for dual axis async limit trigger check. - #define DUAL_AXIS_CHECK_DISABLE 0 // Must be zero - #define DUAL_AXIS_CHECK_ENABLE bit(0) - #define DUAL_AXIS_CHECK_TRIGGER_1 bit(1) - #define DUAL_AXIS_CHECK_TRIGGER_2 bit(2) +// Flags for dual axis async limit trigger check. +#define DUAL_AXIS_CHECK_DISABLE 0 // Must be zero +#define DUAL_AXIS_CHECK_ENABLE bit(0) +#define DUAL_AXIS_CHECK_TRIGGER_1 bit(1) +#define DUAL_AXIS_CHECK_TRIGGER_2 bit(2) #endif -void limits_init() -{ - LIMIT_DDR &= ~(LIMIT_MASK); // Set as input pins +void limits_init() { + LIMIT_DDR &= ~(LIMIT_MASK); // Set as input pins - #ifdef DISABLE_LIMIT_PIN_PULL_UP +#ifdef DISABLE_LIMIT_PIN_PULL_UP LIMIT_PORT &= ~(LIMIT_MASK); // Normal low operation. Requires external pull-down. - #else - LIMIT_PORT |= (LIMIT_MASK); // Enable internal pull-up resistors. Normal high operation. - #endif +#else + LIMIT_PORT |= (LIMIT_MASK); // Enable internal pull-up resistors. Normal high operation. +#endif - if (bit_istrue(settings.flags,BITFLAG_HARD_LIMIT_ENABLE)) { - LIMIT_PCMSK |= LIMIT_MASK; // Enable specific pins of the Pin Change Interrupt - PCICR |= (1 << LIMIT_INT); // Enable Pin Change Interrupt - } else { - limits_disable(); - } + if (bit_istrue(settings.flags, BITFLAG_HARD_LIMIT_ENABLE)) { + LIMIT_PCMSK |= LIMIT_MASK; // Enable specific pins of the Pin Change Interrupt + PCICR |= (1 << LIMIT_INT); // Enable Pin Change Interrupt + } else { + limits_disable(); + } - #ifdef ENABLE_SOFTWARE_DEBOUNCE - MCUSR &= ~(1<condition = (PL_COND_FLAG_SYSTEM_MOTION|PL_COND_FLAG_NO_FEED_OVERRIDE); - #ifdef USE_LINE_NUMBERS + // Initialize plan data struct for homing motion. Spindle and coolant are disabled. + plan_line_data_t plan_data; + plan_line_data_t *pl_data = &plan_data; + memset(pl_data, 0, sizeof(plan_line_data_t)); + pl_data->condition = (PL_COND_FLAG_SYSTEM_MOTION | PL_COND_FLAG_NO_FEED_OVERRIDE); +#ifdef USE_LINE_NUMBERS pl_data->line_number = HOMING_CYCLE_LINE_NUMBER; - #endif +#endif - // Initialize variables used for homing computations. - uint8_t n_cycle = (2*N_HOMING_LOCATE_CYCLE+1); - uint8_t step_pin[N_AXIS]; - #ifdef ENABLE_DUAL_AXIS + // Initialize variables used for homing computations. + uint8_t n_cycle = (2 * N_HOMING_LOCATE_CYCLE + 1); + uint8_t step_pin[N_AXIS]; +#ifdef ENABLE_DUAL_AXIS uint8_t step_pin_dual; uint8_t dual_axis_async_check; int32_t dual_trigger_position; - #if (DUAL_AXIS_SELECT == X_AXIS) - float fail_distance = (-DUAL_AXIS_HOMING_FAIL_AXIS_LENGTH_PERCENT/100.0)*settings.max_travel[Y_AXIS]; - #else - float fail_distance = (-DUAL_AXIS_HOMING_FAIL_AXIS_LENGTH_PERCENT/100.0)*settings.max_travel[X_AXIS]; - #endif +#if (DUAL_AXIS_SELECT == X_AXIS) + float fail_distance = (-DUAL_AXIS_HOMING_FAIL_AXIS_LENGTH_PERCENT / 100.0) * settings.max_travel[Y_AXIS]; +#else + float fail_distance = (-DUAL_AXIS_HOMING_FAIL_AXIS_LENGTH_PERCENT / 100.0) * settings.max_travel[X_AXIS]; +#endif fail_distance = min(fail_distance, DUAL_AXIS_HOMING_FAIL_DISTANCE_MAX); fail_distance = max(fail_distance, DUAL_AXIS_HOMING_FAIL_DISTANCE_MIN); - int32_t dual_fail_distance = trunc(fail_distance*settings.steps_per_mm[DUAL_AXIS_SELECT]); - // int32_t dual_fail_distance = trunc((DUAL_AXIS_HOMING_TRIGGER_FAIL_DISTANCE)*settings.steps_per_mm[DUAL_AXIS_SELECT]); - #endif - float target[N_AXIS]; - float max_travel = 0.0; - uint8_t idx; - for (idx=0; idxfeed_rate = homing_rate; // Set current homing rate. - plan_buffer_line(target, pl_data); // Bypass mc_line(). Directly plan homing motion. + // Set search mode with approach at seek rate to quickly engage the specified cycle_mask limit switches. + bool approach = true; + float homing_rate = settings.homing_seek_rate; - sys.step_control = STEP_CONTROL_EXECUTE_SYS_MOTION; // Set to execute homing motion and clear existing flags. - st_prep_buffer(); // Prep and fill segment buffer from newly planned block. - st_wake_up(); // Initiate motion + uint8_t limit_state, axislock, n_active_axis; do { - if (approach) { - // Check limit state. Lock out cycle axes when they change. - limit_state = limits_get_state(); - for (idx=0; idx dual_fail_distance) { - system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_DUAL_APPROACH); - mc_reset(); - protocol_execute_realtime(); - return; + + system_convert_array_steps_to_mpos(target, sys_position); + + // Initialize and declare variables needed for homing routine. + axislock = 0; +#ifdef ENABLE_DUAL_AXIS + sys.homing_axis_lock_dual = 0; + dual_trigger_position = 0; + dual_axis_async_check = DUAL_AXIS_CHECK_DISABLE; +#endif + n_active_axis = 0; + for (idx = 0; idx < N_AXIS; idx++) { + // Set target location for active axes and setup computation for homing rate. + if (bit_istrue(cycle_mask, bit(idx))) { + n_active_axis++; +#ifdef COREXY + if (idx == X_AXIS) { + int32_t axis_position = system_convert_corexy_to_y_axis_steps(sys_position); + sys_position[A_MOTOR] = axis_position; + sys_position[B_MOTOR] = -axis_position; + } else if (idx == Y_AXIS) { + int32_t axis_position = system_convert_corexy_to_x_axis_steps(sys_position); + sys_position[A_MOTOR] = sys_position[B_MOTOR] = axis_position; + } else { + sys_position[Z_AXIS] = 0; } - } - } else { - dual_axis_async_check |= DUAL_AXIS_CHECK_ENABLE; - dual_trigger_position = sys_position[DUAL_AXIS_SELECT]; +#else + sys_position[idx] = 0; +#endif + // Set target direction based on cycle mask and homing cycle approach state. + // NOTE: This happens to compile smaller than any other implementation tried. + if (bit_istrue(settings.homing_dir_mask, bit(idx))) { + if (approach) { + target[idx] = -max_travel; + } else { + target[idx] = max_travel; + } + } else { + if (approach) { + target[idx] = max_travel; + } else { + target[idx] = -max_travel; + } + } + // Apply axislock to the step port pins active in this cycle. + axislock |= step_pin[idx]; +#ifdef ENABLE_DUAL_AXIS + if (idx == DUAL_AXIS_SELECT) { + sys.homing_axis_lock_dual = step_pin_dual; + } +#endif } - } - #endif - } - - st_prep_buffer(); // Check and prep segment buffer. NOTE: Should take no longer than 200us. - - // Exit routines: No time to run protocol_execute_realtime() in this loop. - if (sys_rt_exec_state & (EXEC_SAFETY_DOOR | EXEC_RESET | EXEC_CYCLE_STOP)) { - uint8_t rt_exec = sys_rt_exec_state; - // Homing failure condition: Reset issued during cycle. - if (rt_exec & EXEC_RESET) { system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_RESET); } - // Homing failure condition: Safety door was opened. - if (rt_exec & EXEC_SAFETY_DOOR) { system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_DOOR); } - // Homing failure condition: Limit switch still engaged after pull-off motion - if (!approach && (limits_get_state() & cycle_mask)) { system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_PULLOFF); } - // Homing failure condition: Limit switch not found during approach. - if (approach && (rt_exec & EXEC_CYCLE_STOP)) { system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_APPROACH); } - if (sys_rt_exec_alarm) { - mc_reset(); // Stop motors, if they are running. - protocol_execute_realtime(); - return; - } else { - // Pull-off motion complete. Disable CYCLE_STOP from executing. - system_clear_exec_state_flag(EXEC_CYCLE_STOP); - break; } - } + homing_rate *= sqrt(n_active_axis); // [sqrt(N_AXIS)] Adjust so individual axes all move at homing rate. + sys.homing_axis_lock = axislock; - #ifdef ENABLE_DUAL_AXIS - } while ((STEP_MASK & axislock) || (sys.homing_axis_lock_dual)); - #else - } while (STEP_MASK & axislock); - #endif + // Perform homing cycle. Planner buffer should be empty, as required to initiate the homing cycle. + pl_data->feed_rate = homing_rate; // Set current homing rate. + plan_buffer_line(target, pl_data); // Bypass mc_line(). Directly plan homing motion. - st_reset(); // Immediately force kill steppers and reset step segment buffer. - delay_ms(settings.homing_debounce_delay); // Delay to allow transient dynamics to dissipate. + sys.step_control = STEP_CONTROL_EXECUTE_SYS_MOTION; // Set to execute homing motion and clear existing flags. + st_prep_buffer(); // Prep and fill segment buffer from newly planned block. + st_wake_up(); // Initiate motion + do { + if (approach) { + // Check limit state. Lock out cycle axes when they change. + limit_state = limits_get_state(); + for (idx = 0; idx < N_AXIS; idx++) { + if (axislock & step_pin[idx]) { + if (limit_state & (1 << idx)) { +#ifdef COREXY + if (idx == Z_AXIS) { + axislock &= ~(step_pin[Z_AXIS]); + } else { + axislock &= ~(step_pin[A_MOTOR] | step_pin[B_MOTOR]); + } +#else + axislock &= ~(step_pin[idx]); +#ifdef ENABLE_DUAL_AXIS + if (idx == DUAL_AXIS_SELECT) { + dual_axis_async_check |= DUAL_AXIS_CHECK_TRIGGER_1; + } +#endif +#endif + } + } + } + sys.homing_axis_lock = axislock; +#ifdef ENABLE_DUAL_AXIS + if (sys.homing_axis_lock_dual) { // NOTE: Only true when homing dual axis. + if (limit_state & (1 << N_AXIS)) { + sys.homing_axis_lock_dual = 0; + dual_axis_async_check |= DUAL_AXIS_CHECK_TRIGGER_2; + } + } - // Reverse direction and reset homing rate for locate cycle(s). - approach = !approach; + // When first dual axis limit triggers, record position and begin checking distance until other limit + // triggers. Bail upon failure. + if (dual_axis_async_check) { + if (dual_axis_async_check & DUAL_AXIS_CHECK_ENABLE) { + if ((dual_axis_async_check & (DUAL_AXIS_CHECK_TRIGGER_1 | DUAL_AXIS_CHECK_TRIGGER_2)) == + (DUAL_AXIS_CHECK_TRIGGER_1 | DUAL_AXIS_CHECK_TRIGGER_2)) { + dual_axis_async_check = DUAL_AXIS_CHECK_DISABLE; + } else { + if (abs(dual_trigger_position - sys_position[DUAL_AXIS_SELECT]) > dual_fail_distance) { + system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_DUAL_APPROACH); + mc_reset(); + protocol_execute_realtime(); + return; + } + } + } else { + dual_axis_async_check |= DUAL_AXIS_CHECK_ENABLE; + dual_trigger_position = sys_position[DUAL_AXIS_SELECT]; + } + } +#endif + } - // After first cycle, homing enters locating phase. Shorten search to pull-off distance. - if (approach) { - max_travel = settings.homing_pulloff*HOMING_AXIS_LOCATE_SCALAR; - homing_rate = settings.homing_feed_rate; - } else { - max_travel = settings.homing_pulloff; - homing_rate = settings.homing_seek_rate; + st_prep_buffer(); // Check and prep segment buffer. NOTE: Should take no longer than 200us. + + // Exit routines: No time to run protocol_execute_realtime() in this loop. + if (sys_rt_exec_state & (EXEC_SAFETY_DOOR | EXEC_RESET | EXEC_CYCLE_STOP)) { + uint8_t rt_exec = sys_rt_exec_state; + // Homing failure condition: Reset issued during cycle. + if (rt_exec & EXEC_RESET) { + system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_RESET); + } + // Homing failure condition: Safety door was opened. + if (rt_exec & EXEC_SAFETY_DOOR) { + system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_DOOR); + } + // Homing failure condition: Limit switch still engaged after pull-off motion + if (!approach && (limits_get_state() & cycle_mask)) { + system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_PULLOFF); + } + // Homing failure condition: Limit switch not found during approach. + if (approach && (rt_exec & EXEC_CYCLE_STOP)) { + system_set_exec_alarm(EXEC_ALARM_HOMING_FAIL_APPROACH); + } + if (sys_rt_exec_alarm) { + mc_reset(); // Stop motors, if they are running. + protocol_execute_realtime(); + return; + } else { + // Pull-off motion complete. Disable CYCLE_STOP from executing. + system_clear_exec_state_flag(EXEC_CYCLE_STOP); + break; + } + } + +#ifdef ENABLE_DUAL_AXIS + } while ((STEP_MASK & axislock) || (sys.homing_axis_lock_dual)); +#else + } while (STEP_MASK & axislock); +#endif + + st_reset(); // Immediately force kill steppers and reset step segment buffer. + delay_ms(settings.homing_debounce_delay); // Delay to allow transient dynamics to dissipate. + + // Reverse direction and reset homing rate for locate cycle(s). + approach = !approach; + + // After first cycle, homing enters locating phase. Shorten search to pull-off distance. + if (approach) { + max_travel = settings.homing_pulloff * HOMING_AXIS_LOCATE_SCALAR; + homing_rate = settings.homing_feed_rate; + } else { + max_travel = settings.homing_pulloff; + homing_rate = settings.homing_seek_rate; + } + + } while (n_cycle-- > 0); + + // The active cycle axes should now be homed and machine limits have been located. By + // default, Grbl defines machine space as all negative, as do most CNCs. Since limit switches + // can be on either side of an axes, check and set axes machine zero appropriately. Also, + // set up pull-off maneuver from axes limit switches that have been homed. This provides + // some initial clearance off the switches and should also help prevent them from falsely + // triggering when hard limits are enabled or when more than one axes shares a limit pin. + int32_t set_axis_position; + // Set machine positions for homed limit switches. Don't update non-homed axes. + for (idx = 0; idx < N_AXIS; idx++) { + // NOTE: settings.max_travel[] is stored as a negative value. + if (cycle_mask & bit(idx)) { +#ifdef HOMING_FORCE_SET_ORIGIN + set_axis_position = 0; +#else + if (bit_istrue(settings.homing_dir_mask, bit(idx))) { + set_axis_position = + lround((settings.max_travel[idx] + settings.homing_pulloff) * settings.steps_per_mm[idx]); + } else { + set_axis_position = lround(-settings.homing_pulloff * settings.steps_per_mm[idx]); + } +#endif + +#ifdef COREXY + if (idx == X_AXIS) { + int32_t off_axis_position = system_convert_corexy_to_y_axis_steps(sys_position); + sys_position[A_MOTOR] = set_axis_position + off_axis_position; + sys_position[B_MOTOR] = set_axis_position - off_axis_position; + } else if (idx == Y_AXIS) { + int32_t off_axis_position = system_convert_corexy_to_x_axis_steps(sys_position); + sys_position[A_MOTOR] = off_axis_position + set_axis_position; + sys_position[B_MOTOR] = off_axis_position - set_axis_position; + } else { + sys_position[idx] = set_axis_position; + } +#else + sys_position[idx] = set_axis_position; +#endif + } } - - } while (n_cycle-- > 0); - - // The active cycle axes should now be homed and machine limits have been located. By - // default, Grbl defines machine space as all negative, as do most CNCs. Since limit switches - // can be on either side of an axes, check and set axes machine zero appropriately. Also, - // set up pull-off maneuver from axes limit switches that have been homed. This provides - // some initial clearance off the switches and should also help prevent them from falsely - // triggering when hard limits are enabled or when more than one axes shares a limit pin. - int32_t set_axis_position; - // Set machine positions for homed limit switches. Don't update non-homed axes. - for (idx=0; idxcondition & PL_COND_FLAG_SPINDLE_CW) { - spindle_sync(PL_COND_FLAG_SPINDLE_CW, pl_data->spindle_speed); - } +void mc_line(float *target, plan_line_data_t *pl_data) { + // If enabled, check for soft limit violations. Placed here all line motions are picked up + // from everywhere in Grbl. + if (bit_istrue(settings.flags, BITFLAG_SOFT_LIMIT_ENABLE)) { + // NOTE: Block jog state. Jogging is a special case and soft limits are handled independently. + if (sys.state != STATE_JOG) { + limits_soft_check(target); + } } - } -} + // If in check gcode mode, prevent motion by blocking planner. Soft limits still work. + if (sys.state == STATE_CHECK_MODE) { + return; + } + + // NOTE: Backlash compensation may be installed here. It will need direction info to track when + // to insert a backlash line motion(s) before the intended line motion and will require its own + // plan_check_full_buffer() and check for system abort loop. Also for position reporting + // backlash steps will need to be also tracked, which will need to be kept at a system level. + // There are likely some other things that will need to be tracked as well. However, we feel + // that backlash compensation should NOT be handled by Grbl itself, because there are a myriad + // of ways to implement it and can be effective or ineffective for different CNC machines. This + // would be better handled by the interface as a post-processor task, where the original g-code + // is translated and inserts backlash motions that best suits the machine. + // NOTE: Perhaps as a middle-ground, all that needs to be sent is a flag or special command that + // indicates to Grbl what is a backlash compensation motion, so that Grbl executes the move but + // doesn't update the machine position values. Since the position values used by the g-code + // parser and planner are separate from the system machine positions, this is doable. + + // If the buffer is full: good! That means we are well ahead of the robot. + // Remain in this loop until there is room in the buffer. + do { + protocol_execute_realtime(); // Check for any run-time commands + if (sys.abort) { + return; + } // Bail, if system abort. + if (plan_check_full_buffer()) { + protocol_auto_cycle_start(); + } // Auto-cycle start when buffer is full. + else { + break; + } + } while (1); + + // Plan and queue motion into planner buffer + if (plan_buffer_line(target, pl_data) == PLAN_EMPTY_BLOCK) { + if (bit_istrue(settings.flags, BITFLAG_LASER_MODE)) { + // Correctly set spindle state, if there is a coincident position passed. Forces a buffer + // sync while in M3 laser mode only. + if (pl_data->condition & PL_COND_FLAG_SPINDLE_CW) { + spindle_sync(PL_COND_FLAG_SPINDLE_CW, pl_data->spindle_speed); + } + } + } +} // Execute an arc in offset mode format. position == current xyz, target == target xyz, // offset == offset from current xyz, axis_X defines circle plane in tool space, axis_linear is @@ -84,305 +91,327 @@ void mc_line(float *target, plan_line_data_t *pl_data) // The arc is approximated by generating a huge number of tiny, linear segments. The chordal tolerance // of each segment is configured in settings.arc_tolerance, which is defined to be the maximum normal // distance from segment to the circle when the end points both lie on the circle. -void mc_arc(float *target, plan_line_data_t *pl_data, float *position, float *offset, float radius, - uint8_t axis_0, uint8_t axis_1, uint8_t axis_linear, uint8_t is_clockwise_arc) -{ - float center_axis0 = position[axis_0] + offset[axis_0]; - float center_axis1 = position[axis_1] + offset[axis_1]; - float r_axis0 = -offset[axis_0]; // Radius vector from center to current location - float r_axis1 = -offset[axis_1]; - float rt_axis0 = target[axis_0] - center_axis0; - float rt_axis1 = target[axis_1] - center_axis1; +void mc_arc(float *target, plan_line_data_t *pl_data, float *position, float *offset, float radius, uint8_t axis_0, + uint8_t axis_1, uint8_t axis_linear, uint8_t is_clockwise_arc) { + float center_axis0 = position[axis_0] + offset[axis_0]; + float center_axis1 = position[axis_1] + offset[axis_1]; + float r_axis0 = -offset[axis_0]; // Radius vector from center to current location + float r_axis1 = -offset[axis_1]; + float rt_axis0 = target[axis_0] - center_axis0; + float rt_axis1 = target[axis_1] - center_axis1; - // CCW angle between position and target from circle center. Only one atan2() trig computation required. - float angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1); - if (is_clockwise_arc) { // Correct atan2 output per direction - if (angular_travel >= -ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel -= 2*M_PI; } - } else { - if (angular_travel <= ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel += 2*M_PI; } - } - - // NOTE: Segment end points are on the arc, which can lead to the arc diameter being smaller by up to - // (2x) settings.arc_tolerance. For 99% of users, this is just fine. If a different arc segment fit - // is desired, i.e. least-squares, midpoint on arc, just change the mm_per_arc_segment calculation. - // For the intended uses of Grbl, this value shouldn't exceed 2000 for the strictest of cases. - uint16_t segments = floor(fabs(0.5*angular_travel*radius)/ - sqrt(settings.arc_tolerance*(2*radius - settings.arc_tolerance)) ); - - if (segments) { - // Multiply inverse feed_rate to compensate for the fact that this movement is approximated - // by a number of discrete segments. The inverse feed_rate should be correct for the sum of - // all segments. - if (pl_data->condition & PL_COND_FLAG_INVERSE_TIME) { - pl_data->feed_rate *= segments; - bit_false(pl_data->condition,PL_COND_FLAG_INVERSE_TIME); // Force as feed absolute mode over arc segments. + // CCW angle between position and target from circle center. Only one atan2() trig computation required. + float angular_travel = atan2(r_axis0 * rt_axis1 - r_axis1 * rt_axis0, r_axis0 * rt_axis0 + r_axis1 * rt_axis1); + if (is_clockwise_arc) { // Correct atan2 output per direction + if (angular_travel >= -ARC_ANGULAR_TRAVEL_EPSILON) { + angular_travel -= 2 * M_PI; + } + } else { + if (angular_travel <= ARC_ANGULAR_TRAVEL_EPSILON) { + angular_travel += 2 * M_PI; + } } - - float theta_per_segment = angular_travel/segments; - float linear_per_segment = (target[axis_linear] - position[axis_linear])/segments; - /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector, - and phi is the angle of rotation. Solution approach by Jens Geisler. - r_T = [cos(phi) -sin(phi); - sin(phi) cos(phi] * r ; + // NOTE: Segment end points are on the arc, which can lead to the arc diameter being smaller by up to + // (2x) settings.arc_tolerance. For 99% of users, this is just fine. If a different arc segment fit + // is desired, i.e. least-squares, midpoint on arc, just change the mm_per_arc_segment calculation. + // For the intended uses of Grbl, this value shouldn't exceed 2000 for the strictest of cases. + uint16_t segments = floor(fabs(0.5 * angular_travel * radius) / + sqrt(settings.arc_tolerance * (2 * radius - settings.arc_tolerance))); - For arc generation, the center of the circle is the axis of rotation and the radius vector is - defined from the circle center to the initial position. Each line segment is formed by successive - vector rotations. Single precision values can accumulate error greater than tool precision in rare - cases. So, exact arc path correction is implemented. This approach avoids the problem of too many very - expensive trig operations [sin(),cos(),tan()] which can take 100-200 usec each to compute. + if (segments) { + // Multiply inverse feed_rate to compensate for the fact that this movement is approximated + // by a number of discrete segments. The inverse feed_rate should be correct for the sum of + // all segments. + if (pl_data->condition & PL_COND_FLAG_INVERSE_TIME) { + pl_data->feed_rate *= segments; + bit_false(pl_data->condition, PL_COND_FLAG_INVERSE_TIME); // Force as feed absolute mode over arc segments. + } - Small angle approximation may be used to reduce computation overhead further. A third-order approximation - (second order sin() has too much error) holds for most, if not, all CNC applications. Note that this - approximation will begin to accumulate a numerical drift error when theta_per_segment is greater than - ~0.25 rad(14 deg) AND the approximation is successively used without correction several dozen times. This - scenario is extremely unlikely, since segment lengths and theta_per_segment are automatically generated - and scaled by the arc tolerance setting. Only a very large arc tolerance setting, unrealistic for CNC - applications, would cause this numerical drift error. However, it is best to set N_ARC_CORRECTION from a - low of ~4 to a high of ~20 or so to avoid trig operations while keeping arc generation accurate. + float theta_per_segment = angular_travel / segments; + float linear_per_segment = (target[axis_linear] - position[axis_linear]) / segments; - This approximation also allows mc_arc to immediately insert a line segment into the planner - without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied - a correction, the planner should have caught up to the lag caused by the initial mc_arc overhead. - This is important when there are successive arc motions. - */ - // Computes: cos_T = 1 - theta_per_segment^2/2, sin_T = theta_per_segment - theta_per_segment^3/6) in ~52usec - float cos_T = 2.0 - theta_per_segment*theta_per_segment; - float sin_T = theta_per_segment*0.16666667*(cos_T + 4.0); - cos_T *= 0.5; + /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector, + and phi is the angle of rotation. Solution approach by Jens Geisler. + r_T = [cos(phi) -sin(phi); + sin(phi) cos(phi] * r ; - float sin_Ti; - float cos_Ti; - float r_axisi; - uint16_t i; - uint8_t count = 0; + For arc generation, the center of the circle is the axis of rotation and the radius vector is + defined from the circle center to the initial position. Each line segment is formed by successive + vector rotations. Single precision values can accumulate error greater than tool precision in rare + cases. So, exact arc path correction is implemented. This approach avoids the problem of too many very + expensive trig operations [sin(),cos(),tan()] which can take 100-200 usec each to compute. - for (i = 1; i 0) { + do { + fval *= 10.0; + } while (--exp > 0); + } + } + + // Assign floating point value with correct sign. + if (isnegative) { + *float_ptr = -fval; } else { - break; + *float_ptr = fval; } - c = *ptr++; - } - // Return if no digits have been read. - if (!ndigit) { return(false); }; + *char_counter = ptr - line - 1; // Set char_counter to next statement - // Convert integer into floating point. - float fval; - fval = (float)intval; - - // Apply decimal. Should perform no more than two floating point multiplications for the - // expected range of E0 to E-4. - if (fval != 0) { - while (exp <= -2) { - fval *= 0.01; - exp += 2; - } - if (exp < 0) { - fval *= 0.1; - } else if (exp > 0) { - do { - fval *= 10.0; - } while (--exp > 0); - } - } - - // Assign floating point value with correct sign. - if (isnegative) { - *float_ptr = -fval; - } else { - *float_ptr = fval; - } - - *char_counter = ptr - line - 1; // Set char_counter to next statement - - return(true); + return (true); } - // Non-blocking delay function used for general operation and suspend features. -void delay_sec(float seconds, uint8_t mode) -{ - uint16_t i = ceil(1000/DWELL_TIME_STEP*seconds); - while (i-- > 0) { - if (sys.abort) { return; } - if (mode == DELAY_MODE_DWELL) { - protocol_execute_realtime(); - } else { // DELAY_MODE_SYS_SUSPEND - // Execute rt_system() only to avoid nesting suspend loops. - protocol_exec_rt_system(); - if (sys.suspend & SUSPEND_RESTART_RETRACT) { return; } // Bail, if safety door reopens. - } - _delay_ms(DWELL_TIME_STEP); // Delay DWELL_TIME_STEP increment - } +void delay_sec(float seconds, uint8_t mode) { + uint16_t i = ceil(1000 / DWELL_TIME_STEP * seconds); + while (i-- > 0) { + if (sys.abort) { + return; + } + if (mode == DELAY_MODE_DWELL) { + protocol_execute_realtime(); + } else { // DELAY_MODE_SYS_SUSPEND + // Execute rt_system() only to avoid nesting suspend loops. + protocol_exec_rt_system(); + if (sys.suspend & SUSPEND_RESTART_RETRACT) { + return; + } // Bail, if safety door reopens. + } + _delay_ms(DWELL_TIME_STEP); // Delay DWELL_TIME_STEP increment + } } - // Delays variable defined milliseconds. Compiler compatibility fix for _delay_ms(), // which only accepts constants in future compiler releases. -void delay_ms(uint16_t ms) -{ - while ( ms-- ) { _delay_ms(1); } +void delay_ms(uint16_t ms) { + while (ms--) { + _delay_ms(1); + } } - // Delays variable defined microseconds. Compiler compatibility fix for _delay_us(), // which only accepts constants in future compiler releases. Written to perform more // efficiently with larger delays, as the counter adds parasitic time in each iteration. -void delay_us(uint32_t us) -{ - while (us) { - if (us < 10) { - _delay_us(1); - us--; - } else if (us < 100) { - _delay_us(10); - us -= 10; - } else if (us < 1000) { - _delay_us(100); - us -= 100; - } else { - _delay_ms(1); - us -= 1000; +void delay_us(uint32_t us) { + while (us) { + if (us < 10) { + _delay_us(1); + us--; + } else if (us < 100) { + _delay_us(10); + us -= 10; + } else if (us < 1000) { + _delay_us(100); + us -= 100; + } else { + _delay_ms(1); + us -= 1000; + } } - } } - // Simple hypotenuse computation function. -float hypot_f(float x, float y) { return(sqrt(x*x + y*y)); } - - -float convert_delta_vector_to_unit_vector(float *vector) -{ - uint8_t idx; - float magnitude = 0.0; - for (idx=0; idx (b)) ? (a) : (b)) -#define min(a,b) (((a) < (b)) ? (a) : (b)) -#define isequal_position_vector(a,b) !(memcmp(a, b, sizeof(float)*N_AXIS)) +#define max(a, b) (((a) > (b)) ? (a) : (b)) +#define min(a, b) (((a) < (b)) ? (a) : (b)) +#define isequal_position_vector(a, b) !(memcmp(a, b, sizeof(float) * N_AXIS)) // Bit field and masking macros -#define bit(n) (1 << n) -#define bit_true(x,mask) (x) |= (mask) -#define bit_false(x,mask) (x) &= ~(mask) -#define bit_istrue(x,mask) ((x & mask) != 0) -#define bit_isfalse(x,mask) ((x & mask) == 0) +#define bit(n) (1 << n) +#define bit_true(x, mask) (x) |= (mask) +#define bit_false(x, mask) (x) &= ~(mask) +#define bit_istrue(x, mask) ((x & mask) != 0) +#define bit_isfalse(x, mask) ((x & mask) == 0) // Read a floating point value from a string. Line points to the input buffer, char_counter // is the indexer pointing to the current character of the line, while float_ptr is diff --git a/grbl/planner.c b/grbl/planner.c index 49ff722..a0754ab 100644 --- a/grbl/planner.c +++ b/grbl/planner.c @@ -22,42 +22,41 @@ #include "grbl.h" - -static plan_block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions -static uint8_t block_buffer_tail; // Index of the block to process now -static uint8_t block_buffer_head; // Index of the next block to be pushed -static uint8_t next_buffer_head; // Index of the next buffer head -static uint8_t block_buffer_planned; // Index of the optimally planned block +static plan_block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions +static uint8_t block_buffer_tail; // Index of the block to process now +static uint8_t block_buffer_head; // Index of the next block to be pushed +static uint8_t next_buffer_head; // Index of the next buffer head +static uint8_t block_buffer_planned; // Index of the optimally planned block // Define planner variables typedef struct { - int32_t position[N_AXIS]; // The planner position of the tool in absolute steps. Kept separate + int32_t position[N_AXIS]; // The planner position of the tool in absolute steps. Kept separate // from g-code position for movements requiring multiple line motions, // i.e. arcs, canned cycles, and backlash compensation. - float previous_unit_vec[N_AXIS]; // Unit vector of previous path line segment - float previous_nominal_speed; // Nominal speed of previous path line segment + float previous_unit_vec[N_AXIS]; // Unit vector of previous path line segment + float previous_nominal_speed; // Nominal speed of previous path line segment } planner_t; + static planner_t pl; - // Returns the index of the next block in the ring buffer. Also called by stepper segment buffer. -uint8_t plan_next_block_index(uint8_t block_index) -{ - block_index++; - if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; } - return(block_index); +uint8_t plan_next_block_index(uint8_t block_index) { + block_index++; + if (block_index == BLOCK_BUFFER_SIZE) { + block_index = 0; + } + return (block_index); } - // Returns the index of the previous block in the ring buffer -static uint8_t plan_prev_block_index(uint8_t block_index) -{ - if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; } - block_index--; - return(block_index); +static uint8_t plan_prev_block_index(uint8_t block_index) { + if (block_index == 0) { + block_index = BLOCK_BUFFER_SIZE; + } + block_index--; + return (block_index); } - /* PLANNER SPEED DEFINITION +--------+ <- current->nominal_speed / \ @@ -123,181 +122,187 @@ static uint8_t plan_prev_block_index(uint8_t block_index) ARM versions should have enough memory and speed for look-ahead blocks numbering up to a hundred or more. */ -static void planner_recalculate() -{ - // Initialize block index to the last block in the planner buffer. - uint8_t block_index = plan_prev_block_index(block_buffer_head); +static void planner_recalculate() { + // Initialize block index to the last block in the planner buffer. + uint8_t block_index = plan_prev_block_index(block_buffer_head); - // Bail. Can't do anything with one only one plan-able block. - if (block_index == block_buffer_planned) { return; } + // Bail. Can't do anything with one only one plan-able block. + if (block_index == block_buffer_planned) { + return; + } - // Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last - // block in buffer. Cease planning when the last optimal planned or tail pointer is reached. - // NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan. - float entry_speed_sqr; - plan_block_t *next; - plan_block_t *current = &block_buffer[block_index]; + // Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last + // block in buffer. Cease planning when the last optimal planned or tail pointer is reached. + // NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan. + float entry_speed_sqr; + plan_block_t *next; + plan_block_t *current = &block_buffer[block_index]; - // Calculate maximum entry speed for last block in buffer, where the exit speed is always zero. - current->entry_speed_sqr = min( current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters); + // Calculate maximum entry speed for last block in buffer, where the exit speed is always zero. + current->entry_speed_sqr = min(current->max_entry_speed_sqr, 2 * current->acceleration * current->millimeters); - block_index = plan_prev_block_index(block_index); - if (block_index == block_buffer_planned) { // Only two plannable blocks in buffer. Reverse pass complete. - // Check if the first block is the tail. If so, notify stepper to update its current parameters. - if (block_index == block_buffer_tail) { st_update_plan_block_parameters(); } - } else { // Three or more plan-able blocks - while (block_index != block_buffer_planned) { - next = current; - current = &block_buffer[block_index]; - block_index = plan_prev_block_index(block_index); - - // Check if next block is the tail block(=planned block). If so, update current stepper parameters. - if (block_index == block_buffer_tail) { st_update_plan_block_parameters(); } - - // Compute maximum entry speed decelerating over the current block from its exit speed. - if (current->entry_speed_sqr != current->max_entry_speed_sqr) { - entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; - if (entry_speed_sqr < current->max_entry_speed_sqr) { - current->entry_speed_sqr = entry_speed_sqr; - } else { - current->entry_speed_sqr = current->max_entry_speed_sqr; + block_index = plan_prev_block_index(block_index); + if (block_index == block_buffer_planned) { // Only two plannable blocks in buffer. Reverse pass complete. + // Check if the first block is the tail. If so, notify stepper to update its current parameters. + if (block_index == block_buffer_tail) { + st_update_plan_block_parameters(); } - } - } - } + } else { // Three or more plan-able blocks + while (block_index != block_buffer_planned) { + next = current; + current = &block_buffer[block_index]; + block_index = plan_prev_block_index(block_index); - // Forward Pass: Forward plan the acceleration curve from the planned pointer onward. - // Also scans for optimal plan breakpoints and appropriately updates the planned pointer. - next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer - block_index = plan_next_block_index(block_buffer_planned); - while (block_index != block_buffer_head) { - current = next; - next = &block_buffer[block_index]; + // Check if next block is the tail block(=planned block). If so, update current stepper parameters. + if (block_index == block_buffer_tail) { + st_update_plan_block_parameters(); + } - // Any acceleration detected in the forward pass automatically moves the optimal planned - // pointer forward, since everything before this is all optimal. In other words, nothing - // can improve the plan from the buffer tail to the planned pointer by logic. - if (current->entry_speed_sqr < next->entry_speed_sqr) { - entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; - // If true, current block is full-acceleration and we can move the planned pointer forward. - if (entry_speed_sqr < next->entry_speed_sqr) { - next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this. - block_buffer_planned = block_index; // Set optimal plan pointer. - } + // Compute maximum entry speed decelerating over the current block from its exit speed. + if (current->entry_speed_sqr != current->max_entry_speed_sqr) { + entry_speed_sqr = next->entry_speed_sqr + 2 * current->acceleration * current->millimeters; + if (entry_speed_sqr < current->max_entry_speed_sqr) { + current->entry_speed_sqr = entry_speed_sqr; + } else { + current->entry_speed_sqr = current->max_entry_speed_sqr; + } + } + } } - // Any block set at its maximum entry speed also creates an optimal plan up to this - // point in the buffer. When the plan is bracketed by either the beginning of the - // buffer and a maximum entry speed or two maximum entry speeds, every block in between - // cannot logically be further improved. Hence, we don't have to recompute them anymore. - if (next->entry_speed_sqr == next->max_entry_speed_sqr) { block_buffer_planned = block_index; } - block_index = plan_next_block_index( block_index ); - } + // Forward Pass: Forward plan the acceleration curve from the planned pointer onward. + // Also scans for optimal plan breakpoints and appropriately updates the planned pointer. + next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer + block_index = plan_next_block_index(block_buffer_planned); + while (block_index != block_buffer_head) { + current = next; + next = &block_buffer[block_index]; + + // Any acceleration detected in the forward pass automatically moves the optimal planned + // pointer forward, since everything before this is all optimal. In other words, nothing + // can improve the plan from the buffer tail to the planned pointer by logic. + if (current->entry_speed_sqr < next->entry_speed_sqr) { + entry_speed_sqr = current->entry_speed_sqr + 2 * current->acceleration * current->millimeters; + // If true, current block is full-acceleration and we can move the planned pointer forward. + if (entry_speed_sqr < next->entry_speed_sqr) { + next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this. + block_buffer_planned = block_index; // Set optimal plan pointer. + } + } + + // Any block set at its maximum entry speed also creates an optimal plan up to this + // point in the buffer. When the plan is bracketed by either the beginning of the + // buffer and a maximum entry speed or two maximum entry speeds, every block in between + // cannot logically be further improved. Hence, we don't have to recompute them anymore. + if (next->entry_speed_sqr == next->max_entry_speed_sqr) { + block_buffer_planned = block_index; + } + block_index = plan_next_block_index(block_index); + } } - -void plan_reset() -{ - memset(&pl, 0, sizeof(planner_t)); // Clear planner struct - plan_reset_buffer(); +void plan_reset() { + memset(&pl, 0, sizeof(planner_t)); // Clear planner struct + plan_reset_buffer(); } - -void plan_reset_buffer() -{ - block_buffer_tail = 0; - block_buffer_head = 0; // Empty = tail - next_buffer_head = 1; // plan_next_block_index(block_buffer_head) - block_buffer_planned = 0; // = block_buffer_tail; +void plan_reset_buffer() { + block_buffer_tail = 0; + block_buffer_head = 0; // Empty = tail + next_buffer_head = 1; // plan_next_block_index(block_buffer_head) + block_buffer_planned = 0; // = block_buffer_tail; } - -void plan_discard_current_block() -{ - if (block_buffer_head != block_buffer_tail) { // Discard non-empty buffer. - uint8_t block_index = plan_next_block_index( block_buffer_tail ); - // Push block_buffer_planned pointer, if encountered. - if (block_buffer_tail == block_buffer_planned) { block_buffer_planned = block_index; } - block_buffer_tail = block_index; - } +void plan_discard_current_block() { + if (block_buffer_head != block_buffer_tail) { // Discard non-empty buffer. + uint8_t block_index = plan_next_block_index(block_buffer_tail); + // Push block_buffer_planned pointer, if encountered. + if (block_buffer_tail == block_buffer_planned) { + block_buffer_planned = block_index; + } + block_buffer_tail = block_index; + } } - // Returns address of planner buffer block used by system motions. Called by segment generator. -plan_block_t *plan_get_system_motion_block() -{ - return(&block_buffer[block_buffer_head]); +plan_block_t *plan_get_system_motion_block() { + return (&block_buffer[block_buffer_head]); } - // Returns address of first planner block, if available. Called by various main program functions. -plan_block_t *plan_get_current_block() -{ - if (block_buffer_head == block_buffer_tail) { return(NULL); } // Buffer empty - return(&block_buffer[block_buffer_tail]); +plan_block_t *plan_get_current_block() { + if (block_buffer_head == block_buffer_tail) { + return (NULL); + } // Buffer empty + return (&block_buffer[block_buffer_tail]); } - -float plan_get_exec_block_exit_speed_sqr() -{ - uint8_t block_index = plan_next_block_index(block_buffer_tail); - if (block_index == block_buffer_head) { return( 0.0 ); } - return( block_buffer[block_index].entry_speed_sqr ); +float plan_get_exec_block_exit_speed_sqr() { + uint8_t block_index = plan_next_block_index(block_buffer_tail); + if (block_index == block_buffer_head) { + return (0.0); + } + return (block_buffer[block_index].entry_speed_sqr); } - // Returns the availability status of the block ring buffer. True, if full. -uint8_t plan_check_full_buffer() -{ - if (block_buffer_tail == next_buffer_head) { return(true); } - return(false); +uint8_t plan_check_full_buffer() { + if (block_buffer_tail == next_buffer_head) { + return (true); + } + return (false); } - // Computes and returns block nominal speed based on running condition and override values. // NOTE: All system motion commands, such as homing/parking, are not subject to overrides. -float plan_compute_profile_nominal_speed(plan_block_t *block) -{ - float nominal_speed = block->programmed_rate; - if (block->condition & PL_COND_FLAG_RAPID_MOTION) { nominal_speed *= (0.01*sys.r_override); } - else { - if (!(block->condition & PL_COND_FLAG_NO_FEED_OVERRIDE)) { nominal_speed *= (0.01*sys.f_override); } - if (nominal_speed > block->rapid_rate) { nominal_speed = block->rapid_rate; } - } - if (nominal_speed > MINIMUM_FEED_RATE) { return(nominal_speed); } - return(MINIMUM_FEED_RATE); +float plan_compute_profile_nominal_speed(plan_block_t *block) { + float nominal_speed = block->programmed_rate; + if (block->condition & PL_COND_FLAG_RAPID_MOTION) { + nominal_speed *= (0.01 * sys.r_override); + } else { + if (!(block->condition & PL_COND_FLAG_NO_FEED_OVERRIDE)) { + nominal_speed *= (0.01 * sys.f_override); + } + if (nominal_speed > block->rapid_rate) { + nominal_speed = block->rapid_rate; + } + } + if (nominal_speed > MINIMUM_FEED_RATE) { + return (nominal_speed); + } + return (MINIMUM_FEED_RATE); } - // Computes and updates the max entry speed (sqr) of the block, based on the minimum of the junction's // previous and current nominal speeds and max junction speed. -static void plan_compute_profile_parameters(plan_block_t *block, float nominal_speed, float prev_nominal_speed) -{ - // Compute the junction maximum entry based on the minimum of the junction speed and neighboring nominal speeds. - if (nominal_speed > prev_nominal_speed) { block->max_entry_speed_sqr = prev_nominal_speed*prev_nominal_speed; } - else { block->max_entry_speed_sqr = nominal_speed*nominal_speed; } - if (block->max_entry_speed_sqr > block->max_junction_speed_sqr) { block->max_entry_speed_sqr = block->max_junction_speed_sqr; } +static void plan_compute_profile_parameters(plan_block_t *block, float nominal_speed, float prev_nominal_speed) { + // Compute the junction maximum entry based on the minimum of the junction speed and neighboring nominal speeds. + if (nominal_speed > prev_nominal_speed) { + block->max_entry_speed_sqr = prev_nominal_speed * prev_nominal_speed; + } else { + block->max_entry_speed_sqr = nominal_speed * nominal_speed; + } + if (block->max_entry_speed_sqr > block->max_junction_speed_sqr) { + block->max_entry_speed_sqr = block->max_junction_speed_sqr; + } } - // Re-calculates buffered motions profile parameters upon a motion-based override change. -void plan_update_velocity_profile_parameters() -{ - uint8_t block_index = block_buffer_tail; - plan_block_t *block; - float nominal_speed; - float prev_nominal_speed = SOME_LARGE_VALUE; // Set high for first block nominal speed calculation. - while (block_index != block_buffer_head) { - block = &block_buffer[block_index]; - nominal_speed = plan_compute_profile_nominal_speed(block); - plan_compute_profile_parameters(block, nominal_speed, prev_nominal_speed); - prev_nominal_speed = nominal_speed; - block_index = plan_next_block_index(block_index); - } - pl.previous_nominal_speed = prev_nominal_speed; // Update prev nominal speed for next incoming block. +void plan_update_velocity_profile_parameters() { + uint8_t block_index = block_buffer_tail; + plan_block_t *block; + float nominal_speed; + float prev_nominal_speed = SOME_LARGE_VALUE; // Set high for first block nominal speed calculation. + while (block_index != block_buffer_head) { + block = &block_buffer[block_index]; + nominal_speed = plan_compute_profile_nominal_speed(block); + plan_compute_profile_parameters(block, nominal_speed, prev_nominal_speed); + prev_nominal_speed = nominal_speed; + block_index = plan_next_block_index(block_index); + } + pl.previous_nominal_speed = prev_nominal_speed; // Update prev nominal speed for next incoming block. } - /* Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position in millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes. @@ -312,211 +317,221 @@ void plan_update_velocity_profile_parameters() head. It avoids changing the planner state and preserves the buffer to ensure subsequent gcode motions are still planned correctly, while the stepper module only points to the block buffer head to execute the special system motion. */ -uint8_t plan_buffer_line(float *target, plan_line_data_t *pl_data) -{ - // Prepare and initialize new block. Copy relevant pl_data for block execution. - plan_block_t *block = &block_buffer[block_buffer_head]; - memset(block,0,sizeof(plan_block_t)); // Zero all block values. - block->condition = pl_data->condition; - #ifdef VARIABLE_SPINDLE +uint8_t plan_buffer_line(float *target, plan_line_data_t *pl_data) { + // Prepare and initialize new block. Copy relevant pl_data for block execution. + plan_block_t *block = &block_buffer[block_buffer_head]; + memset(block, 0, sizeof(plan_block_t)); // Zero all block values. + block->condition = pl_data->condition; +#ifdef VARIABLE_SPINDLE block->spindle_speed = pl_data->spindle_speed; - #endif - #ifdef USE_LINE_NUMBERS +#endif +#ifdef USE_LINE_NUMBERS block->line_number = pl_data->line_number; - #endif +#endif - // Compute and store initial move distance data. - int32_t target_steps[N_AXIS], position_steps[N_AXIS]; - float unit_vec[N_AXIS], delta_mm; - uint8_t idx; + // Compute and store initial move distance data. + int32_t target_steps[N_AXIS], position_steps[N_AXIS]; + float unit_vec[N_AXIS], delta_mm; + uint8_t idx; - // Copy position data based on type of motion being planned. - if (block->condition & PL_COND_FLAG_SYSTEM_MOTION) { - #ifdef COREXY - position_steps[X_AXIS] = system_convert_corexy_to_x_axis_steps(sys_position); - position_steps[Y_AXIS] = system_convert_corexy_to_y_axis_steps(sys_position); - position_steps[Z_AXIS] = sys_position[Z_AXIS]; - #else - memcpy(position_steps, sys_position, sizeof(sys_position)); - #endif - } else { memcpy(position_steps, pl.position, sizeof(pl.position)); } - - #ifdef COREXY - target_steps[A_MOTOR] = lround(target[A_MOTOR]*settings.steps_per_mm[A_MOTOR]); - target_steps[B_MOTOR] = lround(target[B_MOTOR]*settings.steps_per_mm[B_MOTOR]); - block->steps[A_MOTOR] = labs((target_steps[X_AXIS]-position_steps[X_AXIS]) + (target_steps[Y_AXIS]-position_steps[Y_AXIS])); - block->steps[B_MOTOR] = labs((target_steps[X_AXIS]-position_steps[X_AXIS]) - (target_steps[Y_AXIS]-position_steps[Y_AXIS])); - #endif - - for (idx=0; idxsteps[idx] = labs(target_steps[idx]-position_steps[idx]); - } - block->step_event_count = max(block->step_event_count, block->steps[idx]); - if (idx == A_MOTOR) { - delta_mm = (target_steps[X_AXIS]-position_steps[X_AXIS] + target_steps[Y_AXIS]-position_steps[Y_AXIS])/settings.steps_per_mm[idx]; - } else if (idx == B_MOTOR) { - delta_mm = (target_steps[X_AXIS]-position_steps[X_AXIS] - target_steps[Y_AXIS]+position_steps[Y_AXIS])/settings.steps_per_mm[idx]; - } else { - delta_mm = (target_steps[idx] - position_steps[idx])/settings.steps_per_mm[idx]; - } - #else - target_steps[idx] = lround(target[idx]*settings.steps_per_mm[idx]); - block->steps[idx] = labs(target_steps[idx]-position_steps[idx]); - block->step_event_count = max(block->step_event_count, block->steps[idx]); - delta_mm = (target_steps[idx] - position_steps[idx])/settings.steps_per_mm[idx]; - #endif - unit_vec[idx] = delta_mm; // Store unit vector numerator - - // Set direction bits. Bit enabled always means direction is negative. - if (delta_mm < 0.0 ) { block->direction_bits |= get_direction_pin_mask(idx); } - } - - // Bail if this is a zero-length block. Highly unlikely to occur. - if (block->step_event_count == 0) { return(PLAN_EMPTY_BLOCK); } - - // Calculate the unit vector of the line move and the block maximum feed rate and acceleration scaled - // down such that no individual axes maximum values are exceeded with respect to the line direction. - // NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes, - // if they are also orthogonal/independent. Operates on the absolute value of the unit vector. - block->millimeters = convert_delta_vector_to_unit_vector(unit_vec); - block->acceleration = limit_value_by_axis_maximum(settings.acceleration, unit_vec); - block->rapid_rate = limit_value_by_axis_maximum(settings.max_rate, unit_vec); - - // Store programmed rate. - if (block->condition & PL_COND_FLAG_RAPID_MOTION) { block->programmed_rate = block->rapid_rate; } - else { - block->programmed_rate = pl_data->feed_rate; - if (block->condition & PL_COND_FLAG_INVERSE_TIME) { block->programmed_rate *= block->millimeters; } - } - - // TODO: Need to check this method handling zero junction speeds when starting from rest. - if ((block_buffer_head == block_buffer_tail) || (block->condition & PL_COND_FLAG_SYSTEM_MOTION)) { - - // Initialize block entry speed as zero. Assume it will be starting from rest. Planner will correct this later. - // If system motion, the system motion block always is assumed to start from rest and end at a complete stop. - block->entry_speed_sqr = 0.0; - block->max_junction_speed_sqr = 0.0; // Starting from rest. Enforce start from zero velocity. - - } else { - // Compute maximum allowable entry speed at junction by centripetal acceleration approximation. - // Let a circle be tangent to both previous and current path line segments, where the junction - // deviation is defined as the distance from the junction to the closest edge of the circle, - // colinear with the circle center. The circular segment joining the two paths represents the - // path of centripetal acceleration. Solve for max velocity based on max acceleration about the - // radius of the circle, defined indirectly by junction deviation. This may be also viewed as - // path width or max_jerk in the previous Grbl version. This approach does not actually deviate - // from path, but used as a robust way to compute cornering speeds, as it takes into account the - // nonlinearities of both the junction angle and junction velocity. - // - // NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path - // mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact - // stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here - // is exactly the same. Instead of motioning all the way to junction point, the machine will - // just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform - // a continuous mode path, but ARM-based microcontrollers most certainly do. - // - // NOTE: The max junction speed is a fixed value, since machine acceleration limits cannot be - // changed dynamically during operation nor can the line move geometry. This must be kept in - // memory in the event of a feedrate override changing the nominal speeds of blocks, which can - // change the overall maximum entry speed conditions of all blocks. - - float junction_unit_vec[N_AXIS]; - float junction_cos_theta = 0.0; - for (idx=0; idx 0.999999) { - // For a 0 degree acute junction, just set minimum junction speed. - block->max_junction_speed_sqr = MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED; + // Copy position data based on type of motion being planned. + if (block->condition & PL_COND_FLAG_SYSTEM_MOTION) { +#ifdef COREXY + position_steps[X_AXIS] = system_convert_corexy_to_x_axis_steps(sys_position); + position_steps[Y_AXIS] = system_convert_corexy_to_y_axis_steps(sys_position); + position_steps[Z_AXIS] = sys_position[Z_AXIS]; +#else + memcpy(position_steps, sys_position, sizeof(sys_position)); +#endif } else { - if (junction_cos_theta < -0.999999) { - // Junction is a straight line or 180 degrees. Junction speed is infinite. - block->max_junction_speed_sqr = SOME_LARGE_VALUE; - } else { - convert_delta_vector_to_unit_vector(junction_unit_vec); - float junction_acceleration = limit_value_by_axis_maximum(settings.acceleration, junction_unit_vec); - float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive. - block->max_junction_speed_sqr = max( MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED, - (junction_acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2) ); - } + memcpy(position_steps, pl.position, sizeof(pl.position)); } - } - // Block system motion from updating this data to ensure next g-code motion is computed correctly. - if (!(block->condition & PL_COND_FLAG_SYSTEM_MOTION)) { - float nominal_speed = plan_compute_profile_nominal_speed(block); - plan_compute_profile_parameters(block, nominal_speed, pl.previous_nominal_speed); - pl.previous_nominal_speed = nominal_speed; - - // Update previous path unit_vector and planner position. - memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[] - memcpy(pl.position, target_steps, sizeof(target_steps)); // pl.position[] = target_steps[] +#ifdef COREXY + target_steps[A_MOTOR] = lround(target[A_MOTOR] * settings.steps_per_mm[A_MOTOR]); + target_steps[B_MOTOR] = lround(target[B_MOTOR] * settings.steps_per_mm[B_MOTOR]); + block->steps[A_MOTOR] = + labs((target_steps[X_AXIS] - position_steps[X_AXIS]) + (target_steps[Y_AXIS] - position_steps[Y_AXIS])); + block->steps[B_MOTOR] = + labs((target_steps[X_AXIS] - position_steps[X_AXIS]) - (target_steps[Y_AXIS] - position_steps[Y_AXIS])); +#endif - // New block is all set. Update buffer head and next buffer head indices. - block_buffer_head = next_buffer_head; - next_buffer_head = plan_next_block_index(block_buffer_head); + for (idx = 0; idx < N_AXIS; idx++) { +// Calculate target position in absolute steps, number of steps for each axis, and determine max step events. +// Also, compute individual axes distance for move and prep unit vector calculations. +// NOTE: Computes true distance from converted step values. +#ifdef COREXY + if (!(idx == A_MOTOR) && !(idx == B_MOTOR)) { + target_steps[idx] = lround(target[idx] * settings.steps_per_mm[idx]); + block->steps[idx] = labs(target_steps[idx] - position_steps[idx]); + } + block->step_event_count = max(block->step_event_count, block->steps[idx]); + if (idx == A_MOTOR) { + delta_mm = (target_steps[X_AXIS] - position_steps[X_AXIS] + target_steps[Y_AXIS] - position_steps[Y_AXIS]) / + settings.steps_per_mm[idx]; + } else if (idx == B_MOTOR) { + delta_mm = (target_steps[X_AXIS] - position_steps[X_AXIS] - target_steps[Y_AXIS] + position_steps[Y_AXIS]) / + settings.steps_per_mm[idx]; + } else { + delta_mm = (target_steps[idx] - position_steps[idx]) / settings.steps_per_mm[idx]; + } +#else + target_steps[idx] = lround(target[idx] * settings.steps_per_mm[idx]); + block->steps[idx] = labs(target_steps[idx] - position_steps[idx]); + block->step_event_count = max(block->step_event_count, block->steps[idx]); + delta_mm = (target_steps[idx] - position_steps[idx]) / settings.steps_per_mm[idx]; +#endif + unit_vec[idx] = delta_mm; // Store unit vector numerator - // Finish up by recalculating the plan with the new block. - planner_recalculate(); - } - return(PLAN_OK); + // Set direction bits. Bit enabled always means direction is negative. + if (delta_mm < 0.0) { + block->direction_bits |= get_direction_pin_mask(idx); + } + } + + // Bail if this is a zero-length block. Highly unlikely to occur. + if (block->step_event_count == 0) { + return (PLAN_EMPTY_BLOCK); + } + + // Calculate the unit vector of the line move and the block maximum feed rate and acceleration scaled + // down such that no individual axes maximum values are exceeded with respect to the line direction. + // NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes, + // if they are also orthogonal/independent. Operates on the absolute value of the unit vector. + block->millimeters = convert_delta_vector_to_unit_vector(unit_vec); + block->acceleration = limit_value_by_axis_maximum(settings.acceleration, unit_vec); + block->rapid_rate = limit_value_by_axis_maximum(settings.max_rate, unit_vec); + + // Store programmed rate. + if (block->condition & PL_COND_FLAG_RAPID_MOTION) { + block->programmed_rate = block->rapid_rate; + } else { + block->programmed_rate = pl_data->feed_rate; + if (block->condition & PL_COND_FLAG_INVERSE_TIME) { + block->programmed_rate *= block->millimeters; + } + } + + // TODO: Need to check this method handling zero junction speeds when starting from rest. + if ((block_buffer_head == block_buffer_tail) || (block->condition & PL_COND_FLAG_SYSTEM_MOTION)) { + + // Initialize block entry speed as zero. Assume it will be starting from rest. Planner will correct this later. + // If system motion, the system motion block always is assumed to start from rest and end at a complete stop. + block->entry_speed_sqr = 0.0; + block->max_junction_speed_sqr = 0.0; // Starting from rest. Enforce start from zero velocity. + + } else { + // Compute maximum allowable entry speed at junction by centripetal acceleration approximation. + // Let a circle be tangent to both previous and current path line segments, where the junction + // deviation is defined as the distance from the junction to the closest edge of the circle, + // colinear with the circle center. The circular segment joining the two paths represents the + // path of centripetal acceleration. Solve for max velocity based on max acceleration about the + // radius of the circle, defined indirectly by junction deviation. This may be also viewed as + // path width or max_jerk in the previous Grbl version. This approach does not actually deviate + // from path, but used as a robust way to compute cornering speeds, as it takes into account the + // nonlinearities of both the junction angle and junction velocity. + // + // NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path + // mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact + // stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here + // is exactly the same. Instead of motioning all the way to junction point, the machine will + // just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform + // a continuous mode path, but ARM-based microcontrollers most certainly do. + // + // NOTE: The max junction speed is a fixed value, since machine acceleration limits cannot be + // changed dynamically during operation nor can the line move geometry. This must be kept in + // memory in the event of a feedrate override changing the nominal speeds of blocks, which can + // change the overall maximum entry speed conditions of all blocks. + + float junction_unit_vec[N_AXIS]; + float junction_cos_theta = 0.0; + for (idx = 0; idx < N_AXIS; idx++) { + junction_cos_theta -= pl.previous_unit_vec[idx] * unit_vec[idx]; + junction_unit_vec[idx] = unit_vec[idx] - pl.previous_unit_vec[idx]; + } + + // NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta). + if (junction_cos_theta > 0.999999) { + // For a 0 degree acute junction, just set minimum junction speed. + block->max_junction_speed_sqr = MINIMUM_JUNCTION_SPEED * MINIMUM_JUNCTION_SPEED; + } else { + if (junction_cos_theta < -0.999999) { + // Junction is a straight line or 180 degrees. Junction speed is infinite. + block->max_junction_speed_sqr = SOME_LARGE_VALUE; + } else { + convert_delta_vector_to_unit_vector(junction_unit_vec); + float junction_acceleration = limit_value_by_axis_maximum(settings.acceleration, junction_unit_vec); + float sin_theta_d2 = + sqrt(0.5 * (1.0 - junction_cos_theta)); // Trig half angle identity. Always positive. + block->max_junction_speed_sqr = + max(MINIMUM_JUNCTION_SPEED * MINIMUM_JUNCTION_SPEED, + (junction_acceleration * settings.junction_deviation * sin_theta_d2) / (1.0 - sin_theta_d2)); + } + } + } + + // Block system motion from updating this data to ensure next g-code motion is computed correctly. + if (!(block->condition & PL_COND_FLAG_SYSTEM_MOTION)) { + float nominal_speed = plan_compute_profile_nominal_speed(block); + plan_compute_profile_parameters(block, nominal_speed, pl.previous_nominal_speed); + pl.previous_nominal_speed = nominal_speed; + + // Update previous path unit_vector and planner position. + memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[] + memcpy(pl.position, target_steps, sizeof(target_steps)); // pl.position[] = target_steps[] + + // New block is all set. Update buffer head and next buffer head indices. + block_buffer_head = next_buffer_head; + next_buffer_head = plan_next_block_index(block_buffer_head); + + // Finish up by recalculating the plan with the new block. + planner_recalculate(); + } + return (PLAN_OK); } - // Reset the planner position vectors. Called by the system abort/initialization routine. -void plan_sync_position() -{ - // TODO: For motor configurations not in the same coordinate frame as the machine position, - // this function needs to be updated to accomodate the difference. - uint8_t idx; - for (idx=0; idx= block_buffer_tail) { return((BLOCK_BUFFER_SIZE-1)-(block_buffer_head-block_buffer_tail)); } - return((block_buffer_tail-block_buffer_head-1)); +uint8_t plan_get_block_buffer_available() { + if (block_buffer_head >= block_buffer_tail) { + return ((BLOCK_BUFFER_SIZE - 1) - (block_buffer_head - block_buffer_tail)); + } + return ((block_buffer_tail - block_buffer_head - 1)); } - // Returns the number of active blocks are in the planner buffer. // NOTE: Deprecated. Not used unless classic status reports are enabled in config.h -uint8_t plan_get_block_buffer_count() -{ - if (block_buffer_head >= block_buffer_tail) { return(block_buffer_head-block_buffer_tail); } - return(BLOCK_BUFFER_SIZE - (block_buffer_tail-block_buffer_head)); +uint8_t plan_get_block_buffer_count() { + if (block_buffer_head >= block_buffer_tail) { + return (block_buffer_head - block_buffer_tail); + } + return (BLOCK_BUFFER_SIZE - (block_buffer_tail - block_buffer_head)); } - // Re-initialize buffer plan with a partially completed block, assumed to exist at the buffer tail. // Called after a steppers have come to a complete stop for a feed hold and the cycle is stopped. -void plan_cycle_reinitialize() -{ - // Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer. - st_update_plan_block_parameters(); - block_buffer_planned = block_buffer_tail; - planner_recalculate(); +void plan_cycle_reinitialize() { + // Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer. + st_update_plan_block_parameters(); + block_buffer_planned = block_buffer_tail; + planner_recalculate(); } diff --git a/grbl/planner.h b/grbl/planner.h index e3751e1..206b181 100644 --- a/grbl/planner.h +++ b/grbl/planner.h @@ -22,83 +22,80 @@ #ifndef planner_h #define planner_h - // The number of linear motions that can be in the plan at any give time #ifndef BLOCK_BUFFER_SIZE - #ifdef USE_LINE_NUMBERS - #define BLOCK_BUFFER_SIZE 15 - #else - #define BLOCK_BUFFER_SIZE 16 - #endif +#ifdef USE_LINE_NUMBERS +#define BLOCK_BUFFER_SIZE 15 +#else +#define BLOCK_BUFFER_SIZE 16 +#endif #endif // Returned status message from planner. -#define PLAN_OK true +#define PLAN_OK true #define PLAN_EMPTY_BLOCK false // Define planner data condition flags. Used to denote running conditions of a block. -#define PL_COND_FLAG_RAPID_MOTION bit(0) -#define PL_COND_FLAG_SYSTEM_MOTION bit(1) // Single motion. Circumvents planner state. Used by home/park. -#define PL_COND_FLAG_NO_FEED_OVERRIDE bit(2) // Motion does not honor feed override. -#define PL_COND_FLAG_INVERSE_TIME bit(3) // Interprets feed rate value as inverse time when set. -#define PL_COND_FLAG_SPINDLE_CW bit(4) -#define PL_COND_FLAG_SPINDLE_CCW bit(5) -#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) - +#define PL_COND_FLAG_RAPID_MOTION bit(0) +#define PL_COND_FLAG_SYSTEM_MOTION bit(1) // Single motion. Circumvents planner state. Used by home/park. +#define PL_COND_FLAG_NO_FEED_OVERRIDE bit(2) // Motion does not honor feed override. +#define PL_COND_FLAG_INVERSE_TIME bit(3) // Interprets feed rate value as inverse time when set. +#define PL_COND_FLAG_SPINDLE_CW bit(4) +#define PL_COND_FLAG_SPINDLE_CCW bit(5) +#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) // This struct stores a linear movement of a g-code block motion with its critical "nominal" values // are as specified in the source g-code. typedef struct { - // Fields used by the bresenham algorithm for tracing the line - // NOTE: Used by stepper algorithm to execute the block correctly. Do not alter these values. - uint32_t steps[N_AXIS]; // Step count along each axis - uint32_t step_event_count; // The maximum step axis count and number of steps required to complete this block. - uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) + // Fields used by the bresenham algorithm for tracing the line + // NOTE: Used by stepper algorithm to execute the block correctly. Do not alter these values. + uint32_t steps[N_AXIS]; // Step count along each axis + uint32_t step_event_count; // The maximum step axis count and number of steps required to complete this block. + uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) - // Block condition data to ensure correct execution depending on states and overrides. - uint8_t condition; // Block bitflag variable defining block run conditions. Copied from pl_line_data. - #ifdef USE_LINE_NUMBERS - int32_t line_number; // Block line number for real-time reporting. Copied from pl_line_data. - #endif + // Block condition data to ensure correct execution depending on states and overrides. + uint8_t condition; // Block bitflag variable defining block run conditions. Copied from pl_line_data. +#ifdef USE_LINE_NUMBERS + int32_t line_number; // Block line number for real-time reporting. Copied from pl_line_data. +#endif - // Fields used by the motion planner to manage acceleration. Some of these values may be updated - // by the stepper module during execution of special motion cases for replanning purposes. - float entry_speed_sqr; // The current planned entry speed at block junction in (mm/min)^2 - float max_entry_speed_sqr; // Maximum allowable entry speed based on the minimum of junction limit and - // neighboring nominal speeds with overrides in (mm/min)^2 - float acceleration; // Axis-limit adjusted line acceleration in (mm/min^2). Does not change. - float millimeters; // The remaining distance for this block to be executed in (mm). - // NOTE: This value may be altered by stepper algorithm during execution. + // Fields used by the motion planner to manage acceleration. Some of these values may be updated + // by the stepper module during execution of special motion cases for replanning purposes. + float entry_speed_sqr; // The current planned entry speed at block junction in (mm/min)^2 + float max_entry_speed_sqr; // Maximum allowable entry speed based on the minimum of junction limit and + // neighboring nominal speeds with overrides in (mm/min)^2 + float acceleration; // Axis-limit adjusted line acceleration in (mm/min^2). Does not change. + float millimeters; // The remaining distance for this block to be executed in (mm). + // NOTE: This value may be altered by stepper algorithm during execution. - // Stored rate limiting data used by planner when changes occur. - float max_junction_speed_sqr; // Junction entry speed limit based on direction vectors in (mm/min)^2 - float rapid_rate; // Axis-limit adjusted maximum rate for this block direction in (mm/min) - float programmed_rate; // Programmed rate of this block (mm/min). + // Stored rate limiting data used by planner when changes occur. + float max_junction_speed_sqr; // Junction entry speed limit based on direction vectors in (mm/min)^2 + float rapid_rate; // Axis-limit adjusted maximum rate for this block direction in (mm/min) + float programmed_rate; // Programmed rate of this block (mm/min). - #ifdef VARIABLE_SPINDLE - // Stored spindle speed data used by spindle overrides and resuming methods. - float spindle_speed; // Block spindle speed. Copied from pl_line_data. - #endif +#ifdef VARIABLE_SPINDLE + // Stored spindle speed data used by spindle overrides and resuming methods. + float spindle_speed; // Block spindle speed. Copied from pl_line_data. +#endif } plan_block_t; - // Planner data prototype. Must be used when passing new motions to the planner. typedef struct { - float feed_rate; // Desired feed rate for line motion. Value is ignored, if rapid motion. - float spindle_speed; // Desired spindle speed through line motion. - uint8_t condition; // Bitflag variable to indicate planner conditions. See defines above. - #ifdef USE_LINE_NUMBERS - int32_t line_number; // Desired line number to report when executing. - #endif + float feed_rate; // Desired feed rate for line motion. Value is ignored, if rapid motion. + float spindle_speed; // Desired spindle speed through line motion. + uint8_t condition; // Bitflag variable to indicate planner conditions. See defines above. +#ifdef USE_LINE_NUMBERS + int32_t line_number; // Desired line number to report when executing. +#endif } plan_line_data_t; - // Initialize and reset the motion plan subsystem -void plan_reset(); // Reset all +void plan_reset(); // Reset all void plan_reset_buffer(); // Reset buffer only. // Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position @@ -146,5 +143,4 @@ uint8_t plan_check_full_buffer(); void plan_get_planner_mpos(float *target); - #endif diff --git a/grbl/print.c b/grbl/print.c index 771e399..657da09 100644 --- a/grbl/print.c +++ b/grbl/print.c @@ -21,23 +21,16 @@ #include "grbl.h" - -void printString(const char *s) -{ - while (*s) - serial_write(*s++); +void printString(const char *s) { + while (*s) serial_write(*s++); } - // Print a string stored in PGM-memory -void printPgmString(const char *s) -{ - char c; - while ((c = pgm_read_byte_near(s++))) - serial_write(c); +void printPgmString(const char *s) { + char c; + while ((c = pgm_read_byte_near(s++))) serial_write(c); } - // void printIntegerInBase(unsigned long n, unsigned long base) // { // unsigned char buf[8 * sizeof(long)]; // Assumes 8-bit chars. @@ -59,133 +52,129 @@ void printPgmString(const char *s) // 'A' + buf[i - 1] - 10); // } - // Prints an uint8 variable in base 10. -void print_uint8_base10(uint8_t n) -{ - uint8_t digit_a = 0; - uint8_t digit_b = 0; - if (n >= 100) { // 100-255 - digit_a = '0' + n % 10; - n /= 10; - } - if (n >= 10) { // 10-99 - digit_b = '0' + n % 10; - n /= 10; - } - serial_write('0' + n); - if (digit_b) { serial_write(digit_b); } - if (digit_a) { serial_write(digit_a); } +void print_uint8_base10(uint8_t n) { + uint8_t digit_a = 0; + uint8_t digit_b = 0; + if (n >= 100) { // 100-255 + digit_a = '0' + n % 10; + n /= 10; + } + if (n >= 10) { // 10-99 + digit_b = '0' + n % 10; + n /= 10; + } + serial_write('0' + n); + if (digit_b) { + serial_write(digit_b); + } + if (digit_a) { + serial_write(digit_a); + } } - // Prints an uint8 variable in base 2 with desired number of desired digits. void print_uint8_base2_ndigit(uint8_t n, uint8_t digits) { - unsigned char buf[digits]; - uint8_t i = 0; + unsigned char buf[digits]; + uint8_t i = 0; - for (; i < digits; i++) { - buf[i] = n % 2 ; - n /= 2; - } + for (; i < digits; i++) { + buf[i] = n % 2; + n /= 2; + } - for (; i > 0; i--) - serial_write('0' + buf[i - 1]); + for (; i > 0; i--) serial_write('0' + buf[i - 1]); } +void print_uint32_base10(uint32_t n) { + if (n == 0) { + serial_write('0'); + return; + } -void print_uint32_base10(uint32_t n) -{ - if (n == 0) { - serial_write('0'); - return; - } + unsigned char buf[10]; + uint8_t i = 0; - unsigned char buf[10]; - uint8_t i = 0; + while (n > 0) { + buf[i++] = n % 10; + n /= 10; + } - while (n > 0) { - buf[i++] = n % 10; - n /= 10; - } - - for (; i > 0; i--) - serial_write('0' + buf[i-1]); + for (; i > 0; i--) serial_write('0' + buf[i - 1]); } - -void printInteger(long n) -{ - if (n < 0) { - serial_write('-'); - print_uint32_base10(-n); - } else { - print_uint32_base10(n); - } +void printInteger(long n) { + if (n < 0) { + serial_write('-'); + print_uint32_base10(-n); + } else { + print_uint32_base10(n); + } } - // Convert float to string by immediately converting to a long integer, which contains // more digits than a float. Number of decimal places, which are tracked by a counter, // may be set by the user. The integer is then efficiently converted to a string. // NOTE: AVR '%' and '/' integer operations are very efficient. Bitshifting speed-up // techniques are actually just slightly slower. Found this out the hard way. -void printFloat(float n, uint8_t decimal_places) -{ - if (n < 0) { - serial_write('-'); - n = -n; - } +void printFloat(float n, uint8_t decimal_places) { + if (n < 0) { + serial_write('-'); + n = -n; + } - uint8_t decimals = decimal_places; - while (decimals >= 2) { // Quickly convert values expected to be E0 to E-4. - n *= 100; - decimals -= 2; - } - if (decimals) { n *= 10; } - n += 0.5; // Add rounding factor. Ensures carryover through entire value. + uint8_t decimals = decimal_places; + while (decimals >= 2) { // Quickly convert values expected to be E0 to E-4. + n *= 100; + decimals -= 2; + } + if (decimals) { + n *= 10; + } + n += 0.5; // Add rounding factor. Ensures carryover through entire value. - // Generate digits backwards and store in string. - unsigned char buf[13]; - uint8_t i = 0; - uint32_t a = (long)n; - while(a > 0) { - buf[i++] = (a % 10) + '0'; // Get digit - a /= 10; - } - while (i < decimal_places) { - buf[i++] = '0'; // Fill in zeros to decimal point for (n < 1) - } - if (i == decimal_places) { // Fill in leading zero, if needed. - buf[i++] = '0'; - } + // Generate digits backwards and store in string. + unsigned char buf[13]; + uint8_t i = 0; + uint32_t a = (long)n; + while (a > 0) { + buf[i++] = (a % 10) + '0'; // Get digit + a /= 10; + } + while (i < decimal_places) { + buf[i++] = '0'; // Fill in zeros to decimal point for (n < 1) + } + if (i == decimal_places) { // Fill in leading zero, if needed. + buf[i++] = '0'; + } - // Print the generated string. - for (; i > 0; i--) { - if (i == decimal_places) { serial_write('.'); } // Insert decimal point in right place. - serial_write(buf[i-1]); - } + // Print the generated string. + for (; i > 0; i--) { + if (i == decimal_places) { + serial_write('.'); + } // Insert decimal point in right place. + serial_write(buf[i - 1]); + } } - // Floating value printing handlers for special variables types used in Grbl and are defined // in the config.h. // - CoordValue: Handles all position or coordinate values in inches or mm reporting. // - RateValue: Handles feed rate and current velocity in inches or mm reporting. void printFloat_CoordValue(float n) { - if (bit_istrue(settings.flags,BITFLAG_REPORT_INCHES)) { - printFloat(n*INCH_PER_MM,N_DECIMAL_COORDVALUE_INCH); - } else { - printFloat(n,N_DECIMAL_COORDVALUE_MM); - } + if (bit_istrue(settings.flags, BITFLAG_REPORT_INCHES)) { + printFloat(n * INCH_PER_MM, N_DECIMAL_COORDVALUE_INCH); + } else { + printFloat(n, N_DECIMAL_COORDVALUE_MM); + } } void printFloat_RateValue(float n) { - if (bit_istrue(settings.flags,BITFLAG_REPORT_INCHES)) { - printFloat(n*INCH_PER_MM,N_DECIMAL_RATEVALUE_INCH); - } else { - printFloat(n,N_DECIMAL_RATEVALUE_MM); - } + if (bit_istrue(settings.flags, BITFLAG_REPORT_INCHES)) { + printFloat(n * INCH_PER_MM, N_DECIMAL_RATEVALUE_INCH); + } else { + printFloat(n, N_DECIMAL_RATEVALUE_MM); + } } // Debug tool to print free memory in bytes at the called point. diff --git a/grbl/print.h b/grbl/print.h index 31e0a57..b71add6 100644 --- a/grbl/print.h +++ b/grbl/print.h @@ -22,7 +22,6 @@ #ifndef print_h #define print_h - void printString(const char *s); void printPgmString(const char *s); diff --git a/grbl/probe.c b/grbl/probe.c index 60c9073..c72dd7e 100644 --- a/grbl/probe.c +++ b/grbl/probe.c @@ -20,47 +20,45 @@ #include "grbl.h" - // Inverts the probe pin state depending on user settings and probing cycle mode. uint8_t probe_invert_mask; - // Probe pin initialization routine. -void probe_init() -{ - PROBE_DDR &= ~(PROBE_MASK); // Configure as input pins - #ifdef DISABLE_PROBE_PIN_PULL_UP +void probe_init() { + PROBE_DDR &= ~(PROBE_MASK); // Configure as input pins +#ifdef DISABLE_PROBE_PIN_PULL_UP PROBE_PORT &= ~(PROBE_MASK); // Normal low operation. Requires external pull-down. - #else - PROBE_PORT |= PROBE_MASK; // Enable internal pull-up resistors. Normal high operation. - #endif - probe_configure_invert_mask(false); // Initialize invert mask. +#else + PROBE_PORT |= PROBE_MASK; // Enable internal pull-up resistors. Normal high operation. +#endif + probe_configure_invert_mask(false); // Initialize invert mask. } - // Called by probe_init() and the mc_probe() routines. Sets up the probe pin invert mask to // appropriately set the pin logic according to setting for normal-high/normal-low operation // and the probing cycle modes for toward-workpiece/away-from-workpiece. -void probe_configure_invert_mask(uint8_t is_probe_away) -{ - probe_invert_mask = 0; // Initialize as zero. - if (bit_isfalse(settings.flags,BITFLAG_INVERT_PROBE_PIN)) { probe_invert_mask ^= PROBE_MASK; } - if (is_probe_away) { probe_invert_mask ^= PROBE_MASK; } +void probe_configure_invert_mask(uint8_t is_probe_away) { + probe_invert_mask = 0; // Initialize as zero. + if (bit_isfalse(settings.flags, BITFLAG_INVERT_PROBE_PIN)) { + probe_invert_mask ^= PROBE_MASK; + } + if (is_probe_away) { + probe_invert_mask ^= PROBE_MASK; + } } - // Returns the probe pin state. Triggered = true. Called by gcode parser and probe state monitor. -uint8_t probe_get_state() { return((PROBE_PIN & PROBE_MASK) ^ probe_invert_mask); } - +uint8_t probe_get_state() { + return ((PROBE_PIN & PROBE_MASK) ^ probe_invert_mask); +} // Monitors probe pin state and records the system position when detected. Called by the // stepper ISR per ISR tick. // NOTE: This function must be extremely efficient as to not bog down the stepper ISR. -void probe_state_monitor() -{ - if (probe_get_state()) { - sys_probe_state = PROBE_OFF; - memcpy(sys_probe_position, sys_position, sizeof(sys_position)); - bit_true(sys_rt_exec_state, EXEC_MOTION_CANCEL); - } +void probe_state_monitor() { + if (probe_get_state()) { + sys_probe_state = PROBE_OFF; + memcpy(sys_probe_position, sys_position, sizeof(sys_position)); + bit_true(sys_rt_exec_state, EXEC_MOTION_CANCEL); + } } diff --git a/grbl/probe.h b/grbl/probe.h index 03d5fd3..1269312 100644 --- a/grbl/probe.h +++ b/grbl/probe.h @@ -22,8 +22,8 @@ #define probe_h // Values that define the probing state machine. -#define PROBE_OFF 0 // Probing disabled or not in use. (Must be zero.) -#define PROBE_ACTIVE 1 // Actively watching the input pin. +#define PROBE_OFF 0 // Probing disabled or not in use. (Must be zero.) +#define PROBE_ACTIVE 1 // Actively watching the input pin. // Probe pin initialization routine. void probe_init(); diff --git a/grbl/protocol.c b/grbl/protocol.c index 08ea48b..e12ff51 100644 --- a/grbl/protocol.c +++ b/grbl/protocol.c @@ -22,175 +22,174 @@ #include "grbl.h" // Define line flags. Includes comment type tracking and line overflow detection. -#define LINE_FLAG_OVERFLOW bit(0) +#define LINE_FLAG_OVERFLOW bit(0) #define LINE_FLAG_COMMENT_PARENTHESES bit(1) -#define LINE_FLAG_COMMENT_SEMICOLON bit(2) - +#define LINE_FLAG_COMMENT_SEMICOLON bit(2) static char line[LINE_BUFFER_SIZE]; // Line to be executed. Zero-terminated. static void protocol_exec_rt_suspend(); - /* GRBL PRIMARY LOOP: */ -void protocol_main_loop() -{ - // Perform some machine checks to make sure everything is good to go. - #ifdef CHECK_LIMITS_AT_INIT +void protocol_main_loop() { +// Perform some machine checks to make sure everything is good to go. +#ifdef CHECK_LIMITS_AT_INIT if (bit_istrue(settings.flags, BITFLAG_HARD_LIMIT_ENABLE)) { - if (limits_get_state()) { - sys.state = STATE_ALARM; // Ensure alarm state is active. - report_feedback_message(MESSAGE_CHECK_LIMITS); - } + if (limits_get_state()) { + sys.state = STATE_ALARM; // Ensure alarm state is active. + report_feedback_message(MESSAGE_CHECK_LIMITS); + } } - #endif - // Check for and report alarm state after a reset, error, or an initial power up. - // NOTE: Sleep mode disables the stepper drivers and position can't be guaranteed. - // Re-initialize the sleep state as an ALARM mode to ensure user homes or acknowledges. - if (sys.state & (STATE_ALARM | STATE_SLEEP)) { - report_feedback_message(MESSAGE_ALARM_LOCK); - sys.state = STATE_ALARM; // Ensure alarm state is set. - } else { - // Check if the safety door is open. - sys.state = STATE_IDLE; - if (system_check_safety_door_ajar()) { - bit_true(sys_rt_exec_state, EXEC_SAFETY_DOOR); - protocol_execute_realtime(); // Enter safety door mode. Should return as IDLE state. +#endif + // Check for and report alarm state after a reset, error, or an initial power up. + // NOTE: Sleep mode disables the stepper drivers and position can't be guaranteed. + // Re-initialize the sleep state as an ALARM mode to ensure user homes or acknowledges. + if (sys.state & (STATE_ALARM | STATE_SLEEP)) { + report_feedback_message(MESSAGE_ALARM_LOCK); + sys.state = STATE_ALARM; // Ensure alarm state is set. + } else { + // Check if the safety door is open. + sys.state = STATE_IDLE; + if (system_check_safety_door_ajar()) { + bit_true(sys_rt_exec_state, EXEC_SAFETY_DOOR); + protocol_execute_realtime(); // Enter safety door mode. Should return as IDLE state. + } + // All systems go! + system_execute_startup(line); // Execute startup script. } - // All systems go! - system_execute_startup(line); // Execute startup script. - } - // --------------------------------------------------------------------------------- - // Primary loop! Upon a system abort, this exits back to main() to reset the system. - // This is also where Grbl idles while waiting for something to do. - // --------------------------------------------------------------------------------- + // --------------------------------------------------------------------------------- + // Primary loop! Upon a system abort, this exits back to main() to reset the system. + // This is also where Grbl idles while waiting for something to do. + // --------------------------------------------------------------------------------- - uint8_t line_flags = 0; - uint8_t char_counter = 0; - uint8_t c; - for (;;) { + uint8_t line_flags = 0; + uint8_t char_counter = 0; + uint8_t c; + for (;;) { - // Process one line of incoming serial data, as the data becomes available. Performs an - // initial filtering by removing spaces and comments and capitalizing all letters. - while((c = serial_read()) != SERIAL_NO_DATA) { - if ((c == '\n') || (c == '\r')) { // End of line reached + // Process one line of incoming serial data, as the data becomes available. Performs an + // initial filtering by removing spaces and comments and capitalizing all letters. + while ((c = serial_read()) != SERIAL_NO_DATA) { + if ((c == '\n') || (c == '\r')) { // End of line reached + + protocol_execute_realtime(); // Runtime command check point. + if (sys.abort) { + return; + } // Bail to calling function upon system abort + + line[char_counter] = 0; // Set string termination character. +#ifdef REPORT_ECHO_LINE_RECEIVED + report_echo_line_received(line); +#endif + + // Direct and execute one line of formatted input, and report status of execution. + if (line_flags & LINE_FLAG_OVERFLOW) { + // Report line overflow error. + report_status_message(STATUS_OVERFLOW); + } else if (line[0] == 0) { + // Empty or comment line. For syncing purposes. + report_status_message(STATUS_OK); + } else if (line[0] == '$') { + // Grbl '$' system command + report_status_message(system_execute_line(line)); + } else if (sys.state & (STATE_ALARM | STATE_JOG)) { + // Everything else is gcode. Block if in alarm or jog mode. + report_status_message(STATUS_SYSTEM_GC_LOCK); + } else { + // Parse and execute g-code block. + report_status_message(gc_execute_line(line)); + } + + // Reset tracking data for next line. + line_flags = 0; + char_counter = 0; + + } else { + + if (line_flags) { + // Throw away all (except EOL) comment characters and overflow characters. + if (c == ')') { + // End of '()' comment. Resume line allowed. + if (line_flags & LINE_FLAG_COMMENT_PARENTHESES) { + line_flags &= ~(LINE_FLAG_COMMENT_PARENTHESES); + } + } + } else { + if (c <= ' ') { + // Throw away whitepace and control characters + } else if (c == '/') { + // Block delete NOT SUPPORTED. Ignore character. + // NOTE: If supported, would simply need to check the system if block delete is enabled. + } else if (c == '(') { + // Enable comments flag and ignore all characters until ')' or EOL. + // NOTE: This doesn't follow the NIST definition exactly, but is good enough for now. + // In the future, we could simply remove the items within the comments, but retain the + // comment control characters, so that the g-code parser can error-check it. + line_flags |= LINE_FLAG_COMMENT_PARENTHESES; + } else if (c == ';') { + // NOTE: ';' comment to EOL is a LinuxCNC definition. Not NIST. + line_flags |= LINE_FLAG_COMMENT_SEMICOLON; + // TODO: Install '%' feature + // } else if (c == '%') { + // Program start-end percent sign NOT SUPPORTED. + // NOTE: This maybe installed to tell Grbl when a program is running vs manual input, + // where, during a program, the system auto-cycle start will continue to execute + // everything until the next '%' sign. This will help fix resuming issues with certain + // functions that empty the planner buffer to execute its task on-time. + } else if (char_counter >= (LINE_BUFFER_SIZE - 1)) { + // Detect line buffer overflow and set flag. + line_flags |= LINE_FLAG_OVERFLOW; + } else if (c >= 'a' && c <= 'z') { // Upcase lowercase + line[char_counter++] = c - 'a' + 'A'; + } else { + line[char_counter++] = c; + } + } + } + } + + // If there are no more characters in the serial read buffer to be processed and executed, + // this indicates that g-code streaming has either filled the planner buffer or has + // completed. In either case, auto-cycle start, if enabled, any queued moves. + protocol_auto_cycle_start(); protocol_execute_realtime(); // Runtime command check point. - if (sys.abort) { return; } // Bail to calling function upon system abort - - line[char_counter] = 0; // Set string termination character. - #ifdef REPORT_ECHO_LINE_RECEIVED - report_echo_line_received(line); - #endif - - // Direct and execute one line of formatted input, and report status of execution. - if (line_flags & LINE_FLAG_OVERFLOW) { - // Report line overflow error. - report_status_message(STATUS_OVERFLOW); - } else if (line[0] == 0) { - // Empty or comment line. For syncing purposes. - report_status_message(STATUS_OK); - } else if (line[0] == '$') { - // Grbl '$' system command - report_status_message(system_execute_line(line)); - } else if (sys.state & (STATE_ALARM | STATE_JOG)) { - // Everything else is gcode. Block if in alarm or jog mode. - report_status_message(STATUS_SYSTEM_GC_LOCK); - } else { - // Parse and execute g-code block. - report_status_message(gc_execute_line(line)); - } - - // Reset tracking data for next line. - line_flags = 0; - char_counter = 0; - - } else { - - if (line_flags) { - // Throw away all (except EOL) comment characters and overflow characters. - if (c == ')') { - // End of '()' comment. Resume line allowed. - if (line_flags & LINE_FLAG_COMMENT_PARENTHESES) { line_flags &= ~(LINE_FLAG_COMMENT_PARENTHESES); } - } - } else { - if (c <= ' ') { - // Throw away whitepace and control characters - } else if (c == '/') { - // Block delete NOT SUPPORTED. Ignore character. - // NOTE: If supported, would simply need to check the system if block delete is enabled. - } else if (c == '(') { - // Enable comments flag and ignore all characters until ')' or EOL. - // NOTE: This doesn't follow the NIST definition exactly, but is good enough for now. - // In the future, we could simply remove the items within the comments, but retain the - // comment control characters, so that the g-code parser can error-check it. - line_flags |= LINE_FLAG_COMMENT_PARENTHESES; - } else if (c == ';') { - // NOTE: ';' comment to EOL is a LinuxCNC definition. Not NIST. - line_flags |= LINE_FLAG_COMMENT_SEMICOLON; - // TODO: Install '%' feature - // } else if (c == '%') { - // Program start-end percent sign NOT SUPPORTED. - // NOTE: This maybe installed to tell Grbl when a program is running vs manual input, - // where, during a program, the system auto-cycle start will continue to execute - // everything until the next '%' sign. This will help fix resuming issues with certain - // functions that empty the planner buffer to execute its task on-time. - } else if (char_counter >= (LINE_BUFFER_SIZE-1)) { - // Detect line buffer overflow and set flag. - line_flags |= LINE_FLAG_OVERFLOW; - } else if (c >= 'a' && c <= 'z') { // Upcase lowercase - line[char_counter++] = c-'a'+'A'; - } else { - line[char_counter++] = c; - } - } - - } + if (sys.abort) { + return; + } // Bail to main() program loop to reset system. } - // If there are no more characters in the serial read buffer to be processed and executed, - // this indicates that g-code streaming has either filled the planner buffer or has - // completed. In either case, auto-cycle start, if enabled, any queued moves. - protocol_auto_cycle_start(); - - protocol_execute_realtime(); // Runtime command check point. - if (sys.abort) { return; } // Bail to main() program loop to reset system. - } - - return; /* Never reached */ + return; /* Never reached */ } - // Block until all buffered steps are executed or in a cycle state. Works with feed hold // during a synchronize call, if it should happen. Also, waits for clean cycle end. -void protocol_buffer_synchronize() -{ - // If system is queued, ensure cycle resumes if the auto start flag is present. - protocol_auto_cycle_start(); - do { - protocol_execute_realtime(); // Check and execute run-time commands - if (sys.abort) { return; } // Check for system abort - } while (plan_get_current_block() || (sys.state == STATE_CYCLE)); +void protocol_buffer_synchronize() { + // If system is queued, ensure cycle resumes if the auto start flag is present. + protocol_auto_cycle_start(); + do { + protocol_execute_realtime(); // Check and execute run-time commands + if (sys.abort) { + return; + } // Check for system abort + } while (plan_get_current_block() || (sys.state == STATE_CYCLE)); } - // Auto-cycle start triggers when there is a motion ready to execute and if the main program is not // actively parsing commands. // NOTE: This function is called from the main loop, buffer sync, and mc_line() only and executes // when one of these conditions exist respectively: There are no more blocks sent (i.e. streaming // is finished, single commands), a command that needs to wait for the motions in the buffer to // execute calls a buffer sync, or the planner buffer is full and ready to go. -void protocol_auto_cycle_start() -{ - if (plan_get_current_block() != NULL) { // Check if there are any blocks in the buffer. - system_set_exec_state_flag(EXEC_CYCLE_START); // If so, execute them! - } +void protocol_auto_cycle_start() { + if (plan_get_current_block() != NULL) { // Check if there are any blocks in the buffer. + system_set_exec_state_flag(EXEC_CYCLE_START); // If so, execute them! + } } - // This function is the general interface to Grbl's real-time command execution system. It is called // from various check points in the main program, primarily where there may be a while loop waiting // for a buffer to clear space or any point where the execution time from the last check point may @@ -202,564 +201,637 @@ void protocol_auto_cycle_start() // the same task, such as the planner recalculating the buffer upon a feedhold or overrides. // NOTE: The sys_rt_exec_state variable flags are set by any process, step or serial interrupts, pinouts, // limit switches, or the main program. -void protocol_execute_realtime() -{ - protocol_exec_rt_system(); - if (sys.suspend) { protocol_exec_rt_suspend(); } +void protocol_execute_realtime() { + protocol_exec_rt_system(); + if (sys.suspend) { + protocol_exec_rt_suspend(); + } } - // Executes run-time commands, when required. This function primarily operates as Grbl's state // machine and controls the various real-time features Grbl has to offer. // NOTE: Do not alter this unless you know exactly what you are doing! -void protocol_exec_rt_system() -{ - uint8_t rt_exec; // Temp variable to avoid calling volatile multiple times. - rt_exec = sys_rt_exec_alarm; // Copy volatile sys_rt_exec_alarm. - if (rt_exec) { // Enter only if any bit flag is true - // System alarm. Everything has shutdown by something that has gone severely wrong. Report - // the source of the error to the user. If critical, Grbl disables by entering an infinite - // loop until system reset/abort. - sys.state = STATE_ALARM; // Set system alarm state - report_alarm_message(rt_exec); - // Halt everything upon a critical event flag. Currently hard and soft limits flag this. - if ((rt_exec == EXEC_ALARM_HARD_LIMIT) || (rt_exec == EXEC_ALARM_SOFT_LIMIT)) { - report_feedback_message(MESSAGE_CRITICAL_EVENT); - system_clear_exec_state_flag(EXEC_RESET); // Disable any existing reset - do { - // Block everything, except reset and status reports, until user issues reset or power - // cycles. Hard limits typically occur while unattended or not paying attention. Gives - // the user and a GUI time to do what is needed before resetting, like killing the - // incoming stream. The same could be said about soft limits. While the position is not - // lost, continued streaming could cause a serious crash if by chance it gets executed. - } while (bit_isfalse(sys_rt_exec_state,EXEC_RESET)); - } - system_clear_exec_alarm(); // Clear alarm - } - - rt_exec = sys_rt_exec_state; // Copy volatile sys_rt_exec_state. - if (rt_exec) { - - // Execute system abort. - if (rt_exec & EXEC_RESET) { - sys.abort = true; // Only place this is set true. - return; // Nothing else to do but exit. +void protocol_exec_rt_system() { + uint8_t rt_exec; // Temp variable to avoid calling volatile multiple times. + rt_exec = sys_rt_exec_alarm; // Copy volatile sys_rt_exec_alarm. + if (rt_exec) { // Enter only if any bit flag is true + // System alarm. Everything has shutdown by something that has gone severely wrong. Report + // the source of the error to the user. If critical, Grbl disables by entering an infinite + // loop until system reset/abort. + sys.state = STATE_ALARM; // Set system alarm state + report_alarm_message(rt_exec); + // Halt everything upon a critical event flag. Currently hard and soft limits flag this. + if ((rt_exec == EXEC_ALARM_HARD_LIMIT) || (rt_exec == EXEC_ALARM_SOFT_LIMIT)) { + report_feedback_message(MESSAGE_CRITICAL_EVENT); + system_clear_exec_state_flag(EXEC_RESET); // Disable any existing reset + do { + // Block everything, except reset and status reports, until user issues reset or power + // cycles. Hard limits typically occur while unattended or not paying attention. Gives + // the user and a GUI time to do what is needed before resetting, like killing the + // incoming stream. The same could be said about soft limits. While the position is not + // lost, continued streaming could cause a serious crash if by chance it gets executed. + } while (bit_isfalse(sys_rt_exec_state, EXEC_RESET)); + } + system_clear_exec_alarm(); // Clear alarm } - // Execute and serial print status - if (rt_exec & EXEC_STATUS_REPORT) { - report_realtime_status(); - system_clear_exec_state_flag(EXEC_STATUS_REPORT); - } + rt_exec = sys_rt_exec_state; // Copy volatile sys_rt_exec_state. + if (rt_exec) { - // NOTE: Once hold is initiated, the system immediately enters a suspend state to block all - // main program processes until either reset or resumed. This ensures a hold completes safely. - if (rt_exec & (EXEC_MOTION_CANCEL | EXEC_FEED_HOLD | EXEC_SAFETY_DOOR | EXEC_SLEEP)) { + // Execute system abort. + if (rt_exec & EXEC_RESET) { + sys.abort = true; // Only place this is set true. + return; // Nothing else to do but exit. + } - // State check for allowable states for hold methods. - if (!(sys.state & (STATE_ALARM | STATE_CHECK_MODE))) { - - // If in CYCLE or JOG states, immediately initiate a motion HOLD. - if (sys.state & (STATE_CYCLE | STATE_JOG)) { - if (!(sys.suspend & (SUSPEND_MOTION_CANCEL | SUSPEND_JOG_CANCEL))) { // Block, if already holding. - st_update_plan_block_parameters(); // Notify stepper module to recompute for hold deceleration. - sys.step_control = STEP_CONTROL_EXECUTE_HOLD; // Initiate suspend state with active flag. - if (sys.state == STATE_JOG) { // Jog cancelled upon any hold event, except for sleeping. - if (!(rt_exec & EXEC_SLEEP)) { sys.suspend |= SUSPEND_JOG_CANCEL; } + // Execute and serial print status + if (rt_exec & EXEC_STATUS_REPORT) { + report_realtime_status(); + system_clear_exec_state_flag(EXEC_STATUS_REPORT); + } + + // NOTE: Once hold is initiated, the system immediately enters a suspend state to block all + // main program processes until either reset or resumed. This ensures a hold completes safely. + if (rt_exec & (EXEC_MOTION_CANCEL | EXEC_FEED_HOLD | EXEC_SAFETY_DOOR | EXEC_SLEEP)) { + + // State check for allowable states for hold methods. + if (!(sys.state & (STATE_ALARM | STATE_CHECK_MODE))) { + + // If in CYCLE or JOG states, immediately initiate a motion HOLD. + if (sys.state & (STATE_CYCLE | STATE_JOG)) { + if (!(sys.suspend & (SUSPEND_MOTION_CANCEL | SUSPEND_JOG_CANCEL))) { // Block, if already holding. + st_update_plan_block_parameters(); // Notify stepper module to recompute for hold deceleration. + sys.step_control = STEP_CONTROL_EXECUTE_HOLD; // Initiate suspend state with active flag. + if (sys.state == STATE_JOG) { // Jog cancelled upon any hold event, except for sleeping. + if (!(rt_exec & EXEC_SLEEP)) { + sys.suspend |= SUSPEND_JOG_CANCEL; + } + } + } + } + // If IDLE, Grbl is not in motion. Simply indicate suspend state and hold is complete. + if (sys.state == STATE_IDLE) { + sys.suspend = SUSPEND_HOLD_COMPLETE; + } + + // Execute and flag a motion cancel with deceleration and return to idle. Used primarily by probing + // cycle to halt and cancel the remainder of the motion. + if (rt_exec & EXEC_MOTION_CANCEL) { + // MOTION_CANCEL only occurs during a CYCLE, but a HOLD and SAFETY_DOOR may been initiated + // beforehand to hold the CYCLE. Motion cancel is valid for a single planner block motion only, + // while jog cancel will handle and clear multiple planner block motions. + if (!(sys.state & STATE_JOG)) { + sys.suspend |= SUSPEND_MOTION_CANCEL; + } // NOTE: State is STATE_CYCLE. + } + + // Execute a feed hold with deceleration, if required. Then, suspend system. + if (rt_exec & EXEC_FEED_HOLD) { + // Block SAFETY_DOOR, JOG, and SLEEP states from changing to HOLD state. + if (!(sys.state & (STATE_SAFETY_DOOR | STATE_JOG | STATE_SLEEP))) { + sys.state = STATE_HOLD; + } + } + + // Execute a safety door stop with a feed hold and disable spindle/coolant. + // NOTE: Safety door differs from feed holds by stopping everything no matter state, disables powered + // devices (spindle/coolant), and blocks resuming until switch is re-engaged. + if (rt_exec & EXEC_SAFETY_DOOR) { + report_feedback_message(MESSAGE_SAFETY_DOOR_AJAR); + // If jogging, block safety door methods until jog cancel is complete. Just flag that it happened. + if (!(sys.suspend & SUSPEND_JOG_CANCEL)) { + // Check if the safety re-opened during a restore parking motion only. Ignore if + // already retracting, parked or in sleep state. + if (sys.state == STATE_SAFETY_DOOR) { + if (sys.suspend & SUSPEND_INITIATE_RESTORE) { // Actively restoring +#ifdef PARKING_ENABLE + // Set hold and reset appropriate control flags to restart parking sequence. + if (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION) { + st_update_plan_block_parameters(); // Notify stepper module to recompute for hold + // deceleration. + sys.step_control = (STEP_CONTROL_EXECUTE_HOLD | STEP_CONTROL_EXECUTE_SYS_MOTION); + sys.suspend &= ~(SUSPEND_HOLD_COMPLETE); + } // else NO_MOTION is active. +#endif + sys.suspend &= + ~(SUSPEND_RETRACT_COMPLETE | SUSPEND_INITIATE_RESTORE | SUSPEND_RESTORE_COMPLETE); + sys.suspend |= SUSPEND_RESTART_RETRACT; + } + } + if (sys.state != STATE_SLEEP) { + sys.state = STATE_SAFETY_DOOR; + } + } + // NOTE: This flag doesn't change when the door closes, unlike sys.state. Ensures any parking + // motions are executed if the door switch closes and the state returns to HOLD. + sys.suspend |= SUSPEND_SAFETY_DOOR_AJAR; + } } - } - } - // If IDLE, Grbl is not in motion. Simply indicate suspend state and hold is complete. - if (sys.state == STATE_IDLE) { sys.suspend = SUSPEND_HOLD_COMPLETE; } - // Execute and flag a motion cancel with deceleration and return to idle. Used primarily by probing cycle - // to halt and cancel the remainder of the motion. - if (rt_exec & EXEC_MOTION_CANCEL) { - // MOTION_CANCEL only occurs during a CYCLE, but a HOLD and SAFETY_DOOR may been initiated beforehand - // to hold the CYCLE. Motion cancel is valid for a single planner block motion only, while jog cancel - // will handle and clear multiple planner block motions. - if (!(sys.state & STATE_JOG)) { sys.suspend |= SUSPEND_MOTION_CANCEL; } // NOTE: State is STATE_CYCLE. - } - - // Execute a feed hold with deceleration, if required. Then, suspend system. - if (rt_exec & EXEC_FEED_HOLD) { - // Block SAFETY_DOOR, JOG, and SLEEP states from changing to HOLD state. - if (!(sys.state & (STATE_SAFETY_DOOR | STATE_JOG | STATE_SLEEP))) { sys.state = STATE_HOLD; } - } - - // Execute a safety door stop with a feed hold and disable spindle/coolant. - // NOTE: Safety door differs from feed holds by stopping everything no matter state, disables powered - // devices (spindle/coolant), and blocks resuming until switch is re-engaged. - if (rt_exec & EXEC_SAFETY_DOOR) { - report_feedback_message(MESSAGE_SAFETY_DOOR_AJAR); - // If jogging, block safety door methods until jog cancel is complete. Just flag that it happened. - if (!(sys.suspend & SUSPEND_JOG_CANCEL)) { - // Check if the safety re-opened during a restore parking motion only. Ignore if - // already retracting, parked or in sleep state. - if (sys.state == STATE_SAFETY_DOOR) { - if (sys.suspend & SUSPEND_INITIATE_RESTORE) { // Actively restoring - #ifdef PARKING_ENABLE - // Set hold and reset appropriate control flags to restart parking sequence. - if (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION) { - st_update_plan_block_parameters(); // Notify stepper module to recompute for hold deceleration. - sys.step_control = (STEP_CONTROL_EXECUTE_HOLD | STEP_CONTROL_EXECUTE_SYS_MOTION); - sys.suspend &= ~(SUSPEND_HOLD_COMPLETE); - } // else NO_MOTION is active. - #endif - sys.suspend &= ~(SUSPEND_RETRACT_COMPLETE | SUSPEND_INITIATE_RESTORE | SUSPEND_RESTORE_COMPLETE); - sys.suspend |= SUSPEND_RESTART_RETRACT; - } + if (rt_exec & EXEC_SLEEP) { + if (sys.state == STATE_ALARM) { + sys.suspend |= (SUSPEND_RETRACT_COMPLETE | SUSPEND_HOLD_COMPLETE); + } + sys.state = STATE_SLEEP; } - if (sys.state != STATE_SLEEP) { sys.state = STATE_SAFETY_DOOR; } - } - // NOTE: This flag doesn't change when the door closes, unlike sys.state. Ensures any parking motions - // are executed if the door switch closes and the state returns to HOLD. - sys.suspend |= SUSPEND_SAFETY_DOOR_AJAR; + + system_clear_exec_state_flag((EXEC_MOTION_CANCEL | EXEC_FEED_HOLD | EXEC_SAFETY_DOOR | EXEC_SLEEP)); } - - } - if (rt_exec & EXEC_SLEEP) { - if (sys.state == STATE_ALARM) { sys.suspend |= (SUSPEND_RETRACT_COMPLETE|SUSPEND_HOLD_COMPLETE); } - sys.state = STATE_SLEEP; - } - - system_clear_exec_state_flag((EXEC_MOTION_CANCEL | EXEC_FEED_HOLD | EXEC_SAFETY_DOOR | EXEC_SLEEP)); - } - - // Execute a cycle start by starting the stepper interrupt to begin executing the blocks in queue. - if (rt_exec & EXEC_CYCLE_START) { - // Block if called at same time as the hold commands: feed hold, motion cancel, and safety door. - // Ensures auto-cycle-start doesn't resume a hold without an explicit user-input. - if (!(rt_exec & (EXEC_FEED_HOLD | EXEC_MOTION_CANCEL | EXEC_SAFETY_DOOR))) { - // Resume door state when parking motion has retracted and door has been closed. - if ((sys.state == STATE_SAFETY_DOOR) && !(sys.suspend & SUSPEND_SAFETY_DOOR_AJAR)) { - if (sys.suspend & SUSPEND_RESTORE_COMPLETE) { - sys.state = STATE_IDLE; // Set to IDLE to immediately resume the cycle. - } else if (sys.suspend & SUSPEND_RETRACT_COMPLETE) { - // Flag to re-energize powered components and restore original position, if disabled by SAFETY_DOOR. - // NOTE: For a safety door to resume, the switch must be closed, as indicated by HOLD state, and - // the retraction execution is complete, which implies the initial feed hold is not active. To - // restore normal operation, the restore procedures must be initiated by the following flag. Once, - // they are complete, it will call CYCLE_START automatically to resume and exit the suspend. - sys.suspend |= SUSPEND_INITIATE_RESTORE; - } - } - // Cycle start only when IDLE or when a hold is complete and ready to resume. - if ((sys.state == STATE_IDLE) || ((sys.state & STATE_HOLD) && (sys.suspend & SUSPEND_HOLD_COMPLETE))) { - if (sys.state == STATE_HOLD && sys.spindle_stop_ovr) { - sys.spindle_stop_ovr |= SPINDLE_STOP_OVR_RESTORE_CYCLE; // Set to restore in suspend routine and cycle start after. - } else { - // Start cycle only if queued motions exist in planner buffer and the motion is not canceled. - sys.step_control = STEP_CONTROL_NORMAL_OP; // Restore step control to normal operation - if (plan_get_current_block() && bit_isfalse(sys.suspend,SUSPEND_MOTION_CANCEL)) { - sys.suspend = SUSPEND_DISABLE; // Break suspend state. - sys.state = STATE_CYCLE; - st_prep_buffer(); // Initialize step segment buffer before beginning cycle. - st_wake_up(); - } else { // Otherwise, do nothing. Set and resume IDLE state. - sys.suspend = SUSPEND_DISABLE; // Break suspend state. - sys.state = STATE_IDLE; + // Execute a cycle start by starting the stepper interrupt to begin executing the blocks in queue. + if (rt_exec & EXEC_CYCLE_START) { + // Block if called at same time as the hold commands: feed hold, motion cancel, and safety door. + // Ensures auto-cycle-start doesn't resume a hold without an explicit user-input. + if (!(rt_exec & (EXEC_FEED_HOLD | EXEC_MOTION_CANCEL | EXEC_SAFETY_DOOR))) { + // Resume door state when parking motion has retracted and door has been closed. + if ((sys.state == STATE_SAFETY_DOOR) && !(sys.suspend & SUSPEND_SAFETY_DOOR_AJAR)) { + if (sys.suspend & SUSPEND_RESTORE_COMPLETE) { + sys.state = STATE_IDLE; // Set to IDLE to immediately resume the cycle. + } else if (sys.suspend & SUSPEND_RETRACT_COMPLETE) { + // Flag to re-energize powered components and restore original position, if disabled by + // SAFETY_DOOR. NOTE: For a safety door to resume, the switch must be closed, as indicated by + // HOLD state, and the retraction execution is complete, which implies the initial feed hold is + // not active. To restore normal operation, the restore procedures must be initiated by the + // following flag. Once, they are complete, it will call CYCLE_START automatically to resume and + // exit the suspend. + sys.suspend |= SUSPEND_INITIATE_RESTORE; + } + } + // Cycle start only when IDLE or when a hold is complete and ready to resume. + if ((sys.state == STATE_IDLE) || ((sys.state & STATE_HOLD) && (sys.suspend & SUSPEND_HOLD_COMPLETE))) { + if (sys.state == STATE_HOLD && sys.spindle_stop_ovr) { + sys.spindle_stop_ovr |= + SPINDLE_STOP_OVR_RESTORE_CYCLE; // Set to restore in suspend routine and cycle start after. + } else { + // Start cycle only if queued motions exist in planner buffer and the motion is not canceled. + sys.step_control = STEP_CONTROL_NORMAL_OP; // Restore step control to normal operation + if (plan_get_current_block() && bit_isfalse(sys.suspend, SUSPEND_MOTION_CANCEL)) { + sys.suspend = SUSPEND_DISABLE; // Break suspend state. + sys.state = STATE_CYCLE; + st_prep_buffer(); // Initialize step segment buffer before beginning cycle. + st_wake_up(); + } else { // Otherwise, do nothing. Set and resume IDLE state. + sys.suspend = SUSPEND_DISABLE; // Break suspend state. + sys.state = STATE_IDLE; + } + } + } } - } + system_clear_exec_state_flag(EXEC_CYCLE_START); } - } - system_clear_exec_state_flag(EXEC_CYCLE_START); - } - if (rt_exec & EXEC_CYCLE_STOP) { - // Reinitializes the cycle plan and stepper system after a feed hold for a resume. Called by - // realtime command execution in the main program, ensuring that the planner re-plans safely. - // NOTE: Bresenham algorithm variables are still maintained through both the planner and stepper - // cycle reinitializations. The stepper path should continue exactly as if nothing has happened. - // NOTE: EXEC_CYCLE_STOP is set by the stepper subsystem when a cycle or feed hold completes. - if ((sys.state & (STATE_HOLD|STATE_SAFETY_DOOR|STATE_SLEEP)) && !(sys.soft_limit) && !(sys.suspend & SUSPEND_JOG_CANCEL)) { - // Hold complete. Set to indicate ready to resume. Remain in HOLD or DOOR states until user - // has issued a resume command or reset. - plan_cycle_reinitialize(); - if (sys.step_control & STEP_CONTROL_EXECUTE_HOLD) { sys.suspend |= SUSPEND_HOLD_COMPLETE; } - bit_false(sys.step_control,(STEP_CONTROL_EXECUTE_HOLD | STEP_CONTROL_EXECUTE_SYS_MOTION)); - } else { - // Motion complete. Includes CYCLE/JOG/HOMING states and jog cancel/motion cancel/soft limit events. - // NOTE: Motion and jog cancel both immediately return to idle after the hold completes. - if (sys.suspend & SUSPEND_JOG_CANCEL) { // For jog cancel, flush buffers and sync positions. - sys.step_control = STEP_CONTROL_NORMAL_OP; - plan_reset(); - st_reset(); - gc_sync_position(); - plan_sync_position(); + if (rt_exec & EXEC_CYCLE_STOP) { + // Reinitializes the cycle plan and stepper system after a feed hold for a resume. Called by + // realtime command execution in the main program, ensuring that the planner re-plans safely. + // NOTE: Bresenham algorithm variables are still maintained through both the planner and stepper + // cycle reinitializations. The stepper path should continue exactly as if nothing has happened. + // NOTE: EXEC_CYCLE_STOP is set by the stepper subsystem when a cycle or feed hold completes. + if ((sys.state & (STATE_HOLD | STATE_SAFETY_DOOR | STATE_SLEEP)) && !(sys.soft_limit) && + !(sys.suspend & SUSPEND_JOG_CANCEL)) { + // Hold complete. Set to indicate ready to resume. Remain in HOLD or DOOR states until user + // has issued a resume command or reset. + plan_cycle_reinitialize(); + if (sys.step_control & STEP_CONTROL_EXECUTE_HOLD) { + sys.suspend |= SUSPEND_HOLD_COMPLETE; + } + bit_false(sys.step_control, (STEP_CONTROL_EXECUTE_HOLD | STEP_CONTROL_EXECUTE_SYS_MOTION)); + } else { + // Motion complete. Includes CYCLE/JOG/HOMING states and jog cancel/motion cancel/soft limit events. + // NOTE: Motion and jog cancel both immediately return to idle after the hold completes. + if (sys.suspend & SUSPEND_JOG_CANCEL) { // For jog cancel, flush buffers and sync positions. + sys.step_control = STEP_CONTROL_NORMAL_OP; + plan_reset(); + st_reset(); + gc_sync_position(); + plan_sync_position(); + } + if (sys.suspend & SUSPEND_SAFETY_DOOR_AJAR) { // Only occurs when safety door opens during jog. + sys.suspend &= ~(SUSPEND_JOG_CANCEL); + sys.suspend |= SUSPEND_HOLD_COMPLETE; + sys.state = STATE_SAFETY_DOOR; + } else { + sys.suspend = SUSPEND_DISABLE; + sys.state = STATE_IDLE; + } + } + system_clear_exec_state_flag(EXEC_CYCLE_STOP); } - if (sys.suspend & SUSPEND_SAFETY_DOOR_AJAR) { // Only occurs when safety door opens during jog. - sys.suspend &= ~(SUSPEND_JOG_CANCEL); - sys.suspend |= SUSPEND_HOLD_COMPLETE; - sys.state = STATE_SAFETY_DOOR; - } else { - sys.suspend = SUSPEND_DISABLE; - sys.state = STATE_IDLE; + } + + // Execute overrides. + rt_exec = sys_rt_exec_motion_override; // Copy volatile sys_rt_exec_motion_override + if (rt_exec) { + system_clear_exec_motion_overrides(); // Clear all motion override flags. + + uint8_t new_f_override = sys.f_override; + if (rt_exec & EXEC_FEED_OVR_RESET) { + new_f_override = DEFAULT_FEED_OVERRIDE; } - } - system_clear_exec_state_flag(EXEC_CYCLE_STOP); - } - } + if (rt_exec & EXEC_FEED_OVR_COARSE_PLUS) { + new_f_override += FEED_OVERRIDE_COARSE_INCREMENT; + } + if (rt_exec & EXEC_FEED_OVR_COARSE_MINUS) { + new_f_override -= FEED_OVERRIDE_COARSE_INCREMENT; + } + if (rt_exec & EXEC_FEED_OVR_FINE_PLUS) { + new_f_override += FEED_OVERRIDE_FINE_INCREMENT; + } + if (rt_exec & EXEC_FEED_OVR_FINE_MINUS) { + new_f_override -= FEED_OVERRIDE_FINE_INCREMENT; + } + new_f_override = min(new_f_override, MAX_FEED_RATE_OVERRIDE); + new_f_override = max(new_f_override, MIN_FEED_RATE_OVERRIDE); - // Execute overrides. - rt_exec = sys_rt_exec_motion_override; // Copy volatile sys_rt_exec_motion_override - if (rt_exec) { - system_clear_exec_motion_overrides(); // Clear all motion override flags. + uint8_t new_r_override = sys.r_override; + if (rt_exec & EXEC_RAPID_OVR_RESET) { + new_r_override = DEFAULT_RAPID_OVERRIDE; + } + if (rt_exec & EXEC_RAPID_OVR_MEDIUM) { + new_r_override = RAPID_OVERRIDE_MEDIUM; + } + if (rt_exec & EXEC_RAPID_OVR_LOW) { + new_r_override = RAPID_OVERRIDE_LOW; + } - uint8_t new_f_override = sys.f_override; - if (rt_exec & EXEC_FEED_OVR_RESET) { new_f_override = DEFAULT_FEED_OVERRIDE; } - if (rt_exec & EXEC_FEED_OVR_COARSE_PLUS) { new_f_override += FEED_OVERRIDE_COARSE_INCREMENT; } - if (rt_exec & EXEC_FEED_OVR_COARSE_MINUS) { new_f_override -= FEED_OVERRIDE_COARSE_INCREMENT; } - if (rt_exec & EXEC_FEED_OVR_FINE_PLUS) { new_f_override += FEED_OVERRIDE_FINE_INCREMENT; } - if (rt_exec & EXEC_FEED_OVR_FINE_MINUS) { new_f_override -= FEED_OVERRIDE_FINE_INCREMENT; } - new_f_override = min(new_f_override,MAX_FEED_RATE_OVERRIDE); - new_f_override = max(new_f_override,MIN_FEED_RATE_OVERRIDE); - - uint8_t new_r_override = sys.r_override; - if (rt_exec & EXEC_RAPID_OVR_RESET) { new_r_override = DEFAULT_RAPID_OVERRIDE; } - if (rt_exec & EXEC_RAPID_OVR_MEDIUM) { new_r_override = RAPID_OVERRIDE_MEDIUM; } - if (rt_exec & EXEC_RAPID_OVR_LOW) { new_r_override = RAPID_OVERRIDE_LOW; } - - if ((new_f_override != sys.f_override) || (new_r_override != sys.r_override)) { - sys.f_override = new_f_override; - sys.r_override = new_r_override; - sys.report_ovr_counter = 0; // Set to report change immediately - plan_update_velocity_profile_parameters(); - plan_cycle_reinitialize(); - } - } - - rt_exec = sys_rt_exec_accessory_override; - if (rt_exec) { - system_clear_exec_accessory_overrides(); // Clear all accessory override flags. - - // NOTE: Unlike motion overrides, spindle overrides do not require a planner reinitialization. - uint8_t last_s_override = sys.spindle_speed_ovr; - if (rt_exec & EXEC_SPINDLE_OVR_RESET) { last_s_override = DEFAULT_SPINDLE_SPEED_OVERRIDE; } - if (rt_exec & EXEC_SPINDLE_OVR_COARSE_PLUS) { last_s_override += SPINDLE_OVERRIDE_COARSE_INCREMENT; } - if (rt_exec & EXEC_SPINDLE_OVR_COARSE_MINUS) { last_s_override -= SPINDLE_OVERRIDE_COARSE_INCREMENT; } - if (rt_exec & EXEC_SPINDLE_OVR_FINE_PLUS) { last_s_override += SPINDLE_OVERRIDE_FINE_INCREMENT; } - if (rt_exec & EXEC_SPINDLE_OVR_FINE_MINUS) { last_s_override -= SPINDLE_OVERRIDE_FINE_INCREMENT; } - last_s_override = min(last_s_override,MAX_SPINDLE_SPEED_OVERRIDE); - last_s_override = max(last_s_override,MIN_SPINDLE_SPEED_OVERRIDE); - - if (last_s_override != sys.spindle_speed_ovr) { - 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_set_state(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 + if ((new_f_override != sys.f_override) || (new_r_override != sys.r_override)) { + sys.f_override = new_f_override; + sys.r_override = new_r_override; + sys.report_ovr_counter = 0; // Set to report change immediately + plan_update_velocity_profile_parameters(); + plan_cycle_reinitialize(); + } } - if (rt_exec & EXEC_SPINDLE_OVR_STOP) { - // Spindle stop override allowed only while in HOLD state. - // NOTE: Report counters are set in spindle_set_state() when spindle stop is executed. - if (sys.state == STATE_HOLD) { - if (!(sys.spindle_stop_ovr)) { sys.spindle_stop_ovr = SPINDLE_STOP_OVR_INITIATE; } - else if (sys.spindle_stop_ovr & SPINDLE_STOP_OVR_ENABLED) { sys.spindle_stop_ovr |= SPINDLE_STOP_OVR_RESTORE; } - } + rt_exec = sys_rt_exec_accessory_override; + if (rt_exec) { + system_clear_exec_accessory_overrides(); // Clear all accessory override flags. + + // NOTE: Unlike motion overrides, spindle overrides do not require a planner reinitialization. + uint8_t last_s_override = sys.spindle_speed_ovr; + if (rt_exec & EXEC_SPINDLE_OVR_RESET) { + last_s_override = DEFAULT_SPINDLE_SPEED_OVERRIDE; + } + if (rt_exec & EXEC_SPINDLE_OVR_COARSE_PLUS) { + last_s_override += SPINDLE_OVERRIDE_COARSE_INCREMENT; + } + if (rt_exec & EXEC_SPINDLE_OVR_COARSE_MINUS) { + last_s_override -= SPINDLE_OVERRIDE_COARSE_INCREMENT; + } + if (rt_exec & EXEC_SPINDLE_OVR_FINE_PLUS) { + last_s_override += SPINDLE_OVERRIDE_FINE_INCREMENT; + } + if (rt_exec & EXEC_SPINDLE_OVR_FINE_MINUS) { + last_s_override -= SPINDLE_OVERRIDE_FINE_INCREMENT; + } + last_s_override = min(last_s_override, MAX_SPINDLE_SPEED_OVERRIDE); + last_s_override = max(last_s_override, MIN_SPINDLE_SPEED_OVERRIDE); + + if (last_s_override != sys.spindle_speed_ovr) { + 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_set_state(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 + } + + if (rt_exec & EXEC_SPINDLE_OVR_STOP) { + // Spindle stop override allowed only while in HOLD state. + // NOTE: Report counters are set in spindle_set_state() when spindle stop is executed. + if (sys.state == STATE_HOLD) { + if (!(sys.spindle_stop_ovr)) { + sys.spindle_stop_ovr = SPINDLE_STOP_OVR_INITIATE; + } else if (sys.spindle_stop_ovr & SPINDLE_STOP_OVR_ENABLED) { + sys.spindle_stop_ovr |= SPINDLE_STOP_OVR_RESTORE; + } + } + } + + // 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 | STATE_JOG))) { + uint8_t coolant_state = gc_state.modal.coolant; +#ifdef ENABLE_M7 + if (rt_exec & EXEC_COOLANT_MIST_OVR_TOGGLE) { + if (coolant_state & COOLANT_MIST_ENABLE) { + bit_false(coolant_state, COOLANT_MIST_ENABLE); + } else { + coolant_state |= COOLANT_MIST_ENABLE; + } + } + if (rt_exec & EXEC_COOLANT_FLOOD_OVR_TOGGLE) { + if (coolant_state & COOLANT_FLOOD_ENABLE) { + bit_false(coolant_state, COOLANT_FLOOD_ENABLE); + } else { + coolant_state |= COOLANT_FLOOD_ENABLE; + } + } +#else + if (coolant_state & COOLANT_FLOOD_ENABLE) { + bit_false(coolant_state, COOLANT_FLOOD_ENABLE); + } else { + coolant_state |= COOLANT_FLOOD_ENABLE; + } +#endif + coolant_set_state(coolant_state); // Report counter set in coolant_set_state(). + gc_state.modal.coolant = coolant_state; + } + } } - // 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 | STATE_JOG))) { - uint8_t coolant_state = gc_state.modal.coolant; - #ifdef ENABLE_M7 - if (rt_exec & EXEC_COOLANT_MIST_OVR_TOGGLE) { - if (coolant_state & COOLANT_MIST_ENABLE) { bit_false(coolant_state,COOLANT_MIST_ENABLE); } - else { coolant_state |= COOLANT_MIST_ENABLE; } - } - if (rt_exec & EXEC_COOLANT_FLOOD_OVR_TOGGLE) { - if (coolant_state & COOLANT_FLOOD_ENABLE) { bit_false(coolant_state,COOLANT_FLOOD_ENABLE); } - else { coolant_state |= COOLANT_FLOOD_ENABLE; } - } - #else - if (coolant_state & COOLANT_FLOOD_ENABLE) { bit_false(coolant_state,COOLANT_FLOOD_ENABLE); } - else { coolant_state |= COOLANT_FLOOD_ENABLE; } - #endif - coolant_set_state(coolant_state); // Report counter set in coolant_set_state(). - gc_state.modal.coolant = coolant_state; - } - } - } - - #ifdef DEBUG +#ifdef DEBUG if (sys_rt_exec_debug) { - report_realtime_debug(); - sys_rt_exec_debug = 0; + report_realtime_debug(); + sys_rt_exec_debug = 0; } - #endif - - // Reload step segment buffer - if (sys.state & (STATE_CYCLE | STATE_HOLD | STATE_SAFETY_DOOR | STATE_HOMING | STATE_SLEEP| STATE_JOG)) { - st_prep_buffer(); - } +#endif + // Reload step segment buffer + if (sys.state & (STATE_CYCLE | STATE_HOLD | STATE_SAFETY_DOOR | STATE_HOMING | STATE_SLEEP | STATE_JOG)) { + st_prep_buffer(); + } } - // Handles Grbl system suspend procedures, such as feed hold, safety door, and parking motion. // The system will enter this loop, create local variables for suspend tasks, and return to // whatever function that invoked the suspend, such that Grbl resumes normal operation. // This function is written in a way to promote custom parking motions. Simply use this as a // template -static void protocol_exec_rt_suspend() -{ - #ifdef PARKING_ENABLE +static void protocol_exec_rt_suspend() { +#ifdef PARKING_ENABLE // Declare and initialize parking local variables - float restore_target[N_AXIS]; - float parking_target[N_AXIS]; - float retract_waypoint = PARKING_PULLOUT_INCREMENT; - plan_line_data_t plan_data; + float restore_target[N_AXIS]; + float parking_target[N_AXIS]; + float retract_waypoint = PARKING_PULLOUT_INCREMENT; + plan_line_data_t plan_data; plan_line_data_t *pl_data = &plan_data; - memset(pl_data,0,sizeof(plan_line_data_t)); - pl_data->condition = (PL_COND_FLAG_SYSTEM_MOTION|PL_COND_FLAG_NO_FEED_OVERRIDE); - #ifdef USE_LINE_NUMBERS - pl_data->line_number = PARKING_MOTION_LINE_NUMBER; - #endif - #endif + memset(pl_data, 0, sizeof(plan_line_data_t)); + pl_data->condition = (PL_COND_FLAG_SYSTEM_MOTION | PL_COND_FLAG_NO_FEED_OVERRIDE); +#ifdef USE_LINE_NUMBERS + pl_data->line_number = PARKING_MOTION_LINE_NUMBER; +#endif +#endif - plan_block_t *block = plan_get_current_block(); - uint8_t restore_condition; - #ifdef VARIABLE_SPINDLE + plan_block_t *block = plan_get_current_block(); + uint8_t restore_condition; +#ifdef VARIABLE_SPINDLE float restore_spindle_speed; if (block == NULL) { - restore_condition = (gc_state.modal.spindle | gc_state.modal.coolant); - restore_spindle_speed = gc_state.spindle_speed; + restore_condition = (gc_state.modal.spindle | gc_state.modal.coolant); + restore_spindle_speed = gc_state.spindle_speed; } else { - restore_condition = (block->condition & PL_COND_SPINDLE_MASK) | coolant_get_state(); - restore_spindle_speed = block->spindle_speed; + restore_condition = (block->condition & PL_COND_SPINDLE_MASK) | coolant_get_state(); + restore_spindle_speed = block->spindle_speed; } - #ifdef DISABLE_LASER_DURING_HOLD - if (bit_istrue(settings.flags,BITFLAG_LASER_MODE)) { +#ifdef DISABLE_LASER_DURING_HOLD + if (bit_istrue(settings.flags, BITFLAG_LASER_MODE)) { system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_STOP); - } - #endif - #else - if (block == NULL) { restore_condition = (gc_state.modal.spindle | gc_state.modal.coolant); } - else { restore_condition = (block->condition & PL_COND_SPINDLE_MASK) | coolant_get_state(); } - #endif - - while (sys.suspend) { - - if (sys.abort) { return; } - - // Block until initial hold is complete and the machine has stopped motion. - if (sys.suspend & SUSPEND_HOLD_COMPLETE) { - - // Parking manager. Handles de/re-energizing, switch state checks, and parking motions for - // the safety door and sleep states. - if (sys.state & (STATE_SAFETY_DOOR | STATE_SLEEP)) { - - // Handles retraction motions and de-energizing. - if (bit_isfalse(sys.suspend,SUSPEND_RETRACT_COMPLETE)) { - - // Ensure any prior spindle stop override is disabled at start of safety door routine. - sys.spindle_stop_ovr = SPINDLE_STOP_OVR_DISABLED; - - #ifndef PARKING_ENABLE - - spindle_set_state(SPINDLE_DISABLE,0.0); // De-energize - coolant_set_state(COOLANT_DISABLE); // De-energize - - #else - - // Get current position and store restore location and spindle retract waypoint. - system_convert_array_steps_to_mpos(parking_target,sys_position); - if (bit_isfalse(sys.suspend,SUSPEND_RESTART_RETRACT)) { - memcpy(restore_target,parking_target,sizeof(parking_target)); - retract_waypoint += restore_target[PARKING_AXIS]; - retract_waypoint = min(retract_waypoint,PARKING_TARGET); - } - - // Execute slow pull-out parking retract motion. Parking requires homing enabled, the - // current location not exceeding the parking target location, and laser mode disabled. - // NOTE: State is will remain DOOR, until the de-energizing and retract is complete. - #ifdef ENABLE_PARKING_OVERRIDE_CONTROL - if ((bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)) && - (parking_target[PARKING_AXIS] < PARKING_TARGET) && - bit_isfalse(settings.flags,BITFLAG_LASER_MODE) && - (sys.override_ctrl == OVERRIDE_PARKING_MOTION)) { - #else - if ((bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)) && - (parking_target[PARKING_AXIS] < PARKING_TARGET) && - bit_isfalse(settings.flags,BITFLAG_LASER_MODE)) { - #endif - // Retract spindle by pullout distance. Ensure retraction motion moves away from - // the workpiece and waypoint motion doesn't exceed the parking target location. - if (parking_target[PARKING_AXIS] < retract_waypoint) { - parking_target[PARKING_AXIS] = retract_waypoint; - pl_data->feed_rate = PARKING_PULLOUT_RATE; - pl_data->condition |= (restore_condition & PL_COND_ACCESSORY_MASK); // Retain accessory state - pl_data->spindle_speed = restore_spindle_speed; - mc_parking_motion(parking_target, pl_data); - } - - // NOTE: Clear accessory state after retract and after an aborted restore motion. - pl_data->condition = (PL_COND_FLAG_SYSTEM_MOTION|PL_COND_FLAG_NO_FEED_OVERRIDE); - pl_data->spindle_speed = 0.0; - spindle_set_state(SPINDLE_DISABLE,0.0); // De-energize - coolant_set_state(COOLANT_DISABLE); // De-energize - - // Execute fast parking retract motion to parking target location. - if (parking_target[PARKING_AXIS] < PARKING_TARGET) { - parking_target[PARKING_AXIS] = PARKING_TARGET; - pl_data->feed_rate = PARKING_RATE; - mc_parking_motion(parking_target, pl_data); - } - - } else { - - // Parking motion not possible. Just disable the spindle and coolant. - // NOTE: Laser mode does not start a parking motion to ensure the laser stops immediately. - spindle_set_state(SPINDLE_DISABLE,0.0); // De-energize - coolant_set_state(COOLANT_DISABLE); // De-energize - - } - - #endif - - sys.suspend &= ~(SUSPEND_RESTART_RETRACT); - sys.suspend |= SUSPEND_RETRACT_COMPLETE; - - } else { - - - if (sys.state == STATE_SLEEP) { - report_feedback_message(MESSAGE_SLEEP_MODE); - // Spindle and coolant should already be stopped, but do it again just to be sure. - spindle_set_state(SPINDLE_DISABLE,0.0); // De-energize - coolant_set_state(COOLANT_DISABLE); // De-energize - st_go_idle(); // Disable steppers - while (!(sys.abort)) { protocol_exec_rt_system(); } // Do nothing until reset. - return; // Abort received. Return to re-initialize. - } - - // Allows resuming from parking/safety door. Actively checks if safety door is closed and ready to resume. - if (sys.state == STATE_SAFETY_DOOR) { - if (!(system_check_safety_door_ajar())) { - sys.suspend &= ~(SUSPEND_SAFETY_DOOR_AJAR); // Reset door ajar flag to denote ready to resume. - } - } - - // Handles parking restore and safety door resume. - if (sys.suspend & SUSPEND_INITIATE_RESTORE) { - - #ifdef PARKING_ENABLE - // Execute fast restore motion to the pull-out position. Parking requires homing enabled. - // NOTE: State is will remain DOOR, until the de-energizing and retract is complete. - #ifdef ENABLE_PARKING_OVERRIDE_CONTROL - if (((settings.flags & (BITFLAG_HOMING_ENABLE|BITFLAG_LASER_MODE)) == BITFLAG_HOMING_ENABLE) && - (sys.override_ctrl == OVERRIDE_PARKING_MOTION)) { - #else - if ((settings.flags & (BITFLAG_HOMING_ENABLE|BITFLAG_LASER_MODE)) == BITFLAG_HOMING_ENABLE) { - #endif - // Check to ensure the motion doesn't move below pull-out position. - if (parking_target[PARKING_AXIS] <= PARKING_TARGET) { - parking_target[PARKING_AXIS] = retract_waypoint; - pl_data->feed_rate = PARKING_RATE; - mc_parking_motion(parking_target, pl_data); - } - } - #endif - - // Delayed Tasks: Restart spindle and coolant, delay to power-up, then resume cycle. - if (gc_state.modal.spindle != SPINDLE_DISABLE) { - // Block if safety door re-opened during prior restore actions. - if (bit_isfalse(sys.suspend,SUSPEND_RESTART_RETRACT)) { - if (bit_istrue(settings.flags,BITFLAG_LASER_MODE)) { - // When in laser mode, ignore spindle spin-up delay. Set to turn on laser when cycle starts. - bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_PWM); - } else { - spindle_set_state((restore_condition & (PL_COND_FLAG_SPINDLE_CW | PL_COND_FLAG_SPINDLE_CCW)), restore_spindle_speed); - delay_sec(SAFETY_DOOR_SPINDLE_DELAY, DELAY_MODE_SYS_SUSPEND); - } - } - } - if (gc_state.modal.coolant != COOLANT_DISABLE) { - // Block if safety door re-opened during prior restore actions. - if (bit_isfalse(sys.suspend,SUSPEND_RESTART_RETRACT)) { - // NOTE: Laser mode will honor this delay. An exhaust system is often controlled by this pin. - coolant_set_state((restore_condition & (PL_COND_FLAG_COOLANT_FLOOD | PL_COND_FLAG_COOLANT_MIST))); - delay_sec(SAFETY_DOOR_COOLANT_DELAY, DELAY_MODE_SYS_SUSPEND); - } - } - - #ifdef PARKING_ENABLE - // Execute slow plunge motion from pull-out position to resume position. - #ifdef ENABLE_PARKING_OVERRIDE_CONTROL - if (((settings.flags & (BITFLAG_HOMING_ENABLE|BITFLAG_LASER_MODE)) == BITFLAG_HOMING_ENABLE) && - (sys.override_ctrl == OVERRIDE_PARKING_MOTION)) { - #else - if ((settings.flags & (BITFLAG_HOMING_ENABLE|BITFLAG_LASER_MODE)) == BITFLAG_HOMING_ENABLE) { - #endif - // Block if safety door re-opened during prior restore actions. - if (bit_isfalse(sys.suspend,SUSPEND_RESTART_RETRACT)) { - // Regardless if the retract parking motion was a valid/safe motion or not, the - // restore parking motion should logically be valid, either by returning to the - // original position through valid machine space or by not moving at all. - pl_data->feed_rate = PARKING_PULLOUT_RATE; - pl_data->condition |= (restore_condition & PL_COND_ACCESSORY_MASK); // Restore accessory state - pl_data->spindle_speed = restore_spindle_speed; - mc_parking_motion(restore_target, pl_data); - } - } - #endif - - if (bit_isfalse(sys.suspend,SUSPEND_RESTART_RETRACT)) { - sys.suspend |= SUSPEND_RESTORE_COMPLETE; - system_set_exec_state_flag(EXEC_CYCLE_START); // Set to resume program. - } - } - - } - - - } else { - - // Feed hold manager. Controls spindle stop override states. - // NOTE: Hold ensured as completed by condition check at the beginning of suspend routine. - if (sys.spindle_stop_ovr) { - // Handles beginning of spindle stop - if (sys.spindle_stop_ovr & SPINDLE_STOP_OVR_INITIATE) { - if (gc_state.modal.spindle != SPINDLE_DISABLE) { - spindle_set_state(SPINDLE_DISABLE,0.0); // De-energize - sys.spindle_stop_ovr = SPINDLE_STOP_OVR_ENABLED; // Set stop override state to enabled, if de-energized. - } else { - sys.spindle_stop_ovr = SPINDLE_STOP_OVR_DISABLED; // Clear stop override state - } - // Handles restoring of spindle state - } else if (sys.spindle_stop_ovr & (SPINDLE_STOP_OVR_RESTORE | SPINDLE_STOP_OVR_RESTORE_CYCLE)) { - if (gc_state.modal.spindle != SPINDLE_DISABLE) { - report_feedback_message(MESSAGE_SPINDLE_RESTORE); - if (bit_istrue(settings.flags,BITFLAG_LASER_MODE)) { - // When in laser mode, ignore spindle spin-up delay. Set to turn on laser when cycle starts. - bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_PWM); - } else { - spindle_set_state((restore_condition & (PL_COND_FLAG_SPINDLE_CW | PL_COND_FLAG_SPINDLE_CCW)), restore_spindle_speed); - } - } - if (sys.spindle_stop_ovr & SPINDLE_STOP_OVR_RESTORE_CYCLE) { - system_set_exec_state_flag(EXEC_CYCLE_START); // Set to resume program. - } - sys.spindle_stop_ovr = SPINDLE_STOP_OVR_DISABLED; // Clear stop override state - } - } else { - // Handles spindle state during hold. NOTE: Spindle speed overrides may be altered during hold state. - // NOTE: STEP_CONTROL_UPDATE_SPINDLE_PWM is automatically reset upon resume in step generator. - if (bit_istrue(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_PWM)) { - spindle_set_state((restore_condition & (PL_COND_FLAG_SPINDLE_CW | PL_COND_FLAG_SPINDLE_CCW)), restore_spindle_speed); - bit_false(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_PWM); - } - } - - } } +#endif +#else + if (block == NULL) { + restore_condition = (gc_state.modal.spindle | gc_state.modal.coolant); + } else { + restore_condition = (block->condition & PL_COND_SPINDLE_MASK) | coolant_get_state(); + } +#endif - protocol_exec_rt_system(); + while (sys.suspend) { - } + if (sys.abort) { + return; + } + + // Block until initial hold is complete and the machine has stopped motion. + if (sys.suspend & SUSPEND_HOLD_COMPLETE) { + + // Parking manager. Handles de/re-energizing, switch state checks, and parking motions for + // the safety door and sleep states. + if (sys.state & (STATE_SAFETY_DOOR | STATE_SLEEP)) { + + // Handles retraction motions and de-energizing. + if (bit_isfalse(sys.suspend, SUSPEND_RETRACT_COMPLETE)) { + + // Ensure any prior spindle stop override is disabled at start of safety door routine. + sys.spindle_stop_ovr = SPINDLE_STOP_OVR_DISABLED; + +#ifndef PARKING_ENABLE + + spindle_set_state(SPINDLE_DISABLE, 0.0); // De-energize + coolant_set_state(COOLANT_DISABLE); // De-energize + +#else + + // Get current position and store restore location and spindle retract waypoint. + system_convert_array_steps_to_mpos(parking_target, sys_position); + if (bit_isfalse(sys.suspend, SUSPEND_RESTART_RETRACT)) { + memcpy(restore_target, parking_target, sizeof(parking_target)); + retract_waypoint += restore_target[PARKING_AXIS]; + retract_waypoint = min(retract_waypoint, PARKING_TARGET); + } + +// Execute slow pull-out parking retract motion. Parking requires homing enabled, the +// current location not exceeding the parking target location, and laser mode disabled. +// NOTE: State is will remain DOOR, until the de-energizing and retract is complete. +#ifdef ENABLE_PARKING_OVERRIDE_CONTROL + if ((bit_istrue(settings.flags, BITFLAG_HOMING_ENABLE)) && + (parking_target[PARKING_AXIS] < PARKING_TARGET) && + bit_isfalse(settings.flags, BITFLAG_LASER_MODE) && + (sys.override_ctrl == OVERRIDE_PARKING_MOTION)) { +#else + if ((bit_istrue(settings.flags, BITFLAG_HOMING_ENABLE)) && + (parking_target[PARKING_AXIS] < PARKING_TARGET) && + bit_isfalse(settings.flags, BITFLAG_LASER_MODE)) { +#endif + // Retract spindle by pullout distance. Ensure retraction motion moves away from + // the workpiece and waypoint motion doesn't exceed the parking target location. + if (parking_target[PARKING_AXIS] < retract_waypoint) { + parking_target[PARKING_AXIS] = retract_waypoint; + pl_data->feed_rate = PARKING_PULLOUT_RATE; + pl_data->condition |= + (restore_condition & PL_COND_ACCESSORY_MASK); // Retain accessory state + pl_data->spindle_speed = restore_spindle_speed; + mc_parking_motion(parking_target, pl_data); + } + + // NOTE: Clear accessory state after retract and after an aborted restore motion. + pl_data->condition = (PL_COND_FLAG_SYSTEM_MOTION | PL_COND_FLAG_NO_FEED_OVERRIDE); + pl_data->spindle_speed = 0.0; + spindle_set_state(SPINDLE_DISABLE, 0.0); // De-energize + coolant_set_state(COOLANT_DISABLE); // De-energize + + // Execute fast parking retract motion to parking target location. + if (parking_target[PARKING_AXIS] < PARKING_TARGET) { + parking_target[PARKING_AXIS] = PARKING_TARGET; + pl_data->feed_rate = PARKING_RATE; + mc_parking_motion(parking_target, pl_data); + } + + } else { + + // Parking motion not possible. Just disable the spindle and coolant. + // NOTE: Laser mode does not start a parking motion to ensure the laser stops immediately. + spindle_set_state(SPINDLE_DISABLE, 0.0); // De-energize + coolant_set_state(COOLANT_DISABLE); // De-energize + } + +#endif + + sys.suspend &= ~(SUSPEND_RESTART_RETRACT); + sys.suspend |= SUSPEND_RETRACT_COMPLETE; + + } else { + + if (sys.state == STATE_SLEEP) { + report_feedback_message(MESSAGE_SLEEP_MODE); + // Spindle and coolant should already be stopped, but do it again just to be sure. + spindle_set_state(SPINDLE_DISABLE, 0.0); // De-energize + coolant_set_state(COOLANT_DISABLE); // De-energize + st_go_idle(); // Disable steppers + while (!(sys.abort)) { + protocol_exec_rt_system(); + } // Do nothing until reset. + return; // Abort received. Return to re-initialize. + } + + // Allows resuming from parking/safety door. Actively checks if safety door is closed and ready to + // resume. + if (sys.state == STATE_SAFETY_DOOR) { + if (!(system_check_safety_door_ajar())) { + sys.suspend &= + ~(SUSPEND_SAFETY_DOOR_AJAR); // Reset door ajar flag to denote ready to resume. + } + } + + // Handles parking restore and safety door resume. + if (sys.suspend & SUSPEND_INITIATE_RESTORE) { + +#ifdef PARKING_ENABLE +// Execute fast restore motion to the pull-out position. Parking requires homing enabled. +// NOTE: State is will remain DOOR, until the de-energizing and retract is complete. +#ifdef ENABLE_PARKING_OVERRIDE_CONTROL + if (((settings.flags & (BITFLAG_HOMING_ENABLE | BITFLAG_LASER_MODE)) == + BITFLAG_HOMING_ENABLE) && + (sys.override_ctrl == OVERRIDE_PARKING_MOTION)) { +#else + if ((settings.flags & (BITFLAG_HOMING_ENABLE | BITFLAG_LASER_MODE)) == BITFLAG_HOMING_ENABLE) { +#endif + // Check to ensure the motion doesn't move below pull-out position. + if (parking_target[PARKING_AXIS] <= PARKING_TARGET) { + parking_target[PARKING_AXIS] = retract_waypoint; + pl_data->feed_rate = PARKING_RATE; + mc_parking_motion(parking_target, pl_data); + } + } +#endif + + // Delayed Tasks: Restart spindle and coolant, delay to power-up, then resume cycle. + if (gc_state.modal.spindle != SPINDLE_DISABLE) { + // Block if safety door re-opened during prior restore actions. + if (bit_isfalse(sys.suspend, SUSPEND_RESTART_RETRACT)) { + if (bit_istrue(settings.flags, BITFLAG_LASER_MODE)) { + // When in laser mode, ignore spindle spin-up delay. Set to turn on laser when cycle + // starts. + bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_PWM); + } else { + spindle_set_state( + (restore_condition & (PL_COND_FLAG_SPINDLE_CW | PL_COND_FLAG_SPINDLE_CCW)), + restore_spindle_speed); + delay_sec(SAFETY_DOOR_SPINDLE_DELAY, DELAY_MODE_SYS_SUSPEND); + } + } + } + if (gc_state.modal.coolant != COOLANT_DISABLE) { + // Block if safety door re-opened during prior restore actions. + if (bit_isfalse(sys.suspend, SUSPEND_RESTART_RETRACT)) { + // NOTE: Laser mode will honor this delay. An exhaust system is often controlled by this + // pin. + coolant_set_state( + (restore_condition & (PL_COND_FLAG_COOLANT_FLOOD | PL_COND_FLAG_COOLANT_MIST))); + delay_sec(SAFETY_DOOR_COOLANT_DELAY, DELAY_MODE_SYS_SUSPEND); + } + } + +#ifdef PARKING_ENABLE +// Execute slow plunge motion from pull-out position to resume position. +#ifdef ENABLE_PARKING_OVERRIDE_CONTROL + if (((settings.flags & (BITFLAG_HOMING_ENABLE | BITFLAG_LASER_MODE)) == + BITFLAG_HOMING_ENABLE) && + (sys.override_ctrl == OVERRIDE_PARKING_MOTION)) { +#else + if ((settings.flags & (BITFLAG_HOMING_ENABLE | BITFLAG_LASER_MODE)) == BITFLAG_HOMING_ENABLE) { +#endif + // Block if safety door re-opened during prior restore actions. + if (bit_isfalse(sys.suspend, SUSPEND_RESTART_RETRACT)) { + // Regardless if the retract parking motion was a valid/safe motion or not, the + // restore parking motion should logically be valid, either by returning to the + // original position through valid machine space or by not moving at all. + pl_data->feed_rate = PARKING_PULLOUT_RATE; + pl_data->condition |= + (restore_condition & PL_COND_ACCESSORY_MASK); // Restore accessory state + pl_data->spindle_speed = restore_spindle_speed; + mc_parking_motion(restore_target, pl_data); + } + } +#endif + + if (bit_isfalse(sys.suspend, SUSPEND_RESTART_RETRACT)) { + sys.suspend |= SUSPEND_RESTORE_COMPLETE; + system_set_exec_state_flag(EXEC_CYCLE_START); // Set to resume program. + } + } + } + + } else { + + // Feed hold manager. Controls spindle stop override states. + // NOTE: Hold ensured as completed by condition check at the beginning of suspend routine. + if (sys.spindle_stop_ovr) { + // Handles beginning of spindle stop + if (sys.spindle_stop_ovr & SPINDLE_STOP_OVR_INITIATE) { + if (gc_state.modal.spindle != SPINDLE_DISABLE) { + spindle_set_state(SPINDLE_DISABLE, 0.0); // De-energize + sys.spindle_stop_ovr = + SPINDLE_STOP_OVR_ENABLED; // Set stop override state to enabled, if de-energized. + } else { + sys.spindle_stop_ovr = SPINDLE_STOP_OVR_DISABLED; // Clear stop override state + } + // Handles restoring of spindle state + } else if (sys.spindle_stop_ovr & (SPINDLE_STOP_OVR_RESTORE | SPINDLE_STOP_OVR_RESTORE_CYCLE)) { + if (gc_state.modal.spindle != SPINDLE_DISABLE) { + report_feedback_message(MESSAGE_SPINDLE_RESTORE); + if (bit_istrue(settings.flags, BITFLAG_LASER_MODE)) { + // When in laser mode, ignore spindle spin-up delay. Set to turn on laser when cycle + // starts. + bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_PWM); + } else { + spindle_set_state( + (restore_condition & (PL_COND_FLAG_SPINDLE_CW | PL_COND_FLAG_SPINDLE_CCW)), + restore_spindle_speed); + } + } + if (sys.spindle_stop_ovr & SPINDLE_STOP_OVR_RESTORE_CYCLE) { + system_set_exec_state_flag(EXEC_CYCLE_START); // Set to resume program. + } + sys.spindle_stop_ovr = SPINDLE_STOP_OVR_DISABLED; // Clear stop override state + } + } else { + // Handles spindle state during hold. NOTE: Spindle speed overrides may be altered during hold + // state. NOTE: STEP_CONTROL_UPDATE_SPINDLE_PWM is automatically reset upon resume in step + // generator. + if (bit_istrue(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_PWM)) { + spindle_set_state((restore_condition & (PL_COND_FLAG_SPINDLE_CW | PL_COND_FLAG_SPINDLE_CCW)), + restore_spindle_speed); + bit_false(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_PWM); + } + } + } + } + + protocol_exec_rt_system(); + } } diff --git a/grbl/protocol.h b/grbl/protocol.h index 7bc6e92..7d0dcf0 100644 --- a/grbl/protocol.h +++ b/grbl/protocol.h @@ -29,7 +29,7 @@ // memory space we can invest into here or we re-write the g-code parser not to have this // buffer. #ifndef LINE_BUFFER_SIZE - #define LINE_BUFFER_SIZE 80 +#define LINE_BUFFER_SIZE 80 #endif // Starts Grbl main loop. It handles all incoming characters from the serial port and executes diff --git a/grbl/report.c b/grbl/report.c index 000836f..0d03646 100644 --- a/grbl/report.c +++ b/grbl/report.c @@ -28,20 +28,39 @@ #include "grbl.h" - // Internal report utilities to reduce flash with repetitive tasks turned into functions. -void report_util_setting_prefix(uint8_t n) { serial_write('$'); print_uint8_base10(n); serial_write('='); } -static void report_util_line_feed() { printPgmString(PSTR("\r\n")); } -static void report_util_feedback_line_feed() { serial_write(']'); report_util_line_feed(); } -static void report_util_gcode_modes_G() { printPgmString(PSTR(" G")); } -static void report_util_gcode_modes_M() { printPgmString(PSTR(" M")); } +void report_util_setting_prefix(uint8_t n) { + serial_write('$'); + print_uint8_base10(n); + serial_write('='); +} + +static void report_util_line_feed() { + printPgmString(PSTR("\r\n")); +} + +static void report_util_feedback_line_feed() { + serial_write(']'); + report_util_line_feed(); +} + +static void report_util_gcode_modes_G() { + printPgmString(PSTR(" G")); +} + +static void report_util_gcode_modes_M() { + printPgmString(PSTR(" M")); +} + // static void report_util_comment_line_feed() { serial_write(')'); report_util_line_feed(); } static void report_util_axis_values(float *axis_value) { - uint8_t idx; - for (idx=0; idx= MOTION_MODE_PROBE_TOWARD) { - printPgmString(PSTR("38.")); - print_uint8_base10(gc_state.modal.motion - (MOTION_MODE_PROBE_TOWARD-2)); - } else { - print_uint8_base10(gc_state.modal.motion); - } - - report_util_gcode_modes_G(); - print_uint8_base10(gc_state.modal.coord_select+54); - - report_util_gcode_modes_G(); - print_uint8_base10(gc_state.modal.plane_select+17); - - report_util_gcode_modes_G(); - print_uint8_base10(21-gc_state.modal.units); - - report_util_gcode_modes_G(); - print_uint8_base10(gc_state.modal.distance+90); - - report_util_gcode_modes_G(); - print_uint8_base10(94-gc_state.modal.feed_rate); - - if (gc_state.modal.program_flow) { - report_util_gcode_modes_M(); - switch (gc_state.modal.program_flow) { - case PROGRAM_FLOW_PAUSED : serial_write('0'); break; - // case PROGRAM_FLOW_OPTIONAL_STOP : serial_write('1'); break; // M1 is ignored and not supported. - case PROGRAM_FLOW_COMPLETED_M2 : - case PROGRAM_FLOW_COMPLETED_M30 : - print_uint8_base10(gc_state.modal.program_flow); - break; +void report_gcode_modes() { + printPgmString(PSTR("[GC:G")); + if (gc_state.modal.motion >= MOTION_MODE_PROBE_TOWARD) { + printPgmString(PSTR("38.")); + print_uint8_base10(gc_state.modal.motion - (MOTION_MODE_PROBE_TOWARD - 2)); + } else { + print_uint8_base10(gc_state.modal.motion); } - } - report_util_gcode_modes_M(); - switch (gc_state.modal.spindle) { - case SPINDLE_ENABLE_CW : serial_write('3'); break; - case SPINDLE_ENABLE_CCW : serial_write('4'); break; - case SPINDLE_DISABLE : serial_write('5'); break; - } + report_util_gcode_modes_G(); + print_uint8_base10(gc_state.modal.coord_select + 54); - #ifdef ENABLE_M7 + report_util_gcode_modes_G(); + print_uint8_base10(gc_state.modal.plane_select + 17); + + report_util_gcode_modes_G(); + print_uint8_base10(21 - gc_state.modal.units); + + report_util_gcode_modes_G(); + print_uint8_base10(gc_state.modal.distance + 90); + + report_util_gcode_modes_G(); + print_uint8_base10(94 - gc_state.modal.feed_rate); + + if (gc_state.modal.program_flow) { + report_util_gcode_modes_M(); + switch (gc_state.modal.program_flow) { + case PROGRAM_FLOW_PAUSED: serial_write('0'); break; + // case PROGRAM_FLOW_OPTIONAL_STOP : serial_write('1'); break; // M1 is ignored and not supported. + case PROGRAM_FLOW_COMPLETED_M2: + case PROGRAM_FLOW_COMPLETED_M30: print_uint8_base10(gc_state.modal.program_flow); break; + } + } + + report_util_gcode_modes_M(); + switch (gc_state.modal.spindle) { + case SPINDLE_ENABLE_CW: serial_write('3'); break; + case SPINDLE_ENABLE_CCW: serial_write('4'); break; + case SPINDLE_DISABLE: serial_write('5'); break; + } + +#ifdef ENABLE_M7 if (gc_state.modal.coolant) { // Note: Multiple coolant states may be active at the same time. - if (gc_state.modal.coolant & PL_COND_FLAG_COOLANT_MIST) { report_util_gcode_modes_M(); serial_write('7'); } - if (gc_state.modal.coolant & PL_COND_FLAG_COOLANT_FLOOD) { report_util_gcode_modes_M(); serial_write('8'); } - } else { report_util_gcode_modes_M(); serial_write('9'); } - #else - report_util_gcode_modes_M(); - if (gc_state.modal.coolant) { serial_write('8'); } - else { serial_write('9'); } - #endif - - #ifdef ENABLE_PARKING_OVERRIDE_CONTROL - if (sys.override_ctrl == OVERRIDE_PARKING_MOTION) { - report_util_gcode_modes_M(); - print_uint8_base10(56); + if (gc_state.modal.coolant & PL_COND_FLAG_COOLANT_MIST) { + report_util_gcode_modes_M(); + serial_write('7'); + } + if (gc_state.modal.coolant & PL_COND_FLAG_COOLANT_FLOOD) { + report_util_gcode_modes_M(); + serial_write('8'); + } + } else { + report_util_gcode_modes_M(); + serial_write('9'); } - #endif - - printPgmString(PSTR(" T")); - print_uint8_base10(gc_state.tool); +#else + report_util_gcode_modes_M(); + if (gc_state.modal.coolant) { + serial_write('8'); + } else { + serial_write('9'); + } +#endif - printPgmString(PSTR(" F")); - printFloat_RateValue(gc_state.feed_rate); +#ifdef ENABLE_PARKING_OVERRIDE_CONTROL + if (sys.override_ctrl == OVERRIDE_PARKING_MOTION) { + report_util_gcode_modes_M(); + print_uint8_base10(56); + } +#endif - #ifdef VARIABLE_SPINDLE + printPgmString(PSTR(" T")); + print_uint8_base10(gc_state.tool); + + printPgmString(PSTR(" F")); + printFloat_RateValue(gc_state.feed_rate); + +#ifdef VARIABLE_SPINDLE printPgmString(PSTR(" S")); - printFloat(gc_state.spindle_speed,N_DECIMAL_RPMVALUE); - #endif + printFloat(gc_state.spindle_speed, N_DECIMAL_RPMVALUE); +#endif - report_util_feedback_line_feed(); + report_util_feedback_line_feed(); } // Prints specified startup line -void report_startup_line(uint8_t n, char *line) -{ - printPgmString(PSTR("$N")); - print_uint8_base10(n); - serial_write('='); - printString(line); - report_util_line_feed(); +void report_startup_line(uint8_t n, char *line) { + printPgmString(PSTR("$N")); + print_uint8_base10(n); + serial_write('='); + printString(line); + report_util_line_feed(); } -void report_execute_startup_message(char *line, uint8_t status_code) -{ - serial_write('>'); - printString(line); - serial_write(':'); - report_status_message(status_code); +void report_execute_startup_message(char *line, uint8_t status_code) { + serial_write('>'); + printString(line); + serial_write(':'); + report_status_message(status_code); } // Prints build info line -void report_build_info(char *line) -{ - printPgmString(PSTR("[VER:" GRBL_VERSION "." GRBL_VERSION_BUILD ":")); - printString(line); - report_util_feedback_line_feed(); - printPgmString(PSTR("[OPT:")); // Generate compile-time build option list - #ifdef VARIABLE_SPINDLE +void report_build_info(char *line) { + printPgmString(PSTR("[VER:" GRBL_VERSION "." GRBL_VERSION_BUILD ":")); + printString(line); + report_util_feedback_line_feed(); + printPgmString(PSTR("[OPT:")); // Generate compile-time build option list +#ifdef VARIABLE_SPINDLE serial_write('V'); - #endif - #ifdef USE_LINE_NUMBERS +#endif +#ifdef USE_LINE_NUMBERS serial_write('N'); - #endif - #ifdef ENABLE_M7 +#endif +#ifdef ENABLE_M7 serial_write('M'); - #endif - #ifdef COREXY +#endif +#ifdef COREXY serial_write('C'); - #endif - #ifdef PARKING_ENABLE +#endif +#ifdef PARKING_ENABLE serial_write('P'); - #endif - #ifdef HOMING_FORCE_SET_ORIGIN +#endif +#ifdef HOMING_FORCE_SET_ORIGIN serial_write('Z'); - #endif - #ifdef HOMING_SINGLE_AXIS_COMMANDS +#endif +#ifdef HOMING_SINGLE_AXIS_COMMANDS serial_write('H'); - #endif - #ifdef LIMITS_TWO_SWITCHES_ON_AXES +#endif +#ifdef LIMITS_TWO_SWITCHES_ON_AXES serial_write('T'); - #endif - #ifdef ALLOW_FEED_OVERRIDE_DURING_PROBE_CYCLES +#endif +#ifdef ALLOW_FEED_OVERRIDE_DURING_PROBE_CYCLES serial_write('A'); - #endif - #ifdef USE_SPINDLE_DIR_AS_ENABLE_PIN +#endif +#ifdef USE_SPINDLE_DIR_AS_ENABLE_PIN serial_write('D'); - #endif - #ifdef SPINDLE_ENABLE_OFF_WITH_ZERO_SPEED +#endif +#ifdef SPINDLE_ENABLE_OFF_WITH_ZERO_SPEED serial_write('0'); - #endif - #ifdef ENABLE_SOFTWARE_DEBOUNCE +#endif +#ifdef ENABLE_SOFTWARE_DEBOUNCE serial_write('S'); - #endif - #ifdef ENABLE_PARKING_OVERRIDE_CONTROL +#endif +#ifdef ENABLE_PARKING_OVERRIDE_CONTROL serial_write('R'); - #endif - #ifndef HOMING_INIT_LOCK +#endif +#ifndef HOMING_INIT_LOCK serial_write('L'); - #endif - #ifdef ENABLE_SAFETY_DOOR_INPUT_PIN +#endif +#ifdef ENABLE_SAFETY_DOOR_INPUT_PIN serial_write('+'); - #endif - #ifndef ENABLE_RESTORE_EEPROM_WIPE_ALL // NOTE: Shown when disabled. +#endif +#ifndef ENABLE_RESTORE_EEPROM_WIPE_ALL // NOTE: Shown when disabled. serial_write('*'); - #endif - #ifndef ENABLE_RESTORE_EEPROM_DEFAULT_SETTINGS // NOTE: Shown when disabled. +#endif +#ifndef ENABLE_RESTORE_EEPROM_DEFAULT_SETTINGS // NOTE: Shown when disabled. serial_write('$'); - #endif - #ifndef ENABLE_RESTORE_EEPROM_CLEAR_PARAMETERS // NOTE: Shown when disabled. +#endif +#ifndef ENABLE_RESTORE_EEPROM_CLEAR_PARAMETERS // NOTE: Shown when disabled. serial_write('#'); - #endif - #ifndef ENABLE_BUILD_INFO_WRITE_COMMAND // NOTE: Shown when disabled. +#endif +#ifndef ENABLE_BUILD_INFO_WRITE_COMMAND // NOTE: Shown when disabled. serial_write('I'); - #endif - #ifndef FORCE_BUFFER_SYNC_DURING_EEPROM_WRITE // NOTE: Shown when disabled. +#endif +#ifndef FORCE_BUFFER_SYNC_DURING_EEPROM_WRITE // NOTE: Shown when disabled. serial_write('E'); - #endif - #ifndef FORCE_BUFFER_SYNC_DURING_WCO_CHANGE // NOTE: Shown when disabled. +#endif +#ifndef FORCE_BUFFER_SYNC_DURING_WCO_CHANGE // NOTE: Shown when disabled. serial_write('W'); - #endif - #ifdef ENABLE_DUAL_AXIS +#endif +#ifdef ENABLE_DUAL_AXIS serial_write('2'); - #endif - // NOTE: Compiled values, like override increments/max/min values, may be added at some point later. - serial_write(','); - print_uint8_base10(BLOCK_BUFFER_SIZE-1); - serial_write(','); - print_uint8_base10(RX_BUFFER_SIZE); +#endif + // NOTE: Compiled values, like override increments/max/min values, may be added at some point later. + serial_write(','); + print_uint8_base10(BLOCK_BUFFER_SIZE - 1); + serial_write(','); + print_uint8_base10(RX_BUFFER_SIZE); - report_util_feedback_line_feed(); + report_util_feedback_line_feed(); } - // Prints the character string line Grbl has received from the user, which has been pre-parsed, // and has been sent into protocol_execute_line() routine to be executed by Grbl. -void report_echo_line_received(char *line) -{ - printPgmString(PSTR("[echo: ")); printString(line); - report_util_feedback_line_feed(); +void report_echo_line_received(char *line) { + printPgmString(PSTR("[echo: ")); + printString(line); + report_util_feedback_line_feed(); } +// Prints real-time data. This function grabs a real-time snapshot of the stepper subprogram +// and the actual location of the CNC machine. Users may change the following function to their +// specific needs, but the desired real-time data report must be as short as possible. This is +// requires as it minimizes the computational overhead and allows grbl to keep running smoothly, +// especially during g-code programs with fast, short line segments and high frequency reports (5-20Hz). +void report_realtime_status() { + uint8_t idx; + int32_t current_position[N_AXIS]; // Copy current state of the system position variable + memcpy(current_position, sys_position, sizeof(sys_position)); + float print_position[N_AXIS]; + system_convert_array_steps_to_mpos(print_position, current_position); - // Prints real-time data. This function grabs a real-time snapshot of the stepper subprogram - // and the actual location of the CNC machine. Users may change the following function to their - // specific needs, but the desired real-time data report must be as short as possible. This is - // requires as it minimizes the computational overhead and allows grbl to keep running smoothly, - // especially during g-code programs with fast, short line segments and high frequency reports (5-20Hz). -void report_realtime_status() -{ - uint8_t idx; - int32_t current_position[N_AXIS]; // Copy current state of the system position variable - memcpy(current_position,sys_position,sizeof(sys_position)); - float print_position[N_AXIS]; - system_convert_array_steps_to_mpos(print_position,current_position); - - // Report current machine state and sub-states - serial_write('<'); - switch (sys.state) { + // Report current machine state and sub-states + serial_write('<'); + switch (sys.state) { case STATE_IDLE: printPgmString(PSTR("Idle")); break; case STATE_CYCLE: printPgmString(PSTR("Run")); break; case STATE_HOLD: - if (!(sys.suspend & SUSPEND_JOG_CANCEL)) { - printPgmString(PSTR("Hold:")); - if (sys.suspend & SUSPEND_HOLD_COMPLETE) { serial_write('0'); } // Ready to resume - else { serial_write('1'); } // Actively holding - break; - } // Continues to print jog state during jog cancel. + if (!(sys.suspend & SUSPEND_JOG_CANCEL)) { + printPgmString(PSTR("Hold:")); + if (sys.suspend & SUSPEND_HOLD_COMPLETE) { + serial_write('0'); + } // Ready to resume + else { + serial_write('1'); + } // Actively holding + break; + } // Continues to print jog state during jog cancel. case STATE_JOG: printPgmString(PSTR("Jog")); break; case STATE_HOMING: printPgmString(PSTR("Home")); break; case STATE_ALARM: printPgmString(PSTR("Alarm")); break; case STATE_CHECK_MODE: printPgmString(PSTR("Check")); break; case STATE_SAFETY_DOOR: - printPgmString(PSTR("Door:")); - if (sys.suspend & SUSPEND_INITIATE_RESTORE) { - serial_write('3'); // Restoring - } else { - if (sys.suspend & SUSPEND_RETRACT_COMPLETE) { - if (sys.suspend & SUSPEND_SAFETY_DOOR_AJAR) { - serial_write('1'); // Door ajar - } else { - serial_write('0'); - } // Door closed and ready to resume + printPgmString(PSTR("Door:")); + if (sys.suspend & SUSPEND_INITIATE_RESTORE) { + serial_write('3'); // Restoring } else { - serial_write('2'); // Retracting + if (sys.suspend & SUSPEND_RETRACT_COMPLETE) { + if (sys.suspend & SUSPEND_SAFETY_DOOR_AJAR) { + serial_write('1'); // Door ajar + } else { + serial_write('0'); + } // Door closed and ready to resume + } else { + serial_write('2'); // Retracting + } } - } - break; + break; case STATE_SLEEP: printPgmString(PSTR("Sleep")); break; - } - - float wco[N_AXIS]; - if (bit_isfalse(settings.status_report_mask,BITFLAG_RT_STATUS_POSITION_TYPE) || - (sys.report_wco_counter == 0) ) { - for (idx=0; idx< N_AXIS; idx++) { - // Apply work coordinate offsets and tool length offset to current position. - wco[idx] = gc_state.coord_system[idx]+gc_state.coord_offset[idx]; - if (idx == TOOL_LENGTH_OFFSET_AXIS) { wco[idx] += gc_state.tool_length_offset; } - if (bit_isfalse(settings.status_report_mask,BITFLAG_RT_STATUS_POSITION_TYPE)) { - print_position[idx] -= wco[idx]; - } } - } - // Report machine position - if (bit_istrue(settings.status_report_mask,BITFLAG_RT_STATUS_POSITION_TYPE)) { - printPgmString(PSTR("|MPos:")); - } else { - printPgmString(PSTR("|WPos:")); - } - report_util_axis_values(print_position); - - // Returns planner and serial read buffer states. - #ifdef REPORT_FIELD_BUFFER_STATE - if (bit_istrue(settings.status_report_mask,BITFLAG_RT_STATUS_BUFFER_STATE)) { - printPgmString(PSTR("|Bf:")); - print_uint8_base10(plan_get_block_buffer_available()); - serial_write(','); - print_uint8_base10(serial_get_rx_buffer_available()); + float wco[N_AXIS]; + if (bit_isfalse(settings.status_report_mask, BITFLAG_RT_STATUS_POSITION_TYPE) || (sys.report_wco_counter == 0)) { + for (idx = 0; idx < N_AXIS; idx++) { + // Apply work coordinate offsets and tool length offset to current position. + wco[idx] = gc_state.coord_system[idx] + gc_state.coord_offset[idx]; + if (idx == TOOL_LENGTH_OFFSET_AXIS) { + wco[idx] += gc_state.tool_length_offset; + } + if (bit_isfalse(settings.status_report_mask, BITFLAG_RT_STATUS_POSITION_TYPE)) { + print_position[idx] -= wco[idx]; + } + } } - #endif - #ifdef USE_LINE_NUMBERS - #ifdef REPORT_FIELD_LINE_NUMBERS - // Report current line number - plan_block_t * cur_block = plan_get_current_block(); - if (cur_block != NULL) { + // Report machine position + if (bit_istrue(settings.status_report_mask, BITFLAG_RT_STATUS_POSITION_TYPE)) { + printPgmString(PSTR("|MPos:")); + } else { + printPgmString(PSTR("|WPos:")); + } + report_util_axis_values(print_position); + +// Returns planner and serial read buffer states. +#ifdef REPORT_FIELD_BUFFER_STATE + if (bit_istrue(settings.status_report_mask, BITFLAG_RT_STATUS_BUFFER_STATE)) { + printPgmString(PSTR("|Bf:")); + print_uint8_base10(plan_get_block_buffer_available()); + serial_write(','); + print_uint8_base10(serial_get_rx_buffer_available()); + } +#endif + +#ifdef USE_LINE_NUMBERS +#ifdef REPORT_FIELD_LINE_NUMBERS + // Report current line number + plan_block_t *cur_block = plan_get_current_block(); + if (cur_block != NULL) { uint32_t ln = cur_block->line_number; if (ln > 0) { - printPgmString(PSTR("|Ln:")); - printInteger(ln); + printPgmString(PSTR("|Ln:")); + printInteger(ln); } - } - #endif - #endif + } +#endif +#endif - // Report realtime feed speed - #ifdef REPORT_FIELD_CURRENT_FEED_SPEED - #ifdef VARIABLE_SPINDLE - printPgmString(PSTR("|FS:")); - printFloat_RateValue(st_get_realtime_rate()); - serial_write(','); - printFloat(sys.spindle_speed,N_DECIMAL_RPMVALUE); - #else - printPgmString(PSTR("|F:")); - printFloat_RateValue(st_get_realtime_rate()); - #endif - #endif +// Report realtime feed speed +#ifdef REPORT_FIELD_CURRENT_FEED_SPEED +#ifdef VARIABLE_SPINDLE + printPgmString(PSTR("|FS:")); + printFloat_RateValue(st_get_realtime_rate()); + serial_write(','); + printFloat(sys.spindle_speed, N_DECIMAL_RPMVALUE); +#else + printPgmString(PSTR("|F:")); + printFloat_RateValue(st_get_realtime_rate()); +#endif +#endif - #ifdef REPORT_FIELD_PIN_STATE +#ifdef REPORT_FIELD_PIN_STATE uint8_t lim_pin_state = limits_get_state(); uint8_t ctrl_pin_state = system_control_get_state(); uint8_t prb_pin_state = probe_get_state(); if (lim_pin_state | ctrl_pin_state | prb_pin_state) { - printPgmString(PSTR("|Pn:")); - if (prb_pin_state) { serial_write('P'); } - if (lim_pin_state) { - #ifdef ENABLE_DUAL_AXIS - #if (DUAL_AXIS_SELECT == X_AXIS) - if (bit_istrue(lim_pin_state,(bit(X_AXIS)|bit(N_AXIS)))) { serial_write('X'); } - if (bit_istrue(lim_pin_state,bit(Y_AXIS))) { serial_write('Y'); } - #endif - #if (DUAL_AXIS_SELECT == Y_AXIS) - if (bit_istrue(lim_pin_state,bit(X_AXIS))) { serial_write('X'); } - if (bit_istrue(lim_pin_state,(bit(Y_AXIS)|bit(N_AXIS)))) { serial_write('Y'); } - #endif - if (bit_istrue(lim_pin_state,bit(Z_AXIS))) { serial_write('Z'); } - #else - if (bit_istrue(lim_pin_state,bit(X_AXIS))) { serial_write('X'); } - if (bit_istrue(lim_pin_state,bit(Y_AXIS))) { serial_write('Y'); } - if (bit_istrue(lim_pin_state,bit(Z_AXIS))) { serial_write('Z'); } - #endif - } - if (ctrl_pin_state) { - #ifdef ENABLE_SAFETY_DOOR_INPUT_PIN - if (bit_istrue(ctrl_pin_state,CONTROL_PIN_INDEX_SAFETY_DOOR)) { serial_write('D'); } - #endif - if (bit_istrue(ctrl_pin_state,CONTROL_PIN_INDEX_RESET)) { serial_write('R'); } - if (bit_istrue(ctrl_pin_state,CONTROL_PIN_INDEX_FEED_HOLD)) { serial_write('H'); } - if (bit_istrue(ctrl_pin_state,CONTROL_PIN_INDEX_CYCLE_START)) { serial_write('S'); } - } - } - #endif - - #ifdef REPORT_FIELD_WORK_COORD_OFFSET - if (sys.report_wco_counter > 0) { sys.report_wco_counter--; } - else { - if (sys.state & (STATE_HOMING | STATE_CYCLE | STATE_HOLD | STATE_JOG | STATE_SAFETY_DOOR)) { - sys.report_wco_counter = (REPORT_WCO_REFRESH_BUSY_COUNT-1); // Reset counter for slow refresh - } else { sys.report_wco_counter = (REPORT_WCO_REFRESH_IDLE_COUNT-1); } - if (sys.report_ovr_counter == 0) { sys.report_ovr_counter = 1; } // Set override on next report. - printPgmString(PSTR("|WCO:")); - report_util_axis_values(wco); - } - #endif - - #ifdef REPORT_FIELD_OVERRIDES - if (sys.report_ovr_counter > 0) { sys.report_ovr_counter--; } - else { - if (sys.state & (STATE_HOMING | STATE_CYCLE | STATE_HOLD | STATE_JOG | STATE_SAFETY_DOOR)) { - sys.report_ovr_counter = (REPORT_OVR_REFRESH_BUSY_COUNT-1); // Reset counter for slow refresh - } else { sys.report_ovr_counter = (REPORT_OVR_REFRESH_IDLE_COUNT-1); } - printPgmString(PSTR("|Ov:")); - print_uint8_base10(sys.f_override); - serial_write(','); - print_uint8_base10(sys.r_override); - serial_write(','); - print_uint8_base10(sys.spindle_speed_ovr); - - uint8_t sp_state = spindle_get_state(); - uint8_t cl_state = coolant_get_state(); - if (sp_state || cl_state) { - printPgmString(PSTR("|A:")); - if (sp_state) { // != SPINDLE_STATE_DISABLE - #ifdef VARIABLE_SPINDLE - #ifdef USE_SPINDLE_DIR_AS_ENABLE_PIN - serial_write('S'); // CW - #else - if (sp_state == SPINDLE_STATE_CW) { serial_write('S'); } // CW - else { serial_write('C'); } // CCW - #endif - #else - if (sp_state & SPINDLE_STATE_CW) { serial_write('S'); } // CW - else { serial_write('C'); } // CCW - #endif + printPgmString(PSTR("|Pn:")); + if (prb_pin_state) { + serial_write('P'); + } + if (lim_pin_state) { +#ifdef ENABLE_DUAL_AXIS +#if (DUAL_AXIS_SELECT == X_AXIS) + if (bit_istrue(lim_pin_state, (bit(X_AXIS) | bit(N_AXIS)))) { + serial_write('X'); + } + if (bit_istrue(lim_pin_state, bit(Y_AXIS))) { + serial_write('Y'); + } +#endif +#if (DUAL_AXIS_SELECT == Y_AXIS) + if (bit_istrue(lim_pin_state, bit(X_AXIS))) { + serial_write('X'); + } + if (bit_istrue(lim_pin_state, (bit(Y_AXIS) | bit(N_AXIS)))) { + serial_write('Y'); + } +#endif + if (bit_istrue(lim_pin_state, bit(Z_AXIS))) { + serial_write('Z'); + } +#else + if (bit_istrue(lim_pin_state, bit(X_AXIS))) { + serial_write('X'); + } + if (bit_istrue(lim_pin_state, bit(Y_AXIS))) { + serial_write('Y'); + } + if (bit_istrue(lim_pin_state, bit(Z_AXIS))) { + serial_write('Z'); + } +#endif + } + if (ctrl_pin_state) { +#ifdef ENABLE_SAFETY_DOOR_INPUT_PIN + if (bit_istrue(ctrl_pin_state, CONTROL_PIN_INDEX_SAFETY_DOOR)) { + serial_write('D'); + } +#endif + if (bit_istrue(ctrl_pin_state, CONTROL_PIN_INDEX_RESET)) { + serial_write('R'); + } + if (bit_istrue(ctrl_pin_state, CONTROL_PIN_INDEX_FEED_HOLD)) { + serial_write('H'); + } + if (bit_istrue(ctrl_pin_state, CONTROL_PIN_INDEX_CYCLE_START)) { + serial_write('S'); + } } - if (cl_state & COOLANT_STATE_FLOOD) { serial_write('F'); } - #ifdef ENABLE_M7 - if (cl_state & COOLANT_STATE_MIST) { serial_write('M'); } - #endif - } } - #endif +#endif - serial_write('>'); - report_util_line_feed(); +#ifdef REPORT_FIELD_WORK_COORD_OFFSET + if (sys.report_wco_counter > 0) { + sys.report_wco_counter--; + } else { + if (sys.state & (STATE_HOMING | STATE_CYCLE | STATE_HOLD | STATE_JOG | STATE_SAFETY_DOOR)) { + sys.report_wco_counter = (REPORT_WCO_REFRESH_BUSY_COUNT - 1); // Reset counter for slow refresh + } else { + sys.report_wco_counter = (REPORT_WCO_REFRESH_IDLE_COUNT - 1); + } + if (sys.report_ovr_counter == 0) { + sys.report_ovr_counter = 1; + } // Set override on next report. + printPgmString(PSTR("|WCO:")); + report_util_axis_values(wco); + } +#endif + +#ifdef REPORT_FIELD_OVERRIDES + if (sys.report_ovr_counter > 0) { + sys.report_ovr_counter--; + } else { + if (sys.state & (STATE_HOMING | STATE_CYCLE | STATE_HOLD | STATE_JOG | STATE_SAFETY_DOOR)) { + sys.report_ovr_counter = (REPORT_OVR_REFRESH_BUSY_COUNT - 1); // Reset counter for slow refresh + } else { + sys.report_ovr_counter = (REPORT_OVR_REFRESH_IDLE_COUNT - 1); + } + printPgmString(PSTR("|Ov:")); + print_uint8_base10(sys.f_override); + serial_write(','); + print_uint8_base10(sys.r_override); + serial_write(','); + print_uint8_base10(sys.spindle_speed_ovr); + + uint8_t sp_state = spindle_get_state(); + uint8_t cl_state = coolant_get_state(); + if (sp_state || cl_state) { + printPgmString(PSTR("|A:")); + if (sp_state) { // != SPINDLE_STATE_DISABLE +#ifdef VARIABLE_SPINDLE +#ifdef USE_SPINDLE_DIR_AS_ENABLE_PIN + serial_write('S'); // CW +#else + if (sp_state == SPINDLE_STATE_CW) { + serial_write('S'); + } // CW + else { + serial_write('C'); + } // CCW +#endif +#else + if (sp_state & SPINDLE_STATE_CW) { + serial_write('S'); + } // CW + else { + serial_write('C'); + } // CCW +#endif + } + if (cl_state & COOLANT_STATE_FLOOD) { + serial_write('F'); + } +#ifdef ENABLE_M7 + if (cl_state & COOLANT_STATE_MIST) { + serial_write('M'); + } +#endif + } + } +#endif + + serial_write('>'); + report_util_line_feed(); } - #ifdef DEBUG - void report_realtime_debug() - { - - } +void report_realtime_debug() { +} #endif diff --git a/grbl/report.h b/grbl/report.h index f148002..7d844fa 100644 --- a/grbl/report.h +++ b/grbl/report.h @@ -21,68 +21,68 @@ #define report_h // Define Grbl status codes. Valid values (0-255) -#define STATUS_OK 0 +#define STATUS_OK 0 #define STATUS_EXPECTED_COMMAND_LETTER 1 -#define STATUS_BAD_NUMBER_FORMAT 2 -#define STATUS_INVALID_STATEMENT 3 -#define STATUS_NEGATIVE_VALUE 4 -#define STATUS_SETTING_DISABLED 5 -#define STATUS_SETTING_STEP_PULSE_MIN 6 -#define STATUS_SETTING_READ_FAIL 7 -#define STATUS_IDLE_ERROR 8 -#define STATUS_SYSTEM_GC_LOCK 9 -#define STATUS_SOFT_LIMIT_ERROR 10 -#define STATUS_OVERFLOW 11 -#define STATUS_MAX_STEP_RATE_EXCEEDED 12 -#define STATUS_CHECK_DOOR 13 -#define STATUS_LINE_LENGTH_EXCEEDED 14 -#define STATUS_TRAVEL_EXCEEDED 15 -#define STATUS_INVALID_JOG_COMMAND 16 -#define STATUS_SETTING_DISABLED_LASER 17 +#define STATUS_BAD_NUMBER_FORMAT 2 +#define STATUS_INVALID_STATEMENT 3 +#define STATUS_NEGATIVE_VALUE 4 +#define STATUS_SETTING_DISABLED 5 +#define STATUS_SETTING_STEP_PULSE_MIN 6 +#define STATUS_SETTING_READ_FAIL 7 +#define STATUS_IDLE_ERROR 8 +#define STATUS_SYSTEM_GC_LOCK 9 +#define STATUS_SOFT_LIMIT_ERROR 10 +#define STATUS_OVERFLOW 11 +#define STATUS_MAX_STEP_RATE_EXCEEDED 12 +#define STATUS_CHECK_DOOR 13 +#define STATUS_LINE_LENGTH_EXCEEDED 14 +#define STATUS_TRAVEL_EXCEEDED 15 +#define STATUS_INVALID_JOG_COMMAND 16 +#define STATUS_SETTING_DISABLED_LASER 17 -#define STATUS_GCODE_UNSUPPORTED_COMMAND 20 -#define STATUS_GCODE_MODAL_GROUP_VIOLATION 21 -#define STATUS_GCODE_UNDEFINED_FEED_RATE 22 +#define STATUS_GCODE_UNSUPPORTED_COMMAND 20 +#define STATUS_GCODE_MODAL_GROUP_VIOLATION 21 +#define STATUS_GCODE_UNDEFINED_FEED_RATE 22 #define STATUS_GCODE_COMMAND_VALUE_NOT_INTEGER 23 -#define STATUS_GCODE_AXIS_COMMAND_CONFLICT 24 -#define STATUS_GCODE_WORD_REPEATED 25 -#define STATUS_GCODE_NO_AXIS_WORDS 26 -#define STATUS_GCODE_INVALID_LINE_NUMBER 27 -#define STATUS_GCODE_VALUE_WORD_MISSING 28 -#define STATUS_GCODE_UNSUPPORTED_COORD_SYS 29 -#define STATUS_GCODE_G53_INVALID_MOTION_MODE 30 -#define STATUS_GCODE_AXIS_WORDS_EXIST 31 -#define STATUS_GCODE_NO_AXIS_WORDS_IN_PLANE 32 -#define STATUS_GCODE_INVALID_TARGET 33 -#define STATUS_GCODE_ARC_RADIUS_ERROR 34 -#define STATUS_GCODE_NO_OFFSETS_IN_PLANE 35 -#define STATUS_GCODE_UNUSED_WORDS 36 -#define STATUS_GCODE_G43_DYNAMIC_AXIS_ERROR 37 -#define STATUS_GCODE_MAX_VALUE_EXCEEDED 38 +#define STATUS_GCODE_AXIS_COMMAND_CONFLICT 24 +#define STATUS_GCODE_WORD_REPEATED 25 +#define STATUS_GCODE_NO_AXIS_WORDS 26 +#define STATUS_GCODE_INVALID_LINE_NUMBER 27 +#define STATUS_GCODE_VALUE_WORD_MISSING 28 +#define STATUS_GCODE_UNSUPPORTED_COORD_SYS 29 +#define STATUS_GCODE_G53_INVALID_MOTION_MODE 30 +#define STATUS_GCODE_AXIS_WORDS_EXIST 31 +#define STATUS_GCODE_NO_AXIS_WORDS_IN_PLANE 32 +#define STATUS_GCODE_INVALID_TARGET 33 +#define STATUS_GCODE_ARC_RADIUS_ERROR 34 +#define STATUS_GCODE_NO_OFFSETS_IN_PLANE 35 +#define STATUS_GCODE_UNUSED_WORDS 36 +#define STATUS_GCODE_G43_DYNAMIC_AXIS_ERROR 37 +#define STATUS_GCODE_MAX_VALUE_EXCEEDED 38 // Define Grbl alarm codes. Valid values (1-255). 0 is reserved. -#define ALARM_HARD_LIMIT_ERROR EXEC_ALARM_HARD_LIMIT -#define ALARM_SOFT_LIMIT_ERROR EXEC_ALARM_SOFT_LIMIT -#define ALARM_ABORT_CYCLE EXEC_ALARM_ABORT_CYCLE -#define ALARM_PROBE_FAIL_INITIAL EXEC_ALARM_PROBE_FAIL_INITIAL -#define ALARM_PROBE_FAIL_CONTACT EXEC_ALARM_PROBE_FAIL_CONTACT -#define ALARM_HOMING_FAIL_RESET EXEC_ALARM_HOMING_FAIL_RESET -#define ALARM_HOMING_FAIL_DOOR EXEC_ALARM_HOMING_FAIL_DOOR -#define ALARM_HOMING_FAIL_PULLOFF EXEC_ALARM_HOMING_FAIL_PULLOFF -#define ALARM_HOMING_FAIL_APPROACH EXEC_ALARM_HOMING_FAIL_APPROACH +#define ALARM_HARD_LIMIT_ERROR EXEC_ALARM_HARD_LIMIT +#define ALARM_SOFT_LIMIT_ERROR EXEC_ALARM_SOFT_LIMIT +#define ALARM_ABORT_CYCLE EXEC_ALARM_ABORT_CYCLE +#define ALARM_PROBE_FAIL_INITIAL EXEC_ALARM_PROBE_FAIL_INITIAL +#define ALARM_PROBE_FAIL_CONTACT EXEC_ALARM_PROBE_FAIL_CONTACT +#define ALARM_HOMING_FAIL_RESET EXEC_ALARM_HOMING_FAIL_RESET +#define ALARM_HOMING_FAIL_DOOR EXEC_ALARM_HOMING_FAIL_DOOR +#define ALARM_HOMING_FAIL_PULLOFF EXEC_ALARM_HOMING_FAIL_PULLOFF +#define ALARM_HOMING_FAIL_APPROACH EXEC_ALARM_HOMING_FAIL_APPROACH // Define Grbl feedback message codes. Valid values (0-255). -#define MESSAGE_CRITICAL_EVENT 1 -#define MESSAGE_ALARM_LOCK 2 -#define MESSAGE_ALARM_UNLOCK 3 -#define MESSAGE_ENABLED 4 -#define MESSAGE_DISABLED 5 +#define MESSAGE_CRITICAL_EVENT 1 +#define MESSAGE_ALARM_LOCK 2 +#define MESSAGE_ALARM_UNLOCK 3 +#define MESSAGE_ENABLED 4 +#define MESSAGE_DISABLED 5 #define MESSAGE_SAFETY_DOOR_AJAR 6 -#define MESSAGE_CHECK_LIMITS 7 -#define MESSAGE_PROGRAM_END 8 +#define MESSAGE_CHECK_LIMITS 7 +#define MESSAGE_PROGRAM_END 8 #define MESSAGE_RESTORE_DEFAULTS 9 -#define MESSAGE_SPINDLE_RESTORE 10 -#define MESSAGE_SLEEP_MODE 11 +#define MESSAGE_SPINDLE_RESTORE 10 +#define MESSAGE_SLEEP_MODE 11 // Prints system status messages. void report_status_message(uint8_t status_code); @@ -125,7 +125,7 @@ void report_execute_startup_message(char *line, uint8_t status_code); void report_build_info(char *line); #ifdef DEBUG - void report_realtime_debug(); +void report_realtime_debug(); #endif #endif diff --git a/grbl/serial.c b/grbl/serial.c index cf5f35e..6739451 100644 --- a/grbl/serial.c +++ b/grbl/serial.c @@ -21,184 +21,200 @@ #include "grbl.h" -#define RX_RING_BUFFER (RX_BUFFER_SIZE+1) -#define TX_RING_BUFFER (TX_BUFFER_SIZE+1) +#define RX_RING_BUFFER (RX_BUFFER_SIZE + 1) +#define TX_RING_BUFFER (TX_BUFFER_SIZE + 1) -uint8_t serial_rx_buffer[RX_RING_BUFFER]; -uint8_t serial_rx_buffer_head = 0; +uint8_t serial_rx_buffer[RX_RING_BUFFER]; +uint8_t serial_rx_buffer_head = 0; volatile uint8_t serial_rx_buffer_tail = 0; -uint8_t serial_tx_buffer[TX_RING_BUFFER]; -uint8_t serial_tx_buffer_head = 0; +uint8_t serial_tx_buffer[TX_RING_BUFFER]; +uint8_t serial_tx_buffer_head = 0; volatile uint8_t serial_tx_buffer_tail = 0; - // Returns the number of bytes available in the RX serial buffer. -uint8_t serial_get_rx_buffer_available() -{ - uint8_t rtail = serial_rx_buffer_tail; // Copy to limit multiple calls to volatile - if (serial_rx_buffer_head >= rtail) { return(RX_BUFFER_SIZE - (serial_rx_buffer_head-rtail)); } - return((rtail-serial_rx_buffer_head-1)); +uint8_t serial_get_rx_buffer_available() { + uint8_t rtail = serial_rx_buffer_tail; // Copy to limit multiple calls to volatile + if (serial_rx_buffer_head >= rtail) { + return (RX_BUFFER_SIZE - (serial_rx_buffer_head - rtail)); + } + return ((rtail - serial_rx_buffer_head - 1)); } - // Returns the number of bytes used in the RX serial buffer. // NOTE: Deprecated. Not used unless classic status reports are enabled in config.h. -uint8_t serial_get_rx_buffer_count() -{ - uint8_t rtail = serial_rx_buffer_tail; // Copy to limit multiple calls to volatile - if (serial_rx_buffer_head >= rtail) { return(serial_rx_buffer_head-rtail); } - return (RX_BUFFER_SIZE - (rtail-serial_rx_buffer_head)); +uint8_t serial_get_rx_buffer_count() { + uint8_t rtail = serial_rx_buffer_tail; // Copy to limit multiple calls to volatile + if (serial_rx_buffer_head >= rtail) { + return (serial_rx_buffer_head - rtail); + } + return (RX_BUFFER_SIZE - (rtail - serial_rx_buffer_head)); } - // Returns the number of bytes used in the TX serial buffer. // NOTE: Not used except for debugging and ensuring no TX bottlenecks. -uint8_t serial_get_tx_buffer_count() -{ - uint8_t ttail = serial_tx_buffer_tail; // Copy to limit multiple calls to volatile - if (serial_tx_buffer_head >= ttail) { return(serial_tx_buffer_head-ttail); } - return (TX_RING_BUFFER - (ttail-serial_tx_buffer_head)); +uint8_t serial_get_tx_buffer_count() { + uint8_t ttail = serial_tx_buffer_tail; // Copy to limit multiple calls to volatile + if (serial_tx_buffer_head >= ttail) { + return (serial_tx_buffer_head - ttail); + } + return (TX_RING_BUFFER - (ttail - serial_tx_buffer_head)); } - -void serial_init() -{ - // Set baud rate - #if BAUD_RATE < 57600 - uint16_t UBRR0_value = ((F_CPU / (8L * BAUD_RATE)) - 1)/2 ; +void serial_init() { +// Set baud rate +#if BAUD_RATE < 57600 + uint16_t UBRR0_value = ((F_CPU / (8L * BAUD_RATE)) - 1) / 2; UCSR0A &= ~(1 << U2X0); // baud doubler off - Only needed on Uno XXX - #else - uint16_t UBRR0_value = ((F_CPU / (4L * BAUD_RATE)) - 1)/2; - UCSR0A |= (1 << U2X0); // baud doubler on for high baud rates, i.e. 115200 - #endif - UBRR0H = UBRR0_value >> 8; - UBRR0L = UBRR0_value; +#else + uint16_t UBRR0_value = ((F_CPU / (4L * BAUD_RATE)) - 1) / 2; + UCSR0A |= (1 << U2X0); // baud doubler on for high baud rates, i.e. 115200 +#endif + UBRR0H = UBRR0_value >> 8; + UBRR0L = UBRR0_value; - // enable rx, tx, and interrupt on complete reception of a byte - UCSR0B |= (1< 0x7F) { // Real-time control characters are extended ACSII only. - switch(data) { - case CMD_SAFETY_DOOR: system_set_exec_state_flag(EXEC_SAFETY_DOOR); break; // Set as true - case CMD_JOG_CANCEL: - if (sys.state & STATE_JOG) { // Block all other states from invoking motion cancel. - system_set_exec_state_flag(EXEC_MOTION_CANCEL); + case CMD_CYCLE_START: system_set_exec_state_flag(EXEC_CYCLE_START); break; // Set as true + case CMD_FEED_HOLD: system_set_exec_state_flag(EXEC_FEED_HOLD); break; // Set as true + default: + if (data > 0x7F) { // Real-time control characters are extended ACSII only. + switch (data) { + case CMD_SAFETY_DOOR: system_set_exec_state_flag(EXEC_SAFETY_DOOR); break; // Set as true + case CMD_JOG_CANCEL: + if (sys.state & STATE_JOG) { // Block all other states from invoking motion cancel. + system_set_exec_state_flag(EXEC_MOTION_CANCEL); + } + break; +#ifdef DEBUG + case CMD_DEBUG_REPORT: { + uint8_t sreg = SREG; + cli(); + bit_true(sys_rt_exec_debug, EXEC_DEBUG_REPORT); + SREG = sreg; + } break; +#endif + case CMD_FEED_OVR_RESET: system_set_exec_motion_override_flag(EXEC_FEED_OVR_RESET); break; + case CMD_FEED_OVR_COARSE_PLUS: system_set_exec_motion_override_flag(EXEC_FEED_OVR_COARSE_PLUS); break; + case CMD_FEED_OVR_COARSE_MINUS: system_set_exec_motion_override_flag(EXEC_FEED_OVR_COARSE_MINUS); break; + case CMD_FEED_OVR_FINE_PLUS: system_set_exec_motion_override_flag(EXEC_FEED_OVR_FINE_PLUS); break; + case CMD_FEED_OVR_FINE_MINUS: system_set_exec_motion_override_flag(EXEC_FEED_OVR_FINE_MINUS); break; + case CMD_RAPID_OVR_RESET: system_set_exec_motion_override_flag(EXEC_RAPID_OVR_RESET); break; + case CMD_RAPID_OVR_MEDIUM: system_set_exec_motion_override_flag(EXEC_RAPID_OVR_MEDIUM); break; + case CMD_RAPID_OVR_LOW: system_set_exec_motion_override_flag(EXEC_RAPID_OVR_LOW); break; + case CMD_SPINDLE_OVR_RESET: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_RESET); break; + case CMD_SPINDLE_OVR_COARSE_PLUS: + system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_COARSE_PLUS); + break; + case CMD_SPINDLE_OVR_COARSE_MINUS: + system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_COARSE_MINUS); + break; + case CMD_SPINDLE_OVR_FINE_PLUS: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_FINE_PLUS); break; + case CMD_SPINDLE_OVR_FINE_MINUS: + system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_FINE_MINUS); + break; + case CMD_SPINDLE_OVR_STOP: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_STOP); break; + case CMD_COOLANT_FLOOD_OVR_TOGGLE: + system_set_exec_accessory_override_flag(EXEC_COOLANT_FLOOD_OVR_TOGGLE); + break; +#ifdef ENABLE_M7 + case CMD_COOLANT_MIST_OVR_TOGGLE: + system_set_exec_accessory_override_flag(EXEC_COOLANT_MIST_OVR_TOGGLE); + break; +#endif + } + // Throw away any unfound extended-ASCII character by not passing it to the serial buffer. + } else { // Write character to buffer + next_head = serial_rx_buffer_head + 1; + if (next_head == RX_RING_BUFFER) { + next_head = 0; } - break; - #ifdef DEBUG - case CMD_DEBUG_REPORT: {uint8_t sreg = SREG; cli(); bit_true(sys_rt_exec_debug,EXEC_DEBUG_REPORT); SREG = sreg;} break; - #endif - case CMD_FEED_OVR_RESET: system_set_exec_motion_override_flag(EXEC_FEED_OVR_RESET); break; - case CMD_FEED_OVR_COARSE_PLUS: system_set_exec_motion_override_flag(EXEC_FEED_OVR_COARSE_PLUS); break; - case CMD_FEED_OVR_COARSE_MINUS: system_set_exec_motion_override_flag(EXEC_FEED_OVR_COARSE_MINUS); break; - case CMD_FEED_OVR_FINE_PLUS: system_set_exec_motion_override_flag(EXEC_FEED_OVR_FINE_PLUS); break; - case CMD_FEED_OVR_FINE_MINUS: system_set_exec_motion_override_flag(EXEC_FEED_OVR_FINE_MINUS); break; - case CMD_RAPID_OVR_RESET: system_set_exec_motion_override_flag(EXEC_RAPID_OVR_RESET); break; - case CMD_RAPID_OVR_MEDIUM: system_set_exec_motion_override_flag(EXEC_RAPID_OVR_MEDIUM); break; - case CMD_RAPID_OVR_LOW: system_set_exec_motion_override_flag(EXEC_RAPID_OVR_LOW); break; - case CMD_SPINDLE_OVR_RESET: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_RESET); break; - case CMD_SPINDLE_OVR_COARSE_PLUS: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_COARSE_PLUS); break; - case CMD_SPINDLE_OVR_COARSE_MINUS: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_COARSE_MINUS); break; - case CMD_SPINDLE_OVR_FINE_PLUS: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_FINE_PLUS); break; - case CMD_SPINDLE_OVR_FINE_MINUS: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_FINE_MINUS); break; - case CMD_SPINDLE_OVR_STOP: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_STOP); break; - case CMD_COOLANT_FLOOD_OVR_TOGGLE: system_set_exec_accessory_override_flag(EXEC_COOLANT_FLOOD_OVR_TOGGLE); break; - #ifdef ENABLE_M7 - case CMD_COOLANT_MIST_OVR_TOGGLE: system_set_exec_accessory_override_flag(EXEC_COOLANT_MIST_OVR_TOGGLE); break; - #endif - } - // Throw away any unfound extended-ASCII character by not passing it to the serial buffer. - } else { // Write character to buffer - next_head = serial_rx_buffer_head + 1; - if (next_head == RX_RING_BUFFER) { next_head = 0; } - // Write data to buffer unless it is full. - if (next_head != serial_rx_buffer_tail) { - serial_rx_buffer[serial_rx_buffer_head] = data; - serial_rx_buffer_head = next_head; + // Write data to buffer unless it is full. + if (next_head != serial_rx_buffer_tail) { + serial_rx_buffer[serial_rx_buffer_head] = data; + serial_rx_buffer_head = next_head; + } } - } - } + } } - -void serial_reset_read_buffer() -{ - serial_rx_buffer_tail = serial_rx_buffer_head; +void serial_reset_read_buffer() { + serial_rx_buffer_tail = serial_rx_buffer_head; } diff --git a/grbl/serial.h b/grbl/serial.h index 5a3f776..390bc6a 100644 --- a/grbl/serial.h +++ b/grbl/serial.h @@ -22,21 +22,19 @@ #ifndef serial_h #define serial_h - #ifndef RX_BUFFER_SIZE - #define RX_BUFFER_SIZE 128 +#define RX_BUFFER_SIZE 128 #endif #ifndef TX_BUFFER_SIZE - #ifdef USE_LINE_NUMBERS - #define TX_BUFFER_SIZE 112 - #else - #define TX_BUFFER_SIZE 104 - #endif +#ifdef USE_LINE_NUMBERS +#define TX_BUFFER_SIZE 112 +#else +#define TX_BUFFER_SIZE 104 +#endif #endif #define SERIAL_NO_DATA 0xff - void serial_init(); // Writes one byte to the TX serial buffer. Called by main program. diff --git a/grbl/settings.c b/grbl/settings.c index a9c830e..f1b0a64 100644 --- a/grbl/settings.c +++ b/grbl/settings.c @@ -23,7 +23,7 @@ settings_t settings; -const __flash settings_t defaults = {\ +const __flash settings_t defaults = { .pulse_microseconds = DEFAULT_STEP_PULSE_MICROSECONDS, .stepper_idle_lock_time = DEFAULT_STEPPER_IDLE_LOCK_TIME, .step_invert_mask = DEFAULT_STEPPING_INVERT_MASK, @@ -38,14 +38,10 @@ const __flash settings_t defaults = {\ .homing_seek_rate = DEFAULT_HOMING_SEEK_RATE, .homing_debounce_delay = DEFAULT_HOMING_DEBOUNCE_DELAY, .homing_pulloff = DEFAULT_HOMING_PULLOFF, - .flags = (DEFAULT_REPORT_INCHES << BIT_REPORT_INCHES) | \ - (DEFAULT_LASER_MODE << BIT_LASER_MODE) | \ - (DEFAULT_INVERT_ST_ENABLE << BIT_INVERT_ST_ENABLE) | \ - (DEFAULT_HARD_LIMIT_ENABLE << BIT_HARD_LIMIT_ENABLE) | \ - (DEFAULT_HOMING_ENABLE << BIT_HOMING_ENABLE) | \ - (DEFAULT_SOFT_LIMIT_ENABLE << BIT_SOFT_LIMIT_ENABLE) | \ - (DEFAULT_INVERT_LIMIT_PINS << BIT_INVERT_LIMIT_PINS) | \ - (DEFAULT_INVERT_PROBE_PIN << BIT_INVERT_PROBE_PIN), + .flags = (DEFAULT_REPORT_INCHES << BIT_REPORT_INCHES) | (DEFAULT_LASER_MODE << BIT_LASER_MODE) | + (DEFAULT_INVERT_ST_ENABLE << BIT_INVERT_ST_ENABLE) | (DEFAULT_HARD_LIMIT_ENABLE << BIT_HARD_LIMIT_ENABLE) | + (DEFAULT_HOMING_ENABLE << BIT_HOMING_ENABLE) | (DEFAULT_SOFT_LIMIT_ENABLE << BIT_SOFT_LIMIT_ENABLE) | + (DEFAULT_INVERT_LIMIT_PINS << BIT_INVERT_LIMIT_PINS) | (DEFAULT_INVERT_PROBE_PIN << BIT_INVERT_PROBE_PIN), .steps_per_mm[X_AXIS] = DEFAULT_X_STEPS_PER_MM, .steps_per_mm[Y_AXIS] = DEFAULT_Y_STEPS_PER_MM, .steps_per_mm[Z_AXIS] = DEFAULT_Z_STEPS_PER_MM, @@ -59,282 +55,313 @@ const __flash settings_t defaults = {\ .max_travel[Y_AXIS] = (-DEFAULT_Y_MAX_TRAVEL), .max_travel[Z_AXIS] = (-DEFAULT_Z_MAX_TRAVEL)}; - // Method to store startup lines into EEPROM -void settings_store_startup_line(uint8_t n, char *line) -{ - #ifdef FORCE_BUFFER_SYNC_DURING_EEPROM_WRITE - protocol_buffer_synchronize(); // A startup line may contain a motion and be executing. - #endif - uint32_t addr = n*(LINE_BUFFER_SIZE+1)+EEPROM_ADDR_STARTUP_BLOCK; - memcpy_to_eeprom_with_checksum(addr,(char*)line, LINE_BUFFER_SIZE); +void settings_store_startup_line(uint8_t n, char *line) { +#ifdef FORCE_BUFFER_SYNC_DURING_EEPROM_WRITE + protocol_buffer_synchronize(); // A startup line may contain a motion and be executing. +#endif + uint32_t addr = n * (LINE_BUFFER_SIZE + 1) + EEPROM_ADDR_STARTUP_BLOCK; + memcpy_to_eeprom_with_checksum(addr, (char *)line, LINE_BUFFER_SIZE); } - // Method to store build info into EEPROM // NOTE: This function can only be called in IDLE state. -void settings_store_build_info(char *line) -{ - // Build info can only be stored when state is IDLE. - memcpy_to_eeprom_with_checksum(EEPROM_ADDR_BUILD_INFO,(char*)line, LINE_BUFFER_SIZE); +void settings_store_build_info(char *line) { + // Build info can only be stored when state is IDLE. + memcpy_to_eeprom_with_checksum(EEPROM_ADDR_BUILD_INFO, (char *)line, LINE_BUFFER_SIZE); } - // Method to store coord data parameters into EEPROM -void settings_write_coord_data(uint8_t coord_select, float *coord_data) -{ - #ifdef FORCE_BUFFER_SYNC_DURING_EEPROM_WRITE +void settings_write_coord_data(uint8_t coord_select, float *coord_data) { +#ifdef FORCE_BUFFER_SYNC_DURING_EEPROM_WRITE protocol_buffer_synchronize(); - #endif - uint32_t addr = coord_select*(sizeof(float)*N_AXIS+1) + EEPROM_ADDR_PARAMETERS; - memcpy_to_eeprom_with_checksum(addr,(char*)coord_data, sizeof(float)*N_AXIS); +#endif + uint32_t addr = coord_select * (sizeof(float) * N_AXIS + 1) + EEPROM_ADDR_PARAMETERS; + memcpy_to_eeprom_with_checksum(addr, (char *)coord_data, sizeof(float) * N_AXIS); } - // Method to store Grbl global settings struct and version number into EEPROM // NOTE: This function can only be called in IDLE state. -void write_global_settings() -{ - eeprom_put_char(0, SETTINGS_VERSION); - memcpy_to_eeprom_with_checksum(EEPROM_ADDR_GLOBAL, (char*)&settings, sizeof(settings_t)); +void write_global_settings() { + eeprom_put_char(0, SETTINGS_VERSION); + memcpy_to_eeprom_with_checksum(EEPROM_ADDR_GLOBAL, (char *)&settings, sizeof(settings_t)); } - // Method to restore EEPROM-saved Grbl global settings back to defaults. void settings_restore(uint8_t restore_flag) { - if (restore_flag & SETTINGS_RESTORE_DEFAULTS) { - settings = defaults; - write_global_settings(); - } + if (restore_flag & SETTINGS_RESTORE_DEFAULTS) { + settings = defaults; + write_global_settings(); + } - if (restore_flag & SETTINGS_RESTORE_PARAMETERS) { - uint8_t idx; - float coord_data[N_AXIS]; - memset(&coord_data, 0, sizeof(coord_data)); - for (idx=0; idx <= SETTING_INDEX_NCOORD; idx++) { settings_write_coord_data(idx, coord_data); } - } + if (restore_flag & SETTINGS_RESTORE_PARAMETERS) { + uint8_t idx; + float coord_data[N_AXIS]; + memset(&coord_data, 0, sizeof(coord_data)); + for (idx = 0; idx <= SETTING_INDEX_NCOORD; idx++) { + settings_write_coord_data(idx, coord_data); + } + } - if (restore_flag & SETTINGS_RESTORE_STARTUP_LINES) { - #if N_STARTUP_LINE > 0 - eeprom_put_char(EEPROM_ADDR_STARTUP_BLOCK, 0); - eeprom_put_char(EEPROM_ADDR_STARTUP_BLOCK+1, 0); // Checksum - #endif - #if N_STARTUP_LINE > 1 - eeprom_put_char(EEPROM_ADDR_STARTUP_BLOCK+(LINE_BUFFER_SIZE+1), 0); - eeprom_put_char(EEPROM_ADDR_STARTUP_BLOCK+(LINE_BUFFER_SIZE+2), 0); // Checksum - #endif - } + if (restore_flag & SETTINGS_RESTORE_STARTUP_LINES) { +#if N_STARTUP_LINE > 0 + eeprom_put_char(EEPROM_ADDR_STARTUP_BLOCK, 0); + eeprom_put_char(EEPROM_ADDR_STARTUP_BLOCK + 1, 0); // Checksum +#endif +#if N_STARTUP_LINE > 1 + eeprom_put_char(EEPROM_ADDR_STARTUP_BLOCK + (LINE_BUFFER_SIZE + 1), 0); + eeprom_put_char(EEPROM_ADDR_STARTUP_BLOCK + (LINE_BUFFER_SIZE + 2), 0); // Checksum +#endif + } - if (restore_flag & SETTINGS_RESTORE_BUILD_INFO) { - eeprom_put_char(EEPROM_ADDR_BUILD_INFO , 0); - eeprom_put_char(EEPROM_ADDR_BUILD_INFO+1 , 0); // Checksum - } + if (restore_flag & SETTINGS_RESTORE_BUILD_INFO) { + eeprom_put_char(EEPROM_ADDR_BUILD_INFO, 0); + eeprom_put_char(EEPROM_ADDR_BUILD_INFO + 1, 0); // Checksum + } } - // Reads startup line from EEPROM. Updated pointed line string data. -uint8_t settings_read_startup_line(uint8_t n, char *line) -{ - uint32_t addr = n*(LINE_BUFFER_SIZE+1)+EEPROM_ADDR_STARTUP_BLOCK; - if (!(memcpy_from_eeprom_with_checksum((char*)line, addr, LINE_BUFFER_SIZE))) { - // Reset line with default value - line[0] = 0; // Empty line - settings_store_startup_line(n, line); - return(false); - } - return(true); +uint8_t settings_read_startup_line(uint8_t n, char *line) { + uint32_t addr = n * (LINE_BUFFER_SIZE + 1) + EEPROM_ADDR_STARTUP_BLOCK; + if (!(memcpy_from_eeprom_with_checksum((char *)line, addr, LINE_BUFFER_SIZE))) { + // Reset line with default value + line[0] = 0; // Empty line + settings_store_startup_line(n, line); + return (false); + } + return (true); } - // Reads startup line from EEPROM. Updated pointed line string data. -uint8_t settings_read_build_info(char *line) -{ - if (!(memcpy_from_eeprom_with_checksum((char*)line, EEPROM_ADDR_BUILD_INFO, LINE_BUFFER_SIZE))) { - // Reset line with default value - line[0] = 0; // Empty line - settings_store_build_info(line); - return(false); - } - return(true); +uint8_t settings_read_build_info(char *line) { + if (!(memcpy_from_eeprom_with_checksum((char *)line, EEPROM_ADDR_BUILD_INFO, LINE_BUFFER_SIZE))) { + // Reset line with default value + line[0] = 0; // Empty line + settings_store_build_info(line); + return (false); + } + return (true); } - // Read selected coordinate data from EEPROM. Updates pointed coord_data value. -uint8_t settings_read_coord_data(uint8_t coord_select, float *coord_data) -{ - uint32_t addr = coord_select*(sizeof(float)*N_AXIS+1) + EEPROM_ADDR_PARAMETERS; - if (!(memcpy_from_eeprom_with_checksum((char*)coord_data, addr, sizeof(float)*N_AXIS))) { - // Reset with default zero vector - clear_vector_float(coord_data); - settings_write_coord_data(coord_select,coord_data); - return(false); - } - return(true); +uint8_t settings_read_coord_data(uint8_t coord_select, float *coord_data) { + uint32_t addr = coord_select * (sizeof(float) * N_AXIS + 1) + EEPROM_ADDR_PARAMETERS; + if (!(memcpy_from_eeprom_with_checksum((char *)coord_data, addr, sizeof(float) * N_AXIS))) { + // Reset with default zero vector + clear_vector_float(coord_data); + settings_write_coord_data(coord_select, coord_data); + return (false); + } + return (true); } - // Reads Grbl global settings struct from EEPROM. uint8_t read_global_settings() { - // Check version-byte of eeprom - uint8_t version = eeprom_get_char(0); - if (version == SETTINGS_VERSION) { - // Read settings-record and check checksum - if (!(memcpy_from_eeprom_with_checksum((char*)&settings, EEPROM_ADDR_GLOBAL, sizeof(settings_t)))) { - return(false); + // Check version-byte of eeprom + uint8_t version = eeprom_get_char(0); + if (version == SETTINGS_VERSION) { + // Read settings-record and check checksum + if (!(memcpy_from_eeprom_with_checksum((char *)&settings, EEPROM_ADDR_GLOBAL, sizeof(settings_t)))) { + return (false); + } + } else { + return (false); } - } else { - return(false); - } - return(true); + return (true); } - // A helper method to set settings from command line uint8_t settings_store_global_setting(uint8_t parameter, float value) { - if (value < 0.0) { return(STATUS_NEGATIVE_VALUE); } - if (parameter >= AXIS_SETTINGS_START_VAL) { - // Store axis configuration. Axis numbering sequence set by AXIS_SETTING defines. - // NOTE: Ensure the setting index corresponds to the report.c settings printout. - parameter -= AXIS_SETTINGS_START_VAL; - uint8_t set_idx = 0; - while (set_idx < AXIS_N_SETTINGS) { - if (parameter < N_AXIS) { - // Valid axis setting found. - switch (set_idx) { - case 0: - #ifdef MAX_STEP_RATE_HZ - if (value*settings.max_rate[parameter] > (MAX_STEP_RATE_HZ*60.0)) { return(STATUS_MAX_STEP_RATE_EXCEEDED); } - #endif - settings.steps_per_mm[parameter] = value; - break; - case 1: - #ifdef MAX_STEP_RATE_HZ - if (value*settings.steps_per_mm[parameter] > (MAX_STEP_RATE_HZ*60.0)) { return(STATUS_MAX_STEP_RATE_EXCEEDED); } - #endif - settings.max_rate[parameter] = value; - break; - case 2: settings.acceleration[parameter] = value*60*60; break; // Convert to mm/min^2 for grbl internal use. - case 3: settings.max_travel[parameter] = -value; break; // Store as negative for grbl internal use. - } - break; // Exit while-loop after setting has been configured and proceed to the EEPROM write call. - } else { - set_idx++; - // If axis index greater than N_AXIS or setting index greater than number of axis settings, error out. - if ((parameter < AXIS_SETTINGS_INCREMENT) || (set_idx == AXIS_N_SETTINGS)) { return(STATUS_INVALID_STATEMENT); } - parameter -= AXIS_SETTINGS_INCREMENT; - } + if (value < 0.0) { + return (STATUS_NEGATIVE_VALUE); } - } else { - // Store non-axis Grbl settings - uint8_t int_value = trunc(value); - switch(parameter) { - case 0: - if (int_value < 3) { return(STATUS_SETTING_STEP_PULSE_MIN); } - settings.pulse_microseconds = int_value; break; - case 1: settings.stepper_idle_lock_time = int_value; break; - case 2: - settings.step_invert_mask = int_value; - st_generate_step_dir_invert_masks(); // Regenerate step and direction port invert masks. - break; - case 3: - settings.dir_invert_mask = int_value; - st_generate_step_dir_invert_masks(); // Regenerate step and direction port invert masks. - break; - case 4: // Reset to ensure change. Immediate re-init may cause problems. - if (int_value) { settings.flags |= BITFLAG_INVERT_ST_ENABLE; } - else { settings.flags &= ~BITFLAG_INVERT_ST_ENABLE; } - break; - case 5: // Reset to ensure change. Immediate re-init may cause problems. - if (int_value) { settings.flags |= BITFLAG_INVERT_LIMIT_PINS; } - else { settings.flags &= ~BITFLAG_INVERT_LIMIT_PINS; } - break; - case 6: // Reset to ensure change. Immediate re-init may cause problems. - if (int_value) { settings.flags |= BITFLAG_INVERT_PROBE_PIN; } - else { settings.flags &= ~BITFLAG_INVERT_PROBE_PIN; } - probe_configure_invert_mask(false); - break; - case 10: settings.status_report_mask = int_value; break; - case 11: settings.junction_deviation = value; break; - case 12: settings.arc_tolerance = value; break; - case 13: - if (int_value) { settings.flags |= BITFLAG_REPORT_INCHES; } - else { settings.flags &= ~BITFLAG_REPORT_INCHES; } - system_flag_wco_change(); // Make sure WCO is immediately updated. - break; - case 20: - if (int_value) { - if (bit_isfalse(settings.flags, BITFLAG_HOMING_ENABLE)) { return(STATUS_SOFT_LIMIT_ERROR); } - settings.flags |= BITFLAG_SOFT_LIMIT_ENABLE; - } else { settings.flags &= ~BITFLAG_SOFT_LIMIT_ENABLE; } - break; - case 21: - if (int_value) { settings.flags |= BITFLAG_HARD_LIMIT_ENABLE; } - else { settings.flags &= ~BITFLAG_HARD_LIMIT_ENABLE; } - limits_init(); // Re-init to immediately change. NOTE: Nice to have but could be problematic later. - break; - case 22: - if (int_value) { settings.flags |= BITFLAG_HOMING_ENABLE; } - else { - settings.flags &= ~BITFLAG_HOMING_ENABLE; - settings.flags &= ~BITFLAG_SOFT_LIMIT_ENABLE; // Force disable soft-limits. + if (parameter >= AXIS_SETTINGS_START_VAL) { + // Store axis configuration. Axis numbering sequence set by AXIS_SETTING defines. + // NOTE: Ensure the setting index corresponds to the report.c settings printout. + parameter -= AXIS_SETTINGS_START_VAL; + uint8_t set_idx = 0; + while (set_idx < AXIS_N_SETTINGS) { + if (parameter < N_AXIS) { + // Valid axis setting found. + switch (set_idx) { + case 0: +#ifdef MAX_STEP_RATE_HZ + if (value * settings.max_rate[parameter] > (MAX_STEP_RATE_HZ * 60.0)) { + return (STATUS_MAX_STEP_RATE_EXCEEDED); + } +#endif + settings.steps_per_mm[parameter] = value; + break; + case 1: +#ifdef MAX_STEP_RATE_HZ + if (value * settings.steps_per_mm[parameter] > (MAX_STEP_RATE_HZ * 60.0)) { + return (STATUS_MAX_STEP_RATE_EXCEEDED); + } +#endif + settings.max_rate[parameter] = value; + break; + case 2: + settings.acceleration[parameter] = value * 60 * 60; + break; // Convert to mm/min^2 for grbl internal use. + case 3: settings.max_travel[parameter] = -value; break; // Store as negative for grbl internal use. + } + break; // Exit while-loop after setting has been configured and proceed to the EEPROM write call. + } else { + set_idx++; + // If axis index greater than N_AXIS or setting index greater than number of axis settings, error out. + if ((parameter < AXIS_SETTINGS_INCREMENT) || (set_idx == AXIS_N_SETTINGS)) { + return (STATUS_INVALID_STATEMENT); + } + parameter -= AXIS_SETTINGS_INCREMENT; + } + } + } else { + // Store non-axis Grbl settings + uint8_t int_value = trunc(value); + switch (parameter) { + case 0: + if (int_value < 3) { + return (STATUS_SETTING_STEP_PULSE_MIN); + } + settings.pulse_microseconds = int_value; + break; + case 1: settings.stepper_idle_lock_time = int_value; break; + case 2: + settings.step_invert_mask = int_value; + st_generate_step_dir_invert_masks(); // Regenerate step and direction port invert masks. + break; + case 3: + settings.dir_invert_mask = int_value; + st_generate_step_dir_invert_masks(); // Regenerate step and direction port invert masks. + break; + case 4: // Reset to ensure change. Immediate re-init may cause problems. + if (int_value) { + settings.flags |= BITFLAG_INVERT_ST_ENABLE; + } else { + settings.flags &= ~BITFLAG_INVERT_ST_ENABLE; + } + break; + case 5: // Reset to ensure change. Immediate re-init may cause problems. + if (int_value) { + settings.flags |= BITFLAG_INVERT_LIMIT_PINS; + } else { + settings.flags &= ~BITFLAG_INVERT_LIMIT_PINS; + } + break; + case 6: // Reset to ensure change. Immediate re-init may cause problems. + if (int_value) { + settings.flags |= BITFLAG_INVERT_PROBE_PIN; + } else { + settings.flags &= ~BITFLAG_INVERT_PROBE_PIN; + } + probe_configure_invert_mask(false); + break; + case 10: settings.status_report_mask = int_value; break; + case 11: settings.junction_deviation = value; break; + case 12: settings.arc_tolerance = value; break; + case 13: + if (int_value) { + settings.flags |= BITFLAG_REPORT_INCHES; + } else { + settings.flags &= ~BITFLAG_REPORT_INCHES; + } + system_flag_wco_change(); // Make sure WCO is immediately updated. + break; + case 20: + if (int_value) { + if (bit_isfalse(settings.flags, BITFLAG_HOMING_ENABLE)) { + return (STATUS_SOFT_LIMIT_ERROR); + } + settings.flags |= BITFLAG_SOFT_LIMIT_ENABLE; + } else { + settings.flags &= ~BITFLAG_SOFT_LIMIT_ENABLE; + } + break; + case 21: + if (int_value) { + settings.flags |= BITFLAG_HARD_LIMIT_ENABLE; + } else { + settings.flags &= ~BITFLAG_HARD_LIMIT_ENABLE; + } + limits_init(); // Re-init to immediately change. NOTE: Nice to have but could be problematic later. + break; + case 22: + if (int_value) { + settings.flags |= BITFLAG_HOMING_ENABLE; + } else { + settings.flags &= ~BITFLAG_HOMING_ENABLE; + settings.flags &= ~BITFLAG_SOFT_LIMIT_ENABLE; // Force disable soft-limits. + } + break; + case 23: settings.homing_dir_mask = int_value; break; + case 24: settings.homing_feed_rate = value; break; + case 25: settings.homing_seek_rate = value; break; + case 26: settings.homing_debounce_delay = int_value; break; + case 27: settings.homing_pulloff = value; break; + case 30: + settings.rpm_max = value; + spindle_init(); + break; // Re-initialize spindle rpm calibration + case 31: + settings.rpm_min = value; + spindle_init(); + break; // Re-initialize spindle rpm calibration + case 32: +#ifdef VARIABLE_SPINDLE + if (int_value) { + settings.flags |= BITFLAG_LASER_MODE; + } else { + settings.flags &= ~BITFLAG_LASER_MODE; + } +#else + return (STATUS_SETTING_DISABLED_LASER); +#endif + break; + default: return (STATUS_INVALID_STATEMENT); } - break; - case 23: settings.homing_dir_mask = int_value; break; - case 24: settings.homing_feed_rate = value; break; - case 25: settings.homing_seek_rate = value; break; - case 26: settings.homing_debounce_delay = int_value; break; - case 27: settings.homing_pulloff = value; break; - case 30: settings.rpm_max = value; spindle_init(); break; // Re-initialize spindle rpm calibration - case 31: settings.rpm_min = value; spindle_init(); break; // Re-initialize spindle rpm calibration - case 32: - #ifdef VARIABLE_SPINDLE - if (int_value) { settings.flags |= BITFLAG_LASER_MODE; } - else { settings.flags &= ~BITFLAG_LASER_MODE; } - #else - return(STATUS_SETTING_DISABLED_LASER); - #endif - break; - default: - return(STATUS_INVALID_STATEMENT); } - } - write_global_settings(); - return(STATUS_OK); + write_global_settings(); + return (STATUS_OK); } - // Initialize the config subsystem void settings_init() { - if(!read_global_settings()) { - report_status_message(STATUS_SETTING_READ_FAIL); - settings_restore(SETTINGS_RESTORE_ALL); // Force restore all EEPROM data. - report_grbl_settings(); - } + if (!read_global_settings()) { + report_status_message(STATUS_SETTING_READ_FAIL); + settings_restore(SETTINGS_RESTORE_ALL); // Force restore all EEPROM data. + report_grbl_settings(); + } } - // Returns step pin mask according to Grbl internal axis indexing. -uint8_t get_step_pin_mask(uint8_t axis_idx) -{ - if ( axis_idx == X_AXIS ) { return((1<= 100 for axis settings. Up to 255. -#define AXIS_SETTINGS_INCREMENT 10 // Must be greater than the number of axis settings +#define AXIS_N_SETTINGS 4 +#define AXIS_SETTINGS_START_VAL 100 // NOTE: Reserving settings values >= 100 for axis settings. Up to 255. +#define AXIS_SETTINGS_INCREMENT 10 // Must be greater than the number of axis settings // Global persistent settings (Stored from byte EEPROM_ADDR_GLOBAL onwards) typedef struct { - // Axis settings - float steps_per_mm[N_AXIS]; - float max_rate[N_AXIS]; - float acceleration[N_AXIS]; - float max_travel[N_AXIS]; + // Axis settings + float steps_per_mm[N_AXIS]; + float max_rate[N_AXIS]; + float acceleration[N_AXIS]; + float max_travel[N_AXIS]; - // Remaining Grbl settings - uint8_t pulse_microseconds; - uint8_t step_invert_mask; - uint8_t dir_invert_mask; - uint8_t stepper_idle_lock_time; // If max value 255, steppers do not disable. - uint8_t status_report_mask; // Mask to indicate desired report data. - float junction_deviation; - float arc_tolerance; + // Remaining Grbl settings + uint8_t pulse_microseconds; + uint8_t step_invert_mask; + uint8_t dir_invert_mask; + uint8_t stepper_idle_lock_time; // If max value 255, steppers do not disable. + uint8_t status_report_mask; // Mask to indicate desired report data. + float junction_deviation; + float arc_tolerance; - float rpm_max; - float rpm_min; + float rpm_max; + float rpm_min; - uint8_t flags; // Contains default boolean settings + uint8_t flags; // Contains default boolean settings - uint8_t homing_dir_mask; - float homing_feed_rate; - float homing_seek_rate; - uint16_t homing_debounce_delay; - float homing_pulloff; + uint8_t homing_dir_mask; + float homing_feed_rate; + float homing_seek_rate; + uint16_t homing_debounce_delay; + float homing_pulloff; } settings_t; + extern settings_t settings; // Initialize the configuration subsystem (load settings from EEPROM) @@ -149,5 +149,4 @@ uint8_t get_direction_pin_mask(uint8_t i); // Returns the limit pin mask according to Grbl's internal axis numbering uint8_t get_limit_pin_mask(uint8_t i); - #endif diff --git a/grbl/spindle_control.c b/grbl/spindle_control.c index 550b752..17772f6 100644 --- a/grbl/spindle_control.c +++ b/grbl/spindle_control.c @@ -21,270 +21,273 @@ #include "grbl.h" - #ifdef VARIABLE_SPINDLE - static float pwm_gradient; // Precalulated value to speed up rpm to PWM conversions. +static float pwm_gradient; // Precalulated value to speed up rpm to PWM conversions. #endif - -void spindle_init() -{ - #ifdef VARIABLE_SPINDLE +void spindle_init() { +#ifdef VARIABLE_SPINDLE // Configure variable spindle PWM and enable pin, if requried. On the Uno, PWM and enable are // combined unless configured otherwise. - SPINDLE_PWM_DDR |= (1<= settings.rpm_max) || (rpm >= RPM_MAX)) { +// Called by spindle_set_state() and step segment generator. Keep routine small and efficient. +uint8_t spindle_compute_pwm_value(float rpm) // 328p PWM register is 8-bit. +{ + uint8_t pwm_value; + rpm *= (0.010 * sys.spindle_speed_ovr); // Scale by spindle speed override value. + // Calculate PWM register value based on rpm max/min settings and programmed rpm. + if ((settings.rpm_min >= settings.rpm_max) || (rpm >= RPM_MAX)) { rpm = RPM_MAX; pwm_value = SPINDLE_PWM_MAX_VALUE; - } else if (rpm <= RPM_MIN) { + } else if (rpm <= RPM_MIN) { if (rpm == 0.0) { // S0 disables spindle - pwm_value = SPINDLE_PWM_OFF_VALUE; + pwm_value = SPINDLE_PWM_OFF_VALUE; } else { - rpm = RPM_MIN; - pwm_value = SPINDLE_PWM_MIN_VALUE; + rpm = RPM_MIN; + pwm_value = SPINDLE_PWM_MIN_VALUE; } - } else { - // Compute intermediate PWM value with linear spindle speed model via piecewise linear fit model. - #if (N_PIECES > 3) - if (rpm > RPM_POINT34) { - pwm_value = floor(RPM_LINE_A4*rpm - RPM_LINE_B4); - } else - #endif - #if (N_PIECES > 2) - if (rpm > RPM_POINT23) { - pwm_value = floor(RPM_LINE_A3*rpm - RPM_LINE_B3); - } else - #endif - #if (N_PIECES > 1) - if (rpm > RPM_POINT12) { - pwm_value = floor(RPM_LINE_A2*rpm - RPM_LINE_B2); - } else - #endif + } else { +// Compute intermediate PWM value with linear spindle speed model via piecewise linear fit model. +#if (N_PIECES > 3) + if (rpm > RPM_POINT34) { + pwm_value = floor(RPM_LINE_A4 * rpm - RPM_LINE_B4); + } else +#endif +#if (N_PIECES > 2) + if (rpm > RPM_POINT23) { + pwm_value = floor(RPM_LINE_A3 * rpm - RPM_LINE_B3); + } else +#endif +#if (N_PIECES > 1) + if (rpm > RPM_POINT12) { + pwm_value = floor(RPM_LINE_A2 * rpm - RPM_LINE_B2); + } else +#endif { - pwm_value = floor(RPM_LINE_A1*rpm - RPM_LINE_B1); + pwm_value = floor(RPM_LINE_A1 * rpm - RPM_LINE_B1); } - } - sys.spindle_speed = rpm; - return(pwm_value); } - - #else - - // Called by spindle_set_state() and step segment generator. Keep routine small and efficient. - uint8_t spindle_compute_pwm_value(float rpm) // 328p PWM register is 8-bit. - { - uint8_t pwm_value; - rpm *= (0.010*sys.spindle_speed_ovr); // Scale by spindle speed override value. - // Calculate PWM register value based on rpm max/min settings and programmed rpm. - if ((settings.rpm_min >= settings.rpm_max) || (rpm >= settings.rpm_max)) { + sys.spindle_speed = rpm; + return (pwm_value); +} + +#else + +// Called by spindle_set_state() and step segment generator. Keep routine small and efficient. +uint8_t spindle_compute_pwm_value(float rpm) // 328p PWM register is 8-bit. +{ + uint8_t pwm_value; + rpm *= (0.010 * sys.spindle_speed_ovr); // Scale by spindle speed override value. + // Calculate PWM register value based on rpm max/min settings and programmed rpm. + if ((settings.rpm_min >= settings.rpm_max) || (rpm >= settings.rpm_max)) { // No PWM range possible. Set simple on/off spindle control pin state. sys.spindle_speed = settings.rpm_max; pwm_value = SPINDLE_PWM_MAX_VALUE; - } else if (rpm <= settings.rpm_min) { + } else if (rpm <= settings.rpm_min) { if (rpm == 0.0) { // S0 disables spindle - sys.spindle_speed = 0.0; - pwm_value = SPINDLE_PWM_OFF_VALUE; + sys.spindle_speed = 0.0; + pwm_value = SPINDLE_PWM_OFF_VALUE; } else { // Set minimum PWM output - sys.spindle_speed = settings.rpm_min; - pwm_value = SPINDLE_PWM_MIN_VALUE; + sys.spindle_speed = settings.rpm_min; + pwm_value = SPINDLE_PWM_MIN_VALUE; } - } else { + } else { // Compute intermediate PWM value with linear spindle speed model. // NOTE: A nonlinear model could be installed here, if required, but keep it VERY light-weight. sys.spindle_speed = rpm; - pwm_value = floor((rpm-settings.rpm_min)*pwm_gradient) + SPINDLE_PWM_MIN_VALUE; - } - return(pwm_value); + pwm_value = floor((rpm - settings.rpm_min) * pwm_gradient) + SPINDLE_PWM_MIN_VALUE; } - - #endif -#endif + return (pwm_value); +} +#endif +#endif // Immediately sets spindle running state with direction and spindle rpm via PWM, if enabled. // Called by g-code parser spindle_sync(), parking retract and restore, g-code program end, // sleep, and spindle stop override. #ifdef VARIABLE_SPINDLE - void spindle_set_state(uint8_t state, float rpm) +void spindle_set_state(uint8_t state, float rpm) #else - void _spindle_set_state(uint8_t state) +void _spindle_set_state(uint8_t state) #endif { - if (sys.abort) { return; } // Block during abort. + if (sys.abort) { + return; + } // Block during abort. - if (state == SPINDLE_DISABLE) { // Halt or set spindle direction and rpm. - - #ifdef VARIABLE_SPINDLE - sys.spindle_speed = 0.0; - #endif - spindle_stop(); - - } else { - - #if !defined(USE_SPINDLE_DIR_AS_ENABLE_PIN) && !defined(ENABLE_DUAL_AXIS) - if (state == SPINDLE_ENABLE_CW) { - SPINDLE_DIRECTION_PORT &= ~(1<> 3); + st.step_pulse_time = -(((settings.pulse_microseconds + STEP_PULSE_DELAY - 2) * TICKS_PER_MICROSECOND) >> 3); // Set delay between direction pin write and step command. - OCR0A = -(((settings.pulse_microseconds)*TICKS_PER_MICROSECOND) >> 3); - #else // Normal operation + OCR0A = -(((settings.pulse_microseconds) * TICKS_PER_MICROSECOND) >> 3); +#else // Normal operation // Set step pulse time. Ad hoc computation from oscilloscope. Uses two's complement. - st.step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3); - #endif + st.step_pulse_time = -(((settings.pulse_microseconds - 2) * TICKS_PER_MICROSECOND) >> 3); +#endif - // Enable Stepper Driver Interrupt - TIMSK1 |= (1<prescaler<prescaler << CS10); +#endif - // Initialize step segment timing per step and load number of steps to execute. - OCR1A = st.exec_segment->cycles_per_tick; - st.step_count = st.exec_segment->n_step; // NOTE: Can sometimes be zero when moving slow. - // If the new segment starts a new planner block, initialize stepper variables and counters. - // NOTE: When the segment data index changes, this indicates a new planner block. - if ( st.exec_block_index != st.exec_segment->st_block_index ) { - st.exec_block_index = st.exec_segment->st_block_index; - st.exec_block = &st_block_buffer[st.exec_block_index]; + // Initialize step segment timing per step and load number of steps to execute. + OCR1A = st.exec_segment->cycles_per_tick; + st.step_count = st.exec_segment->n_step; // NOTE: Can sometimes be zero when moving slow. + // If the new segment starts a new planner block, initialize stepper variables and counters. + // NOTE: When the segment data index changes, this indicates a new planner block. + if (st.exec_block_index != st.exec_segment->st_block_index) { + st.exec_block_index = st.exec_segment->st_block_index; + st.exec_block = &st_block_buffer[st.exec_block_index]; - // Initialize Bresenham line and distance counters - st.counter_x = st.counter_y = st.counter_z = (st.exec_block->step_event_count >> 1); - } - st.dir_outbits = st.exec_block->direction_bits ^ dir_port_invert_mask; - #ifdef ENABLE_DUAL_AXIS - st.dir_outbits_dual = st.exec_block->direction_bits_dual ^ dir_port_invert_mask_dual; - #endif + // Initialize Bresenham line and distance counters + st.counter_x = st.counter_y = st.counter_z = (st.exec_block->step_event_count >> 1); + } + st.dir_outbits = st.exec_block->direction_bits ^ dir_port_invert_mask; +#ifdef ENABLE_DUAL_AXIS + st.dir_outbits_dual = st.exec_block->direction_bits_dual ^ dir_port_invert_mask_dual; +#endif - #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING - // With AMASS enabled, adjust Bresenham axis increment counters according to AMASS level. - st.steps[X_AXIS] = st.exec_block->steps[X_AXIS] >> st.exec_segment->amass_level; - st.steps[Y_AXIS] = st.exec_block->steps[Y_AXIS] >> st.exec_segment->amass_level; - st.steps[Z_AXIS] = st.exec_block->steps[Z_AXIS] >> st.exec_segment->amass_level; - #endif +#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING + // With AMASS enabled, adjust Bresenham axis increment counters according to AMASS level. + st.steps[X_AXIS] = st.exec_block->steps[X_AXIS] >> st.exec_segment->amass_level; + st.steps[Y_AXIS] = st.exec_block->steps[Y_AXIS] >> st.exec_segment->amass_level; + st.steps[Z_AXIS] = st.exec_block->steps[Z_AXIS] >> st.exec_segment->amass_level; +#endif - #ifdef VARIABLE_SPINDLE - // Set real-time spindle output as segment is loaded, just prior to the first step. - spindle_set_speed(st.exec_segment->spindle_pwm); - #endif +#ifdef VARIABLE_SPINDLE + // Set real-time spindle output as segment is loaded, just prior to the first step. + spindle_set_speed(st.exec_segment->spindle_pwm); +#endif - } else { - // Segment buffer empty. Shutdown. - st_go_idle(); - #ifdef VARIABLE_SPINDLE - // Ensure pwm is set properly upon completion of rate-controlled motion. - if (st.exec_block->is_pwm_rate_adjusted) { spindle_set_speed(SPINDLE_PWM_OFF_VALUE); } - #endif - system_set_exec_state_flag(EXEC_CYCLE_STOP); // Flag main program for cycle end - return; // Nothing to do but exit. + } else { + // Segment buffer empty. Shutdown. + st_go_idle(); +#ifdef VARIABLE_SPINDLE + // Ensure pwm is set properly upon completion of rate-controlled motion. + if (st.exec_block->is_pwm_rate_adjusted) { + spindle_set_speed(SPINDLE_PWM_OFF_VALUE); + } +#endif + system_set_exec_state_flag(EXEC_CYCLE_STOP); // Flag main program for cycle end + return; // Nothing to do but exit. + } } - } + // Check probing state. + if (sys_probe_state == PROBE_ACTIVE) { + probe_state_monitor(); + } - // Check probing state. - if (sys_probe_state == PROBE_ACTIVE) { probe_state_monitor(); } - - // Reset step out bits. - st.step_outbits = 0; - #ifdef ENABLE_DUAL_AXIS + // Reset step out bits. + st.step_outbits = 0; +#ifdef ENABLE_DUAL_AXIS st.step_outbits_dual = 0; - #endif +#endif - // Execute step displacement profile by Bresenham line algorithm - #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING +// Execute step displacement profile by Bresenham line algorithm +#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING st.counter_x += st.steps[X_AXIS]; - #else +#else st.counter_x += st.exec_block->steps[X_AXIS]; - #endif - if (st.counter_x > st.exec_block->step_event_count) { - st.step_outbits |= (1<step_event_count; - if (st.exec_block->direction_bits & (1< st.exec_block->step_event_count) { + st.step_outbits |= (1 << X_STEP_BIT); +#if defined(ENABLE_DUAL_AXIS) && (DUAL_AXIS_SELECT == X_AXIS) + st.step_outbits_dual = (1 << DUAL_STEP_BIT); +#endif + st.counter_x -= st.exec_block->step_event_count; + if (st.exec_block->direction_bits & (1 << X_DIRECTION_BIT)) { + sys_position[X_AXIS]--; + } else { + sys_position[X_AXIS]++; + } + } +#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING st.counter_y += st.steps[Y_AXIS]; - #else +#else st.counter_y += st.exec_block->steps[Y_AXIS]; - #endif - if (st.counter_y > st.exec_block->step_event_count) { - st.step_outbits |= (1<step_event_count; - if (st.exec_block->direction_bits & (1< st.exec_block->step_event_count) { + st.step_outbits |= (1 << Y_STEP_BIT); +#if defined(ENABLE_DUAL_AXIS) && (DUAL_AXIS_SELECT == Y_AXIS) + st.step_outbits_dual = (1 << DUAL_STEP_BIT); +#endif + st.counter_y -= st.exec_block->step_event_count; + if (st.exec_block->direction_bits & (1 << Y_DIRECTION_BIT)) { + sys_position[Y_AXIS]--; + } else { + sys_position[Y_AXIS]++; + } + } +#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING st.counter_z += st.steps[Z_AXIS]; - #else +#else st.counter_z += st.exec_block->steps[Z_AXIS]; - #endif - if (st.counter_z > st.exec_block->step_event_count) { - st.step_outbits |= (1<step_event_count; - if (st.exec_block->direction_bits & (1< st.exec_block->step_event_count) { + st.step_outbits |= (1 << Z_STEP_BIT); + st.counter_z -= st.exec_block->step_event_count; + if (st.exec_block->direction_bits & (1 << Z_DIRECTION_BIT)) { + sys_position[Z_AXIS]--; + } else { + sys_position[Z_AXIS]++; + } + } - // During a homing cycle, lock out and prevent desired axes from moving. - if (sys.state == STATE_HOMING) { - st.step_outbits &= sys.homing_axis_lock; - #ifdef ENABLE_DUAL_AXIS - st.step_outbits_dual &= sys.homing_axis_lock_dual; - #endif - } + // During a homing cycle, lock out and prevent desired axes from moving. + if (sys.state == STATE_HOMING) { + st.step_outbits &= sys.homing_axis_lock; +#ifdef ENABLE_DUAL_AXIS + st.step_outbits_dual &= sys.homing_axis_lock_dual; +#endif + } - st.step_count--; // Decrement step events count - if (st.step_count == 0) { - // Segment is complete. Discard current segment and advance segment indexing. - st.exec_segment = NULL; - if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { segment_buffer_tail = 0; } - } + st.step_count--; // Decrement step events count + if (st.step_count == 0) { + // Segment is complete. Discard current segment and advance segment indexing. + st.exec_segment = NULL; + if (++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { + segment_buffer_tail = 0; + } + } - st.step_outbits ^= step_port_invert_mask; // Apply step port invert mask - #ifdef ENABLE_DUAL_AXIS + st.step_outbits ^= step_port_invert_mask; // Apply step port invert mask +#ifdef ENABLE_DUAL_AXIS st.step_outbits_dual ^= step_port_invert_mask_dual; - #endif - busy = false; +#endif + busy = false; } - /* The Stepper Port Reset Interrupt: Timer0 OVF interrupt handles the falling edge of the step pulse. This should always trigger before the next Timer1 COMPA interrupt and independently finish, if Timer1 is disabled after completing a move. @@ -486,172 +504,165 @@ ISR(TIMER1_COMPA_vect) // This interrupt is enabled by ISR_TIMER1_COMPAREA when it sets the motor port bits to execute // a step. This ISR resets the motor port after a short period (settings.pulse_microseconds) // completing one step cycle. -ISR(TIMER0_OVF_vect) -{ - // Reset stepping pins (leave the direction pins) - STEP_PORT = (STEP_PORT & ~STEP_MASK) | (step_port_invert_mask & STEP_MASK); - #ifdef ENABLE_DUAL_AXIS +ISR(TIMER0_OVF_vect) { + // Reset stepping pins (leave the direction pins) + STEP_PORT = (STEP_PORT & ~STEP_MASK) | (step_port_invert_mask & STEP_MASK); +#ifdef ENABLE_DUAL_AXIS STEP_PORT_DUAL = (STEP_PORT_DUAL & ~STEP_MASK_DUAL) | (step_port_invert_mask_dual & STEP_MASK_DUAL); - #endif - TCCR0B = 0; // Disable Timer0 to prevent re-entering this interrupt when it's not needed. +#endif + TCCR0B = 0; // Disable Timer0 to prevent re-entering this interrupt when it's not needed. } #ifdef STEP_PULSE_DELAY - // This interrupt is used only when STEP_PULSE_DELAY is enabled. Here, the step pulse is - // initiated after the STEP_PULSE_DELAY time period has elapsed. The ISR TIMER2_OVF interrupt - // will then trigger after the appropriate settings.pulse_microseconds, as in normal operation. - // The new timing between direction, step pulse, and step complete events are setup in the - // st_wake_up() routine. - ISR(TIMER0_COMPA_vect) - { +// This interrupt is used only when STEP_PULSE_DELAY is enabled. Here, the step pulse is +// initiated after the STEP_PULSE_DELAY time period has elapsed. The ISR TIMER2_OVF interrupt +// will then trigger after the appropriate settings.pulse_microseconds, as in normal operation. +// The new timing between direction, step pulse, and step complete events are setup in the +// st_wake_up() routine. +ISR(TIMER0_COMPA_vect) { STEP_PORT = st.step_bits; // Begin step pulse. - #ifdef ENABLE_DUAL_AXIS - STEP_PORT_DUAL = st.step_bits_dual; - #endif - } +#ifdef ENABLE_DUAL_AXIS + STEP_PORT_DUAL = st.step_bits_dual; +#endif +} #endif - // Generates the step and direction port invert masks used in the Stepper Interrupt Driver. -void st_generate_step_dir_invert_masks() -{ - uint8_t idx; - step_port_invert_mask = 0; - dir_port_invert_mask = 0; - for (idx=0; idxentry_speed_sqr = prep.current_speed*prep.current_speed; // Update entry speed. - pl_block = NULL; // Flag st_prep_segment() to load and check active velocity profile. - } +void st_update_plan_block_parameters() { + if (pl_block != NULL) { // Ignore if at start of a new block. + prep.recalculate_flag |= PREP_FLAG_RECALCULATE; + pl_block->entry_speed_sqr = prep.current_speed * prep.current_speed; // Update entry speed. + pl_block = NULL; // Flag st_prep_segment() to load and check active velocity profile. + } } - // Increments the step segment buffer block data ring buffer. -static uint8_t st_next_block_index(uint8_t block_index) -{ - block_index++; - if ( block_index == (SEGMENT_BUFFER_SIZE-1) ) { return(0); } - return(block_index); +static uint8_t st_next_block_index(uint8_t block_index) { + block_index++; + if (block_index == (SEGMENT_BUFFER_SIZE - 1)) { + return (0); + } + return (block_index); } - #ifdef PARKING_ENABLE - // Changes the run state of the step segment buffer to execute the special parking motion. - void st_parking_setup_buffer() - { +// Changes the run state of the step segment buffer to execute the special parking motion. +void st_parking_setup_buffer() { // Store step execution data of partially completed block, if necessary. if (prep.recalculate_flag & PREP_FLAG_HOLD_PARTIAL_BLOCK) { - prep.last_st_block_index = prep.st_block_index; - prep.last_steps_remaining = prep.steps_remaining; - prep.last_dt_remainder = prep.dt_remainder; - prep.last_step_per_mm = prep.step_per_mm; + prep.last_st_block_index = prep.st_block_index; + prep.last_steps_remaining = prep.steps_remaining; + prep.last_dt_remainder = prep.dt_remainder; + prep.last_step_per_mm = prep.step_per_mm; } // Set flags to execute a parking motion prep.recalculate_flag |= PREP_FLAG_PARKING; prep.recalculate_flag &= ~(PREP_FLAG_RECALCULATE); pl_block = NULL; // Always reset parking motion to reload new block. - } +} - - // Restores the step segment buffer to the normal run state after a parking motion. - void st_parking_restore_buffer() - { +// Restores the step segment buffer to the normal run state after a parking motion. +void st_parking_restore_buffer() { // Restore step execution data and flags of partially completed block, if necessary. if (prep.recalculate_flag & PREP_FLAG_HOLD_PARTIAL_BLOCK) { - st_prep_block = &st_block_buffer[prep.last_st_block_index]; - prep.st_block_index = prep.last_st_block_index; - prep.steps_remaining = prep.last_steps_remaining; - prep.dt_remainder = prep.last_dt_remainder; - prep.step_per_mm = prep.last_step_per_mm; - prep.recalculate_flag = (PREP_FLAG_HOLD_PARTIAL_BLOCK | PREP_FLAG_RECALCULATE); - prep.req_mm_increment = REQ_MM_INCREMENT_SCALAR/prep.step_per_mm; // Recompute this value. + st_prep_block = &st_block_buffer[prep.last_st_block_index]; + prep.st_block_index = prep.last_st_block_index; + prep.steps_remaining = prep.last_steps_remaining; + prep.dt_remainder = prep.last_dt_remainder; + prep.step_per_mm = prep.last_step_per_mm; + prep.recalculate_flag = (PREP_FLAG_HOLD_PARTIAL_BLOCK | PREP_FLAG_RECALCULATE); + prep.req_mm_increment = REQ_MM_INCREMENT_SCALAR / prep.step_per_mm; // Recompute this value. } else { - prep.recalculate_flag = false; + prep.recalculate_flag = false; } pl_block = NULL; // Set to reload next block. - } +} #endif - /* Prepares step segment buffer. Continuously called from main program. The segment buffer is an intermediary buffer interface between the execution of steps @@ -665,431 +676,476 @@ static uint8_t st_next_block_index(uint8_t block_index) Currently, the segment buffer conservatively holds roughly up to 40-50 msec of steps. NOTE: Computation units are in steps, millimeters, and minutes. */ -void st_prep_buffer() -{ - // Block step prep buffer, while in a suspend state and there is no suspend motion to execute. - if (bit_istrue(sys.step_control,STEP_CONTROL_END_MOTION)) { return; } +void st_prep_buffer() { + // Block step prep buffer, while in a suspend state and there is no suspend motion to execute. + if (bit_istrue(sys.step_control, STEP_CONTROL_END_MOTION)) { + return; + } - while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. + while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. - // Determine if we need to load a new planner block or if the block needs to be recomputed. - if (pl_block == NULL) { + // Determine if we need to load a new planner block or if the block needs to be recomputed. + if (pl_block == NULL) { - // Query planner for a queued block - if (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION) { pl_block = plan_get_system_motion_block(); } - else { pl_block = plan_get_current_block(); } - if (pl_block == NULL) { return; } // No planner blocks. Exit. - - // Check if we need to only recompute the velocity profile or load a new block. - if (prep.recalculate_flag & PREP_FLAG_RECALCULATE) { - - #ifdef PARKING_ENABLE - if (prep.recalculate_flag & PREP_FLAG_PARKING) { prep.recalculate_flag &= ~(PREP_FLAG_RECALCULATE); } - else { prep.recalculate_flag = false; } - #else - prep.recalculate_flag = false; - #endif - - } else { - - // Load the Bresenham stepping data for the block. - prep.st_block_index = st_next_block_index(prep.st_block_index); - - // Prepare and copy Bresenham algorithm segment data from the new planner block, so that - // when the segment buffer completes the planner block, it may be discarded when the - // segment buffer finishes the prepped block, but the stepper ISR is still executing it. - st_prep_block = &st_block_buffer[prep.st_block_index]; - st_prep_block->direction_bits = pl_block->direction_bits; - #ifdef ENABLE_DUAL_AXIS - #if (DUAL_AXIS_SELECT == X_AXIS) - if (st_prep_block->direction_bits & (1<direction_bits & (1<direction_bits_dual = (1<direction_bits_dual = 0; } - #endif - uint8_t idx; - #ifndef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING - for (idx=0; idxsteps[idx] = (pl_block->steps[idx] << 1); } - st_prep_block->step_event_count = (pl_block->step_event_count << 1); - #else - // With AMASS enabled, simply bit-shift multiply all Bresenham data by the max AMASS - // level, such that we never divide beyond the original data anywhere in the algorithm. - // If the original data is divided, we can lose a step from integer roundoff. - for (idx=0; idxsteps[idx] = pl_block->steps[idx] << MAX_AMASS_LEVEL; } - st_prep_block->step_event_count = pl_block->step_event_count << MAX_AMASS_LEVEL; - #endif - - // Initialize segment buffer data for generating the segments. - prep.steps_remaining = (float)pl_block->step_event_count; - prep.step_per_mm = prep.steps_remaining/pl_block->millimeters; - prep.req_mm_increment = REQ_MM_INCREMENT_SCALAR/prep.step_per_mm; - prep.dt_remainder = 0.0; // Reset for new segment block - - if ((sys.step_control & STEP_CONTROL_EXECUTE_HOLD) || (prep.recalculate_flag & PREP_FLAG_DECEL_OVERRIDE)) { - // New block loaded mid-hold. Override planner block entry speed to enforce deceleration. - prep.current_speed = prep.exit_speed; - pl_block->entry_speed_sqr = prep.exit_speed*prep.exit_speed; - prep.recalculate_flag &= ~(PREP_FLAG_DECEL_OVERRIDE); - } else { - prep.current_speed = sqrt(pl_block->entry_speed_sqr); - } - - #ifdef VARIABLE_SPINDLE - // Setup laser mode variables. PWM rate adjusted motions will always complete a motion with the - // spindle off. - st_prep_block->is_pwm_rate_adjusted = false; - if (settings.flags & BITFLAG_LASER_MODE) { - if (pl_block->condition & PL_COND_FLAG_SPINDLE_CCW) { - // Pre-compute inverse programmed rate to speed up PWM updating per step segment. - prep.inv_rate = 1.0/pl_block->programmed_rate; - st_prep_block->is_pwm_rate_adjusted = true; + // Query planner for a queued block + if (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION) { + pl_block = plan_get_system_motion_block(); + } else { + pl_block = plan_get_current_block(); } - } - #endif - } + if (pl_block == NULL) { + return; + } // No planner blocks. Exit. - /* --------------------------------------------------------------------------------- - Compute the velocity profile of a new planner block based on its entry and exit - speeds, or recompute the profile of a partially-completed planner block if the - planner has updated it. For a commanded forced-deceleration, such as from a feed - hold, override the planner velocities and decelerate to the target exit speed. - */ - prep.mm_complete = 0.0; // Default velocity profile complete at 0.0mm from end of block. - float inv_2_accel = 0.5/pl_block->acceleration; - if (sys.step_control & STEP_CONTROL_EXECUTE_HOLD) { // [Forced Deceleration to Zero Velocity] - // Compute velocity profile parameters for a feed hold in-progress. This profile overrides - // the planner block profile, enforcing a deceleration to zero speed. - prep.ramp_type = RAMP_DECEL; - // Compute decelerate distance relative to end of block. - float decel_dist = pl_block->millimeters - inv_2_accel*pl_block->entry_speed_sqr; - if (decel_dist < 0.0) { - // Deceleration through entire planner block. End of feed hold is not in this block. - prep.exit_speed = sqrt(pl_block->entry_speed_sqr-2*pl_block->acceleration*pl_block->millimeters); - } else { - prep.mm_complete = decel_dist; // End of feed hold. - prep.exit_speed = 0.0; - } - } else { // [Normal Operation] - // Compute or recompute velocity profile parameters of the prepped planner block. - prep.ramp_type = RAMP_ACCEL; // Initialize as acceleration ramp. - prep.accelerate_until = pl_block->millimeters; + // Check if we need to only recompute the velocity profile or load a new block. + if (prep.recalculate_flag & PREP_FLAG_RECALCULATE) { - float exit_speed_sqr; - float nominal_speed; - if (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION) { - prep.exit_speed = exit_speed_sqr = 0.0; // Enforce stop at end of system motion. - } else { - exit_speed_sqr = plan_get_exec_block_exit_speed_sqr(); - prep.exit_speed = sqrt(exit_speed_sqr); - } +#ifdef PARKING_ENABLE + if (prep.recalculate_flag & PREP_FLAG_PARKING) { + prep.recalculate_flag &= ~(PREP_FLAG_RECALCULATE); + } else { + prep.recalculate_flag = false; + } +#else + prep.recalculate_flag = false; +#endif - nominal_speed = plan_compute_profile_nominal_speed(pl_block); - float nominal_speed_sqr = nominal_speed*nominal_speed; - float intersect_distance = - 0.5*(pl_block->millimeters+inv_2_accel*(pl_block->entry_speed_sqr-exit_speed_sqr)); + } else { - if (pl_block->entry_speed_sqr > nominal_speed_sqr) { // Only occurs during override reductions. - prep.accelerate_until = pl_block->millimeters - inv_2_accel*(pl_block->entry_speed_sqr-nominal_speed_sqr); - if (prep.accelerate_until <= 0.0) { // Deceleration-only. - prep.ramp_type = RAMP_DECEL; - // prep.decelerate_after = pl_block->millimeters; - // prep.maximum_speed = prep.current_speed; + // Load the Bresenham stepping data for the block. + prep.st_block_index = st_next_block_index(prep.st_block_index); - // Compute override block exit speed since it doesn't match the planner exit speed. - prep.exit_speed = sqrt(pl_block->entry_speed_sqr - 2*pl_block->acceleration*pl_block->millimeters); - prep.recalculate_flag |= PREP_FLAG_DECEL_OVERRIDE; // Flag to load next block as deceleration override. + // Prepare and copy Bresenham algorithm segment data from the new planner block, so that + // when the segment buffer completes the planner block, it may be discarded when the + // segment buffer finishes the prepped block, but the stepper ISR is still executing it. + st_prep_block = &st_block_buffer[prep.st_block_index]; + st_prep_block->direction_bits = pl_block->direction_bits; +#ifdef ENABLE_DUAL_AXIS +#if (DUAL_AXIS_SELECT == X_AXIS) + if (st_prep_block->direction_bits & (1 << X_DIRECTION_BIT)) { +#elif (DUAL_AXIS_SELECT == Y_AXIS) + if (st_prep_block->direction_bits & (1 << Y_DIRECTION_BIT)) { +#endif + st_prep_block->direction_bits_dual = (1 << DUAL_DIRECTION_BIT); + } else { + st_prep_block->direction_bits_dual = 0; + } +#endif + uint8_t idx; +#ifndef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING + for (idx = 0; idx < N_AXIS; idx++) { + st_prep_block->steps[idx] = (pl_block->steps[idx] << 1); + } + st_prep_block->step_event_count = (pl_block->step_event_count << 1); +#else + // With AMASS enabled, simply bit-shift multiply all Bresenham data by the max AMASS + // level, such that we never divide beyond the original data anywhere in the algorithm. + // If the original data is divided, we can lose a step from integer roundoff. + for (idx = 0; idx < N_AXIS; idx++) { + st_prep_block->steps[idx] = pl_block->steps[idx] << MAX_AMASS_LEVEL; + } + st_prep_block->step_event_count = pl_block->step_event_count << MAX_AMASS_LEVEL; +#endif - // TODO: Determine correct handling of parameters in deceleration-only. - // Can be tricky since entry speed will be current speed, as in feed holds. - // Also, look into near-zero speed handling issues with this. + // Initialize segment buffer data for generating the segments. + prep.steps_remaining = (float)pl_block->step_event_count; + prep.step_per_mm = prep.steps_remaining / pl_block->millimeters; + prep.req_mm_increment = REQ_MM_INCREMENT_SCALAR / prep.step_per_mm; + prep.dt_remainder = 0.0; // Reset for new segment block - } else { - // Decelerate to cruise or cruise-decelerate types. Guaranteed to intersect updated plan. - prep.decelerate_after = inv_2_accel*(nominal_speed_sqr-exit_speed_sqr); // Should always be >= 0.0 due to planner reinit. - prep.maximum_speed = nominal_speed; - prep.ramp_type = RAMP_DECEL_OVERRIDE; - } - } else if (intersect_distance > 0.0) { - if (intersect_distance < pl_block->millimeters) { // Either trapezoid or triangle types - // NOTE: For acceleration-cruise and cruise-only types, following calculation will be 0.0. - prep.decelerate_after = inv_2_accel*(nominal_speed_sqr-exit_speed_sqr); - if (prep.decelerate_after < intersect_distance) { // Trapezoid type - prep.maximum_speed = nominal_speed; - if (pl_block->entry_speed_sqr == nominal_speed_sqr) { - // Cruise-deceleration or cruise-only type. - prep.ramp_type = RAMP_CRUISE; - } else { - // Full-trapezoid or acceleration-cruise types - prep.accelerate_until -= inv_2_accel*(nominal_speed_sqr-pl_block->entry_speed_sqr); - } - } else { // Triangle type - prep.accelerate_until = intersect_distance; - prep.decelerate_after = intersect_distance; - prep.maximum_speed = sqrt(2.0*pl_block->acceleration*intersect_distance+exit_speed_sqr); - } - } else { // Deceleration-only type - prep.ramp_type = RAMP_DECEL; - // prep.decelerate_after = pl_block->millimeters; - // prep.maximum_speed = prep.current_speed; - } - } else { // Acceleration-only type - prep.accelerate_until = 0.0; - // prep.decelerate_after = 0.0; - prep.maximum_speed = prep.exit_speed; - } - } - - #ifdef VARIABLE_SPINDLE - bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_PWM); // Force update whenever updating block. - #endif - } - - // Initialize new segment - segment_t *prep_segment = &segment_buffer[segment_buffer_head]; + if ((sys.step_control & STEP_CONTROL_EXECUTE_HOLD) || + (prep.recalculate_flag & PREP_FLAG_DECEL_OVERRIDE)) { + // New block loaded mid-hold. Override planner block entry speed to enforce deceleration. + prep.current_speed = prep.exit_speed; + pl_block->entry_speed_sqr = prep.exit_speed * prep.exit_speed; + prep.recalculate_flag &= ~(PREP_FLAG_DECEL_OVERRIDE); + } else { + prep.current_speed = sqrt(pl_block->entry_speed_sqr); + } - // Set new segment to point to the current segment data block. - prep_segment->st_block_index = prep.st_block_index; - - /*------------------------------------------------------------------------------------ - Compute the average velocity of this new segment by determining the total distance - traveled over the segment time DT_SEGMENT. The following code first attempts to create - a full segment based on the current ramp conditions. If the segment time is incomplete - when terminating at a ramp state change, the code will continue to loop through the - progressing ramp states to fill the remaining segment execution time. However, if - an incomplete segment terminates at the end of the velocity profile, the segment is - considered completed despite having a truncated execution time less than DT_SEGMENT. - The velocity profile is always assumed to progress through the ramp sequence: - acceleration ramp, cruising state, and deceleration ramp. Each ramp's travel distance - may range from zero to the length of the block. Velocity profiles can end either at - the end of planner block (typical) or mid-block at the end of a forced deceleration, - such as from a feed hold. - */ - float dt_max = DT_SEGMENT; // Maximum segment time - float dt = 0.0; // Initialize segment time - float time_var = dt_max; // Time worker variable - float mm_var; // mm-Distance worker variable - float speed_var; // Speed worker variable - float mm_remaining = pl_block->millimeters; // New segment distance from end of block. - float minimum_mm = mm_remaining-prep.req_mm_increment; // Guarantee at least one step. - if (minimum_mm < 0.0) { minimum_mm = 0.0; } - - do { - switch (prep.ramp_type) { - case RAMP_DECEL_OVERRIDE: - speed_var = pl_block->acceleration*time_var; - if (prep.current_speed-prep.maximum_speed <= speed_var) { - // Cruise or cruise-deceleration types only for deceleration override. - mm_remaining = prep.accelerate_until; - time_var = 2.0*(pl_block->millimeters-mm_remaining)/(prep.current_speed+prep.maximum_speed); - prep.ramp_type = RAMP_CRUISE; - prep.current_speed = prep.maximum_speed; - } else { // Mid-deceleration override ramp. - mm_remaining -= time_var*(prep.current_speed - 0.5*speed_var); - prep.current_speed -= speed_var; - } - break; - case RAMP_ACCEL: - // NOTE: Acceleration ramp only computes during first do-while loop. - speed_var = pl_block->acceleration*time_var; - mm_remaining -= time_var*(prep.current_speed + 0.5*speed_var); - if (mm_remaining < prep.accelerate_until) { // End of acceleration ramp. - // Acceleration-cruise, acceleration-deceleration ramp junction, or end of block. - mm_remaining = prep.accelerate_until; // NOTE: 0.0 at EOB - time_var = 2.0*(pl_block->millimeters-mm_remaining)/(prep.current_speed+prep.maximum_speed); - if (mm_remaining == prep.decelerate_after) { prep.ramp_type = RAMP_DECEL; } - else { prep.ramp_type = RAMP_CRUISE; } - prep.current_speed = prep.maximum_speed; - } else { // Acceleration only. - prep.current_speed += speed_var; - } - break; - case RAMP_CRUISE: - // NOTE: mm_var used to retain the last mm_remaining for incomplete segment time_var calculations. - // NOTE: If maximum_speed*time_var value is too low, round-off can cause mm_var to not change. To - // prevent this, simply enforce a minimum speed threshold in the planner. - mm_var = mm_remaining - prep.maximum_speed*time_var; - if (mm_var < prep.decelerate_after) { // End of cruise. - // Cruise-deceleration junction or end of block. - time_var = (mm_remaining - prep.decelerate_after)/prep.maximum_speed; - mm_remaining = prep.decelerate_after; // NOTE: 0.0 at EOB - prep.ramp_type = RAMP_DECEL; - } else { // Cruising only. - mm_remaining = mm_var; - } - break; - default: // case RAMP_DECEL: - // NOTE: mm_var used as a misc worker variable to prevent errors when near zero speed. - speed_var = pl_block->acceleration*time_var; // Used as delta speed (mm/min) - if (prep.current_speed > speed_var) { // Check if at or below zero speed. - // Compute distance from end of segment to end of block. - mm_var = mm_remaining - time_var*(prep.current_speed - 0.5*speed_var); // (mm) - if (mm_var > prep.mm_complete) { // Typical case. In deceleration ramp. - mm_remaining = mm_var; - prep.current_speed -= speed_var; - break; // Segment complete. Exit switch-case statement. Continue do-while loop. +#ifdef VARIABLE_SPINDLE + // Setup laser mode variables. PWM rate adjusted motions will always complete a motion with the + // spindle off. + st_prep_block->is_pwm_rate_adjusted = false; + if (settings.flags & BITFLAG_LASER_MODE) { + if (pl_block->condition & PL_COND_FLAG_SPINDLE_CCW) { + // Pre-compute inverse programmed rate to speed up PWM updating per step segment. + prep.inv_rate = 1.0 / pl_block->programmed_rate; + st_prep_block->is_pwm_rate_adjusted = true; + } + } +#endif } - } - // Otherwise, at end of block or end of forced-deceleration. - time_var = 2.0*(mm_remaining-prep.mm_complete)/(prep.current_speed+prep.exit_speed); - mm_remaining = prep.mm_complete; - prep.current_speed = prep.exit_speed; - } - dt += time_var; // Add computed ramp time to total segment time. - if (dt < dt_max) { time_var = dt_max - dt; } // **Incomplete** At ramp junction. - else { - if (mm_remaining > minimum_mm) { // Check for very slow segments with zero steps. - // Increase segment time to ensure at least one step in segment. Override and loop - // through distance calculations until minimum_mm or mm_complete. - dt_max += DT_SEGMENT; - time_var = dt_max - dt; + + /* --------------------------------------------------------------------------------- + Compute the velocity profile of a new planner block based on its entry and exit + speeds, or recompute the profile of a partially-completed planner block if the + planner has updated it. For a commanded forced-deceleration, such as from a feed + hold, override the planner velocities and decelerate to the target exit speed. + */ + prep.mm_complete = 0.0; // Default velocity profile complete at 0.0mm from end of block. + float inv_2_accel = 0.5 / pl_block->acceleration; + if (sys.step_control & STEP_CONTROL_EXECUTE_HOLD) { // [Forced Deceleration to Zero Velocity] + // Compute velocity profile parameters for a feed hold in-progress. This profile overrides + // the planner block profile, enforcing a deceleration to zero speed. + prep.ramp_type = RAMP_DECEL; + // Compute decelerate distance relative to end of block. + float decel_dist = pl_block->millimeters - inv_2_accel * pl_block->entry_speed_sqr; + if (decel_dist < 0.0) { + // Deceleration through entire planner block. End of feed hold is not in this block. + prep.exit_speed = + sqrt(pl_block->entry_speed_sqr - 2 * pl_block->acceleration * pl_block->millimeters); + } else { + prep.mm_complete = decel_dist; // End of feed hold. + prep.exit_speed = 0.0; + } + } else { // [Normal Operation] + // Compute or recompute velocity profile parameters of the prepped planner block. + prep.ramp_type = RAMP_ACCEL; // Initialize as acceleration ramp. + prep.accelerate_until = pl_block->millimeters; + + float exit_speed_sqr; + float nominal_speed; + if (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION) { + prep.exit_speed = exit_speed_sqr = 0.0; // Enforce stop at end of system motion. + } else { + exit_speed_sqr = plan_get_exec_block_exit_speed_sqr(); + prep.exit_speed = sqrt(exit_speed_sqr); + } + + nominal_speed = plan_compute_profile_nominal_speed(pl_block); + float nominal_speed_sqr = nominal_speed * nominal_speed; + float intersect_distance = + 0.5 * (pl_block->millimeters + inv_2_accel * (pl_block->entry_speed_sqr - exit_speed_sqr)); + + if (pl_block->entry_speed_sqr > nominal_speed_sqr) { // Only occurs during override reductions. + prep.accelerate_until = + pl_block->millimeters - inv_2_accel * (pl_block->entry_speed_sqr - nominal_speed_sqr); + if (prep.accelerate_until <= 0.0) { // Deceleration-only. + prep.ramp_type = RAMP_DECEL; + // prep.decelerate_after = pl_block->millimeters; + // prep.maximum_speed = prep.current_speed; + + // Compute override block exit speed since it doesn't match the planner exit speed. + prep.exit_speed = + sqrt(pl_block->entry_speed_sqr - 2 * pl_block->acceleration * pl_block->millimeters); + prep.recalculate_flag |= + PREP_FLAG_DECEL_OVERRIDE; // Flag to load next block as deceleration override. + + // TODO: Determine correct handling of parameters in deceleration-only. + // Can be tricky since entry speed will be current speed, as in feed holds. + // Also, look into near-zero speed handling issues with this. + + } else { + // Decelerate to cruise or cruise-decelerate types. Guaranteed to intersect updated plan. + prep.decelerate_after = + inv_2_accel * + (nominal_speed_sqr - exit_speed_sqr); // Should always be >= 0.0 due to planner reinit. + prep.maximum_speed = nominal_speed; + prep.ramp_type = RAMP_DECEL_OVERRIDE; + } + } else if (intersect_distance > 0.0) { + if (intersect_distance < pl_block->millimeters) { // Either trapezoid or triangle types + // NOTE: For acceleration-cruise and cruise-only types, following calculation will be 0.0. + prep.decelerate_after = inv_2_accel * (nominal_speed_sqr - exit_speed_sqr); + if (prep.decelerate_after < intersect_distance) { // Trapezoid type + prep.maximum_speed = nominal_speed; + if (pl_block->entry_speed_sqr == nominal_speed_sqr) { + // Cruise-deceleration or cruise-only type. + prep.ramp_type = RAMP_CRUISE; + } else { + // Full-trapezoid or acceleration-cruise types + prep.accelerate_until -= inv_2_accel * (nominal_speed_sqr - pl_block->entry_speed_sqr); + } + } else { // Triangle type + prep.accelerate_until = intersect_distance; + prep.decelerate_after = intersect_distance; + prep.maximum_speed = + sqrt(2.0 * pl_block->acceleration * intersect_distance + exit_speed_sqr); + } + } else { // Deceleration-only type + prep.ramp_type = RAMP_DECEL; + // prep.decelerate_after = pl_block->millimeters; + // prep.maximum_speed = prep.current_speed; + } + } else { // Acceleration-only type + prep.accelerate_until = 0.0; + // prep.decelerate_after = 0.0; + prep.maximum_speed = prep.exit_speed; + } + } + +#ifdef VARIABLE_SPINDLE + bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_PWM); // Force update whenever updating block. +#endif + } + + // Initialize new segment + segment_t *prep_segment = &segment_buffer[segment_buffer_head]; + + // Set new segment to point to the current segment data block. + prep_segment->st_block_index = prep.st_block_index; + + /*------------------------------------------------------------------------------------ + Compute the average velocity of this new segment by determining the total distance + traveled over the segment time DT_SEGMENT. The following code first attempts to create + a full segment based on the current ramp conditions. If the segment time is incomplete + when terminating at a ramp state change, the code will continue to loop through the + progressing ramp states to fill the remaining segment execution time. However, if + an incomplete segment terminates at the end of the velocity profile, the segment is + considered completed despite having a truncated execution time less than DT_SEGMENT. + The velocity profile is always assumed to progress through the ramp sequence: + acceleration ramp, cruising state, and deceleration ramp. Each ramp's travel distance + may range from zero to the length of the block. Velocity profiles can end either at + the end of planner block (typical) or mid-block at the end of a forced deceleration, + such as from a feed hold. + */ + float dt_max = DT_SEGMENT; // Maximum segment time + float dt = 0.0; // Initialize segment time + float time_var = dt_max; // Time worker variable + float mm_var; // mm-Distance worker variable + float speed_var; // Speed worker variable + float mm_remaining = pl_block->millimeters; // New segment distance from end of block. + float minimum_mm = mm_remaining - prep.req_mm_increment; // Guarantee at least one step. + if (minimum_mm < 0.0) { + minimum_mm = 0.0; + } + + do { + switch (prep.ramp_type) { + case RAMP_DECEL_OVERRIDE: + speed_var = pl_block->acceleration * time_var; + if (prep.current_speed - prep.maximum_speed <= speed_var) { + // Cruise or cruise-deceleration types only for deceleration override. + mm_remaining = prep.accelerate_until; + time_var = 2.0 * (pl_block->millimeters - mm_remaining) / (prep.current_speed + prep.maximum_speed); + prep.ramp_type = RAMP_CRUISE; + prep.current_speed = prep.maximum_speed; + } else { // Mid-deceleration override ramp. + mm_remaining -= time_var * (prep.current_speed - 0.5 * speed_var); + prep.current_speed -= speed_var; + } + break; + case RAMP_ACCEL: + // NOTE: Acceleration ramp only computes during first do-while loop. + speed_var = pl_block->acceleration * time_var; + mm_remaining -= time_var * (prep.current_speed + 0.5 * speed_var); + if (mm_remaining < prep.accelerate_until) { // End of acceleration ramp. + // Acceleration-cruise, acceleration-deceleration ramp junction, or end of block. + mm_remaining = prep.accelerate_until; // NOTE: 0.0 at EOB + time_var = 2.0 * (pl_block->millimeters - mm_remaining) / (prep.current_speed + prep.maximum_speed); + if (mm_remaining == prep.decelerate_after) { + prep.ramp_type = RAMP_DECEL; + } else { + prep.ramp_type = RAMP_CRUISE; + } + prep.current_speed = prep.maximum_speed; + } else { // Acceleration only. + prep.current_speed += speed_var; + } + break; + case RAMP_CRUISE: + // NOTE: mm_var used to retain the last mm_remaining for incomplete segment time_var calculations. + // NOTE: If maximum_speed*time_var value is too low, round-off can cause mm_var to not change. To + // prevent this, simply enforce a minimum speed threshold in the planner. + mm_var = mm_remaining - prep.maximum_speed * time_var; + if (mm_var < prep.decelerate_after) { // End of cruise. + // Cruise-deceleration junction or end of block. + time_var = (mm_remaining - prep.decelerate_after) / prep.maximum_speed; + mm_remaining = prep.decelerate_after; // NOTE: 0.0 at EOB + prep.ramp_type = RAMP_DECEL; + } else { // Cruising only. + mm_remaining = mm_var; + } + break; + default: // case RAMP_DECEL: + // NOTE: mm_var used as a misc worker variable to prevent errors when near zero speed. + speed_var = pl_block->acceleration * time_var; // Used as delta speed (mm/min) + if (prep.current_speed > speed_var) { // Check if at or below zero speed. + // Compute distance from end of segment to end of block. + mm_var = mm_remaining - time_var * (prep.current_speed - 0.5 * speed_var); // (mm) + if (mm_var > prep.mm_complete) { // Typical case. In deceleration ramp. + mm_remaining = mm_var; + prep.current_speed -= speed_var; + break; // Segment complete. Exit switch-case statement. Continue do-while loop. + } + } + // Otherwise, at end of block or end of forced-deceleration. + time_var = 2.0 * (mm_remaining - prep.mm_complete) / (prep.current_speed + prep.exit_speed); + mm_remaining = prep.mm_complete; + prep.current_speed = prep.exit_speed; + } + dt += time_var; // Add computed ramp time to total segment time. + if (dt < dt_max) { + time_var = dt_max - dt; + } // **Incomplete** At ramp junction. + else { + if (mm_remaining > minimum_mm) { // Check for very slow segments with zero steps. + // Increase segment time to ensure at least one step in segment. Override and loop + // through distance calculations until minimum_mm or mm_complete. + dt_max += DT_SEGMENT; + time_var = dt_max - dt; + } else { + break; // **Complete** Exit loop. Segment execution time maxed. + } + } + } while (mm_remaining > prep.mm_complete); // **Complete** Exit loop. Profile complete. + +#ifdef VARIABLE_SPINDLE + /* ----------------------------------------------------------------------------------- + Compute spindle speed PWM output for step segment + */ + + if (st_prep_block->is_pwm_rate_adjusted || (sys.step_control & STEP_CONTROL_UPDATE_SPINDLE_PWM)) { + if (pl_block->condition & (PL_COND_FLAG_SPINDLE_CW | PL_COND_FLAG_SPINDLE_CCW)) { + float rpm = pl_block->spindle_speed; + // NOTE: Feed and rapid overrides are independent of PWM value and do not alter laser power/rate. + if (st_prep_block->is_pwm_rate_adjusted) { + rpm *= (prep.current_speed * prep.inv_rate); + } + // If current_speed is zero, then may need to be rpm_min*(100/MAX_SPINDLE_SPEED_OVERRIDE) + // but this would be instantaneous only and during a motion. May not matter at all. + prep.current_spindle_pwm = spindle_compute_pwm_value(rpm); + } else { + sys.spindle_speed = 0.0; + prep.current_spindle_pwm = SPINDLE_PWM_OFF_VALUE; + } + bit_false(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_PWM); + } + prep_segment->spindle_pwm = prep.current_spindle_pwm; // Reload segment PWM value + +#endif + + /* ----------------------------------------------------------------------------------- + Compute segment step rate, steps to execute, and apply necessary rate corrections. + NOTE: Steps are computed by direct scalar conversion of the millimeter distance + remaining in the block, rather than incrementally tallying the steps executed per + segment. This helps in removing floating point round-off issues of several additions. + However, since floats have only 7.2 significant digits, long moves with extremely + high step counts can exceed the precision of floats, which can lead to lost steps. + Fortunately, this scenario is highly unlikely and unrealistic in CNC machines + supported by Grbl (i.e. exceeding 10 meters axis travel at 200 step/mm). + */ + float step_dist_remaining = prep.step_per_mm * mm_remaining; // Convert mm_remaining to steps + float n_steps_remaining = ceil(step_dist_remaining); // Round-up current steps remaining + float last_n_steps_remaining = ceil(prep.steps_remaining); // Round-up last steps remaining + prep_segment->n_step = last_n_steps_remaining - n_steps_remaining; // Compute number of steps to execute. + + // Bail if we are at the end of a feed hold and don't have a step to execute. + if (prep_segment->n_step == 0) { + if (sys.step_control & STEP_CONTROL_EXECUTE_HOLD) { + // Less than one step to decelerate to zero speed, but already very close. AMASS + // requires full steps to execute. So, just bail. + bit_true(sys.step_control, STEP_CONTROL_END_MOTION); +#ifdef PARKING_ENABLE + if (!(prep.recalculate_flag & PREP_FLAG_PARKING)) { + prep.recalculate_flag |= PREP_FLAG_HOLD_PARTIAL_BLOCK; + } +#endif + return; // Segment not generated, but current step data still retained. + } + } + + // Compute segment step rate. Since steps are integers and mm distances traveled are not, + // the end of every segment can have a partial step of varying magnitudes that are not + // executed, because the stepper ISR requires whole steps due to the AMASS algorithm. To + // compensate, we track the time to execute the previous segment's partial step and simply + // apply it with the partial step distance to the current segment, so that it minutely + // adjusts the whole segment rate to keep step output exact. These rate adjustments are + // typically very small and do not adversely effect performance, but ensures that Grbl + // outputs the exact acceleration and velocity profiles as computed by the planner. + dt += prep.dt_remainder; // Apply previous segment partial step execute time + float inv_rate = dt / (last_n_steps_remaining - step_dist_remaining); // Compute adjusted step rate inverse + + // Compute CPU cycles per step for the prepped segment. + uint32_t cycles = ceil((TICKS_PER_MICROSECOND * 1000000 * 60) * + inv_rate); // (cycles/step) + +#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING + // Compute step timing and multi-axis smoothing level. + // NOTE: AMASS overdrives the timer with each level, so only one prescalar is required. + if (cycles < AMASS_LEVEL1) { + prep_segment->amass_level = 0; } else { - break; // **Complete** Exit loop. Segment execution time maxed. + if (cycles < AMASS_LEVEL2) { + prep_segment->amass_level = 1; + } else if (cycles < AMASS_LEVEL3) { + prep_segment->amass_level = 2; + } else { + prep_segment->amass_level = 3; + } + cycles >>= prep_segment->amass_level; + prep_segment->n_step <<= prep_segment->amass_level; } - } - } while (mm_remaining > prep.mm_complete); // **Complete** Exit loop. Profile complete. - - #ifdef VARIABLE_SPINDLE - /* ----------------------------------------------------------------------------------- - Compute spindle speed PWM output for step segment - */ - - if (st_prep_block->is_pwm_rate_adjusted || (sys.step_control & STEP_CONTROL_UPDATE_SPINDLE_PWM)) { - if (pl_block->condition & (PL_COND_FLAG_SPINDLE_CW | PL_COND_FLAG_SPINDLE_CCW)) { - float rpm = pl_block->spindle_speed; - // NOTE: Feed and rapid overrides are independent of PWM value and do not alter laser power/rate. - if (st_prep_block->is_pwm_rate_adjusted) { rpm *= (prep.current_speed * prep.inv_rate); } - // If current_speed is zero, then may need to be rpm_min*(100/MAX_SPINDLE_SPEED_OVERRIDE) - // but this would be instantaneous only and during a motion. May not matter at all. - prep.current_spindle_pwm = spindle_compute_pwm_value(rpm); - } else { - sys.spindle_speed = 0.0; - prep.current_spindle_pwm = SPINDLE_PWM_OFF_VALUE; + if (cycles < (1UL << 16)) { + prep_segment->cycles_per_tick = cycles; + } // < 65536 (4.1ms @ 16MHz) + else { + prep_segment->cycles_per_tick = 0xffff; + } // Just set the slowest speed possible. +#else + // Compute step timing and timer prescalar for normal step generation. + if (cycles < (1UL << 16)) { // < 65536 (4.1ms @ 16MHz) + prep_segment->prescaler = 1; // prescaler: 0 + prep_segment->cycles_per_tick = cycles; + } else if (cycles < (1UL << 19)) { // < 524288 (32.8ms@16MHz) + prep_segment->prescaler = 2; // prescaler: 8 + prep_segment->cycles_per_tick = cycles >> 3; + } else { + prep_segment->prescaler = 3; // prescaler: 64 + if (cycles < (1UL << 22)) { // < 4194304 (262ms@16MHz) + prep_segment->cycles_per_tick = cycles >> 6; + } else { // Just set the slowest speed possible. (Around 4 step/sec.) + prep_segment->cycles_per_tick = 0xffff; + } } - bit_false(sys.step_control,STEP_CONTROL_UPDATE_SPINDLE_PWM); - } - prep_segment->spindle_pwm = prep.current_spindle_pwm; // Reload segment PWM value +#endif - #endif - - /* ----------------------------------------------------------------------------------- - Compute segment step rate, steps to execute, and apply necessary rate corrections. - NOTE: Steps are computed by direct scalar conversion of the millimeter distance - remaining in the block, rather than incrementally tallying the steps executed per - segment. This helps in removing floating point round-off issues of several additions. - However, since floats have only 7.2 significant digits, long moves with extremely - high step counts can exceed the precision of floats, which can lead to lost steps. - Fortunately, this scenario is highly unlikely and unrealistic in CNC machines - supported by Grbl (i.e. exceeding 10 meters axis travel at 200 step/mm). - */ - float step_dist_remaining = prep.step_per_mm*mm_remaining; // Convert mm_remaining to steps - float n_steps_remaining = ceil(step_dist_remaining); // Round-up current steps remaining - float last_n_steps_remaining = ceil(prep.steps_remaining); // Round-up last steps remaining - prep_segment->n_step = last_n_steps_remaining-n_steps_remaining; // Compute number of steps to execute. + // Segment complete! Increment segment buffer indices, so stepper ISR can immediately execute it. + segment_buffer_head = segment_next_head; + if (++segment_next_head == SEGMENT_BUFFER_SIZE) { + segment_next_head = 0; + } - // Bail if we are at the end of a feed hold and don't have a step to execute. - if (prep_segment->n_step == 0) { - if (sys.step_control & STEP_CONTROL_EXECUTE_HOLD) { - // Less than one step to decelerate to zero speed, but already very close. AMASS - // requires full steps to execute. So, just bail. - bit_true(sys.step_control,STEP_CONTROL_END_MOTION); - #ifdef PARKING_ENABLE - if (!(prep.recalculate_flag & PREP_FLAG_PARKING)) { prep.recalculate_flag |= PREP_FLAG_HOLD_PARTIAL_BLOCK; } - #endif - return; // Segment not generated, but current step data still retained. - } + // Update the appropriate planner and segment data. + pl_block->millimeters = mm_remaining; + prep.steps_remaining = n_steps_remaining; + prep.dt_remainder = (n_steps_remaining - step_dist_remaining) * inv_rate; + + // Check for exit conditions and flag to load next planner block. + if (mm_remaining == prep.mm_complete) { + // End of planner block or forced-termination. No more distance to be executed. + if (mm_remaining > 0.0) { // At end of forced-termination. + // Reset prep parameters for resuming and then bail. Allow the stepper ISR to complete + // the segment queue, where realtime protocol will set new state upon receiving the + // cycle stop flag from the ISR. Prep_segment is blocked until then. + bit_true(sys.step_control, STEP_CONTROL_END_MOTION); +#ifdef PARKING_ENABLE + if (!(prep.recalculate_flag & PREP_FLAG_PARKING)) { + prep.recalculate_flag |= PREP_FLAG_HOLD_PARTIAL_BLOCK; + } +#endif + return; // Bail! + } else { // End of planner block + // The planner block is complete. All steps are set to be executed in the segment buffer. + if (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION) { + bit_true(sys.step_control, STEP_CONTROL_END_MOTION); + return; + } + pl_block = NULL; // Set pointer to indicate check and load next planner block. + plan_discard_current_block(); + } + } } - - // Compute segment step rate. Since steps are integers and mm distances traveled are not, - // the end of every segment can have a partial step of varying magnitudes that are not - // executed, because the stepper ISR requires whole steps due to the AMASS algorithm. To - // compensate, we track the time to execute the previous segment's partial step and simply - // apply it with the partial step distance to the current segment, so that it minutely - // adjusts the whole segment rate to keep step output exact. These rate adjustments are - // typically very small and do not adversely effect performance, but ensures that Grbl - // outputs the exact acceleration and velocity profiles as computed by the planner. - dt += prep.dt_remainder; // Apply previous segment partial step execute time - float inv_rate = dt/(last_n_steps_remaining - step_dist_remaining); // Compute adjusted step rate inverse - - // Compute CPU cycles per step for the prepped segment. - uint32_t cycles = ceil( (TICKS_PER_MICROSECOND*1000000*60)*inv_rate ); // (cycles/step) - - #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING - // Compute step timing and multi-axis smoothing level. - // NOTE: AMASS overdrives the timer with each level, so only one prescalar is required. - if (cycles < AMASS_LEVEL1) { prep_segment->amass_level = 0; } - else { - if (cycles < AMASS_LEVEL2) { prep_segment->amass_level = 1; } - else if (cycles < AMASS_LEVEL3) { prep_segment->amass_level = 2; } - else { prep_segment->amass_level = 3; } - cycles >>= prep_segment->amass_level; - prep_segment->n_step <<= prep_segment->amass_level; - } - if (cycles < (1UL << 16)) { prep_segment->cycles_per_tick = cycles; } // < 65536 (4.1ms @ 16MHz) - else { prep_segment->cycles_per_tick = 0xffff; } // Just set the slowest speed possible. - #else - // Compute step timing and timer prescalar for normal step generation. - if (cycles < (1UL << 16)) { // < 65536 (4.1ms @ 16MHz) - prep_segment->prescaler = 1; // prescaler: 0 - prep_segment->cycles_per_tick = cycles; - } else if (cycles < (1UL << 19)) { // < 524288 (32.8ms@16MHz) - prep_segment->prescaler = 2; // prescaler: 8 - prep_segment->cycles_per_tick = cycles >> 3; - } else { - prep_segment->prescaler = 3; // prescaler: 64 - if (cycles < (1UL << 22)) { // < 4194304 (262ms@16MHz) - prep_segment->cycles_per_tick = cycles >> 6; - } else { // Just set the slowest speed possible. (Around 4 step/sec.) - prep_segment->cycles_per_tick = 0xffff; - } - } - #endif - - // Segment complete! Increment segment buffer indices, so stepper ISR can immediately execute it. - segment_buffer_head = segment_next_head; - if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; } - - // Update the appropriate planner and segment data. - pl_block->millimeters = mm_remaining; - prep.steps_remaining = n_steps_remaining; - prep.dt_remainder = (n_steps_remaining - step_dist_remaining)*inv_rate; - - // Check for exit conditions and flag to load next planner block. - if (mm_remaining == prep.mm_complete) { - // End of planner block or forced-termination. No more distance to be executed. - if (mm_remaining > 0.0) { // At end of forced-termination. - // Reset prep parameters for resuming and then bail. Allow the stepper ISR to complete - // the segment queue, where realtime protocol will set new state upon receiving the - // cycle stop flag from the ISR. Prep_segment is blocked until then. - bit_true(sys.step_control,STEP_CONTROL_END_MOTION); - #ifdef PARKING_ENABLE - if (!(prep.recalculate_flag & PREP_FLAG_PARKING)) { prep.recalculate_flag |= PREP_FLAG_HOLD_PARTIAL_BLOCK; } - #endif - return; // Bail! - } else { // End of planner block - // The planner block is complete. All steps are set to be executed in the segment buffer. - if (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION) { - bit_true(sys.step_control,STEP_CONTROL_END_MOTION); - return; - } - pl_block = NULL; // Set pointer to indicate check and load next planner block. - plan_discard_current_block(); - } - } - - } } - // Called by realtime status reporting to fetch the current speed being executed. This value // however is not exactly the current speed, but the speed computed in the last step segment // in the segment buffer. It will always be behind by up to the number of segment blocks (-1) // divided by the ACCELERATION TICKS PER SECOND in seconds. -float st_get_realtime_rate() -{ - if (sys.state & (STATE_CYCLE | STATE_HOMING | STATE_HOLD | STATE_JOG | STATE_SAFETY_DOOR)){ - return prep.current_speed; - } - return 0.0f; +float st_get_realtime_rate() { + if (sys.state & (STATE_CYCLE | STATE_HOMING | STATE_HOLD | STATE_JOG | STATE_SAFETY_DOOR)) { + return prep.current_speed; + } + return 0.0f; } diff --git a/grbl/stepper.h b/grbl/stepper.h index 41871a6..05039af 100644 --- a/grbl/stepper.h +++ b/grbl/stepper.h @@ -23,7 +23,7 @@ #define stepper_h #ifndef SEGMENT_BUFFER_SIZE - #define SEGMENT_BUFFER_SIZE 6 +#define SEGMENT_BUFFER_SIZE 6 #endif // Initialize and setup the stepper motor subsystem diff --git a/grbl/system.c b/grbl/system.c index 55e6c40..b006df8 100644 --- a/grbl/system.c +++ b/grbl/system.c @@ -20,98 +20,95 @@ #include "grbl.h" - -void system_init() -{ - CONTROL_DDR &= ~(CONTROL_MASK); // Configure as input pins - #ifdef DISABLE_CONTROL_PIN_PULL_UP +void system_init() { + CONTROL_DDR &= ~(CONTROL_MASK); // Configure as input pins +#ifdef DISABLE_CONTROL_PIN_PULL_UP CONTROL_PORT &= ~(CONTROL_MASK); // Normal low operation. Requires external pull-down. - #else - CONTROL_PORT |= CONTROL_MASK; // Enable internal pull-up resistors. Normal high operation. - #endif - CONTROL_PCMSK |= CONTROL_MASK; // Enable specific pins of the Pin Change Interrupt - PCICR |= (1 << CONTROL_INT); // Enable Pin Change Interrupt +#else + CONTROL_PORT |= CONTROL_MASK; // Enable internal pull-up resistors. Normal high operation. +#endif + CONTROL_PCMSK |= CONTROL_MASK; // Enable specific pins of the Pin Change Interrupt + PCICR |= (1 << CONTROL_INT); // Enable Pin Change Interrupt } - // Returns control pin state as a uint8 bitfield. Each bit indicates the input pin state, where // triggered is 1 and not triggered is 0. Invert mask is applied. Bitfield organization is // defined by the CONTROL_PIN_INDEX in the header file. -uint8_t system_control_get_state() -{ - uint8_t control_state = 0; - uint8_t pin = (CONTROL_PIN & CONTROL_MASK) ^ CONTROL_MASK; - #ifdef INVERT_CONTROL_PIN_MASK +uint8_t system_control_get_state() { + uint8_t control_state = 0; + uint8_t pin = (CONTROL_PIN & CONTROL_MASK) ^ CONTROL_MASK; +#ifdef INVERT_CONTROL_PIN_MASK pin ^= INVERT_CONTROL_PIN_MASK; - #endif - if (pin) { - #ifdef ENABLE_SAFETY_DOOR_INPUT_PIN - if (bit_istrue(pin,(1< 255)) { return(STATUS_INVALID_STATEMENT); } - return(settings_store_global_setting((uint8_t)parameter, value)); - } - } - } - return(STATUS_OK); // If '$' command makes it to here, then everything's ok. + system_set_exec_state_flag(EXEC_SLEEP); // Set to execute sleep mode immediately + break; + case 'I': // Print or store build info. [IDLE/ALARM] + if (line[++char_counter] == 0) { + settings_read_build_info(line); + report_build_info(line); +#ifdef ENABLE_BUILD_INFO_WRITE_COMMAND + } else { // Store startup line [IDLE/ALARM] + if (line[char_counter++] != '=') { + return (STATUS_INVALID_STATEMENT); + } + helper_var = char_counter; // Set helper variable as counter to start of user info line. + do { + line[char_counter - helper_var] = line[char_counter]; + } while (line[char_counter++] != 0); + settings_store_build_info(line); +#endif + } + break; + case 'R': // Restore defaults [IDLE/ALARM] + if ((line[2] != 'S') || (line[3] != 'T') || (line[4] != '=') || (line[6] != 0)) { + return (STATUS_INVALID_STATEMENT); + } + switch (line[5]) { +#ifdef ENABLE_RESTORE_EEPROM_DEFAULT_SETTINGS + case '$': settings_restore(SETTINGS_RESTORE_DEFAULTS); break; +#endif +#ifdef ENABLE_RESTORE_EEPROM_CLEAR_PARAMETERS + case '#': settings_restore(SETTINGS_RESTORE_PARAMETERS); break; +#endif +#ifdef ENABLE_RESTORE_EEPROM_WIPE_ALL + case '*': settings_restore(SETTINGS_RESTORE_ALL); break; +#endif + default: return (STATUS_INVALID_STATEMENT); + } + report_feedback_message(MESSAGE_RESTORE_DEFAULTS); + mc_reset(); // Force reset to ensure settings are initialized correctly. + break; + case 'N': // Startup lines. [IDLE/ALARM] + if (line[++char_counter] == 0) { // Print startup lines + for (helper_var = 0; helper_var < N_STARTUP_LINE; helper_var++) { + if (!(settings_read_startup_line(helper_var, line))) { + report_status_message(STATUS_SETTING_READ_FAIL); + } else { + report_startup_line(helper_var, line); + } + } + break; + } else { // Store startup line [IDLE Only] Prevents motion during ALARM. + if (sys.state != STATE_IDLE) { + return (STATUS_IDLE_ERROR); + } // Store only when idle. + helper_var = true; // Set helper_var to flag storing method. + // No break. Continues into default: to read remaining command characters. + } + default: // Storing setting methods [IDLE/ALARM] + if (!read_float(line, &char_counter, ¶meter)) { + return (STATUS_BAD_NUMBER_FORMAT); + } + if (line[char_counter++] != '=') { + return (STATUS_INVALID_STATEMENT); + } + if (helper_var) { // Store startup line + // Prepare sending gcode block to gcode parser by shifting all characters + helper_var = char_counter; // Set helper variable as counter to start of gcode block + do { + line[char_counter - helper_var] = line[char_counter]; + } while (line[char_counter++] != 0); + // Execute gcode block to ensure block is valid. + helper_var = gc_execute_line(line); // Set helper_var to returned status code. + if (helper_var) { + return (helper_var); + } else { + helper_var = trunc(parameter); // Set helper_var to int value of parameter + settings_store_startup_line(helper_var, line); + } + } else { // Store global setting. + if (!read_float(line, &char_counter, &value)) { + return (STATUS_BAD_NUMBER_FORMAT); + } + if ((line[char_counter] != 0) || (parameter > 255)) { + return (STATUS_INVALID_STATEMENT); + } + return (settings_store_global_setting((uint8_t)parameter, value)); + } + } + } + return (STATUS_OK); // If '$' command makes it to here, then everything's ok. } - - -void system_flag_wco_change() -{ - #ifdef FORCE_BUFFER_SYNC_DURING_WCO_CHANGE +void system_flag_wco_change() { +#ifdef FORCE_BUFFER_SYNC_DURING_WCO_CHANGE protocol_buffer_synchronize(); - #endif - sys.report_wco_counter = 0; +#endif + sys.report_wco_counter = 0; } - // Returns machine position of axis 'idx'. Must be sent a 'step' array. // NOTE: If motor steps and machine position are not in the same coordinate frame, this function // serves as a central place to compute the transformation. -float system_convert_axis_steps_to_mpos(int32_t *steps, uint8_t idx) -{ - float pos; - #ifdef COREXY - if (idx==X_AXIS) { - pos = (float)system_convert_corexy_to_x_axis_steps(steps) / settings.steps_per_mm[idx]; - } else if (idx==Y_AXIS) { - pos = (float)system_convert_corexy_to_y_axis_steps(steps) / settings.steps_per_mm[idx]; +float system_convert_axis_steps_to_mpos(int32_t *steps, uint8_t idx) { + float pos; +#ifdef COREXY + if (idx == X_AXIS) { + pos = (float)system_convert_corexy_to_x_axis_steps(steps) / settings.steps_per_mm[idx]; + } else if (idx == Y_AXIS) { + pos = (float)system_convert_corexy_to_y_axis_steps(steps) / settings.steps_per_mm[idx]; } else { - pos = steps[idx]/settings.steps_per_mm[idx]; + pos = steps[idx] / settings.steps_per_mm[idx]; } - #else - pos = steps[idx]/settings.steps_per_mm[idx]; - #endif - return(pos); +#else + pos = steps[idx] / settings.steps_per_mm[idx]; +#endif + return (pos); } - -void system_convert_array_steps_to_mpos(float *position, int32_t *steps) -{ - uint8_t idx; - for (idx=0; idx -settings.max_travel[idx]) { return(true); } - } else { - if (target[idx] > 0 || target[idx] < settings.max_travel[idx]) { return(true); } - } - #else - // NOTE: max_travel is stored as negative - if (target[idx] > 0 || target[idx] < settings.max_travel[idx]) { return(true); } - #endif - } - return(false); +int32_t system_convert_corexy_to_x_axis_steps(int32_t *steps) { + return ((steps[A_MOTOR] + steps[B_MOTOR]) / 2); } +int32_t system_convert_corexy_to_y_axis_steps(int32_t *steps) { + return ((steps[A_MOTOR] - steps[B_MOTOR]) / 2); +} +#endif + +// Checks and reports if target array exceeds machine travel limits. +uint8_t system_check_travel_limits(float *target) { + uint8_t idx; + for (idx = 0; idx < N_AXIS; idx++) { +#ifdef HOMING_FORCE_SET_ORIGIN + // When homing forced set origin is enabled, soft limits checks need to account for directionality. + // NOTE: max_travel is stored as negative + if (bit_istrue(settings.homing_dir_mask, bit(idx))) { + if (target[idx] < 0 || target[idx] > -settings.max_travel[idx]) { + return (true); + } + } else { + if (target[idx] > 0 || target[idx] < settings.max_travel[idx]) { + return (true); + } + } +#else + // NOTE: max_travel is stored as negative + if (target[idx] > 0 || target[idx] < settings.max_travel[idx]) { + return (true); + } +#endif + } + return (false); +} // Special handlers for setting and clearing Grbl's real-time execution flags. void system_set_exec_state_flag(uint8_t mask) { - uint8_t sreg = SREG; - cli(); - sys_rt_exec_state |= (mask); - SREG = sreg; + uint8_t sreg = SREG; + cli(); + sys_rt_exec_state |= (mask); + SREG = sreg; } void system_clear_exec_state_flag(uint8_t mask) { - uint8_t sreg = SREG; - cli(); - sys_rt_exec_state &= ~(mask); - SREG = sreg; + uint8_t sreg = SREG; + cli(); + sys_rt_exec_state &= ~(mask); + SREG = sreg; } void system_set_exec_alarm(uint8_t code) { - uint8_t sreg = SREG; - cli(); - sys_rt_exec_alarm = code; - SREG = sreg; + uint8_t sreg = SREG; + cli(); + sys_rt_exec_alarm = code; + SREG = sreg; } void system_clear_exec_alarm() { - uint8_t sreg = SREG; - cli(); - sys_rt_exec_alarm = 0; - SREG = sreg; + uint8_t sreg = SREG; + cli(); + sys_rt_exec_alarm = 0; + SREG = sreg; } void system_set_exec_motion_override_flag(uint8_t mask) { - uint8_t sreg = SREG; - cli(); - sys_rt_exec_motion_override |= (mask); - SREG = sreg; + uint8_t sreg = SREG; + cli(); + sys_rt_exec_motion_override |= (mask); + SREG = sreg; } void system_set_exec_accessory_override_flag(uint8_t mask) { - uint8_t sreg = SREG; - cli(); - sys_rt_exec_accessory_override |= (mask); - SREG = sreg; + uint8_t sreg = SREG; + cli(); + sys_rt_exec_accessory_override |= (mask); + SREG = sreg; } void system_clear_exec_motion_overrides() { - uint8_t sreg = SREG; - cli(); - sys_rt_exec_motion_override = 0; - SREG = sreg; + uint8_t sreg = SREG; + cli(); + sys_rt_exec_motion_override = 0; + SREG = sreg; } void system_clear_exec_accessory_overrides() { - uint8_t sreg = SREG; - cli(); - sys_rt_exec_accessory_override = 0; - SREG = sreg; + uint8_t sreg = SREG; + cli(); + sys_rt_exec_accessory_override = 0; + SREG = sreg; } diff --git a/grbl/system.h b/grbl/system.h index cfc9273..8df7ef2 100644 --- a/grbl/system.h +++ b/grbl/system.h @@ -28,141 +28,144 @@ // NOTE: The system executor uses an unsigned 8-bit volatile variable (8 flag limit.) The default // flags are always false, so the realtime protocol only needs to check for a non-zero value to // know when there is a realtime command to execute. -#define EXEC_STATUS_REPORT bit(0) // bitmask 00000001 -#define EXEC_CYCLE_START bit(1) // bitmask 00000010 -#define EXEC_CYCLE_STOP bit(2) // bitmask 00000100 -#define EXEC_FEED_HOLD bit(3) // bitmask 00001000 -#define EXEC_RESET bit(4) // bitmask 00010000 -#define EXEC_SAFETY_DOOR bit(5) // bitmask 00100000 -#define EXEC_MOTION_CANCEL bit(6) // bitmask 01000000 -#define EXEC_SLEEP bit(7) // bitmask 10000000 +#define EXEC_STATUS_REPORT bit(0) // bitmask 00000001 +#define EXEC_CYCLE_START bit(1) // bitmask 00000010 +#define EXEC_CYCLE_STOP bit(2) // bitmask 00000100 +#define EXEC_FEED_HOLD bit(3) // bitmask 00001000 +#define EXEC_RESET bit(4) // bitmask 00010000 +#define EXEC_SAFETY_DOOR bit(5) // bitmask 00100000 +#define EXEC_MOTION_CANCEL bit(6) // bitmask 01000000 +#define EXEC_SLEEP bit(7) // bitmask 10000000 // Alarm executor codes. Valid values (1-255). Zero is reserved. -#define EXEC_ALARM_HARD_LIMIT 1 -#define EXEC_ALARM_SOFT_LIMIT 2 -#define EXEC_ALARM_ABORT_CYCLE 3 -#define EXEC_ALARM_PROBE_FAIL_INITIAL 4 -#define EXEC_ALARM_PROBE_FAIL_CONTACT 5 -#define EXEC_ALARM_HOMING_FAIL_RESET 6 -#define EXEC_ALARM_HOMING_FAIL_DOOR 7 -#define EXEC_ALARM_HOMING_FAIL_PULLOFF 8 -#define EXEC_ALARM_HOMING_FAIL_APPROACH 9 -#define EXEC_ALARM_HOMING_FAIL_DUAL_APPROACH 10 +#define EXEC_ALARM_HARD_LIMIT 1 +#define EXEC_ALARM_SOFT_LIMIT 2 +#define EXEC_ALARM_ABORT_CYCLE 3 +#define EXEC_ALARM_PROBE_FAIL_INITIAL 4 +#define EXEC_ALARM_PROBE_FAIL_CONTACT 5 +#define EXEC_ALARM_HOMING_FAIL_RESET 6 +#define EXEC_ALARM_HOMING_FAIL_DOOR 7 +#define EXEC_ALARM_HOMING_FAIL_PULLOFF 8 +#define EXEC_ALARM_HOMING_FAIL_APPROACH 9 +#define EXEC_ALARM_HOMING_FAIL_DUAL_APPROACH 10 // Override bit maps. Realtime bitflags to control feed, rapid, spindle, and coolant overrides. // Spindle/coolant and feed/rapids are separated into two controlling flag variables. -#define EXEC_FEED_OVR_RESET bit(0) -#define EXEC_FEED_OVR_COARSE_PLUS bit(1) -#define EXEC_FEED_OVR_COARSE_MINUS bit(2) -#define EXEC_FEED_OVR_FINE_PLUS bit(3) -#define EXEC_FEED_OVR_FINE_MINUS bit(4) -#define EXEC_RAPID_OVR_RESET bit(5) -#define EXEC_RAPID_OVR_MEDIUM bit(6) -#define EXEC_RAPID_OVR_LOW bit(7) +#define EXEC_FEED_OVR_RESET bit(0) +#define EXEC_FEED_OVR_COARSE_PLUS bit(1) +#define EXEC_FEED_OVR_COARSE_MINUS bit(2) +#define EXEC_FEED_OVR_FINE_PLUS bit(3) +#define EXEC_FEED_OVR_FINE_MINUS bit(4) +#define EXEC_RAPID_OVR_RESET bit(5) +#define EXEC_RAPID_OVR_MEDIUM bit(6) +#define EXEC_RAPID_OVR_LOW bit(7) // #define EXEC_RAPID_OVR_EXTRA_LOW bit(*) // *NOT SUPPORTED* -#define EXEC_SPINDLE_OVR_RESET bit(0) -#define EXEC_SPINDLE_OVR_COARSE_PLUS bit(1) -#define EXEC_SPINDLE_OVR_COARSE_MINUS bit(2) -#define EXEC_SPINDLE_OVR_FINE_PLUS bit(3) -#define EXEC_SPINDLE_OVR_FINE_MINUS bit(4) -#define EXEC_SPINDLE_OVR_STOP bit(5) -#define EXEC_COOLANT_FLOOD_OVR_TOGGLE bit(6) -#define EXEC_COOLANT_MIST_OVR_TOGGLE bit(7) +#define EXEC_SPINDLE_OVR_RESET bit(0) +#define EXEC_SPINDLE_OVR_COARSE_PLUS bit(1) +#define EXEC_SPINDLE_OVR_COARSE_MINUS bit(2) +#define EXEC_SPINDLE_OVR_FINE_PLUS bit(3) +#define EXEC_SPINDLE_OVR_FINE_MINUS bit(4) +#define EXEC_SPINDLE_OVR_STOP bit(5) +#define EXEC_COOLANT_FLOOD_OVR_TOGGLE bit(6) +#define EXEC_COOLANT_MIST_OVR_TOGGLE bit(7) // Define system state bit map. The state variable primarily tracks the individual functions // of Grbl to manage each without overlapping. It is also used as a messaging flag for // critical events. -#define STATE_IDLE 0 // Must be zero. No flags. -#define STATE_ALARM bit(0) // In alarm state. Locks out all g-code processes. Allows settings access. -#define STATE_CHECK_MODE bit(1) // G-code check mode. Locks out planner and motion only. -#define STATE_HOMING bit(2) // Performing homing cycle -#define STATE_CYCLE bit(3) // Cycle is running or motions are being executed. -#define STATE_HOLD bit(4) // Active feed hold -#define STATE_JOG bit(5) // Jogging mode. -#define STATE_SAFETY_DOOR bit(6) // Safety door is ajar. Feed holds and de-energizes system. -#define STATE_SLEEP bit(7) // Sleep state. +#define STATE_IDLE 0 // Must be zero. No flags. +#define STATE_ALARM bit(0) // In alarm state. Locks out all g-code processes. Allows settings access. +#define STATE_CHECK_MODE bit(1) // G-code check mode. Locks out planner and motion only. +#define STATE_HOMING bit(2) // Performing homing cycle +#define STATE_CYCLE bit(3) // Cycle is running or motions are being executed. +#define STATE_HOLD bit(4) // Active feed hold +#define STATE_JOG bit(5) // Jogging mode. +#define STATE_SAFETY_DOOR bit(6) // Safety door is ajar. Feed holds and de-energizes system. +#define STATE_SLEEP bit(7) // Sleep state. // Define system suspend flags. Used in various ways to manage suspend states and procedures. -#define SUSPEND_DISABLE 0 // Must be zero. -#define SUSPEND_HOLD_COMPLETE bit(0) // Indicates initial feed hold is complete. -#define SUSPEND_RESTART_RETRACT bit(1) // Flag to indicate a retract from a restore parking motion. -#define SUSPEND_RETRACT_COMPLETE bit(2) // (Safety door only) Indicates retraction and de-energizing is complete. -#define SUSPEND_INITIATE_RESTORE bit(3) // (Safety door only) Flag to initiate resume procedures from a cycle start. -#define SUSPEND_RESTORE_COMPLETE bit(4) // (Safety door only) Indicates ready to resume normal operation. -#define SUSPEND_SAFETY_DOOR_AJAR bit(5) // Tracks safety door state for resuming. -#define SUSPEND_MOTION_CANCEL bit(6) // Indicates a canceled resume motion. Currently used by probing routine. -#define SUSPEND_JOG_CANCEL bit(7) // Indicates a jog cancel in process and to reset buffers when complete. +#define SUSPEND_DISABLE 0 // Must be zero. +#define SUSPEND_HOLD_COMPLETE bit(0) // Indicates initial feed hold is complete. +#define SUSPEND_RESTART_RETRACT bit(1) // Flag to indicate a retract from a restore parking motion. +#define SUSPEND_RETRACT_COMPLETE bit(2) // (Safety door only) Indicates retraction and de-energizing is complete. +#define SUSPEND_INITIATE_RESTORE bit(3) // (Safety door only) Flag to initiate resume procedures from a cycle start. +#define SUSPEND_RESTORE_COMPLETE bit(4) // (Safety door only) Indicates ready to resume normal operation. +#define SUSPEND_SAFETY_DOOR_AJAR bit(5) // Tracks safety door state for resuming. +#define SUSPEND_MOTION_CANCEL bit(6) // Indicates a canceled resume motion. Currently used by probing routine. +#define SUSPEND_JOG_CANCEL bit(7) // Indicates a jog cancel in process and to reset buffers when complete. // Define step segment generator state flags. -#define STEP_CONTROL_NORMAL_OP 0 // Must be zero. -#define STEP_CONTROL_END_MOTION bit(0) -#define STEP_CONTROL_EXECUTE_HOLD bit(1) -#define STEP_CONTROL_EXECUTE_SYS_MOTION bit(2) -#define STEP_CONTROL_UPDATE_SPINDLE_PWM bit(3) +#define STEP_CONTROL_NORMAL_OP 0 // Must be zero. +#define STEP_CONTROL_END_MOTION bit(0) +#define STEP_CONTROL_EXECUTE_HOLD bit(1) +#define STEP_CONTROL_EXECUTE_SYS_MOTION bit(2) +#define STEP_CONTROL_UPDATE_SPINDLE_PWM bit(3) // Define control pin index for Grbl internal use. Pin maps may change, but these values don't. #ifdef ENABLE_SAFETY_DOOR_INPUT_PIN - #define N_CONTROL_PIN 4 - #define CONTROL_PIN_INDEX_SAFETY_DOOR bit(0) - #define CONTROL_PIN_INDEX_RESET bit(1) - #define CONTROL_PIN_INDEX_FEED_HOLD bit(2) - #define CONTROL_PIN_INDEX_CYCLE_START bit(3) +#define N_CONTROL_PIN 4 +#define CONTROL_PIN_INDEX_SAFETY_DOOR bit(0) +#define CONTROL_PIN_INDEX_RESET bit(1) +#define CONTROL_PIN_INDEX_FEED_HOLD bit(2) +#define CONTROL_PIN_INDEX_CYCLE_START bit(3) #else - #define N_CONTROL_PIN 3 - #define CONTROL_PIN_INDEX_RESET bit(0) - #define CONTROL_PIN_INDEX_FEED_HOLD bit(1) - #define CONTROL_PIN_INDEX_CYCLE_START bit(2) +#define N_CONTROL_PIN 3 +#define CONTROL_PIN_INDEX_RESET bit(0) +#define CONTROL_PIN_INDEX_FEED_HOLD bit(1) +#define CONTROL_PIN_INDEX_CYCLE_START bit(2) #endif // Define spindle stop override control states. -#define SPINDLE_STOP_OVR_DISABLED 0 // Must be zero. -#define SPINDLE_STOP_OVR_ENABLED bit(0) -#define SPINDLE_STOP_OVR_INITIATE bit(1) -#define SPINDLE_STOP_OVR_RESTORE bit(2) -#define SPINDLE_STOP_OVR_RESTORE_CYCLE bit(3) - +#define SPINDLE_STOP_OVR_DISABLED 0 // Must be zero. +#define SPINDLE_STOP_OVR_ENABLED bit(0) +#define SPINDLE_STOP_OVR_INITIATE bit(1) +#define SPINDLE_STOP_OVR_RESTORE bit(2) +#define SPINDLE_STOP_OVR_RESTORE_CYCLE bit(3) // Define global system variables typedef struct { - uint8_t state; // Tracks the current system state of Grbl. - uint8_t abort; // System abort flag. Forces exit back to main loop for reset. - uint8_t suspend; // System suspend bitflag variable that manages holds, cancels, and safety door. - uint8_t soft_limit; // Tracks soft limit errors for the state machine. (boolean) - uint8_t step_control; // Governs the step segment generator depending on system state. - uint8_t probe_succeeded; // Tracks if last probing cycle was successful. - uint8_t homing_axis_lock; // Locks axes when limits engage. Used as an axis motion mask in the stepper ISR. - #ifdef ENABLE_DUAL_AXIS + uint8_t state; // Tracks the current system state of Grbl. + uint8_t abort; // System abort flag. Forces exit back to main loop for reset. + uint8_t suspend; // System suspend bitflag variable that manages holds, cancels, and safety door. + uint8_t soft_limit; // Tracks soft limit errors for the state machine. (boolean) + uint8_t step_control; // Governs the step segment generator depending on system state. + uint8_t probe_succeeded; // Tracks if last probing cycle was successful. + uint8_t homing_axis_lock; // Locks axes when limits engage. Used as an axis motion mask in the stepper ISR. +#ifdef ENABLE_DUAL_AXIS uint8_t homing_axis_lock_dual; - #endif - uint8_t f_override; // Feed rate override value in percent - uint8_t r_override; // Rapids override value in percent - uint8_t spindle_speed_ovr; // Spindle speed value in percent - uint8_t spindle_stop_ovr; // Tracks spindle stop override states - uint8_t report_ovr_counter; // Tracks when to add override data to status reports. - uint8_t report_wco_counter; // Tracks when to add work coordinate offset data to status reports. - #ifdef ENABLE_PARKING_OVERRIDE_CONTROL - uint8_t override_ctrl; // Tracks override control states. - #endif - #ifdef VARIABLE_SPINDLE +#endif + uint8_t f_override; // Feed rate override value in percent + uint8_t r_override; // Rapids override value in percent + uint8_t spindle_speed_ovr; // Spindle speed value in percent + uint8_t spindle_stop_ovr; // Tracks spindle stop override states + uint8_t report_ovr_counter; // Tracks when to add override data to status reports. + uint8_t report_wco_counter; // Tracks when to add work coordinate offset data to status reports. +#ifdef ENABLE_PARKING_OVERRIDE_CONTROL + uint8_t override_ctrl; // Tracks override control states. +#endif +#ifdef VARIABLE_SPINDLE float spindle_speed; - #endif +#endif } system_t; + extern system_t sys; // NOTE: These position variables may need to be declared as volatiles, if problems arise. -extern int32_t sys_position[N_AXIS]; // Real-time machine (aka home) position vector in steps. +extern int32_t sys_position[N_AXIS]; // Real-time machine (aka home) position vector in steps. extern int32_t sys_probe_position[N_AXIS]; // Last probe position in machine coordinates and steps. -extern volatile uint8_t sys_probe_state; // Probing state value. Used to coordinate the probing cycle with stepper ISR. -extern volatile uint8_t sys_rt_exec_state; // Global realtime executor bitflag variable for state management. See EXEC bitmasks. -extern volatile uint8_t sys_rt_exec_alarm; // Global realtime executor bitflag variable for setting various alarms. -extern volatile uint8_t sys_rt_exec_motion_override; // Global realtime executor bitflag variable for motion-based overrides. -extern volatile uint8_t sys_rt_exec_accessory_override; // Global realtime executor bitflag variable for spindle/coolant overrides. +extern volatile uint8_t sys_probe_state; // Probing state value. Used to coordinate the probing cycle with stepper ISR. +extern volatile uint8_t + sys_rt_exec_state; // Global realtime executor bitflag variable for state management. See EXEC bitmasks. +extern volatile uint8_t sys_rt_exec_alarm; // Global realtime executor bitflag variable for setting various alarms. +extern volatile uint8_t + sys_rt_exec_motion_override; // Global realtime executor bitflag variable for motion-based overrides. +extern volatile uint8_t + sys_rt_exec_accessory_override; // Global realtime executor bitflag variable for spindle/coolant overrides. #ifdef DEBUG - #define EXEC_DEBUG_REPORT bit(0) - extern volatile uint8_t sys_rt_exec_debug; +#define EXEC_DEBUG_REPORT bit(0) +extern volatile uint8_t sys_rt_exec_debug; #endif // Initialize the serial protocol @@ -180,7 +183,6 @@ uint8_t system_execute_line(char *line); // Execute the startup script lines stored in EEPROM upon initialization void system_execute_startup(char *line); - void system_flag_wco_change(); // Returns machine position of axis 'idx'. Must be sent a 'step' array. @@ -191,8 +193,8 @@ void system_convert_array_steps_to_mpos(float *position, int32_t *steps); // CoreXY calculation only. Returns x or y-axis "steps" based on CoreXY motor steps. #ifdef COREXY - int32_t system_convert_corexy_to_x_axis_steps(int32_t *steps); - int32_t system_convert_corexy_to_y_axis_steps(int32_t *steps); +int32_t system_convert_corexy_to_x_axis_steps(int32_t *steps); +int32_t system_convert_corexy_to_y_axis_steps(int32_t *steps); #endif // Checks and reports if target array exceeds machine travel limits. @@ -208,5 +210,4 @@ void system_set_exec_accessory_override_flag(uint8_t mask); void system_clear_exec_motion_overrides(); void system_clear_exec_accessory_overrides(); - #endif