adebaumann/arduino_gauge_controller
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Timing optimised

Browse Source
This commit is contained in:
Adrian A. Baumann
2026-05-02 21:57:30 +02:00
parent abbbd16b5c
commit aa029587a4
1 changed files with 222 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files

324
Gaugecontroller_no_VFD/Gaugecontroller_no_VFD.ino
View File

@@ -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,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 <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"));