Merge pull request #208 from WispySparks/srx-abs

Add support to Talon SRX for an integrated absolute encoder

src/main/java/swervelib/encoders/TalonSRXEncoderSwerve.java

@@ -0,0 +1,79 @@
+package swervelib.encoders;
+
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+
+import swervelib.motors.SwerveMotor;
+import swervelib.motors.TalonSRXSwerve;
+
+/**
+ * Talon SRX attached absolute encoder.
+ */
+public class TalonSRXEncoderSwerve extends SwerveAbsoluteEncoder {
+
+    /**
+     * Multiplying by this converts native Talon SRX units into degrees.
+     */
+    private final double degreesPerSensorUnit;
+    /**
+     * Reference to a Talon SRX for polling its attached absolute encoder.
+     */
+    private final WPI_TalonSRX talon;
+
+    /**
+     * Creates a {@link TalonSRXEncoderSwerve}.
+     * @param motor motor to poll the sensor from.
+     * @param feedbackDevice the feedback device the sensor uses e.g. PWM or Analog.
+     */
+    public TalonSRXEncoderSwerve(SwerveMotor motor, FeedbackDevice feedbackDevice) {
+        if (motor instanceof TalonSRXSwerve talonSRXSwerve) {
+            talonSRXSwerve.setSelectedFeedbackDevice(feedbackDevice);
+            this.talon = (WPI_TalonSRX) talonSRXSwerve.getMotor();
+            // https://v5.docs.ctr-electronics.com/en/stable/ch14_MCSensor.html#sensor-resolution
+            degreesPerSensorUnit = switch (feedbackDevice) {
+                case Analog -> 360.0 / 1024.0;
+                default -> 360.0 / 4096.0;
+            };
+        } 
+        else {
+            throw new RuntimeException("Motor given to instantiate TalonSRXEncoder is not a WPI_TalonSRX");
+        }
+    }
+
+    @Override
+    public void factoryDefault() {
+        // Handled in TalonSRXSwerve
+    }
+
+    @Override
+    public void clearStickyFaults() {
+        // Handled in TalonSRXSwerve
+    }
+
+    @Override
+    public void configure(boolean inverted) {
+        talon.setSensorPhase(inverted);
+    }
+
+    @Override
+    public double getAbsolutePosition() {
+        return (talon.getSelectedSensorPosition() * degreesPerSensorUnit) % 360;
+    }
+
+    @Override
+    public Object getAbsoluteEncoder() {
+        return talon;
+    }
+
+    @Override
+    public boolean setAbsoluteEncoderOffset(double offset) {
+        talon.setSelectedSensorPosition(talon.getSelectedSensorPosition() + offset / degreesPerSensorUnit);
+        return true;
+    }
+
+    @Override
+    public double getVelocity() {
+        return talon.getSelectedSensorVelocity() * 10 * degreesPerSensorUnit;
+    }
+
+}

src/main/java/swervelib/motors/TalonSRXSwerve.java

@@ -359,11 +359,6 @@ public double getPosition()
     } else
     {
       var pos = motor.getSelectedSensorPosition() * positionConversionFactor;
-      pos %= 360;
-      if (pos < 360)
-      {
-        pos += 360;
-      }
       return pos;
     }
   }
@@ -422,6 +417,17 @@ public void setLoopRampRate(double rampRate)
     configChanged = true;
   }
 
+  /**
+   * Set the selected feedback device for the TalonSRX.
+   * 
+   * @param feedbackDevice Feedback device to select.
+   */
+  public void setSelectedFeedbackDevice(FeedbackDevice feedbackDevice) 
+  {
+    configuration.primaryPID.selectedFeedbackSensor = feedbackDevice;
+    configChanged = true;
+  }
+
   /**
    * Get the motor object from the module.
    *

src/main/java/swervelib/parser/json/DeviceJson.java

@@ -4,7 +4,9 @@
 import static swervelib.telemetry.SwerveDriveTelemetry.i2cLockupWarning;
 import static swervelib.telemetry.SwerveDriveTelemetry.serialCommsIssueWarning;
 
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.studica.frc.AHRS.NavXComType;
+
 import edu.wpi.first.wpilibj.DriverStation;
 import swervelib.encoders.AnalogAbsoluteEncoderSwerve;
 import swervelib.encoders.CANCoderSwerve;
@@ -13,6 +15,7 @@
 import swervelib.encoders.SparkMaxAnalogEncoderSwerve;
 import swervelib.encoders.SparkMaxEncoderSwerve;
 import swervelib.encoders.SwerveAbsoluteEncoder;
+import swervelib.encoders.TalonSRXEncoderSwerve;
 import swervelib.imu.ADIS16448Swerve;
 import swervelib.imu.ADIS16470Swerve;
 import swervelib.imu.ADXRS450Swerve;
@@ -90,6 +93,10 @@ public SwerveAbsoluteEncoder createEncoder(SwerveMotor motor)
         return new AnalogAbsoluteEncoderSwerve(id);
       case "cancoder":
         return new CANCoderSwerve(id, canbus != null ? canbus : "");
+      case "talonsrx_pwm":
+        return new TalonSRXEncoderSwerve(motor, FeedbackDevice.PulseWidthEncodedPosition);
+      case "talonsrx_analog": 
+        return new TalonSRXEncoderSwerve(motor, FeedbackDevice.Analog);
       default:
         throw new RuntimeException(type + " is not a recognized absolute encoder type.");
     }