JW/RX - LEDController + Rainbow LEDs

src/main/deploy/pathplanner/3 Piece W Low.path

@@ -0,0 +1,202 @@
+{
+  "waypoints": [
+    {
+      "anchorPoint": {
+        "x": 1.8,
+        "y": 4.6
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.1835287054512387,
+        "y": 4.6
+      },
+      "holonomicAngle": 0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 6.187741628152297,
+        "y": 4.44
+      },
+      "prevControl": {
+        "x": 5.435011671065698,
+        "y": 4.44
+      },
+      "nextControl": {
+        "x": 6.396944039439681,
+        "y": 4.44
+      },
+      "holonomicAngle": 0,
+      "isReversal": false,
+      "velOverride": 1.0,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 6.62,
+        "y": 4.4
+      },
+      "prevControl": {
+        "x": 6.501547629778108,
+        "y": 4.431296466187375
+      },
+      "nextControl": {
+        "x": 6.501547629778108,
+        "y": 4.431296466187375
+      },
+      "holonomicAngle": 0,
+      "isReversal": true,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": true,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 2.1599999999999997,
+        "y": 4.6
+      },
+      "prevControl": {
+        "x": 3.4298747164225913,
+        "y": 4.6
+      },
+      "nextControl": {
+        "x": 3.4298747164225913,
+        "y": 4.6
+      },
+      "holonomicAngle": 6.54,
+      "isReversal": true,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": true,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 6.58,
+        "y": 3.53
+      },
+      "prevControl": {
+        "x": 4.903826385233523,
+        "y": 4.751390831153475
+      },
+      "nextControl": {
+        "x": 6.7315776729384345,
+        "y": 3.419548684990858
+      },
+      "holonomicAngle": -30.0,
+      "isReversal": false,
+      "velOverride": 1.0,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 6.9,
+        "y": 3.25
+      },
+      "prevControl": {
+        "x": 6.842860523327884,
+        "y": 3.3115587598696563
+      },
+      "nextControl": {
+        "x": 6.842860523327884,
+        "y": 3.3115587598696563
+      },
+      "holonomicAngle": -30.0,
+      "isReversal": true,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": true,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 1.7999999999999998,
+        "y": 4.5
+      },
+      "prevControl": {
+        "x": 5.794373267068212,
+        "y": 4.905051768145495
+      },
+      "nextControl": {
+        "x": 5.794373267068212,
+        "y": 4.905051768145495
+      },
+      "holonomicAngle": 10.0,
+      "isReversal": true,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": true,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 7.494791213358649,
+        "y": 5.540372759067196
+      },
+      "prevControl": {
+        "x": 6.738011797701916,
+        "y": 5.2881129538482865
+      },
+      "nextControl": null,
+      "holonomicAngle": 0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    }
+  ],
+  "maxVelocity": 4.0,
+  "maxAcceleration": 3.0,
+  "isReversed": null,
+  "markers": []
+}

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

@@ -5,6 +5,7 @@
 
 package com.stuypulse.robot;
 
+import com.stuypulse.robot.constants.Settings;
 import com.stuypulse.stuylib.util.StopWatch;
 
 import edu.wpi.first.wpilibj.RobotBase;

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

