     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) {

#error Oops!  Make sure you have 'Teensy 3.1' selected from the 'Tools -> Board' menu.
#endif

#include "./Timer.h"
#include "./Bounce2.h"
#include "DiskInterface.h"
#include "anyware_serial.h"

// Set to 1 to start in debug mode
#define DEBUG_MODE 0

// Bottom disk
const int DISK0_HOME_SENSOR = A0;
const int DISK0_ENC_A = 5;
const int DISK0_ENC_B = 6;
const int DISK0_MOTOR_A = 4;
const int DISK0_MOTOR_B = 3;
const int DISK0_TICKS_PER_REVOLUTION = 30089;

// Middle disk
const int DISK1_HOME_SENSOR = A1;
const int DISK1_ENC_A = 9;
const int DISK1_ENC_B = 10;
const int DISK1_MOTOR_A = 8;
const int DISK1_MOTOR_B = 7;
const int DISK1_TICKS_PER_REVOLUTION = 31348;

// Top disk
const int DISK2_HOME_SENSOR = A2;
const int DISK2_ENC_A = 20;
const int DISK2_ENC_B = 19;
const int DISK2_MOTOR_A = 17;
const int DISK2_MOTOR_B = 18;
const int DISK2_TICKS_PER_REVOLUTION = 31385;

// Globals


    return actualPosition;
  }

  void motorMove() {
    switch (direction) {
    case DIR_CW:
