NanoEls 2-axis version [BUGGY] [DO NOT USE]

NanoElsH2.ino

// https://github.com/kachurovskiy/nanoels

/* Change values in this section to suit your hardware. */

// Define your hardware parameters here. Don't remove the ".0" at the end.
#define ENCODER_STEPS 600.0 // 600 step spindle optical rotary encoder
#define MOTOR_STEPS 200.0 // 200 step stepper motor, no microstepping
#define LEAD_SCREW_HMM 200.0 // 2mm lead screw

#define MOTOR_X_STEPS 200.0 // 200 step stepper motor, no microstepping
#define LEAD_SCREW_X_HMM 41.666 // 1.25mm lead screw with 1:3 reduction. Writing this as 125.0/3.0 doesn't work for some reason.

// Tweak these values to move display picture around to center it in the window.
// Acceptable values 0 to ~10 pixels, anything higher and text will be cropped.
#define DISPLAY_LEFT 4
#define DISPLAY_TOP 4

// Spindle rotary encoder pins. Nano only supports interrupts on D2 and D3.
#define ENC_A 3 // D3
#define ENC_B 2 // D2

// Stepper pulse and acceleration constants.
#define PULSE_MIN_US round(500 * (200.0 / MOTOR_STEPS)) // Microseconds to wait after high pulse, min.
#define PULSE_MAX_US round(2000 * (200.0 / MOTOR_STEPS)) // Microseconds to wait after high pulse, max. Slow start.
#define PULSE_DELTA_US 7 // Microseconds remove from waiting time on every step. Acceleration.
#define STEPPER_MAX_RPM 600 // Stepper loses most of it's torque at speeds higher than that.
#define INVERT_STEPPER true // false for geared connection, true for belt connection
#define INVERT_STEPPER_X false // false for geared connection, true for belt connection

// Pitch shortcut buttons, set to your most used values that should be available within 1 click.
#define F3_PITCH 10 // 0.1mm

// Uncomment to show 0-359 angle of the spindle on the screen bottom line.
// #define SHOW_ANGLE

/* Changing anything below shouldn't be needed for basic use. */

#define LOOP_COUNTER_MAX 1500 // 1500 loops without stepper move to start reading buttons
#define HMMPR_MAX 1000 // 10mm

// Ratios between spindle and stepper.
#define ENCODER_TO_STEPPER_STEP_RATIO MOTOR_STEPS / (LEAD_SCREW_HMM * ENCODER_STEPS)
#define STEPPER_TO_ENCODER_STEP_RATIO LEAD_SCREW_HMM * ENCODER_STEPS / MOTOR_STEPS

// If time between encoder ticks is less than this, direction change is not allowed.
// Effectively this limits direction change to the time when spindle is <20rpm.
#define DIR_CHANGE_DIFF_MS (int) (5 * ENCODER_STEPS / 600)

// Version of the EEPROM storage format, should be changed when non-backward-compatible
// changes are made to the storage logic, resulting in EEPROM wipe on first start.
#define EEPROM_VERSION 1

// To be incremented whenever a measurable improvement is made.
#define SOFTWARE_VERSION 1

// To be changed whenever a different PCB / encoder / stepper / ... design is used.
#define HARDWARE_VERSION 2

#define DIR A0
#define STEP A1

#define DIRX A2
#define STEPX 13

#define LEFT 11
#define ONOFF 10
#define RIGHT A3

#define LEFT_STOP 7
#define RIGHT_STOP 9

#define F1 6
#define F2 5
#define F3 4
#define F4 12
#define F5 8

#define ADDR_EEPROM_VERSION 0 // takes 1 byte
#define ADDR_MODE 1 // takes 1 byte
#define ADDR_HMMPR 2 // takes 2 bytes
#define ADDR_POS 4 // takes 4 bytes
#define ADDR_LEFT_STOP 8 // takes 4 bytes
#define ADDR_RIGHT_STOP 12 // takes 4 bytes
#define ADDR_SPINDLE_POS 16 // takes 4 bytes
#define ADDR_OUT_OF_SYNC 20 // takes 2 bytes
#define ADDR_POSX 24 // takes 4 bytes

#define THREADING_STEP_MIN 6 // Minimum depth of cut for the automatic threading mode

#define MODE_OFF 0
#define MODE_LS 1
#define MODE_THREAD 2

