diff --git a/print.c b/print.c
index 05524a3..f8e7a0c 100644
--- a/print.c
+++ b/print.c
@@ -3,6 +3,7 @@
   Part of Grbl
 
   Copyright (c) 2009-2011 Simen Svale Skogsrud
+  Copyright (c) 2011 Sungeun K. Jeon
 
   Grbl is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
@@ -28,20 +29,21 @@
 
 #ifndef DECIMAL_PLACES
 #define DECIMAL_PLACES 3
+#define DECIMAL_MULTIPLIER 10*10*10
 #endif
 
 void printString(const char *s)
 {
-	while (*s)
-		serial_write(*s++);
+  while (*s)
+    serial_write(*s++);
 }
 
 // Print a string stored in PGM-memory
 void printPgmString(const char *s)
 {
   char c;
-	while ((c = pgm_read_byte_near(s++)))
-		serial_write(c);
+  while ((c = pgm_read_byte_near(s++)))
+    serial_write(c);
 }
 
 void printIntegerInBase(unsigned long n, unsigned long base)
@@ -78,18 +80,25 @@ void printInteger(long n)
 // A very simple 
 void printFloat(double n)
 {
-  double integer_part, fractional_part;
-  uint8_t decimal_part;
-  fractional_part = modf(n, &integer_part);
-  printInteger(integer_part);
+  if (n < 0) {
+    serial_write('-');
+    n = -n;
+  }
+  n += 0.5/DECIMAL_MULTIPLIER; // Add rounding factor
+ 
+  long integer_part;
+  integer_part = (int)n;
+  printIntegerInBase(integer_part,10);
+  
   serial_write('.');
-  fractional_part *= 10;
+  
+  n -= integer_part;
   int decimals = DECIMAL_PLACES;  
+  uint8_t decimal_part;  
   while(decimals-- > 0) {
-    decimal_part = floor(fractional_part);
+    n *= 10;
+    decimal_part = (int) n;
     serial_write('0'+decimal_part);
-    fractional_part -= decimal_part;
-    fractional_part *= 10;
+    n -= decimal_part;
   }
 }
-
diff --git a/stepper.c b/stepper.c
index 3f2899c..418355a 100644
--- a/stepper.c
+++ b/stepper.c
@@ -133,21 +133,25 @@ static uint8_t iterate_trapezoid_cycle_counter()
 // config_step_timer. It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately. 
 // It is supported by The Stepper Port Reset Interrupt which it uses to reset the stepper port after each pulse. 
 // The bresenham line tracer algorithm controls all three stepper outputs simultaneously with these two interrupts.
-// NOTE: ISR_NOBLOCK allows SIG_OVERFLOW2 to trigger on-time regardless of time in this handler. This is
-// the compiler optimizable equivalent of the old SIGNAL() and sei() method.
-ISR(TIMER1_COMPA_vect,ISR_NOBLOCK)
+ISR(TIMER1_COMPA_vect)
 {        
   if (busy) { return; } // The busy-flag is used to avoid reentering this interrupt
-  busy = true;
   
   // Set the direction pins a couple of nanoseconds before we step the steppers
   STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (out_bits & DIRECTION_MASK);
   // 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.
-  TCNT2 = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3);
-    
+  // Enable step pulse reset timer so that The Stepper Port Reset Interrupt can reset the signal after
+  // exactly settings.pulse_microseconds microseconds, independent of the main Timer1 prescaler.
+  TCNT2 = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3); // Reload timer counter
+  TCCR2B = (1<<CS21); // Begin timer2. Full speed, 1/8 prescaler
+
+  busy = true;
+  // Re-enable interrupts to allow ISR_TIMER2_OVERFLOW to trigger on-time and allow serial communications
+  // regardless of time in this handler. The following code prepares the stepper driver for the next
+  // step interrupt compare and will always finish before returning to the main program.
+  sei();
+  
   // If there is no current block, attempt to pop one from the buffer
   if (current_block == NULL) {
     // Anything in the buffer? If so, initialize next motion.
@@ -250,12 +254,15 @@ ISR(TIMER1_COMPA_vect,ISR_NOBLOCK)
   busy=false;
 }
 
-// This interrupt is set up by SIG_OUTPUT_COMPARE1A when it sets the motor port bits. It resets
+// This interrupt is set up by ISR_TIMER1_COMPAREA when it sets the motor port bits. It resets
 // the motor port after a short period (settings.pulse_microseconds) completing one step cycle.
+// TODO: It is possible for the serial interrupts to delay this interrupt by a few microseconds, if
+// they execute right before this interrupt. Not a big deal, but could use some TLC at some point.
 ISR(TIMER2_OVF_vect)
 {
   // Reset stepping pins (leave the direction pins)
   STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | (settings.invert_mask & STEP_MASK); 
+  TCCR2B = 0; // Disable Timer2 to prevent re-entering this interrupt when it's not needed. 
 }
 
 // Initialize and start the stepper motor subsystem
@@ -277,9 +284,9 @@ void st_init()
   TCCR1A &= ~(3<<COM1B0); 
 	
   // Configure Timer 2
-  TCCR2A = 0;         // Normal operation
-  TCCR2B = (1<<CS21); // Full speed, 1/8 prescaler
-  TIMSK2 |= (1<<TOIE2);      
+  TCCR2A = 0; // Normal operation
+  TCCR2B = 0; // Disable timer until needed.
+  TIMSK2 |= (1<<TOIE2); // Enable Timer2 interrupt flag
   
   set_step_events_per_minute(MINIMUM_STEPS_PER_MINUTE);
   trapezoid_tick_cycle_counter = 0;