copy paste copy schmaste sys id routines

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

@@ -14,6 +14,7 @@
 package frc.robot;
 
 import choreo.auto.AutoChooser;
+import com.ctre.phoenix6.SignalLogger;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Threads;
 import edu.wpi.first.wpilibj.Timer;

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

@@ -35,6 +35,7 @@
 import frc.robot.commands.ArmCommands.BendCommand;
 import frc.robot.commands.ArmCommands.ManualArmCommand;
 import frc.robot.commands.DriveCommands.RobotRelativeDriveCommand;
+import frc.robot.commands.DriveCommands.WheelBaseCharacterizationRoutineCommand;
 import frc.robot.commands.ElevatorCommands.ElevatorGoToOriginCommand;
 import frc.robot.commands.ElevatorCommands.ElevatorSetPointCommand;
 import frc.robot.commands.ElevatorCommands.ManualElevatorCommand;
@@ -192,16 +193,30 @@ public RobotContainer() {
 
     this.autoSubsystem = new AutoSubsystem(clawSubsystem, driveSubsystem);
     autoChooser = new AutoChooser();
+
     autoChooser.addRoutine("FourL4CoralBottom", AutoSubsystem::FourL4CoralBottomRoutine);
     autoChooser.addRoutine("FourL4CoralTop", AutoSubsystem::FourL4CoralTopRoutine);
     autoChooser.addRoutine("ThreeL4CoralBottom", AutoSubsystem::ThreeL4CoralBottomRoutine);
     autoChooser.addRoutine("ThreeL4CoralTop", AutoSubsystem::ThreeL4CoralTopRoutine);
+
     autoChooser.addCmd("DriveSysIDQuasistaticForward", () -> driveSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
     autoChooser.addCmd("DriveSysIDQuasistaticReverse", () -> driveSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
     autoChooser.addCmd("DriveSysIDDynamicForward", () -> driveSubsystem.sysIdDynamic(SysIdRoutine.Direction.kForward));
     autoChooser.addCmd("DriveSysIDDynamicReverse", () -> driveSubsystem.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+    autoChooser.addCmd("WheelBaseCharacterization", () -> new WheelBaseCharacterizationRoutineCommand(driveSubsystem));
+
+//    autoChooser.addCmd("ElevatorSysIDQuasistaticForward", () -> elevatorSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+//    autoChooser.addCmd("ElevatorSysIDQuasistaticReverse", () -> elevatorSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+//    autoChooser.addCmd("ElevatorSysIDDynamicForward", () -> elevatorSubsystem.sysIdDynamic(SysIdRoutine.Direction.kForward));
+//    autoChooser.addCmd("ElevatorSysIDDynamicReverse", () -> elevatorSubsystem.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+//
+//    autoChooser.addCmd("ArmSysIDQuasistaticForward", () -> armSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+//    autoChooser.addCmd("ArmSysIDQuasistaticReverse", () -> armSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+//    autoChooser.addCmd("ArmSysIDDynamicForward", () -> armSubsystem.sysIdDynamic(SysIdRoutine.Direction.kForward));
+//    autoChooser.addCmd("ArmSysIDDynamicReverse", () -> armSubsystem.sysIdDynamic(SysIdRoutine.Direction.kReverse));
 
     SmartDashboard.putData("autochooser", autoChooser);
+
     RobotModeTriggers.autonomous().whileTrue(autoChooser.selectedCommandScheduler());
 
     loadCommands();

src/main/java/frc/robot/commands/DriveCommands/WheelBaseCharacterizationRoutineCommand.java

@@ -0,0 +1,95 @@
+package frc.robot.commands.DriveCommands;
+
+import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import frc.robot.constants.DriveConstants;
+import frc.robot.subsystems.DriveSubsystem.DriveSubsystem;
+
+import java.text.DecimalFormat;
+import java.text.NumberFormat;
+
+
+/** Measures the robot's wheel radius by spinning in a circle. */
+public class WheelBaseCharacterizationRoutineCommand extends ParallelCommandGroup {
+    private static final double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec
+    private static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2
+    static SlewRateLimiter limiter = new SlewRateLimiter(WHEEL_RADIUS_RAMP_RATE);
+    static WheelRadiusCharacterizationState state = new WheelRadiusCharacterizationState();
+
+    public WheelBaseCharacterizationRoutineCommand(DriveSubsystem drive) {
+        super(Commands.parallel(
+                // Drive control sequence
+                Commands.sequence(
+                        // Reset acceleration limiter
+                        Commands.runOnce(
+                                () -> {
+                                    limiter.reset(0.0);
+                                }),
+
+                        // Turn in place, accelerating up to full speed
+                        Commands.run(
+                                () -> {
+                                    double speed = limiter.calculate(WHEEL_RADIUS_MAX_VELOCITY);
+                                    drive.runVelocity(new ChassisSpeeds(0.0, 0.0, speed));
+                                },
+                                drive)),
+
+                // Measurement sequence
+                Commands.sequence(
+                        // Wait for modules to fully orient before starting measurement
+                        Commands.waitSeconds(1.0),
+
+                        // Record starting measurement
+                        Commands.runOnce(
+                                () -> {
+                                    state.positions = drive.getWheelRadiusCharacterizationPositions();
+                                    state.lastAngle = drive.getRotation();
+                                    state.gyroDelta = 0.0;
+                                }),
+
+                        // Update gyro delta
+                        Commands.run(
+                                        () -> {
+                                            var rotation = drive.getRotation();
+                                            state.gyroDelta += Math.abs(rotation.minus(state.lastAngle).getRadians());
+                                            state.lastAngle = rotation;
+                                        })
+
+                                // When cancelled, calculate and print results
+                                .finallyDo(
+                                        () -> {
+                                            double[] positions = drive.getWheelRadiusCharacterizationPositions();
+                                            double wheelDelta = 0.0;
+                                            for (int i = 0; i < 4; i++) {
+                                                wheelDelta += Math.abs(positions[i] - state.positions[i]) / 4.0;
+                                            }
+                                            double wheelRadius = (state.gyroDelta * DriveConstants.driveBaseRadius) / wheelDelta;
+
+                                            NumberFormat formatter = new DecimalFormat("#0.000");
+                                            System.out.println(
+                                                    "********** Wheel Radius Characterization Results **********");
+                                            System.out.println(
+                                                    "\tWheel Delta: " + formatter.format(wheelDelta) + " radians");
+                                            System.out.println(
+                                                    "\tGyro Delta: " + formatter.format(state.gyroDelta) + " radians");
+                                            System.out.println(
+                                                    "\tWheel Radius: "
+                                                            + formatter.format(wheelRadius)
+                                                            + " meters, "
+                                                            + formatter.format(Units.metersToInches(wheelRadius))
+                                                            + " inches");
+                                        }))));
+    }
+
+    private static class WheelRadiusCharacterizationState {
+        double[] positions = new double[4];
+        Rotation2d lastAngle = new Rotation2d();
+        double gyroDelta = 0.0;
+    }
+}
+

src/main/java/frc/robot/constants/Constants.java

@@ -22,7 +22,7 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
-  public static final Mode currentMode = Mode.SIM;
+  public static final Mode currentMode = Mode.REAL;
 
   public static enum Mode {
     /**

src/main/java/frc/robot/constants/DriveConstants.java

@@ -40,6 +40,8 @@ public class DriveConstants {
     public static final double alignmentSpeed = 1;
 
     public static final double odometryFrequency = 100.0; // Hz
+
+    // update this
     public static final double trackWidth = Units.inchesToMeters(26.5);
     public static final double wheelBase = Units.inchesToMeters(26.5);
     public static final double driveBaseRadius = Math.hypot(trackWidth / 2.0, wheelBase / 2.0);

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

@@ -7,12 +7,16 @@
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.Unit;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.OperatorInput;
 import frc.robot.constants.Constants;
 import frc.robot.constants.ArmConstants;
 import org.littletonrobotics.junction.Logger;
 
+import static edu.wpi.first.units.Units.Volts;
+
 
 public class ArmSubsystem extends SubsystemBase {
     private ArmIO armIO;
@@ -23,6 +27,7 @@ public class ArmSubsystem extends SubsystemBase {
     private final ArmIOInputsAutoLogged armIOInputs;
     private ArmEncoderIOInputsAutoLogged armEncoderIOInputs = new ArmEncoderIOInputsAutoLogged();
     public double hoverAngle = 6969;
+    private final SysIdRoutine sysId;
 
     public ArmSubsystem(ArmIO armIO, ArmEncoderIO armIOEncoder) {
         if (Constants.currentMode == Constants.Mode.SIM) {
@@ -39,6 +44,17 @@ public ArmSubsystem(ArmIO armIO, ArmEncoderIO armIOEncoder) {
         this.armEncoderIOInputs =  new ArmEncoderIOInputsAutoLogged();
 
         armFeedback.setTolerance(ArmConstants.kArmToleranceRPS);
+
+        sysId =
+                new SysIdRoutine(
+                        new SysIdRoutine.Config(
+                                null,
+                                null,
+                                null,
+                                (state) -> Logger.recordOutput("ElevatorSubystem/SysIdState", state.toString())),
+                        new SysIdRoutine.Mechanism(
+                                (voltage) -> runCharacterization(voltage.in(Volts)), null, this));
+
     }
 
 
@@ -68,5 +84,21 @@ public void setArmVoltage(double volts){
         Logger.recordOutput("ArmSubsystem/outputVoltageAdjusted", outputVoltage);
         armIO.setArmMotorVoltage(outputVoltage);
     }
+
+    public void runCharacterization(double output) {
+        // bypasses limits, be careful!!!
+        armIO.setArmMotorVoltage(output);
+    }
+    /** Returns a command to run a quasistatic test in the specified direction. */
+    public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+        return run(() -> runCharacterization(0.0))
+                .withTimeout(1.0)
+                .andThen(sysId.quasistatic(direction));
+    }
+
+    /** Returns a command to run a dynamic test in the specified direction. */
+    public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+        return run(() -> runCharacterization(0.0)).withTimeout(1.0).andThen(sysId.dynamic(direction));
+    }
 }
 

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

@@ -9,13 +9,17 @@
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.OperatorInput;
 import frc.robot.constants.ArmConstants;
 import frc.robot.constants.Constants;
 import frc.robot.constants.ElevatorConstants;
 import org.littletonrobotics.junction.Logger;
 
+import static edu.wpi.first.units.Units.Volts;
+
 
 public class ElevatorSubsystem extends SubsystemBase {
     public ElevatorIO elevatorIO;
@@ -30,6 +34,7 @@ public class ElevatorSubsystem extends SubsystemBase {
     private final Mechanism2d elevator2D = new Mechanism2d(2, 2);
     private final MechanismRoot2d elevator2dRoot = elevator2D.getRoot("Elevator Root", 1, 0);
     private final MechanismLigament2d elevatorMech2d;
+    private final SysIdRoutine sysId;
     // add a method to get profileGoal = new TrapezoidProfile.State(5, 0); based on where you want the robot to switch setpoints to
     //after that, add a method to setpoint = m_profile.calculate(kDt, elevator Heights (L1,L2,etc), profile);
     public ElevatorSubsystem(ElevatorIO elevatorIO, ElevatorEncoderIO elevatorEncoderIO) {
@@ -52,6 +57,16 @@ public ElevatorSubsystem(ElevatorIO elevatorIO, ElevatorEncoderIO elevatorEncode
         setDefaultCommand(runOnce(elevatorIO::disable).andThen(run(() -> {})).withName("Idle"));
         SmartDashboard.putData("ElevatorSubsystem/mechanism", elevator2D);
 
+        // Configure SysId
+        sysId =
+                new SysIdRoutine(
+                        new SysIdRoutine.Config(
+                                null,
+                                null,
+                                null,
+                                (state) -> Logger.recordOutput("ElevatorSubystem/SysIdState", state.toString())),
+                        new SysIdRoutine.Mechanism(
+                                (voltage) -> runCharacterization(voltage.in(Volts)), null, this));
     }
 
 
@@ -87,4 +102,20 @@ public void setElevatorVoltage(double volts) {
         }
         elevatorIO.setElevatorMotorVoltage(outputVoltage);
     }
+
+    public void runCharacterization(double output) {
+        // bypasses limits, be careful!!!
+        elevatorIO.setElevatorMotorVoltage(output);
+    }
+    /** Returns a command to run a quasistatic test in the specified direction. */
+    public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+        return run(() -> runCharacterization(0.0))
+                .withTimeout(1.0)
+                .andThen(sysId.quasistatic(direction));
+    }
+
+    /** Returns a command to run a dynamic test in the specified direction. */
+    public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+        return run(() -> runCharacterization(0.0)).withTimeout(1.0).andThen(sysId.dynamic(direction));
+    }
 }