From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com>
Date: Mon, 16 Feb 2026 18:59:41 +0000 (-0800)
Subject: Merge branch 'arnavs-mega-velocityvoltage-turret' of https://github.com/iron-claw... 
X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=5e539f6382a14da6a7d3e21cb84ebff148940266;p=FRC2026.git

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

5e539f6382a14da6a7d3e21cb84ebff148940266
diff --cc src/main/java/frc/robot/subsystems/turret/Turret.java
index 5f04f66,9ecb52e..63716f2
--- a/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());