Attempted to fix stuck on in reverse. Cleaned up some excessive serial logging and unused declarations. Reading mode switch on release.

QuadLightsV3.ino

@@ -1,24 +1,22 @@
 #include <Button.h>
 #include <Adafruit_NeoPixel.h>
 
-constexpr bool KEEPALIVE_ENABLE = false;
-constexpr int KEEPALIVE_TIMEOUT = 5000; // ms
+constexpr bool KEEPALIVE_ENABLE = true;
+constexpr int KEEPALIVE_TIMEOUT = 30000; // 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_TAIL_RIGHT_RANGE[] = {0, LEDS_TAIL_SIDE_LENGTH};
-constexpr int LEDS_TAIL_LEFT_RANGE[] = {LEDS_TAIL_LENGTH - LEDS_TAIL_SIDE_LENGTH, LEDS_TAIL_SIDE_LENGTH};
 
 constexpr int LEDS_LEFT_PIN = PA5;
 constexpr int LEDS_RIGHT_PIN = PA7;
 
 /* default configuration right lamp (left from the front)
 RING						          EDGE
-29 30 31 32 33 34 35 37		0  1  2  3  4  5  6  7  8  9  10
-28 27 26 25 24 23 22 21		   20 19 18 17 16 15 14 13 12 11
+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;
@@ -74,11 +72,12 @@ enum Mode {
   MODE_HAZARD,
   MODE_POLICE,
   MODE_RAINBOW,
+  MODE_KITT,
   MODE_COUNT,
 };
 
-// Mode activeMode = MODE_HEAD;
-Mode activeMode = MODE_RAINBOW;
+Mode activeMode = MODE_HEAD;
+// Mode activeMode = MODE_KITT;
 
 uint32_t colorHSV(float hue, float sat = 1.0, float val = 1.0) {
   return Adafruit_NeoPixel::ColorHSV(
@@ -90,17 +89,22 @@ uint32_t colorHSV(float hue, float sat = 1.0, float val = 1.0) {
 
 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_FULL = ledsTail.Color(255, 255, 255, 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);
 
 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;
@@ -108,8 +112,6 @@ unsigned long indicatorRightStart = 0;
 bool indicatorLeftActive = false;
 bool indicatorRightActive = false;
 
-bool policeActive = false;
-
 void setup() {
   // initialize digital pin LED_BUILTIN as an output.
   pinMode(LED_BUILTIN, OUTPUT);
@@ -161,27 +163,39 @@ void animateHeadlights() {
 }
 
 void animateTailLight() {
-  ledsTail.fill(COLOR_RED);
+  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) {
+void animateRightTailSide(uint32_t color, bool withMargin = true) {
   ledsTail.fill(color, 0, LEDS_TAIL_SIDE_LENGTH);
-  ledsTail.fill(COLOR_OFF, LEDS_TAIL_SIDE_LENGTH, LEDS_TAIL_SIDE_MARGIN);
+
+  if (withMargin) {
+    ledsTail.fill(COLOR_OFF, LEDS_TAIL_SIDE_LENGTH, LEDS_TAIL_SIDE_MARGIN);
+  }
 }
 
-void animateLeftTailSide(uint32_t color) {
+void animateLeftTailSide(uint32_t color, bool withMargin = true) {
   ledsTail.fill(color, LEDS_TAIL_LENGTH - LEDS_TAIL_SIDE_LENGTH, LEDS_TAIL_SIDE_LENGTH);
-  ledsTail.fill(COLOR_OFF, LEDS_TAIL_LENGTH - LEDS_TAIL_SIDE_LENGTH -  LEDS_TAIL_SIDE_MARGIN, LEDS_TAIL_SIDE_MARGIN);
+
+  if (withMargin) {
+    ledsTail.fill(COLOR_OFF, LEDS_TAIL_LENGTH - LEDS_TAIL_SIDE_LENGTH -  LEDS_TAIL_SIDE_MARGIN, LEDS_TAIL_SIDE_MARGIN);
+  }
 }
 
 void animateDash() {
-  ledsDash.clear();
+  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;
@@ -220,11 +234,7 @@ void animateIndicator(Direction direction) {
     }
   }
 
-  const int* tailRange = direction == LEFT
-    ? LEDS_TAIL_LEFT_RANGE
-    : LEDS_TAIL_RIGHT_RANGE;
-
-  void (*animateTailSide)(uint32_t) = direction == LEFT
+  void (*animateTailSide)(uint32_t, bool) = direction == LEFT
     ? animateLeftTailSide
     : animateRightTailSide;
 
@@ -235,13 +245,13 @@ void animateIndicator(Direction direction) {
   if (indicatorFrame < sweepEnd) {
     const int colorAmberFaded = colorHSV(10, 1, (float)indicatorFrame / sweepEnd);
 
-    animateTailSide(colorAmberFaded);
+    animateTailSide(colorAmberFaded, true);
     ledsDash.setPixelColor(dashIndex, colorAmberFaded);
   } else if (indicatorFrame < holdEnd) {
-    animateTailSide(COLOR_AMBER);
+    animateTailSide(COLOR_AMBER, true);
     ledsDash.setPixelColor(dashIndex, COLOR_AMBER);
   } else {
-    animateTailSide(COLOR_OFF);
+    animateTailSide(COLOR_OFF, true);
     ledsDash.setPixelColor(dashIndex, COLOR_OFF);
   }
 }
@@ -299,41 +309,135 @@ void animatePolice() {
   }
 
   if (policeFrame < 6) {
-    ledsHeadLeft.fill(COLOR_WHITE_DIM, LEDS_EDGE_LENGTH, LEDS_EDGE_LENGTH + LEDS_RING_LENGTH);
-    ledsHeadRight.fill(COLOR_WHITE, LEDS_EDGE_LENGTH, LEDS_EDGE_LENGTH + LEDS_RING_LENGTH);
+    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_EDGE_LENGTH + LEDS_RING_LENGTH);
-    ledsHeadRight.fill(COLOR_WHITE_DIM, LEDS_EDGE_LENGTH, LEDS_EDGE_LENGTH + LEDS_RING_LENGTH);
+    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 = 5.0;
+constexpr float RAINBOW_DENSITY = 10.0;
 
 void animateRainbow() {
-  for (int ledIndex = 0; ledIndex < LEDS_HEAD_LENGTH; ledIndex++) {
-    const uint32_t pixelColor = colorHSV((animationFrame * RAINBOW_SPEED) + (ledIndex * RAINBOW_DENSITY));
+  // 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 uint32_t pixelColor = ledsHeadLeft.ColorHSV((animationFrame * 1000 * RAINBOW_SPEED) + (ledIndex * 1000 * RAINBOW_DENSITY * 5), 255, 255);
+    const int distFromCenter = abs(ledIndex * 2 - (LEDS_TAIL_LENGTH - 1));
+    const uint32_t pixelColor = colorHSV((stableAnimationFrame * RAINBOW_SPEED) - (distFromCenter * RAINBOW_DENSITY * 2));
 
-    if (ledIndex <= 5) {
-      ledsTail.setPixelColor(ledIndex, pixelColor);
-    } else {
-      ledsTail.setPixelColor(LEDS_TAIL_LENGTH - ledIndex, pixelColor);
-    }
+    ledsTail.setPixelColor(ledIndex, pixelColor);
   }
-  */
 