#define THREAD_MIN_HMMPR 10 // 0.1mm pitch is the minimal one considered valid for threading mode
#define THREAD_HMMPR_TO_DEPTH_RATIO 0.65 // multiplying pitch by this to get the thread depth
#define THREAD_DEPTH_TO_SINGLE_DEPTH_RATIO 0.2 // multiplying thread depth by this to get the single threading pass cut depth
#define THREAD_DEPTH_MAX_HMM 30 // 0.3mm maximum single threading depth of cut
#define THREAD_DEPTH_MIN_HMM 10 // 0.1mm minimum single threading depth of cut
#define THREAD_X_BACKSTEP_HMM 100 // Move X axis back 1mm + already cut depth to move to the cut starting point

// Uncomment to print out debug info in Serial.
// #define DEBUG

// Uncomment to run the self-test of the code instead of the actual program.
// #define TEST
#ifdef TEST
  bool mockDigitalPins[20] = {0};
  int mockDigitalPinToggleOnRead = -1;
  int mockDigitalRead(int x) {
    int value = mockDigitalPins[x];
    if (x == mockDigitalPinToggleOnRead) {
      mockDigitalPins[x] ^= 1;
    }
    return value;
  }
  #define DREAD(x) mockDigitalRead(x)
#else
  #include <FastGPIO.h>
  #define DREAD(x) FastGPIO::Pin<x>::isInputHigh()
#endif

#ifndef TEST
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_SSD1306.h>
Adafruit_SSD1306 display(128, 64, &Wire, -1);
#endif

#include <EEPROM.h>

unsigned long buttonTime = 0;
long loopCounter = 0;
int mode = MODE_OFF;
int savedMode = MODE_OFF;
unsigned long onPressMillis = 0;
unsigned long onReleaseMillis = 0;
bool resetOnStartup = false;

int hmmpr = 0; // hundredth of a mm per rotation
int savedHmmpr = 0; // hmmpr saved in EEPROM
int hmmprPrevious = 0;

long pos = 0; // relative position of the stepper motor, in steps
long savedPos = 0; // value saved in EEPROM

long leftStop = 0; // left stop value of pos
long savedLeftStop = 0; // value saved in EEPROM
bool leftStopFlag = true; // prevent toggling the stop while button is pressed.

long rightStop = 0; // right stop value of pos
long savedRightStop = 0; // value saved in EEPROM
bool rightStopFlag = true; // prevent toggling the stop while button is pressed.

volatile unsigned long spindleDeltaTime = 0; // millis() of the previous spindle update
volatile int spindleDeltaPrev = 0; // Previously detected spindle direction
volatile long spindlePos = 0; // Spindle position
long savedSpindlePos = 0; // spindlePos value saved in EEPROM
long spindleLeftStop = 0;
long spindleRightStop = 0;

volatile int spindlePosSync = 0;
int savedSpindlePosSync = 0;

int stepDelayUs = PULSE_MAX_US;
bool stepDelayDirection = true; // To reset stepDelayUs when direction changes.
unsigned long stepStartMs = 0;

// X axis (cross slide)

long posX = 0; // relative position of the stepper motor, in steps
long savedPosX = 0; // value saved in EEPROM

int stepXDelayUs = PULSE_MAX_US;
int stepXDelayDirection = -1; // To reset stepDelayUs when direction changes.
unsigned long stepXStartMs = 0;

// 2-axis threading mode

bool isThreading = false;
int threadingDepthTotalHmm = 0;
int threadingDepthLeftHmm = 0;
String threadingMessage = "";

void updateDisplay() {
  #ifndef TEST
  display.clearDisplay();
  display.setTextColor(WHITE);
  display.setCursor(DISPLAY_LEFT, DISPLAY_TOP);
  display.setTextSize(2);
  if (mode == MODE_OFF) {
    display.print("off");
  } else if (mode == MODE_LS) {
    display.print("ON");
  } else if (mode == MODE_THREAD) {
    display.print("THR");
  } else {
    display.print("ERR");
  }
  if (leftStop != 0 && rightStop != 0) {
    display.print(" B");
  } else if (leftStop != 0) {
    display.print(" L");
  } else if (rightStop != 0) {
    display.print(" R");
  }
  if (spindlePosSync) {
    display.print(" SYN");
  } else if (!spindlePosSync && resetOnStartup) {
    display.print(" LTW");
  } else {
    display.print(" ");
    display.print(hmmpr * 1.0 / 100, 2);
  }

  display.setCursor(DISPLAY_LEFT, 20 + DISPLAY_TOP);
  if (mode == MODE_THREAD) {
    if (threadingMessage) {
      display.print(threadingMessage);
    }
  } else {
    float posMm = pos * LEAD_SCREW_HMM / MOTOR_STEPS / 100;
    display.print(posMm, 2);
  
    #ifdef SHOW_ANGLE
    display.print(" ");
    if (abs(posMm) < 100) {
      display.print(round(((spindlePos % (int) ENCODER_STEPS + (int) ENCODER_STEPS) % (int) ENCODER_STEPS) * 360 / ENCODER_STEPS));
    }
    #else
    display.print("mm");
    #endif
  }

  display.setCursor(DISPLAY_LEFT, 40 + DISPLAY_TOP);
  display.print(posXToHmm(posX) / 100, 2);
  display.print("mm");

  display.display();
  #endif
}

