Planner function call fix. More clean up.

planner.c

@@ -148,34 +148,18 @@ void plan_update_partial_block(uint8_t block_index, float exit_speed_sqr)
   recomputed as stated in the general guidelines.
   
   Planner buffer index mapping:
-  - block_buffer_head: Points to the newest incoming buffer block just added by plan_buffer_line(). The 
-      planner never touches the exit speed of this block, which always defaults to 0.
   - block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed. 
-      Can dynamically change with the old stepper algorithm, but with the new algorithm, this should be impossible
-      as long as the segment buffer is not empty.
-  - next_buffer_head: Points to next planner buffer block after the last block. Should always be empty.
-  - block_buffer_safe: Points to the first planner block in the buffer for which it is safe to change. Since
-      the stepper can be executing the first block and if the planner changes its conditions, this will cause
-      a discontinuity and error in the stepper profile with lost steps likely. With the new stepper algorithm,
-      the block_buffer_safe is always where the stepper segment buffer ends and can never be overwritten, but
-      this can change the state of the block profile from a pure trapezoid assumption. Meaning, if that block
-      is decelerating, the planner conditions can change such that the block can new accelerate mid-block.
-      
-      !!! I need to make sure that the stepper algorithm can modify the acceleration mid-block. Needed for feedrate overrides too.
-      
-      !!! planner_recalculate() may not work correctly with re-planning.... may need to artificially set both the
-          block_buffer_head and next_buffer_head back one index so that this works correctly, or allow the operation
-          of this function to accept two different conditions to operate on.
-          
-  - block_buffer_planned: Points to the first buffer block after the last optimally fixed block, which can no longer be 
-      improved. This block and the trailing buffer blocks that can still be altered when new blocks are added. This planned
-      block points to the transition point between the fixed and non-fixed states and is handled slightly different. The entry
-      speed is fixed, indicating the reverse pass cannot maximize the speed further, but the velocity profile within it
-      can still be changed, meaning the forward pass calculations must start from here and influence the following block
-      entry speed.
-
-      !!! Need to check if this is the start of the non-optimal or the end of the optimal block.
-
+  - block_buffer_head: Points to the buffer block after the last block in the buffer. Used to indicate whether
+      the buffer is full or empty. As described for standard ring buffers, this block is always empty.
+  - next_buffer_head: Points to next planner buffer block after the buffer head block. When equal to the 
+      buffer tail, this indicates the buffer is full.
+  - block_buffer_safe: Points to the first sequential planner block for which it is safe to recompute, which
+      is defined to be where the stepper's step segment buffer ends. This may or may not be the buffer tail, 
+      since the step segment buffer queues steps which may have not finished executing and could span a few
+      blocks, if the block moves are very short.   
+  - block_buffer_planned: Points to the first buffer block after the last optimally planned block for normal
+      streaming operating conditions. Use for planning optimizations by avoiding recomputing parts of the 
+      planner buffer that don't change with the addition of a new block, as describe above.
   
   NOTE: All planner computations are performed in floating point to minimize numerical round-off errors.
   When a planner block is executed, the floating point values are converted to fast integers by the stepper
@@ -198,7 +182,7 @@ static void planner_recalculate()
 {   
 
   // Initialize block index to the last block in the planner buffer.
-  uint8_t block_index = prev_block_index(block_buffer_head);
+  uint8_t block_index = plan_prev_block_index(block_buffer_head);
 
   // Query stepper module for safe planner block index to recalculate to, which corresponds to the end
   // of the step segment buffer.
@@ -324,58 +308,6 @@ static void planner_recalculate()
     
   }  
   
