WORKS NOW!! We have true range (150, -250)

diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java
index 57bb09a2d1fde28486ec35829d285619ab3c431e..5048da00e2f3b4513f3dae41ec40f483871ef5b3 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/Superstructure.java
+++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java
@@ -159,7 +159,7 @@ 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()), TurretConstants.MIN_ANGLE, TurretConstants.MAX_ANGLE);

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 23ed33aeb0d4279280b67e0d616ab47889c7e1d8..630913dc10d3075b629f49b81c3401c0f969d976 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 ---------------- */

diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
index 27565841bb27e4cb1786e069f715a111de5110b5..2055b749a1b30efa4c7bef56c39f34ee618caed0 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
+++ b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
@@ -4,7 +4,7 @@ import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
 
 public class TurretConstants {
-    public static double MAX_ANGLE = 175; // Deg
+    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 CALIBRATION_OFFSET = 0.0; // TODO: find this at hardstop