float posXToHmm(long value) {
  return value * LEAD_SCREW_X_HMM / MOTOR_X_STEPS;
}

long hmmToPosXDelta(long hmm) {
  return round(abs((float) hmm) * MOTOR_X_STEPS / LEAD_SCREW_X_HMM);
}

void saveInt(int i, int v) {
  // Can't concatenate all in one line due to compiler problems, same throughout the code.
  #ifdef DEBUG
  Serial.print("Saving int at ");
  Serial.print(i);
  Serial.print(" = ");
  Serial.println(v);
  #endif
  EEPROM.write(i, v >> 8 & 0xFF);
  EEPROM.write(i + 1, v & 0xFF);
}
int loadInt(int i) {
  // 255 is the default value when nothing was written before.
  if (EEPROM.read(i) == 255 && EEPROM.read(i + 1) == 255) {
    return 0;
  }
  return (EEPROM.read(i) << 8) + EEPROM.read(i + 1);
}
void saveLong(int i, long v) {
  #ifdef DEBUG
  Serial.print("Saving long at ");
  Serial.print(i);
  Serial.print(" = ");
  Serial.println(v);
  #endif
  EEPROM.write(i, v >> 24 & 0xFF);
  EEPROM.write(i + 1, v >> 16 & 0xFF);
  EEPROM.write(i + 2, v >> 8 & 0xFF);
  EEPROM.write(i + 3, v & 0xFF);
}
long loadLong(int i) {
  long p0 = EEPROM.read(i);
  long p1 = EEPROM.read(i + 1);
  long p2 = EEPROM.read(i + 2);
  long p3 = EEPROM.read(i + 3);
  // 255 is the default value when nothing was written before.
  if (p0 == 255 && p1 == 255 && p2 == 255 && p3 == 255) {
    return 0;
  }
  return (p0 << 24) + (p1 << 16) + (p2 << 8) + p3;
}

// Called on a FALLING interrupt for the spindle rotary encoder pin.
// Keeps track of the spindle position.
void spinEnc() {
  int delta;
  unsigned long timeDiff = millis() - spindleDeltaTime;
  if (timeDiff > DIR_CHANGE_DIFF_MS) {
    delta = DREAD(ENC_B) ? -1 : 1;
    spindleDeltaPrev = delta;
  } else {
    // Spindle is going fast, unlikely to change direction momentarily.
    delta = spindleDeltaPrev;
  }
  spindlePos += delta;
  spindleDeltaTime += timeDiff;

  if (spindlePosSync != 0) {
    spindlePosSync += delta;
    if (spindlePosSync == 0 || spindlePosSync == ENCODER_STEPS) {
      spindlePosSync = 0;
      spindlePos = spindleFromPos(pos);
    }
  }
}

