Command Changes

src/main/java/org/trigon/commands/GearRatioCalculationCommand.java

@@ -1,64 +1,137 @@
 package org.trigon.commands;
 
+import com.ctre.phoenix6.controls.VoltageOut;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import org.littletonrobotics.junction.Logger;
-import org.littletonrobotics.junction.networktables.LoggedDashboardBoolean;
 import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
+import org.trigon.hardware.phoenix6.cancoder.CANcoderEncoder;
+import org.trigon.hardware.phoenix6.cancoder.CANcoderSignal;
+import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor;
+import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal;
 
 import java.util.function.DoubleConsumer;
 import java.util.function.DoubleSupplier;
 
+/**
+ * A command that calculates and logs the gear ratio of a mechanism by comparing the distance traveled by a rotor and an encoder.
+ */
 public class GearRatioCalculationCommand extends Command {
     private final DoubleSupplier rotorPositionSupplier;
     private final DoubleSupplier encoderPositionSupplier;
     private final DoubleConsumer runGearRatioCalculation;
     private final String subsystemName;
-
+    private final double backlashAccountabilityTimeSeconds;
     private final LoggedDashboardNumber movementVoltage;
-    private final LoggedDashboardBoolean shouldMoveClockwise;
 
     private double startingRotorPosition;
     private double startingEncoderPosition;
     private double gearRatio;
+    private double startTime;
+    private boolean hasSetStartingPositions = false;
+
+    /**
+     * Creates a new GearRatioCalculationCommand.
+     * This constructor takes a motor to run the gear ratio calculation on, and to measure the distance the rotor travels.
+     * It also takes an encoder to measure the distance traveled.
+     *
+     * @param motor                             the motor that drives the rotor
+     * @param encoder                           the encoder that measures the distance traveled
+     * @param backlashAccountabilityTimeSeconds the time to wait before setting the starting positions in order to account for backlash
+     * @param requirement                       the subsystem that this command requires
+     */
+    public GearRatioCalculationCommand(TalonFXMotor motor, CANcoderEncoder encoder, double backlashAccountabilityTimeSeconds, SubsystemBase requirement) {
+        this(
+                () -> motor.getSignal(TalonFXSignal.ROTOR_POSITION),
+                () -> encoder.getSignal(CANcoderSignal.POSITION),
+                (voltage) -> motor.setControl(new VoltageOut(voltage)),
+                backlashAccountabilityTimeSeconds,
+                requirement
+        );
+    }
 
-    public GearRatioCalculationCommand(DoubleSupplier rotorPositionSupplier, DoubleSupplier encoderPositionSupplier, DoubleConsumer runGearRatioCalculation, SubsystemBase requirement) {
+    /**
+     * Creates a new GearRatioCalculationCommand.
+     *
+     * @param rotorPositionSupplier             a supplier that returns the current position of the rotor in degrees
+     * @param encoderPositionSupplier           a supplier that returns the current position of the encoder in degrees
+     * @param runGearRatioCalculation           a consumer that drives the motor with a given voltage
+     * @param backlashAccountabilityTimeSeconds the time to wait before setting the starting positions in order to account for backlash
+     * @param requirement                       the subsystem that this command requires
+     */
+    public GearRatioCalculationCommand(
+            DoubleSupplier rotorPositionSupplier,
+            DoubleSupplier encoderPositionSupplier,
+            DoubleConsumer runGearRatioCalculation,
+            double backlashAccountabilityTimeSeconds,
+            SubsystemBase requirement) {
         this.rotorPositionSupplier = rotorPositionSupplier;
         this.encoderPositionSupplier = encoderPositionSupplier;
         this.runGearRatioCalculation = runGearRatioCalculation;
         this.subsystemName = requirement.getName();
+        this.backlashAccountabilityTimeSeconds = backlashAccountabilityTimeSeconds;
 
         this.movementVoltage = new LoggedDashboardNumber("GearRatioCalculation/" + this.subsystemName + "/Voltage", 1);
-        this.shouldMoveClockwise = new LoggedDashboardBoolean("GearRatioCalculation/" + this.subsystemName + "/ShouldMoveClockwise", false);
 
         addRequirements(requirement);
     }
 
     @Override
     public void initialize() {
-        startingRotorPosition = rotorPositionSupplier.getAsDouble();
-        startingEncoderPosition = encoderPositionSupplier.getAsDouble();
+        startTime = Timer.getFPGATimestamp();
+        gearRatio = 0;
+        resetLogs();
     }
 
     @Override
     public void execute() {
-        runGearRatioCalculation.accept(movementVoltage.get() * getRotationDirection());
+        runGearRatioCalculation();
+
+        if (Timer.getFPGATimestamp() - startTime > backlashAccountabilityTimeSeconds && !hasSetStartingPositions)
+            setStartingPositions();
+
+        if (hasSetStartingPositions) {
+            gearRatio = calculateGearRatio();
+            logGearRatioAndDistance();
+        }
+    }
+
+    @Override
+    public void end(boolean interrupted) {
         gearRatio = calculateGearRatio();
+        logGearRatioAndDistance();
+        printResult();
+    }
 
+    private void setStartingPositions() {
+        startingRotorPosition = rotorPositionSupplier.getAsDouble();
+        startingEncoderPosition = encoderPositionSupplier.getAsDouble();
+        hasSetStartingPositions = true;
+    }
+
+    private void runGearRatioCalculation() {
+        runGearRatioCalculation.accept(movementVoltage.get());
+    }
+
+    private void logGearRatioAndDistance() {
         Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/RotorDistance", getRotorDistance());
         Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/EncoderDistance", getEncoderDistance());
         Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/GearRatio", gearRatio);
     }
 
-    @Override
-    public void end(boolean interrupted) {
-        printResult();
+    private void resetLogs() {
+        Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/RotorDistance", 0);
+        Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/EncoderDistance", 0);
+        Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/GearRatio", 0);
     }
 
     private double calculateGearRatio() {
-        final double currentRotorPosition = getRotorDistance();
-        final double currentEncoderPosition = getEncoderDistance();
-        return currentRotorPosition / currentEncoderPosition;
+        final double currentRotorDistance = getRotorDistance();
+        final double currentEncoderDistance = getEncoderDistance();
+        if (currentEncoderDistance == 0)
+            return 0;
+        return currentRotorDistance / currentEncoderDistance;
     }
 
     private double getRotorDistance() {
@@ -69,11 +142,7 @@ private double getEncoderDistance() {
         return startingEncoderPosition - encoderPositionSupplier.getAsDouble();
     }
 
-    private int getRotationDirection() {
-        return shouldMoveClockwise.get() ? -1 : 1;
-    }
-
     private void printResult() {
         System.out.println(subsystemName + " Gear Ratio: " + gearRatio);
     }
-}
+}

src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java

@@ -12,20 +12,23 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import org.littletonrobotics.junction.Logger;
-import org.littletonrobotics.junction.networktables.LoggedDashboardBoolean;
 import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
 
 import java.util.Arrays;
 import java.util.function.DoubleConsumer;
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
 
+/**
+ * A command for characterizing the wheel radius of each module of a swerve drive system by calculating the radius of each drive wheel based on rotational data and robot yaw during operation.
+ */
 public class WheelRadiusCharacterizationCommand extends Command {
     private static final LoggedDashboardNumber CHARACTERIZATION_SPEED = new LoggedDashboardNumber("WheelRadiusCharacterization/SpeedRadiansPerSecond", 1);
     private static final LoggedDashboardNumber ROTATION_RATE_LIMIT = new LoggedDashboardNumber("WheelRadiusCharacterization/RotationRateLimit", 1);
-    private static final LoggedDashboardBoolean SHOULD_MOVE_CLOCKWISE = new LoggedDashboardBoolean("WheelRadiusCharacterization/ShouldMoveClockwise", false);
 
-    private final double[] wheelDistancesFromCenterMeters;
+    private final double[]
+            wheelDistancesFromCenterMeters,
+            driveWheelRadii;
     private final Supplier<double[]> wheelPositionsRadiansSupplier;
     private final DoubleSupplier gyroYawRadiansSupplier;
     private final DoubleConsumer runWheelRadiusCharacterization;
@@ -34,25 +37,97 @@ public class WheelRadiusCharacterizationCommand extends Command {
     private double startingGyroYawRadians;
     private double accumulatedGyroYawRadians;
     private double[] startingWheelPositions;
-    private double driveWheelsRadius;
 
-    public WheelRadiusCharacterizationCommand(Translation2d[] wheelDistancesFromCenterMeters, Supplier<double[]> wheelPositionsRadiansSupplier,
-                                              DoubleSupplier gyroYawRadiansSupplier, DoubleConsumer runWheelRadiusCharacterization, SubsystemBase requirement
-    ) {
-        this.wheelDistancesFromCenterMeters = Arrays.stream(wheelDistancesFromCenterMeters).mapToDouble(Translation2d::getNorm).toArray();
-        this.wheelPositionsRadiansSupplier = wheelPositionsRadiansSupplier;
-        this.gyroYawRadiansSupplier = gyroYawRadiansSupplier;
-        this.runWheelRadiusCharacterization = runWheelRadiusCharacterization;
-        addRequirements(requirement);
+    /**
+     * Creates a new WheelRadiusCharacterizationCommand.
+     *
+     * @param wheelDistanceFromCenterMeters  an array of the distances of each wheel from the center of the robot in meters
+     * @param wheelPositionsRadiansSupplier  a supplier that returns the current position of each wheel in radians
+     * @param gyroYawRadiansSupplier         a supplier that returns the current yaw of the robot in radians
+     * @param runWheelRadiusCharacterization a consumer that runs the swerve drive with a given speed
+     * @param requirement                    the subsystem that this command requires
+     */
+    public WheelRadiusCharacterizationCommand(Translation2d wheelDistanceFromCenterMeters,
+                                              Supplier<double[]> wheelPositionsRadiansSupplier,
+                                              DoubleSupplier gyroYawRadiansSupplier,
+                                              DoubleConsumer runWheelRadiusCharacterization,
+                                              SubsystemBase requirement) {
+        this(
+                new Translation2d[]{wheelDistanceFromCenterMeters, wheelDistanceFromCenterMeters, wheelDistanceFromCenterMeters, wheelDistanceFromCenterMeters},
+                wheelPositionsRadiansSupplier,
+                gyroYawRadiansSupplier,
+                runWheelRadiusCharacterization,
+                requirement
+        );
+    }
+
+    /**
+     * Creates a new WheelRadiusCharacterizationCommand.
+     *
+     * @param wheelDistancesFromCenterMeters an array of the distances of each wheel from the center of the robot in meters
+     * @param wheelPositionsRadiansSupplier  a supplier that returns the current position of each wheel in radians
+     * @param gyroYawRadiansSupplier         a supplier that returns the current yaw of the robot in radians
+     * @param runWheelRadiusCharacterization a consumer that runs the swerve drive with a given speed
+     * @param requirement                    the subsystem that this command requires
+     */
+    public WheelRadiusCharacterizationCommand(
+            Translation2d[] wheelDistancesFromCenterMeters,
+            Supplier<double[]> wheelPositionsRadiansSupplier,
+            DoubleSupplier gyroYawRadiansSupplier,
+            DoubleConsumer runWheelRadiusCharacterization,
+            SubsystemBase requirement) {
+        this(
+                Arrays.stream(wheelDistancesFromCenterMeters).mapToDouble(Translation2d::getNorm).toArray(),
+                wheelPositionsRadiansSupplier,
+                gyroYawRadiansSupplier,
+                runWheelRadiusCharacterization,
+                requirement
+        );
+    }
+
+    /**
+     * Creates a new WheelRadiusCharacterizationCommand.
+     *
+     * @param wheelDistanceFromCenterMeters  the distance of the wheels from the center of the robot in meters
+     * @param wheelPositionsRadiansSupplier  a supplier that returns the current position of each wheel in radians
+     * @param gyroYawRadiansSupplier         a supplier that returns the current yaw of the robot in radians
+     * @param runWheelRadiusCharacterization a consumer that runs the swerve drive with a given speed
+     * @param requirement                    the subsystem that this command requires
+     */
+    public WheelRadiusCharacterizationCommand(
+            double wheelDistanceFromCenterMeters,
+            Supplier<double[]> wheelPositionsRadiansSupplier,
+            DoubleSupplier gyroYawRadiansSupplier,
+            DoubleConsumer runWheelRadiusCharacterization,
+            SubsystemBase requirement) {
+        this(
+                new double[]{wheelDistanceFromCenterMeters, wheelDistanceFromCenterMeters, wheelDistanceFromCenterMeters, wheelDistanceFromCenterMeters},
+                wheelPositionsRadiansSupplier,
+                gyroYawRadiansSupplier,
+                runWheelRadiusCharacterization,
+                requirement
+        );
     }
 
-    public WheelRadiusCharacterizationCommand(double[] wheelDistancesFromCenterMeters, Supplier<double[]> wheelPositionsRadiansSupplier,
-                                              DoubleSupplier gyroYawRadiansSupplier, DoubleConsumer runWheelRadiusCharacterization, SubsystemBase requirement
-    ) {
+    /**
+     * Creates a new WheelRadiusCharacterizationCommand.
+     *
+     * @param wheelDistancesFromCenterMeters an array of the distances of each wheel from the center of the robot in meters
+     * @param wheelPositionsRadiansSupplier  a supplier that returns the current position of each wheel in radians
+     * @param gyroYawRadiansSupplier         a supplier that returns the current yaw of the robot in radians
+     * @param runWheelRadiusCharacterization a consumer that runs the swerve drive with a given speed
+     * @param requirement                    the subsystem that this command requires
+     */
+    public WheelRadiusCharacterizationCommand(double[] wheelDistancesFromCenterMeters,
+                                              Supplier<double[]> wheelPositionsRadiansSupplier,
+                                              DoubleSupplier gyroYawRadiansSupplier,
+                                              DoubleConsumer runWheelRadiusCharacterization,
+                                              SubsystemBase requirement) {
         this.wheelDistancesFromCenterMeters = wheelDistancesFromCenterMeters;
         this.wheelPositionsRadiansSupplier = wheelPositionsRadiansSupplier;
         this.gyroYawRadiansSupplier = gyroYawRadiansSupplier;
         this.runWheelRadiusCharacterization = runWheelRadiusCharacterization;
+        this.driveWheelRadii = new double[wheelDistancesFromCenterMeters.length];
         addRequirements(requirement);
     }
 
@@ -66,10 +141,10 @@ public void execute() {
         driveMotors();
 
         accumulatedGyroYawRadians = getAccumulatedGyroYaw();
-        driveWheelsRadius = calculateDriveWheelRadius();
+        calculateDriveWheelRadius();
 
         Logger.recordOutput("WheelRadiusCharacterization/AccumulatedGyroYawRadians", accumulatedGyroYawRadians);
-        Logger.recordOutput("RadiusCharacterization/DriveWheelRadius", driveWheelsRadius);
+        logWheelRadii();
     }
 
     @Override
@@ -87,34 +162,36 @@ private void configureStartingStats() {
     }
 
     private void driveMotors() {
-        runWheelRadiusCharacterization.accept(rotationSlewRateLimiter.calculate(getRotationDirection() * CHARACTERIZATION_SPEED.get()));
+        runWheelRadiusCharacterization.accept(rotationSlewRateLimiter.calculate(CHARACTERIZATION_SPEED.get()));
     }
 
     private double getAccumulatedGyroYaw() {
         return Math.abs(startingGyroYawRadians - gyroYawRadiansSupplier.getAsDouble());
     }
 
-    private double calculateDriveWheelRadius() {
-        driveWheelsRadius = 0;
+    private void calculateDriveWheelRadius() {
         final double[] wheelPositionsRadians = wheelPositionsRadiansSupplier.get();
 
         for (int i = 0; i < 4; i++) {
             final double accumulatedWheelRadians = Math.abs(wheelPositionsRadians[i] - startingWheelPositions[i]);
-            driveWheelsRadius += (accumulatedGyroYawRadians * wheelDistancesFromCenterMeters[i]) / accumulatedWheelRadians;
+            driveWheelRadii[i] = (accumulatedGyroYawRadians * wheelDistancesFromCenterMeters[i]) / accumulatedWheelRadians;
             Logger.recordOutput("RadiusCharacterization/AccumulatedWheelRadians" + i, accumulatedWheelRadians);
         }
-
-        return driveWheelsRadius /= 4;
     }
 
     private void printResults() {
-        if (accumulatedGyroYawRadians <= Math.PI * 2.0)
+        if (accumulatedGyroYawRadians <= Math.PI * 2.0) {
             System.out.println("Not enough data for characterization");
-        else
-            System.out.println("Drive Wheel Radius: " + driveWheelsRadius + " meters");
+            return;
+        }
+        for (int i = 0; i < driveWheelRadii.length; i++)
+            System.out.println("Drive Wheel Radius for Module " + i + ": " + driveWheelRadii[i] + " meters");
+        System.out.println("Average Drive Wheel Radius: " + Arrays.stream(driveWheelRadii).average().getAsDouble() + " meters");
     }
 
-    private int getRotationDirection() {
-        return SHOULD_MOVE_CLOCKWISE.get() ? -1 : 1;
+    private void logWheelRadii() {
+        for (int i = 0; i < driveWheelRadii.length; i++)
+            Logger.recordOutput("RadiusCharacterization/DriveWheelRadiusModule" + i, driveWheelRadii[i]);
+        Logger.recordOutput("RadiusCharacterization/AverageDriveWheelRadius", Arrays.stream(driveWheelRadii).average().getAsDouble());
     }
 }