-  ledsTail.fill(colorHSV(animationFrame * RAINBOW_SPEED));
+  ledsDash.setPixelColor(1, colorHSV(stableAnimationFrame * RAINBOW_SPEED));
+  ledsDash.setPixelColor(0, colorHSV(stableAnimationFrame * RAINBOW_SPEED + 90));
+}
 
-  ledsDash.setPixelColor(1, colorHSV(animationFrame * RAINBOW_SPEED));
-  ledsDash.setPixelColor(0, colorHSV(animationFrame * 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 animate(long now) {
@@ -355,6 +459,15 @@ void animate(long now) {
     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();
   }
@@ -379,11 +492,15 @@ void render() {
 bool isOff = false;
 
 void shutdown() {
-    // digitalWrite(PIN_BUCK, BUCK_OFF);
+  // when STLink or USB prevents full power cut, loop keeps running, only shut down once
+  if (isOff) {
+    return;
+  }
+
   Serial.println("SHUTTING DOWN");
 
   Serial.println("BUCK OFF");
-  digitalWrite(PIN_BUCK, LOW);
+  digitalWrite(PIN_BUCK, BUCK_OFF);
 
   delay(100);
 
@@ -397,6 +514,7 @@ void shutdown() {
 void restart() {
   lastFrame = millis();
   animationFrame = 0;
+  stableAnimationFrame = 0;
   isOff = false;
 
   digitalWrite(PIN_BUCK, BUCK_ON);
@@ -413,14 +531,19 @@ void loop() {
     modePressed = true;
     modePressStart = now;
 
-    activeMode = (Mode)((activeMode + 1) % MODE_COUNT);
-    animationFrame = 0; // ensures animations start cleanly instead of halfway
-
-    Serial.print("BUTTON MODE ");
-    Serial.println(activeMode);
+    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;
   }
 
@@ -437,6 +560,8 @@ void loop() {
     indicatorRightActive = false;
     indicatorRightStart = 0;
 
+    lastButtonPress = now;
+
     Serial.println("BUTTON LEFT");
   }
 
@@ -449,41 +574,47 @@ void loop() {
     indicatorLeftActive = false;
     indicatorLeftStart = 0;
 
+    lastButtonPress = now;
+
     Serial.println("BUTTON RIGHT");
   }
 
-  if (pedalReverse.read() == Button::PRESSED && pedalForward.read() != Button::PRESSED) {
+  // only the pedal counts as activity, forward and reverse inputs can read active while parked
+  if (pedal.read() == Button::PRESSED) {
     lastPedalPress = now;
-    isReversing = true;
 
     if (isOff) {
       restart();
     }
-
-    Serial.println("PEDAL REVERSE");
-  } else {
-    isReversing = false;
   }
 
-  if (pedalForward.read() == Button::PRESSED) {
-    lastPedalPress = now;
+  if (pedal.pressed()) {
+    Serial.println("PEDAL PRESSED");
+  }
 
-    if (isOff) {
-      restart();
-    }
+  if (pedal.released()) {
+    lastPedalRelease = now;
 
-    Serial.println("PEDAL FORWARD");
+    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");
   }
 
   if (!isOff && now - lastFrame >= ANIMATION_INTERVAL) {
     lastFrame += ANIMATION_INTERVAL;
     animationFrame += 1;
+    stableAnimationFrame += 1;
 
     animate(now);
     render();
   }
 
-  if (KEEPALIVE_ENABLE && now - lastPedalPress > KEEPALIVE_TIMEOUT) {
+  if (KEEPALIVE_ENABLE && now - lastPedalPress > KEEPALIVE_TIMEOUT && now - lastButtonPress > KEEPALIVE_TIMEOUT) {
     shutdown();
   }
 }