#ifdef TEST
void setup() {
  Serial.begin(9600);
}
#else
void setup() {
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(LEFT, INPUT_PULLUP);
  pinMode(ONOFF, INPUT_PULLUP);
  pinMode(RIGHT, INPUT_PULLUP);

  pinMode(LEFT_STOP, INPUT_PULLUP);
  pinMode(RIGHT_STOP, INPUT_PULLUP);

  pinMode(F1, INPUT_PULLUP);
  pinMode(F2, INPUT_PULLUP);
  pinMode(F3, INPUT_PULLUP);
  pinMode(F4, INPUT_PULLUP);
  pinMode(F5, INPUT_PULLUP);

  pinMode(ENC_A, INPUT_PULLUP);
  pinMode(ENC_B, INPUT_PULLUP);

  pinMode(DIR, OUTPUT);
  pinMode(STEP, OUTPUT);

  pinMode(DIRX, OUTPUT);
  pinMode(STEPX, OUTPUT);

  // Wipe EEPROM if this is the first start after uploading a new build.
  if (EEPROM.read(ADDR_EEPROM_VERSION) != EEPROM_VERSION) {
    for (int i = 0; i < 256; i++) {
      EEPROM.write(i, 255); // 255 is the default value.
    }
    EEPROM.write(ADDR_EEPROM_VERSION, EEPROM_VERSION);
  }

  savedMode = mode = EEPROM.read(ADDR_MODE);
  savedHmmpr = hmmpr = loadInt(ADDR_HMMPR);
  savedPos = pos = loadLong(ADDR_POS);
  savedLeftStop = leftStop = loadLong(ADDR_LEFT_STOP);
  savedRightStop = rightStop = loadLong(ADDR_RIGHT_STOP);
  savedSpindlePos = spindlePos = loadLong(ADDR_SPINDLE_POS);
  savedSpindlePosSync = spindlePosSync = loadInt(ADDR_OUT_OF_SYNC);
  savedPosX = posX = loadLong(ADDR_POSX);
  
  attachInterrupt(digitalPinToInterrupt(ENC_A), spinEnc, FALLING);

  Serial.begin(9600);
  preventMoveOnStart();

  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  updateDisplay();

  Serial.print("NanoEls H");
  Serial.print(HARDWARE_VERSION);
  Serial.print(" V");
  Serial.println(SOFTWARE_VERSION);
}
#endif

void preventMoveOnStart() {
  // Sometimes, especially if ELS was run outside above max RPM before, pos and spindlePos
  // will be out of sync causing immediate stepper movement if isOn. This could be dangerous
  // and surely won't be expected by the operator.
  long newPos = posFromSpindle(spindlePos, true);
  if (mode == MODE_LS && newPos != pos) {
    #ifdef DEBUG
    Serial.println("Losing the thread");
    #endif
    resetOnStartup = true;
    markAsZero();
  } else if (mode == MODE_THREAD) {
    #ifdef DEBUG
    Serial.println("Can't restore into threading");
    #endif
    mode = MODE_OFF;
    markAsZero();
  }
}

// Saves all positions in EEPROM, should be called infrequently to reduce EEPROM wear.
void saveIfChanged() {
  if (savedMode != mode) {
    EEPROM.write(ADDR_MODE, savedMode = mode);
  }
  if (hmmpr != savedHmmpr) {
    saveInt(ADDR_HMMPR, savedHmmpr = hmmpr);
  }
  if (pos != savedPos) {
    saveLong(ADDR_POS, savedPos = pos);
  }
  if (leftStop != savedLeftStop) {
    saveLong(ADDR_LEFT_STOP, savedLeftStop = leftStop);
  }
  if (rightStop != savedRightStop) {
    saveLong(ADDR_RIGHT_STOP, savedRightStop = rightStop);
  }
  if (spindlePos != savedSpindlePos) {
    saveLong(ADDR_SPINDLE_POS, savedSpindlePos = spindlePos);
  }
  if (spindlePosSync != savedSpindlePosSync) {
    saveInt(ADDR_OUT_OF_SYNC, savedSpindlePosSync = spindlePosSync);
  }
  if (posX != savedPosX) {
    saveLong(ADDR_POSX, savedPosX = posX);
  }
}

// Checks if some button was recently pressed. Returns true if not.
bool checkAndMarkButtonTime() {
  if (millis() > buttonTime + 300) {
    buttonTime = millis();
    return true;
  }
  return false;
}

// Loose the thread and mark current physical positions of
// encoder and stepper as a new 0. To be called when hmmpr changes
// or ELS is turned on/off. Without this, changing hmmpr will
// result in stepper rushing across the lathe to the new position.
void markAsZero() {
  noInterrupts();
  if (leftStop != 0) {
    leftStop -= pos;
    if (leftStop == 0) {
      leftStop = 1;
    }
  }
  if (rightStop != 0) {
    rightStop -= pos;
    if (rightStop == 0) {
      rightStop = -1;
    }
  }
  pos = 0;
  spindlePos = 0;
  spindlePosSync = 0;
  posX = 0;
  interrupts();
}

void setHmmpr(int value) {
  hmmpr = value;
  markAsZero();
  updateDisplay();
}

