From b3a53a46835c13fcd3c29997134f7ded72d28047 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Thu, 27 Aug 2015 21:37:19 -0600 Subject: [PATCH] v1.0 Beta Release. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Tons of new stuff in this release, which is fairly stable and well tested. However, much more is coming soon! - Real-time parking motion with safety door. When this compile option is enabled, an opened safety door will cause Grbl to automatically feed hold, retract, de-energize the spindle/coolant, and parks near Z max. After the door is closed and resume is commanded, this reverses and the program continues as if nothing happened. This is also highly configurable. See config.h for details. - New spindle max and min rpm ‘$’ settings! This has been requested often. Grbl will output 5V when commanded to turn on the spindle at its max rpm, and 0.02V with min rpm. The voltage and the rpm range are linear to each other. This should help users tweak their settings to get close to true rpm’s. - If the new max rpm ‘$’ setting is set = 0 or less than min rpm, the spindle speed PWM pin will act like a regular on/off spindle enable pin. On pin D11. - BEWARE: Your old EEPROM settings will be wiped! The new spindle rpm settings require a new settings version, so Grbl will automatically wipe and restore the EEPROM with the new defaults. - Control pin can now be inverted individually with a CONTROL_INVERT_MASK in the cpu_map header file. Not typical for users to need this, but handy to have. - Fixed bug when Grbl receive too many characters in a line and overflows. Previously it would respond with an error per overflow character and another acknowledge upon an EOL character. This broke the streaming protocol. Now fixed to only respond with an error after an EOL character. - Fixed a bug with the safety door during an ALARM mode. You now can’t home or unlock the axes until the safety door has been closed. This is for safety reasons (obviously.) - Tweaked some the Mega2560 cpu_map settings . Increased segment buffer size and fixed the spindle PWM settings to output at a higher PWM frequency. - Generalized the delay function used by G4 delay for use by parking motion. Allows non-blocking status reports and real-time control during re-energizing of the spindle and coolant. - Added spindle rpm max and min defaults to default.h files. - Added a new print float for rpm values. --- README.md | 68 +-- grbl/config.h | 63 +- grbl/coolant_control.c | 2 + grbl/cpu_map/cpu_map_atmega2560.h | 34 +- grbl/cpu_map/cpu_map_atmega328p.h | 21 +- grbl/defaults/defaults_generic.h | 2 + grbl/defaults/defaults_oxcnc.h | 2 + grbl/defaults/defaults_shapeoko.h | 2 + grbl/defaults/defaults_shapeoko2.h | 2 + grbl/defaults/defaults_shapeoko3.h | 2 + grbl/defaults/defaults_sherline.h | 2 + grbl/defaults/defaults_simulator.h | 2 + grbl/defaults/defaults_x_carve_1000mm.h | 2 + grbl/defaults/defaults_x_carve_500mm.h | 2 + grbl/defaults/defaults_zen_toolworks_7x7.h | 2 + grbl/grbl.h | 4 +- grbl/limits.c | 4 +- grbl/motion_control.c | 48 +- grbl/motion_control.h | 3 + grbl/nuts_bolts.c | 18 + grbl/nuts_bolts.h | 6 + grbl/planner.c | 92 +-- grbl/planner.h | 15 +- grbl/print.c | 1 + grbl/print.h | 4 +- grbl/protocol.c | 663 +++++++++++++-------- grbl/protocol.h | 1 + grbl/report.c | 12 +- grbl/report.h | 1 + grbl/settings.c | 6 + grbl/settings.h | 5 +- grbl/spindle_control.c | 95 +-- grbl/stepper.c | 331 ++++++---- grbl/stepper.h | 6 + grbl/system.c | 23 +- grbl/system.h | 24 +- 36 files changed, 972 insertions(+), 598 deletions(-) diff --git a/README.md b/README.md index 4d6d6a9..85f3575 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,10 @@ ![GitHub Logo](/doc/media/Grbl Logo 250px.png) +*** + +This is the development branch for Grbl v1.0's upcoming release. In general, the new features here are beta, so use with caution. If you'd like to help, please report any bugs or oddities that you find! Thanks! + *** Grbl is a no-compromise, high performance, low cost alternative to parallel-port-based motion control for CNC milling. It will run on a vanilla Arduino (Duemillanove/Uno) as long as it sports an Atmega 328. @@ -24,67 +28,21 @@ Grbl includes full acceleration management with look ahead. That means the contr ### Official Supporters of the Grbl CNC Project [![Carbide3D](http://carbide3d.com/files/logo_240px.png)](http://carbide3d.com) [![Inventables](https://dzevsq2emy08i.cloudfront.net/paperclip/press_image_uploads/62/low_res/inventables-logo.png)](http://inventables.com) -*** - -_**Master Branch:**_ -* [Grbl v0.9j Atmega328p 16mhz 115200baud with generic defaults](http://bit.ly/1I8Ey4S) _(2015-07-17)_ -* [Grbl v0.9j Atmega328p 16mhz 115200baud with ShapeOko2 defaults](http://bit.ly/1OjUSia) _(2015-07-17)_ - - **IMPORTANT INFO WHEN UPGRADING TO GRBL v0.9 :** - - Baudrate is now **115200** (Up from 9600). - - Homing cycle updated. Located based on switch trigger, rather than release point. - - Variable spindle is now enabled by default. Z-limit(D12) and spindle enable(D11) have switched to access the hardware PWM on D11. Homing will not work if you do not re-wire your Z-limit switch to D12. - -_**Archives:**_ -* [Grbl v0.9i Atmega328p 16mhz 115200baud with generic defaults](http://bit.ly/1EiviDk) -* [Grbl v0.9i Atmega328p 16mhz 115200baud with ShapeOko2 defaults](http://bit.ly/1NYIfKl) -* [Grbl v0.9g Atmega328p 16mhz 115200baud with generic defaults](http://bit.ly/1m8E1Qa) -* [Grbl v0.9g Atmega328p 16mhz 115200baud with ShapeOko2 defaults](http://bit.ly/1kOAzig) -* [Grbl v0.8c Atmega328p 16mhz 9600baud](http://bit.ly/SSdCJE) -* [Grbl v0.7d Atmega328p 16mhz 9600baud](http://bit.ly/ZhL15G) -* [Grbl v0.6b Atmega328p 16mhz 9600baud](http://bit.ly/VD04A5) -* [Grbl v0.51 Atmega328p 16mhz 9600baud](http://bit.ly/W75BS1) -* [Grbl v0.6b Atmega168 16mhz 9600baud](http://bit.ly/SScWnE) -* [Grbl v0.51 Atmega168 16mhz 9600baud](http://bit.ly/VXyrYu) - *** -##Update Summary for v0.9j - - **Restore EEPROM feature:** A new set of restore EEPROM features to help OEMs and users reset their Grbl installation to the build defaults. See Configuring Grbl Wiki for details. - -##Update Summary for v0.9i - - **IMPORTANT:** - - **Homing cycle updated. Locates based on trigger point, rather than release point.** - - **System tweaks: $14 cycle auto-start has been removed. No more QUEUE state.** - - **New G-Codes** - - **CoreXY Support** - - **Safety Door Support** - - **Full Limit and Control Pin Configurability** - - **Additional Compile-Time Feature Options** +##Update Summary for v1.0b +- **IMPORTANT:** Your EEPROM will be wiped and restored with new settings. This is due to the addition of two new spindle speed '$' settings. + +- New safety door parking motion as a compile-option. Grbl will retract, disable the spindle/coolant, and park near Z max. When resumed, it will perform these task in reverse order and continue the program. Highly configurable. See config.h for details. + +- New '$' Grbl settings for max and min spindle rpm. Allows for tweaking the PWM output to more closely match true spindle rpm. When max rpm is set to zero or less than min rpm, the PWM pin D11 will act like a simple enable on/off output. + +- A few bug fixes and lots of refactoring to make the code more efficient and flexible. -##Update Summary for v0.9h from v0.8 - - **IMPORTANT:** - - **Default serial baudrate is now 115200! (Up from 9600)** - - **Z-limit(D12) and spindle enable(D11) pins have switched to support variable spindle!** - - **Super Smooth Stepper Algorithm** - - **Stability and Robustness Updates** - - **(x4)+ Faster Planner** - - **Compile-able via Arduino IDE!** - - **G-Code Parser Overhaul** - - **Independent Acceleration and Velocity Settings** - - **Soft Limits** - - **Probing** - - **Dynamic Tool Length Offsets** - - **Improved Arc Performance** - - **CPU Pin Mapping** - - **New Grbl SIMULATOR! (by @jgeisler and @ashelly)** - - **Configurable Real-time Status Reporting** - - **Updated Homing Routine** - - **Optional Limit Pin Sharing** - - **Optional Variable Spindle Speed Output** - - **Additional Compile-Time Feature Options** - + ``` List of Supported G-Codes in Grbl v0.9 Master: - Non-Modal Commands: G4, G10L2, G10L20, G28, G30, G28.1, G30.1, G53, G92, G92.1 diff --git a/grbl/config.h b/grbl/config.h index e8a600b..d7eebe2 100644 --- a/grbl/config.h +++ b/grbl/config.h @@ -102,6 +102,7 @@ #define N_DECIMAL_RATEVALUE_INCH 1 // Rate or velocity value in in/min #define N_DECIMAL_RATEVALUE_MM 0 // Rate or velocity value in mm/min #define N_DECIMAL_SETTINGVALUE 3 // Decimals for floating point setting values +#define N_DECIMAL_RPMVALUE 0 // RPM value in rotations per min. // If your machine has two limits switches wired in parallel to one axis, you will need to enable // this feature. Since the two switches are sharing a single pin, there is no way for Grbl to tell @@ -138,9 +139,8 @@ // After the safety door switch has been toggled and restored, this setting sets the power-up delay // between restoring the spindle and coolant and resuming the cycle. -// NOTE: Delay value is defined in milliseconds from zero to 65,535. -#define SAFETY_DOOR_SPINDLE_DELAY 4000 -#define SAFETY_DOOR_COOLANT_DELAY 1000 +#define SAFETY_DOOR_SPINDLE_DELAY 4.0 // Float (seconds) +#define SAFETY_DOOR_COOLANT_DELAY 1.0 // Float (seconds) // Enable CoreXY kinematics. Use ONLY with CoreXY machines. // IMPORTANT: If homing is enabled, you must reconfigure the homing cycle #defines above to @@ -238,19 +238,14 @@ // The hardware PWM output on pin D11 is required for variable spindle output voltages. #define VARIABLE_SPINDLE // Default enabled. Comment to disable. -// Used by the variable spindle output only. These parameters set the maximum and minimum spindle speed -// "S" g-code values to correspond to the maximum and minimum pin voltages. There are 256 discrete and -// equally divided voltage bins between the maximum and minimum spindle speeds. So for a 5V pin, 1000 -// max rpm, and 250 min rpm, the spindle output voltage would be set for the following "S" commands: -// "S1000" @ 5V, "S250" @ 0.02V, and "S625" @ 2.5V (mid-range). The pin outputs 0V when disabled. -#define SPINDLE_MAX_RPM 1000.0 // Max spindle RPM. This value is equal to 100% duty cycle on the PWM. -#define SPINDLE_MIN_RPM 0.0 // Min spindle RPM. This value is equal to (1/256) duty cycle on the PWM. // Used by variable spindle output only. This forces the PWM output to a minimum duty cycle when enabled. -// When disabled, the PWM pin will still read 0V. Most users will not need this option, but it may be -// useful in certain scenarios. This setting does not update the minimum spindle RPM calculations. Any -// spindle RPM output lower than this value will be set to this value. -// #define MINIMUM_SPINDLE_PWM 5 // Default disabled. Uncomment to enable. Integer (0-255) +// The PWM pin will still read 0V when the spindle is disabled. Most users will not need this option, but +// it may be useful in certain scenarios. This minimum PWM settings coincides with the spindle rpm minimum +// setting, like rpm max to max PWM. So the variable spindle pin will not output the voltage range between +// 0V for disabled and the voltage set by the minimum PWM for minimum rpm. +// NOTE: Compute duty cycle at the minimum PWM by this equation: (% duty cycle)=(SPINDLE_MINIMUM_PWM/256)*100 +// #define SPINDLE_MINIMUM_PWM 5 // Default disabled. Uncomment to enable. Integer (0-255) // By default on a 328p(Uno), Grbl combines the variable spindle PWM and the enable into one pin to help // preserve I/O pins. For certain setups, these may need to be separate pins. This configure option uses @@ -383,6 +378,40 @@ // NOTE: This option has no effect if SOFTWARE_DEBOUNCE is enabled. // #define HARD_LIMIT_FORCE_STATE_CHECK // Default disabled. Uncomment to enable. +// Adjusts homing cycle search and locate scalars. These are the multipliers used by Grbl's +// homing cycle to ensure the limit switches are engaged and cleared through each phase of +// the cycle. The search phase uses the axes max-travel setting times the SEARCH_SCALAR to +// determine distance to look for the limit switch. Once found, the locate phase begins and +// uses the homing pull-off distance setting times the LOCATE_SCALAR to pull-off and re-engage +// the limit switch. +// NOTE: Both of these values must be greater than 1.0 to ensure proper function. +// #define HOMING_AXIS_SEARCH_SCALAR 1.5 // Uncomment to override defaults in limits.c. +// #define HOMING_AXIS_LOCATE_SCALAR 10.0 // Uncomment to override defaults in limits.c. + + +// Enables and configures parking motion methods upon a safety door state. Primarily for OEMs +// that desire this feature for their integrated machines. At the moment, Grbl assumes that +// the parking motion only involves one axis, although the parking implementation was written +// to be easily refactored for any number of motions on different axes by altering the parking +// source code. At this time, Grbl only supports parking one axis (typically the Z-axis) that +// moves in the positive direction upon retracting and negative direction upon restoring position. +// The motion executes with a slow pull-out retraction motion, power-down, and a fast park. +// Restoring to the resume position follows these set motions in reverse: fast restore to +// pull-out position, power-up with a time-out, and plunge back to the original position at the +// slower pull-out rate. +// NOTE: Still a work-in-progress. Machine coordinates must be in all negative space and +// does not work with HOMING_FORCE_SET_ORIGIN enabled. Parking motion also moves only in +// positive direction. +// #define PARKING_ENABLE // Default disabled. Uncomment to enable + +// Configure options for the parking motion, if enabled. +#define PARKING_AXIS Z_AXIS // Define which axis that performs the parking motion +#define PARKING_TARGET -5.0 // Parking axis target. In mm, as machine coordinate [-max_travel,0]. +#define PARKING_RATE -1.0 // Parking fast rate after pull-out. In mm/min or (-1.0) for seek rate. +#define PARKING_PULLOUT_RATE 250.0 // Pull-out/plunge slow feed rate in mm/min. +#define PARKING_PULLOUT_INCREMENT 5.0 // Spindle pull-out and plunge distance in mm. Incremental distance. + // Must be positive value or equal to zero. + // --------------------------------------------------------------------------------------- // COMPILE-TIME ERROR CHECKING OF DEFINE VALUES: @@ -399,6 +428,12 @@ #error "USE_SPINDLE_DIR_AS_ENABLE_PIN may only be used with a 328p processor" #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 +#endif + // --------------------------------------------------------------------------------------- diff --git a/grbl/coolant_control.c b/grbl/coolant_control.c index 4429dd4..bf6e3ee 100644 --- a/grbl/coolant_control.c +++ b/grbl/coolant_control.c @@ -42,6 +42,8 @@ void coolant_stop() void coolant_set_state(uint8_t mode) { + if (sys.abort) { return; } // Block during abort. + if (mode == COOLANT_FLOOD_ENABLE) { COOLANT_FLOOD_PORT |= (1 << COOLANT_FLOOD_BIT); diff --git a/grbl/cpu_map/cpu_map_atmega2560.h b/grbl/cpu_map/cpu_map_atmega2560.h index 0287f44..807ffae 100644 --- a/grbl/cpu_map/cpu_map_atmega2560.h +++ b/grbl/cpu_map/cpu_map_atmega2560.h @@ -33,10 +33,11 @@ #define SERIAL_UDRE USART0_UDRE_vect // Increase Buffers to make use of extra SRAM -//#define RX_BUFFER_SIZE 256 -//#define TX_BUFFER_SIZE 128 -//#define BLOCK_BUFFER_SIZE 36 -//#define LINE_BUFFER_SIZE 100 +//#define RX_BUFFER_SIZE 256 +//#define TX_BUFFER_SIZE 128 +//#define BLOCK_BUFFER_SIZE 36 +//#define LINE_BUFFER_SIZE 100 +//#define SEGMENT_BUFFER_SIZE 10 // Define step pulse output pins. NOTE: All step bit pins must be on the same port. #define STEP_DDR DDRA @@ -107,8 +108,8 @@ #define CONTROL_INT PCIE2 // Pin change interrupt enable pin #define CONTROL_INT_vect PCINT2_vect #define CONTROL_PCMSK PCMSK2 // Pin change interrupt register -#define CONTROL_MASK ((1< diff --git a/grbl/limits.c b/grbl/limits.c index 65869d1..8991ec5 100644 --- a/grbl/limits.c +++ b/grbl/limits.c @@ -203,9 +203,9 @@ void limits_go_home(uint8_t cycle_mask) // Perform homing cycle. Planner buffer should be empty, as required to initiate the homing cycle. #ifdef USE_LINE_NUMBERS - plan_buffer_line(target, homing_rate, false, HOMING_CYCLE_LINE_NUMBER); // Bypass mc_line(). Directly plan homing motion. + plan_buffer_line(target, homing_rate, false, false, HOMING_CYCLE_LINE_NUMBER); // Bypass mc_line(). Directly plan homing motion. #else - plan_buffer_line(target, homing_rate, false); // Bypass mc_line(). Directly plan homing motion. + plan_buffer_line(target, homing_rate, false, false); // Bypass mc_line(). Directly plan homing motion. #endif st_prep_buffer(); // Prep and fill segment buffer from newly planned block. diff --git a/grbl/motion_control.c b/grbl/motion_control.c index 0785200..f530b11 100644 --- a/grbl/motion_control.c +++ b/grbl/motion_control.c @@ -67,10 +67,11 @@ } while (1); // Plan and queue motion into planner buffer +// uint8_t plan_status; // Not used in normal operation. #ifdef USE_LINE_NUMBERS - plan_buffer_line(target, feed_rate, invert_feed_rate, line_number); + plan_buffer_line(target, feed_rate, invert_feed_rate, false, line_number); #else - plan_buffer_line(target, feed_rate, invert_feed_rate); + plan_buffer_line(target, feed_rate, invert_feed_rate, false); #endif } @@ -202,17 +203,9 @@ // Execute dwell in seconds. void mc_dwell(float seconds) { - if (sys.state == STATE_CHECK_MODE) { return; } - - uint16_t i = floor(1000/DWELL_TIME_STEP*seconds); - protocol_buffer_synchronize(); - delay_ms(floor(1000*seconds-i*DWELL_TIME_STEP)); // Delay millisecond remainder. - while (i-- > 0) { - // NOTE: Check and execute realtime commands during dwell every <= DWELL_TIME_STEP milliseconds. - protocol_execute_realtime(); - if (sys.abort) { return; } - _delay_ms(DWELL_TIME_STEP); // Delay DWELL_TIME_STEP increment - } + if (sys.state == STATE_CHECK_MODE) { return; } + protocol_buffer_synchronize(); + delay_sec(seconds, DELAY_MODE_DWELL); } @@ -334,6 +327,32 @@ void mc_homing_cycle() } +// Plans and executes the single special motion case for parking. Independent of main planner buffer. +// NOTE: Uses the always free planner ring buffer head to store motion parameters for execution. +void mc_parking_motion(float *parking_target, float feed_rate) +{ + if (sys.abort) { return; } // Block during abort. + + uint8_t plan_status = plan_buffer_line(parking_target, feed_rate, false, true); + if (plan_status) { + bit_true(sys.step_control, STEP_CONTROL_EXECUTE_PARK); + bit_false(sys.step_control, STEP_CONTROL_END_MOTION); // Allow parking motion to execute, if feed hold is active. + st_parking_setup_buffer(); // Setup step segment buffer for special parking motion case + st_prep_buffer(); + st_wake_up(); + do { + protocol_exec_rt_system(); + if (sys.abort) { return; } + } while (sys.step_control & STEP_CONTROL_EXECUTE_PARK); + st_parking_restore_buffer(); // Restore step segment buffer to normal run state. + } else { + bit_false(sys.step_control, STEP_CONTROL_EXECUTE_PARK); + protocol_exec_rt_system(); + } + +} + + // Method to ready the system to reset by setting the realtime reset command and killing any // active processes in the system. This also checks if a system reset is issued while Grbl // is in a motion state. If so, kills the steppers and sets the system alarm to flag position @@ -353,7 +372,8 @@ void mc_reset() // NOTE: If steppers are kept enabled via the step idle delay setting, this also keeps // the steppers enabled by avoiding the go_idle call altogether, unless the motion state is // violated, by which, all bets are off. - if ((sys.state & (STATE_CYCLE | STATE_HOMING)) || (sys.suspend == SUSPEND_ENABLE_HOLD)) { + if ((sys.state & (STATE_CYCLE | STATE_HOMING)) || + (sys.step_control & (STEP_CONTROL_EXECUTE_HOLD | STEP_CONTROL_EXECUTE_PARK))) { if (sys.state == STATE_HOMING) { bit_true_atomic(sys.rt_exec_alarm, EXEC_ALARM_HOMING_FAIL); } else { bit_true_atomic(sys.rt_exec_alarm, EXEC_ALARM_ABORT_CYCLE); } st_go_idle(); // Force kill steppers. Position has likely been lost. diff --git a/grbl/motion_control.h b/grbl/motion_control.h index 584d231..5c9517a 100644 --- a/grbl/motion_control.h +++ b/grbl/motion_control.h @@ -61,6 +61,9 @@ void mc_probe_cycle(float *target, float feed_rate, uint8_t invert_feed_rate, ui uint8_t is_no_error); #endif +// Plans and executes the single special motion case for parking. Independent of main planner buffer. +void mc_parking_motion(float *parking_target, float feed_rate); + // Performs system reset. If in motion state, kills all motion and sets system alarm. void mc_reset(); diff --git a/grbl/nuts_bolts.c b/grbl/nuts_bolts.c index b33e230..f893e29 100644 --- a/grbl/nuts_bolts.c +++ b/grbl/nuts_bolts.c @@ -108,6 +108,24 @@ uint8_t read_float(char *line, uint8_t *char_counter, float *float_ptr) } +// 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_SAFETY_DOOR + // 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) diff --git a/grbl/nuts_bolts.h b/grbl/nuts_bolts.h index 882d337..1f2cd07 100644 --- a/grbl/nuts_bolts.h +++ b/grbl/nuts_bolts.h @@ -44,6 +44,9 @@ #define INCH_PER_MM (0.0393701) #define TICKS_PER_MICROSECOND (F_CPU/1000000) +#define DELAY_MODE_DWELL 0 +#define DELAY_MODE_SAFETY_DOOR 1 + // Useful macros #define clear_vector(a) memset(a, 0, sizeof(a)) #define clear_vector_float(a) memset(a, 0.0, sizeof(float)*N_AXIS) @@ -66,6 +69,9 @@ // a pointer to the result variable. Returns true when it succeeds uint8_t read_float(char *line, uint8_t *char_counter, float *float_ptr); +// Non-blocking delay function used for general operation and suspend features. +void delay_sec(float seconds, uint8_t mode); + // Delays variable-defined milliseconds. Compiler compatibility fix for _delay_ms(). void delay_ms(uint16_t ms); diff --git a/grbl/planner.c b/grbl/planner.c index 1e0bd35..6bc32ee 100644 --- a/grbl/planner.c +++ b/grbl/planner.c @@ -219,6 +219,12 @@ void plan_discard_current_block() } +plan_block_t *plan_get_parking_block() +{ + return(&block_buffer[block_buffer_head]); +} + + plan_block_t *plan_get_current_block() { if (block_buffer_head == block_buffer_tail) { return(NULL); } // Buffer empty @@ -251,11 +257,15 @@ uint8_t plan_check_full_buffer() In other words, the buffer head is never equal to the buffer tail. Also the feed rate input value is used in three ways: as a normal feed rate if invert_feed_rate is false, as inverse time if invert_feed_rate is true, or as seek/rapids rate if the feed_rate value is negative (and - invert_feed_rate always false). */ + invert_feed_rate always false). + The is_parking_motion boolean tells the planner to plan a motion in the always unused block buffer + 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 parking motion. */ #ifdef USE_LINE_NUMBERS - void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate, int32_t line_number) + uint8_t plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate, uint8_t is_parking_motion, int32_t line_number) #else - void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) + uint8_t plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate, uint8_t is_parking_motion) #endif { // Prepare and initialize new block @@ -271,14 +281,19 @@ uint8_t plan_check_full_buffer() // Compute and store initial move distance data. // TODO: After this for-loop, we don't touch the stepper algorithm data. Might be a good idea // to try to keep these types of things completely separate from the planner for portability. - int32_t target_steps[N_AXIS]; + 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 (is_parking_motion) { memcpy(position_steps, sys.position, sizeof(sys.position)); } + 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]-pl.position[X_AXIS]) + (target_steps[Y_AXIS]-pl.position[Y_AXIS])); - block->steps[B_MOTOR] = labs((target_steps[X_AXIS]-pl.position[X_AXIS]) - (target_steps[Y_AXIS]-pl.position[Y_AXIS])); + 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]-pl.position[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]-pl.position[X_AXIS]) + (target_steps[Y_AXIS]-pl.position[Y_AXIS]))/settings.steps_per_mm[idx]; + 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]-pl.position[X_AXIS]) - (target_steps[Y_AXIS]-pl.position[Y_AXIS]))/settings.steps_per_mm[idx]; + 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] - pl.position[idx])/settings.steps_per_mm[idx]; + 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]-pl.position[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] - pl.position[idx])/settings.steps_per_mm[idx]; - #endif + delta_mm = (target_steps[idx] - position_steps[idx])/settings.steps_per_mm[idx]; + #endif unit_vec[idx] = delta_mm; // Store unit vector numerator. Denominator computed later. // Set direction bits. Bit enabled always means direction is negative. @@ -315,7 +330,7 @@ uint8_t plan_check_full_buffer() block->millimeters = sqrt(block->millimeters); // Complete millimeters calculation with sqrt() // Bail if this is a zero-length block. Highly unlikely to occur. - if (block->step_event_count == 0) { return; } + if (block->step_event_count == 0) { return(PLAN_EMPTY_BLOCK); } // Adjust feed_rate value to mm/min depending on type of rate input (normal, inverse time, or rapids) // TODO: Need to distinguish a rapids vs feed move for overrides. Some flag of some sort. @@ -329,7 +344,7 @@ uint8_t plan_check_full_buffer() // if they are also orthogonal/independent. Operates on the absolute value of the unit vector. float inverse_unit_vec_value; float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple float divides - float junction_cos_theta = 0; + float junction_cos_theta = 0.0; for (idx=0; idxentry_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. @@ -371,7 +387,7 @@ uint8_t plan_check_full_buffer() 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 @@ -388,31 +404,35 @@ uint8_t plan_check_full_buffer() // TODO: Technically, the acceleration used in calculation needs to be limited by the minimum of the // two junctions. However, this shouldn't be a significant problem except in extreme circumstances. block->max_junction_speed_sqr = max( MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED, - (block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2) ); + (block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2) ); } } - + // Store block nominal speed block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min). Always > 0 // Compute the junction maximum entry based on the minimum of the junction speed and neighboring nominal speeds. block->max_entry_speed_sqr = min(block->max_junction_speed_sqr, - min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr)); - - // Update previous path unit_vector and nominal speed (squared) - memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[] - pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; - - // Update planner position - memcpy(pl.position, target_steps, sizeof(target_steps)); // pl.position[] = target_steps[] + min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr)); - // 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); + // Block parking motion from updating this data to ensure next g-code motion is computed correctly. + if (!is_parking_motion) { + // Update previous path unit_vector and nominal speed (squared) + memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[] + pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; - // Finish up by recalculating the plan with the new block. - planner_recalculate(); + // Update planner position + 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); } @@ -424,7 +444,7 @@ void plan_sync_position() uint8_t idx; for (idx=0; idx= (LINE_BUFFER_SIZE-1)) { - // Detect line buffer overflow. Report error and reset line buffer. - report_status_message(STATUS_OVERFLOW); - comment = COMMENT_NONE; - char_counter = 0; + // 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; } } + } } @@ -171,209 +166,6 @@ void protocol_main_loop() } -// Executes run-time commands, when required. This 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 be more than a fraction of a second. -// This is a way to execute realtime commands asynchronously (aka multitasking) with grbl's g-code -// parsing and planning functions. This function also serves as an interface for the interrupts to -// set the system realtime flags, where only the main program handles them, removing the need to -// define more computationally-expensive volatile variables. This also provides a controlled way to -// execute certain tasks without having two or more instances of the same task, such as the planner -// recalculating the buffer upon a feedhold or override. -// 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() -{ - uint8_t rt_exec; // Temp variable to avoid calling volatile multiple times. - - do { // If system is suspended, suspend loop restarts here. - - // Check and execute alarms. - 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 - if (rt_exec & EXEC_ALARM_HARD_LIMIT) { - report_alarm_message(ALARM_HARD_LIMIT_ERROR); - } else if (rt_exec & EXEC_ALARM_SOFT_LIMIT) { - report_alarm_message(ALARM_SOFT_LIMIT_ERROR); - } else if (rt_exec & EXEC_ALARM_ABORT_CYCLE) { - report_alarm_message(ALARM_ABORT_CYCLE); - } else if (rt_exec & EXEC_ALARM_PROBE_FAIL) { - report_alarm_message(ALARM_PROBE_FAIL); - } else if (rt_exec & EXEC_ALARM_HOMING_FAIL) { - report_alarm_message(ALARM_HOMING_FAIL); - } - // Halt everything upon a critical event flag. Currently hard and soft limits flag this. - if (rt_exec & EXEC_CRITICAL_EVENT) { - report_feedback_message(MESSAGE_CRITICAL_EVENT); - bit_false_atomic(sys.rt_exec_state,EXEC_RESET); // Disable any existing reset - do { - // Nothing. Block EVERYTHING until user issues reset or power cycles. Hard limits - // typically occur while unattended or not paying attention. Gives the user 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, the incoming - // stream could be still engaged and cause a serious crash if it continues afterwards. - - // TODO: Allow status reports during a critical alarm. Still need to think about implications of this. -// if (sys.rt_exec_state & EXEC_STATUS_REPORT) { -// report_realtime_status(); -// bit_false_atomic(sys.rt_exec_state,EXEC_STATUS_REPORT); -// } - } while (bit_isfalse(sys.rt_exec_state,EXEC_RESET)); - } - bit_false_atomic(sys.rt_exec_alarm,0xFF); // Clear all alarm flags - } - - // Check amd execute realtime commands - rt_exec = sys.rt_exec_state; // Copy volatile sys.rt_exec_state. - if (rt_exec) { // Enter only if any bit flag is true - - // Execute system abort. - if (rt_exec & EXEC_RESET) { - sys.abort = true; // Only place this is set true. - return; // Nothing else to do but exit. - } - - // Execute and serial print status - if (rt_exec & EXEC_STATUS_REPORT) { - report_realtime_status(); - bit_false_atomic(sys.rt_exec_state,EXEC_STATUS_REPORT); - } - - // Execute hold states. - // NOTE: The math involved to calculate the hold should be low enough for most, if not all, - // operational scenarios. Once hold is initiated, the system enters a suspend state to block - // all main program processes until either reset or resumed. - if (rt_exec & (EXEC_MOTION_CANCEL | EXEC_FEED_HOLD | EXEC_SAFETY_DOOR)) { - - // TODO: CHECK MODE? How to handle this? Likely nothing, since it only works when IDLE and then resets Grbl. - - // State check for allowable states for hold methods. - if ((sys.state == STATE_IDLE) || (sys.state & (STATE_CYCLE | STATE_HOMING | STATE_MOTION_CANCEL | STATE_HOLD | STATE_SAFETY_DOOR))) { - - // If in CYCLE state, all hold states immediately initiate a motion HOLD. - if (sys.state == STATE_CYCLE) { - st_update_plan_block_parameters(); // Notify stepper module to recompute for hold deceleration. - sys.suspend = SUSPEND_ENABLE_HOLD; // Initiate holding cycle with flag. - } - // If IDLE, Grbl is not in motion. Simply indicate suspend ready state. - if (sys.state == STATE_IDLE) { sys.suspend = SUSPEND_ENABLE_READY; } - - // 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. If so, only flag that motion cancel is complete. - if (sys.state == STATE_CYCLE) { sys.state = STATE_MOTION_CANCEL; } - sys.suspend |= SUSPEND_MOTION_CANCEL; // Indicate motion cancel when resuming. Special motion complete. - } - - // Execute a feed hold with deceleration, only during cycle. - if (rt_exec & EXEC_FEED_HOLD) { - // Block SAFETY_DOOR state from prematurely changing back to HOLD. - if (bit_isfalse(sys.state,STATE_SAFETY_DOOR)) { sys.state = STATE_HOLD; } - } - - // Execute a safety door stop with a feed hold, only during a cycle, 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. The power-down is - // executed here, if IDLE, or when the CYCLE completes via the EXEC_CYCLE_STOP flag. - if (rt_exec & EXEC_SAFETY_DOOR) { - report_feedback_message(MESSAGE_SAFETY_DOOR_AJAR); - // If already in active, ready-to-resume HOLD, set CYCLE_STOP flag to force de-energize. - // NOTE: Only temporarily sets the 'rt_exec' variable, not the volatile 'rt_exec_state' variable. - if (sys.suspend & SUSPEND_ENABLE_READY) { bit_true(rt_exec,EXEC_CYCLE_STOP); } - sys.suspend |= SUSPEND_ENERGIZE; - sys.state = STATE_SAFETY_DOOR; - } - - } - bit_false_atomic(sys.rt_exec_state,(EXEC_MOTION_CANCEL | EXEC_FEED_HOLD | EXEC_SAFETY_DOOR)); - } - - // 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))) { - // Cycle start only when IDLE or when a hold is complete and ready to resume. - // NOTE: SAFETY_DOOR is implicitly blocked. It reverts to HOLD when the door is closed. - if ((sys.state == STATE_IDLE) || ((sys.state & (STATE_HOLD | STATE_MOTION_CANCEL)) && (sys.suspend & SUSPEND_ENABLE_READY))) { - // Re-energize powered components, if disabled by SAFETY_DOOR. - if (sys.suspend & SUSPEND_ENERGIZE) { - // Delayed Tasks: Restart spindle and coolant, delay to power-up, then resume cycle. - if (gc_state.modal.spindle != SPINDLE_DISABLE) { - spindle_set_state(gc_state.modal.spindle, gc_state.spindle_speed); - delay_ms(SAFETY_DOOR_SPINDLE_DELAY); // TODO: Blocking function call. Need a non-blocking one eventually. - } - if (gc_state.modal.coolant != COOLANT_DISABLE) { - coolant_set_state(gc_state.modal.coolant); - delay_ms(SAFETY_DOOR_COOLANT_DELAY); // TODO: Blocking function call. Need a non-blocking one eventually. - } - // TODO: Install return to pre-park position. - } - // Start cycle only if queued motions exist in planner buffer and the motion is not canceled. - if (plan_get_current_block() && bit_isfalse(sys.suspend,SUSPEND_MOTION_CANCEL)) { - 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.state = STATE_IDLE; - } - sys.suspend = SUSPEND_DISABLE; // Break suspend state. - } - } - bit_false_atomic(sys.rt_exec_state,EXEC_CYCLE_START); - } - - // 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 (rt_exec & EXEC_CYCLE_STOP) { - if (sys.state & (STATE_HOLD | STATE_SAFETY_DOOR)) { - // Hold complete. Set to indicate ready to resume. Remain in HOLD or DOOR states until user - // has issued a resume command or reset. - if (sys.suspend & SUSPEND_ENERGIZE) { // De-energize system if safety door has been opened. - spindle_stop(); - coolant_stop(); - // TODO: Install parking motion here. - } - bit_true(sys.suspend,SUSPEND_ENABLE_READY); - } else { // Motion is complete. Includes CYCLE, HOMING, and MOTION_CANCEL states. - sys.suspend = SUSPEND_DISABLE; - sys.state = STATE_IDLE; - } - bit_false_atomic(sys.rt_exec_state,EXEC_CYCLE_STOP); - } - - } - - // Overrides flag byte (sys.override) and execution should be installed here, since they - // are realtime and require a direct and controlled interface to the main stepper program. - - // Reload step segment buffer - if (sys.state & (STATE_CYCLE | STATE_HOLD | STATE_MOTION_CANCEL | STATE_SAFETY_DOOR | STATE_HOMING)) { st_prep_buffer(); } - - // If safety door was opened, actively check when safety door is closed and ready to resume. - // NOTE: This unlocks the SAFETY_DOOR state to a HOLD state, such that CYCLE_START can activate a resume. - if (sys.state == STATE_SAFETY_DOOR) { - if (bit_istrue(sys.suspend,SUSPEND_ENABLE_READY)) { - if (!(system_check_safety_door_ajar())) { - sys.state = STATE_HOLD; // Update to HOLD state to indicate door is closed and ready to resume. - } - } - } - - } while(sys.suspend); // Check for system suspend state before exiting. - -} - - // 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() @@ -399,3 +191,366 @@ void protocol_buffer_synchronize() // 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() { bit_true_atomic(sys.rt_exec_state, EXEC_CYCLE_START); } + + +// 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 +// be more than a fraction of a second. This is a way to execute realtime commands asynchronously +// (aka multitasking) with grbl's g-code parsing and planning functions. This function also serves +// as an interface for the interrupts to set the system realtime flags, where only the main program +// handles them, removing the need to define more computationally-expensive volatile variables. This +// also provides a controlled way to execute certain tasks without having two or more instances of +// 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(); } +} + + +// 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 + if (rt_exec & EXEC_ALARM_HARD_LIMIT) { + report_alarm_message(ALARM_HARD_LIMIT_ERROR); + } else if (rt_exec & EXEC_ALARM_SOFT_LIMIT) { + report_alarm_message(ALARM_SOFT_LIMIT_ERROR); + } else if (rt_exec & EXEC_ALARM_ABORT_CYCLE) { + report_alarm_message(ALARM_ABORT_CYCLE); + } else if (rt_exec & EXEC_ALARM_PROBE_FAIL) { + report_alarm_message(ALARM_PROBE_FAIL); + } else if (rt_exec & EXEC_ALARM_HOMING_FAIL) { + report_alarm_message(ALARM_HOMING_FAIL); + } + // Halt everything upon a critical event flag. Currently hard and soft limits flag this. + if (rt_exec & EXEC_CRITICAL_EVENT) { + report_feedback_message(MESSAGE_CRITICAL_EVENT); + bit_false_atomic(sys.rt_exec_state,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, streaming could cause a serious crash if it continues afterwards. + +// TODO: Allow status reports during a critical alarm. Still need to think about implications of this. + // if (sys.rt_exec_state & EXEC_STATUS_REPORT) { + // report_realtime_status(); + // bit_false_atomic(sys.rt_exec_state,EXEC_STATUS_REPORT); + // } + + } while (bit_isfalse(sys.rt_exec_state,EXEC_RESET)); + } + bit_false_atomic(sys.rt_exec_alarm,0xFF); // Clear all alarm flags + } + + 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. + } + + // Execute and serial print status + if (rt_exec & EXEC_STATUS_REPORT) { + report_realtime_status(); + bit_false_atomic(sys.rt_exec_state,EXEC_STATUS_REPORT); + } + + // NOTE: The math involved to calculate the hold should be low enough for most, if not all, + // operational scenarios. Once hold is initiated, the system enters a suspend state to block + // all main program processes until either reset or resumed. + if (rt_exec & (EXEC_MOTION_CANCEL | EXEC_FEED_HOLD | EXEC_SAFETY_DOOR)) { + + // TODO: CHECK MODE? How to handle this? Likely nothing, since it only works when IDLE and then resets Grbl. + + // State check for allowable states for hold methods. + if ((sys.state == STATE_IDLE) || (sys.state & (STATE_CYCLE | STATE_HOMING | STATE_MOTION_CANCEL | STATE_HOLD | STATE_SAFETY_DOOR))) { + + // If in CYCLE state, all hold states immediately initiate a motion HOLD. + if (sys.state == STATE_CYCLE) { + 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 IDLE, Grbl is not in motion. Simply indicate suspend state and hold is complete. + if (sys.state == STATE_IDLE) { + sys.suspend = SUSPEND_HOLD_COMPLETE; + sys.step_control = STEP_CONTROL_END_MOTION; + } + + // 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. If so, only flag that motion cancel is complete. + if (sys.state == STATE_CYCLE) { sys.state = STATE_MOTION_CANCEL; } + // NOTE: Ensures the motion cancel is handled correctly if it is active during a HOLD or DOOR state. + sys.suspend |= SUSPEND_MOTION_CANCEL; // Indicate motion cancel when resuming. + } + + // Execute a feed hold with deceleration, if required. Then, suspend system. + if (rt_exec & EXEC_FEED_HOLD) { + // Block SAFETY_DOOR state from prematurely changing back to HOLD, which should only + // occur if the safety door switch closes. + if (sys.state != STATE_SAFETY_DOOR) { 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); + + // Check if the safety re-opened during a restore parking motion only. Ignore if + // already retracting or parked. + if (sys.suspend & SUSPEND_SAFETY_DOOR_AJAR) { + 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_PARK) { + st_update_plan_block_parameters(); // Notify stepper module to recompute for hold deceleration. + sys.step_control = (STEP_CONTROL_EXECUTE_HOLD | STEP_CONTROL_EXECUTE_PARK); + 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; + } + } + + // 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; + sys.state = STATE_SAFETY_DOOR; + } + + } + + bit_false_atomic(sys.rt_exec_state,(EXEC_MOTION_CANCEL | EXEC_FEED_HOLD | EXEC_SAFETY_DOOR)); + } + + // 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))) { + // Cycle start only when IDLE or when a hold is complete and ready to resume. + // NOTE: SAFETY_DOOR is implicitly blocked. It reverts to HOLD when the door is closed. + if ((sys.state == STATE_IDLE) || ((sys.state & (STATE_HOLD | STATE_MOTION_CANCEL)) && (sys.suspend & SUSPEND_HOLD_COMPLETE))) { + if (sys.suspend & SUSPEND_SAFETY_DOOR_AJAR) { + if (sys.suspend & SUSPEND_RETRACT_COMPLETE) { + if bit_isfalse(sys.suspend,SUSPEND_RESTORE_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; + } else { + bit_false(sys.suspend,SUSPEND_SAFETY_DOOR_AJAR); + } + } + } + if (!(sys.suspend & SUSPEND_SAFETY_DOOR_AJAR)) { + // 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; + } + } + } + } + bit_false_atomic(sys.rt_exec_state,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)) { + // 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_PARK)); + } else { // Motion is complete. Includes CYCLE, HOMING, and MOTION_CANCEL states. + sys.suspend = SUSPEND_DISABLE; + sys.state = STATE_IDLE; + } + bit_false_atomic(sys.rt_exec_state,EXEC_CYCLE_STOP); + } + } + + // Overrides flag byte (sys.override) and execution should be installed here, since they + // are realtime and require a direct and controlled interface to the main stepper program. + + // Reload step segment buffer + if (sys.state & (STATE_CYCLE | STATE_HOLD | STATE_MOTION_CANCEL | STATE_SAFETY_DOOR | STATE_HOMING)) { + 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 + // Declare parking local variables + float restore_target[N_AXIS]; + float parking_target[N_AXIS]; + float retract_waypoint = PARKING_PULLOUT_INCREMENT; + #endif + + while (sys.suspend) { + + if (sys.abort) { return; } + + // Safety door manager. Handles de/re-energizing, switch state checks, and parking motions. + if ((sys.suspend & SUSPEND_SAFETY_DOOR_AJAR) && (sys.suspend & SUSPEND_HOLD_COMPLETE)) { + + // Handles retraction motions and de-energizing. + if (bit_isfalse(sys.suspend,SUSPEND_RETRACT_COMPLETE)) { + + #ifndef PARKING_ENABLE + + spindle_stop(); // De-energize + coolant_stop(); // 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 and + // the current location not exceeding the parking target location. + // NOTE: State is will remain DOOR, until the de-energizing and retract is complete. + if ((bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)) && + (parking_target[PARKING_AXIS] < PARKING_TARGET)) { + + // 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; + mc_parking_motion(parking_target, PARKING_PULLOUT_RATE); + } + + spindle_stop(); // De-energize + coolant_stop(); // De-energize + + // Execute fast parking retract motion to parking target location. + if (parking_target[PARKING_AXIS] < PARKING_TARGET) { + parking_target[PARKING_AXIS] = PARKING_TARGET; + mc_parking_motion(parking_target, PARKING_RATE); + } + + } else { + + // Parking motion not possible. Just disable the spindle and coolant. + spindle_stop(); // De-energize + coolant_stop(); // De-energize + + } + + #endif + + sys.suspend &= ~(SUSPEND_RESTART_RETRACT); + sys.suspend |= SUSPEND_RETRACT_COMPLETE; + + } else { + + // Allows resuming from parking/safety door. Actively checks if safety door is closed and ready to resume. + // NOTE: This unlocks the SAFETY_DOOR state to a HOLD state, such that CYCLE_START can activate a resume. + if (sys.state == STATE_SAFETY_DOOR) { + if (!(system_check_safety_door_ajar())) { + sys.state = STATE_HOLD; // Update to HOLD state to indicate door is closed and 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. + if (bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)) { + // 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; + mc_parking_motion(parking_target, PARKING_RATE); + } + } + #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)) { + spindle_set_state(gc_state.modal.spindle, gc_state.spindle_speed); + delay_sec(SAFETY_DOOR_SPINDLE_DELAY, DELAY_MODE_SAFETY_DOOR); + } + } + 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)) { + coolant_set_state(gc_state.modal.coolant); + delay_sec(SAFETY_DOOR_COOLANT_DELAY, DELAY_MODE_SAFETY_DOOR); + } + } + + #ifdef PARKING_ENABLE + // Execute slow plunge motion from pull-out position to resume position. + if (bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)) { + // 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. + mc_parking_motion(restore_target, PARKING_PULLOUT_RATE); + } + } + #endif + + if (bit_isfalse(sys.suspend,SUSPEND_RESTART_RETRACT)) { + sys.suspend |= SUSPEND_RESTORE_COMPLETE; + bit_true_atomic(sys.rt_exec_state,EXEC_CYCLE_START); // Set to resume program. + } + } + + } + } + + protocol_exec_rt_system(); + } +} diff --git a/grbl/protocol.h b/grbl/protocol.h index 3fc3780..a5d00b1 100644 --- a/grbl/protocol.h +++ b/grbl/protocol.h @@ -38,6 +38,7 @@ void protocol_main_loop(); // Checks and executes a realtime command at various stop points in main program void protocol_execute_realtime(); +void protocol_exec_rt_system(); // Notify the stepper subsystem to start executing the g-code program in buffer. // void protocol_cycle_start(); diff --git a/grbl/report.c b/grbl/report.c index 86782bc..b141e9c 100644 --- a/grbl/report.c +++ b/grbl/report.c @@ -72,7 +72,9 @@ void report_status_message(uint8_t status_code) #ifdef MAX_STEP_RATE_HZ case STATUS_MAX_STEP_RATE_EXCEEDED: printPgmString(PSTR("Step rate > 30kHz")); break; - #endif + #endif + case STATUS_CHECK_DOOR: + printPgmString(PSTR("Check Door")); break; // Common g-code parser errors. case STATUS_GCODE_MODAL_GROUP_VIOLATION: printPgmString(PSTR("Modal group violation")); break; @@ -196,6 +198,8 @@ void report_grbl_settings() { printPgmString(PSTR("\r\n$25=")); printFloat_SettingValue(settings.homing_seek_rate); printPgmString(PSTR("\r\n$26=")); print_uint8_base10(settings.homing_debounce_delay); printPgmString(PSTR("\r\n$27=")); printFloat_SettingValue(settings.homing_pulloff); + printPgmString(PSTR("\r\n$30=")); printFloat_RPMValue(settings.rpm_max); + printPgmString(PSTR("\r\n$31=")); printFloat_RPMValue(settings.rpm_min); printPgmString(PSTR("\r\n")); #else printPgmString(PSTR("$0=")); print_uint8_base10(settings.pulse_microseconds); @@ -221,7 +225,9 @@ void report_grbl_settings() { printPgmString(PSTR(" (homing feed, mm/min)\r\n$25=")); printFloat_SettingValue(settings.homing_seek_rate); printPgmString(PSTR(" (homing seek, mm/min)\r\n$26=")); print_uint8_base10(settings.homing_debounce_delay); printPgmString(PSTR(" (homing debounce, msec)\r\n$27=")); printFloat_SettingValue(settings.homing_pulloff); - printPgmString(PSTR(" (homing pull-off, mm)\r\n")); + printPgmString(PSTR(" (homing pull-off, mm)\r\n$30=")); printFloat_RPMValue(settings.rpm_max); + printPgmString(PSTR(" (rpm max)\r\n$31=")); printFloat_RPMValue(settings.rpm_min); + printPgmString(PSTR(" (rpm min)\r\n")); #endif // Print axis settings @@ -380,7 +386,7 @@ void report_gcode_modes() #ifdef VARIABLE_SPINDLE printPgmString(PSTR(" S")); - printFloat_RateValue(gc_state.spindle_speed); + printFloat_RPMValue(gc_state.spindle_speed); #endif printPgmString(PSTR("]\r\n")); diff --git a/grbl/report.h b/grbl/report.h index 69a1d83..f4349b0 100644 --- a/grbl/report.h +++ b/grbl/report.h @@ -34,6 +34,7 @@ #define STATUS_SOFT_LIMIT_ERROR 10 #define STATUS_OVERFLOW 11 #define STATUS_MAX_STEP_RATE_EXCEEDED 12 +#define STATUS_CHECK_DOOR 13 #define STATUS_GCODE_UNSUPPORTED_COMMAND 20 #define STATUS_GCODE_MODAL_GROUP_VIOLATION 21 diff --git a/grbl/settings.c b/grbl/settings.c index 2b32918..924ea9f 100644 --- a/grbl/settings.c +++ b/grbl/settings.c @@ -65,6 +65,10 @@ void settings_restore(uint8_t restore_flag) { settings.status_report_mask = DEFAULT_STATUS_REPORT_MASK; settings.junction_deviation = DEFAULT_JUNCTION_DEVIATION; settings.arc_tolerance = DEFAULT_ARC_TOLERANCE; + + settings.rpm_max = DEFAULT_SPINDLE_RPM_MAX; + settings.rpm_min = DEFAULT_SPINDLE_RPM_MIN; + settings.homing_dir_mask = DEFAULT_HOMING_DIR_MASK; settings.homing_feed_rate = DEFAULT_HOMING_FEED_RATE; settings.homing_seek_rate = DEFAULT_HOMING_SEEK_RATE; @@ -265,6 +269,8 @@ uint8_t settings_store_global_setting(uint8_t parameter, float value) { 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; break; + case 31: settings.rpm_min = value; break; default: return(STATUS_INVALID_STATEMENT); } diff --git a/grbl/settings.h b/grbl/settings.h index 3442b9e..aebf480 100644 --- a/grbl/settings.h +++ b/grbl/settings.h @@ -27,7 +27,7 @@ // Version of the EEPROM data. Will be used to migrate existing data from older versions of Grbl // when firmware is upgraded. Always stored in byte 0 of eeprom -#define SETTINGS_VERSION 9 // NOTE: Check settings_reset() when moving to next version. +#define SETTINGS_VERSION 10 // NOTE: Check settings_reset() when moving to next version. // Define bit flag masks for the boolean settings in settings.flag. #define BITFLAG_REPORT_INCHES bit(0) @@ -92,6 +92,9 @@ typedef struct { float junction_deviation; float arc_tolerance; + float rpm_max; + float rpm_min; + uint8_t flags; // Contains default boolean settings uint8_t homing_dir_mask; diff --git a/grbl/spindle_control.c b/grbl/spindle_control.c index d9c5c10..3356709 100644 --- a/grbl/spindle_control.c +++ b/grbl/spindle_control.c @@ -24,21 +24,33 @@ void spindle_init() { - // Configure variable spindle PWM and enable pin, if requried. On the Uno, PWM and enable are - // combined unless configured otherwise. #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< SPINDLE_RPM_RANGE ) { rpm = SPINDLE_RPM_RANGE; } // Prevent integer overflow + // Calculate PWM register value based on rpm max/min settings and programmed rpm. + if (settings.rpm_max <= settings.rpm_min) { + // No PWM range possible. Set simple on/off spindle control pin state. + current_pwm = PWM_MAX_VALUE; + } else { + if (rpm > settings.rpm_max) { rpm = settings.rpm_max; } + if (rpm < settings.rpm_min) { rpm = settings.rpm_min; } + #ifdef SPINDLE_MINIMUM_PWM + float pwm_gradient = (PWM_MAX_VALUE-SPINDLE_MINIMUM_PWM)/(settings.rpm_max-settings.rpm_min); + current_pwm = floor( (rpm-settings.rpm_min)*pwm_gradient + (SPINDLE_MINIMUM_PWM+0.5)); + #else + float pwm_gradient = (PWM_MAX_VALUE)/(settings.rpm_max-settings.rpm_min); + current_pwm = floor( (rpm-settings.rpm_min)*pwm_gradient + 0.5); + #endif } - current_pwm = floor( rpm*(PWM_MAX_VALUE/SPINDLE_RPM_RANGE) + 0.5); - #ifdef MINIMUM_SPINDLE_PWM - if (current_pwm < MINIMUM_SPINDLE_PWM) { current_pwm = MINIMUM_SPINDLE_PWM; } - #endif - OCR_REGISTER = current_pwm; // Set PWM pin output + + OCR_REGISTER = current_pwm; // Set PWM output level. + TCCRA_REGISTER |= (1<entry_speed_sqr = prep.current_speed*prep.current_speed; // Update entry speed. - pl_block = NULL; // Flag st_prep_segment() to load new velocity profile. + 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); +} + + +#ifdef PARKING_ENABLE + // 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; + } + // 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. + // NOTE: This function does not compile if parking is disabled. + 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) { + 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; + st_prep_block = &st_block_buffer[prep.st_block_index]; + prep.recalculate_flag = (PREP_FLAG_HOLD_PARTIAL_BLOCK | PREP_FLAG_RECALCULATE); + } else { + 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 @@ -521,120 +580,140 @@ void st_update_plan_block_parameters() */ 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; } - if (sys.state & (STATE_HOLD|STATE_MOTION_CANCEL|STATE_SAFETY_DOOR)) { - // Check if we still need to generate more segments for a motion suspend. - if (prep.current_speed == 0.0) { return; } // Nothing to do. Bail. - } - 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 has been replanned. + // Determine if we need to load a new planner block or if the block needs to be recomputed. if (pl_block == NULL) { - pl_block = plan_get_current_block(); // Query planner for a queued block - if (pl_block == NULL) { return; } // No planner blocks. Exit. - - // Check if the segment buffer completed the last planner block. If so, load the Bresenham - // data for the block. If not, we are still mid-block and the velocity profile was updated. - if (prep.flag_partial_block) { - prep.flag_partial_block = false; // Reset flag + + #ifdef PARKING_ENABLE + + // Query planner for a queued block + if (sys.step_control & STEP_CONTROL_EXECUTE_PARK) { pl_block = plan_get_parking_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) { + if (prep.recalculate_flag & PREP_FLAG_PARKING) { prep.recalculate_flag &= ~(PREP_FLAG_RECALCULATE); } + else { prep.recalculate_flag = false; } + + #else + + // Query planner for a queued block + 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) { + prep.recalculate_flag = false; + + #endif + } else { - // Increment stepper common data index to store new planner block data. - if ( ++prep.st_block_index == (SEGMENT_BUFFER_SIZE-1) ) { prep.st_block_index = 0; } - + + // 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; + uint8_t idx; #ifndef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING - st_prep_block->steps[X_AXIS] = pl_block->steps[X_AXIS]; - st_prep_block->steps[Y_AXIS] = pl_block->steps[Y_AXIS]; - st_prep_block->steps[Z_AXIS] = pl_block->steps[Z_AXIS]; + for (idx=0; idxsteps[idx] = pl_block->steps[idx]; } st_prep_block->step_event_count = pl_block->step_event_count; #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. - st_prep_block->steps[X_AXIS] = pl_block->steps[X_AXIS] << MAX_AMASS_LEVEL; - st_prep_block->steps[Y_AXIS] = pl_block->steps[Y_AXIS] << MAX_AMASS_LEVEL; - st_prep_block->steps[Z_AXIS] = pl_block->steps[Z_AXIS] << MAX_AMASS_LEVEL; + 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 = pl_block->step_event_count; + 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 planner block + prep.dt_remainder = 0.0; // Reset for new segment block - if (sys.state & (STATE_HOLD|STATE_MOTION_CANCEL|STATE_SAFETY_DOOR)) { - // Override planner block entry speed and enforce deceleration during feed hold. + if (sys.step_control & STEP_CONTROL_EXECUTE_HOLD) { + // 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; + pl_block->entry_speed_sqr = prep.exit_speed*prep.exit_speed; + } else { + prep.current_speed = sqrt(pl_block->entry_speed_sqr); } - else { prep.current_speed = sqrt(pl_block->entry_speed_sqr); } } - - /* --------------------------------------------------------------------------------- - 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.state & (STATE_HOLD|STATE_MOTION_CANCEL|STATE_SAFETY_DOOR)) { // [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; - prep.exit_speed = plan_get_exec_block_exit_speed(); - float exit_speed_sqr = prep.exit_speed*prep.exit_speed; - float intersect_distance = - 0.5*(pl_block->millimeters+inv_2_accel*(pl_block->entry_speed_sqr-exit_speed_sqr)); - 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*(pl_block->nominal_speed_sqr-exit_speed_sqr); - if (prep.decelerate_after < intersect_distance) { // Trapezoid type - prep.maximum_speed = sqrt(pl_block->nominal_speed_sqr); - if (pl_block->entry_speed_sqr == pl_block->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*(pl_block->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; - } - } + + /* --------------------------------------------------------------------------------- + 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; + + #ifdef PARKING_ENABLE + if (sys.step_control & STEP_CONTROL_EXECUTE_PARK) { prep.exit_speed = 0.0; } + else { prep.exit_speed = plan_get_exec_block_exit_speed(); } + #else + prep.exit_speed = plan_get_exec_block_exit_speed(); + #endif + + float exit_speed_sqr = prep.exit_speed*prep.exit_speed; + float intersect_distance = + 0.5*(pl_block->millimeters+inv_2_accel*(pl_block->entry_speed_sqr-exit_speed_sqr)); + 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*(pl_block->nominal_speed_sqr-exit_speed_sqr); + if (prep.decelerate_after < intersect_distance) { // Trapezoid type + prep.maximum_speed = sqrt(pl_block->nominal_speed_sqr); + if (pl_block->entry_speed_sqr == pl_block->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*(pl_block->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; + } + } } // Initialize new segment @@ -703,14 +782,16 @@ void st_prep_buffer() 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) { // Deceleration only. + 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. } - } // End of block or end of forced-deceleration. + } + // 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. @@ -737,21 +818,20 @@ void st_prep_buffer() 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 steps_remaining = prep.step_per_mm*mm_remaining; // Convert mm_remaining to steps - float n_steps_remaining = ceil(steps_remaining); // Round-up current steps remaining + 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.state & (STATE_HOLD|STATE_MOTION_CANCEL|STATE_SAFETY_DOOR)) { + 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. - prep.current_speed = 0.0; // NOTE: (=0.0) Used to indicate completed segment calcs for hold. - prep.dt_remainder = 0.0; - prep.steps_remaining = n_steps_remaining; - pl_block->millimeters = prep.steps_remaining/prep.step_per_mm; // Update with full steps. - plan_cycle_reinitialize(); + 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. } } @@ -765,8 +845,7 @@ void st_prep_buffer() // 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 - steps_remaining); // Compute adjusted step rate inverse - prep.dt_remainder = (n_steps_remaining - steps_remaining)*inv_rate; // Update segment partial step 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) @@ -802,29 +881,35 @@ void st_prep_buffer() } #endif - // Segment complete! Increment segment buffer indices. + // 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; } - // Setup initial conditions for next segment. - if (mm_remaining > prep.mm_complete) { - // Normal operation. Block incomplete. Distance remaining in block to be executed. - pl_block->millimeters = mm_remaining; - prep.steps_remaining = steps_remaining; - } else { + // 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. - prep.current_speed = 0.0; // NOTE: (=0.0) Used to indicate completed segment calcs for hold. - prep.dt_remainder = 0.0; - prep.steps_remaining = ceil(steps_remaining); - pl_block->millimeters = prep.steps_remaining/prep.step_per_mm; // Update with full steps. - plan_cycle_reinitialize(); + 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. + #ifdef PARKING_ENABLE + if (sys.step_control & STEP_CONTROL_EXECUTE_PARK) { + bit_true(sys.step_control,STEP_CONTROL_END_MOTION); + return; + } + #endif pl_block = NULL; // Set pointer to indicate check and load next planner block. plan_discard_current_block(); } diff --git a/grbl/stepper.h b/grbl/stepper.h index 0745704..1c88680 100644 --- a/grbl/stepper.h +++ b/grbl/stepper.h @@ -40,6 +40,12 @@ void st_generate_step_dir_invert_masks(); // Reset the stepper subsystem variables void st_reset(); + +// Changes the run state of the step segment buffer to execute the special parking motion. +void st_parking_setup_buffer(); + +// Restores the step segment buffer to the normal run state after a parking motion. +void st_parking_restore_buffer(); // Reloads step segment buffer. Called continuously by realtime execution system. void st_prep_buffer(); diff --git a/grbl/system.c b/grbl/system.c index 1a11d3e..7cc4f8b 100644 --- a/grbl/system.c +++ b/grbl/system.c @@ -21,7 +21,7 @@ #include "grbl.h" -void system_init() +void system_init() { CONTROL_DDR &= ~(CONTROL_MASK); // Configure as input pins #ifdef DISABLE_CONTROL_PIN_PULL_UP @@ -135,13 +135,11 @@ uint8_t system_execute_line(char *line) break; case 'X' : // Disable alarm lock [ALARM] if (sys.state == STATE_ALARM) { + // Block if safety door is ajar. + if (system_check_safety_door_ajar()) { return(STATUS_CHECK_DOOR); } report_feedback_message(MESSAGE_ALARM_UNLOCK); sys.state = STATE_IDLE; // Don't run startup script. Prevents stored moves in startup from causing accidents. - if (system_check_safety_door_ajar()) { // Check safety door switch before returning. - bit_true(sys.rt_exec_state, EXEC_SAFETY_DOOR); - protocol_execute_realtime(); // Enter safety door mode. - } } // Otherwise, no effect. break; // case 'J' : break; // Jogging methods @@ -156,8 +154,6 @@ uint8_t system_execute_line(char *line) // handled by the planner. It would be possible for the jog subprogram to insert blocks into the // block buffer without having the planner plan them. It would need to manage de/ac-celerations // on its own carefully. This approach could be effective and possibly size/memory efficient. -// } -// break; } break; default : @@ -170,16 +166,9 @@ uint8_t system_execute_line(char *line) break; case 'H' : // Perform homing cycle [IDLE/ALARM] if (bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)) { + // Block if safety door is ajar. + if (system_check_safety_door_ajar()) { return(STATUS_CHECK_DOOR); } sys.state = STATE_HOMING; // Set system state variable - // Only perform homing if Grbl is idle or lost. - - // TODO: Likely not required. - if (system_check_safety_door_ajar()) { // Check safety door switch before homing. - bit_true(sys.rt_exec_state, EXEC_SAFETY_DOOR); - protocol_execute_realtime(); // Enter safety door mode. - } - - mc_homing_cycle(); if (!sys.abort) { // Execute startup scripts after successful homing. sys.state = STATE_IDLE; // Set to IDLE when complete. @@ -276,7 +265,7 @@ float system_convert_axis_steps_to_mpos(int32_t *steps, uint8_t idx) #endif return(pos); } - + void system_convert_array_steps_to_mpos(float *position, int32_t *steps) { diff --git a/grbl/system.h b/grbl/system.h index 94f2143..4294c29 100644 --- a/grbl/system.h +++ b/grbl/system.h @@ -59,12 +59,23 @@ #define STATE_SAFETY_DOOR bit(5) // Safety door is ajar. Feed holds and de-energizes system. #define STATE_MOTION_CANCEL bit(6) // Motion cancel by feed hold and return to idle. -// Define system suspend states. -#define SUSPEND_DISABLE 0 // Must be zero. -#define SUSPEND_ENABLE_HOLD bit(0) // Enabled. Indicates the cycle is active and currently undergoing a hold. -#define SUSPEND_ENABLE_READY bit(1) // Ready to resume with a cycle start command. -#define SUSPEND_ENERGIZE bit(2) // Re-energizes output before resume. -#define SUSPEND_MOTION_CANCEL bit(3) // Cancels resume motion. Used by probing routine. +// 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) // Indicates suspend was initiated by a safety door state. +#define SUSPEND_MOTION_CANCEL bit(6) // Indicates a canceled resume motion. Currently used by probing routine. + + +#define STEP_CONTROL_NORMAL_OP 0 +// #define STEP_CONTROL_RECOMPUTE_ACTIVE_BLOCK bit(0) +#define STEP_CONTROL_END_MOTION bit(1) +#define STEP_CONTROL_EXECUTE_HOLD bit(2) +#define STEP_CONTROL_EXECUTE_PARK bit(3) + // Define global system variables @@ -72,6 +83,7 @@ typedef struct { uint8_t abort; // System abort flag. Forces exit back to main loop for reset. uint8_t state; // Tracks the current state of Grbl. uint8_t suspend; // System suspend bitflag variable that manages holds, cancels, and safety door. + uint8_t step_control; volatile uint8_t rt_exec_state; // Global realtime executor bitflag variable for state management. See EXEC bitmasks. volatile uint8_t rt_exec_alarm; // Global realtime executor bitflag variable for setting various alarms.