diff --git a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java index 5299898b..3222ed96 100644 --- a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java +++ b/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); } -} +} \ No newline at end of file diff --git a/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java b/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java index 481a7d32..07a7cc52 100644 --- a/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java +++ b/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java @@ -12,7 +12,6 @@ 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; @@ -20,12 +19,16 @@ 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 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 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 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 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 wheelPositionsRadiansSupplier, + DoubleSupplier gyroYawRadiansSupplier, + DoubleConsumer runWheelRadiusCharacterization, + SubsystemBase requirement) { + this( + new double[]{wheelDistanceFromCenterMeters, wheelDistanceFromCenterMeters, wheelDistanceFromCenterMeters, wheelDistanceFromCenterMeters}, + wheelPositionsRadiansSupplier, + gyroYawRadiansSupplier, + runWheelRadiusCharacterization, + requirement + ); } - public WheelRadiusCharacterizationCommand(double[] wheelDistancesFromCenterMeters, Supplier 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 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()); } } \ No newline at end of file