fix profiler breaking

Frc5572 · Sep 26, 2024 · 7458b7c · 7458b7c
1 parent 0db7fcb
commit 7458b7c
Showing 1 changed file with 3 additions and 0 deletions.
diff --git a/src/main/java/frc/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/robot/subsystems/swerve/Swerve.java
@@ -276,6 +276,7 @@ public void periodic() {
                     swerveOdometry.resetPosition(getGyroYaw(), getModulePositions(),
                         robotPose.get().robotPose);
                     hasInitialized = true;
+                    Robot.profiler.pop();
                     break;
                 }
                 Robot.profiler.pop();
@@ -288,9 +289,11 @@ public void periodic() {
                 var result = cameras[i].getEstimatedGlobalPose(getPose());
                 if (result.isPresent()) {
                     if (DriverStation.isAutonomous() && result.get().targetsUsed.size() < 2) {
+                        Robot.profiler.pop();
                         continue;
                     } else if (result.get().targetsUsed.size() == 1
                         && result.get().targetsUsed.get(0).getPoseAmbiguity() > 0.1) {
+                        Robot.profiler.pop();
                         continue;
                     }
                     swerveOdometry.addVisionMeasurement(result.get().estimatedPose.toPose2d(),