add static current characterization routines

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

@@ -190,6 +190,9 @@ public Robot() {
         "Wrist SysId (Dynamic Forward)", m_arm.sysIdWristDynamic(SysIdRoutine.Direction.kForward));
     m_autoChooser.addOption(
         "Wrist SysId (Dynamic Reverse)", m_arm.sysIdWristDynamic(SysIdRoutine.Direction.kReverse));
+    m_autoChooser.addOption("elbow static characterization", m_arm.elbowStaticCharacterization());
+    m_autoChooser.addOption("wrist static characterization", m_arm.wristStaticCharacterization());
+
     m_autoChooser.addOption("Start Signal Logger", Commands.runOnce(SignalLogger::start));
     m_autoChooser.addOption("End Signal Logger", Commands.runOnce(SignalLogger::stop));
     m_autoChooser.addOption("6 Piece", autos.sixPiece());
@@ -203,8 +206,8 @@ public Robot() {
         .hasIntookPieceSim
         .or(m_feeder.hasNote)
         .onTrue(Commands.runOnce(() -> NoteVisualizer.setHasNote(true)));
-    m_feeder.hasNote.whileTrue(m_lights.setBlink(Color.kOrangeRed));
-    m_drivetrain.inRangeOfGoal.whileTrue(m_lights.setBlink(Color.kBlue));
+    m_feeder.hasNote.whileTrue(m_lights.startBlink(Color.kOrangeRed));
+    m_drivetrain.inRangeOfGoal.whileTrue(m_lights.startBlink(Color.kBlue));
 
     m_driverController
         .rightTrigger()
@@ -300,7 +303,9 @@ public void teleopInit() {}
   public void teleopPeriodic() {}
 
   @Override
-  public void disabledInit() {}
+  public void disabledInit() {
+    m_lights.startGreenFlow().until(DriverStation::isEnabled).schedule();
+  }
 
   @Override
   public void disabledPeriodic() {}

src/main/java/frc/robot/arm/Arm.java

@@ -150,10 +150,10 @@ private void runSetpoint(
     Logger.recordOutput("Arm/Elbow Progress", elbowProgress);
     Logger.recordOutput("Arm/Wrist Progress", wristProgress);
 
-    if (wristProgress < elbowDelay) m_io.setElbowRotations(m_inputs.elbowPositionRot);
-    else m_io.setElbowRotations(Units.radiansToRotations(elbowAngleRad));
-    if (elbowProgress < wristDelay) m_io.setWristRotations(m_inputs.wristPositionRot);
-    else m_io.setWristRotations(Units.radiansToRotations(wristAngleRad));
+    // if (wristProgress < elbowDelay) m_io.setElbowRotations(m_inputs.elbowPositionRot);
+    m_io.setElbowRotations(Units.radiansToRotations(elbowAngleRad));
+    // if (elbowProgress < wristDelay) m_io.setWristRotations(m_inputs.wristPositionRot);
+    m_io.setWristRotations(Units.radiansToRotations(wristAngleRad));
   }
 
   @AutoLogOutput(key = "Arm/JointsAtGoal")
@@ -284,6 +284,16 @@ public Command sysIdWristDynamic(SysIdRoutine.Direction direction) {
     return m_sysIdRoutineWrist.dynamic(direction);
   }
 
+  public Command elbowStaticCharacterization() {
+    return new ArmStaticCharacterization(
+        this, m_io::setElbowCurrent, () -> Units.rotationsToRadians(m_inputs.elbowVelocityRps));
+  }
+
+  public Command wristStaticCharacterization() {
+    return new ArmStaticCharacterization(
+        this, m_io::setWristCurrent, () -> Units.rotationsToRadians(m_inputs.wristVelocityRps));
+  }
+
   public static enum ArmSetpoints {
     kStowed(-0.548, 2.485, 0.15, 0.0),
     kAmp(1.49 + 0.0873, Units.degreesToRadians(230), 0.0, 0.0),

src/main/java/frc/robot/arm/ArmIO.java

@@ -33,6 +33,10 @@ public default void setElbowVoltage(double volts) {}
 
   public default void setWristVoltage(double volts) {}
 
+  public default void setElbowCurrent(double amps) {}
+
+  public default void setWristCurrent(double amps) {}
+
   public default void stop() {}
 
   public default void setBrakeMode(boolean enableBrakeMode) {}

src/main/java/frc/robot/arm/ArmIOTalonFX.java

@@ -6,6 +6,7 @@
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.Follower;
 import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.controls.TorqueCurrentFOC;
 import com.ctre.phoenix6.hardware.ParentDevice;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.GravityTypeValue;
@@ -202,6 +203,16 @@ public void setWristVoltage(double volts) {
     m_wristLeftMotor.setVoltage(volts);
   }
 
+  @Override
+  public void setElbowCurrent(double amps) {
+    m_elbowLeftMotor.setControl(new TorqueCurrentFOC(amps));
+  }
+
+  @Override
+  public void setWristCurrent(double amps) {
+    m_wristLeftMotor.setControl(new TorqueCurrentFOC(amps));
+  }
+
   @Override
   public void stop() {
     m_elbowLeftMotor.setVoltage(0.0);

src/main/java/frc/robot/arm/ArmStaticCharacterization.java

@@ -0,0 +1,48 @@
+package frc.robot.arm;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.lib.LoggedTunableNumber;
+import java.util.function.DoubleConsumer;
+import java.util.function.DoubleSupplier;
+
+public class ArmStaticCharacterization extends Command {
+  private static final LoggedTunableNumber currentRampFactor =
+      new LoggedTunableNumber("StaticCharacterization/CurrentRampPerSec", 1.0);
+  private static final LoggedTunableNumber minVelocity =
+      new LoggedTunableNumber("StaticCharacterization/MinStaticVelocity", 0.1);
+
+  private final DoubleConsumer m_inputConsumer;
+  private final DoubleSupplier m_velocitySupplier;
+  private final Timer m_timer = new Timer();
+  private double m_currentInput = 0.0;
+
+  public ArmStaticCharacterization(
+      Arm arm, DoubleConsumer characterizationInputConsumer, DoubleSupplier velocitySupplier) {
+    m_inputConsumer = characterizationInputConsumer;
+    this.m_velocitySupplier = velocitySupplier;
+    addRequirements(arm);
+  }
+
+  @Override
+  public void initialize() {
+    m_timer.restart();
+  }
+
+  @Override
+  public void execute() {
+    m_currentInput = m_timer.get() * currentRampFactor.get();
+    m_inputConsumer.accept(m_currentInput);
+  }
+
+  @Override
+  public boolean isFinished() {
+    return m_velocitySupplier.getAsDouble() >= minVelocity.get();
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    System.out.println("Static Characterization output: " + m_currentInput + " amps");
+    m_inputConsumer.accept(0);
+  }
+}

src/main/java/frc/robot/lights/Lights.java

@@ -3,6 +3,8 @@
 import com.ctre.phoenix.led.CANdle;
 import com.ctre.phoenix.led.CANdle.LEDStripType;
 import com.ctre.phoenix.led.CANdleConfiguration;
+import com.ctre.phoenix.led.ColorFlowAnimation;
+import com.ctre.phoenix.led.ColorFlowAnimation.Direction;
 import com.ctre.phoenix.led.RainbowAnimation;
 import com.ctre.phoenix.led.StrobeAnimation;
 import edu.wpi.first.wpilibj.util.Color;
@@ -18,6 +20,7 @@
 public class Lights extends SubsystemBase {
 
   private final int kLedCount = Constants.kIsViper ? 8 : 32;
+  private final int kCandleLedOffset = Constants.kIsViper ? 0 : 8;
   private final CANdle m_ledController;
 
   public Lights() {
@@ -27,7 +30,7 @@ public Lights() {
     config.disableWhenLOS = true;
     config.brightnessScalar = 1;
     m_ledController.configAllSettings(config);
-    setDefaultCommand(setRainbow());
+    setDefaultCommand(startRainbow());
   }
 
   @Override
@@ -38,61 +41,65 @@ public void periodic() {
     Logger.recordOutput("CANdle/Temperature", m_ledController.getTemperature());
   }
 
-  public Command setRainbow() {
+  public Command startRainbow() {
     return this.runOnce(() -> m_ledController.animate(new RainbowAnimation(1.0, 0.1, kLedCount)))
         .andThen(this.run(() -> {}))
         .ignoringDisable(true);
   }
 
-  public Command setBlink(Color color) {
-    final int[] blinkColor = colorToRGB(color);
-    return Commands.sequence(
-            this.runOnce(
-                () ->
-                    m_ledController.animate(
-                        new StrobeAnimation(
-                            blinkColor[0], blinkColor[1], blinkColor[2], 0, 0.4, kLedCount))),
-            this.run(() -> {}))
+  public Command startGreenFlow() {
+    final ColorRGB color = colorToRGB(Color.kGreen);
+    return this.runOnce(
+            () ->
+                m_ledController.animate(
+                    new ColorFlowAnimation(
+                        color.red,
+                        color.green,
+                        color.blue,
+                        0,
+                        0.5,
+                        kLedCount,
+                        Direction.Forward,
+                        kCandleLedOffset)))
+        .andThen(this.run(() -> {}))
         .ignoringDisable(true);
   }
 
-  public void setBlinkMethod(Color color) {
-    final int[] blinkColor = colorToRGB(color);
+  public Command startBlink(Color color) {
+    return Commands.sequence(this.runOnce(() -> setBlink(color)), this.run(() -> {}))
+        .ignoringDisable(true);
+  }
 
+  public void setBlink(Color color) {
+    final ColorRGB blinkColor = colorToRGB(color);
     m_ledController.animate(
-        new StrobeAnimation(blinkColor[0], blinkColor[1], blinkColor[2], 0, 0.4, kLedCount));
+        new StrobeAnimation(blinkColor.red, blinkColor.green, blinkColor.blue, 0, 0.4, kLedCount));
   }
 
-  /**
-   * @param color
-   * @return an array of integers representing the RGB values of the given color: [0] is red, [1] is
-   *     green, [2] is blue
-   */
-  private int[] colorToRGB(Color color) {
+  private ColorRGB colorToRGB(Color color) {
     final Function<Double, Integer> mapValue = value -> (int) (value * 255.0);
-    return new int[] {
-      mapValue.apply(color.red), mapValue.apply(color.green), mapValue.apply(color.blue)
-    };
+    return new ColorRGB(
+        mapValue.apply(color.red), mapValue.apply(color.green), mapValue.apply(color.blue));
   }
 
   public Command showReadyToShootStatus(Trigger readyToShoot) {
-    var lastState =
+    var state =
         new Object() {
-          boolean value = false;
+          boolean lastTriggerState = false;
         };
     return new FunctionalCommand(
-        () -> setBlinkMethod(Color.kRed),
+        () -> setBlink(Color.kRed),
         () -> {
-          boolean currentTriggerState = readyToShoot.getAsBoolean();
-          if (lastState.value == currentTriggerState) return;
-          if (readyToShoot.getAsBoolean()) setBlinkMethod(Color.kGreen);
-          else setBlinkMethod(Color.kRed);
-          lastState.value = readyToShoot.getAsBoolean();
-        },
-        (interrupted) -> {
-          setRainbow();
+          final boolean currentTriggerState = readyToShoot.getAsBoolean();
+          if (state.lastTriggerState == currentTriggerState) return;
+          if (readyToShoot.getAsBoolean()) setBlink(Color.kGreen);
+          else setBlink(Color.kRed);
+          state.lastTriggerState = readyToShoot.getAsBoolean();
         },
+        (interrupted) -> startRainbow().schedule(),
         () -> false,
         this);
   }
+
+  private record ColorRGB(int red, int green, int blue) {}
 }