diff --git a/Gaugecontroller_no_VFD/Gaugecontroller_no_VFD.ino b/Gaugecontroller_no_VFD/Gaugecontroller_no_VFD.ino index 82f247a..87499f5 100644 --- a/Gaugecontroller_no_VFD/Gaugecontroller_no_VFD.ino +++ b/Gaugecontroller_no_VFD/Gaugecontroller_no_VFD.ino @@ -1,4 +1,5 @@ #include +#include #include #include #include @@ -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,25 +527,20 @@ 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; - if (g.homingStepIntervalUs == 0) g.homingStepIntervalUs = 1; - g.homingLastStepMicros = nowUs; + g.homingStartPos = readStepperCurrent(id); + setStepperCommand(id, -(float)g.homingSpeed, true, false); g.homingState = HS_BACKING; break; case HS_BACKING: { - if ((uint32_t)(nowUs - g.homingLastStepMicros) >= g.homingStepIntervalUs) { - g.homingLastStepMicros += g.homingStepIntervalUs; - - if (g.homingStepsRemaining > 0) { - doStep(id, -1, true); - g.homingStepsRemaining--; - } else { - g.homingState = HS_SETTLE; - g.homingStateStartMs = nowMs; - } + g.currentPos = readStepperCurrent(id); + long stepsDone = g.homingStartPos - g.currentPos; + g.homingStepsRemaining = g.homingBackoffSteps - stepsDone; + if (stepsDone >= g.homingBackoffSteps) { + setStepperCommand(id, 0.0f, false, true); + 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. - 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; - } - } + setStepperCommand(id, g.velocity, false, true); } // Parses `SET ` 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"));