Add LED Subsystem + LED Replay

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

@@ -39,7 +39,11 @@
 import frc.robot.lib.subsystem.angular.AngularIOSim;
 import frc.robot.lib.subsystem.angular.AngularIOTalonFX;
 import frc.robot.lib.subsystem.angular.AngularSubsystem;
+import frc.robot.subsystems.CANdle.CANdleSystem;
+import frc.robot.subsystems.CANdle.io.CANdleIOReal;
+import frc.robot.subsystems.CANdle.io.CANdleIOSim;
 import frc.robot.subsystems.SuperstructureVisualizer;
+import frc.robot.subsystems.climber.Climber;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.GyroIO;
 import frc.robot.subsystems.drive.GyroIOPigeon2;
@@ -73,6 +77,8 @@ public class RobotContainer {
   private final Vision vision;
   private final Shooter shooter;
   private final Intake intake;
+  private final Climber climber;
+  private final CANdleSystem candle;
 
   private final RobotSuperstructure superstructure;
 
@@ -133,6 +139,8 @@ public RobotContainer() {
                     FlywheelConstants.kSubsystemConfigReal),
                 drive::getPose,
                 drive::getPoseVelocity);
+        climber = new Climber();
+        candle = new CANdleSystem(new CANdleIOReal());
         break;
 
       case SIM:
@@ -174,6 +182,8 @@ public RobotContainer() {
                     FlywheelConstants.kSubsystemConfigSim),
                 drive::getPose,
                 drive::getPoseVelocity);
+        climber = new Climber();
+        candle = new CANdleSystem(new CANdleIOSim());
         break;
 
       default:
@@ -189,6 +199,8 @@ public RobotContainer() {
             new Vision(drive::addVisionMeasurement, new VisionIO() {} /* , new VisionIO() {} */);
         intake = new Intake();
         shooter = new Shooter(drive::getPose, drive::getPoseVelocity);
+        climber = new Climber();
+        candle = new CANdleSystem();
         break;
     }
 