-/* 
-   uint8_t block_buffer_safe = st_get_prep_block_index();
-   if (block_buffer_head == block_buffer_safe) { // Also catches head = tail
-     block_buffer_planned = block_buffer_head;
-   } else {
-     uint8_t block_index = block_buffer_head;
-     plan_block_t *current = &block_buffer[block_index];
-     current->entry_speed_sqr = min( current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters);
-     float entry_speed_sqr;
-     plan_block_t *next;
-     block_index = plan_prev_block_index(block_index);
-     if (block_index == block_buffer_safe) { // !! OR plan pointer? Yes I think so.
-       plan_update_partial_block(block_index, 0.0);
-       block_buffer_planned = block_index;
-     } else {
-       while (block_index != block_buffer_planned) { 
-         next = current;
-         current = &block_buffer[block_index];
-         block_index = plan_prev_block_index(block_index);
-         if (block_index == block_buffer_safe) {
-           plan_update_partial_block(block_index,current->entry_speed_sqr);
-           block_buffer_planned = block_index; 
-         } 
-         if (current->entry_speed_sqr != current->max_entry_speed_sqr) {
-           entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters;
-           if (entry_speed_sqr < current->max_entry_speed_sqr) {
-             current->entry_speed_sqr = entry_speed_sqr;
-           } else {
-             current->entry_speed_sqr = current->max_entry_speed_sqr;
-           }
-         }
-       }
-   
-     }    
-     next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer
-     block_index = plan_next_block_index(block_buffer_planned); 
-     while (block_index != next_buffer_head) {
-       current = next;
-       next = &block_buffer[block_index];
-       if (current->entry_speed_sqr < next->entry_speed_sqr) {
-         entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters;
-         if (entry_speed_sqr < next->entry_speed_sqr) {
-           next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this.
-           block_buffer_planned = block_index; // Set optimal plan pointer.
-         }
-       }
-       if (next->entry_speed_sqr == next->max_entry_speed_sqr) {
-         block_buffer_planned = block_index; // Set optimal plan pointer
-       } 
-       block_index = plan_next_block_index( block_index );
-     }
-   }  */
 }
 
 
@@ -439,16 +371,16 @@ void plan_synchronize()
 }
 
 
-// Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position
-// in millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed
-// rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes.
-// All position data passed to the planner must be in terms of machine position to keep the planner 
-// independent of any coordinate system changes and offsets, which are handled by the g-code parser.
-// NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control.
-// In other words, the buffer head is never equal to the buffer tail.  Also the feed rate input value
-// is used in three ways: as a normal feed rate if invert_feed_rate is false, as inverse time if
-// invert_feed_rate is true, or as seek/rapids rate if the feed_rate value is negative (and
-// invert_feed_rate always false).
+/* Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position
+   in millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed
+   rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes.
+   All position data passed to the planner must be in terms of machine position to keep the planner 
+   independent of any coordinate system changes and offsets, which are handled by the g-code parser.
+   NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control.
+   In other words, the buffer head is never equal to the buffer tail.  Also the feed rate input value
+   is used in three ways: as a normal feed rate if invert_feed_rate is false, as inverse time if
+   invert_feed_rate is true, or as seek/rapids rate if the feed_rate value is negative (and
+   invert_feed_rate always false). */
 void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate) 
 {
   // Prepare and initialize new block
@@ -675,3 +607,63 @@ void plan_cycle_reinitialize(int32_t step_events_remaining)
   block_buffer_planned = block_buffer_tail;
   planner_recalculate();  
 }
