Shooting while moving

build.gradle

@@ -146,6 +146,9 @@ tasks.withType(JavaCompile) {
 
 javadoc {
     configure(options) {
+        options.encoding = 'UTF-8'
+        options.docEncoding = 'UTF-8'
+        options.charSet = 'UTF-8'
         options.addBooleanOption("-allow-script-in-comments",true)
         options.addStringOption("link", "https://github.wpilib.org/allwpilib/docs/release/java/")
         options.header = "<script type=\"text/javascript\" async" +

src/generate/java/frc/gen/GenerateLUTs.java

@@ -175,7 +175,7 @@ public static void main(String[] argv) {
 
         double maxGroundDistance = 0.0;
         for (double flywheel = maxFlywheelSpeed + 2.0; flywheel < 90.0; flywheel += 2.0) {
-            var hoodAngle = Degrees.of(90 - 12.695 - 25);
+            var hoodAngle = Degrees.of(90 - Constants.AdjustableHood.HoodOffset - 25);
             var exitVelocity =
                 MetersPerSecond.of(olsRes.evaluate(new Tuple2<AngularVelocity, LinearVelocity>(
                     RotationsPerSecond.of(flywheel), null)));
@@ -204,7 +204,7 @@ public static void main(String[] argv) {
         }
 
         for (double flywheel = maxFlywheelSpeed + 2.0; flywheel < 90.0; flywheel += 2.0) {
-            var hoodAngle = Degrees.of(90 - 12.695 - 35);
+            var hoodAngle = Degrees.of(90 - Constants.AdjustableHood.HoodOffset - 35);
             var exitVelocity =
                 MetersPerSecond.of(olsRes.evaluate(new Tuple2<AngularVelocity, LinearVelocity>(
                     RotationsPerSecond.of(flywheel), null)));

src/main/java/frc/robot/Constants.java

@@ -48,7 +48,7 @@ public final class Constants {
 
     public static final boolean tunable = true;
 
-    public static final boolean keepInField = true;
+    public static final boolean keepInField = false;
 
     public static final int odometryQueueSize = 100;
 
@@ -382,6 +382,7 @@ public static final class Vision {
     public static final class AdjustableHood {
         public static final int HoodMotorID = 11;
 
+        public static final double HoodOffset = 12.695;
         /* PID Values */
         /** Proportional PID Value for hood position control. */
         public static final double KP = 200.0;
@@ -599,11 +600,17 @@ public static final class Shooter {
         /** Height at which the ball leaves the shooter. */
         public static final Distance shooterHeight = Inches.of(20);
 
+        public static final AngularVelocity maxFlywheelSpeed = RotationsPerSecond.of(90);
+
         /** ID for Shooter Motor 1 */
         public static final int motor1ID = 10;
         /** ID for Shooter Motor 2 */
         public static final int motor2ID = 12;
 
+        /** Flywheel speed constant */
+        public static final double transferCoeff = .43;
+        public static final double k = transferCoeff * 2 * Math.PI * Units.inchesToMeters(2);
+
         /** Motor Invert for Shooter Motors */
         public static final InvertedValue shooterMotorInvert = InvertedValue.Clockwise_Positive;
         /** Motor Alignment for Shooter Motors */

src/main/java/frc/robot/math/ShootOnMove.java

@@ -0,0 +1,122 @@
+package frc.robot.math;
+
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+import org.littletonrobotics.junction.Logger;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.LinearVelocity;
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.Constants;
+import frc.robot.shotdata.ShotData;
+import frc.robot.shotdata.ShotData.ShotParams;
+
+/** Shoot while moving */
+public class ShootOnMove {
+
+    /** Records moving shot */
+    public static record MovingShot(Rotation2d turretAngleFieldRelative, Angle pitch,
+        LinearVelocity exitSpeed, boolean feasible) {
+    }
+
+    static final Translation2d SHOOTER_OFFSET = new Translation2d(-0.155575, -0.13335);
+    static final double LATENCY_SEC = 0.0; // tune: vision + feed + spin-up latency
+
+    static final Angle MIN_PITCH = Degrees
+        .of(90 - Constants.AdjustableHood.HoodOffset - Constants.AdjustableHood.maxHoodAngleDeg);
+    static final Angle MAX_PITCH = Degrees.of(90 - Constants.AdjustableHood.HoodOffset);
+    static final LinearVelocity MAX_EXIT_SPEED = MetersPerSecond
+        .of(Constants.Shooter.maxFlywheelSpeed.in(RotationsPerSecond) * Constants.Shooter.k);
+
+    /** solve moving shot */
+    public static MovingShot solveMovingShot(Pose2d robotPose, ChassisSpeeds fieldSpeeds,
+        Translation2d hub, double flywheelSpeed, double trimUp, double trimLeft) {
+        double vx = fieldSpeeds.vxMetersPerSecond;
+        double vy = fieldSpeeds.vyMetersPerSecond;
+        double omega = fieldSpeeds.omegaRadiansPerSecond;
+        double currentFlywheelSpeed = flywheelSpeed;
+
+        // 1. Latency compensation: predict where the robot is when the ball leaves
+        Pose2d pose = new Pose2d(
+            robotPose.getTranslation().plus(new Translation2d(vx, vy).times(LATENCY_SEC)),
+            robotPose.getRotation().plus(new Rotation2d(omega * LATENCY_SEC)));
+
+        // 2. Shooter position and velocity in the field frame
+        // v_shooter = v_center + omega x r
+        Translation2d offsetField = SHOOTER_OFFSET.rotateBy(pose.getRotation());
+        Translation2d shooterPos = pose.getTranslation().plus(offsetField);
+        double svx = vx - omega * offsetField.getY();
+        double svy = vy + omega * offsetField.getX();
+
+        // 3. Radial/tangential frame about the hub
+        Translation2d toHub = hub.minus(shooterPos);
+        double d = toHub.getNorm() + trimUp;
+        Logger.recordOutput("ShootOnMove/d", d);
+        Logger.recordOutput("ShootOnMove/time", Timer.getFPGATimestamp());
+
+        Rotation2d rHat = toHub.getAngle();
+        double cosR = rHat.getCos();
+        double sinR = rHat.getSin();
+
+        double vrRobot = svx * cosR + svy * sinR; // + toward hub
+        double vtRobot = -svx * sinR + svy * cosR; // + is CCW (r-hat rotated +90°)
+        double vzRobot = 0.0; // assume flat field
+
+        // 4. Stationary solution -> required field-relative ball velocity
+        ShotParams s = ShotData.staticShotParameters(d, currentFlywheelSpeed);
+        Logger.recordOutput("ShootOnMove/s", s);
+
+        double stationaryPitch = s.pitch().in(Radians);
+        Logger.recordOutput("ShootOnMove/stationaryPitch", stationaryPitch);
+
+        double stationarySpeed = s.exitSpeed().in(MetersPerSecond);
+        Logger.recordOutput("ShootOnMove/stationarySpeed", stationarySpeed);
+
+        double vrTarget = stationarySpeed * Math.cos(stationaryPitch);
+        Logger.recordOutput("ShootOnMove/vrTarget", vrTarget);
+
+        double vzTarget = stationarySpeed * Math.sin(stationaryPitch);
+        Logger.recordOutput("ShootOnMove/vzTarget", vzTarget);
+
+        // 5. Robot-relative launch vector = target field velocity - shooter velocity
+        double a = vrTarget - vrRobot; // radial
+        Logger.recordOutput("ShootOnMove/aRadial", a);
+
+        double b = -vtRobot; // tangential (target v_t is 0)
+        Logger.recordOutput("ShootOnMove/b", b);
+
+        double c = vzTarget - vzRobot; // vertical
+        Logger.recordOutput("ShootOnMove/c", c);
+
+
+        // 6. Convert to spherical coordinates
+        double h = Math.hypot(a, b); // horizontal launch speed, >= 0
+        Logger.recordOutput("ShootOnMove/h", h);
+
+        double deltaYaw = Math.atan2(b, a);
+        Logger.recordOutput("ShootOnMove/deltaYaw", deltaYaw);
+
+        Angle pitch = Radians.of(Math.atan2(c, h));
+        Logger.recordOutput("ShootOnMove/pitch", pitch);
+
+        LinearVelocity exitSpeed = MetersPerSecond.of(Math.sqrt(h * h + c * c));
+        Logger.recordOutput("ShootOnMove/exitSpeed", exitSpeed);
+
+        // 7. Turret angle: field heading, then robot-relative
+        Rotation2d turretField =
+            rHat.plus(Rotation2d.fromRadians(deltaYaw)).plus(Rotation2d.fromDegrees(trimLeft));
+        Logger.recordOutput("ShootOnMove/turretField", turretField);
+
+        // 8. Feasibility against mechanism limits
+        boolean feasible =
+            pitch.gte(MIN_PITCH) && pitch.lte(MAX_PITCH) && exitSpeed.lte(MAX_EXIT_SPEED);
+        Logger.recordOutput("ShootOnMove/feasible", feasible);
+
+        return new MovingShot(turretField, pitch, exitSpeed, feasible);
+    }
+}

src/main/java/frc/robot/shotdata/ShotData.java

@@ -120,7 +120,8 @@ public static record ShotEntry(Distance targetDistance, AngularVelocity flywheel
         public ShotEntry(double distanceFeet, double flywheelSpeed, double hoodAngleDeg,
             double tof) {
             this(Feet.of(distanceFeet), RotationsPerSecond.of(flywheelSpeed),
-                Degrees.of(90 - 12.695 - hoodAngleDeg), MetersPerSecond.of(0.0), Seconds.of(tof));
+                Degrees.of(90 - Constants.AdjustableHood.HoodOffset - hoodAngleDeg),
+                MetersPerSecond.of(0.0), Seconds.of(tof));
         }
 
         /**
@@ -196,7 +197,15 @@ public LinearVelocity speedTransferExitVelocity() {
          * @return hood angle in degrees
          */
         public Angle hoodAngle() {
-            return Degrees.of(90 - 12.695 - exitAngle.in(Degrees));
+            return Degrees.of(90 - Constants.AdjustableHood.HoodOffset - exitAngle.in(Degrees));
+        }
+
+        public Angle pitchAngle(Angle currentAngle) {
+            return Degrees.of(90 - Constants.AdjustableHood.HoodOffset - currentAngle.in(Degrees));
+        }
+
+        public LinearVelocity exitVelocity() {
+            return MetersPerSecond.of(flywheelSpeed().in(RotationsPerSecond) * Constants.Shooter.k);
         }
     }
 
@@ -273,6 +282,15 @@ public static record ShotParameters(double desiredSpeed, double hoodAngleDeg,
         double timeOfFlight, boolean isOkayToShoot) {
     }
 
+    /**
+     * Encapsolates the computed shooter parameters for a single shot
+     * 
+     * @param pitch the angle of the shot
+     * @param exitSpeed the speed in meters per second that the ball is exiting the hoot.
+     */
+    public static record ShotParams(Angle pitch, LinearVelocity exitSpeed) {
+    }
+
     /**
      * Computes shooter parameters for a hub shot given the current robot state.
      *
@@ -303,6 +321,17 @@ public static ShotParameters getShotParameters(double distance, double currentFl
         return new ShotParameters(desiredSpeed, hoodAngleDeg, tof, isOkay);
     }
 
+    /** computes shooter parameters for a hub shot */
+    public static ShotParams staticShotParameters(double distance, double flywheelSpeed) {
+        var res = shotMap.get(distance);
+        double desiredSpeed = res.flywheelSpeed().in(RotationsPerSecond) + 1;
+        LinearVelocity exitSpeed =
+            MetersPerSecond.of(res.flywheelSpeed().in(RotationsPerSecond) * Constants.Shooter.k);
+        double hoodAngle = res.hoodAngle().in(Degrees);
+        Angle pitch = Degrees.of(90 - Constants.AdjustableHood.HoodOffset - hoodAngle);
+        return new ShotParams(pitch, exitSpeed);
+    }
+
     /**
      * Computes shooter parameters for a ground pass given the current robot state.
      *

src/main/java/frc/robot/subsystems/adjustable_hood/AdjustableHood.java

@@ -48,10 +48,10 @@ public void setTargetAngle(Angle setAngle) {
     }
 
     public Command setGoal(Angle setAngle) {
-        return runOnce(() -> io.setTargetAngle(setAngle));
+        return run(() -> io.setTargetAngle(setAngle));
     }
 
     public Command setGoal(Supplier<Angle> setAngle) {
-        return runOnce(() -> io.setTargetAngle(setAngle.get()));
+        return run(() -> io.setTargetAngle(setAngle.get()));
     }
 }

src/main/java/frc/robot/subsystems/shooter/TargetingState.java

@@ -1,5 +1,7 @@
 package frc.robot.subsystems.shooter;
 
+import static edu.wpi.first.units.Units.Degree;
+import static edu.wpi.first.units.Units.MetersPerSecond;
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
 import org.littletonrobotics.junction.Logger;
@@ -8,7 +10,10 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.util.Units;
+import frc.robot.Constants;
 import frc.robot.FieldConstants;
+import frc.robot.math.ShootOnMove;
+import frc.robot.math.ShootOnMove.MovingShot;
 import frc.robot.shotdata.ShotData;
 import frc.robot.util.AllianceFlipUtil;
 
@@ -116,7 +121,25 @@ public void updateTargeting() {
 
         Translation2d adjustedTarget = shootingTarget;
 
-        if (currentFlywheelSpeed > 10.0) {
+        boolean moveAndShoot = true;
+
+        if (currentFlywheelSpeed > 10.0 && moveAndShoot == true) {
+            Pose2d robotPosition = poseSource.get();
+            ChassisSpeeds robotSpeed = speedSource.get(); // Field relative
+            Translation2d hub = shootingTarget;
+
+            MovingShot movingShot = ShootOnMove.solveMovingShot(robotPosition, robotSpeed, hub,
+                currentFlywheelSpeed, trimUp, trimLeft);
+
+            desiredFlywheelSpeed = movingShot.exitSpeed().in(MetersPerSecond) / Constants.Shooter.k;
+            desiredHoodAngleDeg = targetIsGround ? 30.0
+                : 90 - Constants.AdjustableHood.HoodOffset - movingShot.pitch().in(Degree);
+            okayToShoot = movingShot.feasible() && currentFlywheelSpeed > desiredFlywheelSpeed - 6;
+
+            desiredTurretHeadingFieldRelative = movingShot.turretAngleFieldRelative();
+
+            Logger.recordOutput("State/Trim/TrimUp", trimUp);
+            Logger.recordOutput("State/Trim/TrimLeft", trimLeft);
 
             /*
              * for (int i = 0; i < 5; i++) { double distance =
@@ -135,37 +158,39 @@ public void updateTargeting() {
              */
         } else {
             adjustedTarget = AllianceFlipUtil.apply(FieldConstants.Hub.centerHub);
-        }
 
-        Logger.recordOutput("State/AdjustedShootingTarget", adjustedTarget);
+            Logger.recordOutput("State/AdjustedShootingTarget", adjustedTarget);
+
+            double distance = adjustedTarget.getDistance(turretCenter) + Units.feetToMeters(trimUp);
 
-        double distance = adjustedTarget.getDistance(turretCenter) + Units.feetToMeters(trimUp);
+            Logger.recordOutput("State/distance", distance);
 
-        Logger.recordOutput("State/distance", distance);
+            var parameters =
+                targetIsGround ? ShotData.getPassParameters(distance, currentFlywheelSpeed, false)
+                    : ShotData.getShotParameters(distance, currentFlywheelSpeed, true);
 
-        var parameters =
-            targetIsGround ? ShotData.getPassParameters(distance, currentFlywheelSpeed, false)
-                : ShotData.getShotParameters(distance, currentFlywheelSpeed, true);
+            desiredFlywheelSpeed = parameters.desiredSpeed();
+            desiredHoodAngleDeg = targetIsGround ? 30.0 : parameters.hoodAngleDeg();
+            okayToShoot = parameters.isOkayToShoot();
 
-        desiredFlywheelSpeed = parameters.desiredSpeed();
-        desiredHoodAngleDeg = targetIsGround ? 30.0 : parameters.hoodAngleDeg();
-        okayToShoot = parameters.isOkayToShoot();
+            desiredTurretHeadingFieldRelative = adjustedTarget.minus(turretCenter).getAngle()
+                .plus(Rotation2d.fromDegrees(trimLeft));
 
-        desiredTurretHeadingFieldRelative =
-            adjustedTarget.minus(turretCenter).getAngle().plus(Rotation2d.fromDegrees(trimLeft));
+            Logger.recordOutput("State/desiredTurretHeading", desiredTurretHeadingFieldRelative);
 
-        Logger.recordOutput("State/desiredTurretHeading", desiredTurretHeadingFieldRelative);
+            Logger.recordOutput("State/Trim/TrimUp", trimUp);
+            Logger.recordOutput("State/Trim/TrimLeft", trimLeft);
 
-        Logger.recordOutput("State/Trim/TrimUp", trimUp);
-        Logger.recordOutput("State/Trim/TrimLeft", trimLeft);
+            Translation2d[] turretDirection = new Translation2d[2];
 
-        Translation2d[] turretDirection = new Translation2d[2];
+            turretDirection[0] = turretCenter;
+            turretDirection[1] =
+                turretCenter.plus(new Translation2d(2.0, desiredTurretHeadingFieldRelative));
+
+            Logger.recordOutput("State/DesiredTurretDirection", turretDirection);
+        }
 
-        turretDirection[0] = turretCenter;
-        turretDirection[1] =
-            turretCenter.plus(new Translation2d(2.0, desiredTurretHeadingFieldRelative));
 
-        Logger.recordOutput("State/DesiredTurretDirection", turretDirection);
     }
 
     public double getDesiredFlywheelSpeed() {