added LED commands

copy pasted them from previous LED code

diff --git a/src/main/java/frc/robot/commands/led_comm/ChangeModeCommand.java b/src/main/java/frc/robot/commands/led_comm/ChangeModeCommand.java
new file mode 100644 (file)
index 0000000..3c738ab
--- /dev/null
+++ b/src/main/java/frc/robot/commands/led_comm/ChangeModeCommand.java
@@ -0,0 +1,35 @@
+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);
+    }
+    public void initialize(){
+        mode++;
+    }
+    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;
+        }
+    }
+    public boolean isFinished(){
+        return false;
+    }
+    public void end(){
+
+    }
+}

diff --git a/src/main/java/frc/robot/commands/led_comm/DefenseLightsCommand.java b/src/main/java/frc/robot/commands/led_comm/DefenseLightsCommand.java
new file mode 100644 (file)
index 0000000..ca2ab4f
--- /dev/null
+++ b/src/main/java/frc/robot/commands/led_comm/DefenseLightsCommand.java
@@ -0,0 +1,47 @@
+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);
+    }
+
+    public void initialize(){
+        counter = 0;
+    }
+
+    public void execute(){
+        counter++;
+
+        if(counter == 1){
+            //setLEDs(255, 0, 0);
+            led.alternate(255, 0, 0, 0, 0, 255, 5, startOffset, length);
+        }else if(counter == 20){
+            //setLEDs(0, 0, 255);
+            led.alternate(0, 0, 255, 255, 0, 0, 5, startOffset, length);
+        }
+        if(counter >= 40){
+            counter = 0;
+        }
+    
+    }
+
+    public boolean isFinished(){
+        return false;
+    }
+
+    public void end(boolean interrupted){
+
+    }
+}

diff --git a/src/main/java/frc/robot/commands/led_comm/LEDDefaultCommand.java b/src/main/java/frc/robot/commands/led_comm/LEDDefaultCommand.java
new file mode 100644 (file)
index 0000000..e54cf26
--- /dev/null
+++ b/src/main/java/frc/robot/commands/led_comm/LEDDefaultCommand.java
@@ -0,0 +1,68 @@
+package frc.robot.commands.led_comm;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.LED.LED;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.subsystems.outtake.Outtake;
+import frc.robot.util.Vision.Vision;
+
+public class LEDDefaultCommand extends Command {
+    private Vision vision;
+    private LED led;
+    private Outtake outtake;
+    private Drivetrain drivetrain;
+    //TODO: change this to actual climb coordinate
+    private double climbYCoordinate = 1.08;
+    private boolean allianceIsRed = DriverStation.getAlliance().get() == DriverStation.Alliance.Red;
+
+    public LEDDefaultCommand(LED led, Outtake outtake, Drivetrain drivetrain, Vision vision){
+        this.led = led;
+        this.outtake = outtake;
+        this.drivetrain = drivetrain;
+        this.vision = vision;
+
+        addRequirements(led);
+    }
+
+    @Override
+    public void execute(){
+        if (climbAligned()){
+            //When aligned to climb
+            led.setTwoColorWave(255, 0, 100, 100, 0, 255);
+        }else if(vision.oneCameraDisconnected()){
+            //flash if camera disconnected
+            led.setStrobeLights(255, 0, 0);
+        }else if (playingDefense()){
+            //When playing defense
+            led.defenseLights();
+        }
+        else if (outtake.coralLoaded()){
+            //When coral detected
+            led.setLEDs(0, 255, 0);
+        }else if(allianceIsRed){
+            //Red alliance
+            led.setTwoColorWave(255, 0, 0, 255, 255, 255);
+        }else{
+            //Blue alliance
+            led.setTwoColorWave(0, 0, 255, 255, 255, 255);
+        }
+    }
+
+    private boolean climbAligned(){
+        double yCoordinate = drivetrain.getPose().getY();
+        return Math.abs(yCoordinate - climbYCoordinate) < 0.03;
+    }
+
+    private boolean playingDefense(){
+        double xCoordinate = drivetrain.getPose().getX();
+        double xCoordinateHalfway = 50;
+        if (allianceIsRed){
+            return xCoordinate > xCoordinateHalfway;
+        }
+        else if (!allianceIsRed) {
+            return xCoordinate < xCoordinateHalfway;
+        }
+        return false;
+    }
+}

diff --git a/src/main/java/frc/robot/commands/led_comm/LEDSensorCommand.java b/src/main/java/frc/robot/commands/led_comm/LEDSensorCommand.java
new file mode 100644 (file)
index 0000000..e5d88a1
--- /dev/null
+++ b/src/main/java/frc/robot/commands/led_comm/LEDSensorCommand.java
@@ -0,0 +1,31 @@
+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;
+
+public class LEDSensorCommand extends Command{
+    private LED led;
+    private Sensor sensor;
+    private int alliance = 0;
+
+    public LEDSensorCommand(LED led, Sensor sensor, int alliance){
+        this.led = led;
+        this.sensor = sensor;
+
+        addRequirements(led, sensor);
+    }
+
+    @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);
+        }
+    }
+
+}

diff --git a/src/main/java/frc/robot/commands/led_comm/PaintCommand.java b/src/main/java/frc/robot/commands/led_comm/PaintCommand.java
new file mode 100644 (file)
index 0000000..6764019
--- /dev/null
+++ b/src/main/java/frc/robot/commands/led_comm/PaintCommand.java
@@ -0,0 +1,30 @@
+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);
+    }
+    public void initialize(){
+        led.setSection(0, 0, 255, start, end);
+    }
+    public void execute(){
+        led.setTwoColorWave(30, 0, 80, 255, 255, 255);
+    }
+    public boolean isFinished(){
+        return false;
+    }
+    public void end(boolean interrupted){
+
+    }
+}

diff --git a/src/main/java/frc/robot/commands/led_comm/TurnOffLEDsCommand.java b/src/main/java/frc/robot/commands/led_comm/TurnOffLEDsCommand.java
new file mode 100644 (file)
index 0000000..8a91433
--- /dev/null
+++ b/src/main/java/frc/robot/commands/led_comm/TurnOffLEDsCommand.java
@@ -0,0 +1,25 @@
+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);
+    }
+    public void initialize(){
+        led.setLEDs(0, 0, 0);
+    }
+    public void execute(){
+
+    }
+    public boolean isFinished(){
+        return false;
+    }
+    public void end(boolean interrupted){
+
+    }
+}