From 42a3ed390dc92155a52d9ba74739cdf6117b4b82 Mon Sep 17 00:00:00 2001 From: Elvis Osmanov <gamerprotime777@gmail.com> Date: Wed, 25 Dec 2024 15:18:13 -0500 Subject: [PATCH 1/6] wheel radius characterization --- .../commands/WheelRadiusCharacterization.java | 63 +++++++++++++++---- .../frc/robot/generated/TunerConstants.java | 2 +- .../java/frc/robot/subsystems/Swerve.java | 13 ++++ 3 files changed, 64 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/commands/WheelRadiusCharacterization.java b/src/main/java/frc/robot/commands/WheelRadiusCharacterization.java index 88dbcac..dc32879 100644 --- a/src/main/java/frc/robot/commands/WheelRadiusCharacterization.java +++ b/src/main/java/frc/robot/commands/WheelRadiusCharacterization.java @@ -4,33 +4,70 @@ package frc.robot.commands; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; +import frc.lib.FaultLogger; +import frc.lib.FaultsTable.FaultType; +import frc.robot.Constants.SwerveConstants; +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 = 0; + private double _accumGyroYaw = 0; + + private double[] _initialWheelPositions; + + private double _wheelRadius; + + public WheelRadiusCharacterization(Swerve swerve) { + _swerve = swerve; + addRequirements(_swerve); } // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + _lastGyroYaw = _swerve.getHeading().getRadians(); + _accumGyroYaw = 0; + + _initialWheelPositions = _swerve.getWheelPositionsRadians(); + } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + _swerve.drive(0, 0, 1); // Make this tunable + + _accumGyroYaw += MathUtil.angleModulus(_swerve.getHeading().getRadians() - _lastGyroYaw); + _lastGyroYaw = _swerve.getHeading().getRadians(); + + double averageWheelPosition = 0; + double[] wheelPositions = _swerve.getWheelPositionsRadians(); + + for(int i = 0; i < _swerve.getModules().length; i++){ + averageWheelPosition += Math.abs(wheelPositions[i] - _initialWheelPositions[i]); + } + + averageWheelPosition /= 4; + + _wheelRadius = (_accumGyroYaw + SwerveConstants.driveRadius.magnitude()) / averageWheelPosition; + } // 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 <= Math.PI * 2){ + FaultLogger.report("Need more info for characterization!", FaultType.ERROR); + } + else{ + System.out.println("Wheel Radius (inches): " + Units.metersToInches(_wheelRadius)); + } } } 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..d9c0104 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -24,6 +24,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.Notifier; @@ -41,11 +42,13 @@ import frc.robot.Constants.FieldConstants; import frc.robot.Constants.SwerveConstants; import frc.robot.Constants.VisionConstants; +import frc.robot.generated.TunerConstants; import frc.robot.Robot; import frc.robot.utils.SysId; import frc.robot.utils.VisionPoseEstimator; import frc.robot.utils.VisionPoseEstimator.VisionPoseEstimate; import java.util.ArrayList; +import java.util.Arrays; import java.util.HashSet; import java.util.List; import java.util.Set; @@ -417,6 +420,16 @@ public ChassisSpeeds getChassisSpeeds() { return getState().Speeds; } + /* Wrapper for getting swerve module positions in radians. */ + public double[] getWheelPositionsRadians(){ + double[] distances = new double[getModules().length]; + for (int i = 0; i < getModules().length; i++){ + distances[i] = getModule(i).getPosition(true).distanceMeters / TunerConstants.kWheelRadius.in(Meters); + } + + return distances; + } + // updates pose estimator with vision private void updateVisionPoseEstimates() { _acceptedEstimates.clear(); From e6e1c4ab336f2882e5ebe26f35e8560ebb9c96c7 Mon Sep 17 00:00:00 2001 From: PGgit08 <peter.mty8@gmail.com> Date: Fri, 27 Dec 2024 21:28:23 -0500 Subject: [PATCH 2/6] re-organized wheel radius characterization and changed some of the math --- src/main/java/frc/robot/Robot.java | 4 + .../commands/WheelRadiusCharacterization.java | 75 ++++++++++++++----- .../java/frc/robot/subsystems/Swerve.java | 13 ---- 3 files changed, 61 insertions(+), 31 deletions(-) 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 dc32879..063da5f 100644 --- a/src/main/java/frc/robot/commands/WheelRadiusCharacterization.java +++ b/src/main/java/frc/robot/commands/WheelRadiusCharacterization.java @@ -4,58 +4,97 @@ 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; - public class WheelRadiusCharacterization extends Command { - /** Creates a new WheelRadiusCharacterization. */ private final Swerve _swerve; - private double _lastGyroYaw = 0; - private double _accumGyroYaw = 0; + private double _lastGyroYaw; + private double _accumGyroYaw; - private double[] _initialWheelPositions; + 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() { _lastGyroYaw = _swerve.getHeading().getRadians(); _accumGyroYaw = 0; - _initialWheelPositions = _swerve.getWheelPositionsRadians(); + _initialWheelDistances = getWheelDistancesRadians(); + + _wheelRadius = 0; } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - _swerve.drive(0, 0, 1); // Make this tunable + _swerve.drive(0, 0, -1); // TODO: Make this tunable - _accumGyroYaw += MathUtil.angleModulus(_swerve.getHeading().getRadians() - _lastGyroYaw); + // 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(); - double averageWheelPosition = 0; - double[] wheelPositions = _swerve.getWheelPositionsRadians(); + DogLog.log( + "Wheel Radius Characterization/Meters Turned", + SwerveConstants.driveRadius.in(Meters) * _accumGyroYaw); - for(int i = 0; i < _swerve.getModules().length; i++){ - averageWheelPosition += Math.abs(wheelPositions[i] - _initialWheelPositions[i]); + 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]); } - averageWheelPosition /= 4; + averageWheelDistance /= _swerve.getModules().length; + + _wheelRadius = (SwerveConstants.driveRadius.in(Meters) * _accumGyroYaw) / averageWheelDistance; - _wheelRadius = (_accumGyroYaw + SwerveConstants.driveRadius.magnitude()) / averageWheelPosition; + DogLog.log( + "Wheel Radius Characterization/Estimated Wheel Radius", Units.metersToInches(_wheelRadius)); } // Called once the command ends or is interrupted. @@ -63,11 +102,11 @@ public void execute() { public void end(boolean interrupted) { _swerve.drive(0, 0, 0); - if (_accumGyroYaw <= Math.PI * 2){ + if (_accumGyroYaw <= 2 * Math.PI) { FaultLogger.report("Need more info for characterization!", FaultType.ERROR); - } - else{ - System.out.println("Wheel Radius (inches): " + Units.metersToInches(_wheelRadius)); + } else { + FaultLogger.report( + "Wheel Radius (inches): " + Units.metersToInches(_wheelRadius), FaultType.INFO); } } } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index d9c0104..21bcd51 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -24,7 +24,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.Notifier; @@ -42,13 +41,11 @@ import frc.robot.Constants.FieldConstants; import frc.robot.Constants.SwerveConstants; import frc.robot.Constants.VisionConstants; -import frc.robot.generated.TunerConstants; import frc.robot.Robot; import frc.robot.utils.SysId; import frc.robot.utils.VisionPoseEstimator; import frc.robot.utils.VisionPoseEstimator.VisionPoseEstimate; import java.util.ArrayList; -import java.util.Arrays; import java.util.HashSet; import java.util.List; import java.util.Set; @@ -420,16 +417,6 @@ public ChassisSpeeds getChassisSpeeds() { return getState().Speeds; } - /* Wrapper for getting swerve module positions in radians. */ - public double[] getWheelPositionsRadians(){ - double[] distances = new double[getModules().length]; - for (int i = 0; i < getModules().length; i++){ - distances[i] = getModule(i).getPosition(true).distanceMeters / TunerConstants.kWheelRadius.in(Meters); - } - - return distances; - } - // updates pose estimator with vision private void updateVisionPoseEstimates() { _acceptedEstimates.clear(); From 5526bdd2e93753ed1ce4c0364e58f6502b85bd0b Mon Sep 17 00:00:00 2001 From: Peter Gutkovich <60079012+PGgit08@users.noreply.github.com> Date: Fri, 27 Dec 2024 22:06:09 -0500 Subject: [PATCH 3/6] Update README.md --- README.md | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 05bfa30..f9e3d2f 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. \ No newline at end of file From 9307a077385da8c8757f4d200d56fad2ece415fc Mon Sep 17 00:00:00 2001 From: PGgit08 <peter.mty8@gmail.com> Date: Fri, 27 Dec 2024 22:59:06 -0500 Subject: [PATCH 4/6] wheel radius characterization unit test --- src/main/java/frc/lib/UnitTestingUtil.java | 1 + .../commands/WheelRadiusCharacterization.java | 2 +- .../java/frc/robot/subsystems/Swerve.java | 2 + .../frc/robot/utils/VisionPoseEstimator.java | 2 +- src/test/java/frc/robot/RobotTest.java | 3 ++ .../WheelRadiusCharacterizationTest.java | 46 +++++++++++++++++++ 6 files changed, 54 insertions(+), 2 deletions(-) create mode 100644 src/test/java/frc/robot/WheelRadiusCharacterizationTest.java 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/commands/WheelRadiusCharacterization.java b/src/main/java/frc/robot/commands/WheelRadiusCharacterization.java index 063da5f..44957df 100644 --- a/src/main/java/frc/robot/commands/WheelRadiusCharacterization.java +++ b/src/main/java/frc/robot/commands/WheelRadiusCharacterization.java @@ -68,7 +68,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - _swerve.drive(0, 0, -1); // TODO: Make this tunable + _swerve.drive(0, 0, -1); // TODO: make this tunable // 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] 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..17723bc --- /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 tunable 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 charactierzation 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); + } +} From f6d475b39f67e040051085cdb90efaad54798ad7 Mon Sep 17 00:00:00 2001 From: PGgit08 <peter.mty8@gmail.com> Date: Fri, 27 Dec 2024 23:02:07 -0500 Subject: [PATCH 5/6] fixed readme spotless thing --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index f9e3d2f..4751ada 100644 --- a/README.md +++ b/README.md @@ -25,4 +25,4 @@ A base project for future robots that has CTRE generated swerve drive code and P # Calibration Process - 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. -- If the calibration was good, load the generated JSON into photon vision. \ No newline at end of file +- If the calibration was good, load the generated JSON into photon vision. From 2ded3ac530487beb6cb670e68cd068306c44a0ff Mon Sep 17 00:00:00 2001 From: PGgit08 <peter.mty8@gmail.com> Date: Sat, 28 Dec 2024 11:58:14 -0500 Subject: [PATCH 6/6] typo fixes --- .../java/frc/robot/commands/WheelRadiusCharacterization.java | 2 +- src/test/java/frc/robot/WheelRadiusCharacterizationTest.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/WheelRadiusCharacterization.java b/src/main/java/frc/robot/commands/WheelRadiusCharacterization.java index 44957df..9ccd609 100644 --- a/src/main/java/frc/robot/commands/WheelRadiusCharacterization.java +++ b/src/main/java/frc/robot/commands/WheelRadiusCharacterization.java @@ -68,7 +68,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - _swerve.drive(0, 0, -1); // TODO: make this tunable + _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] diff --git a/src/test/java/frc/robot/WheelRadiusCharacterizationTest.java b/src/test/java/frc/robot/WheelRadiusCharacterizationTest.java index 17723bc..424ae3e 100644 --- a/src/test/java/frc/robot/WheelRadiusCharacterizationTest.java +++ b/src/test/java/frc/robot/WheelRadiusCharacterizationTest.java @@ -13,7 +13,7 @@ import org.junit.jupiter.api.Test; public class WheelRadiusCharacterizationTest { - // chassis omega during characterization (TODO: this would be tunable later) + // chassis omega during characterization (TODO: this would be tuneable later) private final double velOmega = 1; private Swerve _swerve; @@ -34,7 +34,7 @@ public void close() throws Exception { @Test public void wheelRadiusCharacterization() { - // run enough ticks to complete 4pi radians at the given charactierzation omega + // 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?)