stepper.c

00001 /*
00002   stepper.c - stepper motor driver: executes motion plans using stepper motors
00003   Part of Grbl
00004 
00005   Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC
00006   Copyright (c) 2009-2011 Simen Svale Skogsrud
00007 
00008   Grbl is free software: you can redistribute it and/or modify
00009   it under the terms of the GNU General Public License as published by
00010   the Free Software Foundation, either version 3 of the License, or
00011   (at your option) any later version.
00012 
00013   Grbl is distributed in the hope that it will be useful,
00014   but WITHOUT ANY WARRANTY; without even the implied warranty of
00015   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016   GNU General Public License for more details.
00017 
00018   You should have received a copy of the GNU General Public License
00019   along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
00020 */
00021 
00022 #include "grbl.h"
00023 
00024 #ifdef STM32F103C8
00025 typedef int bool;
00026 #include "stm32f10x_rcc.h"
00027 #include "stm32f10x_tim.h"
00028 #include "misc.h"
00029 void TIM_Configuration(TIM_TypeDef* TIMER, u16 Period, u16 Prescaler, u8 PP);
00030 #endif
00031 
00032 
00033 // Some useful constants.
00034 #define DT_SEGMENT (1.0f/(ACCELERATION_TICKS_PER_SECOND*60.0f)) // min/segment
00035 #define REQ_MM_INCREMENT_SCALAR 1.25f
00036 #define RAMP_ACCEL 0
00037 #define RAMP_CRUISE 1
00038 #define RAMP_DECEL 2
00039 #define RAMP_DECEL_OVERRIDE 3
00040 
00041 #define PREP_FLAG_RECALCULATE bit(0)
00042 #define PREP_FLAG_HOLD_PARTIAL_BLOCK bit(1)
00043 #define PREP_FLAG_PARKING bit(2)
00044 #define PREP_FLAG_DECEL_OVERRIDE bit(3)
00045 const PORTPINDEF step_pin_mask[N_AXIS] =
00046 {
00047     1 << X_STEP_BIT,
00048     1 << Y_STEP_BIT,
00049     1 << Z_STEP_BIT,
00050 
00051 };
00052 const PORTPINDEF direction_pin_mask[N_AXIS] =
00053 {
00054     1 << X_DIRECTION_BIT,
00055     1 << Y_DIRECTION_BIT,
00056     1 << Z_DIRECTION_BIT,
00057 };
00058 const PORTPINDEF limit_pin_mask[N_AXIS] =
00059 {
00060     1 << X_LIMIT_BIT,
00061     1 << Y_LIMIT_BIT,
00062     1 << Z_LIMIT_BIT,
00063 };
00064 
00065 // Define Adaptive Multi-Axis Step-Smoothing(AMASS) levels and cutoff frequencies. The highest level
00066 // frequency bin starts at 0Hz and ends at its cutoff frequency. The next lower level frequency bin
00067 // starts at the next higher cutoff frequency, and so on. The cutoff frequencies for each level must
00068 // be considered carefully against how much it over-drives the stepper ISR, the accuracy of the 16-bit
00069 // timer, and the CPU overhead. Level 0 (no AMASS, normal operation) frequency bin starts at the
00070 // Level 1 cutoff frequency and up to as fast as the CPU allows (over 30kHz in limited testing).
00071 // NOTE: AMASS cutoff frequency multiplied by ISR overdrive factor must not exceed maximum step frequency.
00072 // NOTE: Current settings are set to overdrive the ISR to no more than 16kHz, balancing CPU overhead
00073 // and timer accuracy.  Do not alter these settings unless you know what you are doing.
00074 #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
00075     #define MAX_AMASS_LEVEL 3
00076     // AMASS_LEVEL0: Normal operation. No AMASS. No upper cutoff frequency. Starts at LEVEL1 cutoff frequency.
00077     #define AMASS_LEVEL1 (F_CPU/8000) // Over-drives ISR (x2). Defined as F_CPU/(Cutoff frequency in Hz)
00078     #define AMASS_LEVEL2 (F_CPU/4000) // Over-drives ISR (x4)
00079     #define AMASS_LEVEL3 (F_CPU/2000) // Over-drives ISR (x8)
00080 
00081   #if MAX_AMASS_LEVEL <= 0
00082     error "AMASS must have 1 or more levels to operate correctly."
00083   #endif
00084 #endif
00085 #ifdef WIN32
00086 #include <process.h> 
00087 unsigned char PORTB = 0;
00088 unsigned char DDRD = 0;
00089 unsigned char DDRB = 0;
00090 unsigned char PORTD = 0;
00091 LARGE_INTEGER Win32Frequency;
00092 LONGLONG nTimer1Out = 0;
00093 LONGLONG nTimer0Out = 0;
00094 #endif
00095 
00096 
00097 // Stores the planner block Bresenham algorithm execution data for the segments in the segment
00098 // buffer. Normally, this buffer is partially in-use, but, for the worst case scenario, it will
00099 // never exceed the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1).
00100 // NOTE: This data is copied from the prepped planner blocks so that the planner blocks may be
00101 // discarded when entirely consumed and completed by the segment buffer. Also, AMASS alters this
00102 // data for its own use.
00103 typedef struct {
00104   uint32_t steps[N_AXIS];
00105   uint32_t step_event_count;
00106   uint8_t direction_bits;
00107   #ifdef VARIABLE_SPINDLE
00108     uint8_t is_pwm_rate_adjusted; // Tracks motions that require constant laser power/rate
00109   #endif
00110 } st_block_t;
00111 static st_block_t st_block_buffer[SEGMENT_BUFFER_SIZE-1];
00112 
00113 // Primary stepper segment ring buffer. Contains small, short line segments for the stepper
00114 // algorithm to execute, which are "checked-out" incrementally from the first block in the
00115 // planner buffer. Once "checked-out", the steps in the segments buffer cannot be modified by
00116 // the planner, where the remaining planner block steps still can.
00117 typedef struct {
00118   uint16_t n_step;           // Number of step events to be executed for this segment
00119   uint16_t cycles_per_tick;  // Step distance traveled per ISR tick, aka step rate.
00120   uint8_t  st_block_index;   // Stepper block data index. Uses this information to execute this segment.
00121   #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
00122     uint8_t amass_level;    // Indicates AMASS level for the ISR to execute this segment
00123   #else
00124     uint8_t prescaler;      // Without AMASS, a prescaler is required to adjust for slow timing.
00125   #endif
00126   #ifdef VARIABLE_SPINDLE
00127     uint8_t spindle_pwm;
00128   #endif
00129 } segment_t;
00130 static segment_t segment_buffer[SEGMENT_BUFFER_SIZE];
00131 
00132 // Stepper ISR data struct. Contains the running data for the main stepper ISR.
00133 typedef struct {
00134   // Used by the bresenham line algorithm
00135   uint32_t counter_x,        // Counter variables for the bresenham line tracer
00136            counter_y,
00137            counter_z;
00138   #ifdef STEP_PULSE_DELAY
00139     uint8_t step_bits;  // Stores out_bits output to complete the step pulse delay
00140   #endif
00141 
00142   uint8_t execute_step;     // Flags step execution for each interrupt.
00143 #ifndef WIN32
00144   uint8_t step_pulse_time;  // Step pulse reset time after step rise
00145 #else
00146   LONGLONG step_pulse_time;
00147 #endif
00148   PORTPINDEF step_outbits;         // The next stepping-bits to be output
00149   PORTPINDEF dir_outbits;
00150   #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
00151     uint32_t steps[N_AXIS];
00152   #endif
00153 
00154   uint16_t step_count;       // Steps remaining in line segment motion
00155   uint8_t exec_block_index; // Tracks the current st_block index. Change indicates new block.
00156   st_block_t *exec_block;   // Pointer to the block data for the segment being executed
00157   segment_t *exec_segment;  // Pointer to the segment being executed
00158 } stepper_t;
00159 static stepper_t st;
00160 
00161 // Step segment ring buffer indices
00162 static volatile uint8_t segment_buffer_tail;
00163 static uint8_t segment_buffer_head;
00164 static uint8_t segment_next_head;
00165 
00166 // Step and direction port invert masks.
00167 static PORTPINDEF step_port_invert_mask;
00168 static PORTPINDEF dir_port_invert_mask;
00169 
00170 // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though.
00171 static volatile uint8_t busy;
00172 
00173 // Pointers for the step segment being prepped from the planner buffer. Accessed only by the
00174 // main program. Pointers may be planning segments or planner blocks ahead of what being executed.
00175 static plan_block_t *pl_block;     // Pointer to the planner block being prepped
00176 static st_block_t *st_prep_block;  // Pointer to the stepper block data being prepped
00177 
00178 // Segment preparation data struct. Contains all the necessary information to compute new segments
00179 // based on the current executing planner block.
00180 typedef struct {
00181   uint8_t st_block_index;  // Index of stepper common data block being prepped
00182   uint8_t recalculate_flag;
00183 
00184   float dt_remainder;
00185   float steps_remaining;
00186   float step_per_mm;
00187   float req_mm_increment;
00188 
00189   #ifdef PARKING_ENABLE
00190     uint8_t last_st_block_index;
00191     float last_steps_remaining;
00192     float last_step_per_mm;
00193     float last_dt_remainder;
00194   #endif
00195 
00196   uint8_t ramp_type;      // Current segment ramp state
00197   float mm_complete;      // End of velocity profile from end of current planner block in (mm).
00198                           // NOTE: This value must coincide with a step(no mantissa) when converted.
00199   float current_speed;    // Current speed at the end of the segment buffer (mm/min)
00200   float maximum_speed;    // Maximum speed of executing block. Not always nominal speed. (mm/min)
00201   float exit_speed;       // Exit speed of executing block (mm/min)
00202   float accelerate_until; // Acceleration ramp end measured from end of block (mm)
00203   float decelerate_after; // Deceleration ramp start measured from end of block (mm)
00204 
00205   #ifdef VARIABLE_SPINDLE
00206     float inv_rate;    // Used by PWM laser mode to speed up segment calculations.
00207     uint8_t current_spindle_pwm;
00208   #endif
00209 } st_prep_t;
00210 static st_prep_t prep;
00211 
00212 
00213 /*    BLOCK VELOCITY PROFILE DEFINITION
00214           __________________________
00215          /|                        |\     _________________         ^
00216         / |                        | \   /|               |\        |
00217        /  |                        |  \ / |               | \       s
00218       /   |                        |   |  |               |  \      p
00219      /    |                        |   |  |               |   \     e
00220     +-----+------------------------+---+--+---------------+----+    e
00221     |               BLOCK 1            ^      BLOCK 2          |    d
00222                                        |
00223                   time ----->      EXAMPLE: Block 2 entry speed is at max junction velocity
00224 
00225   The planner block buffer is planned assuming constant acceleration velocity profiles and are
00226   continuously joined at block junctions as shown above. However, the planner only actively computes
00227   the block entry speeds for an optimal velocity plan, but does not compute the block internal
00228   velocity profiles. These velocity profiles are computed ad-hoc as they are executed by the
00229   stepper algorithm and consists of only 7 possible types of profiles: cruise-only, cruise-
00230   deceleration, acceleration-cruise, acceleration-only, deceleration-only, full-trapezoid, and
00231   triangle(no cruise).
00232 
00233                                         maximum_speed (< nominal_speed) ->  +
00234                     +--------+ <- maximum_speed (= nominal_speed)          /|\
00235                    /          \                                           / | \
00236  current_speed -> +            \                                         /  |  + <- exit_speed
00237                   |             + <- exit_speed                         /   |  |
00238                   +-------------+                     current_speed -> +----+--+
00239                    time -->  ^  ^                                           ^  ^
00240                              |  |                                           |  |
00241                 decelerate_after(in mm)                             decelerate_after(in mm)
00242                     ^           ^                                           ^  ^
00243                     |           |                                           |  |
00244                 accelerate_until(in mm)                             accelerate_until(in mm)
00245 
00246   The step segment buffer computes the executing block velocity profile and tracks the critical
00247   parameters for the stepper algorithm to accurately trace the profile. These critical parameters
00248   are shown and defined in the above illustration.
00249 */
00250 
00251 
00252 // Stepper state initialization. Cycle should only start if the st.cycle_start flag is
00253 // enabled. Startup init and limits call this function but shouldn't start the cycle.
00254 void st_wake_up()
00255 {
00256   // Enable stepper drivers.
00257   if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) 
00258   { 
00259       SetStepperDisableBit();
00260   }
00261   else 
00262   { 
00263       ResetStepperDisableBit(); 
00264   }
00265 
00266   // Initialize stepper output bits to ensure first ISR call does not step.
00267   st.step_outbits = step_port_invert_mask;
00268 
00269   // Initialize step pulse timing from settings. Here to ensure updating after re-writing.
00270   #ifdef STEP_PULSE_DELAY
00271     // Set total step pulse time after direction pin set. Ad hoc computation from oscilloscope.
00272     st.step_pulse_time = -(((settings.pulse_microseconds+STEP_PULSE_DELAY-2)*TICKS_PER_MICROSECOND) >> 3);
00273     // Set delay between direction pin write and step command.
00274     OCR0A = -(((settings.pulse_microseconds)*TICKS_PER_MICROSECOND) >> 3);
00275   #else // Normal operation
00276     // Set step pulse time. Ad hoc computation from oscilloscope. Uses two's complement.
00277 #ifdef AVRTARGET
00278   st.step_pulse_time = -(((settings.pulse_microseconds - 2)*TICKS_PER_MICROSECOND) >> 3);
00279 #elif defined (WIN32)
00280   st.step_pulse_time = (settings.pulse_microseconds)*TICKS_PER_MICROSECOND;
00281 #elif defined(STM32F103C8)
00282   st.step_pulse_time = (settings.pulse_microseconds)*TICKS_PER_MICROSECOND;
00283 #endif
00284   #endif
00285 
00286   // Enable Stepper Driver Interrupt
00287 #ifdef AVRTARGET
00288   TIMSK1 |= (1<<OCIE1A);
00289 #endif
00290 #ifdef WIN32
00291   nTimer1Out = 1;
00292 #endif
00293 #if defined (STM32F103C8)
00294   TIM3->ARR = st.step_pulse_time - 1;
00295   TIM3->EGR = TIM_PSCReloadMode_Immediate;
00296   TIM_ClearITPendingBit(TIM3, TIM_IT_Update);
00297 
00298   TIM2->ARR = st.exec_segment->cycles_per_tick - 1;
00299   /* Set the Autoreload value */
00300 #ifndef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING        
00301   TIM2->PSC = st.exec_segment->prescaler;
00302 #endif
00303   TIM2->EGR = TIM_PSCReloadMode_Immediate;
00304   NVIC_EnableIRQ(TIM2_IRQn);
00305 #endif
00306 }
00307 
00308 
00309 // Stepper shutdown
00310 void st_go_idle()
00311 {
00312   // Disable Stepper Driver Interrupt. Allow Stepper Port Reset Interrupt to finish, if active.
00313 #ifdef AVRTARGET
00314   TIMSK1 &= ~(1<<OCIE1A); // Disable Timer1 interrupt
00315   TCCR1B = (TCCR1B & ~((1<<CS12) | (1<<CS11))) | (1<<CS10); // Reset clock to no prescaling.
00316 #endif
00317 #ifdef WIN32
00318   nTimer1Out = 0;
00319 #endif
00320 #ifdef STM32F103C8
00321   NVIC_DisableIRQ(TIM2_IRQn);
00322 #endif
00323 
00324   busy = false;
00325 
00326   // Set stepper driver idle state, disabled or enabled, depending on settings and circumstances.
00327   bool pin_state = false; // Keep enabled.
00328   if (((settings.stepper_idle_lock_time != 0xff) || sys_rt_exec_alarm || sys.state == STATE_SLEEP) && sys.state != STATE_HOMING) {
00329     // Force stepper dwell to lock axes for a defined amount of time to ensure the axes come to a complete
00330     // stop and not drift from residual inertial forces at the end of the last movement.
00331     delay_ms(settings.stepper_idle_lock_time);
00332     pin_state = true; // Override. Disable steppers.
00333   }
00334   if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { pin_state = !pin_state; } // Apply pin invert.
00335   if (pin_state) 
00336   { 
00337       SetStepperDisableBit();
00338   }
00339   else 
00340   { 
00341       ResetStepperDisableBit();
00342   }
00343 }
00344 
00345 
00346 /* "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. Grbl employs
00347    the venerable Bresenham line algorithm to manage and exactly synchronize multi-axis moves.
00348    Unlike the popular DDA algorithm, the Bresenham algorithm is not susceptible to numerical
00349    round-off errors and only requires fast integer counters, meaning low computational overhead
00350    and maximizing the Arduino's capabilities. However, the downside of the Bresenham algorithm
00351    is, for certain multi-axis motions, the non-dominant axes may suffer from un-smooth step
00352    pulse trains, or aliasing, which can lead to strange audible noises or shaking. This is
00353    particularly noticeable or may cause motion issues at low step frequencies (0-5kHz), but
00354    is usually not a physical problem at higher frequencies, although audible.
00355      To improve Bresenham multi-axis performance, Grbl uses what we call an Adaptive Multi-Axis
00356    Step Smoothing (AMASS) algorithm, which does what the name implies. At lower step frequencies,
00357    AMASS artificially increases the Bresenham resolution without effecting the algorithm's
00358    innate exactness. AMASS adapts its resolution levels automatically depending on the step
00359    frequency to be executed, meaning that for even lower step frequencies the step smoothing
00360    level increases. Algorithmically, AMASS is acheived by a simple bit-shifting of the Bresenham
00361    step count for each AMASS level. For example, for a Level 1 step smoothing, we bit shift
00362    the Bresenham step event count, effectively multiplying it by 2, while the axis step counts
00363    remain the same, and then double the stepper ISR frequency. In effect, we are allowing the
00364    non-dominant Bresenham axes step in the intermediate ISR tick, while the dominant axis is
00365    stepping every two ISR ticks, rather than every ISR tick in the traditional sense. At AMASS
00366    Level 2, we simply bit-shift again, so the non-dominant Bresenham axes can step within any
00367    of the four ISR ticks, the dominant axis steps every four ISR ticks, and quadruple the
00368    stepper ISR frequency. And so on. This, in effect, virtually eliminates multi-axis aliasing
00369    issues with the Bresenham algorithm and does not significantly alter Grbl's performance, but
00370    in fact, more efficiently utilizes unused CPU cycles overall throughout all configurations.
00371      AMASS retains the Bresenham algorithm exactness by requiring that it always executes a full
00372    Bresenham step, regardless of AMASS Level. Meaning that for an AMASS Level 2, all four
00373    intermediate steps must be completed such that baseline Bresenham (Level 0) count is always
00374    retained. Similarly, AMASS Level 3 means all eight intermediate steps must be executed.
00375    Although the AMASS Levels are in reality arbitrary, where the baseline Bresenham counts can
00376    be multiplied by any integer value, multiplication by powers of two are simply used to ease
00377    CPU overhead with bitshift integer operations.
00378      This interrupt is simple and dumb by design. All the computational heavy-lifting, as in
00379    determining accelerations, is performed elsewhere. This interrupt pops pre-computed segments,
00380    defined as constant velocity over n number of steps, from the step segment buffer and then
00381    executes them by pulsing the stepper pins appropriately via the Bresenham algorithm. This
00382    ISR is supported by The Stepper Port Reset Interrupt which it uses to reset the stepper port
00383    after each pulse. The bresenham line tracer algorithm controls all stepper outputs
00384    simultaneously with these two interrupts.
00385 
00386    NOTE: This interrupt must be as efficient as possible and complete before the next ISR tick,
00387    which for Grbl must be less than 33.3usec (@30kHz ISR rate). Oscilloscope measured time in
00388    ISR is 5usec typical and 25usec maximum, well below requirement.
00389    NOTE: This ISR expects at least one step to be executed per segment.
00390 */
00391 // TODO: Replace direct updating of the int32 position counters in the ISR somehow. Perhaps use smaller
00392 // int8 variables and update position counters only when a segment completes. This can get complicated
00393 // with probing and homing cycles that require true real-time positions.
00394 #ifdef STM32F103C8
00395 void TIM2_IRQHandler(void)
00396 #endif
00397 #ifdef AVRTARGET
00398 ISR(TIMER1_COMPA_vect)
00399 #endif
00400 #ifdef WIN32
00401 void Timer1Proc()
00402 #endif
00403 {
00404 #ifdef STM32F103C8
00405     if ((TIM2->SR & 0x0001) != 0)                  // check interrupt source
00406     {
00407         TIM2->SR &= ~(1 << 0);                          // clear UIF flag
00408         TIM2->CNT = 0;
00409     }
00410     else
00411     {
00412         return;
00413     }
00414 #endif
00415 
00416   if (busy) { return; } // The busy-flag is used to avoid reentering this interrupt
00417 #ifdef AVRTARGET
00418   // Set the direction pins a couple of nanoseconds before we step the steppers
00419   DIRECTION_PORT = (DIRECTION_PORT & ~DIRECTION_MASK) | (st.dir_outbits & DIRECTION_MASK);
00420 #endif
00421 #ifdef STM32F103C8
00422   GPIO_Write(DIRECTION_PORT, (GPIO_ReadOutputData(DIRECTION_PORT) & ~DIRECTION_MASK) | (st.dir_outbits & DIRECTION_MASK));
00423   TIM_ClearITPendingBit(TIM3, TIM_IT_Update);
00424 #endif
00425 
00426   // Then pulse the stepping pins
00427   #ifdef STEP_PULSE_DELAY
00428     st.step_bits = (STEP_PORT & ~STEP_MASK) | st.step_outbits; // Store out_bits to prevent overwriting.
00429   #else  // Normal operation
00430 #ifdef AVRTARGET
00431     STEP_PORT = (STEP_PORT & ~STEP_MASK) | st.step_outbits;
00432 #endif
00433 #ifdef STM32F103C8
00434     GPIO_Write(STEP_PORT, (GPIO_ReadOutputData(STEP_PORT) & ~STEP_MASK) | st.step_outbits);
00435 #endif
00436   #endif
00437 
00438   // Enable step pulse reset timer so that The Stepper Port Reset Interrupt can reset the signal after
00439   // exactly settings.pulse_microseconds microseconds, independent of the main Timer1 prescaler.
00440 #ifdef AVRTARGET
00441   TCNT0 = st.step_pulse_time; // Reload Timer0 counter
00442   TCCR0B = (1<<CS01); // Begin Timer0. Full speed, 1/8 prescaler
00443 #endif
00444 #ifdef WIN32
00445   nTimer0Out = st.step_pulse_time;
00446 #endif
00447 #ifdef STM32F103C8
00448   NVIC_EnableIRQ(TIM3_IRQn);
00449 #endif
00450 
00451   busy = true;
00452 #ifdef AVRTARGET
00453   sei(); // Re-enable interrupts to allow Stepper Port Reset Interrupt to fire on-time.
00454          // NOTE: The remaining code in this ISR will finish before returning to main program.
00455 #endif
00456 
00457   // If there is no step segment, attempt to pop one from the stepper buffer
00458   if (st.exec_segment == NULL) {
00459     // Anything in the buffer? If so, load and initialize next step segment.
00460     if (segment_buffer_head != segment_buffer_tail) {
00461       // Initialize new step segment and load number of steps to execute
00462       st.exec_segment = &segment_buffer[segment_buffer_tail];
00463 
00464       #ifndef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
00465         // With AMASS is disabled, set timer prescaler for segments with slow step frequencies (< 250Hz).
00466 #ifdef AVRTARGET
00467         TCCR1B = (TCCR1B & ~(0x07<<CS10)) | (st.exec_segment->prescaler<<CS10);
00468 #endif
00469       #endif
00470 
00471       // Initialize step segment timing per step and load number of steps to execute.
00472 #ifdef AVRTARGET
00473       OCR1A = st.exec_segment->cycles_per_tick;
00474 #endif
00475 #ifdef WIN32
00476 #ifndef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
00477       nTimer1Out = st.exec_segment->cycles_per_tick * (st.exec_segment->prescaler + 1);
00478 #else
00479       nTimer1Out = st.exec_segment->cycles_per_tick;
00480 #endif
00481 #endif
00482 #ifdef STM32F103C8
00483       TIM2->ARR = st.exec_segment->cycles_per_tick - 1;
00484       /* Set the Autoreload value */
00485 #ifndef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING        
00486       TIM2->PSC = st.exec_segment->prescaler;
00487 #endif
00488 #endif
00489       st.step_count = st.exec_segment->n_step; // NOTE: Can sometimes be zero when moving slow.
00490       // If the new segment starts a new planner block, initialize stepper variables and counters.
00491       // NOTE: When the segment data index changes, this indicates a new planner block.
00492       if ( st.exec_block_index != st.exec_segment->st_block_index ) {
00493         st.exec_block_index = st.exec_segment->st_block_index;
00494         st.exec_block = &st_block_buffer[st.exec_block_index];
00495 
00496         // Initialize Bresenham line and distance counters
00497         st.counter_x = st.counter_y = st.counter_z = (st.exec_block->step_event_count >> 1);
00498       }
00499       st.dir_outbits = st.exec_block->direction_bits ^ dir_port_invert_mask;
00500 
00501       #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
00502         // With AMASS enabled, adjust Bresenham axis increment counters according to AMASS level.
00503         st.steps[X_AXIS] = st.exec_block->steps[X_AXIS] >> st.exec_segment->amass_level;
00504         st.steps[Y_AXIS] = st.exec_block->steps[Y_AXIS] >> st.exec_segment->amass_level;
00505         st.steps[Z_AXIS] = st.exec_block->steps[Z_AXIS] >> st.exec_segment->amass_level;
00506       #endif
00507 
00508       #ifdef VARIABLE_SPINDLE
00509         // Set real-time spindle output as segment is loaded, just prior to the first step.
00510         spindle_set_speed(st.exec_segment->spindle_pwm);
00511       #endif
00512 
00513     } else {
00514       // Segment buffer empty. Shutdown.
00515       st_go_idle();
00516       // Ensure pwm is set properly upon completion of rate-controlled motion.
00517       #ifdef VARIABLE_SPINDLE
00518       if (st.exec_block->is_pwm_rate_adjusted) { spindle_set_speed(SPINDLE_PWM_OFF_VALUE); }
00519       #endif
00520       system_set_exec_state_flag(EXEC_CYCLE_STOP); // Flag main program for cycle end
00521       return; // Nothing to do but exit.
00522     }
00523   }
00524 
00525 
00526   // Check probing state.
00527   if (sys_probe_state == PROBE_ACTIVE) { probe_state_monitor(); }
00528 
00529   // Reset step out bits.
00530   st.step_outbits = 0;
00531 
00532   // Execute step displacement profile by Bresenham line algorithm
00533   #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
00534     st.counter_x += st.steps[X_AXIS];
00535   #else
00536     st.counter_x += st.exec_block->steps[X_AXIS];
00537   #endif
00538   if (st.counter_x > st.exec_block->step_event_count) {
00539     st.step_outbits |= (1<<X_STEP_BIT);
00540     st.counter_x -= st.exec_block->step_event_count;
00541     if (st.exec_block->direction_bits & (1<<X_DIRECTION_BIT)) { sys_position[X_AXIS]--; }
00542     else { sys_position[X_AXIS]++; }
00543   }
00544   #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
00545     st.counter_y += st.steps[Y_AXIS];
00546   #else
00547     st.counter_y += st.exec_block->steps[Y_AXIS];
00548   #endif
00549   if (st.counter_y > st.exec_block->step_event_count) {
00550     st.step_outbits |= (1<<Y_STEP_BIT);
00551     st.counter_y -= st.exec_block->step_event_count;
00552     if (st.exec_block->direction_bits & (1<<Y_DIRECTION_BIT)) { sys_position[Y_AXIS]--; }
00553     else { sys_position[Y_AXIS]++; }
00554   }
00555   #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
00556     st.counter_z += st.steps[Z_AXIS];
00557   #else
00558     st.counter_z += st.exec_block->steps[Z_AXIS];
00559   #endif
00560   if (st.counter_z > st.exec_block->step_event_count) {
00561     st.step_outbits |= (1<<Z_STEP_BIT);
00562     st.counter_z -= st.exec_block->step_event_count;
00563     if (st.exec_block->direction_bits & (1<<Z_DIRECTION_BIT)) { sys_position[Z_AXIS]--; }
00564     else { sys_position[Z_AXIS]++; }
00565   }
00566 
00567   // During a homing cycle, lock out and prevent desired axes from moving.
00568   if (sys.state == STATE_HOMING) { st.step_outbits &= sys.homing_axis_lock; }
00569 
00570   st.step_count--; // Decrement step events count
00571   if (st.step_count == 0) {
00572     // Segment is complete. Discard current segment and advance segment indexing.
00573     st.exec_segment = NULL;
00574 #ifndef WIN32
00575     uint8_t segment_tail_next = segment_buffer_tail + 1;
00576     if (segment_tail_next == SEGMENT_BUFFER_SIZE)
00577         segment_tail_next = 0;
00578     segment_buffer_tail = segment_tail_next;
00579 #else
00580     if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) 
00581     { 
00582         segment_buffer_tail = 0; 
00583     }
00584 #endif
00585   }
00586 
00587   st.step_outbits ^= step_port_invert_mask;  // Apply step port invert mask
00588   busy = false;
00589 }
00590 
00591 
00592 /* The Stepper Port Reset Interrupt: Timer0 OVF interrupt handles the falling edge of the step
00593    pulse. This should always trigger before the next Timer1 COMPA interrupt and independently
00594    finish, if Timer1 is disabled after completing a move.
00595    NOTE: Interrupt collisions between the serial and stepper interrupts can cause delays by
00596    a few microseconds, if they execute right before one another. Not a big deal, but can
00597    cause issues at high step rates if another high frequency asynchronous interrupt is
00598    added to Grbl.
00599 */
00600 // This interrupt is enabled by ISR_TIMER1_COMPAREA when it sets the motor port bits to execute
00601 // a step. This ISR resets the motor port after a short period (settings.pulse_microseconds)
00602 // completing one step cycle.
00603 #ifdef STM32F103C8
00604 void TIM3_IRQHandler(void)
00605 #endif
00606 #ifdef AVRTARGET
00607 ISR(TIMER0_OVF_vect)
00608 #endif
00609 #ifdef WIN32
00610 void Timer0Proc()
00611 #endif
00612 {
00613 #ifdef STM32F103C8
00614     if ((TIM3->SR & 0x0001) != 0)                  // check interrupt source
00615     {
00616         TIM3->SR &= ~(1<<0);                          // clear UIF flag
00617         TIM3->CNT = 0;
00618         NVIC_DisableIRQ(TIM3_IRQn);
00619         GPIO_Write(STEP_PORT, (GPIO_ReadOutputData(STEP_PORT) & ~STEP_MASK) | (step_port_invert_mask & STEP_MASK));
00620     }
00621 #endif
00622 #ifdef AVRTARGET
00623   // Reset stepping pins (leave the direction pins)
00624   STEP_PORT = (STEP_PORT & ~STEP_MASK) | (step_port_invert_mask & STEP_MASK);
00625   TCCR0B = 0; // Disable Timer0 to prevent re-entering this interrupt when it's not needed.
00626 #endif
00627 #ifdef WIN32
00628   nTimer0Out = 0;
00629 #endif
00630 }
00631 #ifdef STEP_PULSE_DELAY
00632   // This interrupt is used only when STEP_PULSE_DELAY is enabled. Here, the step pulse is
00633   // initiated after the STEP_PULSE_DELAY time period has elapsed. The ISR TIMER2_OVF interrupt
00634   // will then trigger after the appropriate settings.pulse_microseconds, as in normal operation.
00635   // The new timing between direction, step pulse, and step complete events are setup in the
00636   // st_wake_up() routine.
00637   ISR(TIMER0_COMPA_vect)
00638   {
00639     STEP_PORT = st.step_bits; // Begin step pulse.
00640   }
00641 #endif
00642 
00643 
00644 // Generates the step and direction port invert masks used in the Stepper Interrupt Driver.
00645 void st_generate_step_dir_invert_masks()
00646 {
00647   uint8_t idx;
00648   step_port_invert_mask = 0;
00649   dir_port_invert_mask = 0;
00650   for (idx=0; idx<N_AXIS; idx++) {
00651     if (bit_istrue(settings.step_invert_mask,bit(idx))) { step_port_invert_mask |= step_pin_mask[idx]; }
00652     if (bit_istrue(settings.dir_invert_mask,bit(idx))) { dir_port_invert_mask |= direction_pin_mask[idx]; }
00653   }
00654 }
00655 
00656 
00657 // Reset and clear stepper subsystem variables
00658 void st_reset()
00659 {
00660   // Initialize stepper driver idle state.
00661   st_go_idle();
00662 
00663   // Initialize stepper algorithm variables.
00664   memset(&prep, 0, sizeof(st_prep_t));
00665   memset(&st, 0, sizeof(stepper_t));
00666   st.exec_segment = NULL;
00667   pl_block = NULL;  // Planner block pointer used by segment buffer
00668   segment_buffer_tail = 0;
00669   segment_buffer_head = 0; // empty = tail
00670   segment_next_head = 1;
00671   busy = false;
00672 
00673   st_generate_step_dir_invert_masks();
00674   st.dir_outbits = dir_port_invert_mask; // Initialize direction bits to default.
00675 
00676   // Initialize step and direction port pins.
00677 #ifdef AVRTARGET
00678   STEP_PORT = (STEP_PORT & ~STEP_MASK) | step_port_invert_mask;
00679   DIRECTION_PORT = (DIRECTION_PORT & ~DIRECTION_MASK) | dir_port_invert_mask;
00680 #endif
00681 #ifdef STM32F103C8
00682   GPIO_Write(STEP_PORT, (GPIO_ReadOutputData(STEP_PORT) & ~STEP_MASK) | (step_port_invert_mask & STEP_MASK));
00683   GPIO_Write(DIRECTION_PORT, (GPIO_ReadOutputData(DIRECTION_PORT) & ~DIRECTION_MASK) | (dir_port_invert_mask & DIRECTION_MASK));
00684 #endif
00685 }
00686 
00687 #ifdef WIN32
00688 void Timer1Thread(void *pVoid)
00689 {
00690     LARGE_INTEGER StartingTime, EndingTime, ElapsedMicroseconds;
00691 
00692     for (;;)
00693     {
00694         while (nTimer1Out == 0)
00695             Sleep(0);
00696         QueryPerformanceCounter(&StartingTime);
00697         do
00698         {
00699             QueryPerformanceCounter(&EndingTime);
00700             ElapsedMicroseconds.QuadPart = EndingTime.QuadPart - StartingTime.QuadPart;
00701         } while (ElapsedMicroseconds.QuadPart < nTimer1Out);
00702         Timer1Proc();
00703     }
00704 }
00705 
00706 void Timer0Thread(void *pVoid)
00707 {
00708     LARGE_INTEGER StartingTime, EndingTime, ElapsedMicroseconds;
00709 
00710     for (;;)
00711     {
00712         while (nTimer0Out == 0)
00713             Sleep(0);
00714         QueryPerformanceCounter(&StartingTime);
00715         do
00716         {
00717             QueryPerformanceCounter(&EndingTime);
00718             ElapsedMicroseconds.QuadPart = EndingTime.QuadPart - StartingTime.QuadPart;
00719         } while (ElapsedMicroseconds.QuadPart < nTimer0Out);
00720         Timer0Proc();
00721     }
00722 }
00723 
00724 #endif
00725 
00726 // Initialize and start the stepper motor subsystem
00727 void stepper_init()
00728 {
00729   // Configure step and direction interface pins
00730 #ifdef STM32F103C8
00731     GPIO_InitTypeDef GPIO_InitStructure;
00732     RCC_APB2PeriphClockCmd(RCC_STEPPERS_DISABLE_PORT, ENABLE);
00733     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
00734     GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
00735     GPIO_InitStructure.GPIO_Pin = STEPPERS_DISABLE_MASK;
00736     GPIO_Init(STEPPERS_DISABLE_PORT, &GPIO_InitStructure);
00737 
00738     RCC_APB2PeriphClockCmd(RCC_STEP_PORT, ENABLE);
00739     GPIO_InitStructure.GPIO_Pin = STEP_MASK;
00740     GPIO_Init(STEP_PORT, &GPIO_InitStructure);
00741 
00742     RCC_APB2PeriphClockCmd(RCC_DIRECTION_PORT, ENABLE);
00743     GPIO_InitStructure.GPIO_Pin = DIRECTION_MASK;
00744     GPIO_Init(DIRECTION_PORT, &GPIO_InitStructure);
00745 
00746     RCC->APB1ENR |= RCC_APB1Periph_TIM2;
00747     TIM_Configuration(TIM2, 1, 1, 1);
00748     RCC->APB1ENR |= RCC_APB1Periph_TIM3;
00749     TIM_Configuration(TIM3, 1, 1, 1);
00750     NVIC_DisableIRQ(TIM3_IRQn);
00751     NVIC_DisableIRQ(TIM2_IRQn);
00752 #endif
00753 #ifdef AVRTARGET
00754   STEP_DDR |= STEP_MASK;
00755   STEPPERS_DISABLE_DDR |= 1<<STEPPERS_DISABLE_BIT;
00756   DIRECTION_DDR |= DIRECTION_MASK;
00757 
00758   // Configure Timer 1: Stepper Driver Interrupt
00759   TCCR1B &= ~(1<<WGM13); // waveform generation = 0100 = CTC
00760   TCCR1B |=  (1<<WGM12);
00761   TCCR1A &= ~((1<<WGM11) | (1<<WGM10));
00762   TCCR1A &= ~((1<<COM1A1) | (1<<COM1A0) | (1<<COM1B1) | (1<<COM1B0)); // Disconnect OC1 output
00763   // TCCR1B = (TCCR1B & ~((1<<CS12) | (1<<CS11))) | (1<<CS10); // Set in st_go_idle().
00764   // TIMSK1 &= ~(1<<OCIE1A);  // Set in st_go_idle().
00765 
00766   // Configure Timer 0: Stepper Port Reset Interrupt
00767   TIMSK0 &= ~((1<<OCIE0B) | (1<<OCIE0A) | (1<<TOIE0)); // Disconnect OC0 outputs and OVF interrupt.
00768   TCCR0A = 0; // Normal operation
00769   TCCR0B = 0; // Disable Timer0 until needed
00770   TIMSK0 |= (1<<TOIE0); // Enable Timer0 overflow interrupt
00771   #ifdef STEP_PULSE_DELAY
00772     TIMSK0 |= (1<<OCIE0A); // Enable Timer0 Compare Match A interrupt
00773   #endif
00774 #endif
00775 #ifdef WIN32
00776     QueryPerformanceFrequency(&Win32Frequency);
00777 
00778     _beginthread(Timer1Thread, 0, NULL);
00779     _beginthread(Timer0Thread, 0, NULL);
00780 #endif
00781 }
00782 
00783 
00784 // Called by planner_recalculate() when the executing block is updated by the new plan.
00785 void st_update_plan_block_parameters()
00786 {
00787   if (pl_block != NULL) { // Ignore if at start of a new block.
00788     prep.recalculate_flag |= PREP_FLAG_RECALCULATE;
00789     pl_block->entry_speed_sqr = prep.current_speed*prep.current_speed; // Update entry speed.
00790     pl_block = NULL; // Flag st_prep_segment() to load and check active velocity profile.
00791   }
00792 }
00793 
00794 
00795 // Increments the step segment buffer block data ring buffer.
00796 static uint8_t st_next_block_index(uint8_t block_index)
00797 {
00798   block_index++;
00799   if ( block_index == (SEGMENT_BUFFER_SIZE-1) ) { return(0); }
00800   return(block_index);
00801 }
00802 
00803 
00804 #ifdef PARKING_ENABLE
00805   // Changes the run state of the step segment buffer to execute the special parking motion.
00806   void st_parking_setup_buffer()
00807   {
00808     // Store step execution data of partially completed block, if necessary.
00809     if (prep.recalculate_flag & PREP_FLAG_HOLD_PARTIAL_BLOCK) {
00810       prep.last_st_block_index = prep.st_block_index;
00811       prep.last_steps_remaining = prep.steps_remaining;
00812       prep.last_dt_remainder = prep.dt_remainder;
00813       prep.last_step_per_mm = prep.step_per_mm;
00814     }
00815     // Set flags to execute a parking motion
00816     prep.recalculate_flag |= PREP_FLAG_PARKING;
00817     prep.recalculate_flag &= ~(PREP_FLAG_RECALCULATE);
00818     pl_block = NULL; // Always reset parking motion to reload new block.
00819   }
00820 
00821 
00822   // Restores the step segment buffer to the normal run state after a parking motion.
00823   void st_parking_restore_buffer()
00824   {
00825     // Restore step execution data and flags of partially completed block, if necessary.
00826     if (prep.recalculate_flag & PREP_FLAG_HOLD_PARTIAL_BLOCK) {
00827       st_prep_block = &st_block_buffer[prep.last_st_block_index];
00828       prep.st_block_index = prep.last_st_block_index;
00829       prep.steps_remaining = prep.last_steps_remaining;
00830       prep.dt_remainder = prep.last_dt_remainder;
00831       prep.step_per_mm = prep.last_step_per_mm;
00832       prep.recalculate_flag = (PREP_FLAG_HOLD_PARTIAL_BLOCK | PREP_FLAG_RECALCULATE);
00833       prep.req_mm_increment = REQ_MM_INCREMENT_SCALAR/prep.step_per_mm; // Recompute this value.
00834     } else {
00835       prep.recalculate_flag = false;
00836     }
00837     pl_block = NULL; // Set to reload next block.
00838   }
00839 #endif
00840 
00841 
00842 /* Prepares step segment buffer. Continuously called from main program.
00843 
00844    The segment buffer is an intermediary buffer interface between the execution of steps
00845    by the stepper algorithm and the velocity profiles generated by the planner. The stepper
00846    algorithm only executes steps within the segment buffer and is filled by the main program
00847    when steps are "checked-out" from the first block in the planner buffer. This keeps the
00848    step execution and planning optimization processes atomic and protected from each other.
00849    The number of steps "checked-out" from the planner buffer and the number of segments in
00850    the segment buffer is sized and computed such that no operation in the main program takes
00851    longer than the time it takes the stepper algorithm to empty it before refilling it.
00852    Currently, the segment buffer conservatively holds roughly up to 40-50 msec of steps.
00853    NOTE: Computation units are in steps, millimeters, and minutes.
00854 */
00855 void st_prep_buffer()
00856 {
00857   // Block step prep buffer, while in a suspend state and there is no suspend motion to execute.
00858   if (bit_istrue(sys.step_control,STEP_CONTROL_END_MOTION)) { return; }
00859 
00860   while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer.
00861 
00862     // Determine if we need to load a new planner block or if the block needs to be recomputed.
00863     if (pl_block == NULL) {
00864 
00865       // Query planner for a queued block
00866       if (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION) { pl_block = plan_get_system_motion_block(); }
00867       else { pl_block = plan_get_current_block(); }
00868       if (pl_block == NULL) { return; } // No planner blocks. Exit.
00869 
00870       // Check if we need to only recompute the velocity profile or load a new block.
00871       if (prep.recalculate_flag & PREP_FLAG_RECALCULATE) {
00872 
00873         #ifdef PARKING_ENABLE
00874           if (prep.recalculate_flag & PREP_FLAG_PARKING) { prep.recalculate_flag &= ~(PREP_FLAG_RECALCULATE); }
00875           else { prep.recalculate_flag = false; }
00876         #else
00877           prep.recalculate_flag = false;
00878         #endif
00879 
00880       } else {
00881 
00882         // Load the Bresenham stepping data for the block.
00883         prep.st_block_index = st_next_block_index(prep.st_block_index);
00884 
00885         // Prepare and copy Bresenham algorithm segment data from the new planner block, so that
00886         // when the segment buffer completes the planner block, it may be discarded when the
00887         // segment buffer finishes the prepped block, but the stepper ISR is still executing it.
00888         st_prep_block = &st_block_buffer[prep.st_block_index];
00889         st_prep_block->direction_bits = pl_block->direction_bits;
00890         uint8_t idx;
00891         #ifndef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
00892           for (idx=0; idx<N_AXIS; idx++) { st_prep_block->steps[idx] = (pl_block->steps[idx] << 1); }
00893           st_prep_block->step_event_count = (pl_block->step_event_count << 1);
00894         #else
00895           // With AMASS enabled, simply bit-shift multiply all Bresenham data by the max AMASS
00896           // level, such that we never divide beyond the original data anywhere in the algorithm.
00897           // If the original data is divided, we can lose a step from integer roundoff.
00898           for (idx=0; idx<N_AXIS; idx++) { st_prep_block->steps[idx] = pl_block->steps[idx] << MAX_AMASS_LEVEL; }
00899           st_prep_block->step_event_count = pl_block->step_event_count << MAX_AMASS_LEVEL;
00900         #endif
00901 
00902         // Initialize segment buffer data for generating the segments.
00903         prep.steps_remaining = (float)pl_block->step_event_count;
00904         prep.step_per_mm = prep.steps_remaining/pl_block->millimeters;
00905         prep.req_mm_increment = REQ_MM_INCREMENT_SCALAR/prep.step_per_mm;
00906         prep.dt_remainder = 0.0f; // Reset for new segment block
00907 
00908         if ((sys.step_control & STEP_CONTROL_EXECUTE_HOLD) || (prep.recalculate_flag & PREP_FLAG_DECEL_OVERRIDE)) {
00909           // New block loaded mid-hold. Override planner block entry speed to enforce deceleration.
00910           prep.current_speed = prep.exit_speed;
00911           pl_block->entry_speed_sqr = prep.exit_speed*prep.exit_speed;
00912           prep.recalculate_flag &= ~(PREP_FLAG_DECEL_OVERRIDE);
00913         } else {
00914           prep.current_speed = sqrtf(pl_block->entry_speed_sqr);
00915         }
00916 #ifdef VARIABLE_SPINDLE
00917         // Setup laser mode variables. PWM rate adjusted motions will always complete a motion with the
00918         // spindle off. 
00919         st_prep_block->is_pwm_rate_adjusted = false;
00920         if (settings.flags & BITFLAG_LASER_MODE) {
00921           if (pl_block->condition & PL_COND_FLAG_SPINDLE_CCW) {
00922             // Pre-compute inverse programmed rate to speed up PWM updating per step segment.
00923             prep.inv_rate = 1.0 / pl_block->programmed_rate;
00924             st_prep_block->is_pwm_rate_adjusted = true;
00925           }
00926         }
00927 #endif
00928       }
00929 
00930             /* ---------------------------------------------------------------------------------
00931              Compute the velocity profile of a new planner block based on its entry and exit
00932              speeds, or recompute the profile of a partially-completed planner block if the
00933              planner has updated it. For a commanded forced-deceleration, such as from a feed
00934              hold, override the planner velocities and decelerate to the target exit speed.
00935             */
00936             prep.mm_complete = 0.0f; // Default velocity profile complete at 0.0mm from end of block.
00937             float inv_2_accel = 0.5f/pl_block->acceleration;
00938             if (sys.step_control & STEP_CONTROL_EXECUTE_HOLD) { // [Forced Deceleration to Zero Velocity]
00939                 // Compute velocity profile parameters for a feed hold in-progress. This profile overrides
00940                 // the planner block profile, enforcing a deceleration to zero speed.
00941                 prep.ramp_type = RAMP_DECEL;
00942                 // Compute decelerate distance relative to end of block.
00943                 float decel_dist = pl_block->millimeters - inv_2_accel*pl_block->entry_speed_sqr;
00944                 if (decel_dist < 0.0f) {
00945                     // Deceleration through entire planner block. End of feed hold is not in this block.
00946                     prep.exit_speed = sqrtf(pl_block->entry_speed_sqr-2*pl_block->acceleration*pl_block->millimeters);
00947                 } else {
00948                     prep.mm_complete = decel_dist; // End of feed hold.
00949                     prep.exit_speed = 0.0f;
00950                 }
00951             } else { // [Normal Operation]
00952                 // Compute or recompute velocity profile parameters of the prepped planner block.
00953                 prep.ramp_type = RAMP_ACCEL; // Initialize as acceleration ramp.
00954                 prep.accelerate_until = pl_block->millimeters;
00955 
00956                 float exit_speed_sqr;
00957                 float nominal_speed;
00958         if (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION) {
00959           prep.exit_speed = exit_speed_sqr = 0.0f; // Enforce stop at end of system motion.
00960         } else {
00961           exit_speed_sqr = plan_get_exec_block_exit_speed_sqr();
00962           prep.exit_speed = sqrtf(exit_speed_sqr);
00963         }
00964 
00965         nominal_speed = plan_compute_profile_nominal_speed(pl_block);
00966                 float nominal_speed_sqr = nominal_speed*nominal_speed;
00967                 float intersect_distance =
00968                                 0.5f*(pl_block->millimeters+inv_2_accel*(pl_block->entry_speed_sqr-exit_speed_sqr));
00969 
00970         if (pl_block->entry_speed_sqr > nominal_speed_sqr) { // Only occurs during override reductions.
00971           prep.accelerate_until = pl_block->millimeters - inv_2_accel*(pl_block->entry_speed_sqr-nominal_speed_sqr);
00972           if (prep.accelerate_until <= 0.0f) { // Deceleration-only.
00973             prep.ramp_type = RAMP_DECEL;
00974             // prep.decelerate_after = pl_block->millimeters;
00975             // prep.maximum_speed = prep.current_speed;
00976 
00977             // Compute override block exit speed since it doesn't match the planner exit speed.
00978             prep.exit_speed = sqrtf(pl_block->entry_speed_sqr - 2*pl_block->acceleration*pl_block->millimeters);
00979             prep.recalculate_flag |= PREP_FLAG_DECEL_OVERRIDE; // Flag to load next block as deceleration override.
00980 
00981             // TODO: Determine correct handling of parameters in deceleration-only.
00982             // Can be tricky since entry speed will be current speed, as in feed holds.
00983             // Also, look into near-zero speed handling issues with this.
00984 
00985           } else {
00986             // Decelerate to cruise or cruise-decelerate types. Guaranteed to intersect updated plan.
00987             prep.decelerate_after = inv_2_accel*(nominal_speed_sqr-exit_speed_sqr);
00988             prep.maximum_speed = nominal_speed;
00989             prep.ramp_type = RAMP_DECEL_OVERRIDE;
00990           }
00991                 } else if (intersect_distance > 0.0f) {
00992                     if (intersect_distance < pl_block->millimeters) { // Either trapezoid or triangle types
00993                         // NOTE: For acceleration-cruise and cruise-only types, following calculation will be 0.0.
00994                         prep.decelerate_after = inv_2_accel*(nominal_speed_sqr-exit_speed_sqr);
00995                         if (prep.decelerate_after < intersect_distance) { // Trapezoid type
00996                             prep.maximum_speed = nominal_speed;
00997                             if (pl_block->entry_speed_sqr == nominal_speed_sqr) {
00998                                 // Cruise-deceleration or cruise-only type.
00999                                 prep.ramp_type = RAMP_CRUISE;
01000                             } else {
01001                                 // Full-trapezoid or acceleration-cruise types
01002                                 prep.accelerate_until -= inv_2_accel*(nominal_speed_sqr-pl_block->entry_speed_sqr);
01003                             }
01004                         } else { // Triangle type
01005                             prep.accelerate_until = intersect_distance;
01006                             prep.decelerate_after = intersect_distance;
01007                             prep.maximum_speed = sqrtf(2.0f*pl_block->acceleration*intersect_distance+exit_speed_sqr);
01008                         }
01009                     } else { // Deceleration-only type
01010             prep.ramp_type = RAMP_DECEL;
01011             // prep.decelerate_after = pl_block->millimeters;
01012             // prep.maximum_speed = prep.current_speed;
01013                     }
01014                 } else { // Acceleration-only type
01015                     prep.accelerate_until = 0.0f;
01016                     // prep.decelerate_after = 0.0f;
01017                     prep.maximum_speed = prep.exit_speed;
01018                 }
01019             }
01020       
01021       #ifdef VARIABLE_SPINDLE
01022         bit_true(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_PWM); // Force update whenever updating block.
01023       #endif
01024     }
01025     
01026     // Initialize new segment
01027     segment_t *prep_segment = &segment_buffer[segment_buffer_head];
01028 
01029     // Set new segment to point to the current segment data block.
01030     prep_segment->st_block_index = prep.st_block_index;
01031 
01032     /*------------------------------------------------------------------------------------
01033         Compute the average velocity of this new segment by determining the total distance
01034       traveled over the segment time DT_SEGMENT. The following code first attempts to create
01035       a full segment based on the current ramp conditions. If the segment time is incomplete
01036       when terminating at a ramp state change, the code will continue to loop through the
01037       progressing ramp states to fill the remaining segment execution time. However, if
01038       an incomplete segment terminates at the end of the velocity profile, the segment is
01039       considered completed despite having a truncated execution time less than DT_SEGMENT.
01040         The velocity profile is always assumed to progress through the ramp sequence:
01041       acceleration ramp, cruising state, and deceleration ramp. Each ramp's travel distance
01042       may range from zero to the length of the block. Velocity profiles can end either at
01043       the end of planner block (typical) or mid-block at the end of a forced deceleration,
01044       such as from a feed hold.
01045     */
01046     float dt_max = DT_SEGMENT; // Maximum segment time
01047     float dt = 0.0f; // Initialize segment time
01048     float time_var = dt_max; // Time worker variable
01049     float mm_var; // mm-Distance worker variable
01050     float speed_var; // Speed worker variable
01051     float mm_remaining = pl_block->millimeters; // New segment distance from end of block.
01052     float minimum_mm = mm_remaining-prep.req_mm_increment; // Guarantee at least one step.
01053     if (minimum_mm < 0.0f) { minimum_mm = 0.0f; }
01054 
01055     do {
01056       switch (prep.ramp_type) {
01057         case RAMP_DECEL_OVERRIDE:
01058           speed_var = pl_block->acceleration*time_var;
01059           mm_var = time_var*(prep.current_speed - 0.5f*speed_var);
01060           mm_remaining -= mm_var;
01061           if ((mm_remaining < prep.accelerate_until) || (mm_var <= 0)) {
01062             // Cruise or cruise-deceleration types only for deceleration override.
01063             mm_remaining = prep.accelerate_until; // NOTE: 0.0 at EOB
01064             time_var = 2.0f*(pl_block->millimeters-mm_remaining)/(prep.current_speed+prep.maximum_speed);
01065             prep.ramp_type = RAMP_CRUISE;
01066             prep.current_speed = prep.maximum_speed;
01067           } else { // Mid-deceleration override ramp.
01068             prep.current_speed -= speed_var;
01069           }
01070           break;
01071         case RAMP_ACCEL:
01072           // NOTE: Acceleration ramp only computes during first do-while loop.
01073           speed_var = pl_block->acceleration*time_var;
01074           mm_remaining -= time_var*(prep.current_speed + 0.5f*speed_var);
01075           if (mm_remaining < prep.accelerate_until) { // End of acceleration ramp.
01076             // Acceleration-cruise, acceleration-deceleration ramp junction, or end of block.
01077             mm_remaining = prep.accelerate_until; // NOTE: 0.0 at EOB
01078             time_var = 2.0f*(pl_block->millimeters-mm_remaining)/(prep.current_speed+prep.maximum_speed);
01079             if (mm_remaining == prep.decelerate_after) { prep.ramp_type = RAMP_DECEL; }
01080             else { prep.ramp_type = RAMP_CRUISE; }
01081             prep.current_speed = prep.maximum_speed;
01082           } else { // Acceleration only.
01083             prep.current_speed += speed_var;
01084           }
01085           break;
01086         case RAMP_CRUISE:
01087           // NOTE: mm_var used to retain the last mm_remaining for incomplete segment time_var calculations.
01088           // NOTE: If maximum_speed*time_var value is too low, round-off can cause mm_var to not change. To
01089           //   prevent this, simply enforce a minimum speed threshold in the planner.
01090           mm_var = mm_remaining - prep.maximum_speed*time_var;
01091           if (mm_var < prep.decelerate_after) { // End of cruise.
01092             // Cruise-deceleration junction or end of block.
01093             time_var = (mm_remaining - prep.decelerate_after)/prep.maximum_speed;
01094             mm_remaining = prep.decelerate_after; // NOTE: 0.0 at EOB
01095             prep.ramp_type = RAMP_DECEL;
01096           } else { // Cruising only.
01097             mm_remaining = mm_var;
01098           }
01099           break;
01100         default: // case RAMP_DECEL:
01101           // NOTE: mm_var used as a misc worker variable to prevent errors when near zero speed.
01102           speed_var = pl_block->acceleration*time_var; // Used as delta speed (mm/min)
01103           if (prep.current_speed > speed_var) { // Check if at or below zero speed.
01104             // Compute distance from end of segment to end of block.
01105             mm_var = mm_remaining - time_var*(prep.current_speed - 0.5f*speed_var); // (mm)
01106             if (mm_var > prep.mm_complete) { // Typical case. In deceleration ramp.
01107               mm_remaining = mm_var;
01108               prep.current_speed -= speed_var;
01109               break; // Segment complete. Exit switch-case statement. Continue do-while loop.
01110             }
01111           }
01112           // Otherwise, at end of block or end of forced-deceleration.
01113           time_var = 2.0f*(mm_remaining-prep.mm_complete)/(prep.current_speed+prep.exit_speed);
01114           mm_remaining = prep.mm_complete;
01115           prep.current_speed = prep.exit_speed;
01116       }
01117       dt += time_var; // Add computed ramp time to total segment time.
01118       if (dt < dt_max) { time_var = dt_max - dt; } // **Incomplete** At ramp junction.
01119       else {
01120         if (mm_remaining > minimum_mm) { // Check for very slow segments with zero steps.
01121           // Increase segment time to ensure at least one step in segment. Override and loop
01122           // through distance calculations until minimum_mm or mm_complete.
01123           dt_max += DT_SEGMENT;
01124           time_var = dt_max - dt;
01125         } else {
01126           break; // **Complete** Exit loop. Segment execution time maxed.
01127         }
01128       }
01129     } while (mm_remaining > prep.mm_complete); // **Complete** Exit loop. Profile complete.
01130 
01131     #ifdef VARIABLE_SPINDLE
01132       /* -----------------------------------------------------------------------------------
01133         Compute spindle speed PWM output for step segment
01134       */
01135 
01136       if (st_prep_block->is_pwm_rate_adjusted || (sys.step_control & STEP_CONTROL_UPDATE_SPINDLE_PWM)) {
01137         if (pl_block->condition & (PL_COND_FLAG_SPINDLE_CW | PL_COND_FLAG_SPINDLE_CCW)) {
01138           float rpm = pl_block->spindle_speed;
01139           // NOTE: Feed and rapid overrides are independent of PWM value and do not alter laser power/rate.        
01140           if (st_prep_block->is_pwm_rate_adjusted) { rpm *= (prep.current_speed * prep.inv_rate); }
01141           // If current_speed is zero, then may need to be rpm_min*(100/MAX_SPINDLE_SPEED_OVERRIDE)
01142           // but this would be instantaneous only and during a motion. May not matter at all.
01143           prep.current_spindle_pwm = spindle_compute_pwm_value(rpm);
01144         }
01145         else {
01146           sys.spindle_speed = 0.0;
01147           prep.current_spindle_pwm = SPINDLE_PWM_OFF_VALUE;
01148         }
01149         bit_false(sys.step_control, STEP_CONTROL_UPDATE_SPINDLE_PWM);
01150       }
01151       prep_segment->spindle_pwm = prep.current_spindle_pwm; // Reload segment PWM value
01152 
01153     #endif
01154     
01155     /* -----------------------------------------------------------------------------------
01156        Compute segment step rate, steps to execute, and apply necessary rate corrections.
01157        NOTE: Steps are computed by direct scalar conversion of the millimeter distance
01158        remaining in the block, rather than incrementally tallying the steps executed per
01159        segment. This helps in removing floating point round-off issues of several additions.
01160        However, since floats have only 7.2 significant digits, long moves with extremely
01161        high step counts can exceed the precision of floats, which can lead to lost steps.
01162        Fortunately, this scenario is highly unlikely and unrealistic in CNC machines
01163        supported by Grbl (i.e. exceeding 10 meters axis travel at 200 step/mm).
01164     */
01165     float step_dist_remaining = prep.step_per_mm*mm_remaining; // Convert mm_remaining to steps
01166     float n_steps_remaining = ceilf(step_dist_remaining); // Round-up current steps remaining
01167     float last_n_steps_remaining = ceilf(prep.steps_remaining); // Round-up last steps remaining
01168     prep_segment->n_step = (uint16_t)(last_n_steps_remaining - n_steps_remaining); // Compute number of steps to execute.
01169 
01170     // Bail if we are at the end of a feed hold and don't have a step to execute.
01171     if (prep_segment->n_step == 0) {
01172       if (sys.step_control & STEP_CONTROL_EXECUTE_HOLD) {
01173         // Less than one step to decelerate to zero speed, but already very close. AMASS
01174         // requires full steps to execute. So, just bail.
01175         bit_true(sys.step_control,STEP_CONTROL_END_MOTION);
01176         #ifdef PARKING_ENABLE
01177           if (!(prep.recalculate_flag & PREP_FLAG_PARKING)) { prep.recalculate_flag |= PREP_FLAG_HOLD_PARTIAL_BLOCK; }
01178         #endif
01179         return; // Segment not generated, but current step data still retained.
01180       }
01181     }
01182 
01183     // Compute segment step rate. Since steps are integers and mm distances traveled are not,
01184     // the end of every segment can have a partial step of varying magnitudes that are not
01185     // executed, because the stepper ISR requires whole steps due to the AMASS algorithm. To
01186     // compensate, we track the time to execute the previous segment's partial step and simply
01187     // apply it with the partial step distance to the current segment, so that it minutely
01188     // adjusts the whole segment rate to keep step output exact. These rate adjustments are
01189     // typically very small and do not adversely effect performance, but ensures that Grbl
01190     // outputs the exact acceleration and velocity profiles as computed by the planner.
01191     dt += prep.dt_remainder; // Apply previous segment partial step execute time
01192     float inv_rate = dt/(last_n_steps_remaining - step_dist_remaining); // Compute adjusted step rate inverse
01193 
01194     // Compute CPU cycles per step for the prepped segment.
01195     uint32_t cycles = (uint32_t)ceilf((TICKS_PER_MICROSECOND * 1000000) *inv_rate * 60); // (cycles/step)
01196 
01197     #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
01198       // Compute step timing and multi-axis smoothing level.
01199       // NOTE: AMASS overdrives the timer with each level, so only one prescalar is required.
01200       if (cycles < AMASS_LEVEL1) { prep_segment->amass_level = 0; }
01201       else {
01202         if (cycles < AMASS_LEVEL2) { prep_segment->amass_level = 1; }
01203         else if (cycles < AMASS_LEVEL3) { prep_segment->amass_level = 2; }
01204         else { prep_segment->amass_level = 3; }
01205         cycles >>= prep_segment->amass_level;
01206         prep_segment->n_step <<= prep_segment->amass_level;
01207       }
01208       if (cycles < (1UL << 16)) { prep_segment->cycles_per_tick = cycles; } // < 65536 (4.1ms @ 16MHz)
01209       else { prep_segment->cycles_per_tick = 0xffff; } // Just set the slowest speed possible.
01210     #else
01211       // Compute step timing and timer prescalar for normal step generation.
01212       if (cycles < (1UL << 16)) { // < 65536  (4.1ms @ 16MHz)
01213         prep_segment->prescaler = 1; // prescaler: 0
01214         prep_segment->cycles_per_tick = cycles;
01215       } else if (cycles < (1UL << 19)) { // < 524288 (32.8ms@16MHz)
01216         prep_segment->prescaler = 2; // prescaler: 8
01217         prep_segment->cycles_per_tick = cycles >> 3;
01218       } else {
01219         prep_segment->prescaler = 3; // prescaler: 64
01220         if (cycles < (1UL << 22)) { // < 4194304 (262ms@16MHz)
01221           prep_segment->cycles_per_tick =  cycles >> 6;
01222         } else { // Just set the slowest speed possible. (Around 4 step/sec.)
01223           prep_segment->cycles_per_tick = 0xffff;
01224         }
01225       }
01226     #endif
01227 
01228     // Segment complete! Increment segment buffer indices, so stepper ISR can immediately execute it.
01229     segment_buffer_head = segment_next_head;
01230     if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; }
01231 
01232     // Update the appropriate planner and segment data.
01233     pl_block->millimeters = mm_remaining;
01234     prep.steps_remaining = n_steps_remaining;
01235     prep.dt_remainder = (n_steps_remaining - step_dist_remaining)*inv_rate;
01236 
01237     // Check for exit conditions and flag to load next planner block.
01238     if (mm_remaining == prep.mm_complete) {
01239       // End of planner block or forced-termination. No more distance to be executed.
01240       if (mm_remaining > 0.0f) { // At end of forced-termination.
01241         // Reset prep parameters for resuming and then bail. Allow the stepper ISR to complete
01242         // the segment queue, where realtime protocol will set new state upon receiving the
01243         // cycle stop flag from the ISR. Prep_segment is blocked until then.
01244         bit_true(sys.step_control,STEP_CONTROL_END_MOTION);
01245         #ifdef PARKING_ENABLE
01246           if (!(prep.recalculate_flag & PREP_FLAG_PARKING)) { prep.recalculate_flag |= PREP_FLAG_HOLD_PARTIAL_BLOCK; }
01247         #endif
01248         return; // Bail!
01249       } else { // End of planner block
01250         // The planner block is complete. All steps are set to be executed in the segment buffer.
01251         if (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION) {
01252           bit_true(sys.step_control,STEP_CONTROL_END_MOTION);
01253           return;
01254         }
01255         pl_block = NULL; // Set pointer to indicate check and load next planner block.
01256         plan_discard_current_block();
01257       }
01258     }
01259 
01260   }
01261 }
01262 
01263 
01264 // Called by realtime status reporting to fetch the current speed being executed. This value
01265 // however is not exactly the current speed, but the speed computed in the last step segment
01266 // in the segment buffer. It will always be behind by up to the number of segment blocks (-1)
01267 // divided by the ACCELERATION TICKS PER SECOND in seconds.
01268 float st_get_realtime_rate()
01269 {
01270   if (sys.state & (STATE_CYCLE | STATE_HOMING | STATE_HOLD | STATE_JOG | STATE_SAFETY_DOOR)){
01271     return prep.current_speed;
01272   }
01273   return 0.0f;
01274 }
01275 #ifdef STM32F103C8
01276 void TIM_Configuration(TIM_TypeDef* TIMER, u16 Period, u16 Prescaler, u8 PP)
01277 {
01278     TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
01279     NVIC_InitTypeDef NVIC_InitStructure;
01280 
01281     TIM_TimeBaseStructure.TIM_Period = Period - 1;
01282     TIM_TimeBaseStructure.TIM_Prescaler = Prescaler - 1;
01283     TIM_TimeBaseStructure.TIM_ClockDivision = 0;
01284     TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
01285     TIM_TimeBaseInit(TIMER, &TIM_TimeBaseStructure);
01286 
01287     TIM_ClearITPendingBit(TIMER, TIM_IT_Update);
01288     TIM_ITConfig(TIMER, TIM_IT_Update, ENABLE);
01289     TIM_Cmd(TIMER, ENABLE);
01290 
01291     NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4);
01292     if (TIMER == TIM2) { NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn; }
01293     else if (TIMER == TIM3) { NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn; }
01294     else if (TIMER == TIM4) { NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn; }
01295 
01296     NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = PP;
01297     NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
01298     NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
01299     NVIC_Init(&NVIC_InitStructure);
01300 }
01301 #endif