diff --git a/Grbl_Esp32/config.h b/Grbl_Esp32/config.h index 0d14b58d..057affb7 100644 --- a/Grbl_Esp32/config.h +++ b/Grbl_Esp32/config.h @@ -49,6 +49,7 @@ Some features should not be changed. See notes below. #define CPU_MAP_ESP32 // these are defined in cpu_map.h #define VERBOSE_HELP // adds addition help info, but could confuse some senders + // Serial baud rate #define BAUD_RATE 115200 @@ -501,6 +502,15 @@ Some features should not be changed. See notes below. // time step. Also, keep in mind that the Arduino delay timer is not very accurate for long delays. #define DWELL_TIME_STEP 50 // Integer (1-255) (milliseconds) + +// For test use only. This uses the ESP32's RMT perifieral to generate step pulses +// It allows the use of the STEP_PULSE_DELAY (see below) and it automatically ends the +// pulse in one operation. +// Dir Pin ____|-------------------- +// Step Pin _______|--|____________ +// While this is experimental, it is intended to be the future default method after testing +//#define USE_RMT_STEPS + // Creates a delay between the direction pin setting and corresponding step pulse by creating // another interrupt (Timer2 compare) to manage it. The main Grbl interrupt (Timer1 compare) // sets the direction pins, and does not immediately set the stepper pins, as it would in @@ -510,8 +520,8 @@ Some features should not be changed. See notes below. // NOTE: Uncomment to enable. The recommended delay must be > 3us, and, when added with the // user-supplied step pulse time, the total time must not exceed 127us. Reported successful // values for certain setups have ranged from 5 to 20us. -// !!!!! ESP32 Not currently implemented -// #define STEP_PULSE_DELAY 10 // Step pulse delay in microseconds. Default disabled. +// must use #define USE_RMT_STEPS for this to work +//#define STEP_PULSE_DELAY 10 // Step pulse delay in microseconds. Default disabled. // The number of linear motions in the planner buffer to be planned at any give time. The vast // majority of RAM that Grbl uses is based on this buffer size. Only increase if there is extra @@ -551,14 +561,11 @@ Some features should not be changed. See notes below. // #define RX_BUFFER_SIZE 128 // (1-254) Uncomment to override defaults in serial.h // #define TX_BUFFER_SIZE 100 // (1-254) -// A simple software debouncing feature for hard limit switches. When enabled, the interrupt -// monitoring the hard limit switch pins will enable the Arduino's watchdog timer to re-check -// the limit pin state after a delay of about 32msec. This can help with CNC machines with -// problematic false triggering of their hard limit switches, but it WILL NOT fix issues with -// electrical interference on the signal cables from external sources. It's recommended to first -// use shielded signal cables with their shielding connected to ground (old USB/computer cables -// work well and are cheap to find) and wire in a low-pass circuit into each limit pin. -// #define ENABLE_SOFTWARE_DEBOUNCE // Default disabled. Uncomment to enable. +// A simple software debouncing feature for hard limit switches. When enabled, the limit +// switch interrupt unblock a waiting task which will recheck the limit switch pins after +// a short delay. Default disabled +//#define ENABLE_SOFTWARE_DEBOUNCE // Default disabled. Uncomment to enable. +#define DEBOUNCE_PERIOD 32 // in milliseconds default 32 microseconds // Configures the position after a probing cycle during Grbl's check mode. Disabled sets // the position to the probe target, when enabled sets the position to the start position. diff --git a/Grbl_Esp32/cpu_map.h b/Grbl_Esp32/cpu_map.h index e84b76af..e46dad24 100644 --- a/Grbl_Esp32/cpu_map.h +++ b/Grbl_Esp32/cpu_map.h @@ -35,7 +35,9 @@ with AVR grbl */ - + + + #ifdef CPU_MAP_ESP32 // This is the CPU Map for the ESP32 CNC Controller R2 @@ -43,13 +45,17 @@ // won't affect operation except that there will be no output // form the pins. Grbl will virtually move the axis. This could // be handy if you are using a servo, etc. for another axis. - #define X_STEP_PIN GPIO_NUM_12 - #define Y_STEP_PIN GPIO_NUM_14 - #define Z_STEP_PIN GPIO_NUM_27 + #define X_STEP_PIN GPIO_NUM_12 + #define X_DIRECTION_PIN GPIO_NUM_26 + #define X_RMT_CHANNEL 0 - #define X_DIRECTION_PIN GPIO_NUM_26 - #define Y_DIRECTION_PIN GPIO_NUM_25 - #define Z_DIRECTION_PIN GPIO_NUM_33 + #define Y_STEP_PIN GPIO_NUM_14 + #define Y_DIRECTION_PIN GPIO_NUM_25 + #define Y_RMT_CHANNEL 1 + + #define Z_STEP_PIN GPIO_NUM_27 + #define Z_DIRECTION_PIN GPIO_NUM_33 + #define Z_RMT_CHANNEL 2 // OK to comment out to use pin for other features #define STEPPERS_DISABLE_PIN GPIO_NUM_13 @@ -851,4 +857,3 @@ #endif - diff --git a/Grbl_Esp32/data/index.html.gz b/Grbl_Esp32/data/index.html.gz index 85405e39..a61156c1 100644 Binary files a/Grbl_Esp32/data/index.html.gz and b/Grbl_Esp32/data/index.html.gz differ diff --git a/Grbl_Esp32/grbl_limits.cpp b/Grbl_Esp32/grbl_limits.cpp index 3fa5251e..250db8a9 100644 --- a/Grbl_Esp32/grbl_limits.cpp +++ b/Grbl_Esp32/grbl_limits.cpp @@ -27,7 +27,7 @@ #include "grbl.h" - +xQueueHandle limit_sw_queue; // used by limit switch debouncing // Homing axis search distance multiplier. Computed by this value times the cycle travel. #ifndef HOMING_AXIS_SEARCH_SCALAR @@ -39,25 +39,32 @@ void IRAM_ATTR isr_limit_switches() { - // Ignore limit switches if already in an alarm state or in-process of executing an alarm. + // Ignore limit switches if already in an alarm state or in-process of executing an alarm. // When in the alarm state, Grbl should have been reset or will force a reset, so any pending // moves in the planner and serial buffers are all cleared and newly sent blocks will be // locked out until a homing cycle or a kill lock command. Allows the user to disable the hard // limit setting if their limits are constantly triggering after a reset and move their axes. - if ( ( sys.state != STATE_ALARM) & (bit_isfalse(sys.state, STATE_HOMING)) ) { - if (!(sys_rt_exec_alarm)) { - #ifdef HARD_LIMIT_FORCE_STATE_CHECK - // Check limit pin state. - if (limits_get_state()) { - mc_reset(); // Initiate system kill. - system_set_exec_alarm(EXEC_ALARM_HARD_LIMIT); // Indicate hard limit critical event - } - #else - mc_reset(); // Initiate system kill. - system_set_exec_alarm(EXEC_ALARM_HARD_LIMIT); // Indicate hard limit critical event - #endif - } + if ( ( sys.state != STATE_ALARM) & (bit_isfalse(sys.state, STATE_HOMING)) ) { + if (!(sys_rt_exec_alarm)) { + + #ifdef ENABLE_SOFTWARE_DEBOUNCE + // we will start a task that will recheck the switches after a small delay + int evt; + xQueueSendFromISR(limit_sw_queue, &evt, NULL); + #else + #ifdef HARD_LIMIT_FORCE_STATE_CHECK + // Check limit pin state. + if (limits_get_state()) { + mc_reset(); // Initiate system kill. + system_set_exec_alarm(EXEC_ALARM_HARD_LIMIT); // Indicate hard limit critical event + } + #else + mc_reset(); // Initiate system kill. + system_set_exec_alarm(EXEC_ALARM_HARD_LIMIT); // Indicate hard limit critical event + #endif + #endif + } } } @@ -300,15 +307,17 @@ void limits_init() limits_disable(); } - -// TODO Debounce -/* - #ifdef ENABLE_SOFTWARE_DEBOUNCE - MCUSR &= ~(1<. @@ -34,12 +34,12 @@ // discarded when entirely consumed and completed by the segment buffer. Also, AMASS alters this // data for its own use. typedef struct { - uint32_t steps[N_AXIS]; - uint32_t step_event_count; - uint8_t direction_bits; - #ifdef VARIABLE_SPINDLE - uint8_t is_pwm_rate_adjusted; // Tracks motions that require constant laser power/rate - #endif + uint32_t steps[N_AXIS]; + uint32_t step_event_count; + uint8_t direction_bits; +#ifdef VARIABLE_SPINDLE + uint8_t is_pwm_rate_adjusted; // Tracks motions that require constant laser power/rate +#endif } st_block_t; static st_block_t st_block_buffer[SEGMENT_BUFFER_SIZE-1]; @@ -48,42 +48,42 @@ static st_block_t st_block_buffer[SEGMENT_BUFFER_SIZE-1]; // planner buffer. Once "checked-out", the steps in the segments buffer cannot be modified by // the planner, where the remaining planner block steps still can. typedef struct { - uint16_t n_step; // Number of step events to be executed for this segment - uint16_t cycles_per_tick; // Step distance traveled per ISR tick, aka step rate. - uint8_t st_block_index; // Stepper block data index. Uses this information to execute this segment. - #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING - uint8_t amass_level; // Indicates AMASS level for the ISR to execute this segment - #else - uint8_t prescaler; // Without AMASS, a prescaler is required to adjust for slow timing. - #endif - #ifdef VARIABLE_SPINDLE - uint8_t spindle_pwm; - #endif + uint16_t n_step; // Number of step events to be executed for this segment + uint16_t cycles_per_tick; // Step distance traveled per ISR tick, aka step rate. + uint8_t st_block_index; // Stepper block data index. Uses this information to execute this segment. +#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING + uint8_t amass_level; // Indicates AMASS level for the ISR to execute this segment +#else + uint8_t prescaler; // Without AMASS, a prescaler is required to adjust for slow timing. +#endif +#ifdef VARIABLE_SPINDLE + uint8_t spindle_pwm; +#endif } segment_t; static segment_t segment_buffer[SEGMENT_BUFFER_SIZE]; // Stepper ISR data struct. Contains the running data for the main stepper ISR. typedef struct { - // Used by the bresenham line algorithm - uint32_t counter_x, // Counter variables for the bresenham line tracer - counter_y, - counter_z; - #ifdef STEP_PULSE_DELAY - uint8_t step_bits; // Stores out_bits output to complete the step pulse delay - #endif + // Used by the bresenham line algorithm + uint32_t counter_x, // Counter variables for the bresenham line tracer + counter_y, + counter_z; +#ifdef STEP_PULSE_DELAY + uint8_t step_bits; // Stores out_bits output to complete the step pulse delay +#endif - uint8_t execute_step; // Flags step execution for each interrupt. - uint8_t step_pulse_time; // Step pulse reset time after step rise - uint8_t step_outbits; // The next stepping-bits to be output - uint8_t dir_outbits; - #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING - uint32_t steps[N_AXIS]; - #endif + uint8_t execute_step; // Flags step execution for each interrupt. + uint8_t step_pulse_time; // Step pulse reset time after step rise + uint8_t step_outbits; // The next stepping-bits to be output + uint8_t dir_outbits; +#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING + uint32_t steps[N_AXIS]; +#endif - uint16_t step_count; // Steps remaining in line segment motion - uint8_t exec_block_index; // Tracks the current st_block index. Change indicates new block. - st_block_t *exec_block; // Pointer to the block data for the segment being executed - segment_t *exec_segment; // Pointer to the segment being executed + uint16_t step_count; // Steps remaining in line segment motion + uint8_t exec_block_index; // Tracks the current st_block index. Change indicates new block. + st_block_t *exec_block; // Pointer to the block data for the segment being executed + segment_t *exec_segment; // Pointer to the segment being executed } stepper_t; static stepper_t st; @@ -111,34 +111,34 @@ bool stepper_idle; // Segment preparation data struct. Contains all the necessary information to compute new segments // based on the current executing planner block. typedef struct { - uint8_t st_block_index; // Index of stepper common data block being prepped - uint8_t recalculate_flag; + uint8_t st_block_index; // Index of stepper common data block being prepped + uint8_t recalculate_flag; - float dt_remainder; - float steps_remaining; - float step_per_mm; - float req_mm_increment; + float dt_remainder; + float steps_remaining; + float step_per_mm; + float req_mm_increment; - #ifdef PARKING_ENABLE - uint8_t last_st_block_index; - float last_steps_remaining; - float last_step_per_mm; - float last_dt_remainder; - #endif +#ifdef PARKING_ENABLE + uint8_t last_st_block_index; + float last_steps_remaining; + float last_step_per_mm; + float last_dt_remainder; +#endif - uint8_t ramp_type; // Current segment ramp state - float mm_complete; // End of velocity profile from end of current planner block in (mm). - // NOTE: This value must coincide with a step(no mantissa) when converted. - float current_speed; // Current speed at the end of the segment buffer (mm/min) - float maximum_speed; // Maximum speed of executing block. Not always nominal speed. (mm/min) - float exit_speed; // Exit speed of executing block (mm/min) - float accelerate_until; // Acceleration ramp end measured from end of block (mm) - float decelerate_after; // Deceleration ramp start measured from end of block (mm) + uint8_t ramp_type; // Current segment ramp state + float mm_complete; // End of velocity profile from end of current planner block in (mm). + // NOTE: This value must coincide with a step(no mantissa) when converted. + float current_speed; // Current speed at the end of the segment buffer (mm/min) + float maximum_speed; // Maximum speed of executing block. Not always nominal speed. (mm/min) + float exit_speed; // Exit speed of executing block (mm/min) + float accelerate_until; // Acceleration ramp end measured from end of block (mm) + float decelerate_after; // Deceleration ramp start measured from end of block (mm) - #ifdef VARIABLE_SPINDLE - float inv_rate; // Used by PWM laser mode to speed up segment calculations. - uint8_t current_spindle_pwm; - #endif +#ifdef VARIABLE_SPINDLE + float inv_rate; // Used by PWM laser mode to speed up segment calculations. + uint8_t current_spindle_pwm; +#endif } st_prep_t; static st_prep_t prep; @@ -182,12 +182,12 @@ static st_prep_t prep; ISR is supported by The Stepper Port Reset Interrupt which it uses to reset the stepper port after each pulse. The bresenham line tracer algorithm controls all stepper outputs simultaneously with these two interrupts. - + NOTE: This interrupt must be as efficient as possible and complete before the next ISR tick, which for ESP32 Grbl must be less than xx.xusec (TBD). Oscilloscope measured time in ISR is 5usec typical and 25usec maximum, well below requirement. NOTE: This ISR expects at least one step to be executed per segment. - + The complete step timing should look this... Direction pin is set An optional (via STEP_PULSE_DELAY in config.h) is put after this @@ -195,8 +195,8 @@ static st_prep_t prep; A pulse length is determine (via option $0 ... settings.pulse_microseconds) The pulse is ended Direction will remain the same until another step occurs with a change in direction. - - + + */ // TODO: Replace direct updating of the int32 position counters in the ISR somehow. Perhaps use smaller // int8 variables and update position counters only when a segment completes. This can get complicated @@ -204,142 +204,187 @@ static st_prep_t prep; void IRAM_ATTR onStepperDriverTimer(void *para) // ISR It is time to take a step ======================================================================================= { uint64_t step_pulse_off_time; - - //const int timer_idx = (int)para; // get the timer index - + + //const int timer_idx = (int)para; // get the timer index + TIMERG0.int_clr_timers.t0 = 1; - - if (busy) { return; } // The busy-flag is used to avoid reentering this interrupt - - - // Set the direction pins a couple of nanoseconds before we step the steppers - set_direction_pins_on(st.dir_outbits); - // TO DO ... do we need a direction change delay? - - - set_stepper_pins_on(st.step_outbits); - step_pulse_off_time = esp_timer_get_time() + (settings.pulse_microseconds); // determine when to turn off pulse - - - - busy = true; - // If there is no step segment, attempt to pop one from the stepper buffer - if (st.exec_segment == NULL) { - // Anything in the buffer? If so, load and initialize next step segment. - if (segment_buffer_head != segment_buffer_tail) { - // Initialize new step segment and load number of steps to execute - st.exec_segment = &segment_buffer[segment_buffer_tail]; - // Initialize step segment timing per step and load number of steps to execute. - Stepper_Timer_WritePeriod(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 ^ settings.dir_invert_mask; - - #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 - - } 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(); } - - // Reset step out bits. - st.step_outbits = 0; - - // Execute step displacement profile by Bresenham line algorithm - #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING - st.counter_x += st.steps[X_AXIS]; - #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<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<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<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 ^ settings.dir_invert_mask; + +#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 + + } 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(); + } + + // Reset step out bits. + st.step_outbits = 0; + + // Execute step displacement profile by Bresenham line algorithm +#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING + st.counter_x += st.steps[X_AXIS]; +#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<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<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<> 3); - #endif - // Enable Stepper Driver Interrupt - Stepper_Timer_Start(); + // Initialize stepper output bits to ensure first ISR call does not step. + st.step_outbits = step_port_invert_mask; + + // Initialize step pulse timing from settings. Here to ensure updating after re-writing. +#ifdef STEP_PULSE_DELAY + // Step pulse delay handling is not require with ESP32...the RMT function does it. +#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 + + // Enable Stepper Driver Interrupt + Stepper_Timer_Start(); } // Reset and clear stepper subsystem variables void st_reset() { - #ifdef ESP_DEBUG - //Serial.println("st_reset()"); - #endif - // Initialize stepper driver idle state. - st_go_idle(); +#ifdef ESP_DEBUG + //Serial.println("st_reset()"); +#endif + // Initialize stepper driver idle state. + st_go_idle(); - // Initialize stepper algorithm variables. - memset(&prep, 0, sizeof(st_prep_t)); - memset(&st, 0, sizeof(stepper_t)); - st.exec_segment = NULL; - pl_block = NULL; // Planner block pointer used by segment buffer - segment_buffer_tail = 0; - segment_buffer_head = 0; // empty = tail - segment_next_head = 1; - busy = false; + // Initialize stepper algorithm variables. + memset(&prep, 0, sizeof(st_prep_t)); + memset(&st, 0, sizeof(stepper_t)); + st.exec_segment = NULL; + pl_block = NULL; // Planner block pointer used by segment buffer + segment_buffer_tail = 0; + segment_buffer_head = 0; // empty = tail + segment_next_head = 1; + busy = false; - st_generate_step_dir_invert_masks(); - st.dir_outbits = dir_port_invert_mask; // Initialize direction bits to default. + st_generate_step_dir_invert_masks(); + st.dir_outbits = dir_port_invert_mask; // Initialize direction bits to default. + + // TODO do we need to turn step pins off? - // TODO do we need to turn step pins off? - } @@ -468,165 +577,214 @@ void st_reset() void set_direction_pins_on(uint8_t onMask) -{ +{ // inverts are applied in step generation - #ifdef X_DIRECTION_PIN - digitalWrite(X_DIRECTION_PIN, (onMask & (1<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. - } + 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. + } } #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. - } +// 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. - 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. - } else { - prep.recalculate_flag = false; - } - pl_block = NULL; // Set to reload next block. - } +// 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. + } else { + prep.recalculate_flag = false; + } + pl_block = NULL; // Set to reload next block. +} #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; idxdirection_bits = pl_block->direction_bits; - uint8_t idx; - #ifndef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING - 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. - 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 + // 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 + 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. + 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 + // 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; - } - } - #endif - } + 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; + } + } +#endif + } /* --------------------------------------------------------------------------------- Compute the velocity profile of a new planner block based on its entry and exit @@ -758,39 +932,39 @@ void st_prep_buffer() 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); - } + 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); + 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)); + 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; + 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. + // 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. + // 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); - prep.maximum_speed = nominal_speed; - prep.ramp_type = RAMP_DECEL_OVERRIDE; - } + } 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); + 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. @@ -810,9 +984,9 @@ void st_prep_buffer() 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; + 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; @@ -820,246 +994,268 @@ void st_prep_buffer() 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; +#ifdef VARIABLE_SPINDLE + bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_PWM); // Force update whenever updating block. +#endif + } - /*------------------------------------------------------------------------------------ - 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; } + // Initialize new segment + segment_t *prep_segment = &segment_buffer[segment_buffer_head]; - do { - switch (prep.ramp_type) { - case RAMP_DECEL_OVERRIDE: - speed_var = pl_block->acceleration*time_var; - mm_var = time_var*(prep.current_speed - 0.5*speed_var); - mm_remaining -= mm_var; - if ((mm_remaining < prep.accelerate_until) || (mm_var <= 0)) { - // Cruise or cruise-deceleration types only for deceleration override. - 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); - prep.ramp_type = RAMP_CRUISE; - prep.current_speed = prep.maximum_speed; - } else { // Mid-deceleration override ramp. - 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. + // Set new segment to point to the current segment data block. + prep_segment->st_block_index = prep.st_block_index; - #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 + /*------------------------------------------------------------------------------------ + 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; + } - #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. + do { + switch (prep.ramp_type) { + case RAMP_DECEL_OVERRIDE: + speed_var = pl_block->acceleration*time_var; + mm_var = time_var*(prep.current_speed - 0.5*speed_var); + mm_remaining -= mm_var; + if ((mm_remaining < prep.accelerate_until) || (mm_var <= 0)) { + // Cruise or cruise-deceleration types only for deceleration override. + 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); + prep.ramp_type = RAMP_CRUISE; + prep.current_speed = prep.maximum_speed; + } else { // Mid-deceleration override ramp. + 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. - // 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. - } - } +#ifdef VARIABLE_SPINDLE + /* ----------------------------------------------------------------------------------- + Compute spindle speed PWM output for step segment + */ - // 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 + 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 - // Compute CPU cycles per step for the prepped segment. - uint32_t cycles = ceil( (TICKS_PER_MICROSECOND*1000000*60)*inv_rate ); // (cycles/step) +#endif - #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 + /* ----------------------------------------------------------------------------------- + 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; + // 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 - // 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 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(); + } + } + + } } @@ -1070,67 +1266,70 @@ void st_prep_buffer() // 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; + if (sys.state & (STATE_CYCLE | STATE_HOMING | STATE_HOLD | STATE_JOG | STATE_SAFETY_DOOR)) { + return prep.current_speed; + } + return 0.0f; } void IRAM_ATTR Stepper_Timer_WritePeriod(uint64_t alarm_val) -{ +{ timer_set_alarm_value(STEP_TIMER_GROUP, STEP_TIMER_INDEX, alarm_val); } void IRAM_ATTR Stepper_Timer_Start() -{ - #ifdef ESP_DEBUG - //Serial.println("ST Start"); - #endif +{ +#ifdef ESP_DEBUG + //Serial.println("ST Start"); +#endif + + timer_set_counter_value(STEP_TIMER_GROUP, STEP_TIMER_INDEX, 0x00000000ULL); - timer_set_counter_value(STEP_TIMER_GROUP, STEP_TIMER_INDEX, 0x00000000ULL); - timer_start(STEP_TIMER_GROUP, STEP_TIMER_INDEX); TIMERG0.hw_timer[STEP_TIMER_INDEX].config.alarm_en = TIMER_ALARM_EN; - + } void IRAM_ATTR Stepper_Timer_Stop() -{ - #ifdef ESP_DEBUG - //Serial.println("ST Stop"); - #endif - +{ +#ifdef ESP_DEBUG + //Serial.println("ST Stop"); +#endif + timer_pause(STEP_TIMER_GROUP, STEP_TIMER_INDEX); - + } void set_stepper_disable(uint8_t isOn) // isOn = true // to disable -{ - if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { isOn = !isOn; } // Apply pin invert. - - #ifdef STEPPERS_DISABLE_PIN - digitalWrite(STEPPERS_DISABLE_PIN, isOn ); - #endif +{ + if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { + isOn = !isOn; // Apply pin invert. + } + +#ifdef STEPPERS_DISABLE_PIN + digitalWrite(STEPPERS_DISABLE_PIN, isOn ); +#endif } -bool get_stepper_disable() { // returns true if steppers are disabled +bool get_stepper_disable() // returns true if steppers are disabled +{ bool disabled = false; - #ifdef STEPPERS_DISABLE_PIN - disabled = digitalRead(STEPPERS_DISABLE_PIN); - #else - return false; // thery are never disabled if there is no pin defined - #endif - - if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { +#ifdef STEPPERS_DISABLE_PIN + disabled = digitalRead(STEPPERS_DISABLE_PIN); +#else + return false; // thery are never disabled if there is no pin defined +#endif + + if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { disabled = !disabled; // Apply pin invert. - } - + } + return disabled; - + } - + diff --git a/Grbl_Esp32/stepper.h b/Grbl_Esp32/stepper.h index 9191053f..527d9e12 100644 --- a/Grbl_Esp32/stepper.h +++ b/Grbl_Esp32/stepper.h @@ -82,6 +82,8 @@ extern uint8_t ganged_mode; void IRAM_ATTR onSteppertimer(); void IRAM_ATTR onStepperOffTimer(); void stepper_init(); +void initRMT(); +inline IRAM_ATTR static void stepperRMT_Outputs(); // Enable steppers, but cycle does not start unless called by motion control or realtime command. void st_wake_up(); diff --git a/Grbl_Esp32/web_server.cpp b/Grbl_Esp32/web_server.cpp index 4ac4f603..aa1764d3 100644 --- a/Grbl_Esp32/web_server.cpp +++ b/Grbl_Esp32/web_server.cpp @@ -522,6 +522,10 @@ void Web_Server::handle_web_command () uint8_t sindex = 0; scmd = get_Splited_Value(cmd,'\n', sindex); while ( scmd != "" ){ + if ((scmd.length() == 2) && (scmd[0] == 0xC2)){ + scmd[0]=scmd[1]; + scmd.remove(1,1); + } if (scmd.length() > 1)scmd += "\n"; else if (!is_realtime_cmd(scmd[0]) )scmd += "\n"; if (!Serial2Socket.push(scmd.c_str()))res = "Error";