diff --git a/g2core/cycle_feedhold.cpp b/g2core/cycle_feedhold.cpp
index d45b53e3..7f8f73a6 100644
--- a/g2core/cycle_feedhold.cpp
+++ b/g2core/cycle_feedhold.cpp
@@ -559,6 +559,56 @@ static void _start_p2_feedhold()
 }
 */
 
+/*  
+ * _enter_p2()  - enter p2 planner with proper state transfer from p1
+ * _exit_p2()   - reenter p1 planner with proper state transfer from p2
+ *
+ * Encapsulate entering and exiting p2, as this is tricky and must be done exactly right
+ */
+
+static void _enter_p2()
+{
+    // Copy the primary canonical machine to the secondary. Here it's OK to co a memcpy.
+    // Set parameters in cm, gm and gmx so you can actually use it
+    memcpy(&cm2, &cm1, sizeof(cmMachine_t));
+    cm2.hold_state = FEEDHOLD_OFF;
+    cm2.gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE;
+    cm2.gm.absolute_override = ABSOLUTE_OVERRIDE_OFF;
+    cm2.queue_flush_state = QUEUE_FLUSH_OFF;
+    cm2.gm.feed_rate = 0;
+
+    // Set mp planner to p2 and reset it
+    cm2.mp = &mp2;
+    planner_reset((mpPlanner_t *)cm2.mp);   // mp is a void pointer
+
+    // Clear the target and set the positions to the current hold position
+    memset(&(cm2.return_flags), 0, sizeof(cm2.return_flags));
+    memset(&(cm2.gm.target), 0, sizeof(cm2.gm.target));
+    memset(&(cm2.gm.target_comp), 0, sizeof(cm2.gm.target_comp)); // zero Kahan compensation
+
+    copy_vector(cm2.gmx.position, mr1.position);
+    copy_vector(mp2.position, mr1.position);
+    copy_vector(mr2.position, mr1.position);
+
+    // Copy MR position and encoder terms - needed for following error correction state
+    copy_vector(mr2.target_steps, mr1.target_steps);
+    copy_vector(mr2.position_steps, mr1.position_steps);
+    copy_vector(mr2.commanded_steps, mr1.commanded_steps);
+    copy_vector(mr2.encoder_steps, mr1.encoder_steps);  // NB: following error is re-computed in p2
+
+    // Reassign the globals to the secondary CM
+    cm = &cm2;
+    mp = (mpPlanner_t *)cm->mp;     // mp is a void pointer
+    mr = mp->mr;
+}
+
+static void _exit_p2()
+{
+    cm = &cm1;                          // return to primary planner (p1)
+    mp = (mpPlanner_t *)cm->mp;         // cm->mp is a void pointer
+    mr = mp->mr;
+}
+
 static void _check_motion_stopped()
 {
     if (mp_runtime_is_idle()) {                         // wait for steppers to actually finish
@@ -606,11 +656,11 @@ static stat_t _feedhold_no_actions()
         cm1.hold_type = FEEDHOLD_TYPE_HOLD;
 //      cm1.hold_exit = FEEDHOLD_EXIT_STOP;     // default exit for NO_ACTIONS is STOP...
 
-        if (cm1.motion_state == MOTION_STOP) {   // if motion has already stopped declare that you are in a feedhold
+        if (cm1.motion_state == MOTION_STOP) {  // if motion has already stopped declare that you are in a feedhold
             _check_motion_stopped();
             cm1.hold_state = FEEDHOLD_HOLD;
         } else {
-            cm1.hold_state = FEEDHOLD_SYNC;         // ... STOP can be overridden by setting hold_exit after this function
+            cm1.hold_state = FEEDHOLD_SYNC;     // ... STOP can be overridden by setting hold_exit after this function
             return (STAT_EAGAIN);
         }
     }
@@ -632,55 +682,6 @@ static void _feedhold_actions_done_callback(float* vect, bool* flag)
     sr_request_status_report(SR_REQUEST_IMMEDIATE);
 }
 
-/****************************************************************************************
- * _enter_p2()  - enter p2 planner with proper state transfer from p1
- * _exit_p2()   - reenter p1 planner with proper state transfer from p2
- *
- * Encapsulate entering and exiting p2, as this is tricky and must be done exactly right
- */
-
-static void _enter_p2()
-{
-    // Copy the primary canonical machine to the secondary. Here it's OK to co a memcpy.
-    // Set parameters in cm, gm and gmx so you can actually use it
-    memcpy(&cm2, &cm1, sizeof(cmMachine_t));
-    cm2.hold_state = FEEDHOLD_OFF;
-    cm2.gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE;
-    cm2.gm.absolute_override = ABSOLUTE_OVERRIDE_OFF;
-    cm2.queue_flush_state = QUEUE_FLUSH_OFF;
-    cm2.gm.feed_rate = 0;
-
-    // Set mp planner to p2 and reset it
-    cm2.mp = &mp2;
-    planner_reset((mpPlanner_t *)cm2.mp);   // mp is a void pointer
-
-    // Clear the target and set the positions to the current hold position
-    memset(&(cm2.gm.target), 0, sizeof(cm2.gm.target));
-    memset(&(cm2.return_flags), 0, sizeof(cm2.return_flags));
-    copy_vector(cm2.gm.target_comp, cm1.gm.target_comp); // preserve original Kahan compensation
-    copy_vector(cm2.gmx.position, mr1.position);
-    copy_vector(mp2.position, mr1.position);
-    copy_vector(mr2.position, mr1.position);
-
-    // Copy MR position and encoder terms - needed for following error correction state
-    copy_vector(mr2.target_steps, mr1.target_steps);
-    copy_vector(mr2.position_steps, mr1.position_steps);
-    copy_vector(mr2.commanded_steps, mr1.commanded_steps);
-    copy_vector(mr2.encoder_steps, mr1.encoder_steps);  // NB: following error is re-computed in p2
-
-    // Reassign the globals to the secondary CM
-    cm = &cm2;
-    mp = (mpPlanner_t *)cm->mp;     // mp is a void pointer
-    mr = mp->mr;
-}
-
-static void _exit_p2()
-{
-    cm = &cm1;                          // return to primary planner (p1)
-    mp = (mpPlanner_t *)cm->mp;         // cm->mp is a void pointer
-    mr = mp->mr;
-}
-
 static stat_t _feedhold_with_actions()          // Execute Case (5)
 {
     // if entered while OFF start a feedhold
diff --git a/g2core/plan_exec.cpp b/g2core/plan_exec.cpp
index b4a8effa..d2b4b1f6 100644
--- a/g2core/plan_exec.cpp
+++ b/g2core/plan_exec.cpp
@@ -892,6 +892,18 @@ static stat_t _exec_aline_tail(mpBuf_t *bf)
  *         -100        -90           -10        encoder is 10 steps behind commanded steps
  */
 
+//+++++ DIAGNOSTIC +++++
+#pragma GCC push_options       
+#pragma GCC optimize ("O0")
+// insert function here
+static void _hold_everything (int32_t linenum, uint32_t segments)
+{
+    if ((mr2.gm.linenum == linenum) && (mr2.segment_count == segments)) {
+        cm1.gm.linenum +=1;
+    }
+}
+#pragma GCC reset_options
+
 static stat_t _exec_aline_segment()
 {
     float travel_steps[MOTORS];
@@ -930,9 +942,14 @@ static stat_t _exec_aline_segment()
         mr->following_error[m] = mr->encoder_steps[m] - mr->commanded_steps[m];
     }
     kn_inverse_kinematics(mr->gm.target, mr->target_steps); // now determine the target steps...
+
+    _hold_everything(159, 2); //+++++
+
     for (uint8_t m=0; m<MOTORS; m++) {                      // and compute the distances to be traveled
-//        mr->travel_steps[m] = mr->target_steps[m] - mr->position_steps[m];
         travel_steps[m] = mr->target_steps[m] - mr->position_steps[m];
+        if (fabs(travel_steps[m]) < 0.01) {                 // truncate very small moves to deal with rounding erors
+            travel_steps[m] = 0;
+        }
     }
 
     // Update the mb->run_time_remaining -- we know it's missing the current segment's time before it's loaded, that's ok.
@@ -942,7 +959,6 @@ static stat_t _exec_aline_segment()
     }
 
     // Call the stepper prep function
-//    ritorno(st_prep_line(mr->travel_steps, mr->following_error, mr->segment_time));
     ritorno(st_prep_line(travel_steps, mr->following_error, mr->segment_time));
     copy_vector(mr->position, mr->gm.target);               // update position from target
     if (mr->segment_count == 0) {
diff --git a/g2core/stepper.cpp b/g2core/stepper.cpp
index 28430c3f..eb7d572c 100644
--- a/g2core/stepper.cpp
+++ b/g2core/stepper.cpp
@@ -78,6 +78,25 @@ Motate::SysTickEvent dwell_systick_event {[&] {
     }
 }, nullptr};
 
+/* Note on the above:
+It's a lambda function creating a closure function. 
+The full implementation that uses it is small and may help: 
+https://github.com/synthetos/Motate/blob/41e5b92a98de4b268d1804bf6eadf3333298fc75/MotateProject/motate/Atmel_sam_common/SamTimers.h#L1147-L1218
+It's just like a function, and is used as a function pointer.
+
+But the closure part means that whatever variables that were in scope where the 
+[&](parameters){code} is will be captured by the compiler as references in the generated 
+function and used wherever the function gets called. In this particular use, there isn't 
+anything that wouldn't be available anywhere in that file, but they're not being called 
+from that file. They're being called by the systick interrupt which is over in SamTmers.cpp
+So this saves a bunch of work exposing bits that the systick would need to call and encapsulates it.
+And there's almost no runtime overhead. Just a check for a valid function pointer and then a call of it.
+I'd like to get rid of that check but it's more work than its worth.
+
+See here for some good info on lambda functions in C++
+http://www.cprogramming.com/c++11/c++11-lambda-closures.html
+http://en.cppreference.com/w/cpp/language/lambda
+*/
 
 /************************************************************************************
  **** CODE **************************************************************************
diff --git a/g2core/util.h b/g2core/util.h
index b0feef23..e531b272 100644
--- a/g2core/util.h
+++ b/g2core/util.h
@@ -49,7 +49,8 @@ using Motate::SysTickTimer;
 
 /****** Global Scope Variables and Functions ******/
 /*
-#pragma GCC push_options        // DIAGNOSTIC +++++
+// +++++ DIAGNOSTIC +++++
+#pragma GCC push_options
 #pragma GCC optimize ("O0")
 // insert function here
 static void _hold_everything (uint32_t n1, uint32_t n2) // example of function