From ba62b3b19f79658c053f9032bd1b1032278df229 Mon Sep 17 00:00:00 2001
From: Cameron Earle <camearle20@me.com>
Date: Sat, 10 Feb 2024 10:14:10 -0500
Subject: [PATCH] Update shoot while moving

---
 .../littletonrobotics/frc2024/RobotState.java | 53 +++++++++++--------
 1 file changed, 30 insertions(+), 23 deletions(-)

diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotState.java b/src/main/java/org/littletonrobotics/frc2024/RobotState.java
index 8071fb3c..75ade016 100644
--- a/src/main/java/org/littletonrobotics/frc2024/RobotState.java
+++ b/src/main/java/org/littletonrobotics/frc2024/RobotState.java
@@ -11,10 +11,14 @@
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import java.util.NoSuchElementException;
+import lombok.experimental.ExtensionMethod;
 import org.littletonrobotics.frc2024.subsystems.drive.DriveConstants;
 import org.littletonrobotics.frc2024.util.AllianceFlipUtil;
+import org.littletonrobotics.frc2024.util.GeomUtil;
+import org.littletonrobotics.frc2024.util.LoggedTunableNumber;
 import org.littletonrobotics.junction.AutoLogOutput;
 
+@ExtensionMethod({GeomUtil.class})
 public class RobotState {
     // Pose Estimation
     public record OdometryObservation(
@@ -25,6 +29,9 @@ public record VisionObservation(Pose2d visionPose, double timestamp, Matrix<N3,
     public record AimingParameters(
             Rotation2d driveHeading, double effectiveDistance, double radialFF) {}
 
+    private static final LoggedTunableNumber lookahead =
+            new LoggedTunableNumber("RobotState/lookaheadS", 0.0);
+
     private static final double poseBufferSizeSeconds = 2.0;
 
     private static RobotState instance;
@@ -147,29 +154,29 @@ public AimingParameters getAimingParameters() {
             return latestParameters;
         }
 
-        Pose2d robot = getEstimatedPose();
-        Twist2d fieldVelocity = fieldVelocity();
-
-        Translation3d originToGoal3d =
-                AllianceFlipUtil.apply(FieldConstants.Speaker.centerSpeakerOpening);
-        Translation2d originToGoal = new Translation2d(originToGoal3d.getX(), originToGoal3d.getY());
-        Translation2d originToRobot = robot.getTranslation();
-
-        // Get robot to goal angle but limit to reasonable range
-        Rotation2d robotToGoalAngle = originToGoal.minus(originToGoal).getAngle();
-        // Subtract goal to robot angle from field velocity
-        Translation2d tangentialVelocity =
-                new Translation2d(fieldVelocity.dx, fieldVelocity.dy)
-                        .rotateBy(robotToGoalAngle.unaryMinus());
-        // Subtract tangential velocity from goal to get virtual goal
-        Translation2d originToVirtualGoal = originToGoal.plus(tangentialVelocity.unaryMinus());
-
-        // Angle to virtual goal
-        Rotation2d driveHeading = originToVirtualGoal.minus(originToRobot).getAngle();
-        // Distance to virtual goal
-        double effectiveDistance = originToRobot.getDistance(originToVirtualGoal);
-        double radialFF = -tangentialVelocity.getX();
-        latestParameters = new AimingParameters(driveHeading, effectiveDistance, radialFF);
+        Transform2d fieldToTarget =
+                AllianceFlipUtil.apply(FieldConstants.Speaker.centerSpeakerOpening)
+                        .toTranslation2d()
+                        .toTransform2d();
+        Pose2d fieldToPredictedVehicle = getPredictedPose(lookahead.get(), lookahead.get());
+        Pose2d fieldToPredictedVehicleFixed =
+                new Pose2d(fieldToPredictedVehicle.getTranslation(), new Rotation2d());
+
+        Translation2d predictedVehicleToTargetTranslation =
+                fieldToPredictedVehicle.inverse().transformBy(fieldToTarget).getTranslation();
+        Translation2d predictedVehicleFixedToTargetTranslation =
+                fieldToPredictedVehicleFixed.inverse().transformBy(fieldToTarget).getTranslation();
+
+        Rotation2d vehicleToGoalDirection = predictedVehicleToTargetTranslation.getAngle();
+
+        Rotation2d targetVehicleDirection = predictedVehicleFixedToTargetTranslation.getAngle();
+        double targetDistance = predictedVehicleToTargetTranslation.getNorm();
+
+        double feedVelocity =
+                robotVelocity.dx * vehicleToGoalDirection.getSin() / targetDistance
+                        - robotVelocity.dy * vehicleToGoalDirection.getCos() / targetDistance;
+
+        latestParameters = new AimingParameters(targetVehicleDirection, targetDistance, feedVelocity);
         return latestParameters;
     }