Implemented wheel characterization

Robocubs · Apr 11, 2024 · 9b11ef8 · 9b11ef8
1 parent 048ef6b
commit 9b11ef8
Show file tree

Hide file tree

Showing 4 changed files with 81 additions and 1 deletion.
diff --git a/src/main/java/com/team1701/robot/Configuration.java b/src/main/java/com/team1701/robot/Configuration.java
@@ -5,7 +5,7 @@
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 
 public final class Configuration {
-    private static final RobotType kRobot = RobotType.SIMULATION_BOT;
+    private static final RobotType kRobot = RobotType.SIMULATION_VISION;
 
     private static boolean mInvalidRobotAlerted = false;
 

diff --git a/src/main/java/com/team1701/robot/RobotContainer.java b/src/main/java/com/team1701/robot/RobotContainer.java
@@ -30,6 +30,7 @@
 import com.team1701.lib.util.GeometryUtil;
 import com.team1701.robot.Configuration.Mode;
 import com.team1701.robot.commands.AutonomousCommands;
+import com.team1701.robot.commands.CharacterizationCommands;
 import com.team1701.robot.commands.DriveCommands;
 import com.team1701.robot.commands.IntakeCommands;
 import com.team1701.robot.commands.ShootCommands;
@@ -636,6 +637,11 @@ private void setupAutonomous() {
         //         CharacterizationCommands.runShooterCharacterization(mShooter)
         //                 .deadlineWith(Commands.idle(mDrive, mIndexer, mIntake))
         //                 .withName("ShooterCharacterization"));
+        autonomousModeChooser.addOption(
+                "Drive Wheel Characterization",
+                CharacterizationCommands.runWheelRadiusCharacterization(mDrive)
+                        .deadlineWith(Commands.idle(mShooter, mIndexer, mIntake))
+                        .withName("DriveWheelCharacterization"));
 
         if (Configuration.getMode() == Mode.SIMULATION) {
             mAutonomousPaths.put("Demo", demoCommand.path());

diff --git a/src/main/java/com/team1701/robot/commands/CharacterizationCommands.java b/src/main/java/com/team1701/robot/commands/CharacterizationCommands.java
@@ -5,18 +5,25 @@
 import java.util.function.DoubleSupplier;
 
 import com.team1701.lib.util.PolynomialRegression;
+import com.team1701.robot.Constants;
 import com.team1701.robot.subsystems.drive.Drive;
 import com.team1701.robot.subsystems.shooter.Shooter;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.Subsystem;
+import org.littletonrobotics.junction.Logger;
 
 public class CharacterizationCommands {
     private static final double kStartDelaySeconds = 2.0;
     private static final double kDriveRampAmpsPerSecond = 0.2;
     private static final double kShooterRampVoltsPerSecond = 0.1;
+    private static final double kWheelCharacterizationVelocity = 1.0;
 
     public static Command runDriveCharacterization(Drive drive) {
         return runFeedforwardCharacterization(
@@ -97,4 +104,62 @@ private static Command runFeedforwardCharacterization(
                         subsystems)
                 .withName("FeedforwardCharacterization");
     }
+
+    private static class WheelRadiusCharacterizationData {
+        private double lastGyroYawRadians = 0.0;
+        private double cumulativeGyroYawRadians = 0.0;
+        private SwerveModulePosition[] startSwerveModulePosition;
+        private double currentEffectiveWheelRadius = 0.0;
+    }
+
+    public static Command runWheelRadiusCharacterization(Drive drive) {
+        var rotationLimiter = new SlewRateLimiter(1.0);
+        var data = new WheelRadiusCharacterizationData();
+        return new FunctionalCommand(
+                        () -> {
+                            data.lastGyroYawRadians =
+                                    drive.getFieldRelativeHeading().getRadians();
+                            data.cumulativeGyroYawRadians = 0.0;
+                            data.startSwerveModulePosition = drive.getMeasuredModulePositions();
+                            rotationLimiter.reset(0);
+                        },
+                        () -> {
+                            drive.runWheelCharacterization(rotationLimiter.calculate(kWheelCharacterizationVelocity));
+
+                            var gyroYawRadians = drive.getFieldRelativeHeading().getRadians();
+                            data.cumulativeGyroYawRadians +=
+                                    MathUtil.angleModulus(gyroYawRadians - data.lastGyroYawRadians);
+                            data.lastGyroYawRadians = gyroYawRadians;
+                            var averageWheelPosition = 0.0;
+                            var modulePositions = drive.getMeasuredModulePositions();
+                            for (int i = 0; i < 4; i++) {
+                                averageWheelPosition += Math.abs(modulePositions[i].distanceMeters
+                                                - data.startSwerveModulePosition[i].distanceMeters)
+                                        / Constants.Drive.kWheelRadiusMeters;
+                            }
+                            averageWheelPosition /= 4.0;
+
+                            data.currentEffectiveWheelRadius =
+                                    (data.cumulativeGyroYawRadians * Constants.Drive.kModuleRadius)
+                                            / averageWheelPosition;
+                            Logger.recordOutput("Drive/RadiusCharacterization/DrivePosition", averageWheelPosition);
+                            Logger.recordOutput(
+                                    "Drive/RadiusCharacterization/AccumGyroYawRads", data.cumulativeGyroYawRadians);
+                            Logger.recordOutput(
+                                    "Drive/RadiusCharacterization/CurrentWheelRadiusInches",
+                                    Units.metersToInches(data.currentEffectiveWheelRadius));
+                        },
+                        (interrupted) -> {
+                            drive.stop();
+                            if (data.cumulativeGyroYawRadians <= Math.PI * 2.0) {
+                                System.out.println("Not enough data for characterization");
+                            } else {
+                                System.out.println("Effective Wheel Radius: "
+                                        + Units.metersToInches(data.currentEffectiveWheelRadius) + " inches");
+                            }
+                        },
+                        () -> false,
+                        drive)
+                .withName("DriveWheelRadiusCharacterization");
+    }
 }
diff --git a/src/main/java/com/team1701/robot/subsystems/drive/Drive.java b/src/main/java/com/team1701/robot/subsystems/drive/Drive.java
@@ -271,6 +271,10 @@ public double getAverageModuleSpeedRadiansPerSecond() {
                 .getAsDouble();
     }
 
+    public SwerveModulePosition[] getMeasuredModulePositions() {
+        return mMeasuredModulePositions;
+    }
+
     public ChassisSpeeds getVelocitySetpoint() {
         return Constants.Drive.kKinematics.toChassisSpeeds(mPreviousSetpoint.moduleStates);
     }
@@ -311,6 +315,11 @@ public void runCharacterization(double volts) {
         mCharacterizationInput = volts;
     }
 
+    public void runWheelCharacterization(double velocityRadiansPerSecond) {
+        mDriveControlState = DriveControlState.VELOCITY_CONTROL;
+        mDesiredChassisSpeeds = new ChassisSpeeds(0, 0, velocityRadiansPerSecond);
+    }
+
     public void zeroGyroscope() {
         zeroGyroscope(GeometryUtil.kRotationIdentity);
     }