Basic LED done

team422 · May 28, 2024 · 04734e6 · 04734e6
1 parent 81261fd
commit 04734e6
Show file tree

Hide file tree

Showing 5 changed files with 106 additions and 12 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -148,5 +148,7 @@ public static final class Ports {
     public static final int kClimbDown = 25;
 
     public static final int kAmp = 54;
+
+    public static final int kLed = 9;
   }
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -63,7 +63,9 @@ public void robotPeriodic() {
 
   /** This function is called once when the robot is disabled. */
   @Override
-  public void disabledInit() {}
+  public void disabledInit() {
+    RobotState.getInstance().onDisable();
+  }
 
   /** This function is called periodically when disabled. */
   @Override
@@ -78,6 +80,8 @@ public void autonomousInit() {
     if (autonomousCommand != null) {
       autonomousCommand.schedule();
     }
+
+    RobotState.getInstance().onEnableAutonomous();
   }
 
   /** This function is called periodically during autonomous. */
@@ -94,6 +98,8 @@ public void teleopInit() {
     if (autonomousCommand != null) {
       autonomousCommand.cancel();
     }
+
+    RobotState.getInstance().onEnableTeleop();
   }
 
   /** This function is called periodically during operator control. */

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -36,6 +36,7 @@
 import frc.robot.subsystems.intake.pivot.IntakePivotIOSim;
 import frc.robot.subsystems.intake.roller.RollerIOKraken;
 import frc.robot.subsystems.intake.roller.RollerIOSim;
+import frc.robot.subsystems.led.Led;
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.shooter.flywheel.FlywheelIOKraken;
 import frc.robot.subsystems.shooter.flywheel.FlywheelIOSim;
@@ -56,6 +57,7 @@ public class RobotContainer {
   private Shooter m_shooter;
   private Indexer m_indexer;
   private Intake m_intake;
+  private Led m_led;
 
   // Controllers
   private DriverControls m_driverControls;
@@ -76,6 +78,7 @@ private void configureSubsystems() {
               new ModuleIOTalonFX(1),
               new ModuleIOTalonFX(2),
               new ModuleIOTalonFX(3));
+
       m_shooter =
           new Shooter(
               new ShooterPivotIOFalcon(
@@ -88,6 +91,7 @@ private void configureSubsystems() {
               ShooterConstants.kRightFlywheelController,
               ShooterConstants.kLeftFlywheelFeedforward,
               ShooterConstants.kRightFlywheelFeedforward);
+
       m_indexer =
           new Indexer(
               new IndexerIOFalcon(
@@ -101,6 +105,9 @@ private void configureSubsystems() {
               new RollerIOKraken(Ports.kIntakeRoller),
               new IntakePivotIONeo(Ports.kIntakePivot),
               IntakeConstants.kPivotController);
+
+      m_led = new Led(Ports.kLed, 20);
+
     } else {
       m_drive =
           new Drive(
@@ -122,12 +129,14 @@ private void configureSubsystems() {
 
       m_intake =
           new Intake(new RollerIOSim(), new IntakePivotIOSim(), IntakeConstants.kPivotController);
+
+      m_led = new Led(Ports.kLed, 20);
     }
 
     m_shooter.setPivotAngle(ShooterConstants.kHomeAngle);
     m_intake.setPivotAngle(IntakeConstants.kHomeAngle);
 
-    m_robotState = RobotState.startInstance(m_drive, m_shooter, m_indexer, m_intake);
+    m_robotState = RobotState.startInstance(m_drive, m_shooter, m_indexer, m_intake, m_led);
   }
 
   private void configureControllers() {

diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java
@@ -13,6 +13,8 @@
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.indexer.Indexer;
 import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.led.Led;
+import frc.robot.subsystems.led.Led.LedState;
 import frc.robot.subsystems.shooter.Shooter;
 import org.littletonrobotics.junction.Logger;
 
@@ -25,6 +27,7 @@ public class RobotState {
   private Shooter m_shooter;
   private Indexer m_indexer;
   private Intake m_intake;
+  private Led m_led;
 
   // mechanism
   private final boolean kMechanismEnabled = true;
@@ -43,12 +46,13 @@ public class RobotState {
           new Translation3d(-0.269, -0.01, 0.2428),
           new Rotation3d(Units.degreesToRadians(180), Units.degreesToRadians(170), 0.0));
 
-  private RobotState(Drive drive, Shooter shooter, Indexer indexer, Intake intake) {
+  private RobotState(Drive drive, Shooter shooter, Indexer indexer, Intake intake, Led led) {
     // subsystems
     m_drive = drive;
     m_shooter = shooter;
     m_indexer = indexer;
     m_intake = intake;
+    m_led = led;
 
     // mechanism set up
     m_mechanism = new Mechanism2d(1.75, 1);
@@ -88,9 +92,9 @@ private RobotState(Drive drive, Shooter shooter, Indexer indexer, Intake intake)
   }
 
   public static RobotState startInstance(
-      Drive drive, Shooter shooter, Indexer indexer, Intake intake) {
+      Drive drive, Shooter shooter, Indexer indexer, Intake intake, Led led) {
     if (instance == null) {
-      instance = new RobotState(drive, shooter, indexer, intake);
+      instance = new RobotState(drive, shooter, indexer, intake, led);
     } else {
       throw new IllegalStateException("RobotState instance already started");
     }
@@ -104,6 +108,16 @@ public static RobotState getInstance() {
     return instance;
   }
 
+  public void updateRobotState() {
+    if (kMechanismEnabled) {
+      updateMechanism();
+    }
+
+    if (kComponentsEnabled) {
+      updateComponents();
+    }
+  }
+
   public void updateMechanism() {
     m_shooterPivotMechLower.setAngle(m_shooter.getPivotAngle().getDegrees() + 180);
     m_shooterPivotMechUpper.setAngle(m_shooter.getPivotAngle().getDegrees());
@@ -132,13 +146,15 @@ public void updateComponents() {
     Logger.recordOutput("Components/ZeroPose", new Pose3d()); // for tuning config
   }
 
-  public void updateRobotState() {
-    if (kMechanismEnabled) {
-      updateMechanism();
-    }
+  public void onEnableTeleop() {
+    m_led.setState(LedState.TELEOP);
+  }
 
-    if (kComponentsEnabled) {
-      updateComponents();
-    }
+  public void onEnableAutonomous() {
+    m_led.setState(LedState.AUTONOMOUS);
+  }
+
+  public void onDisable() {
+    m_led.setState(LedState.DISABLED);
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/led/Led.java b/src/main/java/frc/robot/subsystems/led/Led.java
@@ -0,0 +1,61 @@
+package frc.robot.subsystems.led;
+
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
+
+public class Led extends SubsystemBase {
+  private AddressableLED m_strip;
+  private AddressableLEDBuffer m_buffer;
+  private LedState m_state = LedState.OFF;
+
+  public static enum LedState {
+    OFF,
+    AUTONOMOUS,
+    TELEOP,
+    DISABLED
+  }
+
+  public Led(int port, int length) {
+    m_strip = new AddressableLED(port);
+    m_buffer = new AddressableLEDBuffer(length);
+    m_strip.setLength(length);
+    m_strip.start();
+  }
+
+  @Override
+  public void periodic() {
+    Logger.recordOutput("LED/State", m_state.toString());
+    Logger.recordOutput("LED/Color", m_buffer.getLED(0).toString());
+  }
+
+  public void setSolidColor(Color color) {
+    for (int i = 0; i < m_buffer.getLength(); i++) {
+      m_buffer.setLED(i, color);
+    }
+    m_strip.setData(m_buffer);
+  }
+
+  public void setState(LedState state) {
+    if (state == m_state) {
+      return;
+    }
+    switch (state) {
+      case OFF:
+        setSolidColor(Color.kBlack);
+        break;
+      case AUTONOMOUS:
+        setSolidColor(Color.kBlue);
+        break;
+      case TELEOP:
+        setSolidColor(Color.kGreen);
+        break;
+      case DISABLED:
+        setSolidColor(Color.kRed);
+        break;
+    }
+    m_state = state;
+  }
+}