Added a light strip subsystem

src/main/java/frc/robot/Robot.java

@@ -135,7 +135,9 @@ public void robotPeriodic() {
             SmartDashboard.putString("DeploymentState", robotContainer.getDeployer().getDeploymentState().toString());
             if (!Constants.TESTBED) {
                 Logger.recordOutput("MyPose", robotContainer.getDriveBase().getPose());
-                // Puts data on the elastic dashboard
+                    SmartDashboard.putNumber("Robot X", robotContainer.getDriveBase().getPose().getX());
+            SmartDashboard.putNumber("Robot Y", robotContainer.getDriveBase().getPose().getY());
+        // Puts data on the elastic dashboard
                 SmartDashboard.putString("Alliance Color", Robot.allianceColorString());
                 SmartDashboard.putBoolean("Hub Active?", hubActive());
             }

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

@@ -31,7 +31,10 @@
 import frc.robot.commands.intakeDeployment.ToggleDeployment;
 import frc.robot.commands.intakeDeployment.RunDeployer;
 import frc.robot.commands.intakeDeployment.SetDeploymentState;
+import frc.robot.commands.lightStrip.SetLed;
+import frc.robot.commands.lightStrip.SetLedFromShootingState;
 import frc.robot.commands.parallels.RunHopperAndFeeder;
+import frc.robot.commands.parallels.SetShootingStateAndLight;
 import frc.robot.autochooser.AutoChooser;
 import frc.robot.commands.angler.AimAngler;
 import frc.robot.commands.angler.DefaultAnglerControl;
@@ -58,6 +61,7 @@
 import frc.robot.subsystems.FeederSubsystem;
 import frc.robot.subsystems.GyroSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.LightStripSubsystem;
 import frc.robot.subsystems.TurretSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.ClimberSubsystem;
@@ -113,6 +117,7 @@ public class RobotContainer {
         private final ControllerSubsystem controllerSubsystem;
         private RobotVisualizer robotVisualizer = null;
         private final HopperSubsystem hopperSubsystem;
+        private final LightStripSubsystem lightStripSubsystem;
 
         private final TurretSubsystem turretSubsystem;
         private final IntakeDeployerSubsystem intakeDeployer;
@@ -146,7 +151,7 @@ public RobotContainer() {
                                 hopperSubsystem = new HopperSubsystem(HopperSubsystem.createRealIo());
                                 intakeDeployer = new IntakeDeployerSubsystem(IntakeDeployerSubsystem.createRealIo());
                                 turretSubsystem = new TurretSubsystem(TurretSubsystem.createRealIo());
-
+                               
 
 
                                 climberSubsystem = new ClimberSubsystem(ClimberSubsystem.createRealIo());
@@ -161,7 +166,7 @@ public RobotContainer() {
                                                 new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), swerveIMU) : null;
                             apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createRealIo(), drivebase, truster) : null;
                             controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null;
-
+                                 lightStripSubsystem = new LightStripSubsystem(drivebase, shootState);
             }
                         case REPLAY -> {
                                 // rollerSubsystem = new RollerSubsystem(RollerSubsystem.createMockIo());
@@ -174,11 +179,13 @@ public RobotContainer() {
                                 turretSubsystem = new TurretSubsystem(TurretSubsystem.createMockIo());
                                 shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createMockIo());
                                 intakeDeployer = new IntakeDeployerSubsystem(IntakeDeployerSubsystem.createMockIo());
-                // No GyroSubsystem in REPLAY for now
+
+                                // No GyroSubsystem in REPLAY for now
                 // create the drive subsystem with null gyro (use default json)
                 drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), null) : null;
                 apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createMockIo(), drivebase, truster) : null;
                 controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null;
+                                lightStripSubsystem = new LightStripSubsystem(drivebase, shootState);
 
             }
             case SIM -> {
@@ -195,11 +202,14 @@ public RobotContainer() {
                                 shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createSimIo(robotVisualizer));
                                 intakeDeployer = new IntakeDeployerSubsystem(
                                                 IntakeDeployerSubsystem.createSimIo(robotVisualizer));// No GyroSubsystem in REPLAY for now
-                // create the drive subsystem with null gyro (use default json)
+
+                                                // create the drive subsystem with null gyro (use default json)
                 drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), null) : null;
                 controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null;
                 apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createSimIo(truster,drivebase), drivebase, truster) : null;