@@ -49,6 +49,7 @@ public void robotInit() {
 
     @Override
     public void robotPeriodic() {
+        //new LEDSetRainbow();
         scheduler.run();
     }
 

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

@@ -13,10 +13,6 @@
 import com.stuypulse.robot.commands.arm.*;
 import com.stuypulse.robot.commands.arm.routines.*;
 import com.stuypulse.robot.commands.auton.*;
-import com.stuypulse.robot.commands.auton.battlecry.BCThreePieceBumpBlue;
-import com.stuypulse.robot.commands.auton.battlecry.BCThreePieceBumpRed;
-import com.stuypulse.robot.commands.auton.battlecry.BCTwoPieceDockBumpBlue;
-import com.stuypulse.robot.commands.auton.battlecry.BCTwoPieceDockBumpRed;
 import com.stuypulse.robot.commands.intake.*;
 import com.stuypulse.robot.commands.leds.LEDSet;
 import com.stuypulse.robot.commands.manager.*;
@@ -31,12 +27,14 @@
 import com.stuypulse.robot.subsystems.Manager.*;
 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.plant.*;
 import com.stuypulse.robot.subsystems.swerve.*;
 import com.stuypulse.robot.subsystems.vision.*;
 import com.stuypulse.robot.subsystems.wing.*;
 import com.stuypulse.robot.util.*;
+
 import com.stuypulse.robot.util.BootlegXbox;
 
 import edu.wpi.first.cameraserver.CameraServer;
@@ -236,6 +234,8 @@ private void configureOperatorBindings() {
     /**************/
 
     public void configureAutons() {
+        autonChooser.setDefaultOption("Do Nothing", new DoNothing());
+        /* 
         autonChooser.addOption("Do Nothing", new DoNothing());
         autonChooser.addOption("Mobility", new Mobility());
 
@@ -266,7 +266,7 @@ public void configureAutons() {
         // autonChooser.addOption("Three Piece Bump Removed Blue", new BCThreePieceBumpBlue());
         // autonChooser.addOption("Three Piece Bump Removed Red", new BCThreePieceBumpRed());
         autonChooser.addOption("Three Piece Bump", new ThreePieceBump());
-
+        */
         SmartDashboard.putData("Autonomous", autonChooser);
     }
 

src/main/java/com/stuypulse/robot/commands/RobotAlignThenScore.java

@@ -12,7 +12,6 @@
 import com.stuypulse.stuylib.streams.filters.IFilter;
 import com.stuypulse.robot.constants.ArmTrajectories;
 import com.stuypulse.robot.constants.Settings;
-import com.stuypulse.robot.subsystems.LEDController;
 import com.stuypulse.robot.constants.Settings.Alignment;
 import com.stuypulse.robot.constants.Settings.Alignment.Rotation;
 import com.stuypulse.robot.constants.Settings.Alignment.Translation;
@@ -21,6 +20,7 @@
 import com.stuypulse.robot.subsystems.Manager.NodeLevel;
 import com.stuypulse.robot.subsystems.arm.Arm;
 import com.stuypulse.robot.subsystems.intake.*;
+import com.stuypulse.robot.subsystems.leds.LEDController;
 import com.stuypulse.robot.subsystems.odometry.Odometry;
 import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
 import com.stuypulse.robot.util.Derivative;
@@ -41,6 +41,7 @@ public class RobotAlignThenScore extends CommandBase {
     private final SwerveDrive swerve;
     private final Arm arm;
     private final Intake intake;
+    private final LEDController leds;
 
     private final Manager manager;
 
@@ -61,6 +62,7 @@ public RobotAlignThenScore() {
         this.arm = Arm.getInstance();
         this.intake = Intake.getInstance();
         manager = Manager.getInstance();
+        this.leds = LEDController.getInstance();
 
         controller = new HolonomicController(
             new PIDController(Translation.P,Translation.I,Translation.D),
@@ -111,7 +113,7 @@ public void initialize() {
 
         xErrorChange.reset();
 
-        LEDController.getInstance().setColor(LEDColor.BLUE, 694000000);
+        leds.forceSetLED(LEDColor.BLUE);
     }
 
     @Override
@@ -179,7 +181,7 @@ public void end(boolean interupted) {
         intake.stop();
         targetPose2d.setPose(Double.NaN, Double.NaN, new Rotation2d(Double.NaN));
 
-        LEDController.getInstance().setColor(LEDController.getInstance().getDefaultColor(), 0);
+        leds.forceSetLED(leds.getDefaultColor());
     }
 
 }

src/main/java/com/stuypulse/robot/commands/RobotAlignThenScoreCubes.java

@@ -10,7 +10,9 @@
 import com.stuypulse.stuylib.streams.booleans.BStream;
 import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC;
 
-import com.stuypulse.robot.subsystems.LEDController;
+import com.stuypulse.robot.constants.ArmTrajectories;
+import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.robot.subsystems.leds.LEDController;
 import com.stuypulse.robot.constants.Settings.Alignment;
 import com.stuypulse.robot.constants.Settings.Alignment.Rotation;
 import com.stuypulse.robot.constants.Settings.Alignment.Translation;
@@ -33,6 +35,7 @@ public class RobotAlignThenScoreCubes extends CommandBase {
     // Subsystems
     private final SwerveDrive swerve;
     private final Intake intake;
+    private final LEDController leds;
 
     private final Manager manager;
 
@@ -48,6 +51,7 @@ public RobotAlignThenScoreCubes(){
         this.swerve = SwerveDrive.getInstance();
         this.intake = Intake.getInstance();
         manager = Manager.getInstance();
+        this.leds = LEDController.getInstance();
 
         controller = new HolonomicController(
             new PIDController(Translation.P,Translation.I,Translation.D),
@@ -91,7 +95,7 @@ public void initialize() {
         intake.enableBreak();
         Odometry.USE_VISION_ANGLE.set(true);
 
-        LEDController.getInstance().setColor(LEDColor.BLUE, 694000000);
+        leds.forceSetLED(LEDColor.BLUE);
     }
 
     @Override
@@ -109,7 +113,7 @@ public void execute() {
 
             // simply outtake when low
             if (manager.getNodeLevel() == NodeLevel.LOW) {
-                LEDController.getInstance().setColor(LEDColor.GREEN, 694000000);
+                leds.forceSetLED(LEDColor.GREEN);
                 intake.deacquire();
             }
 
@@ -119,7 +123,7 @@ public void execute() {
 
                 // only score for cubes
                 if (manager.getGamePiece().isCube()) {
-                    LEDController.getInstance().setColor(LEDColor.GREEN, 694000000);
+                    leds.forceSetLED(LEDColor.GREEN);
                     intake.deacquire();
                 }
             }
@@ -144,7 +148,7 @@ public void end(boolean interupted) {
         intake.stop();
         targetPose2d.setPose(Double.NaN, Double.NaN, new Rotation2d(Double.NaN));
 
-        LEDController.getInstance().setColor(LEDController.getInstance().getDefaultColor(), 0);
+        leds.forceSetLED(leds.getDefaultColor());
     }
 
 }