From f947a3ed1bd0eec00f9672d733d89a14ff8476f9 Mon Sep 17 00:00:00 2001
From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com>
Date: Mon, 16 Feb 2026 12:19:28 -0800
Subject: [PATCH] Update Turret.java

---
 .../frc/robot/subsystems/turret/Turret.java   | 76 ++++++++++---------
 1 file changed, 42 insertions(+), 34 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index f097167..a04eb3a 100644
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -181,15 +181,21 @@ public class Turret extends SubsystemBase implements TurretIO{
 
 	@Override
 	public void periodic() {
+		updateInputs();
+		Logger.processInputs("Turret", inputs);
 		
 		double robotRelativeGoal = goalAngle.getRadians();
 
 		// --- MA-style continuous wrap selection ---
+
+		double lookAheadSeconds = 0.060; 
+    	double futureRobotAngle = goalAngle.getRadians() + (goalVelocityRadPerSec * lookAheadSeconds);
+
 		double best = lastGoalRad;
 		boolean found = false;
 
 		for (int i = -2; i <= 2; i++) {
-			double candidate = robotRelativeGoal + 2.0 * Math.PI * i;
+			double candidate = futureRobotAngle + 2.0 * Math.PI * i;
 			if (candidate < MIN_ANGLE_RAD || candidate > MAX_ANGLE_RAD)
 				continue;
 
@@ -201,40 +207,42 @@ public class Turret extends SubsystemBase implements TurretIO{
 
 		lastGoalRad = best;
 
-		// --- Profile in MECHANISM SPACE ---
-		State goalState = new State(
-				MathUtil.clamp(best, MIN_ANGLE_RAD, MAX_ANGLE_RAD),
-				goalVelocityRadPerSec);
-
-		setpoint = profile.calculate(
-				Constants.LOOP_TIME,
-				setpoint,
-				goalState);
-
-		// --- Convert to MOTOR SPACE ---
-		double motorPosRot = Units.radiansToRotations(setpoint.position) * GEAR_RATIO;
-
-		double motorVelRotPerSec = Units.radiansToRotations(setpoint.velocity) * GEAR_RATIO;
-
-		double targetVelocity;
-
-		double motorSetpointPosition = (setpoint.position) * GEAR_RATIO;
-
-		targetVelocity =
-		positionPID.calculate(
-				motor.getPosition().getValue().in(edu.wpi.first.units.Units.Radians),
-				motorSetpointPosition);
-			
-		targetVelocity += Units.rotationsToRadians(motorVelRotPerSec) * 1.0;
-
-		double voltage = feedForward.calculate(targetVelocity);
-
-		// double velocityCorrectionVoltage = velocityPID.calculate(Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()), targetVelocity);
-		// voltage += velocityCorrectionVoltage;
-
-		motor.setVoltage(voltage);
+		// calculate shortest angular delta
+		double delta = best - lastRawSetpoint;
+		delta = MathUtil.angleModulus(delta);
+		
+		// filter delta
+		double filteredDelta = setpointFilter.calculate(delta);
+		
+		// apply filtered range
+		lastFilteredRad = MathUtil.angleModulus(lastFilteredRad + filteredDelta);
+		lastRawSetpoint = best;
+		best = lastFilteredRad;
+
+		// Tells the Kraken to get to this position using 1000Hz profile
+		double motorGoalRotations = Units.radiansToRotations(best) * GEAR_RATIO;
+		// if (isInDeadband) {
+		// 	if (absError > START_THRESHOLD_RAD) {
+		// 		isInDeadband = false;
+		// 		lastGoalRad = best;
+		// 	}
+		// } else {
+		// 	lastGoalRad = best;
+		// 	if (absError < STOP_THRESHOLD_RAD) {
+		// 		isInDeadband = true;
+		// 	}
+		// }
+
+		motorGoalRotations = MathUtil.clamp(motorGoalRotations, Units.degreesToRotations(-180) * GEAR_RATIO, Units.degreesToRotations(180) * GEAR_RATIO);
+		
+		// Add the feedforward for the target velocity (SOTM) here as well
+		double feedforwardVoltage = feedForward.calculate((goalVelocityRadPerSec) * GEAR_RATIO);
+		
+		double robotTurnCompensation = goalVelocityRadPerSec * 0.185;
 
-		lastFrameVelocity = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble());
+		motor.setControl(mmVoltageRequest
+			.withPosition(motorGoalRotations)
+			.withFeedForward(robotTurnCompensation));
 
         Logger.recordOutput("Turret/Voltage", motor.getMotorVoltage().getValue());
 		Logger.recordOutput("Turret/velocitySetpoint", targetVelocity / GEAR_RATIO);
-- 
2.39.5