From 71c8ab1f67e70bbde67769216fb98ffa80c25ec9 Mon Sep 17 00:00:00 2001 From: iefomit Date: Mon, 16 Feb 2026 11:31:26 -0800 Subject: [PATCH] account for 180 rotation --- .../java/frc/robot/subsystems/turret/Turret.java | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) 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; -- 2.39.5