@@ -258,6 +270,9 @@ private void configureButtonBindings() {
     // Intake controls
     controller.buttonA.onTrue(superstructure.intakeDeploy());
     controller.buttonA.onFalse(superstructure.intakeStowed());
+
+    controller.leftBumper.onTrue(climber.down());
+    controller.rightBumper.onTrue(climber.up());
   }
 
   private void logInit() {

src/main/java/frc/robot/subsystems/CANdle/CANdleSystem.java

@@ -0,0 +1,44 @@
+package frc.robot.subsystems.CANdle;
+
+import com.ctre.phoenix6.controls.*;
+import com.ctre.phoenix6.signals.RGBWColor;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.subsystems.CANdle.io.CANdleIO;
+import frc.robot.subsystems.CANdle.io.CANdleIOInputsAutoLogged;
+import org.littletonrobotics.junction.Logger;
+
+public class CANdleSystem extends SubsystemBase {
+
+  private static final int SlotStartIdx = 8;
+  private static final int SlotEndIdx = 37;
+
+  private final CANdleIO io;
+  private final CANdleIOInputsAutoLogged inputs;
+
+  public CANdleSystem() {
+    this(new CANdleIO() {});
+  }
+
+  public CANdleSystem(CANdleIO io) {
+    this.io = io;
+    this.inputs = new CANdleIOInputsAutoLogged();
+  }
+
+  @Override
+  public void periodic() {
+    this.io.updateInputs(inputs);
+
+    Logger.processInputs("CANdle", inputs);
+  }
+
+  private Command setAnimFlow() {
+    return run(
+        () -> {
+          io.setControl(
+              new ColorFlowAnimation(SlotStartIdx, SlotEndIdx)
+                  .withSlot(0)
+                  .withColor(new RGBWColor(0, 217, 0, 0)));
+        });
+  }
+}

src/main/java/frc/robot/subsystems/CANdle/io/CANdleIO.java

@@ -0,0 +1,19 @@
+package frc.robot.subsystems.CANdle.io;
+
+import com.ctre.phoenix6.controls.ControlRequest;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface CANdleIO {
+
+  @AutoLog
+  class CANdleIOInputs {
+    public String animationName;
+    public String animation;
+  }
+
+  default void setControl(ControlRequest request) {}
+  ;
+
+  default void updateInputs(CANdleIOInputs inputs) {}
+  ;
+}

src/main/java/frc/robot/subsystems/CANdle/io/CANdleIOReal.java

@@ -0,0 +1,44 @@
+package frc.robot.subsystems.CANdle.io;
+
+import com.ctre.phoenix6.configs.CANdleConfiguration;
+import com.ctre.phoenix6.controls.ControlRequest;
+import com.ctre.phoenix6.controls.EmptyAnimation;
+import com.ctre.phoenix6.hardware.CANdle;
+import com.ctre.phoenix6.signals.StatusLedWhenActiveValue;
+import com.ctre.phoenix6.signals.StripTypeValue;
+
+public class CANdleIOReal implements CANdleIO {
+
+  private final CANdle m_candle = new CANdle(1, "rio");
+  private ControlRequest request;
+
+  public CANdleIOReal() {
+    var candleconfig = new CANdleConfiguration();
+
+    candleconfig.LED.StripType = StripTypeValue.GRB;
+    candleconfig.LED.BrightnessScalar = 0.5;
+
+    candleconfig.CANdleFeatures.StatusLedWhenActive = StatusLedWhenActiveValue.Disabled;
+
+    m_candle.getConfigurator().apply(candleconfig);
+
+    for (int i = 0; i < 24; ++i) {
+      m_candle.setControl(new EmptyAnimation(i));
+    }
+    request = new EmptyAnimation(0);
+  }
+
+  @Override
+  public void setControl(ControlRequest request) {
+    m_candle.setControl(request);
+    this.request = request;
+  }
+  ;
+
+  @Override
+  public void updateInputs(CANdleIOInputs inputs) {
+    inputs.animationName = request.getName();
+    inputs.animation = request.toString();
+  }
+  ;
+}

src/main/java/frc/robot/subsystems/CANdle/io/CANdleIOSim.java

@@ -0,0 +1,21 @@
+package frc.robot.subsystems.CANdle.io;
+
+import com.ctre.phoenix6.controls.ControlRequest;
+
+public class CANdleIOSim implements CANdleIO {
+
+  private ControlRequest request;
+
+  @Override
+  public void setControl(ControlRequest request) {
+    this.request = request;
+  }
+  ;
+
+  @Override
+  public void updateInputs(CANdleIOInputs inputs) {
+    inputs.animationName = request.getName();
+    inputs.animation = request.toString();
+  }
+  ;
+}

src/main/java/frc/robot/subsystems/climber/Climber.java

@@ -0,0 +1,67 @@
+package frc.robot.subsystems.climber;
+
+import com.ctre.phoenix6.configs.MotionMagicConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Climber extends SubsystemBase {
+  public final TalonFX climber1 = new TalonFX(0);
+  public final TalonFX climber2 = new TalonFX(0);
+
+  private final TalonFXConfiguration talonFXConfigs = new TalonFXConfiguration();
+  private Slot0Configs slot0Configs = talonFXConfigs.Slot0;
+  private MotionMagicConfigs motionMagicConfigs = talonFXConfigs.MotionMagic;
+  private final MotionMagicVoltage m_request = new MotionMagicVoltage(0);
+
+  private boolean up = false;
+
+  private double upAngle = 10;
+
+  public Climber() {
+    slot0Configs.kS = 0;
+    slot0Configs.kV = 0;
+    slot0Configs.kA = 0;
+    slot0Configs.kP = 5;
+    slot0Configs.kI = 0;
+    slot0Configs.kD = 0;
+    slot0Configs.kG = 0;
+
+    motionMagicConfigs.MotionMagicCruiseVelocity = 80;
+    motionMagicConfigs.MotionMagicAcceleration = 160;
+    motionMagicConfigs.MotionMagicJerk = 1600;
+
+    climber1.getConfigurator().apply(talonFXConfigs);
+    climber2.getConfigurator().apply(talonFXConfigs);
+
+    climber1.setPosition(0);
+    climber2.setPosition(0);
+
+    setDefaultCommand(setAngle());
+  }
+
+  private Command setAngle() {
+    return run(
+        () -> {
+          climber1.setControl(m_request.withPosition(up ? upAngle : 0));
+          climber2.setControl(m_request.withPosition(up ? upAngle : 0));
+        });
+  }
+
+  public Command up() {
+    return runOnce(
+        () -> {
+          up = true;
+        });
+  }
+
+  public Command down() {
+    return runOnce(
+        () -> {
+          up = false;
+        });
+  }
+}

src/main/java/frc/robot/subsystems/shooter/Shooter.java

@@ -43,7 +43,6 @@ public class Shooter extends VirtualSubsystem implements AllianceUpdatedObserver
   private Supplier<Pose2d> robotPose;
   private Supplier<ChassisSpeeds> robotVel;
 
-
   /** Creates a new Shooter. */
   public Shooter(Supplier<Pose2d> robotPose, Supplier<ChassisSpeeds> robotVel) {
     this(