   disk[0].handleEncB();
   sei();
 }
+void disk0step_isr() {
+  cli();
+  disk[0].handleStep();
+  sei();
+}
 void disk1a_isr() {
   cli();
   disk[1].handleEncA();

#define USER1_STR "1"
#define USER2_STR "2"

Timer timer;
int8_t homingIdx = -1;

struct Disk {
  int id;
  int homeSensor;
  int enca, encb;
  int motorA,motorB;
  uint32_t ticksPerRevolution;

  void (*enca_isr)();
  void (*encb_isr)();

  bool enca_val;
  bool encb_val;

  int magnetValue;
  int lastMagnetValue;

  uint16_t targetPosition;
  uint16_t actualPosition;
  int32_t tickCount;
  bool posChanged;

  Direction direction;

  static const int DISK_READY = 0;
  static const int DISK_HOMING = 1;
  int state;

  int32_t lastExitTicks;
  int32_t lastHitTicks;

  Disk(int diskid, int hs, int enca, int encb, int ma, int mb, uint32_t ticks, void (*enca_isr)(), void (*encb_isr)()) :
    id(diskid), homeSensor(hs), enca(enca), encb(encb), motorA(ma), motorB(mb),
    ticksPerRevolution(ticks),
    enca_isr(enca_isr), encb_isr(encb_isr),
    enca_val(false), encb_val(false),
    magnetValue(0), lastMagnetValue(0),
    actualPosition(0), tickCount(0), posChanged(false),
    direction(DIR_OFF), state(DISK_READY),
    lastExitTicks(0), lastHitTicks(0)
  {
  }


    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 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++;
      tickDir = DIR_CCW;
    }
    else {
      tickCount--;
      tickDir = DIR_CW;
    }
  }

  inline void handleEncB() {
    encb_val = digitalRead(encb);
    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) {
