use motion magic velocity control in CoralHolderSub

Author: neelkarma

src/main/java/frc/robot/constants/CoralHolderConstants.java

@@ -8,6 +8,9 @@
 public class CoralHolderConstants {
   public static final Time OUTTAKE_DURATION = Seconds.of(0.5);
   public static final Time INTAKE_DURATION = Seconds.of(1);
-  public static final double THROTTLE = 0.2;
-  public static final FalconMotorSupplier MOTOR_ID = new FalconMotorSupplier(5).withInvert();
+  public static final double SPEED_RPS = 1;
+  public static final FalconMotorSupplier MOTOR_ID = new FalconMotorSupplier(5)
+      .withPID(1, 0, 0)
+      .withEncoder(9)
+      .withInvert();
 }

src/main/java/frc/robot/subsystems/CoralHolderSub.java

@@ -1,5 +1,6 @@
 package frc.robot.subsystems;
 
+import com.ctre.phoenix6.controls.MotionMagicVelocityDutyCycle;
 import com.ctre.phoenix6.hardware.TalonFX;
 
 import edu.wpi.first.util.sendable.Sendable;
@@ -11,21 +12,22 @@
 
 public class CoralHolderSub extends SubsystemBase {
   private final TalonFX motor = CoralHolderConstants.MOTOR_ID.get();
+  private final MotionMagicVelocityDutyCycle control = new MotionMagicVelocityDutyCycle(0);
 
   public CoralHolderSub() {
     setupSmartDash();
   }
 
   public void forward() {
-    motor.set(CoralHolderConstants.THROTTLE);
+    motor.setControl(control.withVelocity(CoralHolderConstants.SPEED_RPS));
   }
 
   public void reverse() {
-    motor.set(-CoralHolderConstants.THROTTLE);
+    motor.setControl(control.withVelocity(-CoralHolderConstants.SPEED_RPS));
   }
 
   public void stop() {
-    motor.set(0);
+    motor.setControl(control.withVelocity(0));
   }
 
   public Command manualOutCommand() {

src/main/java/frc/robot/utils/TypeSupliers/motorsupplier/FalconMotorSupplier.java

@@ -28,6 +28,14 @@ public FalconMotorSupplier withInvert() {
     return this;
   }
 
+  public FalconMotorSupplier withFeedForward(double s, double v, double a) {
+    motorConfig.Slot0.kS = s;
+    motorConfig.Slot0.kV = v;
+    motorConfig.Slot0.kA = a;
+
+    return this;
+  }
+
   public FalconMotorSupplier withPID(double p, double i, double d) {
     motorConfig.Slot0.kP = p;
     motorConfig.Slot0.kI = i;