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

Changed timing to timer interrupts - scoping data showed inconsistencies

Browse Source
This commit is contained in:
Adrian A. Baumann
2026-05-19 00:50:12 +02:00
parent 1b699352ce
commit e525dba0c4
1 changed files with 216 additions and 117 deletions
Download Patch File Download Diff File Expand all files Collapse all files

329
Gaugecontroller/Gaugecontroller.ino
View File

@@ -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;
long targetPos = 0;
volatile long currentPos = 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;
unsigned long homingLastStepMicros = 0;
volatile long homingStepsRemaining = 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;
g.homingStepsRemaining = g.homingBackoffSteps;
g.homingLastStepMicros = nowUs;
atomicWriteLong(g.homingStepsRemaining, g.homingBackoffSteps);
setTimerStepRate(id, -g.homingSpeed, true);
g.homingState = HS_BACKING;
break;
case HS_BACKING: {
float intervalUs = 1000000.0f / g.homingSpeed;
if ((nowUs - g.homingLastStepMicros) >= intervalUs) {
g.homingLastStepMicros = nowUs;
if (g.homingStepsRemaining > 0) {
doStep(id, -1, true);
g.homingStepsRemaining--;
} else {
case HS_BACKING:
if (atomicReadLong(g.homingStepsRemaining) <= 0) {
stopTimerStepping(id);
g.homingState = HS_SETTLE;
g.homingStateStartMs = nowMs;
}
}
break;
}
case HS_SETTLE:
if (nowMs - g.homingStateStartMs >= 100) {
g.currentPos = 0;
g.targetPos = 0;
stopTimerStepping(id);
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;
if (g.currentPos >= g.maxPos && absf(g.velocity) < 1.0f) {
atomicWriteLong(g.targetPos, g.maxPos);
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;
if (g.currentPos <= g.minPos && absf(g.velocity) < 1.0f) {
atomicWriteLong(g.targetPos, g.minPos);
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.
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;
}
}
setTimerStepRate(id, g.velocity, false);
}
// 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;
g.targetPos = 0;
stopTimerStepping(id);
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");