diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7eefd7d4..3a669852 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -22,6 +22,7 @@ import frc.subsytem.Elevator.Elevator; import frc.subsytem.Elevator.ElevatorIO; import frc.subsytem.Elevator.ElevatorIOSparkMax; +import frc.subsytem.LED.LED; import frc.subsytem.MechanismStateManager; import frc.subsytem.MechanismStateManager.MechanismStates; import frc.subsytem.drive.Drive; @@ -86,7 +87,7 @@ public class Robot extends LoggedRobot { private @NotNull static TelescopingArm telescopingArm; private @NotNull static Grabber grabber; private @NotNull static MechanismStateManager mechanismStateManager; - + private @NotNull static LED led; private @NotNull Controller xbox; private @NotNull Controller stick; @@ -163,6 +164,7 @@ public void robotInit() { elevator = new Elevator(new ElevatorIOSparkMax()); telescopingArm = new TelescopingArm(new TelescopingArmIOSparkMax()); grabber = new Grabber(new GrabberIOSparkMax()); + led = new LED(5); } else { setUseTiming(false); // Run as fast as possible Logger.getInstance().setReplaySource(new WPILOGReader(logPath)); // Read replay log @@ -173,6 +175,7 @@ public void robotInit() { elevator = new Elevator(new ElevatorIO() {}); telescopingArm = new TelescopingArm(new TelescopingArmIO() {}); grabber = new Grabber(new GrabberIO() {}); + led = new LED(5); } Logger.getInstance().start(); // Start logging! No more data receivers, replay sources, or metadata values may be added @@ -189,6 +192,7 @@ public void robotInit() { telescopingArm.start(); grabber.start(); drive.start(); + led.start(); ScoringPositionManager.getInstance(); xbox = new Controller(0); diff --git a/src/main/java/frc/subsytem/LED/LED.java b/src/main/java/frc/subsytem/LED/LED.java index f691e3aa..e60c1e99 100644 --- a/src/main/java/frc/subsytem/LED/LED.java +++ b/src/main/java/frc/subsytem/LED/LED.java @@ -3,6 +3,7 @@ import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLEDBuffer; import frc.robot.Constants; +import frc.robot.ScoringPositionManager; import frc.subsytem.AbstractSubsystem; import org.littletonrobotics.junction.Logger; @@ -10,11 +11,10 @@ public class LED extends AbstractSubsystem { AddressableLED ledStrip; AddressableLEDBuffer addressableLEDBuffer; - LED(int loggingInterval) { + public LED(int loggingInterval) { ledStrip = new AddressableLED(0); ledStrip.setLength(Constants.LED_LENGTH); addressableLEDBuffer = new AddressableLEDBuffer(Constants.LED_LENGTH); - ledStrip.setData(addressableLEDBuffer); ledStrip.start(); } @@ -34,8 +34,18 @@ public enum LedState { } public void setColor(LedState ledState) { - for(int i = 0; i < addressableLEDBuffer.getLength(); i++) { + for (int i = 0; i < addressableLEDBuffer.getLength(); i++) { addressableLEDBuffer.setRGB(i, ledState.r, ledState.g, ledState.b); + ledStrip.setData(addressableLEDBuffer); + } + } + + public void setLedState() { + ScoringPositionManager.PositionType wantedPositionType = ScoringPositionManager.getInstance().getWantedPositionType(); + if (wantedPositionType == ScoringPositionManager.PositionType.CONE) { + setColor(LedState.YELLOW); + } else if (wantedPositionType == ScoringPositionManager.PositionType.CUBE) { + setColor(LedState.BLUE); } } @@ -43,7 +53,13 @@ public void off() { ledStrip.close(); } + @Override public void logData() { Logger.getInstance().recordOutput("LED Color", String.valueOf(addressableLEDBuffer.getLED(0))); } + + @Override + public void update() { + setLedState(); + } }