Merge branch 'arnavs-mega-velocityvoltage-turret' of https://github.com/iron-claw...

diff --cc src/main/java/frc/robot/subsystems/turret/Turret.java
index 5f04f6654a3d295f384551e1f1e8485d69af7004,9ecb52ed2dba7550f5a4d3b97fb731c848e78a42..63716f2a0cde55dd745d1689184cd967949550a5
--- 1/src/main/java/frc/robot/subsystems/turret/Turret.java
--- 2/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@@ -194,26 -207,39 +192,43 @@@ public class Turret extends SubsystemBa
                        }
                }
  
-               lastGoalRad = best;
+               double currentPositionRad = getPositionRad();
+               double errorRad = best - currentPositionRad;
+               double absError = Math.abs(errorRad);
  
 +              // if(Math.abs(best - lastGoalRad) < Math.PI/2){
 +              //      best = setpointFilter.calculate(best);
 +              // }
 +
 +              // Tells the Kraken to get to this position using 1000Hz profile
 +              double motorGoalRotations = Units.radiansToRotations(best) * GEAR_RATIO;
+               if (isInDeadband) {
+                       if (absError > START_THRESHOLD_RAD) {
+                               isInDeadband = false;
+                               lastGoalRad = best;
+                       }
+               } else {
+                       lastGoalRad = best;
+                       if (absError < STOP_THRESHOLD_RAD) {
+                               isInDeadband = true;
+                       }
+               }
  
 -              double motorGoalRotations = Units.radiansToRotations(lastGoalRad) * GEAR_RATIO;
 -
                motorGoalRotations = MathUtil.clamp(motorGoalRotations, Units.degreesToRotations(-180) * GEAR_RATIO, Units.degreesToRotations(180) * GEAR_RATIO);
                
                double acceleration = (goalVelocityRadPerSec - lastFrameVelocity)/Constants.LOOP_TIME;
                // Add the feedforward for the target velocity (SOTM) here as well
                double feedforwardVoltage = feedForward.calculate((goalVelocityRadPerSec) * GEAR_RATIO);
                
 -              double robotTurnCompensation = goalVelocityRadPerSec * 0.165;
 +              double robotTurnCompensation = goalVelocityRadPerSec * 0.185;
  
+               double feedforward = isInDeadband ? 0.0 : robotTurnCompensation;
+ 
                motor.setControl(mmVoltageRequest
                        .withPosition(motorGoalRotations)
-                       .withFeedForward(robotTurnCompensation));
+                       .withFeedForward(feedforward));
+ 
+               Logger.recordOutput("Turret/InDeadband", isInDeadband);
  
                lastFrameVelocity = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble());