add TalonFXS

Author: mwitcpalek

build.gradle

@@ -11,7 +11,7 @@ plugins {
 }
 
 group = "org.strykeforce"
-version = "25.0.1-SNAPSHOT"
+version = "25.0.1"
 
 java {
     targetCompatibility = JavaVersion.VERSION_17

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

@@ -41,6 +41,7 @@ public class FXSwerveModule implements SwerveModule {
   private final TalonFX driveTalon;
   private final double azimuthCountsPerRev;
   private final double driveCountsPerRev;
+  private final double azimuthPulseWidthCPR;
   private final double driveGearRatio;
   private final double wheelCircumferenceMeters;
   private final double driveDeadbandMetersPerSecond;
@@ -52,7 +53,7 @@ public class FXSwerveModule implements SwerveModule {
   private int closedLoopSlot;
   private int azimuthSlot;
   private boolean compensateLatency;
-  private double azimuthPositionOffset;
+  private double azimuthPositionOffset = 0.0;
 
   // Status Signals
   private StatusSignal<Angle> drivePosition;
@@ -64,6 +65,7 @@ private FXSwerveModule(FXBuilder builder) {
     driveTalon = builder.driveTalon;
     azimuthCountsPerRev = builder.azimuthCountsPerRev;
     driveCountsPerRev = builder.driveCountsPerRev;
+    azimuthPulseWidthCPR = builder.azimuthPulseWidthCPR;
     driveGearRatio = builder.driveGearRatio;
     wheelCircumferenceMeters = Math.PI * Inches.of(builder.wheelDiameterInches).in(Meters);
     driveDeadbandMetersPerSecond = builder.driveDeadbandMetersPerSecond;
@@ -169,7 +171,7 @@ public void loadAndSetAzimuthZeroReference() {
         index,
         azimuthAbsoluteCounts - reference);
 
-    azimuthPositionOffset = reference;
+    azimuthPositionOffset = reference / azimuthPulseWidthCPR;
     setAzimuthPosition(azimuthAbsoluteCounts);
   }
 
@@ -182,17 +184,23 @@ public TalonFX getDriveTalon() {
   }
 
   private double getAzimuthAbsoluteEncoderCounts() {
-    double extraRotations =
-        azimuthPosition.getValueAsDouble() > 0
-            ? Math.floor(azimuthPosition.getValueAsDouble())
-            : Math.ceil(azimuthPosition.getValueAsDouble());
-    return azimuthPosition.getValueAsDouble() - extraRotations; // correct to within one rotation
+    //    double extraRotations =
+    //        azimuthPosition.getValueAsDouble() > 0
+    //            ? Math.floor(azimuthPosition.getValueAsDouble())
+    //            : Math.ceil(azimuthPosition.getValueAsDouble());
+    //    return azimuthPosition.getValueAsDouble() - extraRotations; // correct to within one
+    // rotation
+    return ((int) (azimuthPosition.getValueAsDouble() * azimuthPulseWidthCPR)) & 0xFFF;
   }
 
   private double getAzimuthEcoderPosition() {
     return azimuthPosition.getValueAsDouble() - azimuthPositionOffset;
   }
 
+  public double getAzimuthPositionOffset() {
+    return azimuthPositionOffset;
+  }
+
   @Override
   public Rotation2d getAzimuthRotation2d() {
     double azimuthCounts = getAzimuthEcoderPosition();
@@ -312,6 +320,7 @@ public static class FXBuilder {
     public static final double kDefaultTalonFXCountsPerRev = 1.0;
     private double azimuthCountsPerRev = kDefaultTalonFXCountsPerRev;
     private double driveCountsPerRev = kDefaultTalonFXCountsPerRev;
+    private double azimuthPulseWidthCPR = 4096.0;
 
     private TalonFXS azimuthTalon;
     private TalonFX driveTalon;
@@ -357,6 +366,11 @@ public FXBuilder azimuthEncoderCountsPerRevolution(double countsPerRev) {
       return this;
     }
 
+    public FXBuilder azimuthPulseWidthCPR(double azimuthPulseWidthCPR) {
+      this.azimuthPulseWidthCPR = azimuthPulseWidthCPR;
+      return this;
+    }
+
     public FXBuilder driveDeadbandMetersPerSecond(double driveDeadbandMetersPerSecond) {
       this.driveDeadbandMetersPerSecond = driveDeadbandMetersPerSecond;
       return this;
@@ -416,7 +430,13 @@ private void validateSwerveModuleObject(FXSwerveModule module) {
       if (module.azimuthCountsPerRev <= 0)
         throw new IllegalArgumentException(
             "azimuth encoder counts per revolution must be greater than zero.");
-
+      if (module.azimuthPulseWidthCPR <= 0)
+        throw new IllegalArgumentException("azimuth pulse width cpr must be greater than zero");
+      //      TalonFXSConfiguration azimuthConfig = new TalonFXSConfiguration();
+      //      module.azimuthTalon.getConfigurator().refresh(azimuthConfig);
+      //      if (azimuthConfig.ExternalFeedback.ExternalFeedbackSensorSource
+      //          != ExternalFeedbackSensorSourceValue.PulseWidth)
+      //        throw new IllegalArgumentException("Azimuth must use Pulse Width Feedback");
       if (module.driveCountsPerRev <= 0)
         throw new IllegalArgumentException(
             "drive encoder counts per revolution must be greater than zero.");

src/main/kotlin/org/strykeforce/telemetry/measurable/TalonFXSMeasureable.kt

@@ -118,6 +118,7 @@ class TalonFXSMeasureable @JvmOverloads constructor(
         Measure(DIFF_CLOSED_LOOP_REF, "Differential Closed Loop Reference"){talonFXS.differentialClosedLoopReference.valueAsDouble},
         Measure(DIFF_CLOSED_LOOP_REF_SLOPE, "Differential Closed Loop Reference Slope"){talonFXS.differentialClosedLoopReferenceSlope.valueAsDouble},
         Measure(DIFF_CLOSED_LOOP_SLOT, "Differential Closed Loop Slot"){talonFXS.differentialClosedLoopSlot.valueAsDouble},
+        Measure(PULSE_WIDTH_POSITION, "Pulse Width Position") {((talonFXS.position.valueAsDouble * 4096.0).toInt() and 0xFFF).toDouble()}
     )
 
     override fun equals(other: Any?): Boolean {

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

@@ -123,10 +123,11 @@ void v6ResetDriveEncoder() {
   @DisplayName("FX Should reset drive encoder")
   void FXResetDriveEncoder() {
     TalonFX driveTalon = mock(com.ctre.phoenix6.hardware.TalonFX.class);
+    TalonFXS azimuthTalon = mock(TalonFXS.class);
     when(driveTalon.setPosition(0)).thenReturn(StatusCode.OK);
     FXSwerveModule module =
         new FXSwerveModule.FXBuilder()
-            .azimuthTalon(mock(TalonFXS.class))
+            .azimuthTalon(azimuthTalon)
             .driveTalon(driveTalon)
             .driveGearRatio(kDriveGearRatio)
             .wheelDiameterInches(kWheelDiameterInches)
@@ -283,38 +284,35 @@ void storeFXAzimuthZeroReference() {
       when(azimuthTalon.getPosition()).thenReturn(azimuthPositionStatusSig);
       when(azimuthPositionStatusSig.getValueAsDouble()).thenReturn(expectedZeroReference);
       module.storeAzimuthZeroReference();
-      assertEquals(expectedZeroReference, Preferences.getDouble(key, -1));
+      assertEquals((int) (expectedZeroReference * 4096.0) & 0xFFF, Preferences.getDouble(key, -1));
 
       expectedZeroReference = 67.0 / 4096.0;
       index = 1; // Front Right
       key = String.format("SwerveDrive/wheel.%d", index);
       module = builder.wheelLocationMeters(new Translation2d(1, -1)).build();
       when(azimuthPositionStatusSig.getValueAsDouble()).thenReturn(expectedZeroReference);
       module.storeAzimuthZeroReference();
-      assertEquals(expectedZeroReference, Preferences.getDouble(key, -1));
+      assertEquals((int) (expectedZeroReference * 4096.0) & 0xFFF, Preferences.getDouble(key, -1));
 
       expectedZeroReference = -6767.0 / 4096.0;
       index = 2; // Back Left
       key = String.format("SwerveDrive/wheel.%d", index);
       module = builder.wheelLocationMeters(new Translation2d(-1, 1)).build();
       when(azimuthPositionStatusSig.getValueAsDouble()).thenReturn(expectedZeroReference);
       module.storeAzimuthZeroReference();
-      assertEquals(
-          expectedZeroReference - Math.ceil(expectedZeroReference), Preferences.getDouble(key, -1));
+      assertEquals((int) (expectedZeroReference * 4096.0) & 0xFFF, Preferences.getDouble(key, -1));
 
       expectedZeroReference = 6727.0 / 4096.0;
       index = 3; // Back Right
       key = String.format("SwerveDrive/wheel.%d", index);
       module = builder.wheelLocationMeters(new Translation2d(-1, -1)).build();
       when(azimuthPositionStatusSig.getValueAsDouble()).thenReturn(expectedZeroReference);
       module.storeAzimuthZeroReference();
-      assertEquals(
-          expectedZeroReference - Math.floor(expectedZeroReference),
-          Preferences.getDouble(key, -1));
+      assertEquals((int) (expectedZeroReference * 4096.0) & 0xFFF, Preferences.getDouble(key, -1));
     }
 
     @ParameterizedTest
-    @CsvSource({"0, 0, 0", "1, 0, 0", "1.00024, 0, 0.00024", "0, 0.67554, 0.67554"})
+    @CsvSource({"0, 0, 0", "1, 0, 0", "1.00024, 0, 0.00024", "0, 2767, 0.67554"})
     @DisplayName("should set FX azimuth zero")
     void shouldSetFXAzimuthZero(
         double absoluteEncoderPosition, double zeroReference, double setpoint) {