From: mixxlto <maxtan0626@gmail.com>
Date: Sun, 1 Feb 2026 18:01:48 +0000 (-0800)
Subject: should work now
X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=e3c16116d1ccab85c9dd9d83115a8363db659212;p=FRC2026.git

should work now
---

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 2fa85cc..10490fe 100644
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -47,7 +47,8 @@ public class Turret extends SubsystemBase {
 	private static final double TURRET_RATIO = 140.0 / 10.0;
 	private static final double GEAR_RATIO = VERSA_RATIO * TURRET_RATIO;
 
-	private static final PIDController velocityPid = new PIDController(15, 0, 0.25);
+	private static final PIDController velocityPID = new PIDController(15, 0, 0.25);
+	private static final PIDController longVelocityPID = new PIDController(15, 0, 1.0);
 
 
     private double lastFrameVelocity = 0;
@@ -186,22 +187,32 @@ public class Turret extends SubsystemBase {
 
 		double motorVelRotPerSec = Units.radiansToRotations(setpoint.velocity) * GEAR_RATIO;
 
-        // in rad/sec
-		double targetVelocity = velocityPid.calculate(
-				motor.getPosition().getValue().in(edu.wpi.first.units.Units.Radians),
-				setpoint.position * GEAR_RATIO);
-        
-        targetVelocity += Units.rotationsToRadians(motorVelRotPerSec);
+		double targetVelocity;
 
-        // double voltage = feedForward.calculateWithVelocities(lastFrameVelocity, targetVelocity);
-        double voltage = feedForward.calculate(targetVelocity);
-        lastFrameVelocity = targetVelocity;
 		if(Math.abs(setpoint.position * GEAR_RATIO - Units.rotationsToRadians(motor.getPosition().getValueAsDouble())) > Math.PI/2){
-			motor.setControl(mmVoltageRequest.withPosition(Units.radiansToRotations(setpoint.position * GEAR_RATIO)));
+			// in rad/sec
+			targetVelocity = longVelocityPID.calculate(
+					motor.getPosition().getValue().in(edu.wpi.first.units.Units.Radians),
+					setpoint.position * GEAR_RATIO);
+			
+			targetVelocity += Units.rotationsToRadians(motorVelRotPerSec);
+
+			double voltage = feedForward.calculate(targetVelocity);
+			motor.setVoltage(voltage);
 		} else{
+			// in rad/sec
+			targetVelocity = velocityPID.calculate(
+					motor.getPosition().getValue().in(edu.wpi.first.units.Units.Radians),
+					setpoint.position * GEAR_RATIO);
+			
+			targetVelocity += Units.rotationsToRadians(motorVelRotPerSec);
+
+			double voltage = feedForward.calculate(targetVelocity);
 			motor.setVoltage(voltage);
 		}
 
+		lastFrameVelocity = targetVelocity;
+
 		// var request = velocityRequest.withVelocity(Units.radiansToRotations(targetVelocity)).withEnableFOC(false);
         Logger.recordOutput("Turret Voltage", motor.getMotorVoltage().getValue());
 
@@ -212,7 +223,8 @@ public class Turret extends SubsystemBase {
 		// --- Visualization ---
 		ligament.setAngle(Units.radiansToDegrees(getPositionRad()));
 
-        SmartDashboard.putData(velocityPid);
+        SmartDashboard.putData(velocityPID);
+		SmartDashboard.putData(longVelocityPID);
 		SmartDashboard.putNumber("Turret GoalDeg",
 				Units.radiansToDegrees(best));
 		SmartDashboard.putNumber("Turret SetpointDeg",