Improve shoot on the move

src/main/java/com/team6962/lib/math/TranslationalVelocity.java

@@ -2,6 +2,7 @@
 
 import static edu.wpi.first.units.Units.MetersPerSecond;
 import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
 
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
@@ -12,6 +13,7 @@
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.LinearVelocity;
 
 /**
@@ -95,6 +97,23 @@ public TranslationalVelocity(Matrix<?, N1> vector) {
     this.y = MetersPerSecond.of(vector.get(1, 0));
   }
 
+  /**
+   * Creates a TranslationalVelocity from a Translation2d object and an AngularVelocity. The
+   * translational velocity is calculated based on the tangential velocity induced by the angular
+   * velocity at the given translation from the center of rotation. This is useful for calculating
+   * the translational velocity of a point on the robot given its position relative to the center of
+   * rotation and the robot's angular velocity.
+   *
+   * @param translation The Translation2d representing the position of the point relative to the
+   *     center of rotation
+   * @param angularVelocity The AngularVelocity representing the robot's angular velocity
+   * @return The TranslationalVelocity representing the velocity of the point
+   */
+  public TranslationalVelocity(Translation2d translation, AngularVelocity angularVelocity) {
+    this.x = MetersPerSecond.of(-angularVelocity.in(RadiansPerSecond) * translation.getY());
+    this.y = MetersPerSecond.of(angularVelocity.in(RadiansPerSecond) * translation.getX());
+  }
+
   /**
    * Creates a TranslationalVelocity representing zero velocity. Note that this is equivalent to
    * TranslationalVelocity.ZERO, but creates a new instance.

src/main/java/frc/robot/auto/shoot/AutoShoot.java

@@ -369,6 +369,23 @@ public void log(String path) {
     }
   }
 
+  /**
+   * Calculates the shooter's translational velocity at the point of projectile exit, which is used
+   * to account for the effect of the shooter's movement on the projectile's trajectory. This is
+   * equal to the robot's translational velocity plus the tangential velocity at the shooter caused
+   * by the robot's angular velocity.
+   *
+   * @return the shooter's translational velocity at the point of projectile exit
+   */
+  private TranslationalVelocity calculateShooterVelocity() {
+    return swerveDrive
+        .getTranslationalVelocity()
+        .plus(
+            new TranslationalVelocity(
+                AutoShootConstants.shooterTransform.getTranslation(),
+                swerveDrive.getYawVelocity()));
+  }
+
   private ShootingParameters calculate(Time poseExtrapolationTime) {
     // Get the target position and initial shooter state
     Translation2d target = targetSupplier.get();
@@ -379,7 +396,7 @@ private ShootingParameters calculate(Time poseExtrapolationTime) {
     twist.dtheta *= poseExtrapolationTime.in(Seconds);
     Pose2d shooterPose =
         swerveDrive.getPosition2d().exp(twist).plus(AutoShootConstants.shooterTransform);
-    TranslationalVelocity shooterVelocity = swerveDrive.getTranslationalVelocity();
+    TranslationalVelocity shooterVelocity = calculateShooterVelocity();
 
     DogLog.log("AutoShoot/Distance", shooterPose.getTranslation().getDistance(target));
 
@@ -404,7 +421,7 @@ public void execute() {
     DogLog.log("AutoShoot/TargetY", target.getY());
 
     // Calculate the ideal shooting angles and roller speed to hit the target
-    ShootingParameters appliedShootingParameters = calculate(Seconds.of(0.03));
+    ShootingParameters appliedShootingParameters = calculate(Seconds.of(0.1));
     ShootingParameters currentShootingParameters = calculate(Seconds.of(0));
 
     appliedShootingParameters.log("AutoShoot/AppliedShootingParameters");