Timing optimised

Gaugecontroller_no_VFD/Gaugecontroller_no_VFD.ino

@@ -1,4 +1,5 @@
 #include <Arduino.h>
+#include <avr/interrupt.h>
 #include <math.h>
 #include <stdlib.h>
 #include <FastLED.h>
@@ -13,6 +14,9 @@ static const uint8_t INDICATOR_LED_DATA_PIN = 36;
 static const uint8_t BREATHE_FRAME_MS = 16;
 static const uint8_t LED_SHOW_MIN_INTERVAL_MS = 16;
 static const uint8_t RX_LINE_MAX = 120;
+static const uint16_t STEPPER_TIMER_HZ = 20000;
+static const uint8_t STEPPER_TIMER_PRESCALER = 8;
+static const uint32_t STEPPER_RATE_SCALE = 65536UL;
 
 // For now, command and debug traffic share the same serial port.
 #define CMD_PORT Serial1
@@ -81,25 +85,44 @@ struct Gauge {
   uint32_t maxSpeed = 4000;
   uint32_t accel = 6000;
   uint32_t homingSpeed = 500;
-  uint32_t homingStepIntervalUs = 2000;
 
-  float stepAccumulator = 0.0f;
   unsigned long lastUpdateMicros = 0;
 
   bool enabled = true;
   bool homed = false;
-  bool dirKnown = false;
-  bool dirForward = true;
 
   HomingState homingState = HS_IDLE;
   long homingStepsRemaining = 0;
-  unsigned long homingLastStepMicros = 0;
+  long homingStartPos = 0;
   unsigned long homingStateStartMs = 0;
 
   bool sweepEnabled = false;
   bool sweepTowardMax = true;
 };
 
