From 756572f246ad19668c8474b976f0d4f5b041ae9d Mon Sep 17 00:00:00 2001 From: iefomit Date: Wed, 8 Apr 2026 23:42:12 -0700 Subject: [PATCH] fix turret limits --- src/main/java/frc/robot/commands/gpm/AutoShootCommand.java | 7 +++---- src/main/java/frc/robot/commands/gpm/Superstructure.java | 7 +++---- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java index 75a9ec1..91113e8 100644 --- a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java +++ b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java @@ -145,11 +145,10 @@ public class AutoShootCommand extends Command { double error = MathUtil.inputModulus(Units.radiansToDegrees(adjustedTurretSetpoint) - Units.radiansToDegrees(turret.getPositionRad()), -180, 180); double potentialSetpoint = Units.radiansToDegrees(turret.getPositionRad()) + error; - // Stay within +/- 200 -- if shortest path is past 200, we go long way around - double turretRange = TurretConstants.MAX_ANGLE - TurretConstants.MIN_ANGLE; - if (potentialSetpoint > turretRange/2) { + // Stay within physical limits -- if shortest path is past max angle, we go long way around + if (potentialSetpoint > TurretConstants.MAX_ANGLE) { potentialSetpoint -= 360; - } else if (potentialSetpoint < -turretRange/2) { + } else if (potentialSetpoint < TurretConstants.MIN_ANGLE) { potentialSetpoint += 360; } diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java index cfad74c..3c7cfdc 100644 --- a/src/main/java/frc/robot/commands/gpm/Superstructure.java +++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java @@ -165,11 +165,10 @@ public class Superstructure extends Command { 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 - double turretRange = TurretConstants.MAX_ANGLE - TurretConstants.MIN_ANGLE; - if (potentialSetpoint > turretRange/2) { + // Stay within physical limits -- if shortest path is past max angle, we go long way around + if (potentialSetpoint > TurretConstants.MAX_ANGLE) { potentialSetpoint -= 360; - } else if (potentialSetpoint < -turretRange/2) { + } else if (potentialSetpoint < TurretConstants.MIN_ANGLE) { potentialSetpoint += 360; } -- 2.39.5