Skip to content

Commit 21aba14

Browse files
Nummun14Nummun14
authored
Command Changes (#18)
* made WheelRadiusCharacterization calculate for each module separetly * made gear ratio command account for backlash * cleaned a bit, and fixed gearRatioLogic issue * cleaned up code * added jDocs * fixed minor logic issue * improved method name * fixed a bit of logic and jDocs * improved gear ratio calc command * forgot about that. This is why it get's reviewed * command stuffs * improved gearration jdoc * removed extra new line, I'll test it soon * added new constructor, and thing * added 0 check * clean up gear ratio calc * prettyaffied wheel radius results print * cleaning * subsystem -> mechanism 🤓 * 0 * extracted
1 parent 56be586 commit 21aba14

File tree

2 files changed

+193
-47
lines changed

2 files changed

+193
-47
lines changed

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

Lines changed: 88 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -1,64 +1,137 @@
11
package org.trigon.commands;
22

3+
import com.ctre.phoenix6.controls.VoltageOut;
4+
import edu.wpi.first.wpilibj.Timer;
35
import edu.wpi.first.wpilibj2.command.Command;
46
import edu.wpi.first.wpilibj2.command.SubsystemBase;
57
import org.littletonrobotics.junction.Logger;
6-
import org.littletonrobotics.junction.networktables.LoggedDashboardBoolean;
78
import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
9+
import org.trigon.hardware.phoenix6.cancoder.CANcoderEncoder;
10+
import org.trigon.hardware.phoenix6.cancoder.CANcoderSignal;
11+
import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor;
12+
import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal;
813

914
import java.util.function.DoubleConsumer;
1015
import java.util.function.DoubleSupplier;
1116

17+
/**
18+
* A command that calculates and logs the gear ratio of a mechanism by comparing the distance traveled by a rotor and an encoder.
19+
*/
1220
public class GearRatioCalculationCommand extends Command {
1321
private final DoubleSupplier rotorPositionSupplier;
1422
private final DoubleSupplier encoderPositionSupplier;
1523
private final DoubleConsumer runGearRatioCalculation;
1624
private final String subsystemName;
17-
25+
private final double backlashAccountabilityTimeSeconds;
1826
private final LoggedDashboardNumber movementVoltage;
19-
private final LoggedDashboardBoolean shouldMoveClockwise;
2027

2128
private double startingRotorPosition;
2229
private double startingEncoderPosition;
2330
private double gearRatio;
31+
private double startTime;
32+
private boolean hasSetStartingPositions = false;
33+
34+
/**
35+
* Creates a new GearRatioCalculationCommand.
36+
* This constructor takes a motor to run the gear ratio calculation on, and to measure the distance the rotor travels.
37+
* It also takes an encoder to measure the distance traveled.
38+
*
39+
* @param motor the motor that drives the rotor
40+
* @param encoder the encoder that measures the distance traveled
41+
* @param backlashAccountabilityTimeSeconds the time to wait before setting the starting positions in order to account for backlash
42+
* @param requirement the subsystem that this command requires
43+
*/
44+
public GearRatioCalculationCommand(TalonFXMotor motor, CANcoderEncoder encoder, double backlashAccountabilityTimeSeconds, SubsystemBase requirement) {
45+
this(
46+
() -> motor.getSignal(TalonFXSignal.ROTOR_POSITION),
47+
() -> encoder.getSignal(CANcoderSignal.POSITION),
48+
(voltage) -> motor.setControl(new VoltageOut(voltage)),
49+
backlashAccountabilityTimeSeconds,
50+
requirement
51+
);
52+
}
2453

25-
public GearRatioCalculationCommand(DoubleSupplier rotorPositionSupplier, DoubleSupplier encoderPositionSupplier, DoubleConsumer runGearRatioCalculation, SubsystemBase requirement) {
54+
/**
55+
* Creates a new GearRatioCalculationCommand.
56+
*
57+
* @param rotorPositionSupplier a supplier that returns the current position of the rotor in degrees
58+
* @param encoderPositionSupplier a supplier that returns the current position of the encoder in degrees
59+
* @param runGearRatioCalculation a consumer that drives the motor with a given voltage
60+
* @param backlashAccountabilityTimeSeconds the time to wait before setting the starting positions in order to account for backlash
61+
* @param requirement the subsystem that this command requires
62+
*/
63+
public GearRatioCalculationCommand(
64+
DoubleSupplier rotorPositionSupplier,
65+
DoubleSupplier encoderPositionSupplier,
66+
DoubleConsumer runGearRatioCalculation,
67+
double backlashAccountabilityTimeSeconds,
68+
SubsystemBase requirement) {
2669
this.rotorPositionSupplier = rotorPositionSupplier;
2770
this.encoderPositionSupplier = encoderPositionSupplier;
2871
this.runGearRatioCalculation = runGearRatioCalculation;
2972
this.subsystemName = requirement.getName();
73+
this.backlashAccountabilityTimeSeconds = backlashAccountabilityTimeSeconds;
3074

3175
this.movementVoltage = new LoggedDashboardNumber("GearRatioCalculation/" + this.subsystemName + "/Voltage", 1);
32-
this.shouldMoveClockwise = new LoggedDashboardBoolean("GearRatioCalculation/" + this.subsystemName + "/ShouldMoveClockwise", false);
3376

3477
addRequirements(requirement);
3578
}
3679

3780
@Override
3881
public void initialize() {
39-
startingRotorPosition = rotorPositionSupplier.getAsDouble();
40-
startingEncoderPosition = encoderPositionSupplier.getAsDouble();
82+
startTime = Timer.getFPGATimestamp();
83+
gearRatio = 0;
84+
resetLogs();
4185
}
4286

4387
@Override
4488
public void execute() {
45-
runGearRatioCalculation.accept(movementVoltage.get() * getRotationDirection());
89+
runGearRatioCalculation();
90+
91+
if (Timer.getFPGATimestamp() - startTime > backlashAccountabilityTimeSeconds && !hasSetStartingPositions)
92+
setStartingPositions();
93+
94+
if (hasSetStartingPositions) {
95+
gearRatio = calculateGearRatio();
96+
logGearRatioAndDistance();
97+
}
98+
}
99+
100+
@Override
101+
public void end(boolean interrupted) {
46102
gearRatio = calculateGearRatio();
103+
logGearRatioAndDistance();
104+
printResult();
105+
}
47106

107+
private void setStartingPositions() {
108+
startingRotorPosition = rotorPositionSupplier.getAsDouble();
109+
startingEncoderPosition = encoderPositionSupplier.getAsDouble();
110+
hasSetStartingPositions = true;
111+
}
112+
113+
private void runGearRatioCalculation() {
114+
runGearRatioCalculation.accept(movementVoltage.get());
115+
}
116+
117+
private void logGearRatioAndDistance() {
48118
Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/RotorDistance", getRotorDistance());
49119
Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/EncoderDistance", getEncoderDistance());
50120
Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/GearRatio", gearRatio);
51121
}
52122

53-
@Override
54-
public void end(boolean interrupted) {
55-
printResult();
123+
private void resetLogs() {
124+
Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/RotorDistance", 0);
125+
Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/EncoderDistance", 0);
126+
Logger.recordOutput("GearRatioCalculation/" + subsystemName + "/GearRatio", 0);
56127
}
57128

58129
private double calculateGearRatio() {
59-
final double currentRotorPosition = getRotorDistance();
60-
final double currentEncoderPosition = getEncoderDistance();
61-
return currentRotorPosition / currentEncoderPosition;
130+
final double currentRotorDistance = getRotorDistance();
131+
final double currentEncoderDistance = getEncoderDistance();
132+
if (currentEncoderDistance == 0)
133+
return 0;
134+
return currentRotorDistance / currentEncoderDistance;
62135
}
63136

64137
private double getRotorDistance() {
@@ -69,11 +142,7 @@ private double getEncoderDistance() {
69142
return startingEncoderPosition - encoderPositionSupplier.getAsDouble();
70143
}
71144

72-
private int getRotationDirection() {
73-
return shouldMoveClockwise.get() ? -1 : 1;
74-
}
75-
76145
private void printResult() {
77146
System.out.println(subsystemName + " Gear Ratio: " + gearRatio);
78147
}
79-
}
148+
}

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

Lines changed: 105 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -12,20 +12,23 @@
1212
import edu.wpi.first.wpilibj2.command.Command;
1313
import edu.wpi.first.wpilibj2.command.SubsystemBase;
1414
import org.littletonrobotics.junction.Logger;
15-
import org.littletonrobotics.junction.networktables.LoggedDashboardBoolean;
1615
import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
1716

1817
import java.util.Arrays;
1918
import java.util.function.DoubleConsumer;
2019
import java.util.function.DoubleSupplier;
2120
import java.util.function.Supplier;
2221

22+
/**
23+
* 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.
24+
*/
2325
public class WheelRadiusCharacterizationCommand extends Command {
2426
private static final LoggedDashboardNumber CHARACTERIZATION_SPEED = new LoggedDashboardNumber("WheelRadiusCharacterization/SpeedRadiansPerSecond", 1);
2527
private static final LoggedDashboardNumber ROTATION_RATE_LIMIT = new LoggedDashboardNumber("WheelRadiusCharacterization/RotationRateLimit", 1);
26-
private static final LoggedDashboardBoolean SHOULD_MOVE_CLOCKWISE = new LoggedDashboardBoolean("WheelRadiusCharacterization/ShouldMoveClockwise", false);
2728

28-
private final double[] wheelDistancesFromCenterMeters;
29+
private final double[]
30+
wheelDistancesFromCenterMeters,
31+
driveWheelRadii;
2932
private final Supplier<double[]> wheelPositionsRadiansSupplier;
3033
private final DoubleSupplier gyroYawRadiansSupplier;
3134
private final DoubleConsumer runWheelRadiusCharacterization;
@@ -34,25 +37,97 @@ public class WheelRadiusCharacterizationCommand extends Command {
3437
private double startingGyroYawRadians;
3538
private double accumulatedGyroYawRadians;
3639
private double[] startingWheelPositions;
37-
private double driveWheelsRadius;
3840

39-
public WheelRadiusCharacterizationCommand(Translation2d[] wheelDistancesFromCenterMeters, Supplier<double[]> wheelPositionsRadiansSupplier,
40-
DoubleSupplier gyroYawRadiansSupplier, DoubleConsumer runWheelRadiusCharacterization, SubsystemBase requirement
41-
) {
42-
this.wheelDistancesFromCenterMeters = Arrays.stream(wheelDistancesFromCenterMeters).mapToDouble(Translation2d::getNorm).toArray();
43-
this.wheelPositionsRadiansSupplier = wheelPositionsRadiansSupplier;
44-
this.gyroYawRadiansSupplier = gyroYawRadiansSupplier;
45-
this.runWheelRadiusCharacterization = runWheelRadiusCharacterization;
46-
addRequirements(requirement);
41+
/**
42+
* Creates a new WheelRadiusCharacterizationCommand.
43+
*
44+
* @param wheelDistanceFromCenterMeters an array of the distances of each wheel from the center of the robot in meters
45+
* @param wheelPositionsRadiansSupplier a supplier that returns the current position of each wheel in radians
46+
* @param gyroYawRadiansSupplier a supplier that returns the current yaw of the robot in radians
47+
* @param runWheelRadiusCharacterization a consumer that runs the swerve drive with a given speed
48+
* @param requirement the subsystem that this command requires
49+
*/
50+
public WheelRadiusCharacterizationCommand(Translation2d wheelDistanceFromCenterMeters,
51+
Supplier<double[]> wheelPositionsRadiansSupplier,
52+
DoubleSupplier gyroYawRadiansSupplier,
53+
DoubleConsumer runWheelRadiusCharacterization,
54+
SubsystemBase requirement) {
55+
this(
56+
new Translation2d[]{wheelDistanceFromCenterMeters, wheelDistanceFromCenterMeters, wheelDistanceFromCenterMeters, wheelDistanceFromCenterMeters},
57+
wheelPositionsRadiansSupplier,
58+
gyroYawRadiansSupplier,
59+
runWheelRadiusCharacterization,
60+
requirement
61+
);
62+
}
63+
64+
/**
65+
* Creates a new WheelRadiusCharacterizationCommand.
66+
*
67+
* @param wheelDistancesFromCenterMeters an array of the distances of each wheel from the center of the robot in meters
68+
* @param wheelPositionsRadiansSupplier a supplier that returns the current position of each wheel in radians
69+
* @param gyroYawRadiansSupplier a supplier that returns the current yaw of the robot in radians
70+
* @param runWheelRadiusCharacterization a consumer that runs the swerve drive with a given speed
71+
* @param requirement the subsystem that this command requires
72+
*/
73+
public WheelRadiusCharacterizationCommand(
74+
Translation2d[] wheelDistancesFromCenterMeters,
75+
Supplier<double[]> wheelPositionsRadiansSupplier,
76+
DoubleSupplier gyroYawRadiansSupplier,
77+
DoubleConsumer runWheelRadiusCharacterization,
78+
SubsystemBase requirement) {
79+
this(
80+
Arrays.stream(wheelDistancesFromCenterMeters).mapToDouble(Translation2d::getNorm).toArray(),
81+
wheelPositionsRadiansSupplier,
82+
gyroYawRadiansSupplier,
83+
runWheelRadiusCharacterization,
84+
requirement
85+
);
86+
}
87+
88+
/**
89+
* Creates a new WheelRadiusCharacterizationCommand.
90+
*
91+
* @param wheelDistanceFromCenterMeters the distance of the wheels from the center of the robot in meters
92+
* @param wheelPositionsRadiansSupplier a supplier that returns the current position of each wheel in radians
93+
* @param gyroYawRadiansSupplier a supplier that returns the current yaw of the robot in radians
94+
* @param runWheelRadiusCharacterization a consumer that runs the swerve drive with a given speed
95+
* @param requirement the subsystem that this command requires
96+
*/
97+
public WheelRadiusCharacterizationCommand(
98+
double wheelDistanceFromCenterMeters,
99+
Supplier<double[]> wheelPositionsRadiansSupplier,
100+
DoubleSupplier gyroYawRadiansSupplier,
101+
DoubleConsumer runWheelRadiusCharacterization,
102+
SubsystemBase requirement) {
103+
this(
104+
new double[]{wheelDistanceFromCenterMeters, wheelDistanceFromCenterMeters, wheelDistanceFromCenterMeters, wheelDistanceFromCenterMeters},
105+
wheelPositionsRadiansSupplier,
106+
gyroYawRadiansSupplier,
107+
runWheelRadiusCharacterization,
108+
requirement
109+
);
47110
}
48111

49-
public WheelRadiusCharacterizationCommand(double[] wheelDistancesFromCenterMeters, Supplier<double[]> wheelPositionsRadiansSupplier,
50-
DoubleSupplier gyroYawRadiansSupplier, DoubleConsumer runWheelRadiusCharacterization, SubsystemBase requirement
51-
) {
112+
/**
113+
* Creates a new WheelRadiusCharacterizationCommand.
114+
*
115+
* @param wheelDistancesFromCenterMeters an array of the distances of each wheel from the center of the robot in meters
116+
* @param wheelPositionsRadiansSupplier a supplier that returns the current position of each wheel in radians
117+
* @param gyroYawRadiansSupplier a supplier that returns the current yaw of the robot in radians
118+
* @param runWheelRadiusCharacterization a consumer that runs the swerve drive with a given speed
119+
* @param requirement the subsystem that this command requires
120+
*/
121+
public WheelRadiusCharacterizationCommand(double[] wheelDistancesFromCenterMeters,
122+
Supplier<double[]> wheelPositionsRadiansSupplier,
123+
DoubleSupplier gyroYawRadiansSupplier,
124+
DoubleConsumer runWheelRadiusCharacterization,
125+
SubsystemBase requirement) {
52126
this.wheelDistancesFromCenterMeters = wheelDistancesFromCenterMeters;
53127
this.wheelPositionsRadiansSupplier = wheelPositionsRadiansSupplier;
54128
this.gyroYawRadiansSupplier = gyroYawRadiansSupplier;
55129
this.runWheelRadiusCharacterization = runWheelRadiusCharacterization;
130+
this.driveWheelRadii = new double[wheelDistancesFromCenterMeters.length];
56131
addRequirements(requirement);
57132
}
58133

@@ -66,10 +141,10 @@ public void execute() {
66141
driveMotors();
67142

68143
accumulatedGyroYawRadians = getAccumulatedGyroYaw();
69-
driveWheelsRadius = calculateDriveWheelRadius();
144+
calculateDriveWheelRadius();
70145

71146
Logger.recordOutput("WheelRadiusCharacterization/AccumulatedGyroYawRadians", accumulatedGyroYawRadians);
72-
Logger.recordOutput("RadiusCharacterization/DriveWheelRadius", driveWheelsRadius);
147+
logWheelRadii();
73148
}
74149

75150
@Override
@@ -87,34 +162,36 @@ private void configureStartingStats() {
87162
}
88163

89164
private void driveMotors() {
90-
runWheelRadiusCharacterization.accept(rotationSlewRateLimiter.calculate(getRotationDirection() * CHARACTERIZATION_SPEED.get()));
165+
runWheelRadiusCharacterization.accept(rotationSlewRateLimiter.calculate(CHARACTERIZATION_SPEED.get()));
91166
}
92167

93168
private double getAccumulatedGyroYaw() {
94169
return Math.abs(startingGyroYawRadians - gyroYawRadiansSupplier.getAsDouble());
95170
}
96171

97-
private double calculateDriveWheelRadius() {
98-
driveWheelsRadius = 0;
172+
private void calculateDriveWheelRadius() {
99173
final double[] wheelPositionsRadians = wheelPositionsRadiansSupplier.get();
100174

101175
for (int i = 0; i < 4; i++) {
102176
final double accumulatedWheelRadians = Math.abs(wheelPositionsRadians[i] - startingWheelPositions[i]);
103-
driveWheelsRadius += (accumulatedGyroYawRadians * wheelDistancesFromCenterMeters[i]) / accumulatedWheelRadians;
177+
driveWheelRadii[i] = (accumulatedGyroYawRadians * wheelDistancesFromCenterMeters[i]) / accumulatedWheelRadians;
104178
Logger.recordOutput("RadiusCharacterization/AccumulatedWheelRadians" + i, accumulatedWheelRadians);
105179
}
106-
107-
return driveWheelsRadius /= 4;
108180
}
109181

110182
private void printResults() {
111-
if (accumulatedGyroYawRadians <= Math.PI * 2.0)
183+
if (accumulatedGyroYawRadians <= Math.PI * 2.0) {
112184
System.out.println("Not enough data for characterization");
113-
else
114-
System.out.println("Drive Wheel Radius: " + driveWheelsRadius + " meters");
185+
return;
186+
}
187+
for (int i = 0; i < driveWheelRadii.length; i++)
188+
System.out.println("Drive Wheel Radius for Module " + i + ": " + driveWheelRadii[i] + " meters");
189+
System.out.println("Average Drive Wheel Radius: " + Arrays.stream(driveWheelRadii).average().getAsDouble() + " meters");
115190
}
116191

117-
private int getRotationDirection() {
118-
return SHOULD_MOVE_CLOCKWISE.get() ? -1 : 1;
192+
private void logWheelRadii() {
193+
for (int i = 0; i < driveWheelRadii.length; i++)
194+
Logger.recordOutput("RadiusCharacterization/DriveWheelRadiusModule" + i, driveWheelRadii[i]);
195+
Logger.recordOutput("RadiusCharacterization/AverageDriveWheelRadius", Arrays.stream(driveWheelRadii).average().getAsDouble());
119196
}
120197
}

0 commit comments

Comments
 (0)