Bound the WheelRadiusCharacterization command to the trigger for testing

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

@@ -18,6 +18,7 @@
 import frc.robot.constants.TunerConstants;
 import frc.robot.constants.GlobalConstants.ControllerConstants;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
+import frc.robot.subsystems.WheelRadiusCharacterization;
 
 public class RobotContainer {
     private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed
@@ -74,6 +75,8 @@ private void configureBindings() {
             point.withModuleDirection(new Rotation2d(-operatorController.getLeftYAxis().get(), -operatorController.getLeftXAxis().get()))
         ));
 
+        leftDriveController.getTrigger().whileTrue(new WheelRadiusCharacterization(WheelRadiusCharacterization.Direction.CLOCKWISE, drivetrain));
+
         // Run SysId routines when holding back/start and X/Y.
         // Note that each routine should be run exactly once in a single log.
         operatorController.getBack().and(operatorController.getY()).whileTrue(drivetrain.sysIdDynamic(Direction.kForward));

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

@@ -294,14 +294,7 @@ public SwerveRequest driveRobotRelative(double xVelocity, double yVelocity, doub
         // Note: it is important to not discretize speeds before or after
         // using the setpoint generator, as it will discretize them for you
         ChassisSpeeds speeds = new ChassisSpeeds(xVelocity, yVelocity, rotationRate);
-        previousSetpoint = setpointGenerator.generateSetpoint(
-            previousSetpoint, // The previous setpoint
-            speeds, // The desired target speeds
-            0.02 // The loop time of the robot code, in seconds
-        );
-        return m_applyRobotSpeeds.withSpeeds(previousSetpoint.robotRelativeSpeeds())
-            .withWheelForceFeedforwardsX(previousSetpoint.feedforwards().robotRelativeForcesXNewtons())
-            .withWheelForceFeedforwardsY(previousSetpoint.feedforwards().robotRelativeForcesYNewtons());
+        return m_applyFieldSpeedsOrbit.withChassisSpeeds(speeds);
             // Method that will drive the robot given target module states
     }
 

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

@@ -16,7 +16,7 @@
 
 import org.littletonrobotics.junction.networktables.LoggedNetworkNumber;
 
-public class WheelRadiusCharacterization {
+public class WheelRadiusCharacterization extends Command {
     private static final LoggedNetworkNumber characterizationSpeed =
       new LoggedNetworkNumber("WheelRadiusCharacterization/SpeedRadsPerSec", 0.1);
     private static final Pigeon2 pigeon = new Pigeon2(0,"");
@@ -45,11 +45,13 @@ private Direction(double speed) {
 
     private final CommandSwerveDrivetrain drive;
 
-    private double[] startWheelPositions;
+    private double[] startWheelPositions = new double[4];
 
     public WheelRadiusCharacterization(Direction omegaDirection, CommandSwerveDrivetrain drivetrain) {
         this.omegaDirection = omegaDirection;
         this.drive = drivetrain;
+
+        addRequirements(drivetrain);
       }