diff --git a/README.md b/README.md index 05bfa30..4751ada 100644 --- a/README.md +++ b/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. diff --git a/src/main/java/frc/lib/UnitTestingUtil.java b/src/main/java/frc/lib/UnitTestingUtil.java index a4921dc..17180ba 100644 --- a/src/main/java/frc/lib/UnitTestingUtil.java +++ b/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(); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 697759a..15b8da7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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); } diff --git a/src/main/java/frc/robot/commands/WheelRadiusCharacterization.java b/src/main/java/frc/robot/commands/WheelRadiusCharacterization.java index 88dbcac..9ccd609 100644 --- a/src/main/java/frc/robot/commands/WheelRadiusCharacterization.java +++ b/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); + } } } diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index ce67a3d..1f52241 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/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; diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 21bcd51..ee06b05 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/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(); } } diff --git a/src/main/java/frc/robot/utils/VisionPoseEstimator.java b/src/main/java/frc/robot/utils/VisionPoseEstimator.java index 6ff0035..fcf9f6f 100644 --- a/src/main/java/frc/robot/utils/VisionPoseEstimator.java +++ b/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(); } diff --git a/src/test/java/frc/robot/RobotTest.java b/src/test/java/frc/robot/RobotTest.java index 8fd9b75..c5e4c1a 100644 --- a/src/test/java/frc/robot/RobotTest.java +++ b/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); } diff --git a/src/test/java/frc/robot/WheelRadiusCharacterizationTest.java b/src/test/java/frc/robot/WheelRadiusCharacterizationTest.java new file mode 100644 index 0000000..424ae3e --- /dev/null +++ b/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); + } +}