Static Characterization for Arm and Elevator

src/main/java/frc/robot/Ports.java

@@ -19,21 +19,21 @@ public class Ports {
     /* SUBSYSTEM CAN DEVICE IDS */
     public static final CanDeviceId CLAW_ROLLER = new CanDeviceId(16, "rio");
 
-    public static final CanDeviceId ELEVATOR_MAIN = new CanDeviceId(18, "rio");
-    public static final CanDeviceId ELEVATOR_FOLLOWER = new CanDeviceId(19, "rio");
+    public static final CanDeviceId ELEVATOR_MAIN = new CanDeviceId(21, "rio");
+    public static final CanDeviceId ELEVATOR_FOLLOWER = new CanDeviceId(22, "rio");
     public static final CanDeviceId ELEVATOR_CANCODER = new CanDeviceId(20, "rio");
 
-    public static final CanDeviceId ARM_MAIN = new CanDeviceId(21, "rio");
-    public static final CanDeviceId ARM_FOLLOWER = new CanDeviceId(22, "rio");
-    public static final CanDeviceId ARM_CANCODER = new CanDeviceId(23, "rio");
+    public static final CanDeviceId ARM_MAIN = new CanDeviceId(18, "rio");
+    public static final CanDeviceId ARM_FOLLOWER = new CanDeviceId(30, "rio"); // no arm follower
+    public static final CanDeviceId ARM_CANCODER = new CanDeviceId(17, "rio");
 
     public static final CanDeviceId PROFROLLER_MAIN = new CanDeviceId(24, "rio");
     public static final CanDeviceId PROFROLLER_FOLLOWER = new CanDeviceId(25, "rio");
 
     public static final CanDeviceId CLAW_LASERCAN = new CanDeviceId(33, "rio");
     public static final CanDeviceId RAMP_LASERCAN = new CanDeviceId(34, "rio");
 
-    public static final CanDeviceId CLIMBER = new CanDeviceId(26, "rio");
+    public static final CanDeviceId CLIMBER = new CanDeviceId(23, "rio");
 
     /*
      * public static final CanDeviceId INTAKE_PIVOT = new CanDeviceId(8, "canivore1"); public static

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

@@ -174,6 +174,8 @@ public RobotContainer()
             "Drive SysId (Dynamic Forward)", m_drive.sysIdDynamic(SysIdRoutine.Direction.kForward));
         m_autoChooser.addOption(
             "Drive SysId (Dynamic Reverse)", m_drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+        m_autoChooser.addOption("Elevator static", m_profiledElevator.staticCharacterization(2.0));
+        m_autoChooser.addOption("Arm static", m_profiledArm.staticCharacterization(2.0));
 
         // Configure the controller button and joystick bindings
         configureControllerBindings();

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

@@ -1,9 +1,13 @@
 package frc.robot.subsystems.Arm;
 
+import org.littletonrobotics.junction.Logger;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.subsystems.GenericMotionProfiledSubsystem.GenericMotionProfiledSubsystem;
 import frc.robot.subsystems.GenericMotionProfiledSubsystem.GenericMotionProfiledSubsystem.TargetState;
+import frc.robot.util.LoggedTunableNumber;
 import frc.robot.util.Util;
 import lombok.Getter;
 import lombok.RequiredArgsConstructor;
@@ -23,7 +27,8 @@ public enum State implements TargetState {
         LEVEL_2(Units.degreesToRotations(30.0), 0.0, ProfileType.MM_POSITION),
         LEVEL_3(Units.degreesToRotations(30.0), 0.0, ProfileType.MM_POSITION),
         LEVEL_4(Units.degreesToRotations(60.0), 0.0, ProfileType.MM_POSITION),
-        GROUND(Units.degreesToRotations(105.0), 0.0, ProfileType.MM_POSITION);
+        GROUND(Units.degreesToRotations(105.0), 0.0, ProfileType.MM_POSITION),
+        CHARACTERIZATION(0.0, 0.0, ProfileType.CHARACTERIZATION);
 
         private final double output;
         private final double feedFwd;
@@ -37,11 +42,15 @@ public enum State implements TargetState {
 
     private final boolean debug = true;
 
+    /* For adjusting the Arm's static characterization velocity threshold */
+    private static final LoggedTunableNumber staticCharacterizationVelocityThresh =
+    new LoggedTunableNumber("Arm/StaticCharacterizationVelocityThresh", 0.1);
+
     public Arm(ArmIO io, boolean isSim)
     {
         super(ProfileType.MM_POSITION, ArmConstants.kSubSysConstants, io, isSim);
     }
-
+    
     /** Constructor */
     public Command setStateCommand(State state)
     {
@@ -52,4 +61,31 @@ public boolean atPosition(double tolerance)
     {
         return Util.epsilonEquals(io.getPosition(), state.output, Math.max(0.0001, tolerance));
     }
+
+    public Command staticCharacterization(double outputRampRate) {
+        final StaticCharacterizationState characterizationState = new StaticCharacterizationState();
+        Timer timer = new Timer();
+        return Commands.startRun(
+            () -> {
+                this.state = State.CHARACTERIZATION;
+                timer.restart(); // Starts the timer that tracks the time of the characterization
+            },
+            () -> {
+                characterizationState.characterizationOutput = outputRampRate * timer.get();
+              io.runCurrent(characterizationState.characterizationOutput);
+              Logger.recordOutput(
+                  "Arm/StaticCharacterizationOutput", characterizationState.characterizationOutput);
+            })
+        .until(() -> inputs.velocityRps * 2 * Math.PI >= staticCharacterizationVelocityThresh.get())
+        .finallyDo(
+            () -> {
+              timer.stop();
+              Logger.recordOutput("Arm/CharacterizationOutput", characterizationState.characterizationOutput);
+              this.state = State.HOME;
+            });
+    }
+
+    private static class StaticCharacterizationState {
+        public double characterizationOutput = 0.0;
+    }
 }

src/main/java/frc/robot/subsystems/Elevator/Elevator.java

@@ -1,15 +1,18 @@
 package frc.robot.subsystems.Elevator;
 
+import org.littletonrobotics.junction.Logger;
 import edu.wpi.first.math.filter.Debouncer;
 import edu.wpi.first.math.filter.Debouncer.DebounceType;
 import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.subsystems.GenericMotionProfiledSubsystem.GenericMotionProfiledSubsystem;
 import frc.robot.subsystems.GenericMotionProfiledSubsystem.GenericMotionProfiledSubsystem.TargetState;
+import frc.robot.util.LoggedTunableNumber;
 import frc.robot.util.Util;
 import lombok.Getter;
 import lombok.RequiredArgsConstructor;
@@ -33,7 +36,8 @@ public enum State implements TargetState {
         CORAL_STATION(0.6, 0.0, ProfileType.MM_POSITION),
         ALGAE_LOWER(0.5, 0.0, ProfileType.MM_POSITION),
         ALGAE_UPPER(0.8, 0.0, ProfileType.MM_POSITION),
-        NET(9.0, 0.0, ProfileType.MM_POSITION);
+        NET(9.0, 0.0, ProfileType.MM_POSITION),
+        CHARACTERIZATION(0.0, 0.0, ProfileType.CHARACTERIZATION);
 
         private final double output;
         private final double feedFwd;
@@ -47,6 +51,10 @@ public enum State implements TargetState {
     @Getter
     public final Alert homedAlert = new Alert("NEW HOME SET", Alert.AlertType.kInfo);
 
+    /* For adjusting the Arm's static characterization velocity threshold */
+    private static final LoggedTunableNumber staticCharacterizationVelocityThresh =
+    new LoggedTunableNumber("Elevator/StaticCharacterizationVelocityThresh", 0.1);
+
     /** Constructor */
     public Elevator(ElevatorIO io, boolean isSim)
     {
@@ -82,4 +90,31 @@ public Command homedAlertCommand()
             Commands.waitSeconds(1),
             new InstantCommand(() -> homedAlert.set(false)));
     }
+
+    public Command staticCharacterization(double outputRampRate) {
+        final StaticCharacterizationState state = new StaticCharacterizationState();
+        Timer timer = new Timer();
+        return Commands.startRun(
+            () -> {
+              this.state = State.CHARACTERIZATION;
+              timer.restart(); // Starts the timer that tracks the time of the characterization
+            },
+            () -> {
+              state.characterizationOutput = outputRampRate * timer.get();
+              io.runCurrent(state.characterizationOutput);
+              Logger.recordOutput(
+                  "Elevator/StaticCharacterizationOutput", state.characterizationOutput);
+            })
+        .until(() -> inputs.velocityRps * 2 * Math.PI >= staticCharacterizationVelocityThresh.get())
+        .finallyDo(
+            () -> {
+              timer.stop();
+              Logger.recordOutput("Elevator/CharacterizationOutput", state.characterizationOutput);
+              this.state = State.HOME;
+            });
+    }
+
+    private static class StaticCharacterizationState {
+        public double characterizationOutput = 0.0;
+    }
 }

src/main/java/frc/robot/subsystems/GenericMotionProfiledSubsystem/GenericMotionProfiledSubsystem.java

@@ -19,7 +19,8 @@ public enum ProfileType {
         MM_POSITION,
         MM_VELOCITY,
         OPEN_VOLTAGE,
-        OPEN_CURRENT
+        OPEN_CURRENT, 
+        CHARACTERIZATION
     }
 
     public interface TargetState {
@@ -155,6 +156,9 @@ public void periodic()
                 /* Run Open Loop using specified current in amps */
                 io.runCurrent(getState().getOutput());
                 break;
+            case CHARACTERIZATION:
+                /* Run Open Loop for characterization in the child subsystem class's characterization command. Do nothing here.*/
+                break;
         }
 
         displayInfo();