diff --git a/Arduino/test_r2_constantforce/test_r2_constantforce.ino b/Arduino/test_r2_constantforce/test_r2_constantforce.ino
index 03be2fe684d284b59b722a863b4c31aa4fea8294..87b7ecc6895cc4928e1f52a6a4d8d52ef53b6838 100644
--- a/Arduino/test_r2_constantforce/test_r2_constantforce.ino
+++ b/Arduino/test_r2_constantforce/test_r2_constantforce.ino
@@ -15,11 +15,9 @@
 // - Stepper will switch through these too.
 // - To maintain constant force sign, we want the stepper's internal phase to be ahead/behind the position phase.
 
-// Step 1: Enable motor to position hold at 50% current, wait 100ms
-// Step 2: Zero the encoder
-// Step 3: Step once in target direction
-// Loop:
-// - If 
+
+// BROKEN!!!
+// - Seems to get stuck, possibly losing sync? (thinks the phases are set to move the motor, but no movement)
 
 // NOTE:
 // - Encoder is 1000 pulse (4000 counts) per revolution
@@ -86,23 +84,30 @@ void setup()
   digitalWrite(OUTPUT_MOT_ON, 1);
   
   // Wait some time
-  delay(100);
+  delay(500);
   // Reset the counters
   val_1 = 0;
 }
 
+int step_count;  // Number of steps performed since last log
+int last_val1;  // Last value of `val_1` seen
+int err_count;  // Number of times the phases have been out by 180 degrees
+int loop_count;
 void loop()
 {
+  int should_log = 0;
   // Monitor the encoder value, and see if it exceeds +/- 5
   // - When it does, adjust back into range and update the current axle phase
   int v = val_1;
   if( v <= -5 ) {
     val_1 += 10;
     axle_phase_1 = phase_add(axle_phase_1, -1);
+    should_log = 1;
   }
   else if( v >= 5 ) {
     val_1 -= 10;
     axle_phase_1 = phase_add(axle_phase_1, +1);
+    should_log = 1;
   }
   else {
     // In-range
@@ -117,7 +122,8 @@ void loop()
   static const int TARGET_DIR = 1;
   int target_ctrl_phase_1 = phase_add(axle_phase_1, TARGET_DIR);
   
-  int phase_diff = phase_add(ctrl_phase_1, target_ctrl_phase_1);
+  int cur_ctrl_phase_1 = ctrl_phase_1;
+  int phase_diff = phase_add(cur_ctrl_phase_1, target_ctrl_phase_1);
   if( phase_diff == 0 ) {
     // No need to change
   }
@@ -129,26 +135,35 @@ void loop()
   else if( phase_diff == 1 ) {
     // Need to shift phase down
     step_once_1(-1);
-    step_count -= 1;
+    step_count += 1;
   }
   else {
     // Going opposite! - Run twice
-    step_once_1(1);
-    step_once_1(1);
+    //step_once_1(1);
+    //step_once_1(1);
     err_count += 1;
     step_count += 2;
   }
   
+  loop_count += 1;
+  if(loop_count > 30000) {
+    loop_count = 0;
+    should_log = true;
+  }
+  
   // TODO: Keep track of state to be printed (and only print if there's buffer space)
   // - Current phase values
   // - Total number of steps taken since last log
   // - Number of times the phases have ended up opposed
-  if( false ) {
-    if( Serial.availableForWrite() > 20 ) {
-      Serial.print(v); Serial.print(" "); Serial.print(val_1);
-      Serial.print(" ");
-      Serial.println("STEP");
-      last_val1 = val_1;
+  if( step_count > 0 || err_count > 0 || should_log ) {
+    if( true /*Serial.availableForWrite() > 20*/ ) {
+      Serial.print(step_count);
+      Serial.print(" E="); Serial.print(err_count);
+      Serial.print(" A:"); Serial.print(axle_phase_1);
+      Serial.print(" C:"); Serial.print(cur_ctrl_phase_1);
+      Serial.print("-"); Serial.print(target_ctrl_phase_1);
+      Serial.print(" D="); Serial.print(phase_diff);
+      Serial.println("");
       step_count = 0;
       err_count = 0;
     }