void splashScreen() {
  #ifndef TEST
  display.clearDisplay();
  display.setTextColor(WHITE);
  display.setTextSize(2);
  display.setCursor(DISPLAY_LEFT, 10 + DISPLAY_TOP);
  display.println("NanoEls");
  display.setCursor(DISPLAY_LEFT, 30 + DISPLAY_TOP);
  display.println("H" + String(HARDWARE_VERSION) + " V" + String(SOFTWARE_VERSION));
  display.display();
  delay(2000);
  #endif
}

void reset() {
  resetOnStartup = false;
  leftStop = 0;
  rightStop = 0;
  setHmmpr(0);
  splashScreen();
}

// Called when left/right stop restriction is removed while we're on it.
// Prevents stepper from rushing to a position far away by waiting for the right
// spindle position and starting smoothly.
void setOutOfSync() {
  spindlePosSync = ((spindlePos - spindleFromPos(pos)) % (int) ENCODER_STEPS + (int) ENCODER_STEPS) % (int) ENCODER_STEPS;
  #ifdef DEBUG
  Serial.print("spindlePosSync ");
  Serial.println(spindlePosSync);
  #endif
}

// Check if the - or + buttons are pressed.
void checkPlusMinusButtons() {
  // Speed up scrolling when needed.
  int delta = abs(hmmprPrevious - hmmpr) >= 10 ? 10 : 1;
  bool left = DREAD(LEFT) == LOW;
  bool right = DREAD(RIGHT) == LOW;
  if (left && checkAndMarkButtonTime()) {
    if (hmmpr > -HMMPR_MAX) {
      setHmmpr(max(-HMMPR_MAX, hmmpr - delta));
    }
  } else if (right && checkAndMarkButtonTime()) {
    if (hmmpr < HMMPR_MAX) {
      setHmmpr(min(HMMPR_MAX, hmmpr + delta));
    }
  } else if (!left && !right) {
    hmmprPrevious = hmmpr;
  }
}

// Check if the ON/OFF button is pressed.
void checkOnOffButton() {
  if (DREAD(ONOFF) == LOW) {
    if (onPressMillis == 0) {
      onPressMillis = millis();
      if (onReleaseMillis > 0 && onPressMillis - onReleaseMillis < 1000) {
        mode = (mode + 1) % 3;
      } else if (mode != MODE_OFF) {
        mode = MODE_OFF;
      } else {
        mode = MODE_LS;
      }
      #ifdef DEBUG
      Serial.print("mode ");
      Serial.println(mode);
      #endif
      if (mode == MODE_THREAD) {
        isThreading = false;
        threadingMessage = "";
        threadingDepthTotalHmm = threadingDepthLeftHmm = 0;
      }
      markAsZero();
      updateDisplay();
    } else if (onPressMillis > 0 && millis() - onPressMillis > 6000) {
      onPressMillis = 0;
      reset();
    }
  } else if (onPressMillis) {
    onPressMillis = 0;
    onReleaseMillis = millis();
  }
}

// Check if the left stop button is pressed.
void checkLeftStopButton() {
  if (DREAD(LEFT_STOP) == LOW) {
    if (leftStopFlag) {
      leftStopFlag = false;
      if (leftStop == 0) {
        leftStop = pos == 0 ? 1 : pos;
      } else {
        if (pos == leftStop) {
          // Spindle is most likely out of sync with the stepper because
          // it was spinning while the lead screw was on the stop.
          setOutOfSync();
        }
        leftStop = 0;
      }
    }
  } else {
    leftStopFlag = true;
  }
}

// Check if the right stop button is pressed.
void checkRightStopButton() {
  if (DREAD(RIGHT_STOP) == LOW) {
    if (rightStopFlag) {
      rightStopFlag = false;
      if (rightStop == 0) {
        rightStop = pos == 0 ? -1 : pos;
      } else {
        if (pos == rightStop) {
          // Spindle is most likely out of sync with the stepper because
          // it was spinning while the lead screw was on the stop.
          setOutOfSync();
        }
        rightStop = 0;
      }
    }
  } else {
    rightStopFlag = true;
  }
}

