From f27f2cf0f67321764f761d1a2a90f66677d71c99 Mon Sep 17 00:00:00 2001
From: Hayden Heroux <haydenheroux@gmail.com>
Date: Mon, 1 Apr 2024 20:05:51 -0400
Subject: [PATCH] feat(shooter): Add serializer motor TalonFX.

---
 .../shooter/SerializerMotorIOSparkMax.java    | 36 -------------
 .../shooter/SerializerMotorIOTalonFX.java     | 53 +++++++++++++++++++
 .../frc/robot/shooter/ShooterFactory.java     |  2 +-
 3 files changed, 54 insertions(+), 37 deletions(-)
 delete mode 100644 src/main/java/frc/robot/shooter/SerializerMotorIOSparkMax.java
 create mode 100644 src/main/java/frc/robot/shooter/SerializerMotorIOTalonFX.java

diff --git a/src/main/java/frc/robot/shooter/SerializerMotorIOSparkMax.java b/src/main/java/frc/robot/shooter/SerializerMotorIOSparkMax.java
deleted file mode 100644
index 6267691..0000000
--- a/src/main/java/frc/robot/shooter/SerializerMotorIOSparkMax.java
+++ /dev/null
@@ -1,36 +0,0 @@
-package frc.robot.shooter;
-
-import com.revrobotics.CANSparkLowLevel.MotorType;
-import com.revrobotics.CANSparkMax;
-import frc.lib.Configurator;
-
-/** Serializer motor using Spark Max. */
-public class SerializerMotorIOSparkMax implements SerializerMotorIO {
-
-  /** Hardware Spark Max for leading. */
-  private final CANSparkMax sparkMax;
-
-  public SerializerMotorIOSparkMax() {
-    sparkMax = new CANSparkMax(4, MotorType.kBrushless);
-  }
-
-  @Override
-  public void configure() {
-    Configurator.configureREV(sparkMax::restoreFactoryDefaults);
-
-    Configurator.configureREV(() -> sparkMax.setSmartCurrentLimit(40));
-
-    Configurator.configureStatusFrames(sparkMax);
-  }
-
-  @Override
-  public void update(SerializerMotorIOValues values) {
-    values.velocityRotationsPerSecond = sparkMax.getEncoder().getVelocity();
-    values.currentAmps = sparkMax.getOutputCurrent();
-  }
-
-  @Override
-  public void setSetpoint(double velocityRotationsPerSecond) {
-    // TODO Implement velocity setpoint
-  }
-}
diff --git a/src/main/java/frc/robot/shooter/SerializerMotorIOTalonFX.java b/src/main/java/frc/robot/shooter/SerializerMotorIOTalonFX.java
new file mode 100644
index 0000000..361065e
--- /dev/null
+++ b/src/main/java/frc/robot/shooter/SerializerMotorIOTalonFX.java
@@ -0,0 +1,53 @@
+package frc.robot.shooter;
+
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import frc.lib.Configurator;
+
+/** Serializer motor using TalonFX. */
+public class SerializerMotorIOTalonFX implements SerializerMotorIO {
+
+  private final TalonFX talonFX;
+
+  private final StatusSignal<Double> velocityRotationsPerSecond, statorCurrentAmps;
+
+  private final SimpleMotorFeedforward velocityFeedforward;
+
+  public SerializerMotorIOTalonFX() {
+    talonFX = new TalonFX(20);
+
+    velocityRotationsPerSecond = talonFX.getVelocity();
+    statorCurrentAmps = talonFX.getStatorCurrent();
+
+    velocityFeedforward = new SimpleMotorFeedforward(0, 0);
+  }
+
+  @Override
+  public void configure() {
+    final TalonFXConfiguration config = new TalonFXConfiguration();
+
+    config.MotorOutput.NeutralMode = NeutralModeValue.Coast;
+
+    Configurator.configureTalonFX(talonFX.getConfigurator(), config);
+  }
+
+  @Override
+  public void update(SerializerMotorIOValues values) {
+    velocityRotationsPerSecond.refresh();
+    statorCurrentAmps.refresh();
+
+    values.velocityRotationsPerSecond = velocityRotationsPerSecond.getValue();
+    values.currentAmps = statorCurrentAmps.getValue();
+  }
+
+  @Override
+  public void setSetpoint(double velocityRotationsPerSecond) {
+    double volts = velocityFeedforward.calculate(velocityRotationsPerSecond);
+
+    talonFX.setVoltage(volts);
+  }
+}
diff --git a/src/main/java/frc/robot/shooter/ShooterFactory.java b/src/main/java/frc/robot/shooter/ShooterFactory.java
index a04dd55..95a43bd 100644
--- a/src/main/java/frc/robot/shooter/ShooterFactory.java
+++ b/src/main/java/frc/robot/shooter/ShooterFactory.java
@@ -14,7 +14,7 @@ public class ShooterFactory {
    */
   public static SerializerMotorIO createSerializerMotor() {
     if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.SHOOTER))
-      return new SerializerMotorIOSparkMax();
+      return new SerializerMotorIOTalonFX();
 
     return new SerializerMotorIOSim();
   }