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

diff --cc src/main/java/frc/robot/subsystems/turret/Turret.java
index 6c7e45ca960589d4c45418737b013d4603e08c99,5650661efac1437f95d41c772adf2294f9b07326..23b929f586c5addc83a446eb60c8d4845309c33d
--- 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
@@@ -193,27 -207,13 +189,25 @@@ public class Turret extends SubsystemBa
                        }
                }
  
-               double currentPositionRad = getPositionRad();
-               double errorRad = best - currentPositionRad;
-               double absError = Math.abs(errorRad);
+               lastGoalRad = best;
  
 -              // apply filter to smooth setpoint
 -              double filteredSetpoint = setpointFilter.calculate(best);
 +              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(filteredSetpoint) * GEAR_RATIO;
 +              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;
 +              //      }
 +              // }
  
                motorGoalRotations = MathUtil.clamp(motorGoalRotations, Units.degreesToRotations(-180) * GEAR_RATIO, Units.degreesToRotations(180) * GEAR_RATIO);
                
@@@ -221,15 -221,13 +215,13 @@@
                // 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(feedforward));
+                       .withFeedForward(robotTurnCompensation));
  
-               Logger.recordOutput("Turret/InDeadband", isInDeadband);
+               Logger.recordOutput("Turret/FilteredSetpoint", Units.radiansToDegrees(filteredSetpoint));
  
                lastFrameVelocity = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble());