almost everything works

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index d25756e2225a5b4a3f91f95110ca9a99d8708acf..802048ffdb615e4b18464b722a56932a2544376b 100644 (file)
--- 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<Command> 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 e67f84f6e347f924198cf0e873467b63755817de..ccecb7e028ed1d9b44ecfd80c50afcf811ae23f8 100644 (file)
--- 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 8734e6a33c5634b8798aff42bdbb82f4d774c874..7ea78c37f8e4a1eb10029a252f433c52305f81f7 100644 (file)
--- 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() {
-
-       }
 }