WORKING min and max horray

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index e4823438e50170d6fea494b98b102f9a22dbaa42..0222ed92ea32761855a9252e4cd5c9f2fdf7c54d 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,12 @@ public class Turret extends SubsystemBase implements TurretIO{
 
                // calculate shortest angular delta
                double delta = best - lastRawSetpoint;
-               delta = MathUtil.angleModulus(delta);
                
                // filter delta
                double filteredDelta = setpointFilter.calculate(delta);
                
                // apply filtered range
-               lastFilteredRad = MathUtil.angleModulus(lastFilteredRad + filteredDelta);
+               lastFilteredRad += filteredDelta;
                lastRawSetpoint = best;
                best = lastFilteredRad;
 

diff --git a/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/robot/subsystems/turret/TurretConstants.java
index 27565841bb27e4cb1786e069f715a111de5110b5..f08df03e128da2304647e6d58b8f522ec95b60fd 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
-    public static double MIN_ANGLE = -250; // Deg
+    public static double MAX_ANGLE = 170; // Deg
+    public static double MIN_ANGLE = -215; // Deg
 
     public static double CALIBRATION_OFFSET = 0.0; // TODO: find this at hardstop