diff --git a/planner.c b/planner.c
index d3de443..7684632 100644
--- a/planner.c
+++ b/planner.c
@@ -123,12 +123,12 @@ inline double intersection_distance(double initial_rate, double final_rate, doub
 
 void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor) {
   block->initial_rate = ceil(block->nominal_rate*entry_factor);
-  int32_t final_rate = ceil(block->nominal_rate*exit_factor);
+  block->final_rate = ceil(block->nominal_rate*exit_factor);
   int32_t acceleration_per_minute = block->rate_delta*ACCELERATION_TICKS_PER_SECOND*60.0;
   int32_t accelerate_steps = 
     ceil(estimate_acceleration_distance(block->initial_rate, block->nominal_rate, acceleration_per_minute));
   int32_t decelerate_steps = 
-    floor(estimate_acceleration_distance(block->nominal_rate, final_rate, -acceleration_per_minute));
+    floor(estimate_acceleration_distance(block->nominal_rate, block->final_rate, -acceleration_per_minute));
 
   // Calculate the size of Plateau of Nominal Rate. 
   int32_t plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps;
@@ -138,7 +138,7 @@ void calculate_trapezoid_for_block(block_t *block, double entry_factor, double e
   // in order to reach the final_rate exactly at the end of this block.
   if (plateau_steps < 0) {  
     accelerate_steps = ceil(
-      intersection_distance(block->initial_rate, final_rate, acceleration_per_minute, block->step_event_count));
+      intersection_distance(block->initial_rate, block->final_rate, acceleration_per_minute, block->step_event_count));
     plateau_steps = 0;
   }  
   
@@ -394,6 +394,7 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert
     calculate_trapezoid_for_block(block, safe_speed_factor, safe_speed_factor); 
   } else {
     block->initial_rate = block->nominal_rate;
+    block->final_rate = block->nominal_rate;
     block->accelerate_until = 0;
     block->decelerate_after = block->step_event_count;
     block->rate_delta = 0;
diff --git a/planner.h b/planner.h
index 71524b9..c07242a 100644
--- a/planner.h
+++ b/planner.h
@@ -45,6 +45,7 @@ typedef struct {
   
   // Settings for the trapezoid generator
   uint32_t initial_rate;              // The jerk-adjusted step rate at start of block  
+  uint32_t final_rate;                // The minimal rate at exit
   int32_t rate_delta;                 // The steps/minute to add or subtract when changing speed (must be positive)
   uint32_t accelerate_until;          // The index of the step event on which to stop acceleration
   uint32_t decelerate_after;          // The index of the step event on which to start decelerating
diff --git a/script/trapezoid_simulator.rb b/script/trapezoid_simulator.rb
new file mode 100644
index 0000000..795624f
--- /dev/null
+++ b/script/trapezoid_simulator.rb
@@ -0,0 +1,78 @@
+require 'pp'
+
+def estimate_acceleration_distance(initial_rate, target_rate, acceleration) 
+    (target_rate*target_rate-initial_rate*initial_rate)/(2*acceleration)
+end
+
+def intersection_distance(initial_rate, final_rate, acceleration, distance) 
+    (2*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/(4*acceleration)
+end
+
+ACCELERATION_TICKS_PER_SECOND = 20
+
+def trapezoid_params(step_event_count, nominal_rate, rate_delta, entry_factor, exit_factor)
+  initial_rate = (nominal_rate * entry_factor).round
+  final_rate = (nominal_rate * exit_factor).round
+  acceleration_per_minute = rate_delta*ACCELERATION_TICKS_PER_SECOND*60
+
+  accelerate_steps = 
+    estimate_acceleration_distance(initial_rate, nominal_rate, acceleration_per_minute).round;
+  decelerate_steps = 
+    estimate_acceleration_distance(nominal_rate, final_rate, -acceleration_per_minute).round;
+  
+  # Calculate the size of Plateau of Nominal Rate. 
+  plateau_steps = step_event_count-accelerate_steps-decelerate_steps;
+  
+  # Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
+  # have to use intersection_distance() to calculate when to abort acceleration and start braking 
+  # in order to reach the final_rate exactly at the end of this block.
+  if (plateau_steps < 0)
+    accelerate_steps = 
+      intersection_distance(initial_rate, final_rate, acceleration_per_minute, step_event_count).round
+    plateau_steps = 0;
+  end
+  
+  accelerate_until = accelerate_steps;
+  decelerate_after = accelerate_steps+plateau_steps;
+  {:step_event_count => step_event_count,
+   :initial_rate => initial_rate,
+   :final_rate => final_rate,
+   :nominal_rate => nominal_rate,
+   :rate_delta => rate_delta,
+   :accelerate_until => accelerate_until,
+   :decelerate_after => decelerate_after}
+end
+
+def simulate_trapezoid(params)
+  result = {}
+  rate = params[:initial_rate]
+  step_event = 0.0
+  max_rate = 0
+  while(step_event < params[:step_event_count]) do
+    step_events_in_frame = rate/60.0/ACCELERATION_TICKS_PER_SECOND
+    step_event += step_events_in_frame
+    max_rate = rate if rate > max_rate
+    if (step_event < params[:accelerate_until]) 
+      rate += params[:rate_delta]
+    elsif (step_event > params[:decelerate_after])
+      if rate > params[:final_rate]
+        rate -= params[:rate_delta]
+      else
+        return :underflow_at => step_event, :final_rate => rate, :max_rate => max_rate
+      end
+    end
+#    puts "#{step_event} #{rate}"
+  end 
+  {:final_rate => rate, :max_rate => max_rate}
+end
+
+(10..100).each do |rate|
+  (1..5).each do |steps|
+    params = trapezoid_params(steps*1000, rate*100, 10, 0.1, 0.1)
+    result = simulate_trapezoid(params)
+  #  puts params.inspect
+    line = "#{steps*10} final: #{result[:final_rate]} == #{params[:final_rate]} peak: #{result[:max_rate]} == #{params[:nominal_rate]} d#{params[:nominal_rate]-result[:max_rate]} "
+    line << " (underflow at #{result[:underflow_at]})" if result[:underflow_at]
+    puts line
+  end
+end
\ No newline at end of file
diff --git a/stepper.c b/stepper.c
index 933b847..5b4efcc 100644
--- a/stepper.c
+++ b/stepper.c
@@ -90,6 +90,7 @@ void st_wake_up() {
 // block begins.
 inline void trapezoid_generator_reset() {
   trapezoid_adjusted_rate = current_block->initial_rate;  
+  trapezoid_tick_cycle_counter = 0; // Always start a new trapezoid with a full acceleration tick
   set_step_events_per_minute(trapezoid_adjusted_rate);
 }
 
@@ -104,9 +105,12 @@ inline void trapezoid_generator_tick() {
     } else if (step_events_completed > current_block->decelerate_after) {
       // NOTE: We will only reduce speed if the result will be > 0. This catches small
       // rounding errors that might leave steps hanging after the last trapezoid tick.
-      if(current_block->rate_delta < trapezoid_adjusted_rate) {
+      if (trapezoid_adjusted_rate > current_block->rate_delta) {
         trapezoid_adjusted_rate -= current_block->rate_delta;
-      }          
+      }
+      if (trapezoid_adjusted_rate < current_block->final_rate) {
+        trapezoid_adjusted_rate = current_block->final_rate;
+      }        
       set_step_events_per_minute(trapezoid_adjusted_rate);
     } else {
       // Make sure we cruise at exactly nominal rate