leds simplified

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index f2849bea83eb41d553789e6c354e22ad66ebf91d..f18a165990d6f702bbdccf5fc6f11955dde4944c 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -28,7 +28,6 @@ import frc.robot.commands.gpm.ClimbDriveCommand;
 import frc.robot.commands.gpm.IntakeMovementCommand;
 import frc.robot.commands.gpm.RunSpindexer;
 import frc.robot.commands.gpm.Superstructure;
-import frc.robot.commands.led_comm.LEDDefaultCommand;
 import frc.robot.commands.vision.ShutdownAllPis;
 import frc.robot.constants.AutoConstants;
 import frc.robot.constants.Constants;
@@ -39,6 +38,7 @@ import frc.robot.controls.PS5ControllerDriverConfig;
 import frc.robot.subsystems.Climb.LinearClimb;
 import frc.robot.subsystems.Intake.Intake;
 import frc.robot.subsystems.LED.LED;
+import frc.robot.subsystems.LED.LED2;
 import frc.robot.subsystems.drivetrain.Drivetrain;
 import frc.robot.subsystems.drivetrain.GyroIOPigeon2;
 import frc.robot.subsystems.hood.Hood;
@@ -75,7 +75,7 @@ public class RobotContainer {
   private BaseDriverConfig driver = null;
   private Operator operator = null;
   private LinearClimb linearClimb = null;
-  private LED led = null;
+  private LED2 led = null;
 
   // TODO: move to correct robot and put the correct port?
   private PS5Controller ps5 = new PS5Controller(0);
@@ -114,8 +114,8 @@ public class RobotContainer {
         break;
 
       case TestBed2:
-        led = new LED();
-        led.setDefaultCommand(new LEDDefaultCommand(led));
+        // led = new LED();
+        // led.setDefaultCommand(new LEDDefaultCommand(led));
         break;
 
       default:
@@ -123,8 +123,9 @@ public class RobotContainer {
       case PrimeJr: // AKA Valence
         spindexer = new Spindexer();
         intake = new Intake();
-        led = new LED();
-        led.setDefaultCommand(new LEDDefaultCommand(led));
+        // led = new LED();
+        // led.setDefaultCommand(new LEDDefaultCommand(led));
+        led = new LED2();
 
       case WaffleHouse: // AKA Betabot
         turret = new Turret();
@@ -146,7 +147,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, led);
+        driver = new PS5ControllerDriverConfig(drive, shooter, turret, hood, intake, spindexer, linearClimb);
         operator = new Operator(drive);
 
         // Detected objects need access to the drivetrain

diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index 3349c84d5f5e387db72dee797e3830aa2d0305f4..36453feb8e88c21f5538759e45edccd8d56f5248 100644 (file)
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -40,7 +40,6 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
     private Intake intake;
     private Spindexer spindexer;
     private LinearClimb climb;
-    private LED led;
 
     public PS5ControllerDriverConfig(
             Drivetrain drive,
@@ -49,8 +48,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
             Hood hood,
             Intake intake,
             Spindexer spindexer,
-            LinearClimb climb,
-            LED led) {
+            LinearClimb climb) {
         super(drive);
         this.shooter = shooter;
         this.turret = turret;
@@ -58,7 +56,6 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
         this.intake = intake;
         this.spindexer = spindexer;
         this.climb = climb;
-        this.led = led;
     }
 
     public void configureControls() {

diff --git a/src/main/java/frc/robot/subsystems/LED/LED2.java b/src/main/java/frc/robot/subsystems/LED/LED2.java
new file mode 100644 (file)
index 0000000..5c71860
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/LED/LED2.java
@@ -0,0 +1,103 @@
+package frc.robot.subsystems.LED;
+
+import java.util.Optional;
+
+import com.ctre.phoenix6.configs.CANdleConfigurator;
+import com.ctre.phoenix6.configs.CANdleFeaturesConfigs;
+import com.ctre.phoenix6.configs.LEDConfigs;
+import com.ctre.phoenix6.controls.SolidColor;
+import com.ctre.phoenix6.controls.StrobeAnimation;
+import com.ctre.phoenix6.hardware.CANdle;
+import com.ctre.phoenix6.signals.Enable5VRailValue;
+import com.ctre.phoenix6.signals.LossOfSignalBehaviorValue;
+import com.ctre.phoenix6.signals.RGBWColor;
+import com.ctre.phoenix6.signals.StatusLedWhenActiveValue;
+import com.ctre.phoenix6.signals.StripTypeValue;
+import com.ctre.phoenix6.signals.VBatOutputModeValue;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.IdConstants;
+import frc.robot.util.HubActive;
+
+public class LED2 extends SubsystemBase {
+
+       private CANdle candle;
+       public static final int stripLength = 67;
+
+       public static final double FLASH_INTERVAL = .25;
+
+       private Color color;
+       
+       public LED2() {
+               candle = new CANdle(IdConstants.CANDLE_ID);
+               CANdleConfigurator configurator = candle.getConfigurator();
+
+               LEDConfigs ledConf = new LEDConfigs()
+                               .withStripType(StripTypeValue.GRB)
+                               .withLossOfSignalBehavior(LossOfSignalBehaviorValue.KeepRunning)
+                               .withBrightnessScalar(1);
+
+               CANdleFeaturesConfigs featureConf = new CANdleFeaturesConfigs()
+                               .withEnable5VRail(Enable5VRailValue.Enabled) // Turns off LEDs
+                               .withStatusLedWhenActive(StatusLedWhenActiveValue.Disabled)
+                               .withVBatOutputMode(VBatOutputModeValue.On);
+
+               configurator.apply(featureConf);
+               configurator.apply(ledConf);
+
+               var alliance = DriverStation.getAlliance();
+               if (alliance.isEmpty()) {
+                       color = Color.kWhite;
+               } else if (alliance.get() == Alliance.Red) {
+                       color = Color.kRed;
+               } else if (alliance.get() == Alliance.Blue) {
+                       color = Color.kBlue;
+               } else {
+                       color = Color.kWhite;
+               }
+
+               setStatic();
+
+               System.out.println("CANdle features: " + featureConf + ", LED config: " + ledConf);
+       }
+
+       private boolean flippy = true;
+
+       @Override
+       public void periodic() {
+               if (underSecsToFlip(5.0) && flippy) {
+                       setStrobe();
+                       flippy = false;
+               } else if (!underSecsToFlip(5.0) && !flippy) {
+                       setStatic();
+                       flippy = true;
+               }
+       }
+
+       private void setStrobe() {
+               candle.setControl(new StrobeAnimation(8, 8 + stripLength).withFrameRate(4).withColor(new RGBWColor(color)));
+       }
+
+       private void setStatic() {
+               candle.setControl(new SolidColor(8, 8 + stripLength).withColor(new RGBWColor(color)));
+       }
+
+       private boolean underSecsToFlip(double secs) {
+               Optional<Double> timeToActive = HubActive.timeToActive();
+               Optional<Double> timeToInactive = HubActive.timeToInactive();
+
+               if (timeToActive.isEmpty() && timeToInactive.isEmpty()) {
+                       return false;
+               } else if (timeToActive.isPresent()) {
+                       return (timeToActive.get() <= secs) ? true : false;
+
+               } else if (timeToInactive.isPresent()) {
+                       return (timeToInactive.get() <= secs) ? true : false;
+               } else {
+                       return false;
+               }
+       }
+}