From 7c4d49b181423d1d862267c87e233018cf9a9311 Mon Sep 17 00:00:00 2001
From: mwitcpalek <35316453+mwitcpalek@users.noreply.github.com>
Date: Sat, 25 Jan 2020 13:16:25 -0500
Subject: [PATCH] make swerve work with talonFX

---
 build.gradle                                               | 2 +-
 src/main/java/org/strykeforce/thirdcoast/swerve/Wheel.java | 7 ++++---
 .../strykeforce/thirdcoast/telemetry/TelemetryService.kt   | 4 +++-
 3 files changed, 8 insertions(+), 5 deletions(-)

diff --git a/build.gradle b/build.gradle
index 97636954..a40198b1 100644
--- a/build.gradle
+++ b/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
diff --git a/src/main/java/org/strykeforce/thirdcoast/swerve/Wheel.java b/src/main/java/org/strykeforce/thirdcoast/swerve/Wheel.java
index 89c049ed..c62e3cd5 100644
--- a/src/main/java/org/strykeforce/thirdcoast/swerve/Wheel.java
+++ b/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;
   }
 
diff --git a/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/TelemetryService.kt b/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/TelemetryService.kt
index b1abd673..330f6780 100644
--- a/src/main/kotlin/org/strykeforce/thirdcoast/telemetry/TelemetryService.kt
+++ b/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()
   }
 
   /**