Position and velocity ig

there's also regen braking for velocity which I havent refactored

src/main/java/frc/robot/RobotContainer.java

@@ -52,7 +52,7 @@
 import frc.robot.subsystems.vision.VisionIOLimelight;
 import frc.robot.utils.MappedXboxController;
 import frc.robot.utils.NamedCommands;
-import frc.robot.utils.generics.SingleMotorSubsystemIOTalonFX;
+import frc.robot.utils.generics.VelocityIOTalonFX;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -95,7 +95,7 @@ public class RobotContainer {
   private final Intake intake =
       new Intake(
           Constants.FeatureFlags.kIntakeEnabled,
-          new SingleMotorSubsystemIOTalonFX<IntakeConstants>(),
+          new VelocityIOTalonFX<IntakeConstants>(),
           new BeamBreakIOBanner(IntakeConstants.kIntakeBeamBreakDIO));
   private final Spindex spindex =
       new Spindex(

src/main/java/frc/robot/subsystems/intake/Intake.java

@@ -17,13 +17,13 @@
 import frc.robot.subsystems.BeamBreakIO;
 import frc.robot.subsystems.BeamBreakIOInputsAutoLogged;
 import frc.robot.utils.DisableSubsystem;
-import frc.robot.utils.generics.SingleMotorSubsystemIO;
 import frc.robot.utils.generics.SingleMotorSubsystemInputsAutoLogged;
+import frc.robot.utils.generics.VelocityIO;
 import org.littletonrobotics.junction.Logger;
 
 public class Intake
     extends DisableSubsystem { // note for me later - this has also controls the redirect roller
-  private final SingleMotorSubsystemIO intakeIO;
+  private final VelocityIO intakeIO;
   private final SingleMotorSubsystemInputsAutoLogged intakeIOAutoLogged =
       new SingleMotorSubsystemInputsAutoLogged();
   private final SysIdRoutine intake_sysIdRoutine;
@@ -34,7 +34,7 @@ public class Intake
   private final BeamBreakIOInputsAutoLogged beamBreakIOAutoLogged =
       new BeamBreakIOInputsAutoLogged();
 
-  public Intake(boolean disabled, SingleMotorSubsystemIO intakeIO, BeamBreakIO beamBreakIO) {
+  public Intake(boolean disabled, VelocityIO intakeIO, BeamBreakIO beamBreakIO) {
     super(disabled);
 
     this.intakeIO = intakeIO;

src/main/java/frc/robot/utils/generics/PositionIO.java

@@ -0,0 +1,37 @@
+// Copyright (c) 2024 FRC 3256
+// https://github.com/Team3256
+//
+// Use of this source code is governed by a 
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.utils.generics;
+
+import com.ctre.phoenix6.controls.NeutralOut;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+
+public interface PositionIO {
+
+  public default void updateInputs(SingleMotorSubsystemInputs inputs) {}
+
+  public default void setVoltage(double voltage) {}
+
+  public default void setPosition(double position) {}
+
+  public default TalonFX getMotor() {
+    return new TalonFX(0);
+  }
+
+  public default VoltageOut getVoltageRequest() {
+    return new VoltageOut(0);
+  }
+
+  public default void off() {
+    this.getMotor().setControl(new NeutralOut());
+  }
+
+  public default void zero() {
+    this.getMotor().setPosition(0);
+  }
+}

src/main/java/frc/robot/utils/generics/PositionIOTalonFX.java

@@ -0,0 +1,105 @@
+// Copyright (c) 2024 FRC 3256
+// https://github.com/Team3256
+//
+// Use of this source code is governed by a 
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.utils.generics;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.controls.NeutralOut;
+import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import frc.robot.utils.PhoenixUtil;
+
+public class PositionIOTalonFX<T extends SingleMotorConstants> implements PositionIO {
+
+  private final TalonFX motor;
+  private final PositionVoltage positionRequest = new PositionVoltage(0).withSlot(0);
+  private final MotionMagicVoltage motionMagicRequest = new MotionMagicVoltage(0).withSlot(0);
+  private final VoltageOut voltageReq = new VoltageOut(0);
+
+  private final StatusSignal<Double> motorVoltage;
+  private final StatusSignal<Double> motorPosition;
+  private final StatusSignal<Double> motorStatorCurrent;
+  private final StatusSignal<Double> motorSupplyCurrent;
+  // private final StatusSignal<Double> motorTemperature;
+  private final StatusSignal<Double> motorReferenceSlope;
+
+  public PositionIOTalonFX() {
+    motor = new TalonFX(T.kMotorID);
+    motorVoltage = motor.getMotorVoltage();
+    motorPosition = motor.getPosition();
+    motorStatorCurrent = motor.getStatorCurrent();
+    motorSupplyCurrent = motor.getSupplyCurrent();
+    // motorTemperature = motor.getDeviceTemp();
+    motorReferenceSlope = motor.getClosedLoopReferenceSlope();
+
+    PhoenixUtil.applyMotorConfigs(motor, T.kMotorConfig, T.kFlashConfigRetries);
+
+    BaseStatusSignal.setUpdateFrequencyForAll(
+        T.kStatusSignalUpdateFrequency,
+        motorVoltage,
+        motorPosition,
+        motorStatorCurrent,
+        motorSupplyCurrent,
+        // motorTemperature,
+        motorReferenceSlope);
+    motor.optimizeBusUtilization();
+  }
+
+  @Override
+  public void updateInputs(SingleMotorSubsystemInputs inputs) {
+    BaseStatusSignal.refreshAll(
+        motorVoltage,
+        motorPosition,
+        motorStatorCurrent,
+        motorSupplyCurrent,
+        // motorTemperature,
+        motorReferenceSlope);
+    inputs.motorVoltage = motorVoltage.getValueAsDouble();
+    inputs.motorPosition = motorPosition.getValueAsDouble();
+    inputs.motorStatorCurrent = motorStatorCurrent.getValueAsDouble();
+    inputs.motorSupplyCurrent = motorSupplyCurrent.getValueAsDouble();
+    // inputs.motorTemperature = motorTemperature.getValueAsDouble();
+    inputs.motorReferenceSlope = motorReferenceSlope.getValueAsDouble();
+  }
+
+  @Override
+  public void setPosition(double position) {
+    if (T.kUseMotionMagic) {
+      motor.setControl(motionMagicRequest.withPosition(position));
+    } else {
+      motor.setControl(positionRequest.withPosition(position));
+    }
+  }
+
+  @Override
+  public void setVoltage(double voltage) {
+    motor.setVoltage(voltage);
+  }
+
+  @Override
+  public void off() {
+    motor.setControl(new NeutralOut());
+  }
+
+  @Override
+  public void zero() {
+    motor.setPosition(0);
+  }
+
+  @Override
+  public TalonFX getMotor() {
+    return motor;
+  }
+
+  @Override
+  public VoltageOut getVoltageRequest() {
+    return voltageReq;
+  }
+}

src/main/java/frc/robot/utils/generics/SingleMotorSubsystemIO.java

@@ -10,19 +10,8 @@
 import com.ctre.phoenix6.controls.NeutralOut;
 import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.TalonFX;
-import org.littletonrobotics.junction.AutoLog;
-
-public interface SingleMotorSubsystemIO {
-  @AutoLog
-  public static class SingleMotorSubsystemInputs {
-    public double motorVoltage = 0.0;
-    public double motorPosition = 0.0;
-    public double motorVelocity = 0.0;
-    public double motorStatorCurrent = 0.0;
-    public double motorSupplyCurrent = 0.0;
-    public double motorTemperature = 0.0;
-    public double motorReferenceSlope = 0.0;
-  }
+
+public interface SingleMotorSubsystemIO extends VelocityIO, PositionIO {
 
   public default void updateInputs(SingleMotorSubsystemInputs inputs) {}
 

src/main/java/frc/robot/utils/generics/SingleMotorSubsystemInputs.java

@@ -0,0 +1,21 @@
+// Copyright (c) 2024 FRC 3256
+// https://github.com/Team3256
+//
+// Use of this source code is governed by a 
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.utils.generics;
+
+import org.littletonrobotics.junction.AutoLog;
+
+@AutoLog
+public class SingleMotorSubsystemInputs {
+  public double motorVoltage = 0.0;
+  public double motorVelocity = 0.0;
+  public double motorPosition = 0.0;
+  public double motorStatorCurrent = 0.0;
+  public double motorSupplyCurrent = 0.0;
+  public double motorTemperature = 0.0;
+  public double motorReferenceSlope = 0.0;
+}

src/main/java/frc/robot/utils/generics/VelocityIO.java

@@ -0,0 +1,37 @@
+// Copyright (c) 2024 FRC 3256
+// https://github.com/Team3256
+//
+// Use of this source code is governed by a 
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.utils.generics;
+
+import com.ctre.phoenix6.controls.NeutralOut;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+
+public interface VelocityIO {
+
+  public default void updateInputs(SingleMotorSubsystemInputs inputs) {}
+
+  public default void setVoltage(double voltage) {}
+
+  public default void setVelocity(double velocity) {}
+
+  public default TalonFX getMotor() {
+    return new TalonFX(0);
+  }
+
+  public default VoltageOut getVoltageRequest() {
+    return new VoltageOut(0);
+  }
+
+  public default void off() {
+    this.getMotor().setControl(new NeutralOut());
+  }
+
+  public default void zero() {
+    this.getMotor().setPosition(0);
+  }
+}

src/main/java/frc/robot/utils/generics/VelocityIOTalonFX.java

@@ -0,0 +1,88 @@
+// Copyright (c) 2024 FRC 3256
+// https://github.com/Team3256
+//
+// Use of this source code is governed by a 
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.utils.generics;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.controls.NeutralOut;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import frc.robot.utils.PhoenixUtil;
+
+public class VelocityIOTalonFX<T extends SingleMotorConstants> implements VelocityIO {
+
+  private final TalonFX motor;
+  private final VoltageOut voltageReq = new VoltageOut(0);
+
+  private final StatusSignal<Double> motorVoltage;
+  private final StatusSignal<Double> motorVelocity;
+  private final StatusSignal<Double> motorStatorCurrent;
+  private final StatusSignal<Double> motorSupplyCurrent;
+  private final StatusSignal<Double> motorTemperature;
+  private final StatusSignal<Double> motorReferenceSlope;
+
+  public VelocityIOTalonFX() {
+    motor = new TalonFX(T.kMotorID);
+    motorVoltage = motor.getMotorVoltage();
+    motorVelocity = motor.getVelocity();
+    motorStatorCurrent = motor.getStatorCurrent();
+    motorSupplyCurrent = motor.getSupplyCurrent();
+    motorTemperature = motor.getDeviceTemp();
+    motorReferenceSlope = motor.getClosedLoopReferenceSlope();
+
+    PhoenixUtil.applyMotorConfigs(motor, T.kMotorConfig, T.kFlashConfigRetries);
+
+    BaseStatusSignal.setUpdateFrequencyForAll(
+        T.kStatusSignalUpdateFrequency,
+        motorVoltage,
+        motorVelocity,
+        motorStatorCurrent,
+        motorSupplyCurrent,
+        motorTemperature,
+        motorReferenceSlope);
+    motor.optimizeBusUtilization();
+  }
+
+  @Override
+  public void updateInputs(SingleMotorSubsystemInputs inputs) {
+    BaseStatusSignal.refreshAll(
+        motorVoltage,
+        motorVelocity,
+        motorStatorCurrent,
+        motorSupplyCurrent,
+        motorTemperature,
+        motorReferenceSlope);
+    inputs.motorVoltage = motorVoltage.getValueAsDouble();
+    inputs.motorVelocity = motorVelocity.getValueAsDouble();
+    inputs.motorStatorCurrent = motorStatorCurrent.getValueAsDouble();
+    inputs.motorSupplyCurrent = motorSupplyCurrent.getValueAsDouble();
+    inputs.motorTemperature = motorTemperature.getValueAsDouble();
+    inputs.motorReferenceSlope = motorReferenceSlope.getValueAsDouble();
+  }
+
+  @Override
+  public void setVoltage(double voltage) {
+    motor.setVoltage(voltage);
+  }
+
+  @Override
+  public void off() {
+    // Or VelocityTorqueCurrentFOC for regen braking
+    motor.setControl(new NeutralOut());
+  }
+
+  @Override
+  public TalonFX getMotor() {
+    return motor;
+  }
+
+  @Override
+  public VoltageOut getVoltageRequest() {
+    return voltageReq;
+  }
+}