Merge pull request #291 from thedropbears/climbing-lights

Chase up sides when climbing

Author: james-ward

components/climber.py

@@ -5,12 +5,15 @@
 from wpilib import DutyCycleEncoder
 from wpimath.controller import PIDController
 
+from components.led_component import LightStrip
 from ids import DioChannel, SparkId
 from utilities.functions import constrain_angle
 from utilities.rev import configure_through_bore_encoder
 
 
 class ClimberComponent:
+    status_lights: LightStrip
+
     target_speed = 0.0
     winch_voltage = tunable(12.0)
 
@@ -90,6 +93,8 @@ def is_retracted(self) -> bool:
         )
 
     def execute(self) -> None:
+        if self.is_retracted():
+            self.status_lights.climber_retracting()
         if self.update_pid:
             pid_result = self.pid.calculate(self.raw_encoder_val(), self.desired_angle)
             self.motor.setVoltage(pid_result)

components/led_component.py

@@ -158,7 +158,35 @@ def climber_deploying(self) -> None:
         self.keep_alive()
 
     def climber_retracting(self) -> None:
-        self.pattern = LEDPattern.blink(LEDPattern.solid(Color.kMagenta), 0.15)
+        colour = Color.kRed if is_red() else Color.kBlue
+        steps = []
+        stripes = 16
+        increment = 1.0 / 16
+        for v in range(0, stripes, 2):
+            steps.append((v * increment, colour))
+            steps.append(((v + 1) * increment, Color.kBlack))
+
+        forward_pattern = (
+            LEDPattern.steps(steps).scrollAtAbsoluteSpeed(1.0, LED_SPACING).reversed()
+        )
+
+        def remapper(length: int, idx: int) -> int:
+            idx = int(idx)
+            splits = [
+                0,
+                int(length * self.right_rear_front_split),
+                int(length * self.halfway_split),
+                int(length * self.left_front_rear_split),
+            ]
+            if splits[0] <= int(idx) < splits[1]:
+                return int(splits[1] - splits[0]) - idx
+            elif splits[2] <= int(idx) < splits[3]:
+                return int(splits[3] - splits[2]) - (idx - splits[2]) + splits[2]
+            else:
+                return int(idx)
+
+        forward = forward_pattern.mapIndex(remapper)
+        self.pattern = forward
         self.keep_alive()
 
     def execute(self) -> None: