Merge pull request #6 from Team334/sub-request

wheel radius characterization

README.md

@@ -23,9 +23,6 @@ A base project for future robots that has CTRE generated swerve drive code and P
 ![github templates](https://docs.github.com/assets/cb-76823/mw-1440/images/help/repository/use-this-template-button.webp)
 
 # Calibration Process
-- Run `calibration/calibrator.zsh` in zshell in WSL (make sure all packages are installed).
+- Run `calibration/calibrator.zsh` in zshell in WSL in the `calibration` folder (make sure all packages are installed).
 - Analyze calibration as according to the mrcal tour.
-- Convert the `.cameramodel` calibration data format into `.json` using `calibration/mrcal_to_photon.py` in WSL.
-
-## 2025 Latest Beta Known Issues
-- To run sim, do `./gradlew simulateJava` instead of using the WPILib extension (for Epilogue to work).
+- If the calibration was good, load the generated JSON into photon vision.

src/main/java/frc/lib/UnitTestingUtil.java

@@ -36,6 +36,7 @@ public static void setupTests() {
     _ntInst = NetworkTableInstance.create();
 
     DriverStationSim.setEnabled(true);
+    DriverStation.silenceJoystickConnectionWarning(true);
     DriverStationSim.notifyNewData();
 
     assert DriverStation.isEnabled();

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

@@ -28,6 +28,7 @@
 import frc.robot.Constants.Ports;
 import frc.robot.Constants.SwerveConstants;
 import frc.robot.commands.Autos;
+import frc.robot.commands.WheelRadiusCharacterization;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.Swerve;
 
@@ -89,6 +90,9 @@ public Robot(NetworkTableInstance ntInst) {
                 runOnce(() -> DataLogManager.log("Robot Self Check Successful!")))
             .withName("Robot Self Check"));
 
+    SmartDashboard.putData(new WheelRadiusCharacterization(_swerve));
+    SmartDashboard.putData(runOnce(FaultLogger::clear).withName("Clear Faults"));
+
     addPeriodic(FaultLogger::update, 1);
   }
 

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

@@ -4,33 +4,109 @@
 
 package frc.robot.commands;
 
+import static edu.wpi.first.units.Units.*;
+
+import dev.doglog.DogLog;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.lib.FaultLogger;
+import frc.lib.FaultsTable.FaultType;
+import frc.robot.Constants.SwerveConstants;
+import frc.robot.generated.TunerConstants;
+import frc.robot.subsystems.Swerve;
 
