From: iefomit <timofei.stem@gmail.com>
Date: Mon, 16 Feb 2026 19:31:26 +0000 (-0800)
Subject: account for 180 rotation
X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=71c8ab1f67e70bbde67769216fb98ffa80c25ec9;p=FRC2026.git

account for 180 rotation
---

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index bcb0612..8cbefe5 100644
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -73,6 +73,8 @@ public class Turret extends SubsystemBase implements TurretIO{
 	private Rotation2d goalAngle = Rotation2d.kZero;
 	private double goalVelocityRadPerSec = 0.0;
 	private double lastGoalRad = 0.0;
+	private double lastFilteredRad = 0.0;
+	private double lastRawSetpoint = 0.0;
 
     // private final MotionMagicVelocityVoltage velocityRequest = new MotionMagicVelocityVoltage(0.0).withUpdateFreqHz(0);
 
@@ -191,8 +193,17 @@ public class Turret extends SubsystemBase implements TurretIO{
 
 		lastGoalRad = best;
 
-		// always apply filter to smooth setpoint
-		best = setpointFilter.calculate(best);
+		// 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;