Change odometry thread to use signal timestamps

src/main/java/org/curtinfrc/frc2025/Constants.java

@@ -15,7 +15,7 @@
  * (log replay from a file).
  */
 public final class Constants {
-  public static final RobotType robotType = RobotType.COMPBOT;
+  public static final RobotType robotType = RobotType.SIMBOT;
   public static final double ROBOT_X = 0.705;
   public static final double ROBOT_Y = 0.730;
   public static final double FIELD_LENGTH = aprilTagLayout.getFieldLength();

src/main/java/org/curtinfrc/frc2025/Robot.java

@@ -13,6 +13,7 @@
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.GenericHID.RumbleType;
+import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.Threads;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
@@ -724,6 +725,10 @@ public void robotPeriodic() {
     Threads.setCurrentThreadPriority(true, 99);
 
     PhoenixUtil.refreshAll();
+    Logger.recordOutput("PhoenixTime", PhoenixUtil.getTime());
+    Logger.recordOutput("FPGATime", RobotController.getFPGATime() / 1000000.0);
+    Logger.recordOutput(
+        "TimeDifference", PhoenixUtil.getTime() - RobotController.getFPGATime() / 1000000.0);
 
     controllerDisconnected.set(!controller.isConnected());
 

src/main/java/org/curtinfrc/frc2025/subsystems/drive/PhoenixOdometryThread.java

@@ -1,25 +1,12 @@
-// Copyright 2021-2024 FRC 6328
-// http://github.com/Mechanical-Advantage
-//
-// This program is free software; you can redistribute it and/or
-// modify it under the terms of the GNU General Public License
-// version 3 as published by the Free Software Foundation or
-// available in the root directory of this project.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU General Public License for more details.
-
 package org.curtinfrc.frc2025.subsystems.drive;
 
 import static org.curtinfrc.frc2025.subsystems.drive.DriveConstants.*;
+import static org.curtinfrc.frc2025.util.PhoenixUtil.phoenixToFPGATime;
 
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.CANBus;
 import com.ctre.phoenix6.StatusSignal;
 import edu.wpi.first.units.measure.Angle;
-import edu.wpi.first.wpilibj.RobotController;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.Queue;
@@ -138,28 +125,17 @@ public void run() {
       // Save new data to queues
       Drive.odometryLock.lock();
       try {
-        // Sample timestamp is current FPGA time minus average CAN latency
-        //     Default timestamps from Phoenix are NOT compatible with
-        //     FPGA timestamps, this solution is imperfect but close
-        double timestamp = RobotController.getFPGATime() / 1e6;
-        double totalLatency = 0.0;
-        for (BaseStatusSignal signal : phoenixSignals) {
-          totalLatency += signal.getTimestamp().getLatency();
-        }
-        if (phoenixSignals.length > 0) {
-          timestamp -= totalLatency / phoenixSignals.length;
-        }
-
+        assert (phoenixQueues.size() == timestampQueues.size());
         // Add new samples to queues
         for (int i = 0; i < phoenixSignals.length; i++) {
           phoenixQueues.get(i).offer(phoenixSignals[i].getValueAsDouble());
+          timestampQueues
+              .get(i)
+              .offer(phoenixToFPGATime(phoenixSignals[i].getTimestamp().getTime()));
         }
         for (int i = 0; i < genericSignals.size(); i++) {
           genericQueues.get(i).offer(genericSignals.get(i).getAsDouble());
         }
-        for (int i = 0; i < timestampQueues.size(); i++) {
-          timestampQueues.get(i).offer(timestamp);
-        }
       } finally {
         Drive.odometryLock.unlock();
       }

src/main/java/org/curtinfrc/frc2025/util/PhoenixUtil.java

@@ -5,6 +5,8 @@
 
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusCode;
+import com.ctre.phoenix6.Utils;
+import edu.wpi.first.wpilibj.RobotController;
 import java.util.function.Supplier;
 
 public class PhoenixUtil {
@@ -44,5 +46,25 @@ public static void refreshAll() {
     if (rioSignals.length > 0) {
       BaseStatusSignal.refreshAll(rioSignals);
     }
+
+  /**
+   * Converts a time from Phoenix6's timebase to FPGA time. Should only be used inside of an IO
+   * layer as it relies on raw FPGA time.
+   */
+  public static double phoenixToFPGATime(double timeSeconds) {
+    return (RobotController.getFPGATime() - Utils.getCurrentTimeSeconds()) + timeSeconds;
+  }
+
+  /**
+   * Converts a time from Phoenix6's timebase to the robots timebase. Can be used outside of IO
+   * layers.
+   */
+  public static double phoenixToRobotTime(double timeSeconds) {
+    return (RobotController.getTime() - Utils.getCurrentTimeSeconds()) + timeSeconds;
+  }
+
+  /** Returns raw Phoenix time. Is *NOT* deterministic. */
+  public static double getTime() {
+    return Utils.getCurrentTimeSeconds();
   }
 }