From 48cc78c7d17f0b514889602ee1811d4404c89dbb Mon Sep 17 00:00:00 2001 From: Ethan Mortensen Date: Tue, 24 Feb 2026 16:24:29 -0800 Subject: [PATCH] almost everything works --- src/main/java/frc/robot/RobotContainer.java | 5 +- .../commands/led_comm/LEDDefaultCommand.java | 22 ++++---- .../java/frc/robot/subsystems/LED/LED.java | 50 +++++++++++++++---- 3 files changed, 56 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d25756e..802048f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,6 +11,7 @@ import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.PS5Controller; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -66,6 +67,7 @@ public class RobotContainer { private Operator operator = null; private LinearClimb linearClimb = null; private LED led = null; + private PS5Controller controller = null; // Auto Command selection private final SendableChooser autoChooser = new SendableChooser<>(); @@ -89,7 +91,8 @@ public class RobotContainer { case TestBed2: led = new LED(); - led.setDefaultCommand(new LEDDefaultCommand(led)); + controller = new PS5Controller(Constants.DRIVER_JOY); + led.setDefaultCommand(new LEDDefaultCommand(led, controller)); break; default: 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 e67f84f..ccecb7e 100644 --- a/src/main/java/frc/robot/commands/led_comm/LEDDefaultCommand.java +++ b/src/main/java/frc/robot/commands/led_comm/LEDDefaultCommand.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.PS5Controller; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.Constants; import frc.robot.subsystems.LED.LED; import frc.robot.subsystems.drivetrain.Drivetrain; // import frc.robot.subsystems.outtake.Outtake; // TODO: Outtake subsystem not implemented on current robot @@ -21,17 +22,20 @@ public class LEDDefaultCommand extends Command { - public LEDDefaultCommand(LED led) { + public LEDDefaultCommand(LED led, PS5Controller controller) { this.led = led; + this.controller = controller; // this.outtake = outtake; addRequirements(led); } @Override public void execute() { + controller = new PS5Controller(Constants.DRIVER_JOY); + double matchTime = DriverStation.getMatchTime(); String gameData = DriverStation.getGameSpecificMessage(); - // if (vision.oneCameraDisconnected()) { + // if (vision.oneCameraDisconnected() || DriverStation.isJoystickConnected(Constants.DRIVER_JOY)) { // // flash if camera disconnected // led.setStrobeLights(255, 100, 0); // } else @@ -59,22 +63,20 @@ public class LEDDefaultCommand extends Command { } else if ((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); - } else if ((gameData.equals("B") && matchTime <= 130 && matchTime >= 105) || (gameData.equals("B") && matchTime <= 80 && matchTime >= 55)) { + } else if ((!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); - } else if ((gameData.equals("R") && matchTime <= 105 && matchTime >= 80) || (gameData.equals("R") && matchTime <= 55 && matchTime >= 30)) { + } else if ((!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); } else if (allianceIsRed) { // Red alliance - // TODO need to fix 2 color wave - //led.setTwoColorWave(255, 0, 0, 255, 255, 255); - led.setLEDs(255, 0, 0); + led.setTwoColorWave(255, 0, 0, 255, 255, 255); + // led.setLEDs(255, 0, 0); } else { // Blue alliance - // led.setTwoColorWave(0, 0, 255, 255, 255, 255); - // TODO remake 2 color wave - led.setLEDs(0, 0, 255); + led.setTwoColorWave(0, 0, 255, 255, 255, 255); + // led.setLEDs(0, 0, 255); } } diff --git a/src/main/java/frc/robot/subsystems/LED/LED.java b/src/main/java/frc/robot/subsystems/LED/LED.java index 8734e6a..7ea78c3 100644 --- a/src/main/java/frc/robot/subsystems/LED/LED.java +++ b/src/main/java/frc/robot/subsystems/LED/LED.java @@ -25,6 +25,12 @@ public class LED extends SubsystemBase { 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); @@ -121,13 +127,45 @@ public class LED extends SubsystemBase { * @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) { + 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. - * TODO: Implement actual strobe animation * * @param red Red value (0-255) * @param green Green value (0-255) @@ -143,12 +181,4 @@ public class LED extends SubsystemBase { counter = 0; } } - - /** - * Sets defense lights pattern. - * TODO: Implement actual defense lights pattern - */ - public void defenseLights() { - - } } -- 2.39.5