diff --git a/planner.c b/planner.c
index 9dbe363..f21ced6 100644
--- a/planner.c
+++ b/planner.c
@@ -232,16 +232,18 @@ void planner_reverse_pass() {
 // The kernel called by planner_recalculate() when scanning the plan from first to last entry.
 void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) {
   if(!current) { return; }
-  // If the previous block is an acceleration block, but it is not long enough to 
-  // complete the full speed change within the block, we need to adjust out entry
-  // speed accordingly. Remember current->entry_factor equals the exit factor of 
-  // the previous block.
-  if(previous->entry_factor < current->entry_factor) {
-    double max_entry_speed = max_allowable_speed(-settings.acceleration,
-      current->nominal_speed*previous->entry_factor, previous->millimeters);
-    double max_entry_factor = max_entry_speed/current->nominal_speed;
-    if (max_entry_factor < current->entry_factor) {
-      current->entry_factor = max_entry_factor;
+  if(previous) {
+    // If the previous block is an acceleration block, but it is not long enough to 
+    // complete the full speed change within the block, we need to adjust out entry
+    // speed accordingly. Remember current->entry_factor equals the exit factor of 
+    // the previous block.
+    if(previous->entry_factor < current->entry_factor) {
+      double max_entry_speed = max_allowable_speed(-settings.acceleration,
+        current->nominal_speed*previous->entry_factor, previous->millimeters);
+      double max_entry_factor = max_entry_speed/current->nominal_speed;
+      if (max_entry_factor < current->entry_factor) {
+        current->entry_factor = max_entry_factor;
+      }
     }
   }
 }
diff --git a/stepper.c b/stepper.c
index 5b4efcc..a017852 100644
--- a/stepper.c
+++ b/stepper.c
@@ -101,6 +101,9 @@ inline void trapezoid_generator_tick() {
   if (current_block) {
     if (step_events_completed < current_block->accelerate_until) {
       trapezoid_adjusted_rate += current_block->rate_delta;
+      if (trapezoid_adjusted_rate > current_block->nominal_rate ) {
+        trapezoid_adjusted_rate = current_block->nominal_rate;
+      }
       set_step_events_per_minute(trapezoid_adjusted_rate);
     } else if (step_events_completed > current_block->decelerate_after) {
       // NOTE: We will only reduce speed if the result will be > 0. This catches small
@@ -135,8 +138,10 @@ SIGNAL(TIMER1_COMPA_vect)
   // Then pulse the stepping pins
   STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | out_bits;
   // Reset step pulse reset timer so that The Stepper Port Reset Interrupt can reset the signal after
-  // exactly settings.pulse_microseconds microseconds.
+  // exactly settings.pulse_microseconds microseconds.  Clear the overflow flag to stop a queued
+  // interrupt from resetting the step pulse too soon.
   TCNT2 = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND)/8);
+  TIFR2 |= (1<<TOV2);
 
   busy = TRUE;
   sei(); // Re enable interrupts (normally disabled while inside an interrupt handler)
diff --git a/wiring_serial.c b/wiring_serial.c
index 62c4be4..92da993 100644
--- a/wiring_serial.c
+++ b/wiring_serial.c
@@ -24,6 +24,7 @@
 
 //#include "wiring_private.h"
 #include <math.h>
+#include <stdlib.h>
 #include <avr/pgmspace.h>
 #include <avr/interrupt.h>
 
@@ -176,7 +177,7 @@ void printFloat(double n)
   fractional_part = modf(n, &integer_part);
   printInteger(integer_part);
   printByte('.');
-  printInteger(round(fractional_part*1000));
+  printInteger(labs(round(fractional_part*1000)));
 }
 
 // void printHex(unsigned long n)