+++ /dev/null
-package frc.robot.commands.led_comm;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.LED.LED;
-
-public class ChangeModeCommand extends Command {
- private int mode;
- private LED led;
-
- public ChangeModeCommand(int mode, LED led) {
- this.mode = mode;
- this.led = led;
-
- addRequirements(led);
- }
-
- @Override
- public void initialize() {
- mode++;
- }
-
- @Override
- public void execute() {
- if (mode == 0) {
- led.setLEDs(0, 0, 0);
- } else if (mode == 1) {
- led.setLEDs(0, 130, 12);
- } else if (mode == 2) {
- led.setLEDs(255, 255, 255);
- } else {
- mode = 0;
- }
- }
-
- @Override
- public boolean isFinished() {
- return false;
- }
-
- @Override
- public void end(boolean interrupted) {
-
- }
-}
+++ /dev/null
-package frc.robot.commands.led_comm;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.LED.LED;
-
-public class DefenseLightsCommand extends Command {
- private int counter;
- private int startOffset;
- private int length;
- private LED led;
-
- public DefenseLightsCommand(LED led, int startOffset, int length) {
- this.led = led;
- this.startOffset = startOffset;
- this.length = length;
-
- addRequirements(led);
- }
-
- @Override
- public void initialize() {
- counter = 0;
- }
-
- @Override
- public void execute() {
- counter++;
-
- if (counter == 1) {
- led.alternate(255, 0, 0, 0, 0, 255, 5, startOffset, length);
- } else if (counter == 20) {
- led.alternate(0, 0, 255, 255, 0, 0, 5, startOffset, length);
- }
- if (counter >= 40) {
- counter = 0;
- }
- }
-
- @Override
- public boolean isFinished() {
- return false;
- }
-
- @Override
- public void end(boolean interrupted) {
-
- }
-}
+++ /dev/null
-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 frc.robot.util.HubActive;
-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;
- addRequirements(led);
- }
-
- @Override
- public void execute() {
- double matchTime = DriverStation.getMatchTime();
- String gameData = DriverStation.getGameSpecificMessage();
- // if (vision.oneCameraDisconnected() ||
- // DriverStation.isJoystickConnected(Constants.DRIVER_JOY)) {
- // // flash if camera disconnected
- // led.setStrobeLights(255, 100, 0);
- // controller.endRumble();
- // } else
- 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);
- 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 (allianceIsRed && HubActive.isHubActive()){
- led.setTwoColorWave(255, 0, 0, 255, 255, 255);
- counter = 0;
- } else if (!allianceIsRed && HubActive.isHubActive()) {
- led.setTwoColorWave(0, 0, 255, 255, 255, 255);
- counter = 0;
- } else if (!HubActive.isHubActive()){
- led.setLEDs(0, 0, 0);
- counter = 0;
- }
-
- // previous LED Hub code
-
- // } 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;
- }
-
- @Override
- public boolean isFinished() {
- return false;
- }
-
- private boolean fiveSecondsBeforeChange() {
- double time = DriverStation.getMatchTime();
- if ((time <= 135 && time >= 130) || (time <= 110 && time >= 105) || (time <= 85 && time >= 80)
- || (time <= 60 && time >= 55) || (time <= 35 && time >= 30)) {
- return true;
- }
- return false;
- }
-}
+++ /dev/null
-package frc.robot.commands.led_comm;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.LED.LED;
-// import frc.robot.subsystems.LaserCAN.Sensor; // TODO: LaserCAN Sensor subsystem not implemented on current robot
-
-public class LEDSensorCommand extends Command {
- private LED led;
- // private Sensor sensor;
- private int alliance = 0;
-
- public LEDSensorCommand(LED led, int alliance) {
- this.led = led;
- // this.sensor = sensor;
-
- addRequirements(led);
- }
-
- @Override
- public void execute() {
- // if (sensor.detected()){
- // led.setLEDs(0, 255, 0);
- // }
- // else
- if (alliance == 0) {
- led.setLEDs(0, 0, 255);
- } else {
- led.setLEDs(255, 0, 0);
- }
- }
-
- @Override
- public boolean isFinished() {
- return false;
- }
-
-}
+++ /dev/null
-package frc.robot.commands.led_comm;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.LED.LED;
-
-public class PaintCommand extends Command {
- private int start;
- private int end;
- private LED led;
-
- public PaintCommand(LED led, int start, int end) {
- this.led = led;
- this.end = end;
- this.start = start;
-
- addRequirements(led);
- }
-
- @Override
- public void initialize() {
- led.setSection(0, 0, 255, start, end);
- }
-
- @Override
- public void execute() {
- led.setTwoColorWave(30, 0, 80, 255, 255, 255);
- }
-
- @Override
- public boolean isFinished() {
- return false;
- }
-
- @Override
- public void end(boolean interrupted) {
-
- }
-}
+++ /dev/null
-package frc.robot.commands.led_comm;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.LED.LED;
-
-public class TurnOffLEDsCommand extends Command {
- private LED led;
-
- public TurnOffLEDsCommand(LED led) {
- this.led = led;
-
- addRequirements(led);
- }
-
- @Override
- public void initialize() {
- led.setLEDs(0, 0, 0);
- }
-
- @Override
- public void execute() {
-
- }
-
- @Override
- public boolean isFinished() {
- return false;
- }
-
- @Override
- public void end(boolean interrupted) {
-
- }
-}
+++ /dev/null
-package frc.robot.subsystems.LED;
-
-import com.ctre.phoenix6.configs.CANdleConfigurator;
-import com.ctre.phoenix6.configs.CANdleFeaturesConfigs;
-import com.ctre.phoenix6.configs.LEDConfigs;
-import com.ctre.phoenix6.controls.ControlRequest;
-import com.ctre.phoenix6.controls.SolidColor;
-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.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.Constants;
-import frc.robot.constants.IdConstants;
-
-public class LED extends SubsystemBase {
-
- private CANdle candle;
- public static final int stripLength = 67;
- private double counter = 0;
-
- private double waveOffset = 0;
- private final double waveSpeed = 0.08;
- private final double waveFrequency = 0.25;
- private double period = 10;
- private double midLine = 127.5;
-
- // Constructor
- public LED() {
- candle = new CANdle(IdConstants.CANDLE_ID, Constants.CANIVORE_SUB);
- 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);
-
- System.out.println("CANdle features: " + featureConf + ", LED config: " + ledConf);
- }
-
- @Override
- public void periodic() {
- }
-
- /**
- * Sets the color of all the LEDs.
- *
- * @param red Red value (0-255)
- * @param green Green value (0-255)
- * @param blue Blue value (0-255)
- */
- public void setLEDs(int red, int green, int blue) {
- setSection(red, green, blue, 0, stripLength);
- }
-
- /**
- * Sets an animation for the LEDs.
- *
- * @param animation The animation object (e.g., RainbowAnimation,
- * StrobeAnimation, etc.)
- */
- public void animate(ControlRequest animation) {
- candle.setControl(animation);
- }
-
- /**
- * Sets the color of a specific section of LEDs.
- *
- * @param r Red value (0-255)
- * @param g Green value (0-255)
- * @param b Blue value (0-255)
- * @param start Start index of the section
- * @param end End index of the section
- */
- public void setSection(int r, int g, int b, int start, int end) {
- SolidColor request = new SolidColor(start, end)
- .withColor(new RGBWColor(r, g, b));
- candle.setControl(request);
- }
-
- /**
- * Creates an alternating pattern of two colors across the LEDs.
- *
- * @param r1 Red value of the first color (0-255)
- * @param g1 Green value of the first color (0-255)
- * @param b1 Blue value of the first color (0-255)
- * @param r2 Red value of the second color (0-255)
- * @param g2 Green value of the second color (0-255)
- * @param b2 Blue value of the second color (0-255)
- * @param size Size of each color block
- * @param offset Offset for the starting position of the pattern
- * @param total Total number of LEDs
- */
- public void alternate(int r1, int g1, int b1, int r2, int g2, int b2, int size, int offset, int total) {
- for (int i = -offset; i < total; i += size) {
- boolean color2 = ((i - offset) / size) % 2 == 0;
- if (color2) {
- setSection(r2, g2, b2, i, i + size - 1);
- } else {
- setSection(r1, g1, b1, i, i + size - 1);
- }
- }
- }
-
- /**
- * Creates a two-color wave pattern across the LEDs.
- * TODO: Implement actual wave animation
- *
- * @param r1 Red value of the first color (0-255)
- * @param g1 Green value of the first color (0-255)
- * @param b1 Blue value of the first color (0-255)
- * @param r2 Red value of the second color (0-255)
- * @param g2 Green value of the second color (0-255)
- * @param b2 Blue value of the second color (0-255)
- */
-
- public void setTwoColorWave(int r1, int g1, int b1, int r2, int g2, int b2) {
- for (int i = 0; i < stripLength; i++) {
-
- double wave = (Math.sin(i * waveFrequency + waveOffset) + 1) / 2.0;
- double inverseBias = 5; // higher = more color 2
- wave = 1 - Math.pow(1 - wave, inverseBias);
-
- int r = (int) (r1 * wave + r2 * (1 - wave));
- int g = (int) (g1 * wave + g2 * (1 - wave));
- int b = (int) (b1 * wave + b2 * (1 - wave));
- setSection(r, g, b, i, i + 1);
- }
-
- waveOffset += waveSpeed;
- }
- // public void setTwoColorWave(int r1, int g1, int b1, int r2, int g2, int b2) {
- // for (int i = 0; i < stripLength; i++) {
-
- // double wave = 127.5 * Math.sin( ((2*Math.PI)/period) * (i + waveOffset)) +
- // midLine;
- // double inverseBias = 5; // higher = more color 2
- // wave = 1 - Math.pow(1 - wave, inverseBias);
-
- // //int r = (int)(r1 * wave + r2 * (1 - wave));
- // //int g = (int)(g1 * wave + g2 * (1 - wave));
- // //int b = (int)(b1 * wave + b2 * (1 - wave));
- // // int b = (int)wave;
- // int b = 255;
- // int g = (int)(127.5 * Math.sin( ((2*Math.PI)/period) * (i + waveOffset + 5))
- // + midLine);
- // //int g = r;
-
- // setSection(0, g, b, i, i + 1);
- // }
-
- // waveOffset += waveSpeed;
- // }
-
- /**
- * Sets strobe lights effect.
- *
- * @param red Red value (0-255)
- * @param green Green value (0-255)
- * @param blue Blue value (0-255)
- */
- public void setStrobeLights(int red, int green, int blue) {
- counter++;
- if (counter == 1) {
- setLEDs(red, green, blue);
- } else if (counter == 20) {
- setLEDs(0, 0, 0);
- } else if (counter >= 40) {
- counter = 0;
- }
- }
-}