Wheel radius characterization #196
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -20,6 +20,7 @@ | |
| import edu.wpi.first.wpilibj2.command.Command; | ||
| import edu.wpi.first.wpilibj2.command.Subsystem; | ||
| import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; | ||
| import frc.robot.generated.CompTunerConstants; | ||
| import java.util.function.Supplier; | ||
|
|
||
| /** | ||
|
|
@@ -261,4 +262,13 @@ public Command coastMotors() { | |
| public void brakeMotors() { | ||
| configNeutralMode(NeutralModeValue.Brake); | ||
| } | ||
|
|
||
| public double[] getWheelRotations() { | ||
| double wheelCircumference = 2 * Math.PI * CompTunerConstants.kWheelRadius.abs(Meter); | ||
| double[] values = new double[4]; | ||
| for (int i = 0; i < 4; i++) { | ||
iamawesomecat marked this conversation as resolved.
Show resolved
Hide resolved
|
||
| values[i] = getState().ModulePositions[i].distanceMeters / wheelCircumference; | ||
| } | ||
| return values; | ||
| } | ||
| } | ||
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,116 @@ | ||
| package frc.robot.util; | ||
|
|
||
| import com.ctre.phoenix6.swerve.SwerveRequest; | ||
| import edu.wpi.first.math.filter.SlewRateLimiter; | ||
| import edu.wpi.first.math.geometry.Rotation2d; | ||
| import edu.wpi.first.math.kinematics.ChassisSpeeds; | ||
| import edu.wpi.first.math.util.Units; | ||
| import edu.wpi.first.wpilibj2.command.Command; | ||
| import edu.wpi.first.wpilibj2.command.Commands; | ||
| import frc.robot.generated.CompTunerConstants; | ||
| import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain; | ||
| import java.text.DecimalFormat; | ||
| import java.text.NumberFormat; | ||
|
|
||
| public class WheelRadiusCharacterization { | ||
| private static final double WHEEL_RADIUS_MAX_VELOCITY = 4; // Rad/Sec | ||
| private static final double WHEEL_RADIUS_RAMP_RATE = 0.5; // Rad/Sec^2 | ||
| public static final double DRIVE_BASE_RADIUS = | ||
| Math.max( | ||
| Math.max( | ||
| Math.hypot( | ||
| CompTunerConstants.FrontLeft.LocationX, CompTunerConstants.FrontLeft.LocationY), | ||
| Math.hypot( | ||
| CompTunerConstants.FrontRight.LocationX, | ||
| CompTunerConstants.FrontRight.LocationY)), | ||
| Math.max( | ||
| Math.hypot( | ||
| CompTunerConstants.BackLeft.LocationX, CompTunerConstants.BackLeft.LocationY), | ||
| Math.hypot( | ||
| CompTunerConstants.BackRight.LocationX, CompTunerConstants.BackRight.LocationY))); | ||
|
Comment on lines
+18
to
+30
There was a problem hiding this comment. The calculation for public static final double DRIVE_BASE_RADIUS =
java.util.stream.Stream.of(
CompTunerConstants.FrontLeft,
CompTunerConstants.FrontRight,
CompTunerConstants.BackLeft,
CompTunerConstants.BackRight)
.mapToDouble(c -> Math.hypot(c.LocationX, c.LocationY))
.max()
.orElse(0.0);
Contributor
Author
There was a problem hiding this comment. What is a java Stream? and what does .max() do? |
||
| private static final NumberFormat FORMATTER = new DecimalFormat("#0.000"); | ||
|
|
||
| // get the wheel rotation positions of the swerve modules | ||
| public static double[] getWheelRadiusCharacterizationPositions(CommandSwerveDrivetrain drive) { | ||
| return drive.getWheelRotations(); | ||
| } | ||
|
|
||
| public static Command wheelRadiusCharacterizationCommand(CommandSwerveDrivetrain drive) { | ||
| SlewRateLimiter limiter = new SlewRateLimiter(WHEEL_RADIUS_RAMP_RATE); | ||
| WheelRadiusCharacterizationState state = new WheelRadiusCharacterizationState(); | ||
| return Commands.parallel( | ||
| // Drive control sequence | ||
| Commands.sequence( | ||
| // Reset acceleration limiter | ||
| Commands.runOnce( | ||
| () -> { | ||
| limiter.reset(0.0); | ||
| }), | ||
|
|
||
| // Turn in place, accelerating up to full speed | ||
| Commands.run( | ||
| () -> { | ||
| double speed = limiter.calculate(WHEEL_RADIUS_MAX_VELOCITY); | ||
| System.out.println(speed); | ||
| // drive.runVelocity(new ChassisSpeeds(0.0, 0.0, speed)); | ||
| SwerveRequest driveRequest = | ||
| new SwerveRequest.ApplyRobotSpeeds() | ||
| .withSpeeds(new ChassisSpeeds(0.0, 0.0, speed)); | ||
| drive.setControl(driveRequest); | ||
| }, | ||
| drive)), | ||
|
|
||
| // Measurement sequence | ||
| Commands.sequence( | ||
| // Wait for modules to fully orient before starting measurement | ||
| Commands.waitSeconds(1.0), | ||
|
|
||
| // Record starting measurement | ||
| Commands.runOnce( | ||
| () -> { | ||
| state.positions = getWheelRadiusCharacterizationPositions(drive); | ||
| state.lastAngle = drive.getState().Pose.getRotation(); | ||
| state.gyroDelta = 0.0; | ||
| }), | ||
|
|
||
| // Update gyro delta | ||
| Commands.run( | ||
| () -> { | ||
| var rotation = drive.getState().Pose.getRotation(); | ||
| state.gyroDelta += Math.abs(rotation.minus(state.lastAngle).getRadians()); | ||
| state.lastAngle = rotation; | ||
| }) | ||
|
|
||
| // When cancelled, calculate and print results | ||
| .finallyDo( | ||
| () -> { | ||
| double[] positions = getWheelRadiusCharacterizationPositions(drive); | ||
| double wheelDelta = 0.0; | ||
| for (int i = 0; i < 4; i++) { | ||
iamawesomecat marked this conversation as resolved.
Show resolved
Hide resolved
|
||
| wheelDelta += Math.abs(positions[i] - state.positions[i]) / 4.0; | ||
| } | ||
| wheelDelta *= 2 * Math.PI; | ||
| double wheelRadius = (state.gyroDelta * DRIVE_BASE_RADIUS) / wheelDelta; | ||
|
|
||
| System.out.println( | ||
| "********** Wheel Radius Characterization Results **********"); | ||
| System.out.println( | ||
| "\tWheel Delta: " + FORMATTER.format(wheelDelta) + " radians"); | ||
| System.out.println( | ||
| "\tGyro Delta: " + FORMATTER.format(state.gyroDelta) + " radians"); | ||
| System.out.println( | ||
| "\tWheel Radius: " | ||
| + FORMATTER.format(wheelRadius) | ||
| + " meters, " | ||
| + FORMATTER.format(Units.metersToInches(wheelRadius)) | ||
| + " inches"); | ||
| }))) | ||
| .withName("wheel radius characterization"); | ||
| } | ||
|
|
||
| private static class WheelRadiusCharacterizationState { | ||
| double[] positions = new double[4]; | ||
| Rotation2d lastAngle = Rotation2d.kZero; | ||
| double gyroDelta = 0.0; | ||
| } | ||
| } | ||
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This block of code is commented out. If it's no longer needed, it should be removed to improve code clarity. If it's being temporarily replaced for testing, it's better to use version control to switch between versions rather than leaving commented-out code.