From: iefomit <timofei.stem@gmail.com>
Date: Mon, 16 Feb 2026 19:14:48 +0000 (-0800)
Subject: add LinearFilter
X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=61da1dbd8fe328a0945e49908d8a738e1fad386b;p=FRC2026.git

add LinearFilter
---

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 9ecb52e..5650661 100644
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -12,6 +12,7 @@ import com.ctre.phoenix6.sim.TalonFXSimState;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.filter.LinearFilter;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
@@ -41,13 +42,13 @@ public class Turret extends SubsystemBase implements TurretIO{
 	private static final double TURRET_RATIO = 140.0 / 10.0;
 	private static final double GEAR_RATIO = VERSA_RATIO * TURRET_RATIO;
 
-	private static final double STOP_THRESHOLD_RAD = Units.degreesToRadians(0.5);  // stop if within 0.5 deg
-	private static final double START_THRESHOLD_RAD = Units.degreesToRadians(1.0); // move again if beyond 1.0 deg
-
 	private static final PIDController positionPID = new PIDController(15, 0, 0.25);
 	//private static final PIDController longVelocityPID = new PIDController(15, 0, 1.0);
 	private static final PIDController velocityPID = new PIDController(0.0, 0.0, 0.0);
 
+	// filter to smooth setpoint
+	private final LinearFilter setpointFilter = LinearFilter.movingAverage(5);
+
 
     private final TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged();
 
@@ -72,7 +73,6 @@ public class Turret extends SubsystemBase implements TurretIO{
 	private Rotation2d goalAngle = Rotation2d.kZero;
 	private double goalVelocityRadPerSec = 0.0;
 	private double lastGoalRad = 0.0;
-	private boolean isInDeadband = false;
 
     // private final MotionMagicVelocityVoltage velocityRequest = new MotionMagicVelocityVoltage(0.0).withUpdateFreqHz(0);
 
@@ -207,23 +207,13 @@ public class Turret extends SubsystemBase implements TurretIO{
 			}
 		}
 
-		double currentPositionRad = getPositionRad();
-		double errorRad = best - currentPositionRad;
-		double absError = Math.abs(errorRad);
+		lastGoalRad = best;
 
-		if (isInDeadband) {
-			if (absError > START_THRESHOLD_RAD) {
-				isInDeadband = false;
-				lastGoalRad = best;
-			}
-		} else {
-			lastGoalRad = best;
-			if (absError < STOP_THRESHOLD_RAD) {
-				isInDeadband = true;
-			}
-		}
+		// apply filter to smooth setpoint
+		double filteredSetpoint = setpointFilter.calculate(best);
 
-		double motorGoalRotations = Units.radiansToRotations(lastGoalRad) * GEAR_RATIO;
+		// Tells the Kraken to get to this position using 1000Hz profile
+		double motorGoalRotations = Units.radiansToRotations(filteredSetpoint) * GEAR_RATIO;
 
 		motorGoalRotations = MathUtil.clamp(motorGoalRotations, Units.degreesToRotations(-180) * GEAR_RATIO, Units.degreesToRotations(180) * GEAR_RATIO);
 		
@@ -233,19 +223,17 @@ public class Turret extends SubsystemBase implements TurretIO{
 		
 		double robotTurnCompensation = goalVelocityRadPerSec * 0.165;
 
-		double feedforward = isInDeadband ? 0.0 : robotTurnCompensation;
-
 		motor.setControl(mmVoltageRequest
 			.withPosition(motorGoalRotations)
-			.withFeedForward(feedforward));
+			.withFeedForward(robotTurnCompensation));
 
-		Logger.recordOutput("Turret/InDeadband", isInDeadband);
+		Logger.recordOutput("Turret/FilteredSetpoint", Units.radiansToDegrees(filteredSetpoint));
 
 		lastFrameVelocity = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble());
 
 		// var request = velocityRequest.withVelocity(Units.radiansToRotations(targetVelocity)).withEnableFOC(false);
         Logger.recordOutput("Turret/Voltage", motor.getMotorVoltage().getValue());
-		Logger.recordOutput("Turret/setpointDeg", Units.rotationsToDegrees(motorGoalRotations) / GEAR_RATIO);
+		Logger.recordOutput("Turret/setpointDeg", Units.radiansToDegrees(motorGoalRotations) / GEAR_RATIO);
 		//Logger.recordOutput("Turret/velocitySetpoint", targetVelocity / GEAR_RATIO);
 
 		// --- Position + velocity feedforward (MA-style) ---