Simpler led functions

build.gradle

@@ -2,7 +2,7 @@ import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO;
 
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.4.2"
+    id "edu.wpi.first.GradleRIO" version "2023.4.3"
     id "com.peterabeles.gversion" version "1.10"
 }
 

src/main/java/frc/robot/RobotContainer.java

@@ -20,8 +20,9 @@
 import frc.robot.subsystems.drivetrain.commands.JoystickDrive;
 import frc.robot.subsystems.gripper.Gripper;
 import frc.robot.subsystems.gyroscope.Gyroscope;
-import frc.robot.subsystems.intake.BeamBreaker;
+import frc.robot.subsystems.intake.ProximitySensor;
 import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.intake.commands.ProximitySensorDefualtCommand;
 import frc.robot.subsystems.intake.commands.HoldIntakeInPlace;
 import frc.robot.subsystems.leds.Leds;
 import frc.robot.subsystems.vision.Limelight;
@@ -38,15 +39,15 @@ public class RobotContainer {
     private final Limelight limelight = Limelight.getInstance();
     private final Intake intake = Intake.getInstance();
     private final Gripper gripper = Gripper.getInstance();
-    private final BeamBreaker beamBreaker = BeamBreaker.getInstance();
+    private final ProximitySensor proximitySensor = ProximitySensor.getInstance();
     private final XboxController xboxController = new XboxController(0);
     private final Joystick leftJoystick = new Joystick(1);
     private final Joystick rightJoystick = new Joystick(2);
     private final JoystickButton a = new JoystickButton(xboxController, XboxController.Button.kA.value);
     private final JoystickButton b = new JoystickButton(xboxController, XboxController.Button.kB.value);
     private final JoystickButton y = new JoystickButton(xboxController, XboxController.Button.kY.value);
     private final JoystickButton x = new JoystickButton(xboxController, XboxController.Button.kX.value);
-    private final JoystickButton kBack = new JoystickButton(xboxController, XboxController.Button.kBack.value);
+    private final JoystickButton back = new JoystickButton(xboxController, XboxController.Button.kBack.value);
     private final JoystickButton rb = new JoystickButton(xboxController, XboxController.Button.kRightBumper.value);
     private final JoystickButton lb = new JoystickButton(xboxController, XboxController.Button.kLeftBumper.value);
     private final Trigger xboxRightTrigger = new Trigger(() -> xboxController.getRightTriggerAxis() > 0.2);
@@ -103,6 +104,7 @@ private void configureDefaultCommands() {
         );
         arm.setDefaultCommand(new ArmXboxControl(xboxController));
         intake.setDefaultCommand(new HoldIntakeInPlace());
+        leds.setDefaultCommand(new ProximitySensorDefualtCommand());
     }
 
     private void configureButtonBindings() {
@@ -124,9 +126,13 @@ private void configureButtonBindings() {
         rb.whileTrue(new ArmAxisControl(1, 0.02, 0)
                 .until(() -> gripper.getDistance() < ArmConstants.FEEDER_DISTANCE));
 
+        leftJoystickTopBottom.onTrue(new InstantCommand(leds::toggleRainbow));
+
 //        leftJoystickTrigger.whileTrue(new TurnDrivetrain(leftJoystick));
-        upPOV.whileTrue(new ArmAxisControl(0.33, 0.02, 0, 0, 0));
-        downPOV.whileTrue(new ArmAxisControl(0.33, -0.02, 0, 0, 0));
+        leftPOV.whileTrue(new ArmAxisControl(0.33, 0.02, 0, 0, 0));
+        rightPOV.whileTrue(new ArmAxisControl(0.33, -0.02, 0, 0, 0));
+        upPOV.whileTrue(new ArmAxisControl(0.33, 0, 0.02, 0, 0));
+        downPOV.whileTrue(new ArmAxisControl(0.33, 0, -0.02, 0, 0));
 
 //        leftBottom.onTrue(new Engage(true, true));
 //        leftTop.onTrue(new Engage(false, true));

src/main/java/frc/robot/autonomous/AutonUpperScoring.java

@@ -2,8 +2,8 @@
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.commandgroups.UpperScoring;
-import frc.robot.subsystems.leds.PurpleLed;
-import frc.robot.subsystems.leds.YellowLed;
+import frc.robot.subsystems.leds.command.PurpleLed;
+import frc.robot.subsystems.leds.command.YellowLed;
 
 public class AutonUpperScoring extends SequentialCommandGroup {
 

src/main/java/frc/robot/autonomous/paths/BumperConeCubeHigh.java

@@ -12,7 +12,7 @@
 import frc.robot.subsystems.gripper.Gripper;
 import frc.robot.subsystems.gyroscope.Gyroscope;
 import frc.robot.subsystems.intake.commands.Retract;
-import frc.robot.subsystems.leds.PurpleLed;
+import frc.robot.subsystems.leds.command.PurpleLed;
 
 import static frc.robot.subsystems.intake.commands.Retract.Mode.DOWN;
 

src/main/java/frc/robot/autonomous/paths/BumperConeHighTakeCubeEngage.java

@@ -11,7 +11,7 @@
 import frc.robot.subsystems.gripper.Gripper;
 import frc.robot.subsystems.gyroscope.Gyroscope;
 import frc.robot.subsystems.intake.commands.Retract;
-import frc.robot.subsystems.leds.PurpleLed;
+import frc.robot.subsystems.leds.command.PurpleLed;
 
 import static frc.robot.subsystems.intake.commands.Retract.Mode.DOWN;
 

src/main/java/frc/robot/autonomous/paths/FeederConeCubeHigh.java

@@ -12,7 +12,7 @@
 import frc.robot.subsystems.gripper.Gripper;
 import frc.robot.subsystems.gyroscope.Gyroscope;
 import frc.robot.subsystems.intake.commands.Retract;
-import frc.robot.subsystems.leds.PurpleLed;
+import frc.robot.subsystems.leds.command.PurpleLed;
 
 import static frc.robot.subsystems.intake.commands.Retract.Mode.DOWN;
 

src/main/java/frc/robot/autonomous/paths/FeederConeHighTakeCubeEngage.java

@@ -11,7 +11,7 @@
 import frc.robot.subsystems.gripper.Gripper;
 import frc.robot.subsystems.gyroscope.Gyroscope;
 import frc.robot.subsystems.intake.commands.Retract;
-import frc.robot.subsystems.leds.PurpleLed;
+import frc.robot.subsystems.leds.command.PurpleLed;
 
 import static frc.robot.subsystems.intake.commands.Retract.Mode.DOWN;
 

src/main/java/frc/robot/autonomous/paths/MiddleConeHighCommunityEngage.java

@@ -18,7 +18,7 @@
 import frc.robot.subsystems.gripper.Gripper;
 import frc.robot.subsystems.gyroscope.Gyroscope;
 import frc.robot.subsystems.intake.commands.Retract;
-import frc.robot.subsystems.leds.PurpleLed;
+import frc.robot.subsystems.leds.command.PurpleLed;
 import frc.robot.utils.controllers.DieterController;
 
 import static frc.robot.subsystems.intake.commands.Retract.Mode.DOWN;

src/main/java/frc/robot/autonomous/paths/MiddleConeHighCubeEngage.java

@@ -20,7 +20,7 @@
 import frc.robot.subsystems.gripper.Gripper;
 import frc.robot.subsystems.gyroscope.Gyroscope;
 import frc.robot.subsystems.intake.commands.Retract;
-import frc.robot.subsystems.leds.PurpleLed;
+import frc.robot.subsystems.leds.command.PurpleLed;
 import frc.robot.utils.controllers.DieterController;
 
 import static frc.robot.subsystems.intake.commands.Retract.Mode.DOWN;

src/main/java/frc/robot/commandgroups/FeederBlink.java

@@ -0,0 +1,25 @@
+package frc.robot.commandgroups;
+
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.subsystems.arm.ArmConstants;
+import frc.robot.subsystems.gripper.Gripper;
+import frc.robot.subsystems.leds.LedConstants;
+import frc.robot.subsystems.leds.Leds;
+
+public class FeederBlink extends SequentialCommandGroup {
+
+    public FeederBlink(){
+        Gripper gripper = Gripper.getInstance();
+        Leds leds = Leds.getInstance();
+        addRequirements(leds);
+        addCommands(
+                new ConditionalCommand(
+                        new InstantCommand(() ->leds.setBlink(true, LedConstants.kGalaxiaBlue, LedConstants.FAST_BLINK_TIME)),
+                        new InstantCommand(() -> leds.setBlink(false)),
+                        () -> gripper.getDistance() < ArmConstants.FEEDER_DISTANCE && gripper.isOpen()
+                )
+        );
+    }
+}

src/main/java/frc/robot/subsystems/arm/ArmConstants.java

@@ -86,4 +86,5 @@ public class ArmConstants {
     public static final ArmLinearProfile.Waypoint ARM_OUT_OF_ROBOT_POINT2 = new ArmLinearProfile.Waypoint(0, 0, 0, 0);
 
     public static final double FEEDER_DISTANCE = 30;
+    public static final double FEEDER_MINIMUM_DISTANCE = 18.1;
 }

src/main/java/frc/robot/subsystems/drivetrain/SwerveConstants.java

@@ -6,7 +6,7 @@
 
 public class SwerveConstants {
     public static final double TICKS_PER_ROTATION = 2048;
-    public static final int[] OFFSETS = {12317, 6362, 15034, 10807};
+    public static final int[] OFFSETS = {20155, 6155, 7072, 10786};
 
     public static final double DRIVETRAIN_TRACK_WIDTH_METERS = 0.51594;
     public static final double DRIVETRAIN_WHEELBASE_METERS = 0.66594;

src/main/java/frc/robot/subsystems/intake/BeamBreaker.java → src/main/java/frc/robot/subsystems/intake/ProximitySensor.java

@@ -5,17 +5,17 @@
 import frc.robot.subsystems.LoggedSubsystem;
 
 
-public class BeamBreaker extends LoggedSubsystem<BeamBreakerLoggedInputs> {
-    private static BeamBreaker INSTANCE;
+public class ProximitySensor extends LoggedSubsystem<ProximitySensorLoggedInputs> {
+    private static ProximitySensor INSTANCE;
     private final DigitalInput beam = new DigitalInput(Ports.Intake.BEAM_BREAKER_SENSOR);
 
-    private BeamBreaker() {
-        super(new BeamBreakerLoggedInputs());
+    private ProximitySensor() {
+        super(new ProximitySensorLoggedInputs());
     }
 
-    public static BeamBreaker getInstance() {
+    public static ProximitySensor getInstance() {
         if (INSTANCE == null) {
-            INSTANCE = new BeamBreaker();
+            INSTANCE = new ProximitySensor();
         }
         return INSTANCE;
     }
@@ -33,11 +33,11 @@ public boolean isBeamBlocked() {
      */
     @Override
     public void updateInputs() {
-        loggerInputs.beamBreakerState = isBeamBlocked();
+        loggerInputs.ProximitySensorState = isBeamBlocked();
     }
 
     @Override
     public String getSubsystemName() {
-        return "BeamBreaker";
+        return "ProximitySensor";
     }
 }

src/main/java/frc/robot/subsystems/intake/BeamBreakerLoggedInputs.java → src/main/java/frc/robot/subsystems/intake/ProximitySensorLoggedInputs.java

@@ -3,22 +3,22 @@
 import org.littletonrobotics.junction.LogTable;
 import org.littletonrobotics.junction.inputs.LoggableInputs;
 
-public class BeamBreakerLoggedInputs implements LoggableInputs {
-    public boolean beamBreakerState = true;
+public class ProximitySensorLoggedInputs implements LoggableInputs {
+    public boolean ProximitySensorState = true;
 
     /**
      * Implement the variables into the logger.
      */
     @Override
     public void toLog(LogTable table) {
-        table.put("beamBreakerState", beamBreakerState);
+        table.put("ProximitySensorState", ProximitySensorState);
     }
 
     /**
      * Update the variables value from the logger.
      */
     @Override
     public void fromLog(LogTable table) {
-        beamBreakerState = table.getBoolean("beamBreakerState", beamBreakerState);
+        ProximitySensorState = table.getBoolean("ProximitySensorState", ProximitySensorState);
     }
 }

src/main/java/frc/robot/subsystems/intake/commands/ProximitySensorDefualtCommand.java

@@ -0,0 +1,30 @@
+package frc.robot.subsystems.intake.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.arm.ArmConstants;
+import frc.robot.subsystems.gripper.Gripper;
+import frc.robot.subsystems.intake.ProximitySensor;
+import frc.robot.subsystems.leds.LedConstants;
+import frc.robot.subsystems.leds.Leds;
+
+
+public class ProximitySensorDefualtCommand extends CommandBase {
+    private final ProximitySensor proximitySensor = ProximitySensor.getInstance();
+    private final Leds leds = Leds.getInstance();
+    private final Gripper gripper = Gripper.getInstance();
+
+    private boolean ProximitySensorState = proximitySensor.isBeamBlocked();
+
+    public ProximitySensorDefualtCommand() {
+        this.ProximitySensorState = proximitySensor.isBeamBlocked();
+        addRequirements(proximitySensor, leds);
+    }
+
+    @Override
+    public void execute() {
+        if (proximitySensor.isBeamBlocked() || (gripper.getDistance() < ArmConstants.FEEDER_DISTANCE && gripper.isOpen() && gripper.getDistance() > ArmConstants.FEEDER_MINIMUM_DISTANCE))
+            leds.setBlink(true, LedConstants.kGalaxiaBlue, LedConstants.FAST_BLINK_TIME);
+        else leds.setBlink(false);
+    }
+
+}

src/main/java/frc/robot/subsystems/leds/LedConstants.java

@@ -1,5 +1,16 @@
 package frc.robot.subsystems.leds;
 
+import edu.wpi.first.wpilibj.util.Color;
+
 public final class LedConstants {
     public static final int LED_LENGTH = 19;
+
+    public static final Color kGalaxiaBlue = new Color(0, 156, 189);
+    public static final Color kPurple = Color.kPurple;
+    public static final Color kRed = Color.kRed;
+    public static final Color kGreen = Color.kGreen;
+
+    public static final double SLOW_BLINK_TIME = 0.3;
+    public static final double MID_BLINK_TIME = 0.25;
+    public static final double FAST_BLINK_TIME = 0.2;
 }

src/main/java/frc/robot/subsystems/leds/Leds.java

@@ -14,10 +14,13 @@ public class Leds extends SubsystemBase {
     public AddressableLED leds = new AddressableLED(Ports.Leds.LED);
     public AddressableLEDBuffer ledBuffer = new AddressableLEDBuffer(LedConstants.LED_LENGTH);
     private Color color = Color.kPurple;
-    private double blinkTime = 0;
+    private Color secondaryColor = new Color(0, 0, 0);
+    private double blinkTime = LedConstants.MID_BLINK_TIME;
     private boolean blink = false;
+    private boolean isRainbow = false;
+    private int rainbowHue = 0;
 
-    private Mode mode = Mode.ON;
+    private Mode mode = Mode.PRIMARY_COLOR;
 
     private Leds() {
         leds.setLength(ledBuffer.getLength());
@@ -43,6 +46,10 @@ public void setPurple() {
         color = Color.kPurple;
     }
 
+    public void setColor(Color color) {
+        this.color = color;
+    }
+
     public void setBlinkTime(double blinkTime) {
         this.blinkTime = blinkTime;
     }
@@ -51,6 +58,27 @@ public void setBlink(boolean blink) {
         this.blink = blink;
     }
 
+    public void setBlink(boolean blink, Color secondaryColor) {
+        this.blink = blink;
+        this.secondaryColor = secondaryColor;
+    }
+
+    public void setBlink(boolean blink, Color secondaryColor, double blinkTime) {
+        this.blink = blink;
+        this.secondaryColor = secondaryColor;
+        this.blinkTime = blinkTime;
+    }
+
+    public void toggleRainbow() {
+        if (!isRainbow) {
+            this.mode = Mode.ELSE;
+            this.isRainbow = true;
+        } else {
+            this.mode = Mode.PRIMARY_COLOR;
+            this.isRainbow = false;
+        }
+    }
+
     public void toggle() {
         if (color == Color.kYellow) {
             color = Color.kPurple;
@@ -67,31 +95,44 @@ public boolean inConeMode() {
     public void periodic() {
         if (blink) {
             if (timer.hasElapsed(blinkTime)) {
-                if (mode == Mode.ON) {
-                    mode = Mode.OFF;
+                if (mode == Mode.PRIMARY_COLOR) {
+                    mode = Mode.SECONDARY_COLOR;
                 } else {
-                    mode = Mode.ON;
+                    mode = Mode.PRIMARY_COLOR;
                 }
                 timer.reset();
             }
-        } else {
-            mode = Mode.ON;
+        } else if(mode != Mode.ELSE){
+            mode = Mode.PRIMARY_COLOR;
         }
 
-        if (mode == Mode.ON) {
+
+        if (mode == Mode.PRIMARY_COLOR) {
             for (int i = 0; i < ledBuffer.getLength(); i++) {
                 ledBuffer.setLED(i, color);
             }
             leds.setData(ledBuffer);
-        } else {
+        } else if (mode == Mode.SECONDARY_COLOR) {
             for (int i = 0; i < ledBuffer.getLength(); i++) {
-                ledBuffer.setLED(i, Color.kBlack);
+                ledBuffer.setLED(i, secondaryColor);
             }
             leds.setData(ledBuffer);
         }
+        if (isRainbow){
+            mode = Mode.ELSE;
+            System.out.println("before loop statement");
+            for (int i = 0; i < ledBuffer.getLength(); i++) {
+                ledBuffer.setHSV(i, rainbowHue, 255, 180);
+                rainbowHue += (180 / ledBuffer.getLength());
+                leds.setData(ledBuffer);
+                System.out.println("in-loop check statement");
+            rainbowHue%=180;
+            }
+            System.out.println("after loop statement");
+        }
     }
 
     private enum Mode {
-        OFF, ON
+        SECONDARY_COLOR, PRIMARY_COLOR, ELSE
     }
 }

src/main/java/frc/robot/subsystems/leds/PurpleLed.java → src/main/java/frc/robot/subsystems/leds/command/PurpleLed.java

@@ -1,6 +1,7 @@
-package frc.robot.subsystems.leds;
+package frc.robot.subsystems.leds.command;
 
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.leds.Leds;
 
 public class PurpleLed extends InstantCommand {