+struct StepperRuntime {
+  volatile uint8_t* stepPort = nullptr;
+  volatile uint8_t* dirPort = nullptr;
+  uint8_t stepMask = 0;
+  uint8_t dirMask = 0;
+  bool stepActiveHigh = true;
+  bool dirInverted = false;
+
+  volatile long currentPos = 0;
+  volatile long targetPos = 0;
+  volatile long minPos = 0;
+  volatile long maxPos = 3780;
+  volatile uint32_t rateQ16 = 0;
+  volatile uint32_t phaseQ16 = 0;
+  volatile int8_t dir = 0;
+  volatile bool enabled = true;
+  volatile bool allowPastMin = false;
+  volatile bool limitToTarget = true;
+  volatile bool pulseActive = false;
+  volatile bool dirKnown = false;
+  volatile bool dirForward = true;
+};
+
 enum LedFx : uint8_t { FX_BLINK = 0, FX_BREATHE = 1, FX_DFLASH = 2 };
 
 struct BlinkState {
@@ -116,6 +139,7 @@ struct BlinkState {
 };
 
 Gauge gauges[GAUGE_COUNT];
+StepperRuntime steppers[GAUGE_COUNT];
 char rxLine[RX_LINE_MAX + 1];
 uint8_t rxLen = 0;
 bool rxOverflowed = false;
@@ -300,10 +324,172 @@ float absf(float x) {
   return (x < 0.0f) ? -x : x;
 }
 
+static inline void writeFastPin(volatile uint8_t* port, uint8_t mask, bool high) {
+  if (high) {
+    *port |= mask;
+  } else {
+    *port &= (uint8_t)~mask;
+  }
+}
+
+static inline void writeStepperOutput(uint8_t id, bool active) {
+  StepperRuntime& s = steppers[id];
+  writeFastPin(s.stepPort, s.stepMask, active == s.stepActiveHigh);
+}
+
+static inline void writeStepperDir(uint8_t id, bool forward) {
+  StepperRuntime& s = steppers[id];
+  writeFastPin(s.dirPort, s.dirMask, forward != s.dirInverted);
+  s.dirKnown = true;
+  s.dirForward = forward;
+}
+
+uint32_t velocityToRateQ16(float velocity) {
+  float speed = absf(velocity);
+  if (speed < 0.5f) return 0;
+  return (uint32_t)(speed * ((float)STEPPER_RATE_SCALE / (float)STEPPER_TIMER_HZ));
+}
+
+void initStepperRuntime(uint8_t id) {
+  StepperRuntime& s = steppers[id];
+  s.stepPort = portOutputRegister(digitalPinToPort(gaugePins[id].stepPin));
+  s.dirPort = portOutputRegister(digitalPinToPort(gaugePins[id].dirPin));
+  s.stepMask = digitalPinToBitMask(gaugePins[id].stepPin);
+  s.dirMask = digitalPinToBitMask(gaugePins[id].dirPin);
+  s.stepActiveHigh = gaugePins[id].stepActiveHigh;
+  s.dirInverted = gaugePins[id].dirInverted;
+  s.currentPos = gauges[id].currentPos;
+  s.targetPos = gauges[id].targetPos;
+  s.minPos = gauges[id].minPos;
+  s.maxPos = gauges[id].maxPos;
+  s.enabled = gauges[id].enabled;
+  writeStepperOutput(id, false);
+}
+
+long readStepperCurrent(uint8_t id) {
+  uint8_t oldSreg = SREG;
+  cli();
+  long pos = steppers[id].currentPos;
+  SREG = oldSreg;
+  return pos;
+}
+
+void setStepperCurrent(uint8_t id, long pos) {
+  uint8_t oldSreg = SREG;
+  cli();
+  steppers[id].currentPos = pos;
+  steppers[id].phaseQ16 = 0;
+  SREG = oldSreg;
+}
+
+void setStepperTarget(uint8_t id, long target) {
+  uint8_t oldSreg = SREG;
+  cli();
+  steppers[id].targetPos = target;
+  SREG = oldSreg;
+}
+
+void setStepperEnabled(uint8_t id, bool enabled) {
+  uint8_t oldSreg = SREG;
+  cli();
+  StepperRuntime& s = steppers[id];
+  s.enabled = enabled;
+  if (!enabled) {
+    s.rateQ16 = 0;
+    s.phaseQ16 = 0;
+  }
+  SREG = oldSreg;
+}
+
+void setStepperLimits(uint8_t id, long minPos, long maxPos) {
+  uint8_t oldSreg = SREG;
+  cli();
+  steppers[id].minPos = minPos;
+  steppers[id].maxPos = maxPos;
+  SREG = oldSreg;
+}
+
+void setStepperCommand(uint8_t id, float velocity, bool allowPastMin, bool limitToTarget) {
+  int8_t dir = (velocity > 0.5f) ? 1 : (velocity < -0.5f ? -1 : 0);
+  uint32_t rateQ16 = dir == 0 ? 0 : velocityToRateQ16(velocity);
+
+  uint8_t oldSreg = SREG;
+  cli();
+  StepperRuntime& s = steppers[id];
+  if (s.dir != dir) s.phaseQ16 = 0;
+  s.dir = dir;
+  s.rateQ16 = rateQ16;
+  s.allowPastMin = allowPastMin;
+  s.limitToTarget = limitToTarget;
+  if (rateQ16 == 0) s.phaseQ16 = 0;
+  SREG = oldSreg;
+}
+
+void setupStepperTimer() {
+  uint8_t oldSreg = SREG;
+  cli();
+  TCCR1A = 0;
+  TCCR1B = 0;
+  TCNT1 = 0;
+  OCR1A = (uint16_t)((F_CPU / STEPPER_TIMER_PRESCALER / STEPPER_TIMER_HZ) - 1);
+  TCCR1B |= _BV(WGM12);
+  TCCR1B |= _BV(CS11); // prescaler 8 on ATmega2560
+  TIMSK1 |= _BV(OCIE1A);
+  SREG = oldSreg;
+}
+
+ISR(TIMER1_COMPA_vect) {
+  for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
+    StepperRuntime& s = steppers[i];
+    bool pulseJustEnded = false;
+
+    if (s.pulseActive) {
+      writeStepperOutput(i, false);
+      s.pulseActive = false;
+      pulseJustEnded = true;
+    }
+
+    if (!s.enabled || s.rateQ16 == 0 || s.dir == 0) continue;
+
+    bool forward = s.dir > 0;
+    if (!s.dirKnown || s.dirForward != forward) {
+      writeStepperDir(i, forward);
+      continue;
+    }
+
+    s.phaseQ16 += s.rateQ16;
+    if (s.phaseQ16 < STEPPER_RATE_SCALE) continue;
+    if (pulseJustEnded) continue;
+    s.phaseQ16 -= STEPPER_RATE_SCALE;
+
+    if (forward) {
+      if (s.currentPos >= s.maxPos || (s.limitToTarget && s.currentPos >= s.targetPos)) {
+        s.rateQ16 = 0;
+        s.phaseQ16 = 0;
+        continue;
+      }
+      writeStepperOutput(i, true);
+      s.currentPos++;
+    } else {
+      if ((!s.allowPastMin && s.currentPos <= s.minPos) ||
+          (s.limitToTarget && s.currentPos <= s.targetPos)) {
+        s.rateQ16 = 0;
+        s.phaseQ16 = 0;
+        continue;
+      }
+      writeStepperOutput(i, true);
+      s.currentPos--;
+    }
+
+    s.pulseActive = true;
+  }
+}
+
 // Updates the cached enable state and toggles the hardware pin if one exists.
 void setEnable(uint8_t id, bool en) {
   if (id >= GAUGE_COUNT) return;
   gauges[id].enabled = en;
+  setStepperEnabled(id, en);
 
   int8_t pin = gaugePins[id].enablePin;
   if (pin < 0) return;
@@ -312,51 +498,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) {
-  Gauge& g = gauges[id];
-  if (g.dirKnown && g.dirForward == forward) return;
-
-  bool level = gaugePins[id].dirInverted ? !forward : forward;
-  digitalWrite(gaugePins[id].dirPin, level ? HIGH : LOW);
-  g.dirKnown = true;
-  g.dirForward = forward;
-}
-
-// 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];
+  setStepperCommand(id, 0.0f, false, true);
   g.homingState = HS_START;
   g.homed = false;
   g.velocity = 0.0f;
