changed timestamp to be correct for limelight

Team334 · Apr 5, 2024 · 4f9cc78 · 4f9cc78
1 parent daa0868
commit 4f9cc78
Showing 1 changed file with 3 additions and 3 deletions.
diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
@@ -101,7 +101,7 @@ Constants.Physical.SWERVE_KINEMATICS, getHeadingRaw(),
     new SwerveModulePosition[] {_frontLeft.getPosition(), _frontRight.getPosition(), _backRight.getPosition(), _backLeft.getPosition()},
     new Pose2d(), 
     VecBuilder.fill(0.03, 0.03, 0.01), 
-    VecBuilder.fill(0.9, 0.9, 9999999)
+    VecBuilder.fill(0.4, 0.4, 9999999)
   );
 
   // OTHER POSSIBLE STD DEV VALUES:
@@ -230,11 +230,11 @@ private boolean updateBotpose() {
         0
       );
 
-      if (Math.toRadians(Math.abs(_gyro.getRate())) >= Math.PI * 2) return false;
+      if (Math.abs(_gyro.getRate()) >= 500) return false;
 
       _estimator.addVisionMeasurement(
         visionBotpose.get().pose,
-        visionBotpose.get().latency
+        visionBotpose.get().timestampSeconds
       );
     }