-            }
+                                lightStripSubsystem = new LightStripSubsystem(drivebase, shootState);
+
+        }
 
                         default -> {
                                 throw new RuntimeException("Did not specify Robot Mode");
@@ -498,18 +508,20 @@ public void putShuffleboardCommands() {
                                         new SetDeploymentState(intakeDeployer, DeploymentState.DOWN));
                         SmartDashboard.putData(
                                         "intakedeployer/Deployment State: STOPPED",
-                        new SetDeploymentState(intakeDeployer, DeploymentState.STOPPED));
+                                        new SetDeploymentState(intakeDeployer, DeploymentState.STOPPED));
 
                 }
 
         //basic drive command
         if (!Constants.TESTBED) {
             Command driveDirectionTime = new DriveDirectionTime(drivebase, 0.1, 0.1, true, 1);
             SmartDashboard.putData("Drive Command", driveDirectionTime);
-            SmartDashboard.putData("Fake vision", new FakeVision(drivebase));
+            SmartDashboard.putData("Fake vision near trench", new FakeVision(drivebase, 4, 1));
+            SmartDashboard.putData("Fake vision away from trench", new FakeVision(drivebase, 1, 1));
             SmartDashboard.putData("AddApriltagReading", new AddApriltagReading(apriltagSubsystem, new ApriltagReading(0, 0, 0, 0, 0, 0, 0)));
             SmartDashboard.putData("AddGarbageReading", new AddGarbageReading(apriltagSubsystem));
             SmartDashboard.putData("AddTunedApriltagReading", new AddTunableApriltagReading(apriltagSubsystem));
+
         }
 
         }

src/main/java/frc/robot/commands/drive/FakeVision.java