+
+
+/*
+TODO:
+  When a feed hold or feedrate override is reduced, the velocity profile must execute a 
+  deceleration over the existing plan. By logic, since the plan already decelerates to zero
+  at the end of the buffer, any replanned deceleration mid-way will never exceed this. It
+  will only asymptotically approach this in the worst case scenario.
+  
+  - For a feed hold, we simply need to plan and compute the stopping point within a block 
+    when velocity decelerates to zero. We then can recompute the plan with the already 
+    existing partial block planning code and set the system to a QUEUED state.
+    - When a feed hold is initiated, the main program should be able to continue doing what
+      it has been, i.e. arcs, parsing, but needs to be able to reinitialize the plan after
+      it has come to a stop.
+    
+  - For a feed rate override (reduce-only), we need to enforce a deceleration until we 
+    intersect the reduced nominal speed of a block after it's been planned with the new
+    overrides and the newly planned block is accelerating or cruising only. If the new plan
+    block is decelerating at the intersection point, we keep decelerating until we find a 
+    valid intersection point. Once we find this point, we can then resume onto the new plan,
+    but we may need to adjust the deceleration point in the intersection block since the 
+    feedrate override could have intersected at an acceleration ramp. This would change the
+    acceleration ramp to a cruising, so the deceleration point will have changed, but the 
+    plan will have not. It should still be valid for the rest of the buffer. Coding this
+    can get complicated, but it should be doable. One issue could be is in how to handle 
+    scenarios when a user issues several feedrate overrides and inundates this code. Does 
+    this method still work and is robust enough to compute all of this on the fly? This is
+    the critical question. However, we could block user input until the planner has time to 
+    catch to solve this as well. 
+  
+  - When the feed rate override increases, we don't have to do anything special. We just
+    replan the entire buffer with the new nominal speeds and adjust the maximum junction
+    speeds accordingly.
+
+void plan_compute_deceleration() {
+
+}
+
+
+void plan_recompute_max_junction_velocity() {
+  // Assumes the nominal_speed_sqr values have been updated. May need to just multiply 
+  // override values here.
+  // PROBLEM: Axes-limiting velocities get screwed up. May need to store an int8 value for the
+  // max override value possible for each block when the line is added. So the nominal_speed
+  // is computed with that ceiling, but still retained if the rates change again.
+  uint8_t block_index = block_buffer_tail;
+  plan_block_t *block = &block_buffer[block_index];
+  pl.previous_nominal_speed_sqr = block->nominal_speed_sqr;
+  block_index = plan_next_block_index(block_index);
+  while (block_index != block_buffer_head) {
+    block = &block_buffer[block_index];
+    block->max_entry_speed_sqr = min(block->max_junction_speed_sqr, 
+                                     min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr));    
+    pl.previous_nominal_speed_sqr = block->nominal_speed_sqr;
+    block_index = plan_next_block_index(block_index);
+  }
+}
+
+*/

protocol.c

@@ -153,6 +153,8 @@ void protocol_execute_runtime()
     
     // Initiate stepper feed hold
     if (rt_exec & EXEC_FEED_HOLD) {
+      // !!! During a cycle, the segment buffer has just been reloaded and full. So the math involved
+      // with the feed hold should be fine for most, if not all, operational scenarios.
       st_feed_hold(); // Initiate feed hold.
       bit_false(sys.execute,EXEC_FEED_HOLD);
     }

stepper.c

@@ -39,8 +39,8 @@
 
 #define SEGMENT_NOOP 0
 #define SEGMENT_END_OF_BLOCK bit(0)
-#define SEGMENT_ACCEL bit(1)
-#define SEGMENT_DECEL bit(2)
+#define RAMP_CHANGE_ACCEL bit(1)
+#define RAMP_CHANGE_DECEL bit(2)
 
 #define MINIMUM_STEPS_PER_SEGMENT 1 // Don't change
 
@@ -76,7 +76,7 @@ static stepper_t st;
 // the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1).
 typedef struct {  
   int32_t step_events_remaining; // Tracks step event count for the executing planner block
-  uint32_t dist_next_step;               // Scaled distance to next step
+  uint32_t dist_per_step;               // Scaled distance to next step
   uint32_t initial_rate;         // Initialized step rate at re/start of a planner block
   uint32_t nominal_rate;         // The nominal step rate for this block in step_events/minute
   uint32_t rate_delta;           // The steps/minute to add or subtract when changing speed (must be positive)
@@ -254,7 +254,7 @@ ISR(TIMER2_COMPA_vect)
         st.counter_z = st.counter_x;
         
         // Initialize inverse time, step rate data, and acceleration ramp counters
-        st.counter_dist = st_current_data->dist_next_step;  // dist_next_step always greater than ramp_rate.   
+        st.counter_dist = st_current_data->dist_per_step;  // dist_per_step always greater than ramp_rate.   
         st.ramp_rate = st_current_data->initial_rate; 
         st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2;  // Initialize ramp counter via midpoint rule
         st.ramp_type = RAMP_NOOP_CRUISE; // Initialize as no ramp operation. Corrected later if necessary. 
@@ -265,7 +265,7 @@ ISR(TIMER2_COMPA_vect)
       }
 
       // Check if ramp conditions have changed. If so, update ramp counters and control variables.
