Adjust rpm during shoot on the move

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

@@ -49,7 +49,7 @@ public static final class FeatureFlags {
         public static final boolean runLocalizer = true;
 
         public static final boolean runIntake = true;
-        public static final boolean runScoring = false;
+        public static final boolean runScoring = true;
         public static final boolean runEndgame = false;
         public static final boolean runDrive = true;
     }
@@ -477,6 +477,15 @@ public static HashMap<Double, Double> getShooterMap() { // TODO: Find this
             return map;
         }
 
+        // Key - Velocity toward goal in meters per second
+        // Value - Shooter RPM to subtract
+        public static HashMap<Double, Double> getShooterVelocityToRpmMap() { // TODO: Find this
+            HashMap<Double, Double> map = new HashMap<Double, Double>();
+            map.put(0.0, 0.0);
+            map.put(10.0, 10.0);
+            return map;
+        }
+
         // NOTE - This should be monotonically increasing
         // Key - Distance in meters
         // Value - Time in seconds

src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java

@@ -18,6 +18,7 @@
 import frc.robot.Constants.ScoringConstants;
 import frc.robot.utils.FieldFinder;
 import frc.robot.utils.FieldFinder.FieldLocations;
+import frc.robot.utils.GeomUtil;
 import frc.robot.utils.InterpolateDouble;
 import java.util.function.Supplier;
 import org.littletonrobotics.junction.Logger;
@@ -41,6 +42,7 @@ public class ScoringSubsystem extends SubsystemBase {
     private Supplier<Boolean> driveAllignedSupplier = () -> true;
 
     private final InterpolateDouble shooterInterpolated;
+    private final InterpolateDouble shooterVelocityToRpmInterpolated;
     private final InterpolateDouble aimerInterpolated;
     private final InterpolateDouble timeToPutAimDown;
 
@@ -87,6 +89,8 @@ public ScoringSubsystem(ShooterIO shooterIo, AimerIO aimerIo, HoodIO hoodIo) {
         this.hoodIo = hoodIo;
 
         shooterInterpolated = new InterpolateDouble(ScoringConstants.getShooterMap());
+        shooterVelocityToRpmInterpolated =
+                new InterpolateDouble(ScoringConstants.getShooterVelocityToRpmMap());
 
         aimerInterpolated =
                 new InterpolateDouble(
@@ -105,6 +109,14 @@ private void idle() {
         shooterIo.setKickerVolts(0);
         hoodIo.setHoodAngleRad(0);
 
+        // TODO: Use actual data from findVelocityTowardPoint to adjust RPM, instead of just logging
+        Logger.recordOutput(
+                "scoring/velocityToGoal",
+                GeomUtil.findVelocityTowardPoint(
+                        velocitySupplier.get(),
+                        GeomUtil.poseToTranslation(poseSupplier.get()),
+                        speakerSupplier.get()));
+
         Logger.recordOutput("scoring/aimGoal", 0.0);
 
         if (!hasNote() && action == ScoringAction.INTAKE) {
@@ -137,8 +149,16 @@ private void intake() {
 
     private void prime() {
         double distancetoGoal = findDistanceToGoal();
+        double velocityToGoal =
+                GeomUtil.findVelocityTowardPoint(
+                        velocitySupplier.get(),
+                        GeomUtil.poseToTranslation(poseSupplier.get()),
+                        speakerSupplier.get());
+
         Logger.recordOutput("scoring/aimGoal", aimerInterpolated.getValue(distancetoGoal));
-        shooterIo.setShooterVelocityRPM(shooterInterpolated.getValue(distancetoGoal));
+        shooterIo.setShooterVelocityRPM(
+                shooterInterpolated.getValue(distancetoGoal)
+                        - shooterVelocityToRpmInterpolated.getValue(velocityToGoal));
         aimerIo.setAimAngleRad(aimerInterpolated.getValue(distancetoGoal), false);
 
         boolean shooterReady =

src/main/java/frc/robot/utils/GeomUtil.java

@@ -7,6 +7,7 @@
 // license that can be found in the LICENSE file at
 // the root directory of this project.
 
+import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -15,6 +16,7 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.numbers.N2;
 
 /** Geometry utilities for working with translations, rotations, transforms, and poses. */
 public class GeomUtil {
@@ -80,6 +82,16 @@ public static Pose2d translationToPose(Translation2d translation) {
         return new Pose2d(translation, new Rotation2d());
     }
 
+    /**
+     * Creates a translation from the x and y of a pose, ignoring rotation
+     *
+     * @param pose The pose to create the translation with
+     * @return The resulting translation
+     */
+    public static Translation2d poseToTranslation(Pose2d pose) {
+        return new Translation2d(pose.getX(), pose.getY());
+    }
+
     /**
      * Creates a pure rotated pose
      *
@@ -141,4 +153,39 @@ public static Translation2d translation3dTo2dXY(Translation3d translation) {
     public static Translation2d translation3dTo2dXZ(Translation3d translation) {
         return new Translation2d(translation.getX(), translation.getZ());
     }
+
+    /**
+     * Find the velocity of an object toward a target point
+     *
+     * @param velocity The velocity of the object
+     * @param currentTranslation The current position of the object
+     * @param targetTranslation The position of the target point
+     * @return The velocity toward the target point (a negative number if object is moving away)
+     */
+    public static double findVelocityTowardPoint(
+            Vector<N2> velocity,
+            Translation2d currentTranslation,
+            Translation2d targetTranslation) {
+        // Calculate the velocity to the goal based on this formula
+        // https://web.ma.utexas.edu/users/m408m/Display12-3-4.shtml
+
+        // Vector from current to target (translate target so that current is now 0, 0)
+        Translation2d currentToTarget = targetTranslation.minus(currentTranslation);
+
+        // Get the angle of the vector from current to target
+        Rotation2d targetAngle =
+                Rotation2d.fromRadians(Math.atan2(currentToTarget.getY(), currentToTarget.getX()));
+
+        // Get the angle of the velocity vector
+        Rotation2d velocityAngle =
+                Rotation2d.fromRadians(Math.atan2(velocity.get(1, 0), velocity.get(0, 0)));
+
+        // Calculate theta, the angle between the speakerAngle and the current velocity angle
+        Rotation2d theta = velocityAngle.minus(targetAngle);
+        // Project the velocity vector onto the speaker vector with ||u|| * cos(theta)
+        // Where u is the velocity vector
+        double velocityToGoal = velocity.norm() * theta.getCos();
+
+        return velocityToGoal;
+    }
 }