Merge pull request #8 from AlphaKnights/LEDs

Implement LED support

.github/workflows/main.yml

@@ -4,6 +4,7 @@ on:
   push:
     branches:
       - main
+
 jobs:
   build:
     runs-on: ubuntu-latest

commands/teamcolors.py

@@ -0,0 +1,17 @@
+import commands2
+import typing
+from subsystems.ledsubsystem import LEDSubsystem
+class TeamColorsCommand(commands2.Command):
+    def __init__(self, led_sub: LEDSubsystem) -> None:
+        super().__init__()
+        self.led = led_sub
+        self.addRequirements(led_sub)
+
+    def execute(self) -> None:
+        self.led.teamLights()
+
+    def isFinished(self) -> bool:
+        return False
+
+    def end(self, interrupted: bool) -> None:
+        pass

constants.py

@@ -140,3 +140,15 @@ class AutoConstants:
     kThetaControllerConstraints = TrapezoidProfileRadians.Constraints(
         kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared
     )
+
+class LEDConstants:
+    kLEDPort = 0
+    kLEDBuffer = 64
+
+    kSilverHue = 0
+    kSilverSat = 0
+    kSilverVal = 10
+
+    kBlueHue = 90
+    kBlueSat = 220
+    kBlueVal = 30

robotcontainer.py

@@ -14,6 +14,7 @@
 from commands.auto_align import AutoAlign
 from constants import AutoConstants, DriveConstants, OIConstants
 from subsystems.drivesubsystem import DriveSubsystem
+
 from subsystems.limelight_subsystem import LimelightSystem
 from commands.auto_rotate import AutoRotate
 from commands.drivecommand import DriveCommand
@@ -33,6 +34,7 @@ class RobotContainer:
     def __init__(self) -> None:
         # The robot's subsystems
         self.robotDrive = DriveSubsystem()
+
         self.limelight = LimelightSystem()
 
         self.autoChooser = AutoBuilder.buildAutoChooser()

subsystems/ledsubsystem.py

@@ -0,0 +1,34 @@
+import math
+import typing
+
+import wpilib
+
+from commands2 import Subsystem
+from constants import LEDConstants
+
+class LEDSubsystem(Subsystem):
+    def __init__(self) -> None:
+        super().__init__()
+        self.led = wpilib.AddressableLED(LEDConstants.kLEDPort)
+
+        self.ledData = [wpilib.AddressableLED.LEDData() for _ in range(LEDConstants.kLEDBuffer)]
+
+        self.led.setLength(LEDConstants.kLEDBuffer)
+
+        self.led.setData(self.ledData)
+        self.led.start()
+
+
+    def periodic(self) -> None:
+        self.led.setData(self.ledData)
+
+    def lightAll(self, hue: int, sat: int, val: int) -> None:
+        for i in range(LEDConstants.kLEDBuffer):
+            self.ledData[i].setHSV(hue, sat, val)
+
+    def teamLights(self) -> None:
+        for i in range(LEDConstants.kLEDBuffer):
+            if (i % 2 == 0):
+                self.ledData[i].setHSV(LEDConstants.kSilverHue, LEDConstants.kSilverSat, LEDConstants.kSilverVal)
+            else:
+                self.ledData[i].setHSV(LEDConstants.kBlueHue, LEDConstants.kBlueSat, LEDConstants.kBlueVal)