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

---
 .../frc/robot/subsystems/turret/Turret.java    | 18 ++----------------
 1 file changed, 2 insertions(+), 16 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index e7d3bfe..a2e89ba 100644
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -19,6 +19,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;
@@ -53,17 +54,11 @@ public class Turret extends SubsystemBase implements TurretIO{
 	private static final double MIN_ANGLE_RAD = Units.degreesToRadians(-180); // Change this later to the actual values
 	private static final double MAX_ANGLE_RAD = Units.degreesToRadians(180);
 
-	private static final double MAX_VEL_RAD_PER_SEC = 15;
-	//private static final double MAX_VEL_RAD_PER_SEC = 3.0; // Starting super duper slow for now
-	// private static final double MAX_ACCEL_RAD_PER_SEC2 = 160.0;
+	private static final double MAX_VEL_RAD_PER_SEC = 600;
 	private static final double MAX_ACCEL_RAD_PER_SEC2 = 160.0;
 
 	private static final double GEAR_RATIO = TurretConstants.TURRET_GEAR_RATIO;
 
-	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);
-
 	private final LinearFilter setpointFilter = LinearFilter.singlePoleIIR(0.02
 	, 0.02);
 	//hello
@@ -73,8 +68,6 @@ public class Turret extends SubsystemBase implements TurretIO{
 
     private final TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged();
 
-	private double lastFrameVelocity = 0.0;
-
 	/* ---------------- Hardware ---------------- */
 
 	private final TalonFX motor = new TalonFX(IdConstants.TURRET_MOTOR_ID, Constants.SUBSYSTEM_CANIVORE_CAN);
@@ -84,13 +77,6 @@ public class Turret extends SubsystemBase implements TurretIO{
 
 	/* ---------------- Control ---------------- */
 
-	private final TrapezoidProfile profile = new TrapezoidProfile(
-			new TrapezoidProfile.Constraints(
-					MAX_VEL_RAD_PER_SEC,
-					MAX_ACCEL_RAD_PER_SEC2));
-
-	private State setpoint = new State();
-
 	private Rotation2d goalAngle = Rotation2d.kZero;
 	private double goalVelocityRadPerSec = 0.0;
 	private double lastGoalRad = 0.0;
-- 
2.39.5