Skip to content

Commit d7a5411

Browse files
mwitcpalekmwitcpalek
committed
fix up FX swerve module with new rawEncoder
1 parent df0f008 commit d7a5411

File tree

4 files changed

+33
-49
lines changed

4 files changed

+33
-49
lines changed

‎build.gradle‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,7 @@ plugins {
1111
}
1212

1313
group = "org.strykeforce"
14-
version = "25.0.3"
14+
version = "25.0.4"
1515

1616
java {
1717
targetCompatibility = JavaVersion.VERSION_17

‎src/main/java/org/strykeforce/swerve/FXSwerveModule.java‎

Lines changed: 11 additions & 41 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,7 @@
88
import com.ctre.phoenix6.controls.*;
99
import com.ctre.phoenix6.hardware.TalonFX;
1010
import com.ctre.phoenix6.hardware.TalonFXS;
11+
import edu.wpi.first.math.MathUtil;
1112
import edu.wpi.first.math.geometry.Rotation2d;
1213
import edu.wpi.first.math.geometry.Translation2d;
1314
import edu.wpi.first.math.kinematics.SwerveModulePosition;
@@ -41,7 +42,6 @@ public class FXSwerveModule implements SwerveModule {
4142
private final TalonFX driveTalon;
4243
private final double azimuthCountsPerRev;
4344
private final double driveCountsPerRev;
44-
private final double azimuthPulseWidthCPR;
4545
private final double driveGearRatio;
4646
private final double wheelCircumferenceMeters;
4747
private final double driveDeadbandMetersPerSecond;
@@ -53,7 +53,6 @@ public class FXSwerveModule implements SwerveModule {
5353
private int closedLoopSlot;
5454
private int azimuthSlot;
5555
private boolean compensateLatency;
56-
private double azimuthPositionOffset = 0.0;
5756

5857
// Status Signals
5958
private StatusSignal<Angle> drivePosition;
@@ -65,7 +64,6 @@ private FXSwerveModule(FXBuilder builder) {
6564
driveTalon = builder.driveTalon;
6665
azimuthCountsPerRev = builder.azimuthCountsPerRev;
6766
driveCountsPerRev = builder.driveCountsPerRev;
68-
azimuthPulseWidthCPR = builder.azimuthPulseWidthCPR;
6967
driveGearRatio = builder.driveGearRatio;
7068
wheelCircumferenceMeters = Math.PI * Inches.of(builder.wheelDiameterInches).in(Meters);
7169
driveDeadbandMetersPerSecond = builder.driveDeadbandMetersPerSecond;
@@ -166,13 +164,11 @@ public void loadAndSetAzimuthZeroReference() {
166164

167165
double azimuthAbsoluteCounts = getAzimuthAbsoluteEncoderCounts();
168166
logger.info("swerve module {}: azimuth absolute position = {}", index, azimuthAbsoluteCounts);
169-
logger.info(
170-
"swerve module {}: current azimuth endocder position = {}",
171-
index,
172-
azimuthAbsoluteCounts - reference);
167+
double azimuthSetpoint = azimuthAbsoluteCounts - reference;
168+
azimuthTalon.setPosition(azimuthSetpoint);
169+
logger.info("swerve module {}: set azimuth encoder = {}", index, azimuthSetpoint);
173170

174-
azimuthPositionOffset = reference / azimuthPulseWidthCPR;
175-
setAzimuthPosition(azimuthAbsoluteCounts);
171+
setAzimuthPosition(azimuthSetpoint);
176172
}
177173

178174
public TalonFXS getAzimuthTalon() {
@@ -184,21 +180,12 @@ public TalonFX getDriveTalon() {
184180
}
185181

186182
private double getAzimuthAbsoluteEncoderCounts() {
187-
// double extraRotations =
188-
// azimuthPosition.getValueAsDouble() > 0
189-
// ? Math.floor(azimuthPosition.getValueAsDouble())
190-
// : Math.ceil(azimuthPosition.getValueAsDouble());
191-
// return azimuthPosition.getValueAsDouble() - extraRotations; // correct to within one
192-
// rotation
193-
return ((int) (azimuthPosition.getValueAsDouble() * azimuthPulseWidthCPR)) & 0xFFF;
183+
return MathUtil.inputModulus(
184+
azimuthTalon.getRawPulseWidthPosition().getValueAsDouble(), 0.0, 1.0);
194185
}
195186

196187
private double getAzimuthEcoderPosition() {
197-
return azimuthPosition.getValueAsDouble() - azimuthPositionOffset;
198-
}
199-
200-
public double getAzimuthPositionOffset() {
201-
return azimuthPositionOffset;
188+
return azimuthPosition.getValueAsDouble();
202189
}
203190

204191
@Override
@@ -234,19 +221,15 @@ private SwerveModuleState setAzimuthOptimizedState(SwerveModuleState desiredStat
234221
private void setAzimuthPosition(double position) {
235222
if (units == V6TalonSwerveModule.ClosedLoopUnits.DUTY_CYCLE) {
236223
MotionMagicDutyCycle controlRequest =
237-
new MotionMagicDutyCycle(position + azimuthPositionOffset)
238-
.withEnableFOC(false)
239-
.withSlot(azimuthSlot);
224+
new MotionMagicDutyCycle(position).withEnableFOC(false).withSlot(azimuthSlot);
240225
azimuthTalon.setControl(controlRequest);
241226
} else if (units == V6TalonSwerveModule.ClosedLoopUnits.VOLTAGE) {
242227
MotionMagicVoltage controlReqest =
243-
new MotionMagicVoltage(position + azimuthPositionOffset)
244-
.withEnableFOC(false)
245-
.withSlot(azimuthSlot);
228+
new MotionMagicVoltage(position).withEnableFOC(false).withSlot(azimuthSlot);
246229
azimuthTalon.setControl(controlReqest);
247230
} else {
248231
MotionMagicTorqueCurrentFOC controlRequest =
249-
new MotionMagicTorqueCurrentFOC(position + azimuthPositionOffset).withSlot(azimuthSlot);
232+
new MotionMagicTorqueCurrentFOC(position).withSlot(azimuthSlot);
250233
azimuthTalon.setControl(controlRequest);
251234
}
252235
}
@@ -320,7 +303,6 @@ public static class FXBuilder {
320303
public static final double kDefaultTalonFXCountsPerRev = 1.0;
321304
private double azimuthCountsPerRev = kDefaultTalonFXCountsPerRev;
322305
private double driveCountsPerRev = kDefaultTalonFXCountsPerRev;
323-
private double azimuthPulseWidthCPR = 4096.0;
324306

325307
private TalonFXS azimuthTalon;
326308
private TalonFX driveTalon;
@@ -366,11 +348,6 @@ public FXBuilder azimuthEncoderCountsPerRevolution(double countsPerRev) {
366348
return this;
367349
}
368350

369-
public FXBuilder azimuthPulseWidthCPR(double azimuthPulseWidthCPR) {
370-
this.azimuthPulseWidthCPR = azimuthPulseWidthCPR;
371-
return this;
372-
}
373-
374351
public FXBuilder driveDeadbandMetersPerSecond(double driveDeadbandMetersPerSecond) {
375352
this.driveDeadbandMetersPerSecond = driveDeadbandMetersPerSecond;
376353
return this;
@@ -430,13 +407,6 @@ private void validateSwerveModuleObject(FXSwerveModule module) {
430407
if (module.azimuthCountsPerRev <= 0)
431408
throw new IllegalArgumentException(
432409
"azimuth encoder counts per revolution must be greater than zero.");
433-
if (module.azimuthPulseWidthCPR <= 0)
434-
throw new IllegalArgumentException("azimuth pulse width cpr must be greater than zero");
435-
// TalonFXSConfiguration azimuthConfig = new TalonFXSConfiguration();
436-
// module.azimuthTalon.getConfigurator().refresh(azimuthConfig);
437-
// if (azimuthConfig.ExternalFeedback.ExternalFeedbackSensorSource
438-
// != ExternalFeedbackSensorSourceValue.PulseWidth)
439-
// throw new IllegalArgumentException("Azimuth must use Pulse Width Feedback");
440410
if (module.driveCountsPerRev <= 0)
441411
throw new IllegalArgumentException(
442412
"drive encoder counts per revolution must be greater than zero.");

‎src/main/java/org/strykeforce/swerve/SwerveDrive.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -214,7 +214,7 @@ public SwerveModule[] getSwerveModules() {
214214
*/
215215
public void resetOdometry(Pose2d pose) {
216216
for (int i = 0; i < 4; i++) {
217-
swerveModules[0].refreshMotorControllers();
217+
swerveModules[i].refreshMotorControllers();
218218
}
219219
odometry.resetPosition(
220220
pose,

‎src/test/java/org/strykeforce/swerve/TalonSwerveModuleTest.java‎

Lines changed: 20 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@
2626
import com.ctre.phoenix6.controls.VelocityDutyCycle;
2727
import com.ctre.phoenix6.hardware.TalonFX;
2828
import com.ctre.phoenix6.hardware.TalonFXS;
29+
import edu.wpi.first.math.MathUtil;
2930
import edu.wpi.first.math.geometry.Rotation2d;
3031
import edu.wpi.first.math.geometry.Translation2d;
3132
import edu.wpi.first.math.kinematics.SwerveModulePosition;
@@ -231,6 +232,7 @@ class TestWhenSettingFXAzimuthZero {
231232
FXSwerveModule module;
232233

233234
StatusSignal<Angle> azimuthPositionStatusSig;
235+
StatusSignal<Angle> rawPulseWidthStatusSig;
234236
final ArgumentCaptor<MotionMagicDutyCycle> posDutyCycleCaptor =
235237
ArgumentCaptor.forClass(MotionMagicDutyCycle.class);
236238

@@ -249,6 +251,7 @@ void setUp() {
249251
.driveMaximumMetersPerSecond(kMaxSpeedMetersPerSecond)
250252
.build();
251253
azimuthPositionStatusSig = (StatusSignal<Angle>) mock(StatusSignal.class);
254+
rawPulseWidthStatusSig = (StatusSignal<Angle>) mock(StatusSignal.class);
252255
Field azimuthPosField =
253256
ReflectionUtils.findFields(
254257
FXSwerveModule.class,
@@ -261,7 +264,6 @@ void setUp() {
261264
} catch (Exception e) {
262265
throw new IllegalArgumentException("Error: " + e);
263266
}
264-
265267
when(azimuthTalon.getPosition()).thenReturn(azimuthPositionStatusSig);
266268
when(azimuthPositionStatusSig.getValueAsDouble()).thenReturn(0.0);
267269
}
@@ -283,36 +285,46 @@ void storeFXAzimuthZeroReference() {
283285
module = builder.wheelLocationMeters(new Translation2d(1, 1)).build();
284286
when(azimuthTalon.getPosition()).thenReturn(azimuthPositionStatusSig);
285287
when(azimuthPositionStatusSig.getValueAsDouble()).thenReturn(expectedZeroReference);
288+
when(azimuthTalon.getRawPulseWidthPosition()).thenReturn(rawPulseWidthStatusSig);
289+
when(rawPulseWidthStatusSig.getValueAsDouble()).thenReturn(expectedZeroReference);
286290
module.storeAzimuthZeroReference();
287-
assertEquals((int) (expectedZeroReference * 4096.0) & 0xFFF, Preferences.getDouble(key, -1));
291+
assertEquals(expectedZeroReference, Preferences.getDouble(key, -1));
288292

289293
expectedZeroReference = 67.0 / 4096.0;
290294
index = 1; // Front Right
291295
key = String.format("SwerveDrive/wheel.%d", index);
292296
module = builder.wheelLocationMeters(new Translation2d(1, -1)).build();
293297
when(azimuthPositionStatusSig.getValueAsDouble()).thenReturn(expectedZeroReference);
298+
when(azimuthTalon.getRawPulseWidthPosition()).thenReturn(rawPulseWidthStatusSig);
299+
when(rawPulseWidthStatusSig.getValueAsDouble()).thenReturn(expectedZeroReference);
294300
module.storeAzimuthZeroReference();
295-
assertEquals((int) (expectedZeroReference * 4096.0) & 0xFFF, Preferences.getDouble(key, -1));
301+
assertEquals(expectedZeroReference, Preferences.getDouble(key, -1));
296302

297303
expectedZeroReference = -6767.0 / 4096.0;
298304
index = 2; // Back Left
299305
key = String.format("SwerveDrive/wheel.%d", index);
300306
module = builder.wheelLocationMeters(new Translation2d(-1, 1)).build();
301307
when(azimuthPositionStatusSig.getValueAsDouble()).thenReturn(expectedZeroReference);
308+
when(azimuthTalon.getRawPulseWidthPosition()).thenReturn(rawPulseWidthStatusSig);
309+
when(rawPulseWidthStatusSig.getValueAsDouble()).thenReturn(expectedZeroReference);
302310
module.storeAzimuthZeroReference();
303-
assertEquals((int) (expectedZeroReference * 4096.0) & 0xFFF, Preferences.getDouble(key, -1));
311+
assertEquals(
312+
MathUtil.inputModulus(expectedZeroReference, 0.0, 1.0), Preferences.getDouble(key, -1));
304313

305314
expectedZeroReference = 6727.0 / 4096.0;
306315
index = 3; // Back Right
307316
key = String.format("SwerveDrive/wheel.%d", index);
308317
module = builder.wheelLocationMeters(new Translation2d(-1, -1)).build();
309318
when(azimuthPositionStatusSig.getValueAsDouble()).thenReturn(expectedZeroReference);
319+
when(azimuthTalon.getRawPulseWidthPosition()).thenReturn(rawPulseWidthStatusSig);
320+
when(rawPulseWidthStatusSig.getValueAsDouble()).thenReturn(expectedZeroReference);
310321
module.storeAzimuthZeroReference();
311-
assertEquals((int) (expectedZeroReference * 4096.0) & 0xFFF, Preferences.getDouble(key, -1));
322+
assertEquals(
323+
MathUtil.inputModulus(expectedZeroReference, 0.0, 1.0), Preferences.getDouble(key, -1));
312324
}
313325

314326
@ParameterizedTest
315-
@CsvSource({"0, 0, 0", "1, 0, 0", "1.00024, 0, 0.00024", "0, 2767, 0.67554"})
327+
@CsvSource({"1e-6, 0, 1e-6", "1, 0, 1", "1.00024, 0, 0.00024", "1e-6, 0.67554, -0.67554"})
316328
@DisplayName("should set FX azimuth zero")
317329
void shouldSetFXAzimuthZero(
318330
double absoluteEncoderPosition, double zeroReference, double setpoint) {
@@ -322,6 +334,8 @@ void shouldSetFXAzimuthZero(
322334

323335
when(azimuthTalon.getPosition()).thenReturn(azimuthPositionStatusSig);
324336
when(azimuthPositionStatusSig.getValueAsDouble()).thenReturn(absoluteEncoderPosition);
337+
when(azimuthTalon.getRawPulseWidthPosition()).thenReturn(rawPulseWidthStatusSig);
338+
when(rawPulseWidthStatusSig.getValueAsDouble()).thenReturn(absoluteEncoderPosition);
325339
module.loadAndSetAzimuthZeroReference();
326340
verify(azimuthTalon).setControl(posDutyCycleCaptor.capture());
327341
assertEquals(setpoint, posDutyCycleCaptor.getValue().Position, 0.01);

0 commit comments

Comments
 (0)