Wheel Radius Characterization

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

@@ -0,0 +1,119 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.RobotState;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.TunerConstants;
+
+import com.ctre.phoenix6.hardware.Pigeon2;
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.Pigeon2Configuration;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+
+import static edu.wpi.first.units.Units.Radians;
+
+import java.util.function.DoubleSupplier;
+
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.LoggedNetworkNumber;
+
+public class WheelRadiusCharacterization extends Command {
+    private static final LoggedNetworkNumber characterizationSpeed =
+      new LoggedNetworkNumber("WheelRadiusCharacterization/SpeedRadsPerSec", 1);
+    private static final Pigeon2 pigeon = new Pigeon2(0,"");
+    private double driveBaseRadius = Math.hypot(TunerConstants.BackLeft.LocationX, TunerConstants.BackLeft.LocationY);
+    private static DoubleSupplier gyroYawRadSupplier = ()->(pigeon.getYaw().refresh().getValue().in(Radians));
+
+    private double lastGyroYawRads = 0.0;
+    private double initialGyroYawRads = 0.0;
+
+    public double currentEffectiveWheelRadius = 0.0;
+
+
+    public enum Direction {
+        CLOCKWISE(-1),
+        COUNTER_CLOCKWISE(1);
+
+        private final double value;
+
+        private Direction(double speed) {
+            value = speed;
+        }
+    }
+
+    private final Direction omegaDirection;
+    private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(1.0);
+
+    private final CommandSwerveDrivetrain drive;
+
+    private final Timer startTimer  = new Timer();
+
+    private double[] startWheelPositions = new double[4];
+
+    private boolean hasntStarted = true;
+
+    public WheelRadiusCharacterization(Direction omegaDirection, CommandSwerveDrivetrain drivetrain) {
+        this.omegaDirection = omegaDirection;
+        this.drive = drivetrain;
+
+        addRequirements(drivetrain);
+      }
+
+
+    public void initialize() {
+        // Reset
+        hasntStarted = true;
+        startTimer.restart();
+
+
+
+        omegaLimiter.reset(0);
+    }
+
+
+    public void execute()
+    {
+        drive.driveRobotRelative(0, 0, omegaLimiter.calculate(omegaDirection.value * characterizationSpeed.get()));
+
+        // Get yaw and wheel positions
+
+        if (startTimer.hasElapsed(2) && hasntStarted) {
+            initialGyroYawRads = gyroYawRadSupplier.getAsDouble();
+
+            for(int x = 0; x<4 ; x++)
+            {
+                startWheelPositions[x] = drive.getState().ModulePositions[x].distanceMeters / TunerConstants.BackLeft.WheelRadius;
+            }
+
+            hasntStarted = false;
+        }
+
+        if (hasntStarted) return;
+
+        lastGyroYawRads = gyroYawRadSupplier.getAsDouble();
+
+        double averageWheelPosition = 0.0;
+        double[] wheelPositions = new double[4];
+        for(int x = 0; x<4 ; x++)
+        {
+            wheelPositions[x] = drive.getState().ModulePositions[x].distanceMeters / TunerConstants.BackLeft.WheelRadius;
+        }
+        for (int i = 0; i < 4; i++) {
+            averageWheelPosition += Math.abs(wheelPositions[i] - startWheelPositions[i]);
+        }
+        averageWheelPosition /= 4.0;
+
+        currentEffectiveWheelRadius = ((lastGyroYawRads - initialGyroYawRads) * driveBaseRadius) / averageWheelPosition;
+
+        Logger.recordOutput("/Drive/WheelRadiusCalculated", currentEffectiveWheelRadius * 100.0);
+        Logger.recordOutput("/Drive/WheelRadiusGyro", (lastGyroYawRads - initialGyroYawRads) * driveBaseRadius);
+        Logger.recordOutput("/Drive/WheelPosition", averageWheelPosition);
+    }
+
+
+}