diff --git a/Arduino/test_r2_constantforce/test_r2_constantforce.ino b/Arduino/test_r2_constantforce/test_r2_constantforce.ino
index 1d90acb905027a3070c11729fd7b4fc2cfcfd211..7e4db829677cccababf5277d546a3bbe93529db6 100644
--- a/Arduino/test_r2_constantforce/test_r2_constantforce.ino
+++ b/Arduino/test_r2_constantforce/test_r2_constantforce.ino
@@ -36,8 +36,8 @@
 
 // NOTE: DIR=0 is positive encoder (on CH1)
 
-const int INPUT_ENC1A = 2;
-const int INPUT_ENC1B = 3;
+#define INPUT_ENC1A 2
+#define INPUT_ENC1B 3
 const int OUTPUT_VREF1 = 5;  // D5
 const int OUTPUT_VREF2 = 6;  // D6
 const int OUTPUT_MOT_ON = 9;
@@ -46,7 +46,6 @@ const int OUTPUT_CLK1 = 13;
 const int INPUT_VREF1 = 15;  // A1
 const int INPUT_POT1 = 18;
 
-volatile int last_1;
 volatile int val_1;
 
 int ctrl_phase_1;  // Current "phase" of the controller (0-3)
@@ -75,14 +74,13 @@ void step_once_1(int dir) {
 void setup()
 {
   Serial.begin(9600);
-  pinMode(OUTPUT_VREF1, OUTPUT);
+  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);
+
+  init_isr_enc1();
   
   // Set output current to 50%
   analogWrite(OUTPUT_VREF1, 128);
@@ -230,18 +228,38 @@ void loop()
   }
 }
 
-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,
+const char E = -99;
+// Mapping of transitions of the two encoder state bits to relative encoder counts
+// - low bits are current value
+// - high bits are previous value
+const char GREY_MAPPING[16] = {
+   0, -1,  1,  E,
+   1,  0,  E, -1,
+  -1,  E,  0,  1,
+   E,  1, -1,  0,
 };
 
+
+uint8_t pinmask_ENC1A;
+uint8_t pinmask_ENC1B;
+volatile uint8_t *pinport_ENC1A;
+volatile uint8_t *pinport_ENC1B;
+void init_isr_enc1() {
+  pinmask_ENC1A = digitalPinToBitMask(INPUT_ENC1A);
+  pinmask_ENC1B = digitalPinToBitMask(INPUT_ENC1B);
+  pinport_ENC1A = portInputRegister(digitalPinToPort(INPUT_ENC1A));
+  pinport_ENC1B = portInputRegister(digitalPinToPort(INPUT_ENC1B));
+  attachInterrupt(0, isr_enc1, CHANGE);
+  attachInterrupt(1, isr_enc1, CHANGE);
+}
+volatile char last_1;
 void isr_enc1() {
-  int v = digitalRead(INPUT_ENC1A) * 2 + digitalRead(INPUT_ENC1B);
+  uint8_t pinValue_ENC1A = (*pinport_ENC1A & pinmask_ENC1A) != 0;
+  uint8_t pinValue_ENC1B = (*pinport_ENC1B & pinmask_ENC1B) != 0;
+  
+  char v = (char)pinValue_ENC1A << 1 + (char)pinValue_ENC1B;
   if( v != last_1 ) {
-    int adj = GREY_MAPPING[last_1 * 4 + v];
+    char adj = GREY_MAPPING[ (char)(last_1 << 2) + v ];
     if(adj == E) {
     }
     else {
@@ -250,4 +268,3 @@ void isr_enc1() {
     last_1 = v;
   }
 }
-