Note map

src/main/java/frc/robot/Robot.java

@@ -96,7 +96,7 @@ public class Robot extends TimedRobot {
       new LightsSubsystem(
           new CANdle(RobotConfig.get().lights().deviceID(), "rio"), robotManager, vision, intake);
   private final NoteTrackingManager noteTrackingManager =
-      new NoteTrackingManager(localization, swerve, actions, robotManager);
+      new NoteTrackingManager(localization, swerve, actions, robotManager, imu);
 
   public Robot() {
     System.out.println("roboRIO serial number: " + RobotConfig.SERIAL_NUMBER);
@@ -239,7 +239,7 @@ private void configureBindings() {
         .onFalse(actions.stowCommand());
     operatorController
         .povRight()
-        .whileTrue(noteTrackingManager.intakeDetectedNote())
+        .whileTrue(noteTrackingManager.intakeNearestMapNote())
         .onFalse(actions.stowCommand());
     operatorController
         .rightTrigger()

src/main/java/frc/robot/config/CompConfig.java

@@ -283,14 +283,14 @@ class CompConfig {
               0.4,
               0.4,
               tyToNoteDistance -> {
-                tyToNoteDistance.put(3.0, Units.inchesToMeters(0 + 20.5));
-                tyToNoteDistance.put(10.5, Units.inchesToMeters(10.5 + 20.5));
-                tyToNoteDistance.put(20.0, Units.inchesToMeters(18.5 + 20.5));
-                tyToNoteDistance.put(30.0, Units.inchesToMeters(29.5 + 20.5));
-                tyToNoteDistance.put(35.0, Units.inchesToMeters(40.0 + 20.5));
-                tyToNoteDistance.put(40.0, Units.inchesToMeters(58.5 + 20.5));
-                tyToNoteDistance.put(45.0, Units.inchesToMeters(90 + 20.5));
-                tyToNoteDistance.put(47.0, Units.inchesToMeters(114.0 + 20.5));
+                tyToNoteDistance.put(3.0 - 24.5, Units.inchesToMeters(0 + 20.5));
+                tyToNoteDistance.put(10.5 - 24.5, Units.inchesToMeters(10.5 + 20.5));
+                tyToNoteDistance.put(20.0 - 24.5, Units.inchesToMeters(18.5 + 20.5));
+                tyToNoteDistance.put(30.0 - 24.5, Units.inchesToMeters(29.5 + 20.5));
+                tyToNoteDistance.put(35.0 - 24.5, Units.inchesToMeters(40.0 + 20.5));
+                tyToNoteDistance.put(40.0 - 24.5, Units.inchesToMeters(58.5 + 20.5));
+                tyToNoteDistance.put(45.0 - 24.5, Units.inchesToMeters(90 + 20.5));
+                tyToNoteDistance.put(47.0 - 24.5, Units.inchesToMeters(114.0 + 20.5));
               },
               // x=right, y= forward, z=up
               new Translation3d(-0.025, Units.inchesToMeters(-1), Units.inchesToMeters(23.25)),

src/main/java/frc/robot/imu/ImuSubsystem.java

@@ -97,7 +97,7 @@ private boolean atAngle(Rotation2d angle, Rotation2d tolerance) {
     return Math.abs(getRobotHeading().minus(angle).getDegrees()) < tolerance.getDegrees();
   }
 
-  public boolean belowVelocityForSpeaker(double distance) {
+  public boolean belowVelocityForVision(double distance) {
     return getRobotAngularVelocity().getDegrees() < 10;
   }
 

src/main/java/frc/robot/note_tracking/NoteMapElement.java

@@ -0,0 +1,9 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.note_tracking;
+
+import edu.wpi.first.math.geometry.Pose2d;
+
+public record NoteMapElement(double expiresAt, Pose2d notePose) {}