From 7818a283ac380cd5d69a7b6222a59a5705e7ff08 Mon Sep 17 00:00:00 2001
From: suryatho <suryathoppae@gmail.com>
Date: Wed, 14 Feb 2024 23:08:01 -0500
Subject: [PATCH] Use interpolating tree map for calculating arm angle

---
 .../littletonrobotics/frc2024/RobotState.java | 42 +++++++++++--------
 1 file changed, 25 insertions(+), 17 deletions(-)

diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotState.java b/src/main/java/org/littletonrobotics/frc2024/RobotState.java
index f9f22714..263aa764 100644
--- a/src/main/java/org/littletonrobotics/frc2024/RobotState.java
+++ b/src/main/java/org/littletonrobotics/frc2024/RobotState.java
@@ -11,16 +11,17 @@
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.*;
+import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
 import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveDriveWheelPositions;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.util.Units;
 import java.util.NoSuchElementException;
 import lombok.experimental.ExtensionMethod;
 import org.littletonrobotics.frc2024.subsystems.drive.DriveConstants;
-import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmConstants;
 import org.littletonrobotics.frc2024.util.AllianceFlipUtil;
 import org.littletonrobotics.frc2024.util.GeomUtil;
 import org.littletonrobotics.frc2024.util.LoggedTunableNumber;
@@ -29,7 +30,6 @@
 
 @ExtensionMethod({GeomUtil.class})
 public class RobotState {
-  // Pose Estimation
   public record OdometryObservation(
       SwerveDriveWheelPositions wheelPositions, Rotation2d gyroAngle, double timestamp) {}
 
@@ -41,8 +41,17 @@ public record AimingParameters(
   private static final LoggedTunableNumber lookahead =
       new LoggedTunableNumber("RobotState/lookaheadS", 0.0);
 
-  private static LoggedTunableNumber shotHeightCompensation =
-      new LoggedTunableNumber("RobotState/CompensationMeters", 0.55);
+  private static final LoggedTunableNumber shotHeightCompensation =
+      new LoggedTunableNumber("RobotState/CompensationMeters", 0.05);
+
+  /** Arm angle look up table key: meters, values: radians */
+  private static final InterpolatingDoubleTreeMap armAngleMap = new InterpolatingDoubleTreeMap();
+
+  static {
+    armAngleMap.put(0.0, Units.degreesToRadians(90.0));
+    armAngleMap.put(10.0, Units.degreesToRadians(15.0));
+    armAngleMap.put(Double.MAX_VALUE, Units.degreesToRadians(15.0));
+  }
 
   private static final double poseBufferSizeSeconds = 2.0;
 
@@ -59,7 +68,7 @@ public static RobotState getInstance() {
   private final TimeInterpolatableBuffer<Pose2d> poseBuffer =
       TimeInterpolatableBuffer.createBuffer(poseBufferSizeSeconds);
   private final Matrix<N3, N1> qStdDevs = new Matrix<>(Nat.N3(), Nat.N1());
-  // odometry
+  // Odometry
   private final SwerveDriveKinematics kinematics;
   private SwerveDriveWheelPositions lastWheelPositions =
       new SwerveDriveWheelPositions(
@@ -72,6 +81,7 @@ public static RobotState getInstance() {
   private Rotation2d lastGyroAngle = new Rotation2d();
   private Twist2d robotVelocity = new Twist2d();
 
+  /** Cached latest aiming parameters. Calculated in {@code getAimingParameters()} */
   private AimingParameters latestParameters = null;
 
   private RobotState() {
@@ -155,7 +165,7 @@ public void addVisionObservation(VisionObservation observation) {
   }
 
   public void addVelocityData(Twist2d robotVelocity) {
-    this.latestParameters = null;
+    latestParameters = null;
     this.robotVelocity = robotVelocity;
   }
 
@@ -191,15 +201,13 @@ public AimingParameters getAimingParameters() {
     latestParameters =
         new AimingParameters(
             targetVehicleDirection,
-            new Rotation2d(
-                targetDistance - ArmConstants.armOrigin.getX(),
-                FieldConstants.Speaker.centerSpeakerOpening.getZ()
-                    - ArmConstants.armOrigin.getY()
-                    + shotHeightCompensation.get()),
+            Rotation2d.fromRadians(armAngleMap.get(targetDistance)),
             feedVelocity);
-    Logger.recordOutput("RobotState/AimingParameters/Direction", latestParameters.driveHeading);
-    Logger.recordOutput("RobotState/AimingParameters/ArmAngle", latestParameters.armAngle);
-    Logger.recordOutput("RobotState/AimingParameters/DriveFeedVelocityRadPerS", feedVelocity);
+    Logger.recordOutput("RobotState/AimingParameters/Direction", latestParameters.driveHeading());
+    Logger.recordOutput("RobotState/AimingParameters/ArmAngle", latestParameters.armAngle());
+    Logger.recordOutput(
+        "RobotState/AimingParameters/DriveFeedVelocityRadPerS",
+        latestParameters.driveFeedVelocity());
     return latestParameters;
   }
 
@@ -213,7 +221,7 @@ public void resetPose(Pose2d initialPose) {
     poseBuffer.clear();
   }
 
-  @AutoLogOutput(key = "Odometry/FieldVelocity")
+  @AutoLogOutput(key = "RobotState/FieldVelocity")
   public Twist2d fieldVelocity() {
     Translation2d linearFieldVelocity =
         new Translation2d(robotVelocity.dx, robotVelocity.dy).rotateBy(estimatedPose.getRotation());
@@ -221,7 +229,7 @@ public Twist2d fieldVelocity() {
         linearFieldVelocity.getX(), linearFieldVelocity.getY(), robotVelocity.dtheta);
   }
 
-  @AutoLogOutput(key = "Odometry/EstimatedPose")
+  @AutoLogOutput(key = "RobotState/EstimatedPose")
   public Pose2d getEstimatedPose() {
     return estimatedPose;
   }
@@ -243,7 +251,7 @@ public Pose2d getPredictedPose(double translationLookaheadS, double rotationLook
                 robotVelocity.dtheta * rotationLookaheadS));
   }
 
-  @AutoLogOutput(key = "Odometry/OdometryPose")
+  @AutoLogOutput(key = "RobotState/OdometryPose")
   public Pose2d getOdometryPose() {
     return odometryPose;
   }