make swerve work with talonFX

Author: mwitcpalek

build.gradle

@@ -8,7 +8,7 @@ plugins {
 }
 
 group = 'org.strykeforce'
-version = '20.1.1'
+version = '20.2.0'
 
 sourceCompatibility = JavaVersion.VERSION_11
 targetCompatibility = JavaVersion.VERSION_11

src/main/java/org/strykeforce/thirdcoast/swerve/Wheel.java

@@ -4,6 +4,7 @@
 import static org.strykeforce.thirdcoast.swerve.SwerveDrive.DriveMode.TELEOP;
 
 import com.ctre.phoenix.ErrorCode;
+import com.ctre.phoenix.motorcontrol.can.BaseTalon;
 import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 import java.util.Objects;
 import java.util.function.DoubleConsumer;
@@ -32,7 +33,7 @@ public class Wheel {
 
   private static final Logger logger = LoggerFactory.getLogger(Wheel.class);
   private final double driveSetpointMax;
-  private final TalonSRX driveTalon;
+  private final BaseTalon driveTalon;
   private final TalonSRX azimuthTalon;
   protected DoubleConsumer driver;
   private boolean isInverted = false;
@@ -49,7 +50,7 @@ public class Wheel {
    * @param drive the configured drive TalonSRX
    * @param driveSetpointMax scales closed-loop drive output to this value when drive setpoint = 1.0
    */
-  public Wheel(TalonSRX azimuth, TalonSRX drive, double driveSetpointMax) {
+  public Wheel(TalonSRX azimuth, BaseTalon drive, double driveSetpointMax) {
     this.driveSetpointMax = driveSetpointMax;
     azimuthTalon = Objects.requireNonNull(azimuth);
     driveTalon = Objects.requireNonNull(drive);
@@ -184,7 +185,7 @@ public TalonSRX getAzimuthTalon() {
    *
    * @return drive Talon instance used by wheel
    */
-  public TalonSRX getDriveTalon() {
+  public BaseTalon getDriveTalon() {
     return driveTalon;
   }
 

src/main/kotlin/org/strykeforce/thirdcoast/telemetry/TelemetryService.kt

@@ -119,7 +119,9 @@ class TelemetryService(private val telemetryControllerFactory: Function<Inventor
    */
   fun register(swerveDrive: SwerveDrive) = swerveDrive.wheels.forEach {
     register(TalonSRXItem(it.azimuthTalon))
-    register(TalonSRXItem(it.driveTalon))
+    if(it.driveTalon is TalonSRX) register(TalonSRXItem(it.driveTalon as TalonSRX))
+    else if(it.driveTalon is TalonFX) register(TalonFXItem(it.driveTalon as TalonFX))
+    else throw IllegalArgumentException()
   }
 
   /**