Skip to content

Commit 34083e5

Browse files
mwitcpalekmwitcpalek
authored
Merge pull request #116 from strykeforce/fxSwerve-updates
Add Encoder Opposing Option to FXSwerveModule
2 parents d7a5411 + 8c4969e commit 34083e5

File tree

3 files changed

+42
-14
lines changed

3 files changed

+42
-14
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.4"
14+
version = "25.0.5"
1515

1616
java {
1717
targetCompatibility = JavaVersion.VERSION_17

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

Lines changed: 15 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -53,6 +53,7 @@ public class FXSwerveModule implements SwerveModule {
5353
private int closedLoopSlot;
5454
private int azimuthSlot;
5555
private boolean compensateLatency;
56+
private boolean encoderOpposed;
5657

5758
// Status Signals
5859
private StatusSignal<Angle> drivePosition;
@@ -74,6 +75,7 @@ private FXSwerveModule(FXBuilder builder) {
7475
closedLoopSlot = builder.closedLoopSlot;
7576
azimuthSlot = builder.azimuthSlot;
7677
compensateLatency = builder.compensateLatency;
78+
encoderOpposed = builder.encoderOpposed;
7779
resetDriveEncoder();
7880
}
7981

@@ -164,7 +166,8 @@ public void loadAndSetAzimuthZeroReference() {
164166

165167
double azimuthAbsoluteCounts = getAzimuthAbsoluteEncoderCounts();
166168
logger.info("swerve module {}: azimuth absolute position = {}", index, azimuthAbsoluteCounts);
167-
double azimuthSetpoint = azimuthAbsoluteCounts - reference;
169+
double azimuthSetpoint =
170+
encoderOpposed ? reference - azimuthAbsoluteCounts : azimuthAbsoluteCounts - reference;
168171
azimuthTalon.setPosition(azimuthSetpoint);
169172
logger.info("swerve module {}: set azimuth encoder = {}", index, azimuthSetpoint);
170173

@@ -184,13 +187,13 @@ private double getAzimuthAbsoluteEncoderCounts() {
184187
azimuthTalon.getRawPulseWidthPosition().getValueAsDouble(), 0.0, 1.0);
185188
}
186189

187-
private double getAzimuthEcoderPosition() {
190+
private double getAzimuthEncoderPosition() {
188191
return azimuthPosition.getValueAsDouble();
189192
}
190193

191194
@Override
192195
public Rotation2d getAzimuthRotation2d() {
193-
double azimuthCounts = getAzimuthEcoderPosition();
196+
double azimuthCounts = getAzimuthEncoderPosition();
194197
double radians = 2.0 * Math.PI * azimuthCounts / azimuthCountsPerRev;
195198
return new Rotation2d(radians);
196199
}
@@ -208,7 +211,7 @@ private SwerveModuleState setAzimuthOptimizedState(SwerveModuleState desiredStat
208211
desiredState.optimize(currentAngle);
209212

210213
// setPosition
211-
double countsBefore = getAzimuthEcoderPosition();
214+
double countsBefore = getAzimuthEncoderPosition();
212215
double countsFromAngle =
213216
desiredState.angle.getRadians() / (2.0 * Math.PI) * azimuthCountsPerRev;
214217
double countsDelta = Math.IEEEremainder(countsFromAngle - countsBefore, azimuthCountsPerRev);
@@ -224,9 +227,9 @@ private void setAzimuthPosition(double position) {
224227
new MotionMagicDutyCycle(position).withEnableFOC(false).withSlot(azimuthSlot);
225228
azimuthTalon.setControl(controlRequest);
226229
} else if (units == V6TalonSwerveModule.ClosedLoopUnits.VOLTAGE) {
227-
MotionMagicVoltage controlReqest =
230+
MotionMagicVoltage controlRequest =
228231
new MotionMagicVoltage(position).withEnableFOC(false).withSlot(azimuthSlot);
229-
azimuthTalon.setControl(controlReqest);
232+
azimuthTalon.setControl(controlRequest);
230233
} else {
231234
MotionMagicTorqueCurrentFOC controlRequest =
232235
new MotionMagicTorqueCurrentFOC(position).withSlot(azimuthSlot);
@@ -317,6 +320,7 @@ public static class FXBuilder {
317320
private int closedLoopSlot = 0;
318321
private int azimuthSlot = 0;
319322
private boolean compensateLatency = false;
323+
private boolean encoderOpposed = true;
320324

321325
public FXBuilder azimuthTalon(TalonFXS azimuthTalon) {
322326
this.azimuthTalon = azimuthTalon;
@@ -388,6 +392,11 @@ public FXBuilder latencyCompensation(boolean compensate) {
388392
return this;
389393
}
390394

395+
public FXBuilder encoderOpposed(boolean encoderOpposed) {
396+
this.encoderOpposed = encoderOpposed;
397+
return this;
398+
}
399+
391400
public FXSwerveModule build() {
392401
if (driveDeadbandMetersPerSecond < 0) {
393402
driveDeadbandMetersPerSecond = driveMaximumVelocityMetersPerSecond * 0.01;

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

Lines changed: 26 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -96,7 +96,8 @@ void FXShouldSetEncoderCountsPerREv() {
9696
.driveGearRatio(kDriveGearRatio)
9797
.wheelDiameterInches(kWheelDiameterInches)
9898
.driveMaximumMetersPerSecond(kMaxSpeedMetersPerSecond)
99-
.wheelLocationMeters(new Translation2d());
99+
.wheelLocationMeters(new Translation2d())
100+
.encoderOpposed(false);
100101
assertThat(builder.build().getDriveCountsPerRev()).isEqualTo(1);
101102
assertThat(builder.driveEncoderCountsPerRevolution(2).build().getDriveCountsPerRev())
102103
.isEqualTo(2);
@@ -134,6 +135,7 @@ void FXResetDriveEncoder() {
134135
.wheelDiameterInches(kWheelDiameterInches)
135136
.driveMaximumMetersPerSecond(kMaxSpeedMetersPerSecond)
136137
.wheelLocationMeters(new Translation2d())
138+
.encoderOpposed(false)
137139
.build();
138140
module.resetDriveEncoder();
139141
verify(driveTalon, times(2)).setPosition(0);
@@ -249,6 +251,7 @@ void setUp() {
249251
.wheelDiameterInches(kWheelDiameterInches)
250252
.wheelLocationMeters(new Translation2d(1, 1))
251253
.driveMaximumMetersPerSecond(kMaxSpeedMetersPerSecond)
254+
.encoderOpposed(false)
252255
.build();
253256
azimuthPositionStatusSig = (StatusSignal<Angle>) mock(StatusSignal.class);
254257
rawPulseWidthStatusSig = (StatusSignal<Angle>) mock(StatusSignal.class);
@@ -277,7 +280,8 @@ void storeFXAzimuthZeroReference() {
277280
.driveTalon(driveTalon)
278281
.driveGearRatio(kDriveGearRatio)
279282
.wheelDiameterInches(kWheelDiameterInches)
280-
.driveMaximumMetersPerSecond(kMaxSpeedMetersPerSecond);
283+
.driveMaximumMetersPerSecond(kMaxSpeedMetersPerSecond)
284+
.encoderOpposed(false);
281285

282286
double expectedZeroReference = 27.0 / 4096.0;
283287
int index = 0; // Front Left
@@ -446,7 +450,8 @@ void whenFXTalonIsNull() {
446450
.driveGearRatio(kDriveGearRatio)
447451
.wheelDiameterInches(kWheelDiameterInches)
448452
.wheelLocationMeters(new Translation2d(1, 1))
449-
.driveDeadbandMetersPerSecond(kMaxSpeedMetersPerSecond);
453+
.driveDeadbandMetersPerSecond(kMaxSpeedMetersPerSecond)
454+
.encoderOpposed(false);
450455

451456
assertThrows(IllegalArgumentException.class, builder.azimuthTalon(null)::build);
452457
}
@@ -460,7 +465,8 @@ void whenFXDriveGearRatioLteZero() {
460465
.driveTalon(driveTalon)
461466
.wheelDiameterInches(kWheelDiameterInches)
462467
.wheelLocationMeters(new Translation2d())
463-
.driveMaximumMetersPerSecond(kMaxSpeedMetersPerSecond);
468+
.driveMaximumMetersPerSecond(kMaxSpeedMetersPerSecond)
469+
.encoderOpposed(false);
464470
assertThrows(IllegalArgumentException.class, builder::build);
465471
}
466472

@@ -473,7 +479,8 @@ void whenFXWheelDiameterLteZero() {
473479
.driveTalon(driveTalon)
474480
.driveGearRatio(kDriveGearRatio)
475481
.wheelLocationMeters(new Translation2d())
476-
.driveMaximumMetersPerSecond(kMaxSpeedMetersPerSecond);
482+
.driveMaximumMetersPerSecond(kMaxSpeedMetersPerSecond)
483+
.encoderOpposed(false);
477484
assertThrows(IllegalArgumentException.class, builder::build);
478485
}
479486

@@ -486,7 +493,8 @@ void whenFXDriveMaximumMetersPerSecondLteZero() {
486493
.driveTalon(driveTalon)
487494
.driveGearRatio(kDriveGearRatio)
488495
.wheelLocationMeters(new Translation2d())
489-
.wheelDiameterInches(kWheelDiameterInches);
496+
.wheelDiameterInches(kWheelDiameterInches)
497+
.encoderOpposed(false);
490498
assertThrows(IllegalArgumentException.class, builder::build);
491499
}
492500

@@ -499,7 +507,8 @@ void whenFXWheelLocationNotSet() {
499507
.driveTalon(driveTalon)
500508
.driveGearRatio(kDriveGearRatio)
501509
.wheelDiameterInches(kWheelDiameterInches)
502-
.driveMaximumMetersPerSecond(kMaxSpeedMetersPerSecond);
510+
.driveMaximumMetersPerSecond(kMaxSpeedMetersPerSecond)
511+
.encoderOpposed(false);
503512
assertThrows(IllegalArgumentException.class, builder::build);
504513
}
505514
}
@@ -722,6 +731,7 @@ void FXSpeedWithDefaultEncoderCounts() {
722731
.wheelDiameterInches(kWheelDiameterInches)
723732
.driveMaximumMetersPerSecond(kMaxSpeedMetersPerSecond)
724733
.wheelLocationMeters(new Translation2d())
734+
.encoderOpposed(false)
725735
.build();
726736
// when(driveTalon.getVelocity()).thenReturn(velocityStatusSig);
727737
Field driveVelField =
@@ -768,6 +778,7 @@ void FXSpeedWithEncoderCounts(
768778
.wheelDiameterInches(kWheelDiameterInches)
769779
.driveMaximumMetersPerSecond(kMaxSpeedMetersPerSecond)
770780
.wheelLocationMeters(new Translation2d())
781+
.encoderOpposed(false)
771782
.build();
772783
Field driveVelField =
773784
ReflectionUtils.findFields(
@@ -855,6 +866,7 @@ void FXAngleWithEncoderCounts(
855866
.wheelDiameterInches(kWheelDiameterInches)
856867
.driveMaximumMetersPerSecond(kMaxSpeedMetersPerSecond)
857868
.wheelLocationMeters(new Translation2d())
869+
.encoderOpposed(false)
858870
.build();
859871
Field driveVelField =
860872
ReflectionUtils.findFields(
@@ -899,6 +911,7 @@ void FXGetMaxSpeedMetersPerSecond() {
899911
.wheelDiameterInches(kWheelDiameterInches)
900912
.driveMaximumMetersPerSecond(kMaxSpeedMetersPerSecond)
901913
.wheelLocationMeters(new Translation2d())
914+
.encoderOpposed(false)
902915
.build();
903916
assertEquals(kMaxSpeedMetersPerSecond, module.getMaxSpeedMetersPerSecond());
904917
}
@@ -914,6 +927,7 @@ void FXGetWheelLocationMeters() {
914927
.wheelDiameterInches(kWheelDiameterInches)
915928
.driveMaximumMetersPerSecond(kMaxSpeedMetersPerSecond)
916929
.wheelLocationMeters(expectedWheelLocation)
930+
.encoderOpposed(false)
917931
.build();
918932
assertEquals(expectedWheelLocation, module.getWheelLocationMeters());
919933
}
@@ -1143,6 +1157,7 @@ void driveOpenLoopSpeed(
11431157
.wheelDiameterInches(kWheelDiameterInches)
11441158
.wheelLocationMeters(new Translation2d())
11451159
.driveMaximumMetersPerSecond(kMaxSpeedMetersPerSecond)
1160+
.encoderOpposed(false)
11461161
.build();
11471162
Field azimuthPosField =
11481163
ReflectionUtils.findFields(
@@ -1187,6 +1202,7 @@ void FXDriveClosedLoopSpeed(
11871202
.wheelDiameterInches(kWheelDiameterInches)
11881203
.wheelLocationMeters(new Translation2d())
11891204
.driveMaximumMetersPerSecond(kMaxSpeedMetersPerSecond)
1205+
.encoderOpposed(false)
11901206
.build();
11911207
Field azimuthPosField =
11921208
ReflectionUtils.findFields(
@@ -1285,6 +1301,7 @@ void FXAzimuthAngleWhenOpenLoop(
12851301
.wheelDiameterInches(kWheelDiameterInches)
12861302
.wheelLocationMeters(new Translation2d())
12871303
.driveMaximumMetersPerSecond(kMaxSpeedMetersPerSecond)
1304+
.encoderOpposed(false)
12881305
.build();
12891306
var desiredState =
12901307
new SwerveModuleState(speedMetersPerSecond, Rotation2d.fromDegrees(angleDegrees));
@@ -1478,6 +1495,7 @@ void FXPositionWithDefaultEncoderCounts() {
14781495
.wheelDiameterInches(kWheelDiameterInches)
14791496
.driveMaximumMetersPerSecond(kMaxSpeedMetersPerSecond)
14801497
.wheelLocationMeters(new Translation2d())
1498+
.encoderOpposed(false)
14811499
.build();
14821500
Field drivePosField =
14831501
ReflectionUtils.findFields(
@@ -1544,6 +1562,7 @@ void FXPositionWithEncoderCounts(
15441562
.wheelDiameterInches(kWheelDiameterInches)
15451563
.driveMaximumMetersPerSecond(kMaxSpeedMetersPerSecond)
15461564
.wheelLocationMeters(new Translation2d())
1565+
.encoderOpposed(false)
15471566
.build();
15481567
Field drivePosField =
15491568
ReflectionUtils.findFields(

0 commit comments

Comments
 (0)