@@ -14,15 +14,20 @@
 public class FakeVision extends LoggableCommand {
   /** Creates a new FakeVision. */
   private final SwerveSubsystem drivebase;
-  public FakeVision(SwerveSubsystem drivebase) {
+  private double x;
+  private double y;
+
+  public FakeVision(SwerveSubsystem drivebase, double x, double y) {
     // Use addRequirements() here to declare subsystem dependencies.
     this.drivebase = drivebase;
+    this.x = x;
+    this.y = y;
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    drivebase.addVisionMeasurement(new Pose2d(new Translation2d(-10, 2), new Rotation2d()));
+    drivebase.addVisionMeasurement(new Pose2d(new Translation2d(x, y), new Rotation2d()));
   }
 
   // Called every time the scheduler runs while the command is scheduled.

src/main/java/frc/robot/commands/lightStrip/SetLed.java

@@ -0,0 +1,80 @@
+package frc.robot.commands.lightStrip;
+
+import frc.robot.constants.enums.ShootingState;
+import frc.robot.constants.enums.Trench;
+import frc.robot.subsystems.LightStripSubsystem;
+import frc.robot.subsystems.swervedrive.SwerveSubsystem;
+import frc.robot.utils.BlinkinPattern;
+import frc.robot.utils.logging.commands.LoggableCommand;
+
+public class SetLed extends LoggableCommand{
+
+    private final LightStripSubsystem lightStrip;
+    private final SwerveSubsystem drivebase;
+    private ShootingState shootingState;
+    private double x;
+    private double y;
+
+    public SetLed(LightStripSubsystem lightStrip, SwerveSubsystem drivebase, ShootingState shootingState) {
+
+        this.lightStrip = lightStrip;
+        this.drivebase = drivebase;
+        this.shootingState = shootingState;
+        addRequirements(lightStrip);
+
+    }
+
+    @Override
+    public void initialize() {
+
+    }  
+
+    @Override
+    public void execute() {
+
+        x = drivebase.getPose().getX();
+        y = drivebase.getPose().getY();
+
+        // If statement checks if the robot is near the trench
+        if (x > Trench.RED_BOTTOM_LOWER.getX() && x < Trench.RED_BOTTOM_HIGHER.getX() && y > Trench.RED_BOTTOM_LOWER.getY() && y < Trench.RED_BOTTOM_HIGHER.getY() ||
+            x > Trench.RED_TOP_LOWER.getX() && x < Trench.RED_TOP_HIGHER.getX() && y > Trench.RED_TOP_LOWER.getY() && y < Trench.RED_TOP_HIGHER.getY() ||
+            x > Trench.BLUE_BOTTOM_LOWER.getX() && x < Trench.BLUE_BOTTOM_HIGHER.getX() && y > Trench.BLUE_BOTTOM_LOWER.getY() && y < Trench.BLUE_BOTTOM_HIGHER.getY() ||
+            x > Trench.BLUE_TOP_LOWER.getX() && x < Trench.BLUE_TOP_HIGHER.getX() && y > Trench.BLUE_TOP_LOWER.getY() && y < Trench.BLUE_TOP_HIGHER.getY()) {
+
+                lightStrip.setPattern(BlinkinPattern.STROBE_RED);
+
+        } else {
+
+            switch (shootingState.getShootState()) {
+            case STOPPED:
+                lightStrip.setPattern(BlinkinPattern.WHITE);
+                break;
+            case FIXED:
+                lightStrip.setPattern(BlinkinPattern.COLOR_WAVES_OCEAN_PALETTE);
+                break;
+            case FIXED_2:
+                lightStrip.setPattern(BlinkinPattern.RED_ORANGE);
+                break;
+            case SHOOTING_HUB:
+                lightStrip.setPattern(BlinkinPattern.RAINBOW_RAINBOW_PALETTE);
+                break;
+            case SHUTTLING:
+                lightStrip.setPattern(BlinkinPattern.GREEN);
+                break;
+            }
+
+        }
+
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+
+    }
+
+}

src/main/java/frc/robot/commands/lightStrip/SetLedFromShootingState.java

@@ -0,0 +1,57 @@
+package frc.robot.commands.lightStrip;
+
+import frc.robot.constants.enums.ShootingState;
+import frc.robot.subsystems.LightStripSubsystem;
+import frc.robot.utils.BlinkinPattern;
+import frc.robot.utils.logging.commands.LoggableCommand;
+
+// This code is unused. The light strip is fully controlled through SetLed.
+
+public class SetLedFromShootingState extends LoggableCommand{
+
+    private final LightStripSubsystem subsystem;
+    private ShootingState shootingState;
+
+    public SetLedFromShootingState(LightStripSubsystem subsystem, ShootingState shootingState) {
+        this.subsystem = subsystem;
+        this.shootingState = shootingState;
+        addRequirements(subsystem);
+    }
+
+    @Override
+    public void initialize() {
+
+    }  
+
+    @Override
+    public void execute() {
+        switch (shootingState.getShootState()) {
+            case STOPPED:
+                subsystem.setPattern(BlinkinPattern.DARK_RED);
+                break;
+            case FIXED:
+                subsystem.setPattern(BlinkinPattern.COLOR_WAVES_OCEAN_PALETTE);
+                break;
+            case FIXED_2:
+                subsystem.setPattern(BlinkinPattern.RED_ORANGE);
+                break;
+            case SHOOTING_HUB:
+                subsystem.setPattern(BlinkinPattern.RAINBOW_RAINBOW_PALETTE);
+                break;
+            case SHUTTLING:
+                subsystem.setPattern(BlinkinPattern.GREEN);
+                break;
+        }
+    }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+
+    }
+
+}

src/main/java/frc/robot/commands/parallels/SetShootingStateAndLight.java

@@ -0,0 +1,23 @@
+package frc.robot.commands.parallels;
+
+import frc.robot.commands.lightStrip.SetLedFromShootingState;
+import frc.robot.commands.shooter.SetShootingState;
+import frc.robot.constants.enums.ShootingState;
+import frc.robot.constants.enums.ShootingState.ShootState;
+import frc.robot.subsystems.LightStripSubsystem;
+import frc.robot.utils.logging.commands.LoggableParallelCommandGroup;
+
+// This code is unused. The light strip is fully controlled through SetLed.
+
+public class SetShootingStateAndLight extends LoggableParallelCommandGroup{
+
+    public SetShootingStateAndLight(ShootingState shootState, LightStripSubsystem lightStrip, ShootState newState) {
+
+        super(
+            new SetShootingState(shootState, newState),
+            new SetLedFromShootingState(lightStrip, shootState)
+        );
+
+    }
+
+}

src/main/java/frc/robot/constants/ConstantsPushbot2026.java

@@ -22,6 +22,7 @@ public class ConstantsPushbot2026 extends GameConstants {
     public static final double INITIAL_ROBOT_HEIGHT = 0;
 
     public static final int INTAKE_DIGITAL_INPUT_CHANNEL = 0;
+    public static final int LIGHT_STRIP_CHANNEL = 0;
 
     public static final double GYRO_DIAGS_ANGLE = 30;
 

src/main/java/frc/robot/constants/ConstantsReal2026.java

@@ -24,6 +24,7 @@ public class ConstantsReal2026 extends GameConstants {
     public static final double INITIAL_ROBOT_HEIGHT = 0;
 
     public static final int INTAKE_DIGITAL_INPUT_CHANNEL = 0;
+    public static final int LIGHT_STRIP_CHANNEL = 1; // Example ID, may change later
 
     public static final double GYRO_DIAGS_ANGLE = 30;
 

src/main/java/frc/robot/constants/ConstantsTestbed2026.java

@@ -27,5 +27,7 @@ public class ConstantsTestbed2026 extends ConstantsPushbot2026 {
   // public static final int SHOOTER_MOTOR_ID = 50;
   // public static final int SHOOTER_FOLLOWER_MOTOR_ID = 40;
 
+  // public static final int LIGHT_STRIP_CHANNEL = 0;
+
     public static final boolean TESTBED = true;
 }

src/main/java/frc/robot/constants/enums/Trench.java

@@ -0,0 +1,30 @@
+package frc.robot.constants.enums;
+
+public enum Trench {
+
+        RED_BOTTOM_LOWER(10,0),
+        RED_BOTTOM_HIGHER(14,1.5),
+        RED_TOP_LOWER(10,6.5),
+        RED_TOP_HIGHER(14,8),
+        BLUE_BOTTOM_LOWER(2.5,0),
+        BLUE_BOTTOM_HIGHER(6.5,1.5),
+        BLUE_TOP_LOWER(2.5,6.5),
+        BLUE_TOP_HIGHER(6.5,8);
+
+        private double x;
+        private double y;
+
+        private Trench(double x, double y) {
+            this.x = x;
+            this.y = y;
+        }
+
+        public double getX() {
+        return x;
+        }
+
+        public double getY() {
+        return y;
+        } 
+
+}

src/main/java/frc/robot/subsystems/LightStripSubsystem.java

@@ -0,0 +1,31 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.commands.lightStrip.SetLed;
+import frc.robot.constants.Constants;
+import frc.robot.constants.enums.ShootingState;
+import frc.robot.subsystems.swervedrive.SwerveSubsystem;
+import frc.robot.utils.BlinkinPattern;
+
+public class LightStripSubsystem extends SubsystemBase{
+
+    public static final String LOGGING_NAME = "LightStripSubsystem";
+    private final Spark io;
+    private BlinkinPattern pattern;
+
+    public LightStripSubsystem(SwerveSubsystem drivebase, ShootingState shootingState) {
+        this.io = new Spark(Constants.LIGHT_STRIP_CHANNEL);
+        setDefaultCommand(new SetLed(this, drivebase, shootingState));
+    }
+
+    public void setPattern(BlinkinPattern pattern) {
+        this.pattern = pattern;
+        io.set(this.pattern.getPwm());
+    }
+
+    public BlinkinPattern getPattern() {
+        return pattern;
+    }
+
+}

src/main/java/frc/robot/subsystems/RollerSubsystem.java

@@ -19,9 +19,7 @@
 import frc.robot.utils.logging.io.motor.SimSparkMaxIo;
 import frc.robot.utils.logging.io.motor.SparkMaxIo;
 import frc.robot.utils.simulation.MotorSimulator;
-import frc.robot.utils.simulation.RobotVisualizer;
-
-// The Roller subsystem spins the wheel that releases the algae.
+import frc.robot.utils.simulation.RobotVisualizer;  
 
 public class RollerSubsystem extends SubsystemBase {
     public static final String LOGGING_NAME = "RollerSubsystem";