diff --git a/Arduino/test_r2_constantforce/test_r2_constantforce.ino b/Arduino/test_r2_constantforce/test_r2_constantforce.ino
index 87b7ecc6895cc4928e1f52a6a4d8d52ef53b6838..1d90acb905027a3070c11729fd7b4fc2cfcfd211 100644
--- a/Arduino/test_r2_constantforce/test_r2_constantforce.ino
+++ b/Arduino/test_r2_constantforce/test_r2_constantforce.ino
@@ -16,8 +16,17 @@
 // - To maintain constant force sign, we want the stepper's internal phase to be ahead/behind the position phase.
 
 
-// BROKEN!!!
-// - Seems to get stuck, possibly losing sync? (thinks the phases are set to move the motor, but no movement)
+// Starting from a zero rest state
+// - Set the coils ahead
+//   > When count goes below -5, need to switch backwards one step (pulling towards zero)
+//   > When count goes above 5, switch forwards (pushing positive)
+
+// STATUS:
+// - Sometimes moves well, sometimes stops
+// - When moving, the motor can't be reversed.
+// - When it stops, need to override step count to get it moving again.
+//   - Might be a miscount of encoder ticks?
+// IDEA: In final build, have a self-calibration routine that moves the motor with no load to calibrate/reset state
 
 // NOTE:
 // - Encoder is 1000 pulse (4000 counts) per revolution
@@ -37,7 +46,8 @@ const int OUTPUT_CLK1 = 13;
 const int INPUT_VREF1 = 15;  // A1
 const int INPUT_POT1 = 18;
 
-int last_1, val_1;
+volatile int last_1;
+volatile int val_1;
 
 int ctrl_phase_1;  // Current "phase" of the controller (0-3)
 int axle_phase_1;  // Current "phase" of the axle (0-3)
@@ -55,9 +65,11 @@ void step_once_1(int dir) {
   ctrl_phase_1 = phase_add(ctrl_phase_1, dir < 0 ? -1 : 1);
   
   digitalWrite(OUTPUT_DIR1, dir < 0);
+  delayMicroseconds(1);
   digitalWrite(OUTPUT_CLK1, 1);
-  delay(1);
+  delayMicroseconds(1);
   digitalWrite(OUTPUT_CLK1, 0);
+  delayMicroseconds(1);
 }
 
 void setup()
