Revert "sorry it wasn't updated..."

diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java
index 3c7cfdceb6e9d4129753da340b7ada3735705a5f..6cad5be54eb564f204786340f3d10e928c938224 100644 (file)
--- 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.inputModulus(turretAngle.getRadians() - drivepose.getRotation().getRadians(), TurretConstants.MIN_ANGLE, TurretConstants.MAX_ANGLE);
+        double adjustedTurretSetpoint = MathUtil.angleModulus(turretAngle.getRadians() - drivepose.getRotation().getRadians());
 
         // Shortest path
-        double error = MathUtil.inputModulus(Units.radiansToDegrees(adjustedTurretSetpoint) - Units.radiansToDegrees(turret.getPositionRad()), TurretConstants.MIN_ANGLE, TurretConstants.MAX_ANGLE);
+        double error = MathUtil.inputModulus(Units.radiansToDegrees(adjustedTurretSetpoint) - Units.radiansToDegrees(turret.getPositionRad()), -180, 180);
         double potentialSetpoint = Units.radiansToDegrees(turret.getPositionRad()) + error + turretOffset;
 
         // Stay within physical limits -- if shortest path is past max angle, we go long way around
@@ -323,4 +323,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 a2c88ebcf521b860aba1e3a0cbb3d83f0fc750a1..e4823438e50170d6fea494b98b102f9a22dbaa42 100644 (file)
--- 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.inputModulus(delta, Units.degreesToRadians(TurretConstants.MIN_ANGLE), Units.degreesToRadians(TurretConstants.MAX_ANGLE));
+               delta = MathUtil.angleModulus(delta);
                
                // filter delta
                double filteredDelta = setpointFilter.calculate(delta);
                
                // apply filtered range
-               lastFilteredRad = MathUtil.inputModulus(lastFilteredRad + filteredDelta, Units.degreesToRadians(TurretConstants.MIN_ANGLE), Units.degreesToRadians(TurretConstants.MAX_ANGLE));
+               lastFilteredRad = MathUtil.angleModulus(lastFilteredRad + filteredDelta);
                lastRawSetpoint = best;
                best = lastFilteredRad;
 
@@ -239,7 +239,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 ---------------- */
@@ -301,4 +301,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 2055b749a1b30efa4c7bef56c39f34ee618caed0..afd311c827aedd5969c074ce442eab36276e5705 100644 (file)
--- 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 = 175; // Deg NOTE: currently it actually only goes to 150
-    public static double MIN_ANGLE = -250; // Deg
+    public static double MAX_ANGLE = 180; // Deg
+    public static double MIN_ANGLE = -200; // Deg
 
     public static double CALIBRATION_OFFSET = 0.0; // TODO: find this at hardstop