force specific calls instead of double

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 1be64d16578df923804784e740483a678c5b98b7..e417249f47f2c4d9d6c7b0690d95f48d77f65c92 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -1,5 +1,8 @@
 package frc.robot.subsystems.turret;
 
+import static edu.wpi.first.units.Units.Rotations;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.PositionVoltage;
@@ -14,6 +17,7 @@ import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
@@ -50,8 +54,6 @@ public class Turret extends SubsystemBase {
     private TalonFXSimState simState;
     private SingleJointedArmSim turretSim;
 
-    private final PositionVoltage positionRequest = new PositionVoltage(0.0);
-
     /* ---------------- Control ---------------- */
 
     private final TrapezoidProfile profile =
@@ -66,6 +68,8 @@ public class Turret extends SubsystemBase {
     private double goalVelocityRadPerSec = 0.0;
     private double lastGoalRad = 0.0;
 
+    public PositionVoltage voltageRequest;
+
     /* ---------------- Gains ---------------- */
 
     private static final double kP = 3500.0;
@@ -100,6 +104,8 @@ public class Turret extends SubsystemBase {
                 Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RotorSensor;
             }}
         );
+        
+        voltageRequest = new PositionVoltage(0);
 
         setpoint = new State(getPositionRad(), 0.0);
         lastGoalRad = setpoint.position;
@@ -179,10 +185,7 @@ public class Turret extends SubsystemBase {
             Units.radiansToRotations(setpoint.velocity) * GEAR_RATIO;
 
         // --- Position + velocity feedforward (MA-style) ---
-        motor.setControl(
-            positionRequest
-                .withPosition(motorPosRot)
-                .withVelocity(motorVelRotPerSec));
+        motor.setControl(voltageRequest.withPosition(Rotations.of(motorPosRot)).withVelocity(RotationsPerSecond.of(motorVelRotPerSec)));
 
         // --- Visualization ---
         ligament.setAngle(Units.radiansToDegrees(getPositionRad()));