From f7bffabf20c0f596073dbd628312a1239fce7e0e Mon Sep 17 00:00:00 2001 From: Wesley28w Date: Wed, 8 Apr 2026 21:40:03 -0700 Subject: [PATCH] sorry it wasn't updated... --- src/main/java/frc/robot/commands/gpm/Superstructure.java | 6 +++--- src/main/java/frc/robot/subsystems/turret/Turret.java | 8 ++++---- .../java/frc/robot/subsystems/turret/TurretConstants.java | 4 ++-- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java index ca044a1..4fe7e05 100644 --- a/src/main/java/frc/robot/commands/gpm/Superstructure.java +++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java @@ -159,10 +159,10 @@ 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()), -180, 180); + double error = MathUtil.inputModulus(Units.radiansToDegrees(adjustedTurretSetpoint) - Units.radiansToDegrees(turret.getPositionRad()), TurretConstants.MIN_ANGLE, TurretConstants.MAX_ANGLE); double potentialSetpoint = Units.radiansToDegrees(turret.getPositionRad()) + error + turretOffset; // Stay within +/- 200 -- if shortest path is past 200, we go long way around @@ -324,4 +324,4 @@ public class Superstructure extends Command { stowEverything(); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 23ed33a..505ea80 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 ---------------- */ @@ -304,4 +304,4 @@ public class Turret extends SubsystemBase implements TurretIO{ calibrating = false; setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(TurretConstants.CALIBRATION_OFFSET)), 0.0); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java index afd311c..2756584 100644 --- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java +++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java @@ -4,8 +4,8 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; public class TurretConstants { - public static double MAX_ANGLE = 180; // Deg - public static double MIN_ANGLE = -200; // Deg + public static double MAX_ANGLE = 175; // Deg + public static double MIN_ANGLE = -250; // Deg public static double CALIBRATION_OFFSET = 0.0; // TODO: find this at hardstop -- 2.39.5