-  g.stepAccumulator = 0.0f;
   g.sweepEnabled = false;
 }
 
@@ -378,26 +527,21 @@ void updateHoming(uint8_t id, unsigned long nowUs, unsigned long nowMs) {
     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;
       g.homingStepsRemaining = g.homingBackoffSteps;
-      g.homingStepIntervalUs = (1000000UL + (g.homingSpeed / 2)) / g.homingSpeed;
+      g.homingStartPos = readStepperCurrent(id);
-      if (g.homingStepIntervalUs == 0) g.homingStepIntervalUs = 1;
+      setStepperCommand(id, -(float)g.homingSpeed, true, false);
-      g.homingLastStepMicros = nowUs;
       g.homingState = HS_BACKING;
       break;
 
     case HS_BACKING: {
-      if ((uint32_t)(nowUs - g.homingLastStepMicros) >= g.homingStepIntervalUs) {
+      g.currentPos = readStepperCurrent(id);
-        g.homingLastStepMicros += g.homingStepIntervalUs;
+      long stepsDone = g.homingStartPos - g.currentPos;
-
+      g.homingStepsRemaining = g.homingBackoffSteps - stepsDone;
-        if (g.homingStepsRemaining > 0) {
+      if (stepsDone >= g.homingBackoffSteps) {
-          doStep(id, -1, true);
+        setStepperCommand(id, 0.0f, false, true);
-          g.homingStepsRemaining--;
-        } else {
         g.homingState = HS_SETTLE;
         g.homingStateStartMs = nowMs;
       }
-      }
       break;
     }
 
@@ -406,7 +550,8 @@ void updateHoming(uint8_t id, unsigned long nowUs, unsigned long nowMs) {
         g.currentPos = 0;
         g.targetPos = 0;
         g.velocity = 0.0f;
-        g.stepAccumulator = 0.0f;
+        setStepperCurrent(id, 0);
+        setStepperTarget(id, 0);
         g.homed = true;
         g.homingState = HS_DONE;
 
@@ -450,11 +595,17 @@ void updateGauge(uint8_t id, unsigned long nowUs, unsigned long nowMs) {
     return;
   }
 
-  if (!g.homed) return;
+  g.currentPos = readStepperCurrent(id);
+
+  if (!g.homed) {
+    setStepperCommand(id, 0.0f, false, true);
+    return;
+  }
 
   if (g.sweepEnabled) {
     updateSweepTarget(id);
   }
+  setStepperTarget(id, g.targetPos);
 
   if (g.lastUpdateMicros == 0) {
     g.lastUpdateMicros = nowUs;
@@ -470,7 +621,7 @@ void updateGauge(uint8_t id, unsigned long nowUs, unsigned long nowMs) {
 
   if (error == 0 && absf(g.velocity) < 0.01f) {
     g.velocity = 0.0f;
-    g.stepAccumulator = 0.0f;
+    setStepperCommand(id, 0.0f, false, true);
     return;
   }
 
@@ -498,46 +649,7 @@ void updateGauge(uint8_t id, unsigned long nowUs, unsigned long nowMs) {
     g.velocity = dir * 5.0f;
   }
 
-  // Integrate fractional steps until there is enough to emit a real pulse.
+  setStepperCommand(id, g.velocity, false, true);
-  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.
@@ -557,6 +669,7 @@ bool parseSet(const char* line) {
   Gauge& g = gauges[(uint8_t)id];
   pos = constrain(pos, g.minPos, g.maxPos);
   g.targetPos = pos;
+  setStepperTarget((uint8_t)id, pos);
   g.sweepEnabled = false;
   sendReply(F("OK"));
   return true;
@@ -639,7 +752,9 @@ bool parseZero(const char* line) {
     g.currentPos = 0;
     g.targetPos = 0;
     g.velocity = 0.0f;
-    g.stepAccumulator = 0.0f;
+    setStepperCommand((uint8_t)id, 0.0f, false, true);
+    setStepperCurrent((uint8_t)id, 0);
+    setStepperTarget((uint8_t)id, 0);
     g.homed = true;
     g.sweepEnabled = false;
     sendReply(F("OK"));
@@ -696,7 +811,7 @@ bool parseSweep(const char* line) {
   if (accel <= 0 || speed <= 0) {
     g.sweepEnabled = false;
     g.velocity = 0.0f;
-    g.stepAccumulator = 0.0f;
+    setStepperCommand((uint8_t)id, 0.0f, false, true);
     sendReply(F("OK"));
     return true;
   }
@@ -706,6 +821,7 @@ bool parseSweep(const char* line) {
   g.sweepEnabled = true;
   g.sweepTowardMax = true;
   g.targetPos = g.maxPos;
+  setStepperTarget((uint8_t)id, g.targetPos);
   sendReply(F("OK"));
   return true;
 }
@@ -716,6 +832,7 @@ bool parseSweep(const char* line) {
 bool parsePosQuery(const char* line) {
   if (strcmp(line, "POS?") == 0) {
     for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
+      gauges[i].currentPos = readStepperCurrent(i);
       CMD_PORT.print(F("POS "));
       CMD_PORT.print(i);
       CMD_PORT.print(' ');
@@ -1043,6 +1160,8 @@ void setup() {
       setEnable(i, true);
     }
 
+    initStepperRuntime(i);
+    setStepperLimits(i, gauges[i].minPos, gauges[i].maxPos);
     gauges[i].lastUpdateMicros = micros();
   }
 
@@ -1078,6 +1197,7 @@ void setup() {
   mainLedController->showLeds(255);
   indicatorLedController->showLeds(255);
 
+  setupStepperTimer();
   requestHomeAll();
 
   DEBUG_PORT.println(F("READY"));