Make code build.

diff --git a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
index 25059589645b24871c1634537e338474738cd3b5..7b0e834f04d9078502d6d957d42a813910a28992 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
+++ b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
@@ -16,12 +16,12 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.constants.Constants;
 import frc.robot.constants.FieldConstants;
+import frc.robot.constants.ShotInterpolation;
 import frc.robot.subsystems.drivetrain.Drivetrain;
 import frc.robot.subsystems.hood.Hood;
 import frc.robot.subsystems.hood.HoodConstants;
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.spindexer.Spindexer;
-import frc.robot.constants.ShotInterpolation;
 import frc.robot.subsystems.turret.Turret;
 import frc.robot.subsystems.turret.TurretConstants;
 import frc.robot.util.ShooterPhysics;
@@ -70,6 +70,7 @@ public class AutoShootCommand extends Command {
         //drivepose  = new Pose2d(drivepose.getTranslation(), drivepose.getRotation().plus(new Rotation2d(Math.PI)));
 
         goalState = ShooterPhysics.getShotParams(
+                               Translation2d.kZero,
                                FieldConstants.getHubTranslation().minus(new Translation3d(drivepose.getTranslation())),
                                8.0); // Random initial goalState to prevent it being null
         
@@ -107,11 +108,13 @@ public class AutoShootCommand extends Command {
         for (int i = 0; i < 20; i++) {
             Translation3d lookahead3d = new Translation3d(lookaheadPose.getX(), lookaheadPose.getY(), TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getZ());
             Translation3d target3d = new Translation3d(target.getX(), target.getY(), FieldConstants.getHubTranslation().getZ()); // Add if statement so that it's only when it's shooting
-            goalState = ShooterPhysics.getShotParams(
+            var goalStateWithT = ShooterPhysics.getShotParamsWithT(
+                                       Translation2d.kZero,
                                target3d.minus(lookahead3d),
                                2.0);
+                       goalState = goalStateWithT.getFirst();
 
-            timeOfFlight = goalState.timeOfFlight();
+            timeOfFlight = goalStateWithT.getSecond();
             double offsetX = turretVelocityX * timeOfFlight;
             double offsetY = turretVelocityY * timeOfFlight;
             lookaheadPose =
@@ -207,4 +210,4 @@ public class AutoShootCommand extends Command {
         }
     }
 
-}
\ No newline at end of file
+}

diff --git a/src/main/java/frc/robot/util/ShooterPhysics.java b/src/main/java/frc/robot/util/ShooterPhysics.java
index bdc17d684c6debce1db3d3f1f75ec41ac1a50f91..0575981b11d4439cbed785b91e3b61a941317a52 100644 (file)
--- a/src/main/java/frc/robot/util/ShooterPhysics.java
+++ b/src/main/java/frc/robot/util/ShooterPhysics.java
@@ -62,6 +62,16 @@ public class ShooterPhysics {
                return cvtShot(exitVel, height);
        }
 
+       // provided for backwards compatability
+       public static Pair<TurretState, Double> getShotParamsWithT(Translation2d robotVelocity, Translation3d robotToTarget,
+                       double height) {
+               Translation3d exitVel = getRequiredExitVelocity(robotVelocity, robotToTarget, height);
+               double t = (-exitVel.getZ()
+                               - Math.sqrt(Math.pow(exitVel.getZ(), 2) - 2 * Constants.GRAVITY_ACCELERATION * robotToTarget.getZ()))
+                               / -Constants.GRAVITY_ACCELERATION;
+               return new Pair<TurretState, Double>(cvtShot(exitVel, height), t);
+       }
+
        /**
         * Calculates shot parameters for SOTM for a peak height using Physics™.
         *