JW/RX - LEDController + Rainbow LEDs

src/main/java/com/stuypulse/robot/Main.java

@@ -6,8 +6,6 @@
 package com.stuypulse.robot;
 
 import com.stuypulse.robot.constants.Settings;
-import com.stuypulse.robot.test.Testbot;
-import com.stuypulse.robot.test.TestbotContainer;
 import com.stuypulse.stuylib.util.StopWatch;
 import com.stuypulse.robot.Robot;
 
@@ -19,10 +17,6 @@ private Main() {}
 
     public static void main(String... args) {
         StopWatch.setDefaultEngine(StopWatch.kFPGAEngine);
-        if (Settings.ROBOT == Settings.Robot.BLAY_MODE) {
-            RobotBase.startRobot(Testbot::new);
-        } else {
-            RobotBase.startRobot(Robot::new);
-        }
+        RobotBase.startRobot(Robot::new);
     }
 }

src/main/java/com/stuypulse/robot/Robot.java

@@ -6,6 +6,9 @@
 package com.stuypulse.robot;
 
 import com.stuypulse.robot.commands.TeleopInit;
+import com.stuypulse.robot.commands.leds.LEDSet;
+import com.stuypulse.robot.commands.leds.LEDSetRainbow;
+import com.stuypulse.robot.util.LEDColor;
 
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -47,6 +50,7 @@ public void robotInit() {
 
     @Override
     public void robotPeriodic() {
+        //new LEDSetRainbow();
         scheduler.run();
     }
 

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -17,9 +17,11 @@
 import com.stuypulse.robot.commands.wing.*;
 import com.stuypulse.robot.commands.intake.*;
 import com.stuypulse.robot.commands.leds.LEDSet;
+import com.stuypulse.robot.commands.leds.LEDSetRainbow;
 import com.stuypulse.robot.subsystems.*;
 import com.stuypulse.robot.subsystems.arm.*;
 import com.stuypulse.robot.subsystems.intake.*;
+import com.stuypulse.robot.subsystems.leds.LEDController;
 import com.stuypulse.robot.subsystems.odometry.*;
 import com.stuypulse.robot.subsystems.swerve.*;
 import com.stuypulse.robot.subsystems.vision.*;
@@ -34,7 +36,6 @@
 import com.stuypulse.stuylib.streams.booleans.BStream;
 import com.stuypulse.stuylib.streams.booleans.filters.BDebounce;
 import com.stuypulse.stuylib.streams.booleans.filters.BFilter;
-import com.stuypulse.robot.util.BootlegXbox;
 import com.stuypulse.stuylib.input.Gamepad;
 import com.stuypulse.stuylib.input.gamepads.*;
 
@@ -150,7 +151,9 @@ private void configureDriverBindings() {
         driver.getDPadRight().onTrue(new OdometryRealign(Rotation2d.fromDegrees(90)));
 
         // plant
-        driver.getRightButton().onTrue(new PlantEngage());
+        //driver.getRightButton().onTrue(new PlantEngage());
+
+        driver.getRightButton().whileTrue(new LEDSetRainbow());
         driver.getRightBumper().onTrue(new PlantDisengage());
 
         new Trigger(intake::hasCone)
@@ -191,9 +194,11 @@ private void configureOperatorBindings() {
                     .andThen(new ManagerValidateState())
                     .andThen(new ArmReady()));
 
-        operator.getRightButton()
-            .onTrue(new IntakeScore())
-            .onFalse(new IntakeStop());
+        // operator.getRightButton()
+            // .onTrue(new IntakeScore())
+            // .onFalse(new IntakeStop());
+
+        operator.getRightButton().onTrue(new LEDSetRainbow());
 
         // set level to score at
         operator.getDPadDown().onTrue(new ManagerSetNodeLevel(NodeLevel.LOW));

src/main/java/com/stuypulse/robot/commands/auton/OnePieceDock.java

@@ -5,16 +5,19 @@
 import com.stuypulse.robot.commands.arm.routines.*;
 import com.stuypulse.robot.commands.intake.*;
 import com.stuypulse.robot.commands.leds.LEDSet;
+import com.stuypulse.robot.commands.leds.LEDSetRainbow;
 import com.stuypulse.robot.commands.manager.*;
 import com.stuypulse.robot.commands.plant.PlantEngage;
 import com.stuypulse.robot.commands.swerve.*;
 import com.stuypulse.robot.commands.swerve.balance.SwerveDriveBalanceBlay;
 import com.stuypulse.robot.subsystems.Manager.*;
+import com.stuypulse.robot.subsystems.leds.LEDRainbow;
 import com.stuypulse.robot.util.ArmState;
 import com.stuypulse.robot.util.ArmTrajectory;
 import com.stuypulse.robot.util.DebugSequentialCommandGroup;
 import com.stuypulse.robot.util.LEDColor;
 
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
 import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 
@@ -37,6 +40,8 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) {
     private static final double INTAKE_ACQUIRE_TIME = 0.8;
     private static final double ENGAGE_TIME = 10.0;
 
+    private AddressableLEDBuffer ledsBuffer;
+
     private static final PathConstraints DOCK = new PathConstraints(1.8, 2.5);
 
     public OnePieceDock() {
@@ -76,7 +81,7 @@ public OnePieceDock() {
         );
 
         addCommands(
-            new LEDSet(LEDColor.RAINBOW),
+            new LEDSetRainbow(),
 
             new SwerveDriveBalanceBlay()
                 .withMaxSpeed(0.6)

src/main/java/com/stuypulse/robot/commands/auton/OnePieceMobilityDock.java

@@ -5,6 +5,7 @@
 import com.stuypulse.robot.commands.arm.routines.*;
 import com.stuypulse.robot.commands.intake.*;
 import com.stuypulse.robot.commands.leds.LEDSet;
+import com.stuypulse.robot.commands.leds.LEDSetRainbow;
 import com.stuypulse.robot.commands.manager.*;
 import com.stuypulse.robot.commands.plant.PlantEngage;
 import com.stuypulse.robot.commands.swerve.*;
@@ -15,6 +16,7 @@
 import com.stuypulse.robot.util.DebugSequentialCommandGroup;
 import com.stuypulse.robot.util.LEDColor;
 
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
 import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 
@@ -38,6 +40,8 @@ protected ArmTrajectory getTrajectory(ArmState src, ArmState dest) {
     private static final double REF_REACTION_TIME = 0.8;
     private static final double ENGAGE_TIME = 10.0;
 
+    private AddressableLEDBuffer ledsBuffer;
+
     private static final PathConstraints OVER_CHARGE = new PathConstraints(1, 2);
     private static final PathConstraints DOCK = new PathConstraints(1, 2);
 
@@ -90,7 +94,7 @@ public OnePieceMobilityDock() {
         );
 
         addCommands(
-            new LEDSet(LEDColor.RAINBOW),
+            new LEDSetRainbow(),
 
             new SwerveDriveBalanceBlay()
                 .withMaxSpeed(0.6)

src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDock.java

@@ -5,6 +5,7 @@
 import com.stuypulse.robot.commands.arm.routines.*;
 import com.stuypulse.robot.commands.intake.*;
 import com.stuypulse.robot.commands.leds.LEDSet;
+import com.stuypulse.robot.commands.leds.LEDSetRainbow;
 import com.stuypulse.robot.commands.manager.*;
 import com.stuypulse.robot.commands.plant.PlantEngage;
 import com.stuypulse.robot.commands.swerve.*;
@@ -15,6 +16,7 @@
 import com.stuypulse.robot.util.DebugSequentialCommandGroup;
 import com.stuypulse.robot.util.LEDColor;
 
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
 import edu.wpi.first.wpilibj2.command.PrintCommand;
@@ -30,6 +32,8 @@ public class OnePiecePickupDock extends DebugSequentialCommandGroup {
     private static final double ACQUIRE_WAIT_TIME = 0.4;
     private static final double ENGAGE_TIME = 10.0;
 
+    private AddressableLEDBuffer ledsBuffer;
+
     private static final PathConstraints INTAKE_PIECE = new PathConstraints(2, 2);
     private static final PathConstraints DOCK = new PathConstraints(1, 2);
 
@@ -102,7 +106,7 @@ public OnePiecePickupDock() {
         );
 
         addCommands(
-            new LEDSet(LEDColor.RAINBOW),
+            new LEDSetRainbow(),
 
             new SwerveDriveBalanceBlay()
                 .withMaxSpeed(0.7)

src/main/java/com/stuypulse/robot/commands/auton/OnePiecePickupDockWire.java

@@ -5,6 +5,7 @@
 import com.stuypulse.robot.commands.arm.routines.*;
 import com.stuypulse.robot.commands.intake.*;
 import com.stuypulse.robot.commands.leds.LEDSet;
+import com.stuypulse.robot.commands.leds.LEDSetRainbow;
 import com.stuypulse.robot.commands.manager.*;
 import com.stuypulse.robot.commands.plant.PlantEngage;
 import com.stuypulse.robot.commands.swerve.*;
@@ -15,6 +16,7 @@
 import com.stuypulse.robot.util.DebugSequentialCommandGroup;
 import com.stuypulse.robot.util.LEDColor;
 
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
 import edu.wpi.first.wpilibj2.command.PrintCommand;
@@ -31,6 +33,8 @@ public class OnePiecePickupDockWire extends DebugSequentialCommandGroup {
     private static final double ENGAGE_TIME = 10.0;
     private static final double ACQUIRE_WAIT_TIME = 0.4;
 
+    private AddressableLEDBuffer ledsBuffer;
+
     private static final PathConstraints INTAKE_PIECE = new PathConstraints(3, 2);
     private static final PathConstraints DOCK = new PathConstraints(1, 2);
 
@@ -103,7 +107,7 @@ public OnePiecePickupDockWire() {
         );
 
         addCommands(
-            new LEDSet(LEDColor.RAINBOW),
+            new LEDSetRainbow(),
 
             new SwerveDriveBalanceBlay()
                 .withMaxSpeed(0.5)

src/main/java/com/stuypulse/robot/commands/auton/TwoPiece.java

@@ -5,13 +5,15 @@
 import com.stuypulse.robot.commands.arm.routines.*;
 import com.stuypulse.robot.commands.intake.*;
 import com.stuypulse.robot.commands.leds.LEDSet;
+import com.stuypulse.robot.commands.leds.LEDSetRainbow;
 import com.stuypulse.robot.commands.manager.*;
 import com.stuypulse.robot.commands.swerve.*;
 import com.stuypulse.robot.subsystems.Manager.*;
 import com.stuypulse.robot.subsystems.arm.Arm;
 import com.stuypulse.robot.util.DebugSequentialCommandGroup;
 import com.stuypulse.robot.util.LEDColor;
 
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 
@@ -22,6 +24,8 @@ public class TwoPiece extends DebugSequentialCommandGroup {
     private static final double INTAKE_WAIT_TIME = 2.0;
     private static final double ACQUIRE_WAIT_TIME = 0.4;
 
+    private AddressableLEDBuffer ledsBuffer;
+
     private static final PathConstraints INTAKE_PIECE_CONSTRAINTS = new PathConstraints(1.7, 2);
     private static final PathConstraints SCORE_PIECE_CONSTRAINTS = new PathConstraints(2, 2);
 
@@ -106,7 +110,7 @@ public TwoPiece () {
         );
 
         addCommands(
-            new LEDSet(LEDColor.RAINBOW),
+            new LEDSetRainbow(),
             new SwerveDriveFollowTrajectory(
                 paths.get("Back Away"))
                     .fieldRelative()

src/main/java/com/stuypulse/robot/commands/auton/TwoPieceWire.java

@@ -5,13 +5,15 @@
 import com.stuypulse.robot.commands.arm.routines.*;
 import com.stuypulse.robot.commands.intake.*;
 import com.stuypulse.robot.commands.leds.LEDSet;
+import com.stuypulse.robot.commands.leds.LEDSetRainbow;
 import com.stuypulse.robot.commands.manager.*;
 import com.stuypulse.robot.commands.swerve.*;
 import com.stuypulse.robot.subsystems.Manager.*;
 import com.stuypulse.robot.subsystems.arm.Arm;
 import com.stuypulse.robot.util.DebugSequentialCommandGroup;
 import com.stuypulse.robot.util.LEDColor;
 
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
@@ -26,6 +28,8 @@ public class TwoPieceWire extends DebugSequentialCommandGroup {
     private static final double ACQUIRE_WAIT_TIME = 0.4;
     private static final double READY_WAIT_TIME = 0.5;
 
+    private AddressableLEDBuffer ledsBuffer;
+
     private static final PathConstraints INTAKE_PIECE_CONSTRAINTS = new PathConstraints(2, 2);
     private static final PathConstraints SCORE_PIECE_CONSTRAINTS = new PathConstraints(2, 2);
 
@@ -110,7 +114,7 @@ public TwoPieceWire() {
         );
 
         addCommands(
-            new LEDSet(LEDColor.RAINBOW),
+            new LEDSetRainbow(),
             new SwerveDriveFollowTrajectory(
                 paths.get("Back Away"))
                     .withStop()

src/main/java/com/stuypulse/robot/commands/leds/LEDSet.java

@@ -3,12 +3,21 @@
 /* This work is licensed under the terms of the MIT license.    */
 /****************************************************************/
 
+/*-
+ * Contains:
+ *      - commands for LEDs
+ * @author Richie Xue
+ * @author Jo Walkup
+ */
+
 package com.stuypulse.robot.commands.leds;
 
-import com.stuypulse.robot.subsystems.LEDController;
 import com.stuypulse.robot.util.LEDColor;
 
 import com.stuypulse.robot.constants.Settings;
+//import com.stuypulse.robot.constants.Ports.LEDController;
+import com.stuypulse.robot.subsystems.leds.*;
+import com.stuypulse.robot.subsystems.leds.LEDControllerImpl;
 
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 
@@ -17,19 +26,33 @@ public class LEDSet extends InstantCommand {
     private LEDColor color;
     private double updateTime;
     private LEDController controller;
+    private LEDInstruction instruction;
 
     public LEDSet(LEDColor color, double updateTime) {
         this.controller = LEDController.getInstance();
         this.updateTime = updateTime;
         this.color = color;
     }
 
+    public LEDSet(LEDInstruction instruction, double updateTime) {
+        this.controller = LEDController.getInstance();
+        this.updateTime = updateTime;
+        this.instruction = instruction;
+    }
+
     public LEDSet(LEDColor color) {
         this(color, Settings.LED.MANUAL_UPDATE_TIME);
+        //System.out.println("LEDSet was called!!!");
+    }
+
+    public LEDSet(LEDInstruction instruction) {
+        this(instruction, Settings.LED.MANUAL_UPDATE_TIME);
+        //System.out.println("LEDSet was called!!!");
     }
 
     @Override
     public void initialize() {
+        System.out.println("LEDSet was called!!!");
         controller.setColor(color, updateTime);
     }
 }

src/main/java/com/stuypulse/robot/commands/leds/LEDSetPulseRed.java

@@ -0,0 +1,24 @@
+package com.stuypulse.robot.commands.leds;
+
+import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.robot.subsystems.leds.LEDController;
+import com.stuypulse.robot.util.LEDColor;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class LEDSetPulseRed extends InstantCommand {
+    private LEDController controller;
+
+    public LEDSetPulseRed(double updateTime) {
+        this.controller = LEDController.getInstance();
+    }
+
+    public LEDSetPulseRed() {
+        this(Settings.LED.MANUAL_UPDATE_TIME);
+    }
+
+    @Override
+    public void initialize() {
+        controller.forceSetLED(LEDColor.PULSE_RED);
+    }
+}