From f1e5ff35ecf6dfd11c431e4e714c7bc6077ca52a Mon Sep 17 00:00:00 2001
From: Sonny J <chamnit@gmail.com>
Date: Sun, 4 Sep 2011 11:19:08 -0600
Subject: [PATCH] More minor bug fixes in planner.

Reverse planner was over-writing the initial/buffer tail entry speed,
which reset the forward planner and caused it to lose track of its
speed. Should now accelerate into short linear segments much nicer now.
---
 planner.c | 38 +++++++++++++++++++-------------------
 1 file changed, 19 insertions(+), 19 deletions(-)

diff --git a/planner.c b/planner.c
index 8dbde44..7eafc31 100644
--- a/planner.c
+++ b/planner.c
@@ -100,19 +100,21 @@ static double max_allowable_speed(double acceleration, double target_velocity, d
 static void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) {
   if (!current) { return; }
   
-  double entry_speed = current->max_entry_speed;
-  double exit_speed;
-  if (next) {
-    exit_speed = next->entry_speed;
-  } else {
-    exit_speed = 0.0;
+  if (previous) { // Prevent reverse planner from over-writing buffer_tail entry speed.
+    double entry_speed = current->max_entry_speed; // Re-write to ensure at max possible speed
+    double exit_speed;
+    if (next) {
+      exit_speed = next->entry_speed;
+    } else {
+      exit_speed = 0.0; // Assume last block has zero exit velocity
+    }
+    // If the required deceleration across the block is too rapid, reduce the entry_factor accordingly.
+    if (entry_speed > exit_speed) {
+      entry_speed = 
+        min(max_allowable_speed(-settings.acceleration,exit_speed,current->millimeters),entry_speed);
+    }    
+    current->entry_speed = entry_speed;
   }
-  // If the required deceleration across the block is too rapid, reduce the entry_factor accordingly.
-  if (entry_speed > exit_speed) {
-    entry_speed = 
-      min(max_allowable_speed(-settings.acceleration,exit_speed,current->millimeters),entry_speed);
-  }    
-  current->entry_speed = entry_speed;
 }
 
 
@@ -354,19 +356,17 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert
                            -previous->delta_mm[Z_AXIS] * block->delta_mm[Z_AXIS] )/
                           ( previous->millimeters * block->millimeters );
       
-      // Avoid divide by zero for straight junctions around 180 degress
+      // Avoid divide by zero for straight junctions near 180 degrees. Limit to min nominal speeds.
+      vmax_junction = min(previous->nominal_speed,block->nominal_speed);
       if (cos_theta > -0.95) {
         // Compute maximum junction velocity based on maximum acceleration and junction deviation
         double sin_theta_d2 = sqrt((1-cos_theta)/2); // Trig half angle identity
-        vmax_junction = 
-          sqrt(settings.acceleration*60*60 * settings.junction_deviation * sin_theta_d2/(1-sin_theta_d2));
-        vmax_junction = max(0.0,min(vmax_junction, min(previous->nominal_speed,block->nominal_speed)));
-      } else {
-        vmax_junction = min(previous->nominal_speed,block->nominal_speed);
+        vmax_junction = max(0.0, min(vmax_junction,
+          sqrt(settings.acceleration*60*60 * settings.junction_deviation * sin_theta_d2/(1-sin_theta_d2)) ) );
       }
     }
     block->max_entry_speed = vmax_junction;
-    block->entry_speed = 0.0;
+    block->entry_speed = vmax_junction;
   } else {
     block->initial_rate = block->nominal_rate;
     block->final_rate = block->nominal_rate;