void checkMoveButtons() {
  bool left = DREAD(F1) == LOW;
  bool right = DREAD(F2) == LOW;
  if (!left && !right) {
    return;
  }
  if (mode != MODE_OFF && millis() - spindleDeltaTime < 100) {
    // Spindle is still moving.
    return;
  }
  if (spindlePosSync) {
    // Edge case.
    return;
  }

  int sign = left ? 1 : -1;
  // There was some weird bug when hmmpr == 1 and MODE_LS in the first branch.
  // Carriage moved back-and-forth.
  if (mode == MODE_LS && hmmpr != 0 && abs(hmmpr) != 1) {
    int posDiff = 0;
    do {
      noInterrupts();
      // Move 1mm in the desired direction but stay in the thread by possibly traveling a little more.
      spindlePos += ceil(MOTOR_STEPS * 100.0 / LEAD_SCREW_HMM * STEPPER_TO_ENCODER_STEP_RATIO / ENCODER_STEPS / abs(hmmpr))
        * ENCODER_STEPS
        * sign
        * (hmmpr > 0 ? 1 : -1);
      long newPos = posFromSpindle(spindlePos, true);
      interrupts();
      posDiff = abs(newPos - pos);
      step(newPos > pos, posDiff);
    } while (posDiff != 0 && (left ? DREAD(F1) : DREAD(F2)) == LOW);
  } else if (mode == MODE_OFF || mode == MODE_THREAD && !isThreading) {
    int delta = 0;
    do {
      if (hmmpr == 0 || abs(hmmpr) == 1) {
        delta = 100.0 / LEAD_SCREW_HMM * MOTOR_STEPS;
      } else {
        int increments = MOTOR_STEPS * abs(hmmpr) / LEAD_SCREW_HMM;
        delta = ceil(MOTOR_STEPS * 100.0 / LEAD_SCREW_HMM / increments) * increments;
      }

      // Don't left-right move out of stops.
      if (leftStop != 0 && pos + delta * sign > leftStop) {
        delta = leftStop - pos;
      } else if (rightStop != 0 && pos + delta * sign < rightStop) {
        delta = rightStop - pos;
      }

      // markAsZero() can move leftStop and rightStop by 1.
      if (delta == 1) {
        break;
      }

      step(left, abs(delta));
    } while (delta != 0 && (left ? DREAD(F1) : DREAD(F2)) == LOW);
  }
}

void checkMoveXButtons() {
  if (mode == MODE_THREAD) {
    return;
  }

  bool left = DREAD(F4) == LOW;
  bool right = DREAD(F5) == LOW;
  if (!left && !right) {
    return;
  }
  int sign = left ? 1 : -1;
  int delta = 0;
  do {
    // delta calculation logic to go here.
    delta = 100;
    
    stepX(left, abs(delta));
  } while (delta != 0 && (left ? DREAD(F4) : DREAD(F5)) == LOW);
}

// Called when loop() is not busy running the stepper.
// Should take as little time as possible since it's possible that
// lead screw is ON and stepper has to run in a few milliseconds.
void secondaryWork() {
  checkLeftStopButton();
  checkRightStopButton();
  checkMoveButtons();
  checkMoveXButtons();

  if (loopCounter % 8 == 0) {
    // This takes a really long time.
    updateDisplay();
  }
  if (loopCounter % 137 == 0) {
    saveIfChanged();
  }
}

// Moves the stepper.
long step(bool dir, long steps) {
  // Start slow if direction changed.
  if (stepDelayDirection != dir) {
    stepDelayUs = PULSE_MAX_US;
    stepDelayDirection = dir;
    digitalWrite(DIR, dir ^ INVERT_STEPPER ? HIGH : LOW);
    #ifdef DEBUG
    Serial.print("Direction change");
    #endif
  }

  // Stepper basically has no speed if it was standing for 10ms.
  if (millis() - stepStartMs > 10) {
    stepDelayUs = PULSE_MAX_US;
  }

  for (int i = 0; i < steps; i++) {
    unsigned long t = millis();
    int tDiffMs = stepStartMs == 0 ? 0 : t - stepStartMs;
    // long to int can overflow
    if (tDiffMs < 0 || tDiffMs > PULSE_MAX_US) {
      stepDelayUs = PULSE_MAX_US;
    } else {
      stepDelayUs = min(PULSE_MAX_US, max(PULSE_MIN_US, stepDelayUs - PULSE_DELTA_US + tDiffMs));
    }
    stepStartMs = t;

    digitalWrite(STEP, HIGH);
    // digitalWrite() is slow enough that we don't need to wait in the HIGH position.
    digitalWrite(STEP, LOW);
    // Don't wait during the last step, it will pass by itself before we get back to stepping again.
    // This condition is the reason moving left-right is limited to 600rpm but with ELS On and spindle
    // gradually speeding up, stepper can go to ~1200rpm.
    if (i < steps - 1) {
      delayMicroseconds(stepDelayUs);
    }
  }
  pos += (dir ? 1 : -1) * steps;
}

