From 345e01e0861ce41628a4139cb986e6c627883e45 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Thu, 14 Nov 2024 22:05:31 +0200 Subject: [PATCH 01/21] made WheelRadiusCharacterization calculate for each module separetly --- .../WheelRadiusCharacterizationCommand.java | 43 +++++++++++-------- 1 file changed, 26 insertions(+), 17 deletions(-) diff --git a/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java b/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java index 481a7d32..d6fb4552 100644 --- a/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java +++ b/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java @@ -25,7 +25,9 @@ public class WheelRadiusCharacterizationCommand extends Command { 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 +36,32 @@ 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 + 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; + this.driveWheelRadii = new double[wheelDistancesFromCenterMeters.length]; addRequirements(requirement); } - public WheelRadiusCharacterizationCommand(double[] wheelDistancesFromCenterMeters, Supplier wheelPositionsRadiansSupplier, - DoubleSupplier gyroYawRadiansSupplier, DoubleConsumer runWheelRadiusCharacterization, SubsystemBase requirement + 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 +75,11 @@ public void execute() { driveMotors(); accumulatedGyroYawRadians = getAccumulatedGyroYaw(); - driveWheelsRadius = calculateDriveWheelRadius(); + calculateDriveWheelRadius(); Logger.recordOutput("WheelRadiusCharacterization/AccumulatedGyroYawRadians", accumulatedGyroYawRadians); - Logger.recordOutput("RadiusCharacterization/DriveWheelRadius", driveWheelsRadius); + for (int i = 0; i < driveWheelRadii.length; i++) + Logger.recordOutput("RadiusCharacterization/DriveWheelRadiusModule" + i, driveWheelRadii[i]); } @Override @@ -94,24 +104,23 @@ 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; - Logger.recordOutput("RadiusCharacterization/AccumulatedWheelRadians" + i, accumulatedWheelRadians); + driveWheelRadii[i] = (accumulatedGyroYawRadians * wheelDistancesFromCenterMeters[i]) / accumulatedWheelRadians; + Logger.recordOutput("RadiusCharacterization/AccumulatedWheelRadians" + i, driveWheelRadii[i]); } - - 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"); } private int getRotationDirection() { From 5c4a70fbc9bc7d9012d735ce4dca5f80e6aa4442 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Thu, 14 Nov 2024 23:06:20 +0200 Subject: [PATCH 02/21] made gear ratio command account for backlash --- .../commands/GearRatioCalculationCommand.java | 45 +++++++++++++------ 1 file changed, 31 insertions(+), 14 deletions(-) diff --git a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java index 5299898b..5747be3a 100644 --- a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java +++ b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java @@ -1,7 +1,6 @@ package org.trigon.commands; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.*; import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.LoggedDashboardBoolean; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; @@ -9,11 +8,12 @@ import java.util.function.DoubleConsumer; import java.util.function.DoubleSupplier; -public class GearRatioCalculationCommand extends Command { +public class GearRatioCalculationCommand extends SequentialCommandGroup { private final DoubleSupplier rotorPositionSupplier; private final DoubleSupplier encoderPositionSupplier; private final DoubleConsumer runGearRatioCalculation; private final String subsystemName; + private final double backlashAccountabilitySeconds; private final LoggedDashboardNumber movementVoltage; private final LoggedDashboardBoolean shouldMoveClockwise; @@ -22,26 +22,48 @@ public class GearRatioCalculationCommand extends Command { private double startingEncoderPosition; private double gearRatio; - public GearRatioCalculationCommand(DoubleSupplier rotorPositionSupplier, DoubleSupplier encoderPositionSupplier, DoubleConsumer runGearRatioCalculation, SubsystemBase requirement) { + public GearRatioCalculationCommand( + DoubleSupplier rotorPositionSupplier, + DoubleSupplier encoderPositionSupplier, + DoubleConsumer runGearRatioCalculation, + double backlashAccountabilitySeconds, + SubsystemBase requirement + ) { this.rotorPositionSupplier = rotorPositionSupplier; this.encoderPositionSupplier = encoderPositionSupplier; this.runGearRatioCalculation = runGearRatioCalculation; this.subsystemName = requirement.getName(); + this.backlashAccountabilitySeconds = backlashAccountabilitySeconds; this.movementVoltage = new LoggedDashboardNumber("GearRatioCalculation/" + this.subsystemName + "/Voltage", 1); this.shouldMoveClockwise = new LoggedDashboardBoolean("GearRatioCalculation/" + this.subsystemName + "/ShouldMoveClockwise", false); addRequirements(requirement); + addCommands( + getBacklashAccountabilityCommand(), + getGearRatioCalculationCommand() + ); } - @Override - public void initialize() { + private Command getBacklashAccountabilityCommand() { + return new WaitCommand(backlashAccountabilitySeconds); + } + + private Command getGearRatioCalculationCommand() { + return new FunctionalCommand( + this::getStartingPositions, + this::logGearRatio, + interrupted -> printResult(), + () -> false + ); + } + + private void getStartingPositions() { startingRotorPosition = rotorPositionSupplier.getAsDouble(); startingEncoderPosition = encoderPositionSupplier.getAsDouble(); } - @Override - public void execute() { + private void logGearRatio() { runGearRatioCalculation.accept(movementVoltage.get() * getRotationDirection()); gearRatio = calculateGearRatio(); @@ -50,11 +72,6 @@ public void execute() { Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/GearRatio", gearRatio); } - @Override - public void end(boolean interrupted) { - printResult(); - } - private double calculateGearRatio() { final double currentRotorPosition = getRotorDistance(); final double currentEncoderPosition = getEncoderDistance(); @@ -76,4 +93,4 @@ private int getRotationDirection() { private void printResult() { System.out.println(subsystemName + " Gear Ratio: " + gearRatio); } -} +} \ No newline at end of file From 2ed98914e41ef9bd5be6e9d1f0fa2c7555d88838 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Fri, 15 Nov 2024 13:45:25 +0200 Subject: [PATCH 03/21] cleaned a bit, and fixed gearRatioLogic issue --- .../commands/GearRatioCalculationCommand.java | 31 +++++++++++++------ .../WheelRadiusCharacterizationCommand.java | 8 +++-- 2 files changed, 27 insertions(+), 12 deletions(-) diff --git a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java index 5747be3a..1eb7199e 100644 --- a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java +++ b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java @@ -13,7 +13,7 @@ public class GearRatioCalculationCommand extends SequentialCommandGroup { private final DoubleSupplier encoderPositionSupplier; private final DoubleConsumer runGearRatioCalculation; private final String subsystemName; - private final double backlashAccountabilitySeconds; + private final double backlashAccountabilityTimeSeconds; private final LoggedDashboardNumber movementVoltage; private final LoggedDashboardBoolean shouldMoveClockwise; @@ -26,35 +26,43 @@ public GearRatioCalculationCommand( DoubleSupplier rotorPositionSupplier, DoubleSupplier encoderPositionSupplier, DoubleConsumer runGearRatioCalculation, - double backlashAccountabilitySeconds, + double backlashAccountabilityTimeSeconds, SubsystemBase requirement ) { this.rotorPositionSupplier = rotorPositionSupplier; this.encoderPositionSupplier = encoderPositionSupplier; this.runGearRatioCalculation = runGearRatioCalculation; this.subsystemName = requirement.getName(); - this.backlashAccountabilitySeconds = backlashAccountabilitySeconds; + this.backlashAccountabilityTimeSeconds = backlashAccountabilityTimeSeconds; this.movementVoltage = new LoggedDashboardNumber("GearRatioCalculation/" + this.subsystemName + "/Voltage", 1); this.shouldMoveClockwise = new LoggedDashboardBoolean("GearRatioCalculation/" + this.subsystemName + "/ShouldMoveClockwise", false); addRequirements(requirement); addCommands( + getGearRatioCalculationCommand(), getBacklashAccountabilityCommand(), - getGearRatioCalculationCommand() + getLogGearRatioCommand() ); } private Command getBacklashAccountabilityCommand() { - return new WaitCommand(backlashAccountabilitySeconds); + return new WaitCommand(backlashAccountabilityTimeSeconds); } private Command getGearRatioCalculationCommand() { - return new FunctionalCommand( + return new InitExecuteCommand( this::getStartingPositions, - this::logGearRatio, - interrupted -> printResult(), - () -> false + this::runGearRatioCalculation + ); + } + + private Command getLogGearRatioCommand() { + return new InstantCommand( + () -> { + logGearRatio(); + printResult(); + } ); } @@ -63,9 +71,12 @@ private void getStartingPositions() { startingEncoderPosition = encoderPositionSupplier.getAsDouble(); } - private void logGearRatio() { + private void runGearRatioCalculation() { runGearRatioCalculation.accept(movementVoltage.get() * getRotationDirection()); gearRatio = calculateGearRatio(); + } + + private void logGearRatio() { Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/RotorDistance", getRotorDistance()); Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/EncoderDistance", getEncoderDistance()); diff --git a/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java b/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java index d6fb4552..46c61555 100644 --- a/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java +++ b/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java @@ -78,8 +78,7 @@ public void execute() { calculateDriveWheelRadius(); Logger.recordOutput("WheelRadiusCharacterization/AccumulatedGyroYawRadians", accumulatedGyroYawRadians); - for (int i = 0; i < driveWheelRadii.length; i++) - Logger.recordOutput("RadiusCharacterization/DriveWheelRadiusModule" + i, driveWheelRadii[i]); + logWheelRadii(); } @Override @@ -123,6 +122,11 @@ private void printResults() { System.out.println("Drive Wheel Radius for Module " + i + ": " + driveWheelRadii[i] + " meters"); } + private void logWheelRadii() { + for (int i = 0; i < driveWheelRadii.length; i++) + Logger.recordOutput("RadiusCharacterization/DriveWheelRadiusModule" + i, driveWheelRadii[i]); + } + private int getRotationDirection() { return SHOULD_MOVE_CLOCKWISE.get() ? -1 : 1; } From b0c79da29fb899d8354e1d11ebe5c0269c8eea97 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Mon, 18 Nov 2024 11:44:38 +0200 Subject: [PATCH 04/21] cleaned up code --- .../commands/GearRatioCalculationCommand.java | 9 +-- .../WheelRadiusCharacterizationCommand.java | 64 +++++++++++++------ 2 files changed, 46 insertions(+), 27 deletions(-) diff --git a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java index 1eb7199e..6e5e0c9c 100644 --- a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java +++ b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java @@ -2,7 +2,6 @@ import edu.wpi.first.wpilibj2.command.*; import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.networktables.LoggedDashboardBoolean; import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; import java.util.function.DoubleConsumer; @@ -16,7 +15,6 @@ public class GearRatioCalculationCommand extends SequentialCommandGroup { private final double backlashAccountabilityTimeSeconds; private final LoggedDashboardNumber movementVoltage; - private final LoggedDashboardBoolean shouldMoveClockwise; private double startingRotorPosition; private double startingEncoderPosition; @@ -36,7 +34,6 @@ public GearRatioCalculationCommand( this.backlashAccountabilityTimeSeconds = backlashAccountabilityTimeSeconds; this.movementVoltage = new LoggedDashboardNumber("GearRatioCalculation/" + this.subsystemName + "/Voltage", 1); - this.shouldMoveClockwise = new LoggedDashboardBoolean("GearRatioCalculation/" + this.subsystemName + "/ShouldMoveClockwise", false); addRequirements(requirement); addCommands( @@ -72,7 +69,7 @@ private void getStartingPositions() { } private void runGearRatioCalculation() { - runGearRatioCalculation.accept(movementVoltage.get() * getRotationDirection()); + runGearRatioCalculation.accept(movementVoltage.get()); gearRatio = calculateGearRatio(); } @@ -97,10 +94,6 @@ 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); } diff --git a/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java b/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java index 46c61555..3270b925 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; @@ -23,7 +22,6 @@ 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, @@ -37,26 +35,56 @@ public class WheelRadiusCharacterizationCommand extends Command { private double accumulatedGyroYawRadians; private double[] startingWheelPositions; - public WheelRadiusCharacterizationCommand(Translation2d[] wheelDistancesFromCenterMeters, + public WheelRadiusCharacterizationCommand( + Translation2d[] wheelDistancesFromCenterMeters, + Supplier wheelPositionsRadiansSupplier, + DoubleSupplier gyroYawRadiansSupplier, + DoubleConsumer runWheelRadiusCharacterization, + SubsystemBase requirement) { + this( + Arrays.stream(wheelDistancesFromCenterMeters).mapToDouble(Translation2d::getNorm).toArray(), + wheelPositionsRadiansSupplier, + gyroYawRadiansSupplier, + runWheelRadiusCharacterization, + requirement + ); + } + + public WheelRadiusCharacterizationCommand( + double wheelDistancFromCenterMeters, + Supplier wheelPositionsRadiansSupplier, + DoubleSupplier gyroYawRadiansSupplier, + DoubleConsumer runWheelRadiusCharacterization, + SubsystemBase requirement) { + this( + new double[]{wheelDistancFromCenterMeters, wheelDistancFromCenterMeters, wheelDistancFromCenterMeters, wheelDistancFromCenterMeters}, + wheelPositionsRadiansSupplier, + gyroYawRadiansSupplier, + runWheelRadiusCharacterization, + requirement + ); + } + + public WheelRadiusCharacterizationCommand(Translation2d wheelDistanceFromCenterMeters, 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; - this.driveWheelRadii = new double[wheelDistancesFromCenterMeters.length]; - addRequirements(requirement); + SubsystemBase requirement) { + this( + new Translation2d[]{wheelDistanceFromCenterMeters, wheelDistanceFromCenterMeters, wheelDistanceFromCenterMeters, wheelDistanceFromCenterMeters}, + wheelPositionsRadiansSupplier, + gyroYawRadiansSupplier, + runWheelRadiusCharacterization, + requirement + ); } + public WheelRadiusCharacterizationCommand(double[] wheelDistancesFromCenterMeters, Supplier wheelPositionsRadiansSupplier, DoubleSupplier gyroYawRadiansSupplier, DoubleConsumer runWheelRadiusCharacterization, - SubsystemBase requirement - ) { + SubsystemBase requirement) { this.wheelDistancesFromCenterMeters = wheelDistancesFromCenterMeters; this.wheelPositionsRadiansSupplier = wheelPositionsRadiansSupplier; this.gyroYawRadiansSupplier = gyroYawRadiansSupplier; @@ -96,7 +124,7 @@ private void configureStartingStats() { } private void driveMotors() { - runWheelRadiusCharacterization.accept(rotationSlewRateLimiter.calculate(getRotationDirection() * CHARACTERIZATION_SPEED.get())); + runWheelRadiusCharacterization.accept(rotationSlewRateLimiter.calculate(CHARACTERIZATION_SPEED.get())); } private double getAccumulatedGyroYaw() { @@ -109,7 +137,7 @@ private void calculateDriveWheelRadius() { for (int i = 0; i < 4; i++) { final double accumulatedWheelRadians = Math.abs(wheelPositionsRadians[i] - startingWheelPositions[i]); driveWheelRadii[i] = (accumulatedGyroYawRadians * wheelDistancesFromCenterMeters[i]) / accumulatedWheelRadians; - Logger.recordOutput("RadiusCharacterization/AccumulatedWheelRadians" + i, driveWheelRadii[i]); + Logger.recordOutput("RadiusCharacterization/AccumulatedWheelRadians" + i, accumulatedWheelRadians); } } @@ -120,14 +148,12 @@ private void printResults() { } 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() + " meters"); } private void logWheelRadii() { for (int i = 0; i < driveWheelRadii.length; i++) Logger.recordOutput("RadiusCharacterization/DriveWheelRadiusModule" + i, driveWheelRadii[i]); - } - - private int getRotationDirection() { - return SHOULD_MOVE_CLOCKWISE.get() ? -1 : 1; + Logger.recordOutput("RadiusCharacterization/AverageDriveWheelRadius", Arrays.stream(driveWheelRadii).average().getAsDouble()); } } \ No newline at end of file From 5659d9c2145ec1e696d79c33486b28ac982eb2d7 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Mon, 18 Nov 2024 18:07:16 +0200 Subject: [PATCH 05/21] added jDocs --- .../commands/GearRatioCalculationCommand.java | 15 +++- .../WheelRadiusCharacterizationCommand.java | 70 ++++++++++++++----- 2 files changed, 67 insertions(+), 18 deletions(-) diff --git a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java index 6e5e0c9c..21bcf9fd 100644 --- a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java +++ b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java @@ -7,6 +7,9 @@ import java.util.function.DoubleConsumer; import java.util.function.DoubleSupplier; +/** + * A command that calculates and logs the gear ratio of a subsystem by comparing the positions of a rotor and an encoder.. + */ public class GearRatioCalculationCommand extends SequentialCommandGroup { private final DoubleSupplier rotorPositionSupplier; private final DoubleSupplier encoderPositionSupplier; @@ -20,13 +23,21 @@ public class GearRatioCalculationCommand extends SequentialCommandGroup { private double startingEncoderPosition; private double gearRatio; + /** + * Creates a new GearRatioCalculationCommand. + * + * @param rotorPositionSupplier a supplier that returns the current position of the rotor + * @param encoderPositionSupplier a supplier that returns the current position of the encoder + * @param runGearRatioCalculation a consumer that runs the gear ratio calculation with a given voltage + * @param backlashAccountabilityTimeSeconds the time to wait before logging the gear ratio 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 - ) { + SubsystemBase requirement) { this.rotorPositionSupplier = rotorPositionSupplier; this.encoderPositionSupplier = encoderPositionSupplier; this.runGearRatioCalculation = runGearRatioCalculation; diff --git a/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java b/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java index 3270b925..9214b391 100644 --- a/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java +++ b/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java @@ -19,6 +19,9 @@ 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); @@ -35,14 +38,22 @@ public class WheelRadiusCharacterizationCommand extends Command { private double accumulatedGyroYawRadians; private double[] startingWheelPositions; - public WheelRadiusCharacterizationCommand( - Translation2d[] wheelDistancesFromCenterMeters, - Supplier wheelPositionsRadiansSupplier, - DoubleSupplier gyroYawRadiansSupplier, - DoubleConsumer runWheelRadiusCharacterization, - SubsystemBase 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( - Arrays.stream(wheelDistancesFromCenterMeters).mapToDouble(Translation2d::getNorm).toArray(), + new Translation2d[]{wheelDistanceFromCenterMeters, wheelDistanceFromCenterMeters, wheelDistanceFromCenterMeters, wheelDistanceFromCenterMeters}, wheelPositionsRadiansSupplier, gyroYawRadiansSupplier, runWheelRadiusCharacterization, @@ -50,14 +61,23 @@ public WheelRadiusCharacterizationCommand( ); } + /** + * 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 wheelDistancFromCenterMeters, + Translation2d[] wheelDistancesFromCenterMeters, Supplier wheelPositionsRadiansSupplier, DoubleSupplier gyroYawRadiansSupplier, DoubleConsumer runWheelRadiusCharacterization, SubsystemBase requirement) { this( - new double[]{wheelDistancFromCenterMeters, wheelDistancFromCenterMeters, wheelDistancFromCenterMeters, wheelDistancFromCenterMeters}, + Arrays.stream(wheelDistancesFromCenterMeters).mapToDouble(Translation2d::getNorm).toArray(), wheelPositionsRadiansSupplier, gyroYawRadiansSupplier, runWheelRadiusCharacterization, @@ -65,13 +85,23 @@ public WheelRadiusCharacterizationCommand( ); } - public WheelRadiusCharacterizationCommand(Translation2d wheelDistanceFromCenterMeters, - Supplier wheelPositionsRadiansSupplier, - DoubleSupplier gyroYawRadiansSupplier, - DoubleConsumer runWheelRadiusCharacterization, - SubsystemBase 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 Translation2d[]{wheelDistanceFromCenterMeters, wheelDistanceFromCenterMeters, wheelDistanceFromCenterMeters, wheelDistanceFromCenterMeters}, + new double[]{wheelDistanceFromCenterMeters, wheelDistanceFromCenterMeters, wheelDistanceFromCenterMeters, wheelDistanceFromCenterMeters}, wheelPositionsRadiansSupplier, gyroYawRadiansSupplier, runWheelRadiusCharacterization, @@ -79,7 +109,15 @@ public WheelRadiusCharacterizationCommand(Translation2d wheelDistanceFromCenterM ); } - + /** + * 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, From 5c5d5f514154ac65bc595e6f80a7bd7694044471 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Mon, 18 Nov 2024 18:08:21 +0200 Subject: [PATCH 06/21] fixed minor logic issue --- .../java/org/trigon/commands/GearRatioCalculationCommand.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java index 21bcf9fd..346e2dd6 100644 --- a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java +++ b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java @@ -68,6 +68,7 @@ private Command getGearRatioCalculationCommand() { private Command getLogGearRatioCommand() { return new InstantCommand( () -> { + gearRatio = calculateGearRatio(); logGearRatio(); printResult(); } @@ -81,11 +82,9 @@ private void getStartingPositions() { private void runGearRatioCalculation() { runGearRatioCalculation.accept(movementVoltage.get()); - gearRatio = calculateGearRatio(); } private void logGearRatio() { - Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/RotorDistance", getRotorDistance()); Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/EncoderDistance", getEncoderDistance()); Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/GearRatio", gearRatio); From 478af85c8c81d012cd3b05ba3461d076daf64d5e Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Mon, 18 Nov 2024 18:09:06 +0200 Subject: [PATCH 07/21] improved method name --- .../java/org/trigon/commands/GearRatioCalculationCommand.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java index 346e2dd6..bc9cccbc 100644 --- a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java +++ b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java @@ -50,7 +50,7 @@ public GearRatioCalculationCommand( addCommands( getGearRatioCalculationCommand(), getBacklashAccountabilityCommand(), - getLogGearRatioCommand() + getCalculateGearRatioCommand() ); } @@ -65,7 +65,7 @@ private Command getGearRatioCalculationCommand() { ); } - private Command getLogGearRatioCommand() { + private Command getCalculateGearRatioCommand() { return new InstantCommand( () -> { gearRatio = calculateGearRatio(); From e2feecf82a3432dac09ea2e7c599843e2ae47162 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Tue, 19 Nov 2024 12:21:57 +0200 Subject: [PATCH 08/21] fixed a bit of logic and jDocs --- .../commands/GearRatioCalculationCommand.java | 16 ++++++++++------ .../WheelRadiusCharacterizationCommand.java | 2 +- 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java index bc9cccbc..5ed4bcd7 100644 --- a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java +++ b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java @@ -8,7 +8,7 @@ import java.util.function.DoubleSupplier; /** - * A command that calculates and logs the gear ratio of a subsystem by comparing the positions of a rotor and an encoder.. + * A command that calculates and logs the gear ratio of a subsystem by comparing the positions of a rotor and an encoder. */ public class GearRatioCalculationCommand extends SequentialCommandGroup { private final DoubleSupplier rotorPositionSupplier; @@ -48,19 +48,17 @@ public GearRatioCalculationCommand( addRequirements(requirement); addCommands( - getGearRatioCalculationCommand(), - getBacklashAccountabilityCommand(), + getGearRatioCalculationCommand().alongWith(getBacklashAccountabilityCommand()), getCalculateGearRatioCommand() ); } private Command getBacklashAccountabilityCommand() { - return new WaitCommand(backlashAccountabilityTimeSeconds); + return new WaitCommand(backlashAccountabilityTimeSeconds).andThen(getStartingPositionsCommand()); } private Command getGearRatioCalculationCommand() { - return new InitExecuteCommand( - this::getStartingPositions, + return new RunCommand( this::runGearRatioCalculation ); } @@ -75,6 +73,12 @@ private Command getCalculateGearRatioCommand() { ); } + private Command getStartingPositionsCommand() { + return new InstantCommand( + this::getStartingPositions + ); + } + private void getStartingPositions() { startingRotorPosition = rotorPositionSupplier.getAsDouble(); startingEncoderPosition = encoderPositionSupplier.getAsDouble(); diff --git a/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java b/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java index 9214b391..79a4a52f 100644 --- a/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java +++ b/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java @@ -20,7 +20,7 @@ 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. + * 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); From a2788f8b5b4869879de121fd069aa24b7982146a Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Tue, 26 Nov 2024 11:07:13 +0200 Subject: [PATCH 09/21] improved gear ratio calc command --- .../commands/GearRatioCalculationCommand.java | 46 ++++++++----------- 1 file changed, 19 insertions(+), 27 deletions(-) diff --git a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java index 5ed4bcd7..9d721caf 100644 --- a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java +++ b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java @@ -1,6 +1,8 @@ package org.trigon.commands; -import edu.wpi.first.wpilibj2.command.*; +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.LoggedDashboardNumber; @@ -8,9 +10,9 @@ import java.util.function.DoubleSupplier; /** - * A command that calculates and logs the gear ratio of a subsystem by comparing the positions of a rotor and an encoder. + * A command that calculates and logs the gear ratio of a subsystem by comparing the distance traveled of a rotor and an encoder. */ -public class GearRatioCalculationCommand extends SequentialCommandGroup { +public class GearRatioCalculationCommand extends Command { private final DoubleSupplier rotorPositionSupplier; private final DoubleSupplier encoderPositionSupplier; private final DoubleConsumer runGearRatioCalculation; @@ -22,6 +24,7 @@ public class GearRatioCalculationCommand extends SequentialCommandGroup { private double startingRotorPosition; private double startingEncoderPosition; private double gearRatio; + private double startTime; /** * Creates a new GearRatioCalculationCommand. @@ -47,36 +50,25 @@ public GearRatioCalculationCommand( this.movementVoltage = new LoggedDashboardNumber("GearRatioCalculation/" + this.subsystemName + "/Voltage", 1); addRequirements(requirement); - addCommands( - getGearRatioCalculationCommand().alongWith(getBacklashAccountabilityCommand()), - getCalculateGearRatioCommand() - ); } - private Command getBacklashAccountabilityCommand() { - return new WaitCommand(backlashAccountabilityTimeSeconds).andThen(getStartingPositionsCommand()); + @Override + public void initialize() { + startTime = Timer.getFPGATimestamp(); } - private Command getGearRatioCalculationCommand() { - return new RunCommand( - this::runGearRatioCalculation - ); + @Override + public void execute() { + runGearRatioCalculation(); + if (Timer.getFPGATimestamp() - startTime > backlashAccountabilityTimeSeconds) + getStartingPositions(); } - private Command getCalculateGearRatioCommand() { - return new InstantCommand( - () -> { - gearRatio = calculateGearRatio(); - logGearRatio(); - printResult(); - } - ); - } - - private Command getStartingPositionsCommand() { - return new InstantCommand( - this::getStartingPositions - ); + @Override + public void end(boolean interrupted) { + gearRatio = calculateGearRatio(); + logGearRatio(); + printResult(); } private void getStartingPositions() { From 57201b46c2e1ef55be72d02f4174ef24e04d3277 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Tue, 26 Nov 2024 19:32:13 +0200 Subject: [PATCH 10/21] forgot about that. This is why it get's reviewed --- .../org/trigon/commands/GearRatioCalculationCommand.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java index 9d721caf..2a739a9a 100644 --- a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java +++ b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java @@ -10,7 +10,7 @@ import java.util.function.DoubleSupplier; /** - * A command that calculates and logs the gear ratio of a subsystem by comparing the distance traveled of a rotor and an encoder. + * A command that calculates and logs the gear ratio of a subsystem by comparing the distance traveled by a rotor and an encoder. */ public class GearRatioCalculationCommand extends Command { private final DoubleSupplier rotorPositionSupplier; @@ -25,6 +25,7 @@ public class GearRatioCalculationCommand extends Command { private double startingEncoderPosition; private double gearRatio; private double startTime; + private boolean hasSetStartingPositions = false; /** * Creates a new GearRatioCalculationCommand. @@ -60,7 +61,7 @@ public void initialize() { @Override public void execute() { runGearRatioCalculation(); - if (Timer.getFPGATimestamp() - startTime > backlashAccountabilityTimeSeconds) + if (Timer.getFPGATimestamp() - startTime > backlashAccountabilityTimeSeconds && !hasSetStartingPositions) getStartingPositions(); } @@ -74,6 +75,7 @@ public void end(boolean interrupted) { private void getStartingPositions() { startingRotorPosition = rotorPositionSupplier.getAsDouble(); startingEncoderPosition = encoderPositionSupplier.getAsDouble(); + hasSetStartingPositions = true; } private void runGearRatioCalculation() { From be78585b6fb148faab4da1dfc29c346a6d5dc78b Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Wed, 27 Nov 2024 17:57:39 +0200 Subject: [PATCH 11/21] command stuffs --- .../trigon/commands/GearRatioCalculationCommand.java | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java index 2a739a9a..aafaa6c3 100644 --- a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java +++ b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java @@ -33,7 +33,7 @@ public class GearRatioCalculationCommand extends Command { * @param rotorPositionSupplier a supplier that returns the current position of the rotor * @param encoderPositionSupplier a supplier that returns the current position of the encoder * @param runGearRatioCalculation a consumer that runs the gear ratio calculation with a given voltage - * @param backlashAccountabilityTimeSeconds the time to wait before logging the gear ratio in order to account for backlash + * @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( @@ -62,17 +62,18 @@ public void initialize() { public void execute() { runGearRatioCalculation(); if (Timer.getFPGATimestamp() - startTime > backlashAccountabilityTimeSeconds && !hasSetStartingPositions) - getStartingPositions(); + setStartingPositions(); + log(); } @Override public void end(boolean interrupted) { gearRatio = calculateGearRatio(); - logGearRatio(); + log(); printResult(); } - private void getStartingPositions() { + private void setStartingPositions() { startingRotorPosition = rotorPositionSupplier.getAsDouble(); startingEncoderPosition = encoderPositionSupplier.getAsDouble(); hasSetStartingPositions = true; @@ -82,7 +83,7 @@ private void runGearRatioCalculation() { runGearRatioCalculation.accept(movementVoltage.get()); } - private void logGearRatio() { + private void log() { Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/RotorDistance", getRotorDistance()); Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/EncoderDistance", getEncoderDistance()); Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/GearRatio", gearRatio); From 3721187955d091c9c9b548fac2cf8c054181cd45 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Thu, 28 Nov 2024 16:44:26 +0200 Subject: [PATCH 12/21] improved gearration jdoc --- .../java/org/trigon/commands/GearRatioCalculationCommand.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java index aafaa6c3..314a75f3 100644 --- a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java +++ b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java @@ -32,7 +32,7 @@ public class GearRatioCalculationCommand extends Command { * * @param rotorPositionSupplier a supplier that returns the current position of the rotor * @param encoderPositionSupplier a supplier that returns the current position of the encoder - * @param runGearRatioCalculation a consumer that runs the gear ratio calculation with a given voltage + * @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 */ From 1a5b4b6202742d9edf7793fc3499ed9d1306567e Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Fri, 29 Nov 2024 14:24:15 +0200 Subject: [PATCH 13/21] removed extra new line, I'll test it soon --- .../java/org/trigon/commands/GearRatioCalculationCommand.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java index 314a75f3..4c46a2f6 100644 --- a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java +++ b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java @@ -18,9 +18,8 @@ public class GearRatioCalculationCommand extends Command { private final DoubleConsumer runGearRatioCalculation; private final String subsystemName; private final double backlashAccountabilityTimeSeconds; - private final LoggedDashboardNumber movementVoltage; - + private double startingRotorPosition; private double startingEncoderPosition; private double gearRatio; From b039a08ab777cd495d1e0c3004d37eebeff2bef9 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Sun, 1 Dec 2024 17:47:15 +0200 Subject: [PATCH 14/21] added new constructor, and thing --- .../commands/GearRatioCalculationCommand.java | 36 ++++++++++++++++--- 1 file changed, 32 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java index 4c46a2f6..dbd39227 100644 --- a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java +++ b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java @@ -1,10 +1,16 @@ 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.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 org.trigon.utilities.Conversions; import java.util.function.DoubleConsumer; import java.util.function.DoubleSupplier; @@ -19,7 +25,7 @@ public class GearRatioCalculationCommand extends Command { private final String subsystemName; private final double backlashAccountabilityTimeSeconds; private final LoggedDashboardNumber movementVoltage; - + private double startingRotorPosition; private double startingEncoderPosition; private double gearRatio; @@ -29,8 +35,26 @@ public class GearRatioCalculationCommand extends Command { /** * Creates a new GearRatioCalculationCommand. * - * @param rotorPositionSupplier a supplier that returns the current position of the rotor - * @param encoderPositionSupplier a supplier that returns the current position of the encoder + * @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( + () -> Conversions.rotationsToDegrees(motor.getSignal(TalonFXSignal.ROTOR_POSITION)), + () -> Conversions.rotationsToDegrees(encoder.getSignal(CANcoderSignal.POSITION)), + (voltage) -> motor.setControl(new VoltageOut(voltage)), + backlashAccountabilityTimeSeconds, + 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 @@ -55,6 +79,7 @@ public GearRatioCalculationCommand( @Override public void initialize() { startTime = Timer.getFPGATimestamp(); + gearRatio = 0; } @Override @@ -62,7 +87,10 @@ public void execute() { runGearRatioCalculation(); if (Timer.getFPGATimestamp() - startTime > backlashAccountabilityTimeSeconds && !hasSetStartingPositions) setStartingPositions(); - log(); + if (hasSetStartingPositions) { + gearRatio = calculateGearRatio(); + log(); + } } @Override From e632b2970807ce34a626e762e1ec85a7378b17e4 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Sun, 1 Dec 2024 18:09:46 +0200 Subject: [PATCH 15/21] added 0 check --- .../java/org/trigon/commands/GearRatioCalculationCommand.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java index dbd39227..f163755e 100644 --- a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java +++ b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java @@ -119,6 +119,8 @@ private void log() { private double calculateGearRatio() { final double currentRotorPosition = getRotorDistance(); final double currentEncoderPosition = getEncoderDistance(); + if (currentEncoderPosition == 0) + return 0; return currentRotorPosition / currentEncoderPosition; } From 706d9925d57f588b5d8221112ba95cbbe122a414 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Mon, 2 Dec 2024 21:28:36 +0200 Subject: [PATCH 16/21] clean up gear ratio calc --- .../commands/GearRatioCalculationCommand.java | 23 +++++++++++-------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java index f163755e..de7f9d3d 100644 --- a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java +++ b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java @@ -10,7 +10,6 @@ import org.trigon.hardware.phoenix6.cancoder.CANcoderSignal; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; -import org.trigon.utilities.Conversions; import java.util.function.DoubleConsumer; import java.util.function.DoubleSupplier; @@ -34,6 +33,7 @@ public class GearRatioCalculationCommand extends Command { /** * Creates a new GearRatioCalculationCommand. + * This constructor takes a motor to run the gear ratio calculation on, and an encoder to measure the distance traveled. * * @param motor the motor that drives the rotor * @param encoder the encoder that measures the distance traveled @@ -42,8 +42,8 @@ public class GearRatioCalculationCommand extends Command { */ public GearRatioCalculationCommand(TalonFXMotor motor, CANcoderEncoder encoder, double backlashAccountabilityTimeSeconds, SubsystemBase requirement) { this( - () -> Conversions.rotationsToDegrees(motor.getSignal(TalonFXSignal.ROTOR_POSITION)), - () -> Conversions.rotationsToDegrees(encoder.getSignal(CANcoderSignal.POSITION)), + () -> motor.getSignal(TalonFXSignal.ROTOR_POSITION), + () -> encoder.getSignal(CANcoderSignal.POSITION), (voltage) -> motor.setControl(new VoltageOut(voltage)), backlashAccountabilityTimeSeconds, requirement @@ -85,18 +85,20 @@ public void initialize() { @Override public void execute() { runGearRatioCalculation(); + if (Timer.getFPGATimestamp() - startTime > backlashAccountabilityTimeSeconds && !hasSetStartingPositions) setStartingPositions(); + if (hasSetStartingPositions) { gearRatio = calculateGearRatio(); - log(); + logGearRatioAndDistance(); } } @Override public void end(boolean interrupted) { gearRatio = calculateGearRatio(); - log(); + logGearRatioAndDistance(); printResult(); } @@ -110,18 +112,19 @@ private void runGearRatioCalculation() { runGearRatioCalculation.accept(movementVoltage.get()); } - private void log() { + private void logGearRatioAndDistance() { Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/RotorDistance", getRotorDistance()); Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/EncoderDistance", getEncoderDistance()); Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/GearRatio", gearRatio); } private double calculateGearRatio() { - final double currentRotorPosition = getRotorDistance(); - final double currentEncoderPosition = getEncoderDistance(); - if (currentEncoderPosition == 0) + final double currentRotorDistance = getRotorDistance(); + final double currentEncoderDistance = getEncoderDistance(); + if (currentEncoderDistance == 0) { return 0; - return currentRotorPosition / currentEncoderPosition; + } + return currentRotorDistance / currentEncoderDistance; } private double getRotorDistance() { From 12cab58b70a5d242bd3ea23bdf411b70727a838c Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Mon, 2 Dec 2024 21:38:03 +0200 Subject: [PATCH 17/21] prettyaffied wheel radius results print --- .../org/trigon/commands/WheelRadiusCharacterizationCommand.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java b/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java index 79a4a52f..07a7cc52 100644 --- a/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java +++ b/src/main/java/org/trigon/commands/WheelRadiusCharacterizationCommand.java @@ -186,7 +186,7 @@ private void printResults() { } 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() + " meters"); + System.out.println("Average Drive Wheel Radius: " + Arrays.stream(driveWheelRadii).average().getAsDouble() + " meters"); } private void logWheelRadii() { From 28bba1f15ba4acc6f634ff079c1d195a700717f3 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Mon, 2 Dec 2024 21:40:08 +0200 Subject: [PATCH 18/21] cleaning --- .../org/trigon/commands/GearRatioCalculationCommand.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java index de7f9d3d..865beea1 100644 --- a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java +++ b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java @@ -33,7 +33,8 @@ public class GearRatioCalculationCommand extends Command { /** * Creates a new GearRatioCalculationCommand. - * This constructor takes a motor to run the gear ratio calculation on, and an encoder to measure the distance traveled. + * This constructor takes a motor to run the gear ratio calculation on, and to measure the distance the rotor travels. + * It also an encoder to measure the distance traveled. * * @param motor the motor that drives the rotor * @param encoder the encoder that measures the distance traveled @@ -121,9 +122,8 @@ private void logGearRatioAndDistance() { private double calculateGearRatio() { final double currentRotorDistance = getRotorDistance(); final double currentEncoderDistance = getEncoderDistance(); - if (currentEncoderDistance == 0) { + if (currentEncoderDistance == 0) return 0; - } return currentRotorDistance / currentEncoderDistance; } From 24ed18bed0c37fc6296f1c4db29abed864fff0a9 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Mon, 2 Dec 2024 21:47:13 +0200 Subject: [PATCH 19/21] =?UTF-8?q?subsystem=20->=20mechanism=20=F0=9F=A4=93?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../java/org/trigon/commands/GearRatioCalculationCommand.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java index 865beea1..22418c34 100644 --- a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java +++ b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java @@ -15,7 +15,7 @@ import java.util.function.DoubleSupplier; /** - * A command that calculates and logs the gear ratio of a subsystem by comparing the distance traveled by a rotor and an encoder. + * 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; From f5282df2a2e79107fa46de1c5c25b610a9306b71 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Mon, 2 Dec 2024 21:56:40 +0200 Subject: [PATCH 20/21] 0 --- .../org/trigon/commands/GearRatioCalculationCommand.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java index 22418c34..2756da3f 100644 --- a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java +++ b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java @@ -34,7 +34,7 @@ public class GearRatioCalculationCommand extends Command { /** * 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 an encoder to measure the distance traveled. + * 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 @@ -81,6 +81,9 @@ public GearRatioCalculationCommand( public void initialize() { startTime = Timer.getFPGATimestamp(); gearRatio = 0; + Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/RotorDistance", 0); + Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/EncoderDistance", 0); + Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/GearRatio", gearRatio); } @Override From 27c2d2c61973aebce95d6725d9e66097f0db964c Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Mon, 2 Dec 2024 22:11:17 +0200 Subject: [PATCH 21/21] extracted --- .../trigon/commands/GearRatioCalculationCommand.java | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java index 2756da3f..3222ed96 100644 --- a/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java +++ b/src/main/java/org/trigon/commands/GearRatioCalculationCommand.java @@ -81,9 +81,7 @@ public GearRatioCalculationCommand( public void initialize() { startTime = Timer.getFPGATimestamp(); gearRatio = 0; - Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/RotorDistance", 0); - Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/EncoderDistance", 0); - Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/GearRatio", gearRatio); + resetLogs(); } @Override @@ -122,6 +120,12 @@ private void logGearRatioAndDistance() { Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/GearRatio", gearRatio); } + 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 currentRotorDistance = getRotorDistance(); final double currentEncoderDistance = getEncoderDistance();