diff --git a/src/main/java/swervelib/encoders/TalonSRXEncoderSwerve.java b/src/main/java/swervelib/encoders/TalonSRXEncoderSwerve.java new file mode 100644 index 00000000..6303a362 --- /dev/null +++ b/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; + } + +} diff --git a/src/main/java/swervelib/motors/TalonSRXSwerve.java b/src/main/java/swervelib/motors/TalonSRXSwerve.java index 28d2e356..5ba0497f 100644 --- a/src/main/java/swervelib/motors/TalonSRXSwerve.java +++ b/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. * diff --git a/src/main/java/swervelib/parser/json/DeviceJson.java b/src/main/java/swervelib/parser/json/DeviceJson.java index ee03ccd1..341ba129 100644 --- a/src/main/java/swervelib/parser/json/DeviceJson.java +++ b/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."); }