From a3ea256b3e6c1f7246cf8730b9b8ab8c852db41d Mon Sep 17 00:00:00 2001 From: WesleyWong-972 Date: Wed, 8 Apr 2026 14:52:55 -0700 Subject: [PATCH] WORKS NOW!! We have true range (150, -250) --- src/main/java/frc/robot/commands/gpm/Superstructure.java | 2 +- src/main/java/frc/robot/subsystems/turret/Turret.java | 6 +++--- .../java/frc/robot/subsystems/turret/TurretConstants.java | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java index 57bb09a..5048da0 100644 --- a/src/main/java/frc/robot/commands/gpm/Superstructure.java +++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java @@ -159,7 +159,7 @@ public class Superstructure extends Command { } // Subtract the rotational angle of the robot from the setpoint - double adjustedTurretSetpoint = MathUtil.angleModulus(turretAngle.getRadians() - drivepose.getRotation().getRadians()); + double adjustedTurretSetpoint = MathUtil.inputModulus(turretAngle.getRadians() - drivepose.getRotation().getRadians(), TurretConstants.MIN_ANGLE, TurretConstants.MAX_ANGLE); // Shortest path double error = MathUtil.inputModulus(Units.radiansToDegrees(adjustedTurretSetpoint) - Units.radiansToDegrees(turret.getPositionRad()), TurretConstants.MIN_ANGLE, TurretConstants.MAX_ANGLE); diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 23ed33a..630913d 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -193,13 +193,13 @@ public class Turret extends SubsystemBase implements TurretIO{ // calculate shortest angular delta double delta = best - lastRawSetpoint; - delta = MathUtil.angleModulus(delta); + delta = MathUtil.inputModulus(delta, TurretConstants.MIN_ANGLE, TurretConstants.MAX_ANGLE); // filter delta double filteredDelta = setpointFilter.calculate(delta); // apply filtered range - lastFilteredRad = MathUtil.angleModulus(lastFilteredRad + filteredDelta); + lastFilteredRad = MathUtil.inputModulus(lastFilteredRad + filteredDelta, TurretConstants.MIN_ANGLE, TurretConstants.MAX_ANGLE); lastRawSetpoint = best; best = lastFilteredRad; @@ -241,7 +241,7 @@ public class Turret extends SubsystemBase implements TurretIO{ SmartDashboard.putBoolean("Turret Calibrated", !calibrating); SmartDashboard.putBoolean("Turret At Setpoint", atSetpoint()); } - + Logger.recordOutput("Turret/Position", getPositionDeg()); } /* ---------------- Simulation ---------------- */ diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java index 2756584..2055b74 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java @@ -4,7 +4,7 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; public class TurretConstants { - public static double MAX_ANGLE = 175; // Deg + public static double MAX_ANGLE = 175; // Deg NOTE: currently it actually only goes to 150 public static double MIN_ANGLE = -250; // Deg public static double CALIBRATION_OFFSET = 0.0; // TODO: find this at hardstop -- 2.39.5