diff --git a/src/main/java/frc/robot/commands/ChangeLimelightLight.java b/src/main/java/frc/robot/commands/ChangeLimelightLight.java new file mode 100644 index 00000000..cf9cdc53 --- /dev/null +++ b/src/main/java/frc/robot/commands/ChangeLimelightLight.java @@ -0,0 +1,23 @@ +package frc.robot.commands; + +import frc.robot.subsystems.limelight.VisionIO; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class ChangeLimelightLight extends LoggableCommand { + private final VisionIO io; + private boolean on; + + public ChangeLimelightLight(boolean on, VisionIO io) { + this.io = io; + } + + @Override + public void initialize() { + io.setLedMode(on ? 3 : 1); + } + + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/subsystems/limelight/MockVisionIO.java b/src/main/java/frc/robot/subsystems/limelight/MockVisionIO.java index 634f7193..57c2eaf4 100644 --- a/src/main/java/frc/robot/subsystems/limelight/MockVisionIO.java +++ b/src/main/java/frc/robot/subsystems/limelight/MockVisionIO.java @@ -3,4 +3,7 @@ public class MockVisionIO implements VisionIO { @Override public void updateInputs(VisionInputs inputs) {} + + @Override + public void setLedMode(int mode) {} } diff --git a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java index 78af3f46..3a2a41e7 100644 --- a/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java +++ b/src/main/java/frc/robot/subsystems/limelight/RealVisionIO.java @@ -52,4 +52,8 @@ public void updateInputs(VisionInputs inputs) { inputs.valid = results.valid; inputs.ledMode = (int) ledModeEntry.getInteger(-1); } + + public void setLedMode(int mode) { + ledModeEntry.setNumber(mode); + } } diff --git a/src/main/java/frc/robot/subsystems/limelight/VisionIO.java b/src/main/java/frc/robot/subsystems/limelight/VisionIO.java index 990d52a4..9582f551 100644 --- a/src/main/java/frc/robot/subsystems/limelight/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/limelight/VisionIO.java @@ -2,4 +2,6 @@ import frc.robot.utils.logging.LoggableIO; -public interface VisionIO extends LoggableIO {} +public interface VisionIO extends LoggableIO { + void setLedMode(int mode); +}