Copy changes from shooter-fall-project.

diff --git a/src/main/java/frc/robot/util/ShooterPhysics.java b/src/main/java/frc/robot/util/ShooterPhysics.java
index 3cf51d10517c2492c814ea9c7e498f56d7de35ff..d66eadcff8be0f3ab3d7b5209a84a84ea105856d 100644 (file)
--- a/src/main/java/frc/robot/util/ShooterPhysics.java
+++ b/src/main/java/frc/robot/util/ShooterPhysics.java
@@ -17,14 +17,13 @@ public class ShooterPhysics {
         * Calculates shot parameters for SOTM using Physics™.
         *
         * @param robotVelocity The x and y velocity of the robot, field relative.
-        * @param robot         The position of the center of the robot, field relative.
-        * @param target        The position of the target, field relative.
+        * @param robotToTarget The robot to target transform. Angles are field
+        *                      relative, positions are with the robot as the origin.
         * @param height        The peak height the trajectory should reach.
         * @return A TurretState that represents the shot the robot should take.
         */
-       public static TurretState getShotParams(Translation2d robotVelocity, Translation2d robot, Translation3d target,
+       public static TurretState getShotParams(Translation2d robotVelocity, Translation3d robotToTarget,
                        double height) {
-               Translation3d robotToTarget = target.minus(new Translation3d(robot));
                Translation3d exitVel = getRequiredExitVelocity(robotVelocity, robotToTarget, height);
 
                Translation2d onGround = exitVel.toTranslation2d();
@@ -38,6 +37,27 @@ public class ShooterPhysics {
                return new TurretState(yaw, pitch, speed);
        }
 
+       public static Optional<TurretState> getShotParamsBySpeed(Translation2d robotVelocity, Translation2d robot,
+                       Translation3d target,
+                       double speed) {
+               Translation3d robotToTarget = target.minus(new Translation3d(robot));
+               Optional<Translation3d> exitVelOpt = getExitVelocityForSpeed(robotVelocity, robotToTarget, speed);
+
+               if (exitVelOpt.isEmpty())
+                       return Optional.empty();
+               Translation3d exitVel = exitVelOpt.get();
+
+               Translation2d onGround = exitVel.toTranslation2d();
+               Rotation2d yaw = onGround.getAngle();
+               double magnitude2d = onGround.getNorm();
+
+               double pitch = new Translation2d(magnitude2d, exitVel.getZ()).getAngle().getRadians();
+               pitch %= Math.PI * 2;
+               double speed2 = exitVel.getDistance(Translation3d.kZero);
+
+               return Optional.of(new TurretState(yaw, pitch, speed2));
+       }
+
        /**
         * Actually does the SOTM math. Assumes shots are from (0, 0, 0). This is only
         * public so it can be unit tested, and shouldn't be called directly.
@@ -102,7 +122,19 @@ public class ShooterPhysics {
        public static Optional<Translation3d> getExitVelocityForSpeed(Translation2d initialVelocity, Translation3d target,
                        double speed, double tolerance) {
 
-               // TODO: detect when the given velocity is insufficient and exit before maxIters
+               // calculate minimum velocity: v² = g * (R + √(R² + h²))
+               double horizontalDist = target.toTranslation2d().getNorm();
+               double verticalDist = target.getZ();
+               double g = Constants.GRAVITY_ACCELERATION;
+               double robotSpeed = initialVelocity.getNorm();
+
+               double minProjectileSpeed = Math
+                               .sqrt(g * (horizontalDist + Math.sqrt(horizontalDist * horizontalDist + verticalDist * verticalDist)));
+               double minSpeed = Math.max(0, minProjectileSpeed - robotSpeed);
+
+               if (speed < minSpeed - tolerance) {
+                       return Optional.empty();
+               }
 
                // guess a peak height
                double guess = target.getZ() + 2;