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 ca044a1eda4370b33b98db6c6f3e533f1f143658..4fe7e0576399b8263cf5ac0273c019a537819e13 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.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 23ed33aeb0d4279280b67e0d616ab47889c7e1d8..505ea80f912abc02c634f43781fceea4432b4cbf 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.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 afd311c827aedd5969c074ce442eab36276e5705..27565841bb27e4cb1786e069f715a111de5110b5 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 = 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