-      if ( st_current_segment->flag & (SEGMENT_DECEL | SEGMENT_ACCEL) ) {
+      if ( st_current_segment->flag & (RAMP_CHANGE_DECEL | RAMP_CHANGE_ACCEL) ) {
         /* Compute correct ramp count for a ramp change. Upon a switch from acceleration to deceleration,
            or vice-versa, the new ramp count must be set to trigger the next acceleration tick equal to
            the number of ramp ISR ticks counted since the last acceleration tick. This is ensures the 
@@ -274,7 +274,7 @@ ISR(TIMER2_COMPA_vect)
            as mandated by the mid-point rule. For the latter conditions, the ramp count have been
            initialized such that the following computation is still correct. */
         st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK-st.counter_ramp;
-        if ( st_current_segment->flag & SEGMENT_DECEL ) { st.ramp_type = RAMP_DECEL; }
+        if ( st_current_segment->flag & RAMP_CHANGE_DECEL ) { st.ramp_type = RAMP_DECEL; }
         else { st.ramp_type = RAMP_ACCEL; }
       }
       
@@ -319,7 +319,7 @@ ISR(TIMER2_COMPA_vect)
   
   // Execute Bresenham step event, when it's time to do so.
   if (st.counter_dist < 0) {  
-    st.counter_dist += st_current_data->dist_next_step; // Reload inverse time counter
+    st.counter_dist += st_current_data->dist_per_step; // Reload inverse time counter
 
     st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits
     st.execute_step = true;
@@ -493,21 +493,21 @@ void st_cycle_reinitialize()
    Currently, the segment buffer conservatively holds roughly up to 40-60 msec of steps. 
 
    NOTE: The segment buffer executes a set number of steps over an approximate time period.
-   If we try to execute over a set time period, it is difficult to guarantee or predict how
-   many steps will execute over it, especially when the step pulse phasing between the
-   neighboring segments are kept consistent. Meaning that, if the last segment step pulses
-   right before its end, the next segment must delay its first pulse so that the step pulses
-   are consistently spaced apart over time to keep the step pulse train nice and smooth. 
-   Keeping track of phasing and ensuring that the exact number of steps are executed as
-   defined by the planner block, the related computational overhead gets quickly and
+   If we try to execute over a fixed time period, it is difficult to guarantee or predict 
+   how many steps will execute over it, especially when the step pulse phasing between the
+   neighboring segments must also be kept consistent. Meaning that, if the last segment step 
+   pulses right before a segment end, the next segment must delay its first pulse so that the
+   step pulses are consistently spaced apart over time to keep the step pulse train nice and
+   smooth. Keeping track of phasing and ensuring that the exact number of steps are executed
+   as defined by the planner block, the related computational overhead can get quickly and
    prohibitively expensive, especially in real-time.
      Since the stepper algorithm automatically takes care of the step pulse phasing with
-   its ramp and inverse time counters, we don't have to explicitly and expensively track the
-   exact number of steps, time, or phasing of steps. All we need to do is approximate
-   the number of steps in each segment such that the segment buffer has enough execution time
-   for the main program to do what it needs to do and refill it when it has time. In other
-   words, we just need to compute a cheap approximation of the current velocity and the 
-   number of steps over it. 
+   its ramp and inverse time counters by retaining the count remainders, we don't have to
+   explicitly and expensively track and synchronize the exact number of steps, time, and
+   phasing of steps. All we need to do is approximate the number of steps in each segment 
+   such that the segment buffer has enough execution time for the main program to do what 
+   it needs to do and refill it when it comes back. In other words, we just need to compute
+   a cheap approximation of the current velocity and the number of steps over it. 
 */
 
 /* 
@@ -529,7 +529,7 @@ void st_cycle_reinitialize()
 */
 void st_prep_buffer()
 {
-  if (sys.state != STATE_QUEUED) { // Block until a motion state is issued
+  if (sys.state == STATE_QUEUED) { return; } // Block until a motion state is issued
   while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer.
     
     // Initialize new segment
@@ -555,7 +555,7 @@ void st_prep_buffer()
             
         st_prep_data->step_events_remaining = last_st_prep_data->step_events_remaining;
         st_prep_data->rate_delta = last_st_prep_data->rate_delta;
-        st_prep_data->dist_next_step = last_st_prep_data->dist_next_step;
+        st_prep_data->dist_per_step = last_st_prep_data->dist_per_step;
         st_prep_data->nominal_rate = last_st_prep_data->nominal_rate; // TODO: Feedrate overrides recomputes this.
        
         st_prep_data->mm_per_step = last_st_prep_data->mm_per_step;
@@ -575,7 +575,7 @@ void st_prep_buffer()
         st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
         st_prep_data->rate_delta = ceil(pl_prep_block->acceleration*
           ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic)
-        st_prep_data->dist_next_step = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step)
+        st_prep_data->dist_per_step = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step)
         
         // TODO: Check if we really need to store this.
         st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count;        
@@ -593,8 +593,8 @@ void st_prep_buffer()
       // Calculate the planner block velocity profile type, determine deceleration point, and initial ramp.
       float mm_decelerate_after = plan_calculate_velocity_profile(pl_prep_index);
       st_prep_data->decelerate_after = ceil( mm_decelerate_after/st_prep_data->mm_per_step );
-      if (st_prep_data->decelerate_after > 0) { // If 0, SEGMENT_DECEL flag is set later.
-        if (st_prep_data->initial_rate != st_prep_data->nominal_rate) { prep_segment->flag = SEGMENT_ACCEL; }
+      if (st_prep_data->decelerate_after > 0) { // If 0, RAMP_CHANGE_DECEL flag is set later.
+        if (st_prep_data->initial_rate != st_prep_data->nominal_rate) { prep_segment->flag = RAMP_CHANGE_ACCEL; }
       }
     }
 
@@ -604,9 +604,9 @@ void st_prep_buffer()
     // Approximate the velocity over the new segment using the already computed rate values.
     // NOTE: This assumes that each segment will have an execution time roughly equal to every ACCELERATION_TICK.
     // We do this to minimize memory and computational requirements. However, this could easily be replaced with
-    // a more exact approximation or have a unique time per segment, if CPU and memory overhead allows.
+    // a more exact approximation or have an user-defined time per segment, if CPU and memory overhead allows.
     if (st_prep_data->decelerate_after <= 0) {
-      if (st_prep_data->decelerate_after == 0) { prep_segment->flag = SEGMENT_DECEL; } // Set segment deceleration flag
+      if (st_prep_data->decelerate_after == 0) { prep_segment->flag = RAMP_CHANGE_DECEL; } // Set segment deceleration flag
       else { st_prep_data->current_approx_rate -= st_prep_data->rate_delta; }
       if (st_prep_data->current_approx_rate < st_prep_data->rate_delta) { st_prep_data->current_approx_rate >>= 1; }
     } else {    
@@ -618,10 +618,12 @@ void st_prep_buffer()
       }
     }
     
+    // TODO: Look into replacing the following dist_per_step divide with multiplying its inverse to save cycles.
+    
     // Compute the number of steps in the prepped segment based on the approximate current rate.
-    // NOTE: The dist_next_step divide cancels out the INV_TIME_MULTIPLIER and converts the rate value to steps.
+    // NOTE: The dist_per_step divide cancels out the INV_TIME_MULTIPLIER and converts the rate value to steps.
     prep_segment->n_step = ceil(max(MINIMUM_STEP_RATE,st_prep_data->current_approx_rate)*
-                                (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)/st_prep_data->dist_next_step);    
+                                (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)/st_prep_data->dist_per_step);    
     // NOTE: Ensures it moves for very slow motions, but MINIMUM_STEP_RATE should always set this too. Perhaps
     // a compile-time check to see if MINIMUM_STEP_RATE is set high enough is all that is needed.
     prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_SEGMENT);
@@ -668,7 +670,6 @@ void st_prep_buffer()
 // printString(" ");
 
   } 
-  }
 }      
 
 uint8_t st_get_prep_block_index()