fix turret limits

diff --git a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
index 75a9ec12848b24d542096100c8466c95d6d28bdc..91113e8eda87145389b683b2b0d9f9d8dcf45acf 100644 (file)
--- 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 cfad74c670f0616893f9227582eb3187951244e8..3c7cfdceb6e9d4129753da340b7ada3735705a5f 100644 (file)
--- 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;
         }