guh

Greater-Rochester-Robotics · Oct 25, 2024 · 185b425 · 185b425
1 parent 4411989
commit 185b425
Showing 1 changed file with 9 additions and 7 deletions.
diff --git a/src/main/java/org/team340/robot/subsystems/Swerve.java b/src/main/java/org/team340/robot/subsystems/Swerve.java
@@ -14,9 +14,9 @@
 import edu.wpi.first.math.geometry.Twist2d;
 import edu.wpi.first.math.geometry.Twist3d;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.util.WPIUtilJNI;
 import edu.wpi.first.wpilibj.ADIS16470_IMU.CalibrationTime;
 import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis;
+import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.SPI.Port;
 import edu.wpi.first.wpilibj2.command.Command;
 import java.util.ArrayList;
@@ -107,6 +107,7 @@ public class Swerve extends GRRSubsystem {
     private final List<Pose2d> measurements = new ArrayList<>();
     private final List<Pose3d> targets = new ArrayList<>();
     private Pose2d gtsamPose = new Pose2d();
+    private double firstLoop = -1.0;
 
     public Swerve() {
         api = new SwerveAPI(CONFIG);
@@ -126,14 +127,18 @@ public Swerve() {
 
     @Override
     public void periodic() {
-        long loopStart = WPIUtilJNI.now();
-        long tagDetTime = loopStart - 10000; // YIPPEE
+        long loopStart = RobotController.getFPGATime();
+        long tagDetTime = loopStart + 10000; // YIPPEE
+        if (firstLoop == -1.0) firstLoop = loopStart;
 
         api.refresh();
 
         measurements.clear();
         targets.clear();
 
+        Twist2d twist = api.state.twist;
+        gtsamInterface.sendOdomUpdate(loopStart, new Twist3d(twist.dx, twist.dy, 0, 0, 0, twist.dtheta), null);
+
         Pose3d guess = null;
         int mostTargets = 0;
         for (int i = 0; i < cameras.length; i++) {
@@ -150,7 +155,7 @@ public void periodic() {
                 }
 
                 gtsamInterface.setCamIntrinsics(camera.getName(), camera.getCameraMatrix(), camera.getDistCoeffs());
-                gtsamInterface.sendVisionUpdate(
+                if (tagDetTime > firstLoop) gtsamInterface.sendVisionUpdate(
                     camera.getName(),
                     tagDetTime,
                     dets,
@@ -190,9 +195,6 @@ public void periodic() {
             }
         }
 
-        Twist2d twist = api.state.twist;
-        gtsamInterface.sendOdomUpdate(loopStart, new Twist3d(twist.dx, twist.dy, 0, 0, 0, twist.dtheta), guess);
-
         gtsamPose = gtsamInterface.getLatencyCompensatedPoseEstimate().toPose2d();
     }