From: Arnav495 <arnieincyberland@gmail.com>
Date: Tue, 24 Feb 2026 23:53:44 +0000 (-0800)
Subject: Add movement support.
X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=1549aef9da52ce7a6eb5729dfa349befcdd33b22;p=FRC2026.git

Add movement support.
---

diff --git a/src/main/java/frc/robot/commands/gpm/PhysicsAutoShoot.java b/src/main/java/frc/robot/commands/gpm/PhysicsAutoShoot.java
index 7b460cd..5693ade 100644
--- a/src/main/java/frc/robot/commands/gpm/PhysicsAutoShoot.java
+++ b/src/main/java/frc/robot/commands/gpm/PhysicsAutoShoot.java
@@ -4,7 +4,9 @@ import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.Constants;
 import frc.robot.constants.FieldConstants;
 import frc.robot.subsystems.drivetrain.Drivetrain;
 import frc.robot.subsystems.hood.Hood;
@@ -28,7 +30,7 @@ public class PhysicsAutoShoot extends Command {
 		this.drivetrain = drivetrain;
 
 		this.constraints = new ShooterPhysics.Constraints(2.2, ShooterConstants.SHOOTER_VELOCITY,
-				HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE);
+				Units.degreesToRadians(HoodConstants.MIN_ANGLE), Units.degreesToRadians(HoodConstants.MAX_ANGLE));
 
 		addRequirements(turret, hood, shooter);
 	}
@@ -46,13 +48,30 @@ public class PhysicsAutoShoot extends Command {
 				robotToTarget,
 				this.constraints);
 
-		if (stateOpt.isEmpty())
-			return;
-		ShooterPhysics.TurretState state = stateOpt.get();
+		// in one periodic
+		var futureStateOpt = ShooterPhysics.getConstrainedParams(
+				robotVelocity,
+				robotToTarget.plus(new Translation3d(robotVelocity.times(Constants.LOOP_TIME))),
+				this.constraints);
+
+		if (stateOpt.isPresent() && futureStateOpt.isPresent()) {
+			ShooterPhysics.TurretState state = stateOpt.get();
+			ShooterPhysics.TurretState futureState = futureStateOpt.get();
+
+			double yawSlope = (futureState.yaw().getRadians() - state.yaw().getRadians()) / Constants.LOOP_TIME;
+			double hoodSlope = (futureState.pitch() - state.pitch()) / Constants.LOOP_TIME;
+
+			turret.setFieldRelativeTarget(state.yaw(), yawSlope);
+			hood.setFieldRelativeTarget(new Rotation2d(state.pitch()), hoodSlope);
+			shooter.setShooter(state.exitVel());
+
+		} else if (stateOpt.isPresent() && futureStateOpt.isEmpty()) {
+			ShooterPhysics.TurretState state = stateOpt.get();
 
-		turret.setFieldRelativeTarget(state.yaw(), 0.0);
-		hood.setFieldRelativeTarget(new Rotation2d(state.pitch()), 0.0);
-		shooter.setShooter(state.exitVel());
+			turret.setFieldRelativeTarget(state.yaw(), 0.0);
+			hood.setFieldRelativeTarget(new Rotation2d(state.pitch()), 0.0);
+			shooter.setShooter(state.exitVel());
+		}
 	}
 
 	@Override