     pinMode(motorB, OUTPUT);
     pinMode(enca, INPUT_PULLUP);
     pinMode(encb, INPUT_PULLUP);
+    pinMode(stepSensor, INPUT_PULLUP);
     enca_val = digitalRead(enca);
     encb_val = digitalRead(encb);
     reset();
     attachInterrupt(enca, enca_isr, CHANGE);
     attachInterrupt(encb, encb_isr, CHANGE);
+    attachInterrupt(stepSensor, step_isr, CHANGE);
   }
 
   void reset() {
     targetPosition = -1;
+    Serial.println("Reset");
     direction = DIR_OFF;
   }
 
-  inline uint16_t ticksToPos(int32_t ticks) {
-    if (ticks < 0) ticks += ticksPerRevolution;
-    return (ticks * 360 / ticksPerRevolution) % 360;
+  inline uint16_t calcPos() {
+    return (stepCount * 360 / STEPS_PER_REVOLUTION + tickCount * (360 / STEPS_PER_REVOLUTION) / ticksPerStep + 360)%360;
   }
   
   inline void handleEncA() {
     enca_val = digitalRead(enca);
-    if (enca_val != encb_val) tickCount = (tickCount-1+ticksPerRevolution)%ticksPerRevolution;
-    else tickCount = (tickCount+1)%ticksPerRevolution;
+    if (enca_val != encb_val) {
+      tickCount++;
+      tickDir = DIR_CCW;
+    }
+    else {
+      tickCount--;
+      tickDir = DIR_CW;
+    }
   }
 
   inline void handleEncB() {
     encb_val = digitalRead(encb);
-    if (enca_val == encb_val) tickCount = (tickCount-1+ticksPerRevolution)%ticksPerRevolution;
-    else tickCount = (tickCount+1)%ticksPerRevolution;
+    if (enca_val == encb_val) {
+      tickCount++;
+      tickDir = DIR_CCW;
+    }
+    else {
+      tickCount--;
+      tickDir = DIR_CW;
+    }
+  }
+
+  inline void handleStep() {
+    if (abs(tickCount) > ticksPerStep * 2) {
+      if (tickCount > 0) stepCount = 0;
+      else stepCount = STEPS_PER_REVOLUTION-3;
+      tickCount = 0;
+      if (state == DISK_HOMING) home++;
+    }
+    else {
+      if (tickDir == DIR_CCW) stepCount++;
+      else if (tickDir == DIR_CW) stepCount--;
+      lastTickCount = tickCount;
+      tickCount = 0;
+    }
   }
 
   void setState(int s) {

  disk[2].handleEncB();
  sei();
}

Disk disk[3] = {
  Disk(0, DISK0_HOME_SENSOR, DISK0_ENC_A, DISK0_ENC_B, DISK0_MOTOR_A, DISK0_MOTOR_B, DISK0_TICKS_PER_REVOLUTION, &disk0a_isr, &disk0b_isr),
  Disk(1, DISK1_HOME_SENSOR, DISK1_ENC_A, DISK1_ENC_B, DISK1_MOTOR_A, DISK1_MOTOR_B, DISK1_TICKS_PER_REVOLUTION, &disk1a_isr, &disk1b_isr),
  Disk(2, DISK2_HOME_SENSOR, DISK2_ENC_A, DISK2_ENC_B, DISK2_MOTOR_A, DISK2_MOTOR_B, DISK2_TICKS_PER_REVOLUTION, &disk2a_isr, &disk2b_isr)
};

void setGlobalState(int state) {
  if (state == STATE_HOMING) {
    global_state = STATE_BLOCKED; // Don't output anything before we start homing
    // Move out of home zone for a bit to make sure we re-trigger the sensors
    for (int i=0;i<3;i++) disk[i].setTargetPosition(-1, DIR_CCW);
    homingIdx = timer.after(2000, startHoming, NULL);
  }
  else {
    global_state = state;
  }
}

void startHoming(void *context) {
  global_state = STATE_HOMING;
  for (int i=0;i<3;i++) disk[i].setState(Disk::DISK_HOMING);
  homingIdx = -1;
}

void manageDiskGame() {

  disk[1].handleEncB();
  sei();
}
void disk1step_isr() {
  cli();
  disk[1].handleStep();
  sei();
}
void disk2a_isr() {
  cli();
  disk[2].handleEncA();
