From 9b0858f568fecb0b9547a8b542c5b9ffde303e78 Mon Sep 17 00:00:00 2001
From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com>
Date: Sat, 14 Feb 2026 12:53:30 -0800
Subject: [PATCH] Update AutoShootCommand.java

---
 src/main/java/frc/robot/commands/gpm/AutoShootCommand.java | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
index 6117761..89507cd 100644
--- a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
+++ b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
@@ -84,6 +84,8 @@ public class AutoShootCommand extends Command {
 
     private Translation2d target = FieldConstants.getHubTranslation().toTranslation2d();
 
+    private double velocityAdjustment = 0;
+
     public AutoShootCommand(Turret turret, Drivetrain drivetrain, Hood hood, Shooter shooter, Spindexer spindexer) {
         this.turret = turret;
         this.drivetrain = drivetrain;
@@ -177,6 +179,10 @@ public class AutoShootCommand extends Command {
         hoodSetpoint = MathUtil.clamp(Units.radiansToDegrees(hoodAngle), HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE);
         hoodVelocity = hoodAngleFilter.calculate((hoodAngle - lastHoodAngle) / Constants.LOOP_TIME);
         lastHoodAngle = hoodAngle;
+
+        if (Math.abs(lastTurretAngle.getDegrees() - turretSetpoint) > 90) {
+            velocityAdjustment = -drivetrain.getAngularRate(2) * 1.4;
+        }
     }
 
     public void updateDrivePose(){
@@ -198,6 +204,7 @@ public class AutoShootCommand extends Command {
     public void execute() {
         updateDrivePose();
         updateSetpoints(drivepose);
+
         turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity - drivetrain.getAngularRate(2));
         //hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(hoodSetpoint)), hoodVelocity);
         //shooter.setShooter(goalState.exitVel());
-- 
2.39.5