From 8d2155dae81fbcfaa130c6ccf389f38c25363507 Mon Sep 17 00:00:00 2001 From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com> Date: Mon, 16 Feb 2026 12:20:16 -0800 Subject: [PATCH] Update Turret.java --- src/main/java/frc/robot/subsystems/turret/Turret.java | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index a04eb3a..c7e1ad6 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -81,6 +81,8 @@ public class Turret extends SubsystemBase implements TurretIO{ private double goalVelocityRadPerSec = 0.0; private double lastGoalRad = 0.0; private State setpoint = new State(); + private double lastFilteredRad = 0.0; + private double lastRawSetpoint = 0.0; // private final MotionMagicVelocityVoltage velocityRequest = new MotionMagicVelocityVoltage(0.0).withUpdateFreqHz(0); @@ -234,10 +236,7 @@ public class Turret extends SubsystemBase implements TurretIO{ // } motorGoalRotations = MathUtil.clamp(motorGoalRotations, Units.degreesToRotations(-180) * GEAR_RATIO, Units.degreesToRotations(180) * GEAR_RATIO); - - // Add the feedforward for the target velocity (SOTM) here as well - double feedforwardVoltage = feedForward.calculate((goalVelocityRadPerSec) * GEAR_RATIO); - + double robotTurnCompensation = goalVelocityRadPerSec * 0.185; motor.setControl(mmVoltageRequest @@ -245,7 +244,6 @@ public class Turret extends SubsystemBase implements TurretIO{ .withFeedForward(robotTurnCompensation)); Logger.recordOutput("Turret/Voltage", motor.getMotorVoltage().getValue()); - Logger.recordOutput("Turret/velocitySetpoint", targetVelocity / GEAR_RATIO); Logger.recordOutput("Turret/setpointDeg", Units.radiansToDegrees(setpoint.position)); // --- Visualization --- -- 2.39.5