Add arm angle to AimingParameters

Mechanical-Advantage · Feb 11, 2024 · 0beef02 · 0beef02
1 parent 6acf6ce
commit 0beef02
Showing 1 changed file with 13 additions and 3 deletions.
diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotState.java b/src/main/java/org/littletonrobotics/frc2024/RobotState.java
@@ -26,12 +26,14 @@ public record OdometryObservation(
 
     public record VisionObservation(Pose2d visionPose, double timestamp, Matrix<N3, N1> stdDevs) {}
 
-    public record AimingParameters(
-            Rotation2d driveHeading, double effectiveDistance, double radialFF) {}
+    public record AimingParameters(Rotation2d driveHeading, Rotation2d armAngle, double radialFF) {}
 
     private static final LoggedTunableNumber lookahead =
             new LoggedTunableNumber("RobotState/lookaheadS", 0.0);
 
+    private static LoggedTunableNumber shotHeightCompensation =
+            new LoggedTunableNumber("Superstructure/CompensationInches", 6.0);
+
     private static final double poseBufferSizeSeconds = 2.0;
 
     private static RobotState instance;
@@ -176,7 +178,15 @@ public AimingParameters getAimingParameters() {
                 robotVelocity.dx * vehicleToGoalDirection.getSin() / targetDistance
                         - robotVelocity.dy * vehicleToGoalDirection.getCos() / targetDistance;
 
-        latestParameters = new AimingParameters(targetVehicleDirection, targetDistance, feedVelocity);
+        latestParameters =
+                new AimingParameters(
+                        targetVehicleDirection,
+                        new Rotation2d(
+                                targetDistance - SuperstructureConstants.ArmConstants.armOrigin.getX(),
+                                FieldConstants.Speaker.centerSpeakerOpening.getZ()
+                                        - SuperstructureConstants.ArmConstants.armOrigin.getY()
+                                        + shotHeightCompensation.get()),
+                        feedVelocity);
         return latestParameters;
     }