#include #include #include // without USB CDC, Serial falls back to USART1 on PA9/PA10, hijacking the mode and right buttons #ifndef USBD_USE_CDC #error "Set Tools > USB support to CDC (generic 'Serial' supersede U(S)ART)" #endif constexpr bool KEEPALIVE_ENABLE = true; constexpr int KEEPALIVE_TIMEOUT = 45000; // ms constexpr int LEDS_TAIL_PIN = PA6; constexpr int LEDS_TAIL_LENGTH = 10; constexpr int LEDS_TAIL_BRIGHTNESS = 255; constexpr int LEDS_TAIL_SIDE_LENGTH = 2; constexpr int LEDS_TAIL_SIDE_MARGIN = 2; constexpr int LEDS_LEFT_PIN = PA5; constexpr int LEDS_RIGHT_PIN = PA7; /* default configuration right lamp (left from the front) RING EDGE 31 32 33 34 35 36 37 38 0 1 2 3 4 5 6 7 8 9 10 11 30 29 28 27 26 25 24 23 22 21 20 19 18 17 16 15 14 13 12 */ constexpr int LEDS_TOP_LENGTH = 12; constexpr int LEDS_BOTTOM_LENGTH = 11; constexpr int LEDS_RING_LENGTH = 16; constexpr int LEDS_EDGE_LENGTH = LEDS_TOP_LENGTH + LEDS_BOTTOM_LENGTH; constexpr int LEDS_HEAD_LENGTH = LEDS_TOP_LENGTH + LEDS_BOTTOM_LENGTH + LEDS_RING_LENGTH; constexpr int LEDS_HEAD_BRIGHTNESS = 200; constexpr int LEDS_DASH_PIN = PB0; constexpr int LEDS_DASH_LENGTH = 2; constexpr int LEDS_DASH_BRIGHTNESS = 255; constexpr int PIN_ALIVE = PA2; constexpr int PIN_BUCK = PA1; constexpr int PIN_PEDAL = PA0; constexpr int PIN_PEDAL_FORWARD = PA3; constexpr int PIN_PEDAL_REVERSE = PA4; constexpr int PIN_BUTTON_MODE = PA9; constexpr int PIN_BUTTON_LEFT = PA8; constexpr int PIN_BUTTON_RIGHT = PA10; constexpr int PIN_IMU_SCL = PB6; constexpr int PIN_IMU_SDA = PB7; constexpr unsigned long IMU_RETRY_INTERVAL = 250; // ms, after a supply dip the IMU is usually back quickly constexpr unsigned long IMU_RESET_TIME = 110; // ms, the IMU needs ~100ms after a reset before it takes configuration // MPU9250/9255, configured like CrashRecorder but only buffering the accelerometer constexpr uint8_t IMU_ADDRESSES[] = {0x68, 0x69}; // AD0 low, AD0 high constexpr uint8_t IMU_REG_SMPLRT_DIV = 0x19; constexpr uint8_t IMU_REG_CONFIG = 0x1A; constexpr uint8_t IMU_REG_ACCEL_CONFIG = 0x1C; constexpr uint8_t IMU_REG_ACCEL_CONFIG2 = 0x1D; constexpr uint8_t IMU_REG_FIFO_EN = 0x23; constexpr uint8_t IMU_REG_INT_ENABLE = 0x38; constexpr uint8_t IMU_REG_INT_STATUS = 0x3A; constexpr uint8_t IMU_REG_ACCEL_XOUT_H = 0x3B; constexpr uint8_t IMU_REG_USER_CTRL = 0x6A; constexpr uint8_t IMU_REG_PWR_MGMT_1 = 0x6B; constexpr uint8_t IMU_REG_FIFO_COUNT_H = 0x72; constexpr uint8_t IMU_REG_FIFO_R_W = 0x74; constexpr uint8_t IMU_REG_WHO_AM_I = 0x75; constexpr uint8_t IMU_FIFO_EN_ACCEL = 0x08; constexpr uint8_t IMU_USER_CTRL_FIFO_EN = 0x40; constexpr uint8_t IMU_USER_CTRL_FIFO_RST = 0x04; constexpr uint8_t IMU_INT_FIFO_OFLOW = 0x10; constexpr uint8_t IMU_DLPF_184HZ = 0x01; constexpr uint8_t IMU_ACCEL_16G = 0x18; constexpr float ACCEL_LSB_PER_G = 2048; constexpr int IMU_SAMPLE_RATE = 500; // Hz constexpr int FIFO_SIZE = 512; constexpr int FIFO_SAMPLE_SIZE = 6; // accel xyz, big-endian constexpr int FIFO_READ_BATCH = 5; // samples per I2C read, stays within the 32 byte Wire buffer constexpr unsigned long FIFO_INTERVAL = 10; // ms, the fifo holds ~170ms at 500Hz constexpr unsigned long IMU_HEALTH_INTERVAL = 1000; // ms constexpr float IMU_MISALIGNED_DIFFERENCE = 0.5; // g between the fifo stream and a direct read, a shifted stream is ~1.5g off constexpr int IMU_MISALIGNED_CHECKS = 2; // in a row, so a single check during a crash doesn't count /* crash detection, from the CrashRecorder recordings of 2026-10-21 (recordings/, tools/crash_dump.py) speed lost against the direction of travel within 100ms: normal stops <= 0.19 m/s, handling by hand <= 0.25 m/s (parked, so not armed) wall hits 0.58 - 1.07 m/s, driving off a threshold into the doorframe 0.51 m/s the sensor is mounted on its side with +z pointing backward, its offset is removed by the gravity baseline */ constexpr float CRASH_SPEED_LOSS = 0.6; // m/s, only proper hits, 0.35-0.45 also triggered on grazes and lighter bumps constexpr float CRASH_LOG_SPEED_LOSS = 0.2; // m/s, anything above is logged to help tune the threshold constexpr int CRASH_WINDOW = 50; // samples, 100ms constexpr unsigned long CRASH_ARMED_AFTER_RELEASE = 1500; // ms, impacts happen with the pedal still down or just released constexpr unsigned long CRASH_HAZARD_DURATION = 5000; // ms constexpr float GRAVITY_BASELINE_TIME = 2.0; // s, follows gravity leaking into the forward axis on slopes constexpr float STANDARD_GRAVITY = 9.81; // m/s2 // per-sensor calibration from the tilt test of 2026-10-21, keep in sync with tools/crash_dump.py constexpr float ACCEL_OFFSET_X = 0.490; // g, sensor axes constexpr float ACCEL_OFFSET_Y = -0.277; constexpr float ACCEL_OFFSET_Z = -0.004; constexpr float FLAT_FORWARD = -0.060; // g, gravity in the vehicle frame when standing flat constexpr float FLAT_LEFT = -0.019; constexpr float FLAT_UP = 0.996; constexpr unsigned long STARTUP_SWEEP = 500; // ms, the headlight edges sweep on before the normal lights constexpr unsigned long STARTUP_RING_DELAY = 200; // ms from the start, STARTUP_SWEEP waits for the sweep to complete constexpr unsigned long STARTUP_RING_FADE = 300; // ms constexpr unsigned long STARTUP_DURATION = STARTUP_RING_DELAY + STARTUP_RING_FADE > STARTUP_SWEEP ? STARTUP_RING_DELAY + STARTUP_RING_FADE : STARTUP_SWEEP; // the top and bottom edge rows start side by side in the same corner, so the first LEDs light up as a bright pair // they start dimmer, the first at STARTUP_CORNER_LEVEL rising to full over STARTUP_CORNER_LEDS, all full when the sweep completes constexpr int STARTUP_CORNER_LEDS = 4; constexpr float STARTUP_CORNER_LEVEL = 0; // scrambled lights while tilted, an easter egg constexpr float SCRAMBLE_TILT = 20; // degrees from flat to start constexpr float SCRAMBLE_TILT_END = 8; // degrees, lower so it doesn't flicker around the threshold constexpr unsigned long SCRAMBLE_HOLD = 300; // ms above SCRAMBLE_TILT before it starts constexpr float TILT_FILTER_TIME = 1.0; // s, the motor pulling the quad forward reads like tilt but doesn't last constexpr int SCRAMBLE_FRAMES = 4; // between changes, at full frame rate it would blur into white constexpr int SCRAMBLE_CHANGE_PERCENT = 35; // of the pixels changed each time // the dash shows what crash detection sees, for testing without serial, replacing its normal use // first pixel white: debug mode is on // second pixel, by priority: red: IMU problem (read failed, overflow, reconfigured, not found), yellow flash: near miss, // blue: armed and travelling backward (only expected in reverse), green: armed, off: not armed constexpr bool DEBUG_DASH = false; // temporary, for testing by pushing the quad into a wall without the motor: // detects in both directions without the pedal, and disables the keepalive timeout (hold mode to power off) constexpr bool DEBUG_CRASH_WITHOUT_PEDAL = false; // temporary, logs a status line every IMU_HEALTH_INTERVAL to find crash detection dropouts from the log alone constexpr bool DEBUG_IMU_LOG = false; constexpr unsigned long DEBUG_NEAR_MISS_DURATION = 500; // ms constexpr unsigned long DEBUG_IMU_PROBLEM_DURATION = 3000; // ms constexpr int LED_ON = LOW; constexpr int LED_OFF = HIGH; constexpr int PEDAL_ON = HIGH; constexpr int PEDAL_OFF = LOW; constexpr int BUCK_ON = HIGH; constexpr int BUCK_OFF = LOW; constexpr int ANIMATION_FPS = 30; constexpr int ANIMATION_INTERVAL = lround(1000 / ANIMATION_FPS); Button pedal(PIN_PEDAL); Button pedalForward(PIN_PEDAL_FORWARD); Button pedalReverse(PIN_PEDAL_REVERSE); Button buttonMode(PIN_BUTTON_MODE); Button buttonLeft(PIN_BUTTON_LEFT); Button buttonRight(PIN_BUTTON_RIGHT); Adafruit_NeoPixel ledsTail(LEDS_TAIL_LENGTH, LEDS_TAIL_PIN, NEO_GRBW + NEO_KHZ800); Adafruit_NeoPixel ledsHeadLeft(LEDS_HEAD_LENGTH, LEDS_LEFT_PIN, NEO_GRBW + NEO_KHZ800); Adafruit_NeoPixel ledsHeadRight(LEDS_HEAD_LENGTH, LEDS_RIGHT_PIN, NEO_GRBW + NEO_KHZ800); Adafruit_NeoPixel ledsDash(LEDS_DASH_LENGTH, LEDS_DASH_PIN, NEO_GRBW + NEO_KHZ800); enum Direction { LEFT, RIGHT }; enum Mode { MODE_HEAD, MODE_POLICE, MODE_RAINBOW, MODE_KITT, MODE_HAZARD, MODE_COUNT, }; Mode activeMode = MODE_HEAD; // polling stops as soon as the IMU fails, every I2C call to a missing IMU blocks the loop for up to 100ms enum ImuState { IMU_MISSING, IMU_STARTING, IMU_RUNNING }; // Mode activeMode = MODE_KITT; uint32_t colorHSV(float hue, float sat = 1.0, float val = 1.0) { return Adafruit_NeoPixel::ColorHSV( (uint16_t)(hue / 360.0 * 65535), (uint8_t)(sat * 255), (uint8_t)(val * 255) ); } const uint32_t COLOR_OFF = ledsTail.Color(0, 0, 0, 0); const uint32_t COLOR_WHITE = ledsTail.Color(0, 0, 0, 255); const uint32_t COLOR_WHITE_DIM = ledsTail.Color(0, 0, 0, 100); const uint32_t COLOR_RED = ledsTail.Color(255, 0, 0, 0); const uint32_t COLOR_AMBER = colorHSV(10); // easier value fade const uint32_t COLOR_POLICE_BLUE = ledsTail.Color(0, 0, 255, 0); const uint32_t COLOR_POLICE_RED = ledsTail.Color(255, 0, 0, 0); const uint32_t COLOR_GREEN = ledsTail.Color(0, 255, 0, 0); const uint32_t COLOR_YELLOW = ledsTail.Color(255, 150, 0, 0); unsigned long animationFrame = 0; unsigned long stableAnimationFrame = 0; unsigned long lastFrame = 0; unsigned long lastPedalPress = millis(); unsigned long lastPedalRelease = 0; unsigned long lastButtonPress = 0; bool isReversing = false; unsigned long indicatorLeftStart = 0; unsigned long indicatorRightStart = 0; bool indicatorLeftActive = false; bool indicatorRightActive = false; ImuState imuState = IMU_MISSING; unsigned long imuStateSince = 0; bool imuMissingReported = false; // only the first failure of an outage is logged uint8_t imuAddress = 0; uint8_t imuWhoAmI = 0; unsigned long lastFifoRead = 0; unsigned long lastImuHealth = 0; unsigned long imuSamples = 0; // since the last health check unsigned long lastImuProblem = 0; // 0 when none yet unsigned long lastNearMiss = 0; int16_t lastFifoSample[3] = {}; // raw accel xyz int imuMisalignedChecks = 0; float debugSpeedChangeMin = 0; // since the last health check float debugSpeedChangeMax = 0; unsigned long startupStart = 0; bool shuttingDown = false; unsigned long shutdownStart = 0; bool travellingBackward = false; // last direction while the pedal was down bool crashHazardActive = false; unsigned long crashHazardStart = 0; float gravityBaseline = 0; bool gravityBaselineReady = false; float tiltForward = 0; // g, filtered gravity in the vehicle frame float tiltLeft = 0; float tiltUp = 0; bool tiltReady = false; bool scrambleActive = false; bool scrambleTilted = false; // above SCRAMBLE_TILT, waiting for SCRAMBLE_HOLD unsigned long scrambleTiltedSince = 0; uint32_t scrambleHeadLeft[LEDS_HEAD_LENGTH]; uint32_t scrambleHeadRight[LEDS_HEAD_LENGTH]; uint32_t scrambleTail[LEDS_TAIL_LENGTH]; uint32_t scrambleDash[LEDS_DASH_LENGTH]; float crashWindow[CRASH_WINDOW] = {}; float crashWindowSum = 0; int crashWindowIndex = 0; float crashEventPeak = 0; // largest speed loss of the current event above CRASH_LOG_SPEED_LOSS bool crashEventTriggered = false; void writeRegister(uint8_t reg, uint8_t value) { Wire.beginTransmission(imuAddress); Wire.write(reg); Wire.write(value); Wire.endTransmission(); } bool readRegisters(uint8_t address, uint8_t reg, uint8_t* buffer, uint8_t length) { Wire.beginTransmission(address); Wire.write(reg); if (Wire.endTransmission(false) != 0 || Wire.requestFrom(address, length) != length) { return false; } for (int i = 0; i < length; i++) { buffer[i] = Wire.read(); } return true; } uint8_t readRegister(uint8_t reg) { uint8_t value = 0; readRegisters(imuAddress, reg, &value, 1); return value; } void reportImuProblem(const char* message) { Serial.println(message); lastImuProblem = max(millis(), 1ul); } void startFifo() { writeRegister(IMU_REG_FIFO_EN, 0); writeRegister(IMU_REG_USER_CTRL, IMU_USER_CTRL_FIFO_RST); readRegister(IMU_REG_INT_STATUS); // clears a stale overflow flag writeRegister(IMU_REG_USER_CTRL, IMU_USER_CTRL_FIFO_EN); writeRegister(IMU_REG_FIFO_EN, IMU_FIFO_EN_ACCEL); } void beginWire() { // setSCL/SDA have to be called before begin() Wire.setSCL(PIN_IMU_SCL); Wire.setSDA(PIN_IMU_SDA); Wire.begin(); Wire.setClock(400000); } // the IMU can reset halfway through a transfer, likely browning out when the motor stalls against a wall, and keep // holding SDA low, which blocks the bus until power is cut. clocking SCL until it lets go and sending a STOP frees it, // restarting Wire also clears the I2C peripheral if it got stuck busy void recoverWire() { constexpr int PULSE = 5; // us, ~100kHz constexpr int MAX_CLOCKS = 16; Wire.end(); pinMode(PIN_IMU_SDA, INPUT_PULLUP); pinMode(PIN_IMU_SCL, OUTPUT_OPEN_DRAIN); digitalWrite(PIN_IMU_SCL, HIGH); delayMicroseconds(PULSE); for (int clock = 0; clock < MAX_CLOCKS && digitalRead(PIN_IMU_SDA) == LOW; clock++) { digitalWrite(PIN_IMU_SCL, LOW); delayMicroseconds(PULSE); digitalWrite(PIN_IMU_SCL, HIGH); delayMicroseconds(PULSE); } // STOP: SDA rising while SCL is high pinMode(PIN_IMU_SDA, OUTPUT_OPEN_DRAIN); digitalWrite(PIN_IMU_SDA, LOW); delayMicroseconds(PULSE); digitalWrite(PIN_IMU_SDA, HIGH); delayMicroseconds(PULSE); beginWire(); } void setImuState(ImuState state, unsigned long now) { imuState = state; imuStateSince = now; } // stops polling until the next reconnect, retrying a failing IMU would only block the loop on I2C timeouts void imuFailed(const char* message, unsigned long now) { if (!imuMissingReported) { reportImuProblem(message); imuMissingReported = true; } setImuState(IMU_MISSING, now); } // first half of connecting, configureImu() finishes after IMU_RESET_TIME without blocking the loop void connectImu(unsigned long now, bool recover = true) { if (recover) { recoverWire(); } imuAddress = 0; for (const uint8_t address : IMU_ADDRESSES) { if (readRegisters(address, IMU_REG_WHO_AM_I, &imuWhoAmI, 1)) { imuAddress = address; break; } } if (imuAddress == 0) { imuFailed("IMU NOT FOUND ON PB6/PB7, CRASH DETECTION DISABLED UNTIL IT IS BACK", now); return; } writeRegister(IMU_REG_PWR_MGMT_1, 0x80); // reset setImuState(IMU_STARTING, now); } void configureImu(unsigned long now) { writeRegister(IMU_REG_PWR_MGMT_1, 0x01); // gyro PLL clock writeRegister(IMU_REG_CONFIG, IMU_DLPF_184HZ); writeRegister(IMU_REG_SMPLRT_DIV, 1000 / IMU_SAMPLE_RATE - 1); writeRegister(IMU_REG_ACCEL_CONFIG, IMU_ACCEL_16G); writeRegister(IMU_REG_ACCEL_CONFIG2, IMU_DLPF_184HZ); writeRegister(IMU_REG_INT_ENABLE, IMU_INT_FIFO_OFLOW); startFifo(); resetCrashMeasure(); imuSamples = 0; lastImuHealth = now; imuMissingReported = false; setImuState(IMU_RUNNING, now); Serial.print("IMU FOUND AT 0x"); Serial.print(imuAddress, HEX); Serial.print(", WHO_AM_I 0x"); Serial.println(imuWhoAmI, HEX); } // the baseline and window follow whatever the stream delivered, start over after the stream was wrong void resetCrashMeasure() { gravityBaselineReady = false; tiltReady = false; crashWindowSum = 0; crashWindowIndex = 0; for (float& value : crashWindow) { value = 0; } } // recovers from the IMU resetting itself, e.g. after a supply dip, which would otherwise disable crash detection silently void checkImuHealth(unsigned long now) { uint8_t accelConfig = 0; uint8_t fifoEnabled = 0; if (!readRegisters(imuAddress, IMU_REG_ACCEL_CONFIG, &accelConfig, 1) || !readRegisters(imuAddress, IMU_REG_FIFO_EN, &fifoEnabled, 1)) { imuFailed("IMU NOT RESPONDING, RECONNECTING", now); return; } const unsigned long samples = imuSamples; if (accelConfig != IMU_ACCEL_16G || fifoEnabled != IMU_FIFO_EN_ACCEL) { imuFailed("IMU LOST ITS CONFIGURATION, RECONNECTING", now); return; } if (imuSamples == 0) { imuFailed("IMU STOPPED SENDING SAMPLES, RECONNECTING", now); return; } imuSamples = 0; float streamDifference = -1; // g, largest axis difference, -1 when the direct read failed // a byte lost without a read error shifts the stream onto another axis, which still looks sane but never shows a crash // a shift by either one or two axes moves the vertical axis (~1.5g with its offset) where it doesn't belong uint8_t direct[6]; if (readRegisters(imuAddress, IMU_REG_ACCEL_XOUT_H, direct, 6)) { bool misaligned = false; for (int axis = 0; axis < 3; axis++) { const int16_t directValue = (int16_t)(direct[axis * 2] << 8 | direct[axis * 2 + 1]); const float difference = abs(directValue - lastFifoSample[axis]) / ACCEL_LSB_PER_G; streamDifference = max(streamDifference, difference); misaligned |= difference > IMU_MISALIGNED_DIFFERENCE; } imuMisalignedChecks = misaligned ? imuMisalignedChecks + 1 : 0; if (imuMisalignedChecks >= IMU_MISALIGNED_CHECKS) { reportImuProblem("IMU FIFO MISALIGNED, RESET"); startFifo(); resetCrashMeasure(); imuMisalignedChecks = 0; } } if (DEBUG_IMU_LOG) { Serial.print("IMU "); Serial.print(samples); Serial.print(" SAMPLES, BASELINE "); Serial.print(gravityBaseline, 2); Serial.print(" G, SPEED CHANGE "); Serial.print(debugSpeedChangeMin, 2); Serial.print(" / "); Serial.print(debugSpeedChangeMax, 2); Serial.print(" M/S, STREAM VS DIRECT "); Serial.print(streamDifference, 2); Serial.println(" G"); } debugSpeedChangeMin = 0; debugSpeedChangeMax = 0; } void triggerCrash(unsigned long now) { crashHazardActive = true; crashHazardStart = now; animationFrame = 0; // start the hazard sweep cleanly Serial.println("CRASH DETECTED, HAZARD ON"); } bool isCrashArmed(unsigned long now) { return DEBUG_CRASH_WITHOUT_PEDAL || pedal.read() == Button::PRESSED || (lastPedalRelease > 0 && now - lastPedalRelease < CRASH_ARMED_AFTER_RELEASE); } void processImuSample(const uint8_t* sample, unsigned long now) { imuSamples++; for (int axis = 0; axis < 3; axis++) { lastFifoSample[axis] = (int16_t)(sample[axis * 2] << 8 | sample[axis * 2 + 1]); } // mounted on its side: +x up, +y left, +z backward const float sampleForward = -(lastFifoSample[2] / ACCEL_LSB_PER_G - ACCEL_OFFSET_Z); const float sampleLeft = lastFifoSample[1] / ACCEL_LSB_PER_G - ACCEL_OFFSET_Y; const float sampleUp = lastFifoSample[0] / ACCEL_LSB_PER_G - ACCEL_OFFSET_X; if (!tiltReady) { tiltForward = sampleForward; tiltLeft = sampleLeft; tiltUp = sampleUp; tiltReady = true; } tiltForward += (sampleForward - tiltForward) / (TILT_FILTER_TIME * IMU_SAMPLE_RATE); tiltLeft += (sampleLeft - tiltLeft) / (TILT_FILTER_TIME * IMU_SAMPLE_RATE); tiltUp += (sampleUp - tiltUp) / (TILT_FILTER_TIME * IMU_SAMPLE_RATE); const int16_t rawZ = lastFifoSample[2]; const float forward = -rawZ / ACCEL_LSB_PER_G; if (!gravityBaselineReady) { gravityBaseline = forward; gravityBaselineReady = true; } gravityBaseline += (forward - gravityBaseline) / (GRAVITY_BASELINE_TIME * IMU_SAMPLE_RATE); crashWindowSum += forward - gravityBaseline - crashWindow[crashWindowIndex]; crashWindow[crashWindowIndex] = forward - gravityBaseline; crashWindowIndex = (crashWindowIndex + 1) % CRASH_WINDOW; // recompute every window so float rounding can't accumulate if (crashWindowIndex == 0) { crashWindowSum = 0; for (const float value : crashWindow) { crashWindowSum += value; } } const float speedChange = crashWindowSum / IMU_SAMPLE_RATE * STANDARD_GRAVITY; // m/s gained forward within the window debugSpeedChangeMin = min(debugSpeedChangeMin, speedChange); debugSpeedChangeMax = max(debugSpeedChangeMax, speedChange); const float speedLoss = DEBUG_CRASH_WITHOUT_PEDAL ? fabsf(speedChange) : travellingBackward ? speedChange : -speedChange; if (isCrashArmed(now) && speedLoss > CRASH_LOG_SPEED_LOSS) { crashEventPeak = max(crashEventPeak, speedLoss); if (speedLoss >= CRASH_SPEED_LOSS && !crashEventTriggered) { crashEventTriggered = true; triggerCrash(now); } } else if (crashEventPeak > 0) { Serial.print(crashEventTriggered ? "CRASH " : "NEAR MISS "); Serial.print(crashEventPeak, 2); Serial.println(travellingBackward ? " M/S LOST REVERSING" : " M/S LOST"); if (!crashEventTriggered) { lastNearMiss = now; } crashEventPeak = 0; crashEventTriggered = false; } } void readFifo(unsigned long now) { uint8_t status = 0; uint8_t countBytes[2] = {0, 0}; if (!readRegisters(imuAddress, IMU_REG_INT_STATUS, &status, 1) || !readRegisters(imuAddress, IMU_REG_FIFO_COUNT_H, countBytes, 2)) { imuFailed("IMU NOT RESPONDING, RECONNECTING", now); return; } const int count = (countBytes[0] & 0x1F) << 8 | countBytes[1]; // once full, new samples overwrite the oldest and the stream loses alignment if (status & IMU_INT_FIFO_OFLOW || count > FIFO_SIZE - FIFO_SAMPLE_SIZE) { reportImuProblem("IMU FIFO OVERFLOW, RESET"); startFifo(); return; } uint8_t buffer[FIFO_SAMPLE_SIZE * FIFO_READ_BATCH]; for (int samples = count / FIFO_SAMPLE_SIZE; samples > 0; samples -= FIFO_READ_BATCH) { const int batch = min(samples, FIFO_READ_BATCH); // a partial read leaves the fifo misaligned, which silently shifts every following sample to another axis if (!readRegisters(imuAddress, IMU_REG_FIFO_R_W, buffer, batch * FIFO_SAMPLE_SIZE)) { imuFailed("IMU READ FAILED, RECONNECTING", now); return; } for (int i = 0; i < batch; i++) { processImuSample(buffer + i * FIFO_SAMPLE_SIZE, now); } } } void setup() { // initialize digital pin LED_BUILTIN as an output. pinMode(LED_BUILTIN, OUTPUT); pinMode(PIN_ALIVE, OUTPUT); pinMode(PIN_BUCK, OUTPUT); pinMode(PIN_PEDAL, INPUT_PULLUP); pinMode(PIN_PEDAL_FORWARD, INPUT_PULLUP); pinMode(PIN_PEDAL_REVERSE, INPUT_PULLUP); pinMode(PIN_BUTTON_MODE, INPUT_PULLUP); pinMode(PIN_BUTTON_LEFT, INPUT_PULLUP); pinMode(PIN_BUTTON_RIGHT, INPUT_PULLUP); // digitalWrite(LED_BUILTIN, LED_ON); digitalWrite(PIN_ALIVE, HIGH); digitalWrite(PIN_BUCK, BUCK_ON); digitalWrite(LED_BUILTIN, LED_OFF); ledsTail.setBrightness(LEDS_TAIL_BRIGHTNESS); ledsHeadLeft.setBrightness(LEDS_HEAD_BRIGHTNESS); ledsHeadRight.setBrightness(LEDS_HEAD_BRIGHTNESS); ledsDash.setBrightness(LEDS_DASH_BRIGHTNESS); ledsTail.begin(); ledsHeadLeft.begin(); ledsHeadRight.begin(); ledsDash.begin(); pedal.begin(); pedalForward.begin(); pedalReverse.begin(); buttonMode.begin(); buttonLeft.begin(); buttonRight.begin(); Serial.begin(9600); randomSeed(micros() ^ HAL_GetUIDw0() ^ HAL_GetUIDw1()); beginWire(); connectImu(millis(), false); ledsTail.clear(); ledsHeadLeft.clear(); ledsHeadRight.clear(); ledsDash.clear(); startupStart = millis(); } void animateHeadlights() { ledsHeadLeft.fill(COLOR_WHITE); ledsHeadRight.fill(COLOR_WHITE); } void animateTailLight() { ledsTail.fill(COLOR_OFF, 0, LEDS_TAIL_LENGTH); ledsTail.fill(COLOR_RED, LEDS_TAIL_SIDE_LENGTH, LEDS_TAIL_LENGTH - LEDS_TAIL_SIDE_LENGTH * 2); } void animateRightTailSide(uint32_t color, bool withMargin = true) { ledsTail.fill(color, 0, LEDS_TAIL_SIDE_LENGTH); if (withMargin) { ledsTail.fill(COLOR_OFF, LEDS_TAIL_SIDE_LENGTH, LEDS_TAIL_SIDE_MARGIN); } } void animateLeftTailSide(uint32_t color, bool withMargin = true) { ledsTail.fill(color, LEDS_TAIL_LENGTH - LEDS_TAIL_SIDE_LENGTH, LEDS_TAIL_SIDE_LENGTH); if (withMargin) { ledsTail.fill(COLOR_OFF, LEDS_TAIL_LENGTH - LEDS_TAIL_SIDE_LENGTH - LEDS_TAIL_SIDE_MARGIN, LEDS_TAIL_SIDE_MARGIN); } } void animateDash() { ledsDash.fill(COLOR_GREEN); } void animateReverse() { ledsTail.fill(COLOR_WHITE); } void animateBrake() { animateLeftTailSide(COLOR_RED, false); animateRightTailSide(COLOR_RED, false); } constexpr int INDICATOR_DURATION = 3200; // total ms constexpr int INDICATOR_HOLD_FRAMES = 15; constexpr int INDICATOR_OFF_FRAMES = 20; constexpr float INDICATOR_SPEED = 2; // shutdown blinks like locking a car with the key fob, on, off and on again, timed like a normal indicator blink // the second blink stays on and fades out with everything else constexpr unsigned long SHUTDOWN_BLINK_ON = (LEDS_TOP_LENGTH + INDICATOR_HOLD_FRAMES) / INDICATOR_SPEED * ANIMATION_INTERVAL; // ms, sweep and hold constexpr unsigned long SHUTDOWN_BLINK_CYCLE = (LEDS_EDGE_LENGTH + INDICATOR_HOLD_FRAMES + INDICATOR_OFF_FRAMES) / INDICATOR_SPEED * ANIMATION_INTERVAL; // ms constexpr unsigned long SHUTDOWN_FADE = 500; // ms constexpr unsigned long SHUTDOWN_FADE_START = SHUTDOWN_BLINK_CYCLE + SHUTDOWN_BLINK_ON; constexpr unsigned long SHUTDOWN_DURATION = SHUTDOWN_FADE_START + SHUTDOWN_FADE; void animateIndicator(Direction direction) { Adafruit_NeoPixel& ledsTarget = direction == LEFT ? ledsHeadLeft : ledsHeadRight; const long indicatorFrame = lround(animationFrame * INDICATOR_SPEED) % (LEDS_EDGE_LENGTH + INDICATOR_HOLD_FRAMES + INDICATOR_OFF_FRAMES); const long sweepEnd = LEDS_TOP_LENGTH; const long holdEnd = LEDS_TOP_LENGTH + INDICATOR_HOLD_FRAMES; for (int ledIndex = LEDS_TOP_LENGTH - 1; ledIndex >= 0; ledIndex--) { if (indicatorFrame < sweepEnd) { // SWEEP bool isLit = ledIndex >= (LEDS_TOP_LENGTH - 1 - indicatorFrame); ledsTarget.setPixelColor(ledIndex, isLit ? COLOR_AMBER : COLOR_OFF); if (ledIndex > 0) { ledsTarget.setPixelColor(LEDS_EDGE_LENGTH - ledIndex, isLit ? COLOR_AMBER : COLOR_OFF); } } else if (indicatorFrame < holdEnd) { // HOLD ledsTarget.setPixelColor(ledIndex, COLOR_AMBER); if (ledIndex > 0) { ledsTarget.setPixelColor(LEDS_EDGE_LENGTH - ledIndex, COLOR_AMBER); } } else { // OFF ledsTarget.fill(COLOR_OFF, 0, LEDS_EDGE_LENGTH); } } void (*animateTailSide)(uint32_t, bool) = direction == LEFT ? animateLeftTailSide : animateRightTailSide; const int dashIndex = direction == LEFT ? 1 : 0; if (indicatorFrame < sweepEnd) { const int colorAmberFaded = colorHSV(10, 1, (float)indicatorFrame / sweepEnd); animateTailSide(colorAmberFaded, true); ledsDash.setPixelColor(dashIndex, colorAmberFaded); } else if (indicatorFrame < holdEnd) { animateTailSide(COLOR_AMBER, true); ledsDash.setPixelColor(dashIndex, COLOR_AMBER); } else { animateTailSide(COLOR_OFF, true); ledsDash.setPixelColor(dashIndex, COLOR_OFF); } } constexpr float POLICE_SPEED = 0.75; enum PoliceStates { POLICE_LEFT, POLICE_RIGHT, POLICE_OFF }; const PoliceStates policeBeaconFrames[12] = { POLICE_LEFT, POLICE_OFF, POLICE_LEFT, POLICE_OFF, POLICE_OFF, POLICE_OFF, POLICE_RIGHT, POLICE_OFF, POLICE_RIGHT, POLICE_OFF, POLICE_OFF, POLICE_OFF, }; void animatePolice() { const long policeFrame = lround(animationFrame * POLICE_SPEED) % 12; if (policeBeaconFrames[policeFrame] == POLICE_LEFT) { ledsHeadLeft.fill(COLOR_POLICE_BLUE, 0, LEDS_EDGE_LENGTH); ledsHeadRight.fill(COLOR_OFF, 0, LEDS_EDGE_LENGTH); // deliberately inverted from edge lights for a more alternating effect animateLeftTailSide(COLOR_OFF); animateRightTailSide(COLOR_POLICE_BLUE); ledsDash.setPixelColor(1, COLOR_POLICE_BLUE); ledsDash.setPixelColor(0, COLOR_OFF); } else if (policeBeaconFrames[policeFrame] == POLICE_RIGHT) { ledsHeadLeft.fill(COLOR_OFF, 0, LEDS_EDGE_LENGTH); ledsHeadRight.fill(COLOR_POLICE_BLUE, 0, LEDS_EDGE_LENGTH); animateLeftTailSide(COLOR_POLICE_BLUE); animateRightTailSide(COLOR_OFF); ledsDash.setPixelColor(1, COLOR_OFF); ledsDash.setPixelColor(0, COLOR_POLICE_BLUE); } else { ledsHeadLeft.fill(COLOR_OFF, 0, LEDS_EDGE_LENGTH); ledsHeadRight.fill(COLOR_OFF, 0, LEDS_EDGE_LENGTH); animateLeftTailSide(COLOR_OFF); animateRightTailSide(COLOR_OFF); ledsDash.setPixelColor(1, COLOR_OFF); ledsDash.setPixelColor(0, COLOR_OFF); } if (policeFrame < 6) { ledsHeadLeft.fill(COLOR_WHITE_DIM, LEDS_EDGE_LENGTH, LEDS_RING_LENGTH); ledsHeadRight.fill(COLOR_WHITE, LEDS_EDGE_LENGTH, LEDS_RING_LENGTH); } else { ledsHeadLeft.fill(COLOR_WHITE, LEDS_EDGE_LENGTH, LEDS_RING_LENGTH); ledsHeadRight.fill(COLOR_WHITE_DIM, LEDS_EDGE_LENGTH, LEDS_RING_LENGTH); } } constexpr float RAINBOW_SPEED = 2.0; constexpr float RAINBOW_DENSITY = 10.0; void animateRainbow() { // EDGES, flowing color for (int ledIndex = LEDS_TOP_LENGTH - 1; ledIndex >= 0; ledIndex--) { const uint32_t pixelColor = colorHSV((stableAnimationFrame * RAINBOW_SPEED) + (ledIndex * RAINBOW_DENSITY)); ledsHeadLeft.setPixelColor(ledIndex, pixelColor); ledsHeadLeft.setPixelColor(LEDS_EDGE_LENGTH - ledIndex, pixelColor); ledsHeadRight.setPixelColor(ledIndex, pixelColor); ledsHeadRight.setPixelColor(LEDS_EDGE_LENGTH - ledIndex, pixelColor); } // RINGS, single color ledsHeadLeft.fill(colorHSV(stableAnimationFrame * RAINBOW_SPEED), LEDS_EDGE_LENGTH, LEDS_RING_LENGTH); ledsHeadRight.fill(colorHSV(stableAnimationFrame * RAINBOW_SPEED), LEDS_EDGE_LENGTH, LEDS_RING_LENGTH); for (int ledIndex = 0; ledIndex < LEDS_TAIL_LENGTH; ledIndex++) { const int distFromCenter = abs(ledIndex * 2 - (LEDS_TAIL_LENGTH - 1)); const uint32_t pixelColor = colorHSV((stableAnimationFrame * RAINBOW_SPEED) - (distFromCenter * RAINBOW_DENSITY * 2)); ledsTail.setPixelColor(ledIndex, pixelColor); } ledsDash.setPixelColor(1, colorHSV(stableAnimationFrame * RAINBOW_SPEED)); ledsDash.setPixelColor(0, colorHSV(stableAnimationFrame * RAINBOW_SPEED + 90)); } constexpr float KITT_SPEED = 0.5; constexpr int KITT_TRAIL = 4; constexpr float KITT_TAIL_SPEED = 3.0f; int kittScanPos(int vPos) { if (vPos < LEDS_RING_LENGTH) { // first ring — reversed so it flows naturally into the edge return LEDS_EDGE_LENGTH + (LEDS_RING_LENGTH - 1 - vPos); } else if (vPos < LEDS_RING_LENGTH + LEDS_EDGE_LENGTH) { // edge forward return vPos - LEDS_RING_LENGTH; } else if (vPos < 2 * LEDS_RING_LENGTH + LEDS_EDGE_LENGTH) { // second ring return LEDS_EDGE_LENGTH + (vPos - LEDS_RING_LENGTH - LEDS_EDGE_LENGTH); } else { // edge backward return LEDS_EDGE_LENGTH - 1 - (vPos - 2 * LEDS_RING_LENGTH - LEDS_EDGE_LENGTH); } } void animateKitt() { const long kittFrame = lround(stableAnimationFrame * KITT_SPEED); const int cycleLength = 2 * LEDS_EDGE_LENGTH + 2 * LEDS_RING_LENGTH; const int vPos = kittFrame % cycleLength; ledsHeadLeft.fill(COLOR_OFF); ledsHeadRight.fill(COLOR_OFF); // trail behind for (int t = 0; t < KITT_TRAIL; t++) { const int pastVPos = ((vPos - t) % cycleLength + cycleLength) % cycleLength; const int scanPos = kittScanPos(pastVPos); const uint32_t color = colorHSV(0, 1, 1.0f - (float)t / KITT_TRAIL); ledsHeadLeft.setPixelColor(scanPos, color); ledsHeadRight.setPixelColor(scanPos, color); } // lead ahead — shorter and dimmer than trail constexpr int KITT_LEAD = 2; for (int t = 1; t <= KITT_LEAD; t++) { const int futureVPos = ((vPos + t) % cycleLength + cycleLength) % cycleLength; const int scanPos = kittScanPos(futureVPos); const uint32_t color = colorHSV(0, 1, 0.3f * (1.0f - (float)t / KITT_LEAD)); ledsHeadLeft.setPixelColor(scanPos, color); ledsHeadRight.setPixelColor(scanPos, color); } const int scanPos = kittScanPos(vPos); // normalized cycle (0–1), sped up float tailCycle = (float)vPos * KITT_TAIL_SPEED / cycleLength; tailCycle -= (int)tailCycle; // wrap to 0–1 // triangle wave: 0 → 1 → 0 (KITT bounce) float tailT = tailCycle * 2.0f; if (tailT > 1.0f) tailT = 2.0f - tailT; // optional smoothing (gives that analog feel) tailT = tailT * tailT * (3.0f - 2.0f * tailT); // smoothstep // map to LED index const int tailPos = lround(tailT * (LEDS_TAIL_LENGTH - 1)); // draw tail with falloff for (int i = 0; i < LEDS_TAIL_LENGTH; i++) { const int dist = abs(i - tailPos); float brightness = 0.0f; if (dist < KITT_TRAIL) { brightness = 1.0f - (float)dist / KITT_TRAIL; } ledsTail.setPixelColor(i, colorHSV(0, 1, brightness)); } ledsDash.fill(COLOR_OFF); float speed = 3.0; float dashCycle = (float)vPos * speed / cycleLength; dashCycle -= (int)dashCycle; // keep it in 0–1 // triangle wave: 0→1→0 over the cycle float t = dashCycle * 2.0; if (t > 1.0) t = 2.0 - t; ledsDash.setPixelColor(0, colorHSV(0, 1, t)); ledsDash.setPixelColor(1, colorHSV(0, 1, 1.0 - t)); } void animateDebugDash(unsigned long now) { uint32_t status = COLOR_OFF; if (lastImuProblem > 0 && now - lastImuProblem < DEBUG_IMU_PROBLEM_DURATION) { status = COLOR_RED; } else if (lastNearMiss > 0 && now - lastNearMiss < DEBUG_NEAR_MISS_DURATION) { status = COLOR_YELLOW; } else if (isCrashArmed(now)) { status = travellingBackward ? COLOR_POLICE_BLUE : COLOR_GREEN; } ledsDash.setPixelColor(0, COLOR_WHITE); ledsDash.setPixelColor(1, status); } // degrees between gravity now and gravity when standing flat, 0 without the IMU float tiltAngle() { if (!tiltReady || imuState != IMU_RUNNING) { return 0; } const float dot = tiltForward * FLAT_FORWARD + tiltLeft * FLAT_LEFT + tiltUp * FLAT_UP; const float lengths = sqrtf(tiltForward * tiltForward + tiltLeft * tiltLeft + tiltUp * tiltUp) * sqrtf(FLAT_FORWARD * FLAT_FORWARD + FLAT_LEFT * FLAT_LEFT + FLAT_UP * FLAT_UP); if (lengths == 0) { return 0; } return degrees(acosf(constrain(dot / lengths, -1.0f, 1.0f))); } void updateScramble(unsigned long now) { const float tilt = tiltAngle(); if (scrambleActive) { if (tilt < SCRAMBLE_TILT_END) { scrambleActive = false; Serial.println("LEVEL AGAIN, UNSCRAMBLED"); } return; } if (tilt <= SCRAMBLE_TILT) { scrambleTilted = false; } else if (!scrambleTilted) { scrambleTilted = true; scrambleTiltedSince = now; } else if (now - scrambleTiltedSince >= SCRAMBLE_HOLD) { scrambleActive = true; scrambleTilted = false; Serial.print("TILTED "); Serial.print(tilt, 0); Serial.println(" DEGREES, SCRAMBLED"); } } // saturated and far apart on the colour wheel, random hues and brightness blended into pastel const float SCRAMBLE_HUES[] = {0, 60, 120, 180, 240, 300}; void scramblePixels(uint32_t* pixels, int length, bool all) { if (all) { for (int i = 0; i < length; i++) { pixels[i] = COLOR_OFF; } } for (int i = 0; i < length; i++) { if (!all && random(100) >= SCRAMBLE_CHANGE_PERCENT) { continue; } // lit pixels always get a dark neighbour, so the colours don't blend const bool neighbourLit = (i > 0 && pixels[i - 1] != COLOR_OFF) || (i < length - 1 && pixels[i + 1] != COLOR_OFF); pixels[i] = neighbourLit || random(2) == 0 ? COLOR_OFF : colorHSV(SCRAMBLE_HUES[random(sizeof(SCRAMBLE_HUES) / sizeof(SCRAMBLE_HUES[0]))]); } } void drawPixels(Adafruit_NeoPixel& leds, const uint32_t* pixels, int length) { for (int i = 0; i < length; i++) { leds.setPixelColor(i, pixels[i]); } } void animateScramble(bool start) { if (start || stableAnimationFrame % SCRAMBLE_FRAMES == 0) { scramblePixels(scrambleHeadLeft, LEDS_HEAD_LENGTH, start); scramblePixels(scrambleHeadRight, LEDS_HEAD_LENGTH, start); scramblePixels(scrambleTail, LEDS_TAIL_LENGTH, start); scramblePixels(scrambleDash, LEDS_DASH_LENGTH, start); } drawPixels(ledsHeadLeft, scrambleHeadLeft, LEDS_HEAD_LENGTH); drawPixels(ledsHeadRight, scrambleHeadRight, LEDS_HEAD_LENGTH); drawPixels(ledsTail, scrambleTail, LEDS_TAIL_LENGTH); drawPixels(ledsDash, scrambleDash, LEDS_DASH_LENGTH); } // LED output looks much brighter than its level at the dark end, a linear fade seems done halfway through float perceived(float level) { return Adafruit_NeoPixel::gamma8(level * 255) / 255.0f; } // edges sweep on in the same direction as the indicator sweep, the leading LED fading in so it moves smoothly, and the // tail light sweeps from the centre outward and the dash fades in over the whole startup, the rings fade in from STARTUP_RING_DELAY void animateStartup(unsigned long now) { const unsigned long elapsed = now - startupStart; const float progress = min(1.0f, (float)elapsed / STARTUP_SWEEP); const float ringLevel = elapsed < STARTUP_RING_DELAY ? 0 : min(1.0f, (float)(elapsed - STARTUP_RING_DELAY) / STARTUP_RING_FADE); const uint32_t ringColor = ledsHeadLeft.Color(0, 0, 0, 255 * perceived(ringLevel)); // the tail and dash span the whole startup, so they finish when the normal lights take over const float durationProgress = min(1.0f, (float)elapsed / STARTUP_DURATION); constexpr int TAIL_PAIRS = (LEDS_TAIL_LENGTH - LEDS_TAIL_SIDE_LENGTH * 2) / 2; ledsTail.clear(); for (int ledIndex = LEDS_TAIL_SIDE_LENGTH; ledIndex < LEDS_TAIL_LENGTH - LEDS_TAIL_SIDE_LENGTH; ledIndex++) { const int pair = abs(ledIndex * 2 - (LEDS_TAIL_LENGTH - 1)) / 2; // 0 in the centre const float level = constrain(durationProgress * TAIL_PAIRS - pair, 0.0f, 1.0f); ledsTail.setPixelColor(ledIndex, ledsTail.Color(255 * perceived(level), 0, 0, 0)); } ledsDash.fill(ledsDash.Color(0, 255 * perceived(durationProgress), 0, 0)); for (Adafruit_NeoPixel* leds : {&ledsHeadLeft, &ledsHeadRight}) { leds->clear(); leds->fill(ringColor, LEDS_EDGE_LENGTH, LEDS_RING_LENGTH); for (int ledIndex = LEDS_TOP_LENGTH - 1; ledIndex >= 0; ledIndex--) { const int position = LEDS_TOP_LENGTH - 1 - ledIndex; // 0 is reached first const float sweepLevel = constrain(progress * LEDS_TOP_LENGTH - position, 0.0f, 1.0f); const float cornerStart = position < STARTUP_CORNER_LEDS ? STARTUP_CORNER_LEVEL + (1 - STARTUP_CORNER_LEVEL) * position / STARTUP_CORNER_LEDS : 1; const float cornerLevel = cornerStart + (1 - cornerStart) * progress; const uint32_t color = leds->Color(0, 0, 0, 255 * perceived(sweepLevel * cornerLevel)); leds->setPixelColor(ledIndex, color); if (ledIndex > 0) { leds->setPixelColor(LEDS_EDGE_LENGTH - ledIndex, color); } } } } void fadeLeds(Adafruit_NeoPixel& leds, float level) { for (uint16_t i = 0; i < leds.numPixels(); i++) { const uint32_t color = leds.getPixelColor(i); leds.setPixelColor(i, leds.Color( (uint8_t)(color >> 16 & 0xFF) * level, (uint8_t)(color >> 8 & 0xFF) * level, (uint8_t)(color & 0xFF) * level, (uint8_t)(color >> 24 & 0xFF) * level )); } } // one amber blink like locking a car, the second stays on and fades out with the other lights // only the indicator areas blink, like the hazard lights, the rings and tail light keep showing as normal void animateShutdown(unsigned long now) { const unsigned long elapsed = now - shutdownStart; const bool indicatorsOn = elapsed < SHUTDOWN_BLINK_ON || elapsed >= SHUTDOWN_BLINK_CYCLE; const uint32_t indicatorColor = indicatorsOn ? COLOR_AMBER : COLOR_OFF; ledsHeadLeft.fill(indicatorColor, 0, LEDS_EDGE_LENGTH); ledsHeadRight.fill(indicatorColor, 0, LEDS_EDGE_LENGTH); animateLeftTailSide(indicatorColor, true); animateRightTailSide(indicatorColor, true); ledsDash.fill(indicatorColor); if (elapsed < SHUTDOWN_FADE_START) { return; } const float level = perceived(1 - min(1.0f, (float)(elapsed - SHUTDOWN_FADE_START) / SHUTDOWN_FADE)); fadeLeds(ledsHeadLeft, level); fadeLeds(ledsHeadRight, level); fadeLeds(ledsTail, level); fadeLeds(ledsDash, level); } void animate(long now) { if ((unsigned long)now - startupStart < STARTUP_DURATION) { animateStartup(now); return; } // always animate headlights, let other modes override animateHeadlights(); animateTailLight(); animateDash(); if (isReversing) { animateReverse(); } if (activeMode == MODE_POLICE) { animatePolice(); } if (activeMode == MODE_HAZARD) { animateIndicator(LEFT); animateIndicator(RIGHT); } if (activeMode == MODE_KITT) { animateKitt(); } // lastPedalRelease is 0 until the first release, prevents brake lights right after boot if (lastPedalRelease > 0 && now - lastPedalRelease < 1500) { animateBrake(); } if (activeMode == MODE_RAINBOW) { animateRainbow(); } // we need the boolean to prevent the indicators from turning on right after boot if (indicatorLeftActive && now - indicatorLeftStart < INDICATOR_DURATION) { animateIndicator(LEFT); } if (indicatorRightActive && now - indicatorRightStart < INDICATOR_DURATION) { animateIndicator(RIGHT); } const bool wasScrambled = scrambleActive; updateScramble(now); // overrides every mode, only the crash hazard goes on top so a rollover still warns if (scrambleActive) { animateScramble(!wasScrambled); } // drawn last so it overrides every mode if (crashHazardActive && now - crashHazardStart < CRASH_HAZARD_DURATION) { animateIndicator(LEFT); animateIndicator(RIGHT); } else { crashHazardActive = false; } if (DEBUG_DASH) { animateDebugDash(now); } if (shuttingDown) { animateShutdown(now); } } // any button dismisses the crash hazard, that press does nothing else bool dismissCrashHazard() { if (!crashHazardActive) { return false; } crashHazardActive = false; Serial.println("CRASH HAZARD DISMISSED"); return true; } void render() { ledsTail.show(); ledsHeadLeft.show(); ledsHeadRight.show(); ledsDash.show(); } bool isOff = false; unsigned long modePressStart = 0; bool modePressed = false; // the lights play the shutdown animation first, powerOff() follows after SHUTDOWN_DURATION void beginShutdown(unsigned long now) { if (isOff || shuttingDown) { return; } shuttingDown = true; shutdownStart = now; modePressed = false; // releasing the long press shouldn't switch mode Serial.println("SHUTTING DOWN"); } // any press during the shutdown animation keeps it on, that press does nothing else bool cancelShutdown() { if (!shuttingDown) { return false; } shuttingDown = false; Serial.println("SHUTDOWN CANCELLED"); return true; } void powerOff() { // when STLink or USB prevents full power cut, loop keeps running, only power off once if (isOff) { return; } shuttingDown = false; Serial.println("BUCK OFF"); digitalWrite(PIN_BUCK, BUCK_OFF); delay(100); Serial.println("SELF OFF, GOODBYE"); digitalWrite(PIN_ALIVE, LOW); isOff = true; } // only used when STLink or USB prevents full power cut void restart() { Serial.println("RESTARTING"); lastFrame = millis(); startupStart = lastFrame; animationFrame = 0; stableAnimationFrame = 0; isOff = false; digitalWrite(PIN_BUCK, BUCK_ON); } bool indicatorCombo = false; // both indicator buttons pressed, until both are released Mode modeBeforeHazard = MODE_HEAD; void toggleHazardMode() { if (activeMode == MODE_HAZARD) { activeMode = modeBeforeHazard; } else { modeBeforeHazard = activeMode; activeMode = MODE_HAZARD; } animationFrame = 0; // ensures animations start cleanly instead of halfway Serial.print("BUTTONS LEFT AND RIGHT, MODE "); Serial.println(activeMode); } void handleIndicatorPress(Direction direction, unsigned long now) { if (isOff) { restart(); } lastButtonPress = now; if (cancelShutdown() || dismissCrashHazard() || indicatorCombo) { return; } // both buttons are never pressed at exactly the same time, so the first one already started its indicator Button& otherButton = direction == LEFT ? buttonRight : buttonLeft; if (otherButton.read() == Button::PRESSED) { indicatorCombo = true; indicatorLeftActive = false; indicatorRightActive = false; toggleHazardMode(); return; } animationFrame = 0; indicatorLeftActive = direction == LEFT; indicatorLeftStart = direction == LEFT ? now : 0; indicatorRightActive = direction == RIGHT; indicatorRightStart = direction == RIGHT ? now : 0; Serial.println(direction == LEFT ? "BUTTON LEFT" : "BUTTON RIGHT"); } // the loop function runs over and over again forever void loop() { const unsigned long now = millis(); if (buttonMode.pressed()) { if (isOff) { // wake only, this press shouldn't switch mode on release or count towards a shutdown hold restart(); } else if (cancelShutdown() || dismissCrashHazard()) { // same for cancelling and dismissing } else { modePressed = true; modePressStart = now; } lastButtonPress = now; } // switch mode on release, so a long press for shutdown doesn't switch mode first if (buttonMode.released()) { if (modePressed && !isOff) { activeMode = (Mode)((activeMode + 1) % MODE_COUNT); animationFrame = 0; // ensures animations start cleanly instead of halfway Serial.print("BUTTON MODE "); Serial.println(activeMode); } modePressed = false; } if (modePressed && now - modePressStart > 4000) { beginShutdown(now); } if (buttonLeft.pressed()) { handleIndicatorPress(LEFT, now); } if (buttonRight.pressed()) { handleIndicatorPress(RIGHT, now); } if (indicatorCombo && buttonLeft.read() != Button::PRESSED && buttonRight.read() != Button::PRESSED) { indicatorCombo = false; } // only the pedal counts as activity, forward and reverse inputs can read active while parked if (pedal.read() == Button::PRESSED) { lastPedalPress = now; cancelShutdown(); if (isOff) { restart(); } } if (pedal.pressed()) { Serial.println("PEDAL PRESSED"); } if (pedal.released()) { lastPedalRelease = now; Serial.println("PEDAL RELEASED"); } const bool wasReversing = isReversing; isReversing = pedalReverse.read() == Button::PRESSED && pedalForward.read() != Button::PRESSED; if (isReversing != wasReversing) { Serial.println(isReversing ? "REVERSE ON" : "REVERSE OFF"); } // parked in reverse both inputs read active, so the direction only counts while driving if (pedal.read() == Button::PRESSED) { travellingBackward = isReversing; } if (!isOff && imuState == IMU_RUNNING && now - lastFifoRead >= FIFO_INTERVAL) { lastFifoRead = now; readFifo(now); } if (!isOff && imuState == IMU_RUNNING && now - lastImuHealth >= IMU_HEALTH_INTERVAL) { lastImuHealth = now; checkImuHealth(now); } if (imuState == IMU_STARTING && now - imuStateSince >= IMU_RESET_TIME) { configureImu(now); } // retry while missing, so a supply dip or reseating a wire doesn't need a reboot if (imuState == IMU_MISSING && now - imuStateSince >= IMU_RETRY_INTERVAL) { connectImu(now); } if (!isOff && now - lastFrame >= ANIMATION_INTERVAL) { lastFrame += ANIMATION_INTERVAL; animationFrame += 1; stableAnimationFrame += 1; animate(now); render(); } if (shuttingDown && now - shutdownStart >= SHUTDOWN_DURATION) { powerOff(); } if (KEEPALIVE_ENABLE && !DEBUG_CRASH_WITHOUT_PEDAL && now - lastPedalPress > KEEPALIVE_TIMEOUT && now - lastButtonPress > KEEPALIVE_TIMEOUT) { beginShutdown(now); } }