Add characterization for shooter

FRC2713 · Mar 13, 2024 · 19b4157 · 19b4157
1 parent 9c73f1e
commit 19b4157
Show file tree

Hide file tree

Showing 4 changed files with 99 additions and 9 deletions.
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -19,6 +19,7 @@
 import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants.LimeLightConstants;
 import frc.robot.commands.RHRFullRoutine;
 import frc.robot.commands.fullRoutines.BottomTwo;
@@ -133,6 +134,8 @@ public void robotInit() {
     // ? new VisionIOSim(LimeLightConstants.REAR_LIMELIGHT_INFO)
     // : new VisionIOLimelight(LimeLightConstants.REAR_LIMELIGHT_INFO));
 
+    DriverStation.silenceJoystickConnectionWarning(isSimulation());
+
     mechManager = new MechanismManager();
 
     createDriverBindings();
@@ -359,6 +362,30 @@ public void createDriverBindings() {
                         DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue
                             ? 180
                             : 0))));
+
+    driver
+        .a()
+        .whileTrue(
+            Commands.runOnce(() -> shooter.setState(Shooter.State.CHARACTERIZATION))
+                .andThen(shooter.sysIdDynamic(SysIdRoutine.Direction.kForward)));
+
+    driver
+        .b()
+        .whileTrue(
+            Commands.runOnce(() -> shooter.setState(Shooter.State.CHARACTERIZATION))
+                .andThen(shooter.sysIdDynamic(SysIdRoutine.Direction.kReverse)));
+
+    driver
+        .x()
+        .whileTrue(
+            Commands.runOnce(() -> shooter.setState(Shooter.State.CHARACTERIZATION))
+                .andThen(shooter.sysIdQuasistatic(SysIdRoutine.Direction.kForward)));
+
+    driver
+        .y()
+        .whileTrue(
+            Commands.runOnce(() -> shooter.setState(Shooter.State.CHARACTERIZATION))
+                .andThen(shooter.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)));
   }
 
   public void createOperatorBindings() {

diff --git a/src/main/java/frc/robot/subsystems/shooterIO/Shooter.java b/src/main/java/frc/robot/subsystems/shooterIO/Shooter.java
@@ -1,10 +1,22 @@
 package frc.robot.subsystems.shooterIO;
 
+import static edu.wpi.first.units.MutableMeasure.mutable;
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.Rotations;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+import static edu.wpi.first.units.Units.Volts;
+
 import edu.wpi.first.math.filter.Debouncer;
 import edu.wpi.first.math.filter.Debouncer.DebounceType;
+import edu.wpi.first.units.Angle;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.MutableMeasure;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.Voltage;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Robot;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
@@ -124,6 +136,7 @@ public enum State {
     OFF(() -> 0, () -> 0, () -> 0, () -> true),
     OUTTAKE_BACKWARDS(() -> -4000, () -> -4000, () -> -5, () -> true),
     CLEANING(() -> 10, () -> 10, () -> 1, () -> true),
+    CHARACTERIZATION(() -> 0, () -> 0, () -> 0, () -> true),
     FEEDER_SHOT(
         feederShotRPM,
         feederShotRPM,
@@ -141,19 +154,57 @@ public enum State {
   private final ShooterIO IO;
   public final ShooterInputsAutoLogged inputs;
 
+  private final SysIdRoutine sysIdRoutine;
+
+  // Mutable holder for unit-safe voltage values, persisted to avoid reallocation.
+  private final MutableMeasure<Voltage> m_appliedVoltage = mutable(Volts.of(0));
+  // Mutable holder for unit-safe linear distance values, persisted to avoid reallocation.
+  private final MutableMeasure<Angle> m_angle = mutable(Rotations.of(0));
+  // Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation.
+  private final MutableMeasure<Velocity<Angle>> m_velocity = mutable(RotationsPerSecond.of(0));
+
   public Shooter(ShooterIO IO) {
     this.IO = IO;
     this.inputs = new ShooterInputsAutoLogged();
     this.IO.updateInputs(inputs, state);
+
+    sysIdRoutine =
+        new SysIdRoutine(
+            new SysIdRoutine.Config(),
+            new SysIdRoutine.Mechanism(
+                (Measure<Voltage> volts) -> {
+                  this.IO.setShooterVolts(volts.in(Volts), volts.in(Volts));
+                },
+                log -> {
+                  log.motor("shooter")
+                      .voltage(m_appliedVoltage.mut_replace(inputs.leftOutputVoltage, Volts))
+                      .angularPosition(m_angle.mut_replace(inputs.leftPosDeg, Degrees))
+                      .angularVelocity(
+                          m_velocity.mut_replace(inputs.leftSpeedRPM / 60.0, RotationsPerSecond));
+                },
+                this));
+  }
+
+  public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+    return sysIdRoutine.quasistatic(direction);
+  }
+
+  public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+    return sysIdRoutine.dynamic(direction);
   }
 
   @Override
   public void periodic() {
     IO.updateInputs(inputs, state);
+    Logger.processInputs("Shooter", inputs);
 
     boolean shouldSpinFeeder = debouncer.calculate(isAtTarget());
     Logger.recordOutput("Shooter/Should spin feeder", shouldSpinFeeder);
 
+    if (state == State.CHARACTERIZATION) {
+      return;
+    }
+
     if (state == State.INTAKING && hasGamePiece()) {
       state = State.HOLDING_GP;
     }
@@ -181,7 +232,6 @@ public void periodic() {
       IO.setMotorSetPoint(
           state.leftRpm.getAsDouble() + differential, state.rightRpm.getAsDouble() - differential);
     }
-    Logger.processInputs("Shooter", inputs);
   }
 
   @AutoLogOutput(key = "Shooter/isAtTarget")

diff --git a/src/main/java/frc/robot/subsystems/shooterIO/ShooterIOSim.java b/src/main/java/frc/robot/subsystems/shooterIO/ShooterIOSim.java
@@ -25,6 +25,8 @@ public class ShooterIOSim implements ShooterIO {
 
   private static final FlywheelSim feeder = new FlywheelSim(DCMotor.getKrakenX60(1), 1.0, 0.0001);
 
+  private boolean useFlywheelPID = false;
+
   private double leftVolts, rightVolts, feederVolts;
   Timer fakeGamepieceTimer = new Timer();
 
@@ -36,19 +38,23 @@ public ShooterIOSim() {
 
   @Override
   public void updateInputs(ShooterInputsAutoLogged inputs, Shooter.State state) {
-    leftFlyWheel.update(0.02);
-    rightFlyWheel.update(0.02);
-    feeder.update(0.02);
+    if (useFlywheelPID) {
+      leftVolts =
+          MathUtil.clamp(leftController.calculate(leftFlyWheel.getAngularVelocityRPM()), -12, 12);
+      rightVolts =
+          MathUtil.clamp(rightController.calculate(rightFlyWheel.getAngularVelocityRPM()), -12, 12);
+    }
 
-    leftVolts =
-        MathUtil.clamp(leftController.calculate(leftFlyWheel.getAngularVelocityRPM()), -12, 12);
-    rightVolts =
-        MathUtil.clamp(rightController.calculate(rightFlyWheel.getAngularVelocityRPM()), -12, 12);
+    System.err.println(useFlywheelPID + " - updating with volts " + leftVolts);
 
     leftFlyWheel.setInputVoltage(leftVolts);
     rightFlyWheel.setInputVoltage(rightVolts);
     feeder.setInputVoltage(feederVolts);
 
+    leftFlyWheel.update(0.02);
+    rightFlyWheel.update(0.02);
+    feeder.update(0.02);
+
     inputs.leftOutputVoltage = this.leftVolts;
     inputs.rightOutputVoltage = this.rightVolts;
 
@@ -100,6 +106,7 @@ public void updateInputs(ShooterInputsAutoLogged inputs, Shooter.State state) {
   public void setMotorSetPoint(double leftRPM, double rightRPM) {
     leftController.setSetpoint(leftRPM);
     rightController.setSetpoint(rightRPM);
+    useFlywheelPID = true;
   }
 
   @Override
@@ -108,5 +115,10 @@ public void setFeederVolts(double volts) {
   }
 
   @Override
-  public void setShooterVolts(double lVolts, double rVolts) {}
+  public void setShooterVolts(double lVolts, double rVolts) {
+    System.err.println("set shooter volts to " + lVolts);
+    useFlywheelPID = false;
+    leftVolts = lVolts;
+    rightVolts = rVolts;
+  }
 }
diff --git a/src/main/java/frc/robot/util/RedHawkUtil.java b/src/main/java/frc/robot/util/RedHawkUtil.java
@@ -93,6 +93,7 @@ public static void maybeFlipLog(ChoreoTrajectory traj) {
     var alliance = DriverStation.getAlliance();
     if (alliance.isEmpty()) {
       Logger.recordOutput("SHOULD_BE_FLIPPING", false);
+      return;
     }
 
     if (alliance.get() == Alliance.Blue) {