diff --git a/Arduino/ff_main/ff_main.ino b/Arduino/ff_main/ff_main.ino
new file mode 100644
index 0000000000000000000000000000000000000000..8198ba5b0a9b786496241143f7a9edfbfd884eca
--- /dev/null
+++ b/Arduino/ff_main/ff_main.ino
@@ -0,0 +1,90 @@
+/*
+ ForceFeedback main
+ */
+const int PIN_CHA_O_DIR = 2; //D2;
+const int PIN_CHA_O_VRF = 3; //D3;
+const int PIN_CHA_O_CLK = 4; //D4;
+const int PIN_CHB_O_DIR = 5; //D5;
+const int PIN_CHB_O_VRF = 6; //D6;
+const int PIN_CHB_O_CLK = 7; //D7;
+const int PIN_CHA_I_POT = 14; //A0;
+const int PIN_CHA_I_ENC = 15; //A1;
+const int PIN_CHA_I_VRF = 16; //A2;
+const int PIN_CHB_I_POT = 17; //A3;
+const int PIN_CHB_I_ENC = 18; //A4;
+const int PIN_CHB_I_VRF = 19; //A5;
+const int PIN_LED = 13;  // Orange LED
+
+int prev_encoder_a;
+int prev_encoder_b;
+int v;  // Temporary
+int counter;
+int raw_a;
+int prev_raw_a;
+
+void setup()
+{
+  Serial.begin(9600);
+  pinMode(PIN_CHA_O_DIR, OUTPUT);
+  pinMode(PIN_CHA_O_VRF, OUTPUT);	// PWM
+  pinMode(PIN_CHA_O_CLK, OUTPUT);
+  pinMode(PIN_CHB_O_DIR, OUTPUT);
+  pinMode(PIN_CHB_O_VRF, OUTPUT);	// PWM
+  pinMode(PIN_CHB_O_CLK, OUTPUT);
+  pinMode(PIN_LED, OUTPUT);   
+}
+
+void loop()
+{
+  if(true) {
+    raw_a = analogRead(PIN_CHA_I_ENC);
+    if( abs(raw_a - prev_raw_a) > 10 ) {
+      Serial.print("A "); Serial.print(raw_a); Serial.println("");
+      prev_raw_a = raw_a;
+    }
+  }
+  if( false ) {
+    raw_a = analogRead(PIN_CHA_I_ENC);
+  v = map(raw_a, 0, 1024, 0, 15);
+  if( v != prev_encoder_a ) {
+    Serial.print("A ");
+    Serial.print(prev_encoder_a);
+    Serial.print(" -> ");
+    Serial.print(v);
+    Serial.print(": ");
+    Serial.print(raw_a);
+    Serial.println("");
+    prev_encoder_a = v;
+  }
+  }
+
+  if(false) {
+  v = map(analogRead(PIN_CHB_I_ENC), 0, 1023, 0, 15);
+  if( v != prev_encoder_b ) {
+    Serial.print("B ");
+    Serial.print(prev_encoder_b);
+    Serial.print(" -> ");
+    Serial.println(v);
+    prev_encoder_b = v;
+  }
+  }
+  
+  // Blink a LED at 500ms/500ms
+  counter += 1;
+  if( counter == 50 ) {
+    digitalWrite(PIN_LED, HIGH);
+  }
+  if(counter >= 100) {
+    digitalWrite(PIN_LED, LOW);
+    counter = 0;
+    if(false) {
+    Serial.print("RAW ");
+    Serial.print(analogRead(PIN_CHA_I_ENC));
+    Serial.print(" ");
+    Serial.print(analogRead(PIN_CHB_I_ENC));
+    Serial.println("");
+    }
+  }
+  delay(10);
+}
+
diff --git a/Arduino/test_analog/test_analog.ino b/Arduino/test_analog/test_analog.ino
new file mode 100644
index 0000000000000000000000000000000000000000..36dd62c357c613c543530b6bc19232e17adf603e
--- /dev/null
+++ b/Arduino/test_analog/test_analog.ino
@@ -0,0 +1,50 @@
+/*
+ TEST: DAC with feedback.
+ */
+
+const int PIN_CHA_O_VRF = 3;
+const int PIN_CHB_O_VRF = 6;
+const int PIN_CHA_I_VRF = 16; //A2;
+const int PIN_CHB_I_VRF = 19; //A5
+const int PIN_LED = 13;  // Orange LED
+
+void setup()
+{
+  Serial.begin(9600);
+  pinMode(PIN_CHA_O_VRF, OUTPUT);	// PWM
+  pinMode(PIN_CHB_O_VRF, OUTPUT);	// PWM
+  pinMode(PIN_LED, OUTPUT);   
+}
+
+int step_val;
+int cnt;
+void loop()
+{
+  Serial.print(step_val);
+  Serial.print(" ");
+  analogWrite(PIN_CHB_O_VRF, step_val);
+  analogWrite(PIN_CHA_O_VRF, step_val);
+  delay(50);
+  Serial.print(analogRead(PIN_CHA_I_VRF));
+  Serial.print(" ");
+  Serial.print(analogRead(PIN_CHB_I_VRF));
+  Serial.print(",");
+  delay(50);
+  Serial.print(analogRead(PIN_CHA_I_VRF));
+  Serial.print(" ");
+  Serial.print(analogRead(PIN_CHB_I_VRF));
+  Serial.println("");
+  
+  cnt += 1;
+  if( cnt > 1 )
+  {
+    cnt = 0;
+    step_val += 1;
+    if( step_val == 256 ) {
+      step_val = 0;
+    }
+  }
+  digitalWrite(PIN_LED, (step_val & 1) ? HIGH : LOW);
+  delay(100);
+}
+
diff --git a/Arduino/test_r2_analog/test_r2_analog.ino b/Arduino/test_r2_analog/test_r2_analog.ino
new file mode 100644
index 0000000000000000000000000000000000000000..9ccd25d9f874df6c90782dbc10755e896913e5b5
--- /dev/null
+++ b/Arduino/test_r2_analog/test_r2_analog.ino
@@ -0,0 +1,50 @@
+/*
+ TEST: DAC with feedback.
+ */
+
+const int PIN_CHA_O_VRF = 5;
+const int PIN_CHB_O_VRF = 6;
+const int PIN_CHA_I_VRF = 14; //A2;
+const int PIN_CHB_I_VRF = 15; //A5
+const int PIN_LED = 13;  // Orange LED
+
+void setup()
+{
+  Serial.begin(9600);
+  pinMode(PIN_CHA_O_VRF, OUTPUT);	// PWM
+  pinMode(PIN_CHB_O_VRF, OUTPUT);	// PWM
+  pinMode(PIN_LED, OUTPUT);   
+}
+
+int step_val;
+int cnt;
+void loop()
+{
+  Serial.print(step_val);
+  Serial.print(" ");
+  analogWrite(PIN_CHB_O_VRF, step_val);
+  analogWrite(PIN_CHA_O_VRF, step_val);
+  delay(50);
+  Serial.print(analogRead(PIN_CHA_I_VRF));
+  Serial.print(" ");
+  Serial.print(analogRead(PIN_CHB_I_VRF));
+  Serial.print(",");
+  delay(50);
+  Serial.print(analogRead(PIN_CHA_I_VRF));
+  Serial.print(" ");
+  Serial.print(analogRead(PIN_CHB_I_VRF));
+  Serial.println("");
+  
+  cnt += 1;
+  if( cnt > 1 )
+  {
+    cnt = 0;
+    step_val += 1;
+    if( step_val == 256 ) {
+      step_val = 0;
+    }
+  }
+  digitalWrite(PIN_LED, (step_val & 1) ? HIGH : LOW);
+  delay(100);
+}
+
diff --git a/Arduino/test_r2_constantforce/test_r2_constantforce.ino b/Arduino/test_r2_constantforce/test_r2_constantforce.ino
new file mode 100644
index 0000000000000000000000000000000000000000..03be2fe684d284b59b722a863b4c31aa4fea8294
--- /dev/null
+++ b/Arduino/test_r2_constantforce/test_r2_constantforce.ino
@@ -0,0 +1,178 @@
+/*
+ ForceFeedback Test: Constant force movement.
+ BOARD: Rev2
+ */
+
+// To move with constant force, need:
+// - A force (set with VREF)
+// - A direction
+// Always want the energised coils to be pulling one way
+// - When it gets too close to the target position, swap upwards
+// - When too far way, swap closer.
+// NOTE: Want to track the coil phase (position and output) internally, so that if there's slip 
+// the sign can be recovered
+// - Encoder `count/10` gives the step index, there's four phases to the steps (A+, B+, A-, B-)
+// - 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 
+
+// NOTE:
+// - Encoder is 1000 pulse (4000 counts) per revolution
+// - Stepper is 400 steps per resolution
+// > So, expect 10 counts (2.5 cycles) per stepper step.
+// > Step the motor when a change of more than 10 is seen?
+
+// NOTE: DIR=0 is positive encoder (on CH1)
+
+const int INPUT_ENC1A = 2;
+const int INPUT_ENC1B = 3;
+const int OUTPUT_VREF1 = 5;  // D5
+const int OUTPUT_VREF2 = 6;  // D6
+const int OUTPUT_MOT_ON = 9;
+const int OUTPUT_DIR1 = 12;
+const int OUTPUT_CLK1 = 13;
+const int INPUT_VREF1 = 15;  // A1
+const int INPUT_POT1 = 18;
+
+int last_1, val_1;
+
+int ctrl_phase_1;  // Current "phase" of the controller (0-3)
+int axle_phase_1;  // Current "phase" of the axle (0-3)
+
+// Addition modulo 4
+static int phase_add(int v, int i) {
+  v += i;
+  while(v > 3)  v -= 4;
+  while(v < 0)  v += 4;
+  return v;
+}
+
+// Step channel once in the given direction
+void step_once_1(int dir) {
+  ctrl_phase_1 = phase_add(ctrl_phase_1, dir < 0 ? -1 : 1);
+  
+  digitalWrite(OUTPUT_DIR1, dir < 0);
+  digitalWrite(OUTPUT_CLK1, 1);
+  delay(1);
+  digitalWrite(OUTPUT_CLK1, 0);
+}
+
+void setup()
+{
+  Serial.begin(9600);
+  pinMode(OUTPUT_VREF1, OUTPUT);
+  pinMode(OUTPUT_MOT_ON, OUTPUT);
+  pinMode(OUTPUT_DIR1, OUTPUT);
+  pinMode(OUTPUT_CLK1, OUTPUT);
+//  pinMode(PIN_LED, OUTPUT);
+  
+  attachInterrupt(0, isr_enc1, CHANGE);
+  attachInterrupt(1, isr_enc1, CHANGE);
+  
+  // Set output current to 50%
+  analogWrite(OUTPUT_VREF1, 128);
+  analogWrite(OUTPUT_VREF2, 0);
+  delay(5);
+  // Read it back
+  Serial.print("VRef1 = ");
+  Serial.print(analogRead(INPUT_VREF1));
+  Serial.println("");
+  // Turn controllers on
+  digitalWrite(OUTPUT_MOT_ON, 1);
+  
+  // Wait some time
+  delay(100);
+  // Reset the counters
+  val_1 = 0;
+}
+
+void loop()
+{
+  // 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);
+  }
+  else if( v >= 5 ) {
+    val_1 -= 10;
+    axle_phase_1 = phase_add(axle_phase_1, +1);
+  }
+  else {
+    // In-range
+  }
+  
+  // Compare `ctrl_phase_1` and `real_phase_1` to determine the current "mode"
+  // - Equal: The system is now holding, we want to push `ctrl` ahead/behind `real` (depending on target direction)
+  // - +/- One: Either pulling the wrong way (need to to step twice), or pulling the right way (no change)
+  // - 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 phase_diff = phase_add(ctrl_phase_1, target_ctrl_phase_1);
+  if( phase_diff == 0 ) {
+    // No need to change
+  }
+  else if( phase_diff == 3 ) {
+    // Need to shift phase up
+    step_once_1(1);
+    step_count += 1;
+  }
+  else if( phase_diff == 1 ) {
+    // Need to shift phase down
+    step_once_1(-1);
+    step_count -= 1;
+  }
+  else {
+    // Going opposite! - Run twice
+    step_once_1(1);
+    step_once_1(1);
+    err_count += 1;
+    step_count += 2;
+  }
+  
+  // 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;
+      step_count = 0;
+      err_count = 0;
+    }
+  }
+}
+
+const int E = -99;
+const int GREY_MAPPING[16] = {
+  0, -1, 1, E,
+  1, 0, E, -1,
+  -1, E, 0, 1,
+  E, 1, -1, 0,
+};
+
+void isr_enc1() {
+  int v = digitalRead(INPUT_ENC1A) * 2 + digitalRead(INPUT_ENC1B);
+  if( v != last_1 ) {
+    int adj = GREY_MAPPING[last_1 * 4 + v];
+    if(adj == E) {
+    }
+    else {
+      val_1 += adj;
+    }
+    last_1 = v;
+  }
+}
+
diff --git a/Arduino/test_r2_encoder/test_r2_encoder.ino b/Arduino/test_r2_encoder/test_r2_encoder.ino
new file mode 100644
index 0000000000000000000000000000000000000000..b4617c8b5c08f243659f51263fbf4247dc96e9e1
--- /dev/null
+++ b/Arduino/test_r2_encoder/test_r2_encoder.ino
@@ -0,0 +1,92 @@
+/*
+ ForceFeedback Test: Encoder counter
+ BOARD: Rev2
+ */
+// #include "../board_v2.h"
+const int INPUT_ENC1A = 2;
+const int INPUT_ENC1B = 3;
+const int INPUT_ENC2A = 4;
+const int OUTPUT_VREF1 = 5;  // D5
+const int OUTPUT_VREF2 = 6;  // D6
+const int INPUT_ENC2B = 7;
+const int OUTPUT_MOT_ON = 9;
+const int OUTPUT_DIR2 = 10;
+const int OUTPUT_CLK2 = 11;
+const int OUTPUT_DIR1 = 12;
+const int OUTPUT_CLK1 = 13;
+const int INPUT_VREF2 = 14;  // A0
+const int INPUT_VREF1 = 15;  // A1
+const int INPUT_POT1 = 18;
+const int INPUT_POT2 = 19;
+const int PIN_LED = 13;  // Orange LED
+
+void setup()
+{
+  Serial.begin(9600);
+  pinMode(OUTPUT_VREF1, OUTPUT);
+  pinMode(OUTPUT_VREF2, OUTPUT);
+  pinMode(OUTPUT_MOT_ON, OUTPUT);
+  pinMode(OUTPUT_DIR2, OUTPUT);
+  pinMode(OUTPUT_CLK2, OUTPUT);
+  pinMode(OUTPUT_DIR1, OUTPUT);
+  pinMode(OUTPUT_CLK1, OUTPUT);
+  pinMode(PIN_LED, OUTPUT);
+  
+  attachInterrupt(0, isr_enc1, CHANGE);
+  attachInterrupt(1, isr_enc1, CHANGE);
+}
+
+int v;
+int last_1, val_1;
+int last_2, val_2;
+const int E = -99;
+const int GREY_MAPPING[16] = {
+  0, -1, 1, E,
+  1, 0, E, -1,
+  -1, E, 0, 1,
+  E, 1, -1, 0,
+};
+
+int err_count_1;
+void isr_enc1() {
+  int v = digitalRead(INPUT_ENC1A) * 2 + digitalRead(INPUT_ENC1B);
+  if( v != last_1 ) {
+    // Count on rising edge of [A], depending on sign of [B]
+    int adj = GREY_MAPPING[last_1 * 4 + v];
+    if(adj == E) {
+      err_count_1 += 1;
+    }
+    else {
+      val_1 += adj;
+    }
+    last_1 = v;
+  }
+}
+int last_seen_1;
+void loop()
+{
+  if( val_1 != last_seen_1 ) {
+    Serial.print("Encoder 1: ");
+    int v = val_1;
+    Serial.print(last_seen_1);
+    Serial.print("->");
+    Serial.print(v);
+    Serial.print(" (EC="); Serial.print(err_count_1); Serial.print(")");
+    Serial.println("");
+    last_seen_1 = v;
+  }
+  
+  v = digitalRead(INPUT_ENC2A) * 2 + digitalRead(INPUT_ENC2B);
+  if( v != last_2 ) {
+    int diff = v  ^ last_2;
+    last_2 = v;
+    switch(diff)
+    {
+    case 0: break;
+    case 1: val_2 += 1; break;
+    case 2: val_2 -= 1; break;
+    case 3: break;
+    }
+    Serial.print("Encoder 2: "); Serial.println(val_2);
+  }
+}
diff --git a/Arduino/test_r2_stepper/test_r2_stepper.ino b/Arduino/test_r2_stepper/test_r2_stepper.ino
new file mode 100644
index 0000000000000000000000000000000000000000..9c520757528af85e91db53699acd1030f9e3002d
--- /dev/null
+++ b/Arduino/test_r2_stepper/test_r2_stepper.ino
@@ -0,0 +1,93 @@
+/*
+ ForceFeedback Test: Stepper moving with encoder
+ BOARD: Rev2
+ */
+// #include "../board_v2.h"
+const int INPUT_ENC1A = 2;
+const int INPUT_ENC1B = 3;
+const int INPUT_ENC2A = 4;
+const int OUTPUT_VREF1 = 5;  // D5
+const int OUTPUT_VREF2 = 6;  // D6
+const int INPUT_ENC2B = 7;
+const int OUTPUT_MOT_ON = 9;
+const int OUTPUT_DIR2 = 10;
+const int OUTPUT_CLK2 = 11;
+const int OUTPUT_DIR1 = 12;
+const int OUTPUT_CLK1 = 13;
+const int INPUT_VREF2 = 14;  // A0
+const int INPUT_VREF1 = 15;  // A1
+const int INPUT_POT1 = 18;
+const int INPUT_POT2 = 19;
+const int PIN_LED = 13;  // Orange LED
+
+
+const int E = -99;
+const int GREY_MAPPING[16] = {
+  0, -1, 1, E,
+  1, 0, E, -1,
+  -1, E, 0, 1,
+  E, 1, -1, 0,
+};
+int last_1, val_1;
+int err_count_1;
+
+void setup()
+{
+  Serial.begin(9600);
+  pinMode(OUTPUT_VREF1, OUTPUT);
+  pinMode(OUTPUT_VREF2, OUTPUT);
+  pinMode(OUTPUT_MOT_ON, OUTPUT);
+  pinMode(OUTPUT_DIR2, OUTPUT);
+  pinMode(OUTPUT_CLK2, OUTPUT);
+  pinMode(OUTPUT_DIR1, OUTPUT);
+  pinMode(OUTPUT_CLK1, OUTPUT);
+  pinMode(PIN_LED, OUTPUT);
+  
+  attachInterrupt(0, isr_enc1, CHANGE);
+  attachInterrupt(1, isr_enc1, CHANGE);
+  
+  digitalWrite(OUTPUT_MOT_ON, 1);
+  digitalWrite(OUTPUT_DIR1, 0);
+  delay(100);
+  val_1 = 0;
+}
+
+void isr_enc1() {
+  int v = digitalRead(INPUT_ENC1A) * 2 + digitalRead(INPUT_ENC1B);
+  if( v != last_1 ) {
+    // Count on rising edge of [A], depending on sign of [B]
+    int adj = GREY_MAPPING[last_1 * 4 + v];
+    if(adj == E) {
+      err_count_1 += 1;
+    }
+    else {
+      val_1 += adj;
+    }
+    last_1 = v;
+  }
+}
+int last_seen_1;
+int delay_count;
+void loop()
+{
+  if( val_1 != last_seen_1 ) {
+    Serial.print("Encoder 1: ");
+    int v = val_1;
+    Serial.print(last_seen_1);
+    Serial.print("->");
+    Serial.print(v);
+    Serial.print(" (EC="); Serial.print(err_count_1); Serial.print(")");
+    Serial.println("");
+    last_seen_1 = v;
+  }
+  
+  // Shift the motor every 1s
+  if( delay_count > 100 ) {
+    delay_count = 0;
+    digitalWrite(OUTPUT_CLK1, 1);
+    delay(1);
+    digitalWrite(OUTPUT_CLK1, 0);
+  }
+  delay(10);
+  delay_count += 1; 
+}
diff --git a/Arduino/test_stepper/test_stepper.ino b/Arduino/test_stepper/test_stepper.ino
new file mode 100644
index 0000000000000000000000000000000000000000..e040736c65a104a34a3ab8a49dad63be143f7c3a
--- /dev/null
+++ b/Arduino/test_stepper/test_stepper.ino
@@ -0,0 +1,41 @@
+/*
+ ForceFeedback Test: Stepper movement
+ */
+const int PIN_CHA_O_DIR = 2; //D2;
+const int PIN_CHA_O_VRF = 3; //D3;
+const int PIN_CHA_O_CLK = 4; //D4;
+const int PIN_CHB_O_DIR = 5; //D5;
+const int PIN_CHB_O_VRF = 6; //D6;
+const int PIN_CHB_O_CLK = 7; //D7;
+const int PIN_CHA_I_POT = 14; //A0;
+const int PIN_CHA_I_ENC = 15; //A1;
+const int PIN_CHA_I_VRF = 16; //A2;
+const int PIN_CHB_I_POT = 17; //A3;
+const int PIN_CHB_I_ENC = 18; //A4;
+const int PIN_CHB_I_VRF = 19; //A5;
+const int PIN_LED = 13;  // Orange LED
+
+void setup()
+{
+  Serial.begin(9600);
+  pinMode(PIN_CHA_O_DIR, OUTPUT);
+  pinMode(PIN_CHA_O_VRF, OUTPUT);	// PWM
+  pinMode(PIN_CHA_O_CLK, OUTPUT);
+  pinMode(PIN_CHB_O_DIR, OUTPUT);
+  pinMode(PIN_CHB_O_VRF, OUTPUT);	// PWM
+  pinMode(PIN_CHB_O_CLK, OUTPUT);
+  pinMode(PIN_LED, OUTPUT);   
+  
+  digitalWrite(PIN_CHA_O_DIR, LOW);
+}
+
+
+void loop()
+{
+  digitalWrite(PIN_CHA_O_CLK, HIGH);
+    digitalWrite(PIN_LED, HIGH);
+  delay(100);
+  digitalWrite(PIN_CHA_O_CLK, LOW);
+    digitalWrite(PIN_LED, LOW);
+  delay(100);
+}