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 9ecb52ed2dba7550f5a4d3b97fb731c848e78a42..5650661efac1437f95d41c772adf2294f9b07326 100644 (file)
--- 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) ---