Bugfix: proper pulse calculation

thedropbears · Nov 21, 2023 · fbb387f · fbb387f
1 parent b26cdba
commit fbb387f
Showing 1 changed file with 35 additions and 28 deletions.
diff --git a/wsce_display_bot.ino b/wsce_display_bot.ino
@@ -28,13 +28,14 @@ constexpr auto SHOOTER_LEDS_LENGTH = 247U;
 
 constexpr auto TILT_UPPER_LIMIT_PIN = 5U;
 constexpr auto TILT_LOWER_LIMIT_PIN = 6U;
-constexpr auto MOTOR_ENABLE_PIN = 7U;
+constexpr auto MOTOR_ENABLE_PIN     = 7U;
 
 // All routines below take "phase" as an argument
 // This is a value in the range (0,1]
 
-double fractional(const double val) {
-  return val - (int)val;
+double fractional(const double val)
+{
+    return val - (int)val;
 }
 
 /* LED routines */
@@ -55,40 +56,46 @@ void rainbow(const double phase, Adafruit_NeoPixel *leds)
     leds->show();
 }
 
-void pulse(const double phase, Adafruit_NeoPixel *leds, const unsigned char r, const unsigned char g, unsigned char b) {
-    const double intensity = phase > 0.5 ? phase * 2.0: (2.0 - phase * 2.0);  // Ramp up then down
+void pulse(const double phase, Adafruit_NeoPixel *leds, const unsigned char r, const unsigned char g, unsigned char b)
+{
+    const double intensity = phase < 0.5 ? phase * 2.0 : (2.0 - phase * 2.0); // Ramp up then down
     for (int i = 0; i < leds->numPixels(); i++)
     {
-      leds->setPixelColor(i, intensity * r, intensity * g, intensity * b);
+        leds->setPixelColor(i, intensity * r, intensity * g, intensity * b);
     }
-    leds->show();    
+    leds->show();
 }
-void stripes(const double phase, Adafruit_NeoPixel *leds, const unsigned char r, const unsigned char g, unsigned char b) {
-    const int strip_width = 4;
-    const auto n = leds->numPixels();
-    const unsigned int offset = phase * n/2;
-    int count = 0;
+void stripes(const double phase, Adafruit_NeoPixel *leds, const unsigned char r, const unsigned char g, unsigned char b)
+{
+    const int          strip_width = 4;
+    const auto         n           = leds->numPixels();
+    const unsigned int offset      = phase * n / 2;
+    int                count       = 0;
     for (int i = 0; i < n / 2; i++)
     {
-      const auto idx = (i + offset) % (n/2); 
-      if (count < strip_width) {
-        leds->setPixelColor(idx, r, g, b);
-        leds->setPixelColor(n - idx, r, g, b);
-      } else {
-        leds->setPixelColor(idx, 0, 0, 0);
-        leds->setPixelColor(n - idx, 0, 0, 0);
-      }
-      count = (count + 1) % (strip_width * 3);
+        const auto idx = (i + offset) % (n / 2);
+        if (count < strip_width)
+        {
+            leds->setPixelColor(idx, r, g, b);
+            leds->setPixelColor(n - idx, r, g, b);
+        }
+        else
+        {
+            leds->setPixelColor(idx, 0, 0, 0);
+            leds->setPixelColor(n - idx, 0, 0, 0);
+        }
+        count = (count + 1) % (strip_width * 3);
     }
-    leds->show();    
+    leds->show();
 }
-void solid(Adafruit_NeoPixel *leds, const unsigned char r, const unsigned char g, const unsigned char b) {
+void solid(Adafruit_NeoPixel *leds, const unsigned char r, const unsigned char g, const unsigned char b)
+{
     const auto n = leds->numPixels();
     for (int i = 0; i < n; i++)
     {
         leds->setPixelColor(i, r, g, b);
     }
-    leds->show();    
+    leds->show();
 }
 
 /* Motor routines */
@@ -114,7 +121,7 @@ void tick_motion(const double phase)
         const auto segment_phase = (phase - 0.05) / 0.1;
         // Intake on
         stripes(fractional(segment_phase), intake_leds, 0, 255, 0);
-        pulse(fractional(segment_phase * 5), shooter_leds, 255, 0, 0); 
+        pulse(fractional(segment_phase * 5), shooter_leds, 255, 0, 0);
         // Ramp up...
         const double duty = min(segment_phase * 2, 0.5);
         intake_motor.writeMicroseconds(throttle(duty));
@@ -125,7 +132,7 @@ void tick_motion(const double phase)
         intake_motor.writeMicroseconds(throttle(0));
         shooter_motor.writeMicroseconds(throttle(0));
         solid(intake_leds, 0, 0, 0);
-        
+
         // More stuff...
     }
 }
@@ -169,8 +176,8 @@ void loop()
     if (dt < LIGHTS_ONLY_CYCLE_TIME)
     {
         // Just run the light show for the first x seconds
-        const auto phase = 1.0 * dt / LIGHTS_ONLY_CYCLE_TIME;
-        double rainbow_phase    = fractional(1.0 * LIGHTS_ONLY_CYCLE_TIME / 1500 * phase);  // one cycle per 1.5s
+        const auto phase         = 1.0 * dt / LIGHTS_ONLY_CYCLE_TIME;
+        double     rainbow_phase = fractional(1.0 * LIGHTS_ONLY_CYCLE_TIME / 1500 * phase); // one cycle per 1.5s
         rainbow(rainbow_phase, chassis_leds);
         rainbow(rainbow_phase, intake_leds);
         rainbow(rainbow_phase, shooter_leds);