fix code loop overruns.

.gradle-user-home/caches/8.11/groovy-dsl/a65f414d91e7fd46a356249b50d1efee/metadata.bin

@@ -0,0 +1 @@
+�5cotl4u46bgtxgg5b27ftytpuy����X-��ʏ��y+.��instrumentedOutput
�[\Q�8��)8
�U�metadataDir
�)C��e9=�*�s@���propertyUpgradeReportOutput
���y�ޜ3�&���;��

.gradle-user-home/caches/8.11/groovy-dsl/d18b42292f6cfa90b43f9ec147cc41eb/metadata.bin

@@ -0,0 +1 @@
+�5cotl4u46bgtxgg5b27ftytpuy~G`*:*����-���instrumentedOutput
�̑�ӣzI)t-j5�metadataDir
�)C��e9=�*�s@���propertyUpgradeReportOutput
���y�ޜ3�&���;��

.gradle-user-home/caches/CACHEDIR.TAG

@@ -0,0 +1,4 @@
+Signature: 8a477f597d28d172789f06886806bc55
+# This file is a cache directory tag created by Gradle.
+# For information about cache directory tags, see:
+#	https://bford.info/cachedir/

.gradle-user-home/caches/journal-1/file-access.properties

@@ -0,0 +1,2 @@
+#Wed Mar 25 22:19:18 EDT 2026
+inceptionTimestamp=1774491558414

.gradle-user-home/daemon/CACHEDIR.TAG

@@ -0,0 +1,4 @@
+Signature: 8a477f597d28d172789f06886806bc55
+# This file is a cache directory tag created by Gradle.
+# For information about cache directory tags, see:
+#	https://bford.info/cachedir/

.gradle-user-home/native/100fb08df4bc3b14c8652ba06237920a3bd2aa13389f12d3474272988ae205f9/windows-amd64/native-platform-file-events.dll.lock

@@ -0,0 +1 @@
+

.gradle-user-home/native/c067742578af261105cb4f569cf0c3c89f3d7b1fecec35dd04571415982c5e48/windows-amd64/native-platform.dll.lock

@@ -0,0 +1 @@
+

simgui-ds.json

@@ -88,5 +88,10 @@
       "buttonCount": 0,
       "povCount": 0
     }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "Keyboard0"
+    }
   ]
 }

src/main/deploy/autos/paths/Middle_to_depot.json

@@ -0,0 +1,115 @@
+{
+  "path_elements": [
+    {
+      "type": "waypoint",
+      "translation_target": {
+        "x_meters": 13.361426533523543,
+        "y_meters": 2.6083523537803153,
+        "intermediate_handoff_radius_meters": 0.25
+      },
+      "rotation_target": {
+        "rotation_radians": 3.141592653589793,
+        "profiled_rotation": true
+      }
+    },
+    {
+      "type": "event_trigger",
+      "t_ratio": 0.0,
+      "lib_key": "intake"
+    },
+    {
+      "type": "rotation",
+      "rotation_radians": 0.0,
+      "t_ratio": 0.46635262064706745,
+      "profiled_rotation": true
+    },
+    {
+      "type": "waypoint",
+      "translation_target": {
+        "x_meters": 15.087902995720405,
+        "y_meters": 2.4117459175884877,
+        "intermediate_handoff_radius_meters": 0.25
+      },
+      "rotation_target": {
+        "rotation_radians": 0.0,
+        "profiled_rotation": true
+      }
+    },
+    {
+      "type": "event_trigger",
+      "t_ratio": 0.0,
+      "lib_key": "deploy"
+    },
+    {
+      "type": "waypoint",
+      "translation_target": {
+        "x_meters": 15.829598984635563,
+        "y_meters": 2.399,
+        "intermediate_handoff_radius_meters": 0.1
+      },
+      "rotation_target": {
+        "rotation_radians": 0.0,
+        "profiled_rotation": true
+      }
+    },
+    {
+      "type": "translation",
+      "x_meters": 14.757243618343884,
+      "y_meters": 1.7609543509272472,
+      "intermediate_handoff_radius_meters": 0.25
+    },
+    {
+      "type": "waypoint",
+      "translation_target": {
+        "x_meters": 15.83,
+        "y_meters": 1.6514122681883032,
+        "intermediate_handoff_radius_meters": 0.1
+      },
+      "rotation_target": {
+        "rotation_radians": 0.0,
+        "profiled_rotation": true
+      }
+    },
+    {
+      "type": "waypoint",
+      "translation_target": {
+        "x_meters": 14.319480741797435,
+        "y_meters": 2.5342906939501617,
+        "intermediate_handoff_radius_meters": 0.25
+      },
+      "rotation_target": {
+        "rotation_radians": 0.0,
+        "profiled_rotation": true
+      }
+    }
+  ],
+  "constraints": {
+    "max_velocity_meters_per_sec": [
+      {
+        "value": 1.65,
+        "start_ordinal": 0,
+        "end_ordinal": 1
+      },
+      {
+        "value": 0.75,
+        "start_ordinal": 2,
+        "end_ordinal": 2
+      },
+      {
+        "value": 1.35,
+        "start_ordinal": 3,
+        "end_ordinal": 3
+      },
+      {
+        "value": 0.75,
+        "start_ordinal": 4,
+        "end_ordinal": 4
+      },
+      {
+        "value": 1.25,
+        "start_ordinal": 5,
+        "end_ordinal": 5
+      }
+    ]
+  }
+}

src/main/java/frc/robot/Robot.java

@@ -28,6 +28,7 @@
 import frc.robot.constants.FieldConstants;
 import frc.robot.lib.BLine.FollowPath;
 import frc.robot.lib.util.PhoenixUtil;
+import frc.robot.sim.MapleSimManager;
 
 /**
  * The VM is configured to automatically run this class, and to call the
@@ -258,10 +259,21 @@ public void testPeriodic() {
     /** This function is called once when the robot is first started up. */
     @Override
     public void simulationInit() {
+        if (shouldUseMapleSimulation()) {
+            MapleSimManager.getInstance().resetFieldForAuto();
+        }
     }
 
     /** This function is called periodically whilst in simulation. */
     @Override
     public void simulationPeriodic() {
+        if (shouldUseMapleSimulation()) {
+            MapleSimManager.getInstance().simulationPeriodic();
+        }
+    }
+
+    private boolean shouldUseMapleSimulation() {
+        return Constants.shouldUseSimulation(Constants.SimOnlySubsystems.SWERVE)
+            || Constants.shouldUseSimulation(Constants.SimOnlySubsystems.VISION);
     }
 }

src/main/java/frc/robot/RobotState.java

@@ -15,10 +15,11 @@
 import frc.robot.configs.RobotStateConfig;
 import frc.robot.configs.SwerveConfig;
 import frc.robot.configs.SwerveDrivetrainConfig;
+import frc.robot.constants.Constants;
 import frc.robot.lib.util.ConfigLoader;
+import frc.robot.sim.MapleSimManager;
 import frc.robot.subsystems.swerve.SwerveDrive;
 
-import java.util.NoSuchElementException;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
@@ -116,6 +117,10 @@ private RobotState() {
 
     /** Add odometry observation */
     public void addOdometryObservation(OdometryObservation observation) {
+        if (!Double.isFinite(observation.timestampsSeconds()) || !Double.isFinite(observation.yawVelocityRadPerSec())) {
+            return;
+        }
+
         observation = new OdometryObservation(
             observation.timestampsSeconds(),
             observation.isGyroConnected(),
@@ -188,19 +193,17 @@ public void addOdometryObservation(OdometryObservation observation) {
     }
 
     public void addVisionObservation(VisionObservation observation) {
+        if (!Double.isFinite(observation.timestampSeconds())
+            || !Double.isFinite(observation.visionRobotPoseMeters().getX())
+            || !Double.isFinite(observation.visionRobotPoseMeters().getY())) {
+            return;
+        }
+
         boolean shouldLogObservation = shouldLogVisionObservation();
         // If measurement is old enough to be outside the pose buffer's timespan, skip.
-        try {
-            if (poseBuffer.getInternalBuffer().lastKey() - robotStateConfig.poseBufferSizeSeconds > observation.timestampSeconds()) {
-                incrementVisionRejectedCount();
-                if (shouldLogObservation) {
-                    logVisionObservationMetrics(null, observation.timestampSeconds(), false);
-                }
-                return;
-            }
-        } 
-
-        catch (NoSuchElementException ex) {
+        var internalBuffer = poseBuffer.getInternalBuffer();
+        if (internalBuffer.isEmpty()
+            || internalBuffer.lastKey() - robotStateConfig.poseBufferSizeSeconds > observation.timestampSeconds()) {
             incrementVisionRejectedCount();
             if (shouldLogObservation) {
                 logVisionObservationMetrics(null, observation.timestampSeconds(), false);
@@ -239,6 +242,10 @@ public void addVisionObservation(VisionObservation observation) {
      */
     public void resetPose(Pose2d initialPose) {
         SwerveDrive.getInstance().resetGyro(initialPose.getRotation());
+        if (Constants.shouldUseSimulation(Constants.SimOnlySubsystems.SWERVE)
+            && Constants.currentMode != Constants.Mode.REPLAY) {
+            MapleSimManager.getInstance().resetRobotPose(initialPose);
+        }
         swerveDrivePoseEstimator.resetPosition(initialPose.getRotation(), lastWheelPositions, initialPose);
         accumulatedYawRad = initialPose.getRotation().getRadians();
         lastWrappedYawRad = initialPose.getRotation().getRadians();