diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 0adda27d..2116fb54 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -39,6 +39,7 @@ import frc.robot.util.ScoringMode; import frc.robot.util.ScoringType; import frc.robot.util.SoloScoringMode; +import frc.robot.util.WheelRadiusCharacterization; import java.util.function.BooleanSupplier; public class Controls { @@ -1112,13 +1113,18 @@ private void configureSoloControllerBindings() { : Commands.none()) .withName("Manual Coral Intake")); // Ground Intake Hold out + // soloController + // .povUp() + // .whileTrue( + // s.groundArm + // .moveToPosition(GroundArm.GROUND_POSITION) + // .andThen(Commands.idle()) + // .withName("ground up position")); soloController .povUp() - .whileTrue( - s.groundArm - .moveToPosition(GroundArm.GROUND_POSITION) - .andThen(Commands.idle()) - .withName("ground up position")); + .and(soloController.start()) + .onTrue( + WheelRadiusCharacterization.wheelRadiusCharacterizationCommand(s.drivebaseSubsystem)); // Arm manual soloController .rightStick() diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 794d2e2f..c2525856 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -26,6 +26,7 @@ import frc.robot.util.BuildInfo; import frc.robot.util.MatchTab; import frc.robot.util.RobotType; +import frc.robot.util.WheelRadiusCharacterization; public class Robot extends TimedRobot { /** Singleton Stuff */ @@ -144,12 +145,18 @@ public void disabledExit() { @Override public void autonomousInit() { Shuffleboard.startRecording(); - if (SubsystemConstants.DRIVEBASE_ENABLED && AutoLogic.getSelectedAuto() != null) { + if (SubsystemConstants.DRIVEBASE_ENABLED + && AutoLogic.getSelectedAuto() != null + && !AutoLogic.RUN_MEASUREMENT_AUTO) { AutoLogic.getSelectedAuto().schedule(); } if (subsystems.climbPivotSubsystem != null) { subsystems.climbPivotSubsystem.moveCompleteFalse(); } + if (SubsystemConstants.DRIVEBASE_ENABLED && AutoLogic.RUN_MEASUREMENT_AUTO) { + WheelRadiusCharacterization.wheelRadiusCharacterizationCommand(subsystems.drivebaseSubsystem) + .schedule(); + } } @Override diff --git a/src/main/java/frc/robot/generated/CompTunerConstants.java b/src/main/java/frc/robot/generated/CompTunerConstants.java index a9394ce9..8521405c 100644 --- a/src/main/java/frc/robot/generated/CompTunerConstants.java +++ b/src/main/java/frc/robot/generated/CompTunerConstants.java @@ -117,7 +117,7 @@ public class CompTunerConstants { private static final double kDriveGearRatio = 5.357142857142857; private static final double kSteerGearRatio = 18.75; - private static final Distance kWheelRadius = Inches.of(2 * 21 / (22.1 * 0.97)); + public static final Distance kWheelRadius = Inches.of(2 * 21 / (22.1 * 0.97)); private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; diff --git a/src/main/java/frc/robot/subsystems/auto/AutoLogic.java b/src/main/java/frc/robot/subsystems/auto/AutoLogic.java index f65e48a9..a3141710 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoLogic.java @@ -35,6 +35,7 @@ public class AutoLogic { public static Robot r = Robot.getInstance(); public static final Subsystems s = r.subsystems; public static final Controls controls = r.controls; + public static final boolean RUN_MEASUREMENT_AUTO = false; public static enum StartPosition { FAR_LEFT_CAGE( diff --git a/src/main/java/frc/robot/subsystems/drivebase/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/drivebase/CommandSwerveDrivetrain.java index d4427c06..3ae1ff9c 100644 --- a/src/main/java/frc/robot/subsystems/drivebase/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivebase/CommandSwerveDrivetrain.java @@ -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 < values.length; i++) { + values[i] = getState().ModulePositions[i].distanceMeters / wheelCircumference; + } + return values; + } } diff --git a/src/main/java/frc/robot/util/WheelRadiusCharacterization.java b/src/main/java/frc/robot/util/WheelRadiusCharacterization.java new file mode 100644 index 00000000..7e657d5f --- /dev/null +++ b/src/main/java/frc/robot/util/WheelRadiusCharacterization.java @@ -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))); + private static final NumberFormat FORMATTER = new DecimalFormat("#0.000"); + private static final int NUM_MODULES = 4; + + // 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); + 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 < NUM_MODULES; i++) { + wheelDelta += + Math.abs(positions[i] - state.positions[i]) / (double) NUM_MODULES; + } + 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[NUM_MODULES]; + Rotation2d lastAngle = Rotation2d.kZero; + double gyroDelta = 0.0; + } +}