-/*
- * Runs a characterization procudure to find the wheel radius of the swerve modules.
- * TODO
- */
 public class WheelRadiusCharacterization extends Command {
-  /** Creates a new WheelRadiusCharacterization. */
-  public WheelRadiusCharacterization() {
-    // Use addRequirements() here to declare subsystem dependencies.
+  private final Swerve _swerve;
+
+  private double _lastGyroYaw;
+  private double _accumGyroYaw;
+
+  private double[] _initialWheelDistances;
+
+  private double _wheelRadius;
+
+  public WheelRadiusCharacterization(Swerve swerve) {
+    setName("Wheel Radius Characterization");
+
+    _swerve = swerve;
+    addRequirements(_swerve);
+  }
+
+  /** Get the estimated wheel radius. */
+  public Distance getWheelRadius() {
+    return Meters.of(_wheelRadius);
+  }
+
+  // returns the distance traveled by each individual drive wheel in radians
+  private double[] getWheelDistancesRadians() {
+    SwerveModulePosition[] positions = _swerve.getState().ModulePositions;
+
+    double[] distances = new double[4];
+
+    for (int i = 0; i < _swerve.getModules().length; i++) {
+      // radians = (meters / radius)
+      distances[i] = positions[i].distanceMeters / TunerConstants.kWheelRadius.in(Meters);
+    }
+
+    return distances;
   }
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+    _lastGyroYaw = _swerve.getHeading().getRadians();
+    _accumGyroYaw = 0;
+
+    _initialWheelDistances = getWheelDistancesRadians();
+
+    _wheelRadius = 0;
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {}
+  public void execute() {
+    _swerve.drive(0, 0, -1); // TODO: make this tuneable
+
+    // add the heading traveled since the last execute call to the accum yaw
+    // angle modulus is needed for when the gyro crosses from [0, pi] to [-pi, 0]
+    _accumGyroYaw +=
+        Math.abs(MathUtil.angleModulus(_swerve.getHeading().getRadians() - _lastGyroYaw));
+    _lastGyroYaw = _swerve.getHeading().getRadians();
+
+    DogLog.log(
+        "Wheel Radius Characterization/Meters Turned",
+        SwerveConstants.driveRadius.in(Meters) * _accumGyroYaw);
+
+    double averageWheelDistance = 0;
+    double[] wheelDistances = getWheelDistancesRadians();
+
+    for (int i = 0; i < _swerve.getModules().length; i++) {
+      // wheel distances should be positive always since the turning distance will always be
+      // positive
+      averageWheelDistance += Math.abs(wheelDistances[i] - _initialWheelDistances[i]);
+    }
+
+    averageWheelDistance /= _swerve.getModules().length;
+
+    _wheelRadius = (SwerveConstants.driveRadius.in(Meters) * _accumGyroYaw) / averageWheelDistance;
+
+    DogLog.log(
+        "Wheel Radius Characterization/Estimated Wheel Radius", Units.metersToInches(_wheelRadius));
+  }
 
   // Called once the command ends or is interrupted.
   @Override
-  public void end(boolean interrupted) {}
+  public void end(boolean interrupted) {
+    _swerve.drive(0, 0, 0);
 
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
+    if (_accumGyroYaw <= 2 * Math.PI) {
+      FaultLogger.report("Need more info for characterization!", FaultType.ERROR);
+    } else {
+      FaultLogger.report(
+          "Wheel Radius (inches): " + Units.metersToInches(_wheelRadius), FaultType.INFO);
+    }
   }
 }

src/main/java/frc/robot/generated/TunerConstants.java

@@ -86,7 +86,7 @@ public class TunerConstants {
 
   private static final double kDriveGearRatio = 7.363636364;
   private static final double kSteerGearRatio = 12.8;
-  private static final Distance kWheelRadius = Inches.of(2.167);
+  public static final Distance kWheelRadius = Inches.of(2.167);
 
   private static final boolean kInvertLeftSide = false;
   private static final boolean kInvertRightSide = true;

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

@@ -496,6 +496,8 @@ public Command selfCheck() {
   public void close() {
     super.close();
 
+    _cameras.forEach(cam -> cam.close());
+
     _simNotifier.close();
   }
 }

src/main/java/frc/robot/utils/VisionPoseEstimator.java

@@ -363,7 +363,7 @@ public PhotonCameraSim getCameraSim() {
   }
 
   @Override
-  public void close() throws Exception {
+  public void close() {
     _camera.close();
     _cameraSim.close();
   }

src/test/java/frc/robot/RobotTest.java

@@ -25,6 +25,9 @@ public void setup() {
 
   @AfterEach
   public void close() throws Exception {
+    DriverStationSim.setFmsAttached(false);
+    DriverStationSim.notifyNewData();
+
     reset(_robot);
   }
 

src/test/java/frc/robot/WheelRadiusCharacterizationTest.java

@@ -0,0 +1,46 @@
+package frc.robot;
+
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.Seconds;
+import static frc.lib.UnitTestingUtil.*;
+import static org.junit.jupiter.api.Assertions.*;
+
+import frc.robot.commands.WheelRadiusCharacterization;
+import frc.robot.generated.TunerConstants;
+import frc.robot.subsystems.Swerve;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+public class WheelRadiusCharacterizationTest {
+  // chassis omega during characterization (TODO: this would be tuneable later)
+  private final double velOmega = 1;
+
+  private Swerve _swerve;
+  private WheelRadiusCharacterization _characterization;
+
+  @BeforeEach
+  public void setup() {
+    setupTests();
+
+    _swerve = TunerConstants.createDrivetrain();
+    _characterization = new WheelRadiusCharacterization(_swerve);
+  }
+
+  @AfterEach
+  public void close() throws Exception {
+    reset(_swerve);
+  }
+
+  @Test
+  public void wheelRadiusCharacterization() {
+    // run enough ticks to complete 4pi radians at the given characterization omega
+    run(_characterization, (int) ((Math.PI * 4 / velOmega) / TICK_RATE.in(Seconds)));
+
+    // accept with a tolerance of 0.002 meters (i think that's good?)
+    assertEquals(
+        TunerConstants.kWheelRadius.in(Meters),
+        _characterization.getWheelRadius().in(Meters),
+        2e-3);
+  }
+}