diff --git a/config.h b/config.h
index 7ba20ec..17a566d 100644
--- a/config.h
+++ b/config.h
@@ -62,7 +62,8 @@
 // entering g-code into grbl, i.e. locating part zero or simple manual machining. If the axes drift,
 // grbl has no way to know this has happened, since stepper motors are open-loop control. Depending
 // on the machine, this parameter may need to be larger or smaller than the default time.
-#define STEPPER_IDLE_LOCK_TIME 25 // (milliseconds)
+// NOTE: If defined 0, the delay will not be compiled.
+#define STEPPER_IDLE_LOCK_TIME 25 // (milliseconds) - Integer >= 0
 
 // The temporal resolution of the acceleration management subsystem. Higher number give smoother
 // acceleration but may impact performance.
@@ -80,18 +81,18 @@
 // if unwanted behavior is observed on a user's machine when running at very slow speeds.
 #define MINIMUM_PLANNER_SPEED 0.0 // (mm/min)
 
-// Minimum stepper rate. Sets the absolute minimum stepper rate in the stepper program and never run
+// Minimum stepper rate. Sets the absolute minimum stepper rate in the stepper program and never runs
 // slower than this value, except when sleeping. This parameter overrides the minimum planner speed.
 // This is primarily used to guarantee that the end of a movement is always reached and not stop to
 // never reach its target. This parameter should always be greater than zero.
 #define MINIMUM_STEPS_PER_MINUTE 800 // (steps/min) - Integer value only
 
-// Number of arc generation iterations by small angle approximation before exact arc
-// trajectory correction. Value must be 1-255. This parameter maybe decreased if there are issues
-// with the accuracy of the arc generations. In general, the default value is more than enough for
-// the intended CNC applications of grbl, and should be on the order or greater than the size of
-// the buffer to help with the computational efficiency of generating arcs.
-#define N_ARC_CORRECTION 25 
+// Number of arc generation iterations by small angle approximation before exact arc trajectory 
+// correction. This parameter maybe decreased if there are issues with the accuracy of the arc
+// generations. In general, the default value is more than enough for the intended CNC applications
+// of grbl, and should be on the order or greater than the size of the buffer to help with the 
+// computational efficiency of generating arcs.
+#define N_ARC_CORRECTION 25 // Integer (1-255)
 
 #endif
 
diff --git a/stepper.c b/stepper.c
index fdd73f9..f39be06 100644
--- a/stepper.c
+++ b/stepper.c
@@ -86,7 +86,9 @@ void st_wake_up() {
 static void st_go_idle() {
   // Force stepper dwell to lock axes for a defined amount of time to ensure the axes come to a complete
   // stop and not drift from residual inertial forces at the end of the last movement.
-  _delay_ms(STEPPER_IDLE_LOCK_TIME);   
+  #if STEPPER_IDLE_LOCK_TIME
+    _delay_ms(STEPPER_IDLE_LOCK_TIME);   
+  #endif
   // Disable steppers by setting stepper disable
   STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT);
   // Disable stepper driver interrupt
@@ -207,6 +209,8 @@ SIGNAL(TIMER1_COMPA_vect)
             // rounding errors that might leave steps hanging after the last trapezoid tick.
             if (trapezoid_adjusted_rate > current_block->rate_delta) {
               trapezoid_adjusted_rate -= current_block->rate_delta;
+            } else {
+              trapezoid_adjusted_rate >>= 1; // Bit shift divide by 2
             }
             if (trapezoid_adjusted_rate < current_block->final_rate) {
               // Reached final rate a little early. Cruise to end of block at final rate.