Changed timing to timer interrupts - scoping data showed inconsistencies

Gaugecontroller/Gaugecontroller.ino

@@ -1,7 +1,11 @@
 #include <Arduino.h>
+#include <avr/interrupt.h>
 #include <math.h>
 
 static const uint8_t GAUGE_COUNT = 4;
+static const uint16_t STEPPER_TIMER_HZ = 20000;
+static const int32_t STEP_RATE_SCALE = 65536L;
+static const float MAX_TIMER_STEP_RATE = (float)STEPPER_TIMER_HZ / 2.0f;
 
 // For now, command and debug traffic share the same serial port.
 #define CMD_PORT Serial
@@ -34,8 +38,8 @@ enum HomingState : uint8_t {
 };
 
 struct Gauge {
-  long currentPos = 0;
+  volatile long currentPos = 0;
-  long targetPos = 0;
+  volatile long targetPos = 0;
 
   long minPos = 0;
   long maxPos = 3780;
@@ -46,15 +50,18 @@ struct Gauge {
   float accel = 6000.0f;
   float homingSpeed = 500.0f;
 
-  float stepAccumulator = 0.0f;
   unsigned long lastUpdateMicros = 0;
 
-  bool enabled = true;
+  volatile int32_t timerStepRateQ16 = 0;
+  volatile int32_t timerStepAccumulatorQ16 = 0;
+  volatile bool timerAllowPastMin = false;
+  volatile bool timerPulseActive = false;
+
+  volatile bool enabled = true;
   bool homed = false;
 
   HomingState homingState = HS_IDLE;
-  long homingStepsRemaining = 0;
+  volatile long homingStepsRemaining = 0;
-  unsigned long homingLastStepMicros = 0;
   unsigned long homingStateStartMs = 0;
 
   bool sweepEnabled = false;
@@ -64,6 +71,168 @@ struct Gauge {
 Gauge gauges[GAUGE_COUNT];
 String rxLine;
 
+struct StepperHardware {
+  volatile uint8_t* stepPort = nullptr;
+  volatile uint8_t* dirPort = nullptr;
+  uint8_t stepMask = 0;
+  uint8_t dirMask = 0;
+};
+
+StepperHardware stepperHardware[GAUGE_COUNT];
+
+long atomicReadLong(volatile long& value) {
+  uint8_t sreg = SREG;
+  noInterrupts();
+  long copy = value;
+  SREG = sreg;
+  return copy;
+}
+
+void atomicWriteLong(volatile long& value, long newValue) {
+  uint8_t sreg = SREG;
+  noInterrupts();
+  value = newValue;
+  SREG = sreg;
+}
+
+inline void writePortBit(volatile uint8_t* port, uint8_t mask, bool high) {
+  if (high) {
+    *port |= mask;
+  } else {
+    *port &= ~mask;
+  }
+}
+
+inline void writeDirectionPin(uint8_t id, bool forward) {
+  bool level = gaugePins[id].dirInverted ? !forward : forward;
+  writePortBit(stepperHardware[id].dirPort, stepperHardware[id].dirMask, level);
+}
+
+inline void writeStepPin(uint8_t id, bool active) {
+  bool level = gaugePins[id].stepActiveHigh ? active : !active;
+  writePortBit(stepperHardware[id].stepPort, stepperHardware[id].stepMask, level);
+}
+
+inline void stepDirectionSetupDelay() {
+  __asm__ __volatile__(
+      "nop\n\t""nop\n\t""nop\n\t""nop\n\t"
+      "nop\n\t""nop\n\t""nop\n\t""nop\n\t"
+      "nop\n\t""nop\n\t""nop\n\t""nop\n\t"
+      "nop\n\t""nop\n\t""nop\n\t""nop\n\t");
+}
+
+int32_t stepRateToQ16(float stepsPerSecond) {
+  if (stepsPerSecond > MAX_TIMER_STEP_RATE) stepsPerSecond = MAX_TIMER_STEP_RATE;
+  if (stepsPerSecond < -MAX_TIMER_STEP_RATE) stepsPerSecond = -MAX_TIMER_STEP_RATE;
+  return (int32_t)(stepsPerSecond * ((float)STEP_RATE_SCALE / (float)STEPPER_TIMER_HZ));
+}
+
+void setTimerStepRate(uint8_t id, float stepsPerSecond, bool allowPastMin) {
+  int32_t rateQ16 = stepRateToQ16(stepsPerSecond);
+  uint8_t sreg = SREG;
+  noInterrupts();
+  gauges[id].timerStepRateQ16 = rateQ16;
+  gauges[id].timerAllowPastMin = allowPastMin;
+  if (rateQ16 == 0) {
+    gauges[id].timerStepAccumulatorQ16 = 0;
+  }
+  SREG = sreg;
+}
+
+void stopTimerStepping(uint8_t id) {
+  uint8_t sreg = SREG;
+  noInterrupts();
+  gauges[id].timerStepRateQ16 = 0;
+  gauges[id].timerStepAccumulatorQ16 = 0;
+  gauges[id].timerAllowPastMin = false;
+  SREG = sreg;
+}
+
+void configureStepperHardware(uint8_t id) {
+  stepperHardware[id].stepPort = portOutputRegister(digitalPinToPort(gaugePins[id].stepPin));
+  stepperHardware[id].stepMask = digitalPinToBitMask(gaugePins[id].stepPin);
+  stepperHardware[id].dirPort = portOutputRegister(digitalPinToPort(gaugePins[id].dirPin));
+  stepperHardware[id].dirMask = digitalPinToBitMask(gaugePins[id].dirPin);
+}
+
+void beginStepperTimer() {
+  uint8_t sreg = SREG;
+  noInterrupts();
+  TCCR1A = 0;
+  TCCR1B = 0;
+  TIMSK1 = 0;
+  TCNT1 = 0;
+  OCR1A = (uint16_t)((F_CPU / 8UL / STEPPER_TIMER_HZ) - 1UL);
+  TIFR1 = _BV(OCF1A);
+  TCCR1B |= _BV(WGM12);
+  TCCR1B |= _BV(CS11);
+  TIMSK1 = _BV(OCIE1A);
+  SREG = sreg;
+}
+
+ISR(TIMER1_COMPA_vect) {
+  for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
+    if (gauges[i].timerPulseActive) {
+      writeStepPin(i, false);
+      gauges[i].timerPulseActive = false;
+    }
+  }
+
+  for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
+    Gauge& g = gauges[i];
+    int32_t rateQ16 = g.timerStepRateQ16;
+    if (!g.enabled || rateQ16 == 0) continue;
+
+    int32_t incrementQ16 = rateQ16 > 0 ? rateQ16 : -rateQ16;
+    int32_t accumulatorQ16 = g.timerStepAccumulatorQ16 + incrementQ16;
+    if (accumulatorQ16 < STEP_RATE_SCALE) {
+      g.timerStepAccumulatorQ16 = accumulatorQ16;
+      continue;
+    }
+    g.timerStepAccumulatorQ16 = accumulatorQ16 - STEP_RATE_SCALE;
+
+    if (rateQ16 > 0) {
+      if (g.currentPos >= g.targetPos || g.currentPos >= g.maxPos) {
+        g.timerStepRateQ16 = 0;
+        g.timerStepAccumulatorQ16 = 0;
+        continue;
+      }
+
+      writeDirectionPin(i, true);
+      stepDirectionSetupDelay();
+      writeStepPin(i, true);
+      g.timerPulseActive = true;
+      g.currentPos++;
+    } else {
+      if (!g.timerAllowPastMin && (g.currentPos <= g.targetPos || g.currentPos <= g.minPos)) {
+        g.timerStepRateQ16 = 0;
+        g.timerStepAccumulatorQ16 = 0;
+        continue;
+      }
+
+      if (g.timerAllowPastMin && g.homingStepsRemaining <= 0) {
+        g.timerStepRateQ16 = 0;
+        g.timerStepAccumulatorQ16 = 0;
+        continue;
+      }
+
+      writeDirectionPin(i, false);
+      stepDirectionSetupDelay();
+      writeStepPin(i, true);
+      g.timerPulseActive = true;
+      g.currentPos--;
+
+      if (g.timerAllowPastMin) {
+        g.homingStepsRemaining--;
+        if (g.homingStepsRemaining <= 0) {
+          g.timerStepRateQ16 = 0;
+          g.timerStepAccumulatorQ16 = 0;
+        }
+      }
+    }
+  }
+}
+
 // Sends one-line command replies back over the control port.
 //
 // Serial protocol summary.
@@ -126,46 +295,14 @@ void setEnable(uint8_t id, bool en) {
   digitalWrite(pin, level ? HIGH : LOW);
 }
 
-// Applies the logical direction after accounting for per-gauge inversion.
-void setDir(uint8_t id, bool forward) {
-  bool level = gaugePins[id].dirInverted ? !forward : forward;
-  digitalWrite(gaugePins[id].dirPin, level ? HIGH : LOW);
-}
-
-// Emits one step pulse with the polarity expected by the driver.
-void pulseStep(uint8_t id) {
-  bool active = gaugePins[id].stepActiveHigh;
-  digitalWrite(gaugePins[id].stepPin, active ? HIGH : LOW);
-  delayMicroseconds(4);
-  digitalWrite(gaugePins[id].stepPin, active ? LOW : HIGH);
-}
-
-// Moves the motor by one step if the requested direction is still within allowed travel.
-void doStep(uint8_t id, int dir, bool allowPastMin = false) {
-  Gauge& g = gauges[id];
-  if (!g.enabled) return;
-
-  if (dir > 0) {
-    if (g.currentPos >= g.maxPos) return;
-    setDir(id, true);
-    pulseStep(id);
-    g.currentPos++;
-  } else if (dir < 0) {
-    if (!allowPastMin && g.currentPos <= g.minPos) return;
-    setDir(id, false);
-    pulseStep(id);
-    g.currentPos--;
-  }
-}
-
 // Arms the homing state machine for one gauge and clears any in-flight motion.
 void requestHome(uint8_t id) {
   if (id >= GAUGE_COUNT) return;
   Gauge& g = gauges[id];
+  stopTimerStepping(id);
   g.homingState = HS_START;
   g.homed = false;
   g.velocity = 0.0f;
-  g.stepAccumulator = 0.0f;
   g.sweepEnabled = false;
 }
 
@@ -179,7 +316,6 @@ void requestHomeAll() {
 // Advances the simple homing state machine until the gauge is parked at logical zero.
 void updateHoming(uint8_t id) {
   Gauge& g = gauges[id];
-  unsigned long nowUs = micros();
   unsigned long nowMs = millis();
 
   switch (g.homingState) {
@@ -189,34 +325,25 @@ void updateHoming(uint8_t id) {
     case HS_START:
       // No endstop here; homing just walks back far enough to hit the hard stop.
       g.velocity = 0.0f;
-      g.stepAccumulator = 0.0f;
+      atomicWriteLong(g.homingStepsRemaining, g.homingBackoffSteps);
-      g.homingStepsRemaining = g.homingBackoffSteps;
+      setTimerStepRate(id, -g.homingSpeed, true);
-      g.homingLastStepMicros = nowUs;
       g.homingState = HS_BACKING;
       break;
 
-    case HS_BACKING: {
+    case HS_BACKING:
-      float intervalUs = 1000000.0f / g.homingSpeed;
+      if (atomicReadLong(g.homingStepsRemaining) <= 0) {
-      if ((nowUs - g.homingLastStepMicros) >= intervalUs) {
+        stopTimerStepping(id);
-        g.homingLastStepMicros = nowUs;
-
-        if (g.homingStepsRemaining > 0) {
-          doStep(id, -1, true);
-          g.homingStepsRemaining--;
-        } else {
         g.homingState = HS_SETTLE;
         g.homingStateStartMs = nowMs;
       }
-      }
       break;
-    }
 
     case HS_SETTLE:
       if (nowMs - g.homingStateStartMs >= 100) {
-        g.currentPos = 0;
+        stopTimerStepping(id);
-        g.targetPos = 0;
+        atomicWriteLong(g.currentPos, 0);
+        atomicWriteLong(g.targetPos, 0);
         g.velocity = 0.0f;
-        g.stepAccumulator = 0.0f;
         g.homed = true;
         g.homingState = HS_DONE;
 
@@ -236,17 +363,18 @@ void updateSweepTarget(uint8_t id) {
   Gauge& g = gauges[id];
   if (!g.sweepEnabled || !g.homed || g.homingState != HS_IDLE) return;
 
+  long currentPos = atomicReadLong(g.currentPos);
   if (g.sweepTowardMax) {
-    g.targetPos = g.maxPos;
+    atomicWriteLong(g.targetPos, g.maxPos);
-    if (g.currentPos >= g.maxPos && absf(g.velocity) < 1.0f) {
+    if (currentPos >= g.maxPos && absf(g.velocity) < 1.0f) {
       g.sweepTowardMax = false;
-      g.targetPos = g.minPos;
+      atomicWriteLong(g.targetPos, g.minPos);
     }
   } else {
-    g.targetPos = g.minPos;
+    atomicWriteLong(g.targetPos, g.minPos);
-    if (g.currentPos <= g.minPos && absf(g.velocity) < 1.0f) {
+    if (currentPos <= g.minPos && absf(g.velocity) < 1.0f) {
       g.sweepTowardMax = true;
-      g.targetPos = g.maxPos;
+      atomicWriteLong(g.targetPos, g.maxPos);
     }
   }
 }
@@ -260,7 +388,10 @@ void updateGauge(uint8_t id) {
     return;
   }
 
-  if (!g.homed) return;
+  if (!g.homed) {
+    stopTimerStepping(id);
+    return;
+  }
 
   if (g.sweepEnabled) {
     updateSweepTarget(id);
@@ -275,13 +406,18 @@ void updateGauge(uint8_t id) {
   float dt = (now - g.lastUpdateMicros) / 1000000.0f;
   g.lastUpdateMicros = now;
 
-  if (dt <= 0.0f || dt > 0.1f) return;
+  if (dt <= 0.0f || dt > 0.1f) {
+    stopTimerStepping(id);
+    return;
+  }
 
-  long error = g.targetPos - g.currentPos;
+  long currentPos = atomicReadLong(g.currentPos);
+  long targetPos = atomicReadLong(g.targetPos);
+  long error = targetPos - currentPos;
 
   if (error == 0 && absf(g.velocity) < 0.01f) {
     g.velocity = 0.0f;
-    g.stepAccumulator = 0.0f;
+    stopTimerStepping(id);
     return;
   }
 
@@ -307,46 +443,7 @@ void updateGauge(uint8_t id) {
     g.velocity = dir * 5.0f;
   }
 
-  // Integrate fractional steps until there is enough to emit a real pulse.
+  setTimerStepRate(id, g.velocity, false);
-  g.stepAccumulator += g.velocity * dt;
-
-  while (g.stepAccumulator >= 1.0f) {
-    if (g.currentPos == g.targetPos) {
-      g.stepAccumulator = 0.0f;
-      g.velocity = 0.0f;
-      break;
-    }
-
-    doStep(id, +1, false);
-    g.stepAccumulator -= 1.0f;
-
-    if (g.currentPos >= g.maxPos) {
-      g.currentPos = g.maxPos;
-      g.targetPos = g.maxPos;
-      g.velocity = 0.0f;
-      g.stepAccumulator = 0.0f;
-      break;
-    }
-  }
-
-  while (g.stepAccumulator <= -1.0f) {
-    if (g.currentPos == g.targetPos) {
-      g.stepAccumulator = 0.0f;
-      g.velocity = 0.0f;
-      break;
-    }
-
-    doStep(id, -1, false);
-    g.stepAccumulator += 1.0f;
-
-    if (g.currentPos <= g.minPos) {
-      g.currentPos = g.minPos;
-      g.targetPos = g.minPos;
-      g.velocity = 0.0f;
-      g.stepAccumulator = 0.0f;
-      break;
-    }
-  }
 }
 
 // Parses `SET <id> <pos>` and updates the target position.
@@ -362,7 +459,7 @@ bool parseSet(const String& line) {
 
     Gauge& g = gauges[id];
     pos = constrain(pos, g.minPos, g.maxPos);
-    g.targetPos = pos;
+    atomicWriteLong(g.targetPos, pos);
     g.sweepEnabled = false;
     sendReply("OK");
     return true;
@@ -448,10 +545,10 @@ bool parseZero(const String& line) {
     }
 
     Gauge& g = gauges[id];
-    g.currentPos = 0;
+    stopTimerStepping(id);
-    g.targetPos = 0;
+    atomicWriteLong(g.currentPos, 0);
+    atomicWriteLong(g.targetPos, 0);
     g.velocity = 0.0f;
-    g.stepAccumulator = 0.0f;
     g.homed = true;
     g.sweepEnabled = false;
     sendReply("OK");
@@ -508,7 +605,7 @@ bool parseSweep(const String& line) {
   if (accel <= 0.0f || speed <= 0.0f) {
     g.sweepEnabled = false;
     g.velocity = 0.0f;
-    g.stepAccumulator = 0.0f;
+    stopTimerStepping(id);
     sendReply("OK");
     return true;
   }
@@ -517,7 +614,7 @@ bool parseSweep(const String& line) {
   g.maxSpeed = speed;
   g.sweepEnabled = true;
   g.sweepTowardMax = true;
-  g.targetPos = g.maxPos;
+  atomicWriteLong(g.targetPos, g.maxPos);
   sendReply("OK");
   return true;
 }
@@ -531,9 +628,9 @@ bool parsePosQuery(const String& line) {
       CMD_PORT.print("POS ");
       CMD_PORT.print(i);
       CMD_PORT.print(' ');
-      CMD_PORT.print(gauges[i].currentPos);
+      CMD_PORT.print(atomicReadLong(gauges[i].currentPos));
       CMD_PORT.print(' ');
-      CMD_PORT.print(gauges[i].targetPos);
+      CMD_PORT.print(atomicReadLong(gauges[i].targetPos));
       CMD_PORT.print(' ');
       CMD_PORT.print(gauges[i].homed ? 1 : 0);
       CMD_PORT.print(' ');
@@ -623,6 +720,7 @@ void setup() {
   for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
     pinMode(gaugePins[i].dirPin, OUTPUT);
     pinMode(gaugePins[i].stepPin, OUTPUT);
+    configureStepperHardware(i);
 
     digitalWrite(gaugePins[i].dirPin, LOW);
     digitalWrite(gaugePins[i].stepPin, gaugePins[i].stepActiveHigh ? LOW : HIGH);
@@ -635,6 +733,7 @@ void setup() {
     gauges[i].lastUpdateMicros = micros();
   }
 
+  beginStepperTimer();
   requestHomeAll();
 
   DEBUG_PORT.println("READY");