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 7b460cda392889d7cf8839b05b724422953f2bb2..5693ade3bca294ca466fccd23200978862d3fbcd 100644 (file)
--- 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