Note tracking tuning

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

@@ -9,8 +9,6 @@
 import dev.doglog.DogLog;
 import dev.doglog.DogLogOptions;
 import edu.wpi.first.wpilibj.DigitalInput;
-import edu.wpi.first.wpilibj.PowerDistribution;
-import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -30,6 +28,7 @@
 import frc.robot.lights.LightsSubsystem;
 import frc.robot.localization.LocalizationSubsystem;
 import frc.robot.note_manager.NoteManager;
+import frc.robot.note_tracking.NoteTrackingManager;
 import frc.robot.queuer.QueuerSubsystem;
 import frc.robot.robot_manager.RobotCommands;
 import frc.robot.robot_manager.RobotManager;
@@ -52,7 +51,6 @@ public class Robot extends TimedRobot {
   private final RumbleControllerSubsystem operatorRumble =
       new RumbleControllerSubsystem(operatorController, true);
 
-  private final PowerDistribution pdh = new PowerDistribution(1, ModuleType.kRev);
   private final WristSubsystem wrist =
       new WristSubsystem(
           new TalonFX(RobotConfig.get().wrist().motorID(), RobotConfig.get().canivoreName()));
@@ -97,6 +95,8 @@ public class Robot extends TimedRobot {
   private final LightsSubsystem lightsSubsystem =
       new LightsSubsystem(
           new CANdle(RobotConfig.get().lights().deviceID(), "rio"), robotManager, vision, intake);
+  private final NoteTrackingManager noteTrackingManager =
+      new NoteTrackingManager(localization, swerve, actions, robotManager);
 
   public Robot() {
     System.out.println("roboRIO serial number: " + RobotConfig.SERIAL_NUMBER);
@@ -237,6 +237,10 @@ private void configureBindings() {
         .y()
         .onTrue(actions.waitSubwooferShotCommand())
         .onFalse(actions.stowCommand());
+    operatorController
+        .povRight()
+        .whileTrue(noteTrackingManager.intakeDetectedNote())
+        .onFalse(actions.stowCommand());
     operatorController
         .rightTrigger()
         .onTrue(actions.waitForSpeakerShotCommand())

src/main/java/frc/robot/localization/LocalizationSubsystem.java

@@ -10,6 +10,7 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
@@ -48,6 +49,9 @@ public class LocalizationSubsystem extends LifecycleSubsystem {
   private final TimedDataBuffer distanceToSavedHistory =
       new TimedDataBuffer(RobotConfig.get().vision().translationHistoryArraySize());
 
+  private final TimeInterpolatableBuffer<Pose2d> poseHistory =
+      TimeInterpolatableBuffer.createBuffer(1.5);
+
   public LocalizationSubsystem(SwerveSubsystem swerve, ImuSubsystem imu, VisionSubsystem vision) {
     super(SubsystemPriority.LOCALIZATION);
     this.swerve = swerve;
@@ -95,6 +99,8 @@ public void robotPeriodic() {
       }
     }
 
+    poseHistory.addSample(Timer.getFPGATimestamp(), poseEstimator.getEstimatedPosition());
+
     DogLog.log("Localization/OdometryPose", getOdometryPose());
     // DogLog.log("Localization/SavedExpectedPose", getSavedExpectedPose(false));
     DogLog.log("Localization/EstimatedPose", getPose());
@@ -124,6 +130,11 @@ public Pose2d getPose() {
     return poseEstimator.getEstimatedPosition();
   }
 
+  // get pose at timestamp method
+  public Pose2d getPose(double timestamp) {
+    return poseHistory.getSample(timestamp).orElseGet(this::getPose);
+  }
+
   public Pose2d getOdometryPose() {
     return odometry.getPoseMeters();
   }

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

@@ -10,26 +10,33 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.config.RobotConfig;
 import frc.robot.fms.FmsSubsystem;
 import frc.robot.localization.LocalizationSubsystem;
 import frc.robot.robot_manager.RobotCommands;
 import frc.robot.robot_manager.RobotManager;
+import frc.robot.snaps.SnapManager;
 import frc.robot.swerve.SwerveSubsystem;
 import frc.robot.util.scheduling.LifecycleSubsystem;
 import frc.robot.util.scheduling.SubsystemPriority;
+import frc.robot.vision.DistanceAngle;
+import frc.robot.vision.LimelightHelpers;
 import frc.robot.vision.VisionSubsystem;
 import java.util.Optional;
 
 public class NoteTrackingManager extends LifecycleSubsystem {
+  private static final double LIMELIGHT_VERTICAL_FOV_DEGREES = 25.0;
   private final LocalizationSubsystem localization;
   private final SwerveSubsystem swerve;
   private final RobotCommands actions;
   private final RobotManager robot;
+  private final SnapManager snaps;
   private static final String LIMELIGHT_NAME = "limelight-note";
   private final InterpolatingDoubleTreeMap tyToDistance = new InterpolatingDoubleTreeMap();
-  private Pose2d lastNotePose = new Pose2d();
+  private Optional<Pose2d> lastNotePose = Optional.empty();
 
   private double midlineXValue = 8.3;
 
@@ -44,6 +51,7 @@ public NoteTrackingManager(
     this.swerve = swerve;
     this.actions = actions;
     this.robot = robot;
+    this.snaps = robot.snaps;
     RobotConfig.get().vision().tyToNoteDistance().accept(tyToDistance);
   }
 
@@ -52,10 +60,18 @@ private Optional<Pose2d> getNotePose() {
     // long v =
     // NetworkTableInstance.getDefault().getTable(LIMELIGHT_NAME).getEntry("v").getInteger(0);
     double ty =
-        NetworkTableInstance.getDefault().getTable(LIMELIGHT_NAME).getEntry("ty").getDouble(0);
+        LimelightHelpers.getTY(LIMELIGHT_NAME) - LIMELIGHT_VERTICAL_FOV_DEGREES;
     double tx =
         NetworkTableInstance.getDefault().getTable(LIMELIGHT_NAME).getEntry("tx").getDouble(0);
 
+    double latency =
+        (LimelightHelpers.getLatency_Capture(LIMELIGHT_NAME)
+                + LimelightHelpers.getLatency_Pipeline(LIMELIGHT_NAME))
+            / 1000.0;
+    double timestamp = Timer.getFPGATimestamp() - latency;
+
+    Pose2d robotPoseAtCapture = localization.getPose(timestamp);
+
     if (tx == 0) {
       return Optional.empty();
     }
@@ -64,12 +80,9 @@ private Optional<Pose2d> getNotePose() {
       return Optional.empty();
     }
 
-    // TODO: This should probably be the robot pose at the time the image was taken, since there is
-    // latency
-    var robotPose = getPose();
-
     DogLog.log("NoteTracking/TY", ty);
     DogLog.log("NoteTracking/TX", tx);
+    DogLog.log("NoteTracking/LatencyRobotPose", robotPoseAtCapture);
 
     double forwardDistanceToNote = tyToDistance.get(ty);
     Rotation2d angleFromNote = Rotation2d.fromDegrees(tx);
@@ -86,42 +99,61 @@ private Optional<Pose2d> getNotePose() {
     DogLog.log("NoteTracking/SidewaysDistance", sidewaysDistanceToNote);
     var notePoseWithoutRotation =
         new Translation2d(-forwardDistanceToNote, -sidewaysDistanceToNote)
-            .rotateBy(Rotation2d.fromDegrees(getPose().getRotation().getDegrees()));
+            .rotateBy(Rotation2d.fromDegrees(robotPoseAtCapture.getRotation().getDegrees()));
 
     var notePoseWithRobot =
         new Translation2d(
-            getPose().getX() + notePoseWithoutRotation.getX(),
-            getPose().getY() + notePoseWithoutRotation.getY());
+            robotPoseAtCapture.getX() + notePoseWithoutRotation.getX(),
+            robotPoseAtCapture.getY() + notePoseWithoutRotation.getY());
     // Uses distance angle math to aim, inverses the angle for intake
-    double rotation =
+
+    DistanceAngle noteDistanceAngle =
         VisionSubsystem.distanceToTargetPose(
-                new Pose2d(notePoseWithRobot, new Rotation2d()), robotPose)
-            .targetAngle()
-            .getRadians();
-    return Optional.of(new Pose2d(notePoseWithRobot, new Rotation2d(rotation + Math.PI)));
+            new Pose2d(notePoseWithRobot, new Rotation2d()), robotPoseAtCapture);
+    Rotation2d rotation = new Rotation2d(noteDistanceAngle.targetAngle().getRadians() + Math.PI);
+
+    if (noteDistanceAngle.distance() < 3) {
+      if (lastNotePose.isPresent()) {
+        rotation = lastNotePose.get().getRotation();
+      } else {
+        rotation = getPose().getRotation();
+      }
+    }
+    return Optional.of(
+        new Pose2d(notePoseWithRobot, rotation));
   }
 
   private boolean pastMidline() {
-    Pose2d robotPose = getPose();
     double pastMidlineThresholdMeters = 0.65;
 
     // Red alliance
     if (FmsSubsystem.isRedAlliance()) {
-      if (robotPose.getX() < (midlineXValue - pastMidlineThresholdMeters)) {
+      if (getPose().getX() < (midlineXValue - pastMidlineThresholdMeters)) {
         return true;
       }
       return false;
     }
 
     // Blue alliance
-    return robotPose.getX() > (midlineXValue + pastMidlineThresholdMeters);
+    return getPose().getX() > (midlineXValue + pastMidlineThresholdMeters);
   }
 
   public Command intakeDetectedNote() {
-    return swerve
-        .driveToPoseCommand(() -> lastNotePose, this::getPose)
-        .until(() -> pastMidline())
-        .raceWith(actions.intakeCommand().withTimeout(2))
+    return Commands.runOnce(() -> lastNotePose = getNotePose())
+        .andThen(actions.intakeCommand())
+        .raceWith(
+            swerve.driveToPoseCommand(
+                () -> {
+                  if (lastNotePose.isPresent()) {
+                    snaps.setAngle(lastNotePose.get().getRotation());
+                    snaps.setEnabled(true);
+                  } else {
+                    snaps.setEnabled(false);
+                  }
+
+                  return lastNotePose;
+                },
+                this::getPose))
         .finallyDo(robot::stowRequest);
   }
 
@@ -134,7 +166,7 @@ public void robotPeriodic() {
     Optional<Pose2d> notePose = getNotePose();
     if (notePose.isPresent()) {
       DogLog.log("NoteTracking/NotePose", notePose.get());
-      lastNotePose = notePose.get();
+      lastNotePose = notePose;
     }
   }
 }

src/main/java/frc/robot/swerve/SwerveSubsystem.java

@@ -28,6 +28,7 @@
 import frc.robot.util.scheduling.LifecycleSubsystem;
 import frc.robot.util.scheduling.SubsystemPriority;
 import java.util.List;
+import java.util.Optional;
 import java.util.function.Supplier;
 
 public class SwerveSubsystem extends LifecycleSubsystem {
@@ -114,10 +115,9 @@ private static void applyCurrentLimits(SwerveModule module) {
   private ChassisSpeeds fieldRelativeSpeeds = new ChassisSpeeds();
   private boolean closedLoop = false;
 
-  // TODO: tune
-  private final PIDController xPid = new PIDController(1.5, 0, 0);
+  private final PIDController xPid = new PIDController(2.0, 0, 0);
   private final PIDController yPid = new PIDController(2.0, 0, 0);
-  private final PIDController omegaPid = new PIDController(0.5, 0, 0);
+  private final PIDController omegaPid = new PIDController(2.0, 0, 0);
 
   public SwerveSubsystem(CommandXboxController driveController) {
     super(SubsystemPriority.SWERVE);
@@ -382,26 +382,38 @@ private boolean atLocation(Pose2d target, Pose2d current) {
             <= omegaTolerance;
   }
 
-  public Command driveToPoseCommand(Supplier<Pose2d> targetSupplier, Supplier<Pose2d> currentPose) {
+  public Command driveToPoseCommand(
+      Supplier<Optional<Pose2d>> targetSupplier, Supplier<Pose2d> currentPose) {
     return run(() -> {
-          var target = targetSupplier.get();
+          var maybeTarget = targetSupplier.get();
+
+          if (!maybeTarget.isPresent()) {
+            setFieldRelativeSpeeds(new ChassisSpeeds(), closedLoop);
+            return;
+          }
+
+          var target = maybeTarget.get();
+
           var pose = currentPose.get();
           double vx = xPid.calculate(pose.getX(), target.getX());
           double vy = yPid.calculate(pose.getY(), target.getY());
-          double vomega =
-              omegaPid.calculate(
-                  Rotation2d.fromDegrees(drivetrain.getPigeon2().getYaw().getValueAsDouble())
-                      .getRadians(),
-                  target.getRotation().getRadians());
 
-          var newSpeeds = new ChassisSpeeds(vx, vy, -vomega);
+          var newSpeeds = new ChassisSpeeds(vx, vy, 0);
           setFieldRelativeSpeeds(newSpeeds, true);
         })
         .until(
             () -> {
-              var target = targetSupplier.get();
-
-              return atLocation(target, currentPose.get());
+              var maybeTarget = targetSupplier.get();
+              if (maybeTarget.isPresent()) {
+                var target = targetSupplier.get();
+
+                return atLocation(target.get(), currentPose.get());
+              }
+              return false;
+            })
+        .finallyDo(
+            () -> {
+              snapToAngle = false;
             });
   }
 }