fix defense lights

they work on the LB ps5 controller button, can be changed to a different one

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 2ccbcb7b0332e7511ea1d7bff222f6c7b12e1519..c65a49c9c3f043174fb74fb0b155f82c5cfc31a1 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -127,7 +127,7 @@ public class RobotContainer {
 
       case Vertigo: // AKA "French Toast"
         drive = new Drivetrain(vision, new GyroIOPigeon2());
-        driver = new PS5ControllerDriverConfig(drive, shooter, turret, hood, intake, spindexer, linearClimb);
+        driver = new PS5ControllerDriverConfig(drive, shooter, turret, hood, intake, spindexer, linearClimb, led);
         operator = new Operator(drive);
 
         // Detected objects need access to the drivetrain

diff --git a/src/main/java/frc/robot/commands/led_comm/LEDDefaultCommand.java b/src/main/java/frc/robot/commands/led_comm/LEDDefaultCommand.java
index 84ef1e1f3b95e3c7bbe0ca1c31e8eaefbf4d5d91..f0504a84c65c0f1b0f664f73ec3bccd81b5a45c9 100644 (file)
--- a/src/main/java/frc/robot/commands/led_comm/LEDDefaultCommand.java
+++ b/src/main/java/frc/robot/commands/led_comm/LEDDefaultCommand.java
@@ -2,11 +2,16 @@ package frc.robot.commands.led_comm;
 
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.Constants;
 import frc.robot.subsystems.LED.LED;
+import lib.controllers.PS5Controller;
+import lib.controllers.PS5Controller.PS5Button;
 
 public class LEDDefaultCommand extends Command {
     private LED led;
     private boolean allianceIsRed = DriverStation.getAlliance().get() == DriverStation.Alliance.Red;
+    private final PS5Controller controller = new PS5Controller(Constants.DRIVER_JOY);
+    private int counter = 0;
 
     public LEDDefaultCommand(LED led) {
         this.led = led;
@@ -26,39 +31,58 @@ public class LEDDefaultCommand extends Command {
         if (fiveSecondsBeforeChange() && allianceIsRed) {
             // blink alliance color and rumble if red alliance 5 seconds before hub shifts
             led.setStrobeLights(255, 0, 0);
+            counter = 0;
         } else if (fiveSecondsBeforeChange()) {
             // blink alliance color and rumble if blue alliance 5 seconds before hub shifts
             led.setStrobeLights(0, 0, 255);
-        } else if (playingDefense()) {
-            led.alternate(255, 0, 0, 0, 0, 255, 5, 0, 67);
-        } else if (DriverStation.isAutonomous() && allianceIsRed) {
+            counter = 0;
+        } else if(controller.get(PS5Button.LB).getAsBoolean()){
+            counter++;
+
+            if (counter == 1) {
+                led.alternate(255, 0, 0, 0, 0, 255, 5, 0, 67);
+            } else if (counter == 20) {
+                led.alternate(0, 0, 255, 255, 0, 0, 5, 0, 67);
+            }
+            if (counter >= 40) {
+                counter = 0;
+        }
+        }else if (DriverStation.isAutonomous() && allianceIsRed) {
             // Dimmer light for auto in red alliance
             led.setLEDs(50, 0, 0);
+            counter = 0;
         } else if (DriverStation.isAutonomous()) {
             // Dimmer light for auto in blue alliance
             led.setLEDs(0, 0, 50);
+            counter = 0;
         } else if (gameData != null && ((allianceIsRed && gameData.equals("B") && matchTime <= 105 && matchTime >= 80)
                 || (allianceIsRed && gameData.equals("B") && matchTime <= 55 && matchTime >= 30))) {
             // turn light off for inactive hub if red alliance and blue inactive first
             led.setLEDs(0, 0, 0);
+            counter = 0;
         } else if (gameData != null && ((allianceIsRed && gameData.equals("R") && matchTime <= 130 && matchTime >= 105)
                 || (allianceIsRed && gameData.equals("R") && matchTime <= 80 && matchTime >= 55))) {
             // turn light off for inactive hub if red alliance and red inactive first
             led.setLEDs(0, 0, 0);
+            counter = 0;
         } else if (gameData != null && ((!allianceIsRed && gameData.equals("B") && matchTime <= 130 && matchTime >= 105)
                 || (!allianceIsRed && gameData.equals("B") && matchTime <= 80 && matchTime >= 55))) {
             // turn off lights for inactive hub if blue alliance and blue inactive first
             led.setLEDs(0, 0, 0);
+            counter = 0;
         } else if (gameData != null && ((!allianceIsRed && gameData.equals("R") && matchTime <= 105 && matchTime >= 80)
                 || (!allianceIsRed && gameData.equals("R") && matchTime <= 55 && matchTime >= 30))) {
             // turn light off for inactive hub if blue alliance and red inactive first
             led.setLEDs(0, 0, 0);
+            counter = 0;
         } else if (allianceIsRed) {
             // Red alliance
             led.setTwoColorWave(255, 0, 0, 255, 255, 255);
+            counter = 0;
         } else {
             // Blue alliance
             led.setTwoColorWave(0, 0, 255, 255, 255, 255);
+            counter = 0;
         }
     }
 
@@ -75,9 +99,4 @@ public class LEDDefaultCommand extends Command {
         }
         return false;
     }
-
-    private boolean playingDefense() {
-        // TODO: add automatic defense lights
-        return false;
-    }
 }

diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index abc8eb7a22a822f73ea9bd70e34e6a68f7d38aff..f09c1b8b6a820ba58151ce203e1a5526e25dbf40 100644 (file)
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -14,9 +14,12 @@ import frc.robot.commands.gpm.IntakeMovementCommand;
 import frc.robot.commands.gpm.ReverseMotors;
 import frc.robot.commands.gpm.RunSpindexer;
 import frc.robot.commands.gpm.Superstructure;
+import frc.robot.commands.led_comm.DefenseLightsCommand;
+import frc.robot.commands.led_comm.LEDDefaultCommand;
 import frc.robot.constants.Constants;
 import frc.robot.subsystems.Climb.LinearClimb;
 import frc.robot.subsystems.Intake.Intake;
+import frc.robot.subsystems.LED.LED;
 import frc.robot.subsystems.drivetrain.Drivetrain;
 import frc.robot.subsystems.hood.Hood;
 import frc.robot.subsystems.shooter.Shooter;
@@ -42,6 +45,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
     private Intake intake;
     private Spindexer spindexer;
     private LinearClimb climb;
+    private LED led;
 
     public PS5ControllerDriverConfig(
             Drivetrain drive,
@@ -50,7 +54,8 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
             Hood hood,
             Intake intake,
             Spindexer spindexer,
-            LinearClimb climb) {
+            LinearClimb climb,
+            LED led) {
         super(drive);
         this.shooter = shooter;
         this.turret = turret;
@@ -58,6 +63,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
         this.intake = intake;
         this.spindexer = spindexer;
         this.climb = climb;
+        this.led = led;
     }
 
     public void configureControls() {