// Moves the X axis (cross slide) stepper.
long stepX(bool dir, long steps) {
  Serial.println(dir);
  Serial.println(steps);
  // Start slow if direction changed.
  if (stepXDelayDirection != dir) {
    stepXDelayUs = PULSE_MAX_US;
    stepXDelayDirection = dir;
    digitalWrite(DIRX, dir ^ INVERT_STEPPER_X ? HIGH : LOW);
    #ifdef DEBUG
    Serial.print("Direction X change");
    #endif
  }

  // Stepper basically has no speed if it was standing for 10ms.
  if (millis() - stepXStartMs > 10) {
    stepXDelayUs = PULSE_MAX_US;
  }

  for (int i = 0; i < steps; i++) {
    unsigned long t = millis();
    int tDiffMs = stepXStartMs == 0 ? 0 : t - stepXStartMs;
    // long to int can overflow
    if (tDiffMs < 0 || tDiffMs > PULSE_MAX_US) {
      stepXDelayUs = PULSE_MAX_US;
    } else {
      stepXDelayUs = min(PULSE_MAX_US, max(PULSE_MIN_US, stepXDelayUs - PULSE_DELTA_US + tDiffMs));
    }
    stepXStartMs = t;

    digitalWrite(STEPX, HIGH);
    // digitalWrite() is slow enough that we don't need to wait in the HIGH position.
    digitalWrite(STEPX, LOW);
    // Don't wait during the last step, it will pass by itself before we get back to stepping again.
    // This condition is the reason moving left-right is limited to 600rpm but with ELS On and spindle
    // gradually speeding up, stepper can go to ~1200rpm.
    if (i < steps - 1) {
      delayMicroseconds(stepXDelayUs);
    }
  }
  posX += (dir ? -1 : 1) * steps;
  Serial.println(posX);
}

// Calculates stepper position from spindle position.
long posFromSpindle(long s, bool respectStops) {
  long newPos = s * ENCODER_TO_STEPPER_STEP_RATIO * hmmpr;

  // Respect left/right stops.
  if (respectStops) {
    if (rightStop != 0 && newPos < rightStop) {
      newPos = rightStop;
    } else if (leftStop != 0 && newPos > leftStop) {
      newPos = leftStop;
    }
  }

  return newPos;
}

// Calculates spindle position from stepper position.
long spindleFromPos(long p) {
  return p * STEPPER_TO_ENCODER_STEP_RATIO / hmmpr;
}

void checkThreading() {
  if (mode != MODE_THREAD) {
    return;
  }
  long endStop = hmmpr > 0 ? leftStop : rightStop;
  long startStop = hmmpr > 0 ? rightStop : leftStop;
  if (isThreading) {
    if (pos != endStop) {
      // waiting for spindle to turn and stepper to get to the stop.
      threadingMessage = "Turn";
      return;
    } else if (threadingDepthLeftHmm <= 0) {
      if (threadingMessage != "Done") {
        threadingMessage = "Done";
        // Move to starting position.
        stepX(true, abs(posX));
      }
      return;
    }
    int depthDeltaHmm =
      min(THREAD_DEPTH_MAX_HMM,
        min(
          threadingDepthLeftHmm,
          max(THREAD_DEPTH_MIN_HMM, threadingDepthLeftHmm * THREAD_DEPTH_TO_SINGLE_DEPTH_RATIO)));
    int backStepXHmm = THREAD_X_BACKSTEP_HMM + threadingDepthTotalHmm - threadingDepthLeftHmm;
    stepX(true, hmmToPosXDelta(backStepXHmm));
    updateDisplay();
    step(hmmpr < 0, abs(startStop - pos));
    stepX(false, hmmToPosXDelta(backStepXHmm + depthDeltaHmm));
    updateDisplay();
    threadingDepthLeftHmm -= depthDeltaHmm;
    noInterrupts();
    spindlePos =
      spindlePos % (int) ENCODER_STEPS +
      round(spindleFromPos(startStop) / ENCODER_STEPS) * ENCODER_STEPS +
      (hmmpr > 0 ? -1 : 1) * ENCODER_STEPS * 3;
    interrupts();
  } else if (!leftStop) {
    threadingMessage = "Set L";
  } else if (!rightStop) {
    threadingMessage = "Set R";
  } else if (abs(hmmpr) < THREAD_MIN_HMMPR) {
    threadingMessage = "Set pit.";
  } else if (abs(pos - (hmmpr > 0 ? rightStop : leftStop)) > 1) {
    threadingMessage = "To stop";
  } else {
    isThreading = true;
    threadingMessage = "Ready";
    threadingDepthTotalHmm = threadingDepthLeftHmm = abs(hmmpr) * THREAD_HMMPR_TO_DEPTH_RATIO;
    spindlePos = spindleFromPos(startStop);
  }
}

