[utils] Add methods to convert from Phoenix time

Signed-off-by: Jade Turner <[email protected]>

Author: spacey-sooty

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

@@ -14,6 +14,8 @@
 package org.curtinfrc.frc2025.util;
 
 import com.ctre.phoenix6.StatusCode;
+import com.ctre.phoenix6.Utils;
+import edu.wpi.first.wpilibj.RobotController;
 import java.util.function.Supplier;
 
 public class PhoenixUtil {
@@ -24,4 +26,20 @@ public static void tryUntilOk(int maxAttempts, Supplier<StatusCode> command) {
       if (error.isOK()) break;
     }
   }
+
+  /**
+   * 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;
+  }
 }