Made light code (untested)

Iron-Panthers · Dec 5, 2024 · cdacfb5 · cdacfb5
1 parent defe223
commit cdacfb5
Show file tree

Hide file tree

Showing 6 changed files with 64 additions and 432 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -4,15 +4,14 @@
 
 package frc.robot;
 
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import java.util.concurrent.LinkedTransferQueue;
+
 import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.Mode;
-import frc.robot.subsystems.RGBSubsystem;
+import frc.robot.subsystems.LightsSubsystem;
 import frc.robot.subsystems.flywheels.Flywheels;
 import frc.robot.subsystems.flywheels.FlywheelsIOTalonFX;
 import frc.robot.subsystems.rollers.RollerSensorsIO;
@@ -37,7 +36,6 @@
 import frc.robot.subsystems.swerve.GyroIOPigeon2;
 import frc.robot.subsystems.swerve.ModuleIO;
 import frc.robot.subsystems.swerve.ModuleIOTalonFX;
-import java.util.function.DoubleSupplier;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -54,14 +52,14 @@ public class RobotContainer {
   private Rollers rollers;
   private Flywheels flywheels;
   private Superstructure superstructure;
-  private RGBSubsystem rgbSubsystem;
+  private LightsSubsystem lightsSubsystem;
 
   public RobotContainer() {
     Intake intake = null;
     Accelerator accelerator = null;
     Serializer serializer = null;
     RollerSensorsIO rollerSensorsIO = null;
-    rgbSubsystem = new RGBSubsystem();
+
 
     if (Constants.getRobotMode() != Mode.REPLAY) {
       switch (Constants.getRobotType()) {
@@ -122,6 +120,7 @@ public RobotContainer() {
     superstructure =
         new Superstructure(new Elevator(new ElevatorIOTalonFX()), new Pivot(new PivotIOTalonFX()));
     rollers = new Rollers(intake, accelerator, serializer, rollerSensorsIO);
+    lightsSubsystem = new LightsSubsystem();
 
     configureBindings();
     configureAutos();
@@ -139,6 +138,8 @@ private void configureBindings() {
                       driverA.getLeftTriggerAxis() - driverA.getRightTriggerAxis());
                 })
             .withName("Drive Teleop"));
+
+
 
     driverA.start().onTrue(swerve.zeroGyroCommand());
 
@@ -156,13 +157,6 @@ private void configureBindings() {
                         interrupted -> rollers.setTargetState(RollerState.IDLE),
                         () -> rollers.serializerDetected(),
                         rollers))
-                .andThen(
-                    new InstantCommand(
-                        () ->
-                            rgbSubsystem.showMessage(
-                                RGBSubsystem.Lights.Colors.RED,
-                                RGBSubsystem.PatternTypes.STROBE,
-                                RGBSubsystem.MessagePriority.F_NOTE_IN_ROBOT)))
                 .andThen(
                     new InstantCommand(() -> rollers.setTargetState(RollerState.AMP_EJECT), rollers)
                         .withTimeout(0.6))
@@ -186,13 +180,6 @@ private void configureBindings() {
                         interrupted -> rollers.setTargetState(RollerState.IDLE),
                         () -> rollers.serializerDetected(),
                         rollers))
-                .andThen(
-                    new InstantCommand(
-                        () ->
-                            rgbSubsystem.showMessage(
-                                RGBSubsystem.Lights.Colors.RED,
-                                RGBSubsystem.PatternTypes.STROBE,
-                                RGBSubsystem.MessagePriority.F_NOTE_IN_ROBOT)))
                 .andThen(
                     new InstantCommand(() -> rollers.setTargetState(RollerState.AMP_EJECT), rollers)
                         .withTimeout(0.6))
@@ -240,8 +227,7 @@ private void configureBindings() {
                             flywheels.setVelocityTarget(Flywheels.VelocityTarget.IDLE);
                             superstructure.setTargetState(SuperstructureState.STOW);
                           }
-                        }))
-                .andThen(new InstantCommand(() -> rgbSubsystem.expireCurrent())));
+                        })));
     driverB
         .rightBumper()
         .onTrue(
@@ -264,8 +250,7 @@ private void configureBindings() {
                             flywheels.setVelocityTarget(Flywheels.VelocityTarget.IDLE);
                             superstructure.setTargetState(SuperstructureState.STOW);
                           }
-                        }))
-                .andThen(new InstantCommand(() -> rgbSubsystem.expireCurrent())));
+                        })));
 
     // transfer note to shooter
     driverB
@@ -390,7 +375,7 @@ private void configureBindings() {
     //               flywheels.setVelocityTarget(VelocityTarget.IDLE);
     //             },
     //             flywheels));
-    
+
     // cancel everything
     driverB
         .x()
@@ -423,7 +408,7 @@ private void configureBindings() {
         .onTrue(new InstantCommand(() -> superstructure.setTargetState(SuperstructureState.STOW)));
     driverB
         .povUp()
-        .onTrue(new InstantCommand(() -> superstructure.setTargetState(SuperstructureState.AMP)));    
+        .onTrue(new InstantCommand(() -> superstructure.setTargetState(SuperstructureState.AMP)));
   }
 
   private void configureAutos() {}

diff --git a/src/main/java/frc/robot/subsystems/LightsSubsystem.java b/src/main/java/frc/robot/subsystems/LightsSubsystem.java
@@ -0,0 +1,40 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class LightsSubsystem extends SubsystemBase{
+    private final AddressableLED lights;
+    private final AddressableLEDBuffer buffer;
+    private int firstPixelHue;
+    public LightsSubsystem() {
+        lights = new AddressableLED(34);
+        // turn off the LEDs when the can chain fails
+        buffer = new AddressableLEDBuffer(227);
+        lights.setLength(buffer.getLength());
+        lights.setData(buffer);
+        lights.start();
+        firstPixelHue = 0;
+    }
+    private void rainbow() {
+        // For every pixel
+        for (var i = 0; i < buffer.getLength(); i++) {
+            // Calculate the hue - hue is easier for rainbows because the color
+            // shape is a circle so only one value needs to precess
+            final var hue = (firstPixelHue + (i * 180 / buffer.getLength())) % 180;
+            // Set the value
+            buffer.setHSV(i, hue, 255, 128);
+        }
+        // Increase by to make the rainbow "move"
+        firstPixelHue += 3;
+        // Check bounds
+        firstPixelHue %= 180;
+    }
+
+    @Override 
+    public void periodic(){
+        rainbow();
+        lights.setData(buffer);
+    }
+}