// What is called in the loop() function in when not in test mode.
void nonTestLoop() {
  // Has to be called before reading the spindle value because it can zero it.
  checkOnOffButton();
  checkPlusMinusButtons();
  checkThreading();

  noInterrupts();
  long spindlePosCopy = spindlePos;
  long spindlePosSyncCopy = spindlePosSync;
  interrupts();

  // Move the stepper if needed.
  long newPos = posFromSpindle(spindlePosCopy, true);
  if ((mode == MODE_LS || mode == MODE_THREAD && isThreading) && !spindlePosSyncCopy && newPos != pos) {
    // Move the stepper to the right position.
    step(newPos > pos, abs(newPos - pos));
    if (loopCounter > 0) {
      loopCounter = 0;
    }

    // No long calls on this path or stepper will move unevenly.
    return;
  }

  // Perform auxiliary logic but don't take more than a few milliseconds since
  // stepper just be moving slowly and will need signalling soon.

  // When standing at the stop, ignore full spindle turns.
  // This allows to avoid waiting when spindle direction reverses
  // and reduces the chance of the skipped stepper steps since
  // after a reverse the spindle starts slow.
  if (hmmpr != 0) {
    noInterrupts();
    if (rightStop != 0 && pos == rightStop) {
      long stopSpindlePos = spindleFromPos(rightStop);
      if (hmmpr > 0) {
        if (spindlePos < stopSpindlePos - ENCODER_STEPS) {
          spindlePos += ENCODER_STEPS;
        }
      } else {
        if (spindlePos > stopSpindlePos + ENCODER_STEPS) {
          spindlePos -= ENCODER_STEPS;
        }
      }
    } else if (leftStop != 0 && pos == leftStop) {
      long stopSpindlePos = spindleFromPos(leftStop);
      if (hmmpr > 0) {
        if (spindlePos > stopSpindlePos + ENCODER_STEPS) {
          spindlePos -= ENCODER_STEPS;
        }
      } else {
        if (spindlePos < stopSpindlePos - ENCODER_STEPS) {
          spindlePos += ENCODER_STEPS;
        }
      }
    }
    interrupts();
  }

  loopCounter++;
  if (loopCounter > LOOP_COUNTER_MAX) {
    #ifdef DEBUG
    if (loopCounter % LOOP_COUNTER_MAX == 0) {
      Serial.print("mode ");
      Serial.print(mode);
      Serial.print("pos ");
      Serial.print(pos);
      Serial.print(" hmmpr ");
      Serial.print(hmmpr);
      Serial.print(" leftStop ");
      Serial.print(leftStop);
      Serial.print(" rightStop ");
      Serial.print(rightStop);
      Serial.print(" spindlePos ");
      Serial.println(spindlePos);
      Serial.print(" posFromSpindle ");
      Serial.print(posFromSpindle(spindlePosCopy, false));
      Serial.print(" w/s ");
      Serial.print(posFromSpindle(spindlePosCopy, true));
      Serial.print(" isThreading ");
      Serial.print(isThreading);
      Serial.print(" dTotal ");
      Serial.print(threadingDepthTotalHmm);
      Serial.print(" dLeft ");
      Serial.print(threadingDepthLeftHmm);
      Serial.print(" thrMess ");
      Serial.println(threadingMessage);
    }
    #endif

    // Only check buttons when stepper is surely not running.
    // It might have to run any millisecond though e.g. when leaving the stop
    // so it still should complete within milliseconds.
    secondaryWork();

    // Drop the lost thread warning after some time.
    if (resetOnStartup && loopCounter > 2 * LOOP_COUNTER_MAX) {
      resetOnStartup = false;
    }
  }
}

// In unit testing mode, include test library.
#ifdef TEST
#include <AUnit.h>
#endif

void loop() {
  // In unit testing mode, only run tests.
  #ifdef TEST
  setupEach();
  aunit::TestRunner::run();
  #else
  nonTestLoop();
  #endif
}