@@ -93,24 +105,61 @@ 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;
+int backdrive_count;
+int TARGET_DIR = 1;
+int s_run = 1;
+int last_seen_encoder;
 void loop()
 {
   int should_log = 0;
+  if( Serial.available() ) {
+    int b = Serial.read();
+    int upd = 1;
+    switch(b)
+    {
+    case 'w': axle_phase_1 = phase_add(axle_phase_1,  1); break;
+    case 's': axle_phase_1 = phase_add(axle_phase_1, -1); break;
+    case 'a': TARGET_DIR = (TARGET_DIR >= 0 ?  1 : 0); break;
+    case 'd': TARGET_DIR = (TARGET_DIR <= 0 ? -1 : 0); break;
+    case 'p': s_run = s_run ? 0 : 1; break;
+    //case 'o': s_run = !s_run; break;
+    case ' ': break;
+    default:
+      upd = 0;
+      break;
+    }
+    if(upd) {
+      Serial.print("axle_phase_1="); Serial.print(axle_phase_1);
+      Serial.print("TARGET_DIR=");
+      Serial.println(TARGET_DIR);
+      should_log = 1;
+    }
+  }
+  
+  
   // 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 ) {
+  int v0 = val_1;
+  int v = v0;
+  while( v <= -6 ) {
     val_1 += 10;
+    v     += 10;
     axle_phase_1 = phase_add(axle_phase_1, -1);
-    should_log = 1;
+    //should_log = 1;
   }
-  else if( v >= 5 ) {
+  while( v >= 6 ) {
     val_1 -= 10;
+    v     -= 10;
     axle_phase_1 = phase_add(axle_phase_1, +1);
-    should_log = 1;
+    //should_log = 1;
   }
-  else {
-    // In-range
+  // In-range
+   
+  if( last_seen_encoder != v0 ) {
+    last_seen_encoder = v0;
+    if( !s_run ){
+      should_log = true;
+    }
   }
   
   // Compare `ctrl_phase_1` and `real_phase_1` to determine the current "mode"
@@ -119,21 +168,28 @@ void loop()
   // - Opposite: Bad - unstable version of holding configuration. Step once to get direction going the right way again.
   
   // Get a target `ctrl_phase_1` based on `real_phase_1` and a target direction
-  static const int TARGET_DIR = 1;
   int target_ctrl_phase_1 = phase_add(axle_phase_1, TARGET_DIR);
   
   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 ) {
+  int phase_diff = phase_add(cur_ctrl_phase_1, -target_ctrl_phase_1);
+  if( s_run == 0 ) {
+  }
+  else if( phase_diff == 0 ) {
     // No need to change
   }
   else if( phase_diff == 3 ) {
     // Need to shift phase up
+    if( TARGET_DIR < 0 ) {
+      backdrive_count += 1;
+    }
     step_once_1(1);
     step_count += 1;
   }
   else if( phase_diff == 1 ) {
     // Need to shift phase down
+    if( TARGET_DIR > 0 ) {
+      backdrive_count += 1;
+    }
     step_once_1(-1);
     step_count += 1;
   }
@@ -141,8 +197,8 @@ void loop()
     // Going opposite! - Run twice
     //step_once_1(1);
     //step_once_1(1);
+    //step_count += 2;
     err_count += 1;
-    step_count += 2;
   }
   
   loop_count += 1;
@@ -155,14 +211,18 @@ void loop()
   // - Current phase values
   // - Total number of steps taken since last log
   // - Number of times the phases have ended up opposed
-  if( step_count > 0 || err_count > 0 || should_log ) {
-    if( true /*Serial.availableForWrite() > 20*/ ) {
+  if( /*step_count > 0 ||*/ err_count > 0 || should_log /*|| phase_diff != 0*/ ) {
+    if( !s_run /*Serial.availableForWrite() > 20*/ ) {
       Serial.print(step_count);
+      Serial.print(" "); Serial.print(last_seen_encoder);
       Serial.print(" E="); Serial.print(err_count);
+      Serial.print(" B="); Serial.print(backdrive_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.print("="); Serial.print(phase_diff);
+      Serial.print(" D="); Serial.print(TARGET_DIR);
+      Serial.print(s_run);
       Serial.println("");
       step_count = 0;
       err_count = 0;
diff --git a/Arduino/test_r2_encoder/test_r2_encoder.ino b/Arduino/test_r2_encoder/test_r2_encoder.ino
index b4617c8b5c08f243659f51263fbf4247dc96e9e1..1849f3b8535f1d10d61a83072c0b4dfe9f24b2d3 100644
--- a/Arduino/test_r2_encoder/test_r2_encoder.ino
+++ b/Arduino/test_r2_encoder/test_r2_encoder.ino
@@ -32,6 +32,10 @@ void setup()
   pinMode(OUTPUT_CLK1, OUTPUT);
   pinMode(PIN_LED, OUTPUT);
   
+  delay(5);
+  digitalWrite(OUTPUT_MOT_ON, 1);
+  delay(100);
+  
   attachInterrupt(0, isr_enc1, CHANGE);
   attachInterrupt(1, isr_enc1, CHANGE);
 }
@@ -63,8 +67,16 @@ void isr_enc1() {
   }
 }
 int last_seen_1;
+int step_count;
+long count;
 void loop()
 {
+  if( val_1 >= 4000 ) {
+    val_1 -= 4000;
+  }
+  if( val_1 < 0 ) {
+    val_1 += 4000;
+  }
   if( val_1 != last_seen_1 ) {
     Serial.print("Encoder 1: ");
     int v = val_1;
@@ -72,6 +84,7 @@ void loop()
     Serial.print("->");
     Serial.print(v);
     Serial.print(" (EC="); Serial.print(err_count_1); Serial.print(")");
+    Serial.print(step_count);
     Serial.println("");
     last_seen_1 = v;
   }
@@ -89,4 +102,12 @@ void loop()
     }
     Serial.print("Encoder 2: "); Serial.println(val_2);
   }
+  count += 1;
+  if( count > 80000 ) {
+    count = 0;
+    step_count += 1;
+    digitalWrite(OUTPUT_CLK1, 1);
+    delay(1);
+    digitalWrite(OUTPUT_CLK1, 0);
+  }
 }