diff --git a/src/main/deploy/2025-reefscape-andymark.json b/src/main/deploy/2025-reefscape-andymark.json
new file mode 100644
index 00000000..60b60e5e
--- /dev/null
+++ b/src/main/deploy/2025-reefscape-andymark.json
@@ -0,0 +1,404 @@
+{
+  "tags": [
+    {
+      "ID": 1,
+      "pose": {
+        "translation": {
+          "x": 16.687292,
+          "y": 0.628142,
+          "z": 1.4859
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.4539904997395468,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.8910065241883678
+          }
+        }
+      }
+    },
+    {
+      "ID": 2,
+      "pose": {
+        "translation": {
+          "x": 16.687292,
+          "y": 7.414259999999999,
+          "z": 1.4859
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.45399049973954675,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.8910065241883679
+          }
+        }
+      }
+    },
+    {
+      "ID": 3,
+      "pose": {
+        "translation": {
+          "x": 11.49096,
+          "y": 8.031733999999998,
+          "z": 1.30175
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 4,
+      "pose": {
+        "translation": {
+          "x": 9.276079999999999,
+          "y": 6.132575999999999,
+          "z": 1.8679160000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.9659258262890683,
+            "X": 0.0,
+            "Y": 0.25881904510252074,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 5,
+      "pose": {
+        "translation": {
+          "x": 9.276079999999999,
+          "y": 1.9098259999999998,
+          "z": 1.8679160000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.9659258262890683,
+            "X": 0.0,
+            "Y": 0.25881904510252074,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 6,
+      "pose": {
+        "translation": {
+          "x": 13.474446,
+          "y": 3.3012379999999997,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.8660254037844387,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.49999999999999994
+          }
+        }
+      }
+    },
+    {
+      "ID": 7,
+      "pose": {
+        "translation": {
+          "x": 13.890498,
+          "y": 4.0208200000000005,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 8,
+      "pose": {
+        "translation": {
+          "x": 13.474446,
+          "y": 4.740402,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.8660254037844387,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.49999999999999994
+          }
+        }
+      }
+    },
+    {
+      "ID": 9,
+      "pose": {
+        "translation": {
+          "x": 12.643358,
+          "y": 4.740402,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.5000000000000001,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.8660254037844386
+          }
+        }
+      }
+    },
+    {
+      "ID": 10,
+      "pose": {
+        "translation": {
+          "x": 12.227305999999999,
+          "y": 4.0208200000000005,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 11,
+      "pose": {
+        "translation": {
+          "x": 12.643358,
+          "y": 3.3012379999999997,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.4999999999999998,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.8660254037844387
+          }
+        }
+      }
+    },
+    {
+      "ID": 12,
+      "pose": {
+        "translation": {
+          "x": 0.8613139999999999,
+          "y": 0.628142,
+          "z": 1.4859
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.8910065241883679,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.45399049973954675
+          }
+        }
+      }
+    },
+    {
+      "ID": 13,
+      "pose": {
+        "translation": {
+          "x": 0.8613139999999999,
+          "y": 7.414259999999999,
+          "z": 1.4859
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.8910065241883678,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.45399049973954686
+          }
+        }
+      }
+    },
+    {
+      "ID": 14,
+      "pose": {
+        "translation": {
+          "x": 8.272272,
+          "y": 6.132575999999999,
+          "z": 1.8679160000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 5.914589856893349e-17,
+            "X": -0.25881904510252074,
+            "Y": 1.5848095757158825e-17,
+            "Z": 0.9659258262890683
+          }
+        }
+      }
+    },
+    {
+      "ID": 15,
+      "pose": {
+        "translation": {
+          "x": 8.272272,
+          "y": 1.9098259999999998,
+          "z": 1.8679160000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 5.914589856893349e-17,
+            "X": -0.25881904510252074,
+            "Y": 1.5848095757158825e-17,
+            "Z": 0.9659258262890683
+          }
+        }
+      }
+    },
+    {
+      "ID": 16,
+      "pose": {
+        "translation": {
+          "x": 6.057646,
+          "y": 0.010667999999999999,
+          "z": 1.30175
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.7071067811865476,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 17,
+      "pose": {
+        "translation": {
+          "x": 4.073905999999999,
+          "y": 3.3012379999999997,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.4999999999999998,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.8660254037844387
+          }
+        }
+      }
+    },
+    {
+      "ID": 18,
+      "pose": {
+        "translation": {
+          "x": 3.6576,
+          "y": 4.0208200000000005,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 19,
+      "pose": {
+        "translation": {
+          "x": 4.073905999999999,
+          "y": 4.740402,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.5000000000000001,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.8660254037844386
+          }
+        }
+      }
+    },
+    {
+      "ID": 20,
+      "pose": {
+        "translation": {
+          "x": 4.904739999999999,
+          "y": 4.740402,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.8660254037844387,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.49999999999999994
+          }
+        }
+      }
+    },
+    {
+      "ID": 21,
+      "pose": {
+        "translation": {
+          "x": 5.321046,
+          "y": 4.0208200000000005,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 22,
+      "pose": {
+        "translation": {
+          "x": 4.904739999999999,
+          "y": 3.3012379999999997,
+          "z": 0.308102
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.8660254037844387,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.49999999999999994
+          }
+        }
+      }
+    }
+  ],
+  "field": {
+    "length": 17.548,
+    "width": 8.042
+  }
+}
diff --git a/src/main/deploy/choreo/blueCenterToL4G.traj b/src/main/deploy/choreo/blueCenterToL4G.traj
index 4672ec6a..de3722c7 100644
--- a/src/main/deploy/choreo/blueCenterToL4G.traj
+++ b/src/main/deploy/choreo/blueCenterToL4G.traj
@@ -4,8 +4,8 @@
  "snapshot":{
   "waypoints":[
     {"x":7.2, "y":3.8, "heading":3.141592653589793, "intervals":62, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":6.300933361053467, "y":3.7982170581817623, "heading":0.0, "intervals":49, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
-    {"x":5.88, "y":3.8, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+    {"x":6.300933361053467, "y":3.7982170581817623, "heading":0.0, "intervals":52, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":5.85, "y":3.7, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
@@ -18,8 +18,8 @@
  "params":{
   "waypoints":[
     {"x":{"exp":"blueStartingPose.x", "val":7.2}, "y":{"exp":"blueStartingPose.y", "val":3.8}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":62, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"6.300933361053467 m", "val":6.300933361053467}, "y":{"exp":"3.7982170581817627 m", "val":3.7982170581817623}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":49, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
-    {"x":{"exp":"blueL4H.x", "val":5.88}, "y":{"exp":"blueL4H.y", "val":3.8}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+    {"x":{"exp":"6.300933361053467 m", "val":6.300933361053467}, "y":{"exp":"3.7982170581817627 m", "val":3.7982170581817623}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":52, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":{"exp":"blueL4G.x", "val":5.85}, "y":{"exp":"3.7 m", "val":3.7}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
@@ -34,94 +34,97 @@
  },
  "trajectory":{
   "sampleType":"Swerve",
-  "waypoints":[0.0,2.16171,3.75334],
+  "waypoints":[0.0,2.16185,3.88915],
   "samples":[
-    {"t":0.0, "x":7.2, "y":3.8, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.79963, "ay":-0.00165, "alpha":-0.00048, "fx":[-9.05869,-9.08977,-9.06105,-9.06105], "fy":[-0.02159,-0.01575,-0.01575,-0.02159]},
-    {"t":0.04412, "x":7.19922, "y":3.8, "heading":3.14159, "vx":-0.03528, "vy":-0.00007, "omega":-0.00002, "ax":-0.7998, "ay":-0.00165, "alpha":0.00008, "fx":[-9.06402,-9.08171,-9.06628,-9.06628], "fy":[-0.02149,-0.01586,-0.01586,-0.02149]},
-    {"t":0.08823, "x":7.19689, "y":3.79999, "heading":3.14159, "vx":-0.07056, "vy":-0.00015, "omega":-0.00002, "ax":-0.79978, "ay":-0.00165, "alpha":0.00074, "fx":[-9.06736,-9.06736,-9.07141,-9.07141], "fy":[-0.02025,-0.01681,-0.01738,-0.02025]},
-    {"t":0.13235, "x":7.193, "y":3.79999, "heading":3.14159, "vx":-0.10584, "vy":-0.00022, "omega":0.00001, "ax":-0.79976, "ay":-0.00165, "alpha":-0.00011, "fx":[-9.0628,-9.08396,-9.06495,-9.06495], "fy":[-0.02137,-0.01598,-0.01598,-0.02137]},
-    {"t":0.17647, "x":7.18755, "y":3.79997, "heading":3.14159, "vx":-0.14113, "vy":-0.00029, "omega":0.00001, "ax":-0.79974, "ay":-0.00165, "alpha":0.0007, "fx":[-9.06695,-9.06695,-9.07084,-9.07084], "fy":[-0.02017,-0.0169,-0.01745,-0.02017]},
-    {"t":0.22058, "x":7.18054, "y":3.79996, "heading":3.14159, "vx":-0.17641, "vy":-0.00036, "omega":0.00004, "ax":-0.79971, "ay":-0.00165, "alpha":-0.00034, "fx":[-9.06114,-9.08673,-9.0632,-9.0632], "fy":[-0.02128,-0.01606,-0.01606,-0.02128]},
-    {"t":0.2647, "x":7.17198, "y":3.79994, "heading":-3.14159, "vx":-0.21169, "vy":-0.00044, "omega":0.00003, "ax":-0.79967, "ay":-0.00165, "alpha":0.00067, "fx":[-9.06628,-9.06628,-9.07004,-9.07004], "fy":[-0.0201,-0.01699,-0.0175,-0.0201]},
-    {"t":0.30882, "x":7.16187, "y":3.79992, "heading":-3.14159, "vx":-0.24697, "vy":-0.00051, "omega":0.00006, "ax":-0.79963, "ay":-0.00165, "alpha":-0.00068, "fx":[-9.05844,-9.09127,-9.06042,-9.06042], "fy":[-0.02122,-0.01612,-0.01612,-0.02122]},
-    {"t":0.35293, "x":7.15019, "y":3.7999, "heading":-3.14159, "vx":-0.28225, "vy":-0.00058, "omega":0.00003, "ax":-0.79957, "ay":-0.00165, "alpha":0.00064, "fx":[-9.06512,-9.06512,-9.06875,-9.06875], "fy":[-0.02002,-0.01707,-0.01756,-0.02002]},
-    {"t":0.39705, "x":7.13696, "y":3.79987, "heading":-3.14159, "vx":-0.31752, "vy":-0.00065, "omega":0.00005, "ax":-0.79948, "ay":-0.00165, "alpha":-0.00123, "fx":[-9.05382,-9.09858,-9.05571,-9.05571], "fy":[-0.02116,-0.01618,-0.01618,-0.02116]},
-    {"t":0.44117, "x":7.12218, "y":3.79984, "heading":-3.14158, "vx":-0.35279, "vy":-0.00073, "omega":0.0, "ax":-0.79935, "ay":-0.00165, "alpha":0.00061, "fx":[-9.06274,-9.06274,-9.06622,-9.06622], "fy":[-0.01991,-0.0172,-0.01764,-0.01991]},
-    {"t":0.48528, "x":7.10584, "y":3.79981, "heading":-3.14158, "vx":-0.38805, "vy":-0.0008, "omega":0.00003, "ax":-0.79913, "ay":-0.00165, "alpha":-0.00229, "fx":[-9.04413,-9.11215,-9.04589,-9.04589], "fy":[-0.02108,-0.01624,-0.01624,-0.02108]},
-    {"t":0.5294, "x":7.08794, "y":3.79977, "heading":-3.14158, "vx":-0.42331, "vy":-0.00087, "omega":-0.00007, "ax":-0.7987, "ay":-0.00164, "alpha":0.00054, "fx":[-9.05546,-9.05546,-9.05868,-9.05868], "fy":[-0.01972,-0.01741,-0.01775,-0.01972]},
-    {"t":0.57351, "x":7.06849, "y":3.79973, "heading":-3.14159, "vx":-0.45855, "vy":-0.00094, "omega":-0.00005, "ax":-0.79738, "ay":-0.00164, "alpha":-0.00553, "fx":[-9.00638,-9.14634,-9.00788,-9.00788], "fy":[-0.02097,-0.01626,-0.01627,-0.02097]},
-    {"t":0.61763, "x":7.04748, "y":3.79969, "heading":-3.14159, "vx":-0.49372, "vy":-0.00102, "omega":-0.00029, "ax":-0.13968, "ay":-0.00029, "alpha":0.00044, "fx":[-1.58264,-1.58264,-1.58529,-1.58529], "fy":[-0.00412,-0.00216,-0.00269,-0.00412]},
-    {"t":0.66175, "x":7.02556, "y":3.79964, "heading":3.14158, "vx":-0.49989, "vy":-0.00103, "omega":-0.00028, "ax":0.0, "ay":0.0, "alpha":0.00336, "fx":[-0.01651,0.05058,-0.01711,-0.01711], "fy":[-0.00135,0.00138,0.00138,-0.00135]},
-    {"t":0.70586, "x":7.00351, "y":3.7996, "heading":3.14157, "vx":-0.49989, "vy":-0.00103, "omega":-0.00013, "ax":0.0, "ay":0.0, "alpha":0.00031, "fx":[0.00102,0.00102,-0.00102,-0.00102], "fy":[-0.00056,0.00077,0.00036,-0.00056]},
-    {"t":0.74998, "x":6.98146, "y":3.79955, "heading":3.14157, "vx":-0.49989, "vy":-0.00103, "omega":-0.00011, "ax":0.0, "ay":0.0, "alpha":0.00105, "fx":[-0.00383,0.0125,-0.00434,-0.00434], "fy":[-0.00117,0.00116,0.00116,-0.00117]},
-    {"t":0.7941, "x":6.9594, "y":3.7995, "heading":3.14156, "vx":-0.49989, "vy":-0.00103, "omega":-0.00007, "ax":0.0, "ay":0.0, "alpha":0.00024, "fx":[0.00082,0.00082,-0.00082,-0.00082], "fy":[-0.0004,0.00057,0.00024,-0.0004]},
-    {"t":0.83821, "x":6.93735, "y":3.79946, "heading":3.14156, "vx":-0.49989, "vy":-0.00103, "omega":-0.00006, "ax":0.0, "ay":0.0, "alpha":0.0005, "fx":[-0.0011,0.00409,-0.00149,-0.00149], "fy":[-0.00097,0.00097,0.00097,-0.00097]},
-    {"t":0.88233, "x":6.9153, "y":3.79941, "heading":3.14156, "vx":-0.49989, "vy":-0.00103, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":0.00019, "fx":[0.00067,0.00067,-0.00067,-0.00067], "fy":[-0.0003,0.00044,0.00017,-0.0003]},
-    {"t":0.92645, "x":6.89324, "y":3.79937, "heading":3.14155, "vx":-0.49989, "vy":-0.00103, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":0.00027, "fx":[-0.00012,0.00094,-0.00041,-0.00041], "fy":[-0.00081,0.00081,0.00081,-0.00081]},
-    {"t":0.97056, "x":6.87119, "y":3.79932, "heading":3.14155, "vx":-0.49989, "vy":-0.00103, "omega":-0.00001, "ax":0.0, "ay":0.0, "alpha":0.00015, "fx":[0.00056,0.00056,-0.00056,-0.00056], "fy":[-0.00023,0.00035,0.00011,-0.00023]},
-    {"t":1.01468, "x":6.84914, "y":3.79928, "heading":3.14155, "vx":-0.49989, "vy":-0.00103, "omega":-0.00001, "ax":0.0, "ay":0.0, "alpha":0.00019, "fx":[0.00004,0.00032,-0.00018,-0.00018], "fy":[-0.00068,0.00067,0.00067,-0.00068]},
-    {"t":1.0588, "x":6.82708, "y":3.79923, "heading":3.14155, "vx":-0.49989, "vy":-0.00103, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.00012, "fx":[0.00046,0.00046,-0.00046,-0.00046], "fy":[-0.00017,0.00028,0.00007,-0.00017]},
-    {"t":1.10291, "x":6.80503, "y":3.79919, "heading":3.14155, "vx":-0.49989, "vy":-0.00103, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":0.00015, "fx":[0.00003,0.0002,-0.00012,-0.00012], "fy":[-0.00056,0.00056,0.00056,-0.00056]},
-    {"t":1.14703, "x":6.78298, "y":3.79914, "heading":3.14155, "vx":-0.49989, "vy":-0.00103, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":0.0001, "fx":[0.00039,0.00039,-0.00039,-0.00039], "fy":[-0.00013,0.00022,0.00004,-0.00013]},
-    {"t":1.19115, "x":6.76092, "y":3.7991, "heading":3.14155, "vx":-0.49989, "vy":-0.00103, "omega":0.00002, "ax":0.0, "ay":0.0, "alpha":0.00013, "fx":[0.00002,0.00015,-0.00009,-0.00009], "fy":[-0.00047,0.00047,0.00047,-0.00047]},
-    {"t":1.23526, "x":6.73887, "y":3.79905, "heading":3.14155, "vx":-0.49989, "vy":-0.00103, "omega":0.00002, "ax":0.0, "ay":0.0, "alpha":0.00008, "fx":[0.00032,0.00032,-0.00032,-0.00032], "fy":[-0.00009,0.00017,0.00002,-0.00009]},
-    {"t":1.27938, "x":6.71682, "y":3.79901, "heading":3.14155, "vx":-0.49989, "vy":-0.00103, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":0.0001, "fx":[0.0,0.00012,-0.00006,-0.00006], "fy":[-0.00039,0.00039,0.00039,-0.00039]},
-    {"t":1.3235, "x":6.69477, "y":3.79896, "heading":3.14156, "vx":-0.49989, "vy":-0.00103, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":0.00006, "fx":[0.00027,0.00027,-0.00027,-0.00027], "fy":[-0.00006,0.00013,0.0,-0.00006]},
-    {"t":1.36761, "x":6.67271, "y":3.79891, "heading":3.14156, "vx":-0.49989, "vy":-0.00103, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":0.00008, "fx":[-0.00001,0.00009,-0.00004,-0.00004], "fy":[-0.00033,0.00033,0.00033,-0.00033]},
-    {"t":1.41173, "x":6.65066, "y":3.79887, "heading":3.14156, "vx":-0.49989, "vy":-0.00103, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":0.00005, "fx":[0.00023,0.00023,-0.00023,-0.00023], "fy":[-0.00004,0.00009,-0.00002,-0.00004]},
-    {"t":1.45585, "x":6.62861, "y":3.79882, "heading":3.14156, "vx":-0.49989, "vy":-0.00103, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":0.00007, "fx":[0.0,0.00002,-0.00001,-0.00001], "fy":[-0.00028,0.00028,0.00028,-0.00028]},
-    {"t":1.49996, "x":6.60655, "y":3.79878, "heading":3.14156, "vx":-0.49989, "vy":-0.00103, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":0.00004, "fx":[0.00019,0.00019,-0.00019,-0.00019], "fy":[-0.00002,0.00006,-0.00003,-0.00002]},
-    {"t":1.54408, "x":6.5845, "y":3.79873, "heading":3.14156, "vx":-0.49989, "vy":-0.00103, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":0.00003, "fx":[0.0001,-0.00035,0.00012,0.00012], "fy":[-0.00023,0.00023,0.00023,-0.00023]},
-    {"t":1.58819, "x":6.56245, "y":3.79869, "heading":3.14157, "vx":-0.49989, "vy":-0.00103, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":0.00003, "fx":[0.00016,0.00016,-0.00016,-0.00016], "fy":[0.0,0.00006,-0.00004,0.0]},
-    {"t":1.63231, "x":6.54039, "y":3.79864, "heading":3.14157, "vx":-0.49989, "vy":-0.00103, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":-0.0001, "fx":[0.0008,-0.00247,0.00083,0.00083], "fy":[-0.00019,0.00021,0.00021,-0.00019]},
-    {"t":1.67643, "x":6.51834, "y":3.7986, "heading":3.14157, "vx":-0.49989, "vy":-0.00103, "omega":0.00004, "ax":0.0, "ay":0.00001, "alpha":0.00002, "fx":[0.00013,0.00013,-0.00013,-0.00013], "fy":[0.00009,0.00008,0.0,0.00009]},
-    {"t":1.72054, "x":6.49629, "y":3.79855, "heading":3.14157, "vx":-0.49989, "vy":-0.00103, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00056, "fx":[0.00335,-0.01014,0.00339,0.00339], "fy":[-0.00023,0.00013,0.00013,-0.00023]},
-    {"t":1.76466, "x":6.47423, "y":3.79851, "heading":3.14157, "vx":-0.49989, "vy":-0.00103, "omega":0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00004, "fx":[0.00008,0.00008,-0.00008,-0.00008], "fy":[0.00026,-0.00051,0.00008,0.00026]},
-    {"t":1.80878, "x":6.45218, "y":3.79846, "heading":3.14157, "vx":-0.49989, "vy":-0.00103, "omega":0.00002, "ax":0.0, "ay":-0.00006, "alpha":-0.00133, "fx":[0.00755,-0.02281,0.00763,0.00763], "fy":[-0.0008,-0.00053,-0.00053,-0.0008]},
-    {"t":1.85289, "x":6.43013, "y":3.79841, "heading":3.14158, "vx":-0.49989, "vy":-0.00103, "omega":-0.00004, "ax":0.00002, "ay":0.00101, "alpha":-0.00032, "fx":[0.00021,0.00021,0.00021,0.00021], "fy":[0.01282,0.00777,0.01245,0.01282]},
-    {"t":1.89701, "x":6.40807, "y":3.79837, "heading":3.14157, "vx":-0.49988, "vy":-0.00099, "omega":-0.00006, "ax":0.53753, "ay":0.00627, "alpha":0.00014, "fx":[6.09398,6.09883,6.09456,6.09456], "fy":[0.07126,0.07101,0.07101,0.07126]},
-    {"t":1.94113, "x":6.38654, "y":3.79833, "heading":3.14157, "vx":-0.47617, "vy":-0.00071, "omega":-0.00005, "ax":0.79743, "ay":0.00171, "alpha":-0.00013, "fx":[9.04256,9.04256,9.04287,9.04287], "fy":[0.01981,0.0187,0.01915,0.01981]},
-    {"t":1.98524, "x":6.36631, "y":3.7983, "heading":3.14157, "vx":-0.44099, "vy":-0.00063, "omega":-0.00006, "ax":0.79871, "ay":0.00168, "alpha":0.00338, "fx":[9.03682,9.11695,9.03756,9.03756], "fy":[0.01953,0.01853,0.01853,0.01953]},
-    {"t":2.02936, "x":6.34763, "y":3.79828, "heading":3.14157, "vx":-0.40575, "vy":-0.00056, "omega":0.00009, "ax":0.79914, "ay":0.00167, "alpha":-0.00015, "fx":[9.06185,9.06185,9.06232,9.06232], "fy":[0.01937,0.01831,0.01861,0.01937]},
-    {"t":2.07348, "x":6.33051, "y":3.79825, "heading":3.14157, "vx":-0.3705, "vy":-0.00049, "omega":0.00009, "ax":0.79935, "ay":0.00166, "alpha":0.00205, "fx":[9.05175,9.10149,9.05241,9.05241], "fy":[0.01929,0.01843,0.01843,0.01929]},
-    {"t":2.11759, "x":6.31494, "y":3.79823, "heading":3.14157, "vx":-0.33523, "vy":-0.00041, "omega":0.00018, "ax":0.79948, "ay":0.00166, "alpha":-0.00013, "fx":[9.06579,9.06579,9.06617,9.06617], "fy":[0.01921,0.01831,0.01857,0.01921]},
-    {"t":2.16171, "x":6.30093, "y":3.79822, "heading":3.14158, "vx":-0.29996, "vy":-0.00034, "omega":0.00017, "ax":0.00077, "ay":0.03476, "alpha":-0.00117, "fx":[0.01474,-0.01007,0.01514,0.01514], "fy":[0.39431,0.39402,0.39402,0.39431]},
-    {"t":2.20592, "x":6.28767, "y":3.79824, "heading":3.14159, "vx":-0.29993, "vy":0.0012, "omega":0.00012, "ax":0.00001, "ay":0.00239, "alpha":-0.00101, "fx":[0.00002,0.00002,0.00021,0.00021], "fy":[0.03125,0.01496,0.03085,0.03125]},
-    {"t":2.25013, "x":6.27441, "y":3.79829, "heading":-3.14159, "vx":-0.29993, "vy":0.0013, "omega":0.00008, "ax":0.0, "ay":-0.00007, "alpha":-0.00027, "fx":[0.00101,-0.00365,0.00131,0.00131], "fy":[-0.0006,-0.00091,-0.00091,-0.0006]},
-    {"t":2.29435, "x":6.26115, "y":3.79835, "heading":-3.14159, "vx":-0.29993, "vy":0.0013, "omega":0.00006, "ax":0.0, "ay":0.00001, "alpha":-0.00008, "fx":[-0.00012,-0.00012,0.00012,0.00012], "fy":[0.00039,-0.00017,-0.00001,0.00039]},
-    {"t":2.33856, "x":6.24789, "y":3.79841, "heading":-3.14158, "vx":-0.29993, "vy":0.0013, "omega":0.00006, "ax":0.0, "ay":0.0, "alpha":-0.00008, "fx":[-0.00003,-0.00042,0.00023,0.00023], "fy":[0.00021,-0.00012,-0.00012,0.00021]},
-    {"t":2.38277, "x":6.23463, "y":3.79846, "heading":-3.14158, "vx":-0.29993, "vy":0.0013, "omega":0.00006, "ax":0.0, "ay":0.0, "alpha":-0.00007, "fx":[-0.00013,-0.00013,0.00013,0.00013], "fy":[0.0002,-0.00022,-0.00013,0.0002]},
-    {"t":2.42698, "x":6.22137, "y":3.79852, "heading":-3.14158, "vx":-0.29993, "vy":0.0013, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":-0.00006, "fx":[-0.00011,-0.00014,0.00012,0.00012], "fy":[0.00017,-0.00018,-0.00018,0.00017]},
-    {"t":2.47119, "x":6.20811, "y":3.79858, "heading":-3.14158, "vx":-0.29993, "vy":0.0013, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":-0.00007, "fx":[-0.00014,-0.00014,0.00014,0.00014], "fy":[0.00017,-0.00023,-0.00012,0.00017]},
-    {"t":2.5154, "x":6.19485, "y":3.79864, "heading":-3.14157, "vx":-0.29993, "vy":0.0013, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":-0.00007, "fx":[-0.0001,-0.00012,0.00011,0.00011], "fy":[0.0002,-0.0002,-0.0002,0.0002]},
-    {"t":2.55962, "x":6.18159, "y":3.79869, "heading":-3.14157, "vx":-0.29993, "vy":0.0013, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00007, "fx":[-0.00016,-0.00016,0.00016,0.00016], "fy":[0.00016,-0.0002,-0.00011,0.00016]},
-    {"t":2.60383, "x":6.16833, "y":3.79875, "heading":-3.14157, "vx":-0.29993, "vy":0.0013, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00008, "fx":[-0.0001,-0.00011,0.00011,0.00011], "fy":[0.00024,-0.00024,-0.00024,0.00024]},
-    {"t":2.64804, "x":6.15507, "y":3.79881, "heading":-3.14157, "vx":-0.29993, "vy":0.0013, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00007, "fx":[-0.0002,-0.0002,0.0002,0.0002], "fy":[0.00016,-0.00019,-0.00012,0.00016]},
-    {"t":2.69225, "x":6.14181, "y":3.79887, "heading":-3.14157, "vx":-0.29993, "vy":0.0013, "omega":0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00009, "fx":[-0.0001,-0.00012,0.00011,0.00011], "fy":[0.00029,-0.00029,-0.00029,0.00029]},
-    {"t":2.73646, "x":6.12855, "y":3.79892, "heading":-3.14157, "vx":-0.29993, "vy":0.0013, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00008, "fx":[-0.00024,-0.00024,0.00024,0.00024], "fy":[0.00017,-0.0002,-0.00013,0.00017]},
-    {"t":2.78068, "x":6.11529, "y":3.79898, "heading":-3.14156, "vx":-0.29993, "vy":0.0013, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":-0.0001, "fx":[-0.00011,-0.00014,0.00013,0.00013], "fy":[0.00035,-0.00035,-0.00035,0.00035]},
-    {"t":2.82489, "x":6.10203, "y":3.79904, "heading":-3.14156, "vx":-0.29993, "vy":0.0013, "omega":0.00002, "ax":0.0, "ay":0.0, "alpha":-0.0001, "fx":[-0.00029,-0.00029,0.00029,0.00029], "fy":[0.00019,-0.00022,-0.00015,0.00019]},
-    {"t":2.8691, "x":6.08877, "y":3.7991, "heading":-3.14156, "vx":-0.29993, "vy":0.0013, "omega":0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00013, "fx":[-0.00012,-0.00018,0.00015,0.00015], "fy":[0.00042,-0.00042,-0.00042,0.00042]},
-    {"t":2.91331, "x":6.07551, "y":3.79915, "heading":-3.14156, "vx":-0.29993, "vy":0.0013, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00012, "fx":[-0.00035,-0.00035,0.00035,0.00035], "fy":[0.00022,-0.00027,-0.00018,0.00022]},
-    {"t":2.95752, "x":6.06224, "y":3.79921, "heading":-3.14156, "vx":-0.29993, "vy":0.0013, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00016, "fx":[-0.00015,-0.00023,0.00019,0.00019], "fy":[0.00052,-0.00052,-0.00052,0.00052]},
-    {"t":3.00174, "x":6.04898, "y":3.79927, "heading":-3.14156, "vx":-0.29993, "vy":0.0013, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00014, "fx":[-0.00043,-0.00043,0.00043,0.00043], "fy":[0.00027,-0.00033,-0.00022,0.00027]},
-    {"t":3.04595, "x":6.03572, "y":3.79933, "heading":-3.14156, "vx":-0.29993, "vy":0.0013, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":-0.00019, "fx":[-0.00018,-0.00029,0.00023,0.00023], "fy":[0.00064,-0.00064,-0.00064,0.00064]},
-    {"t":3.09016, "x":6.02246, "y":3.79938, "heading":-3.14156, "vx":-0.29993, "vy":0.0013, "omega":-0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00018, "fx":[-0.00053,-0.00053,0.00053,0.00053], "fy":[0.00034,-0.0004,-0.00028,0.00034]},
-    {"t":3.13437, "x":6.0092, "y":3.79944, "heading":-3.14156, "vx":-0.29993, "vy":0.0013, "omega":-0.00002, "ax":0.0, "ay":0.0, "alpha":-0.00026, "fx":[-0.00012,-0.00067,0.0004,0.0004], "fy":[0.00078,-0.00078,-0.00078,0.00078]},
-    {"t":3.17858, "x":5.99594, "y":3.7995, "heading":-3.14156, "vx":-0.29993, "vy":0.0013, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00022, "fx":[-0.00065,-0.00065,0.00065,0.00065], "fy":[0.00043,-0.0005,-0.00036,0.00043]},
-    {"t":3.22279, "x":5.98268, "y":3.79956, "heading":-3.14156, "vx":-0.29993, "vy":0.0013, "omega":-0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00058, "fx":[0.00136,-0.00535,0.00199,0.00199], "fy":[0.00095,-0.00095,-0.00095,0.00095]},
-    {"t":3.26701, "x":5.96942, "y":3.79961, "heading":-3.14156, "vx":-0.29993, "vy":0.0013, "omega":-0.00007, "ax":0.0, "ay":0.0, "alpha":-0.00027, "fx":[-0.00081,-0.00081,0.00081,0.00081], "fy":[0.00053,-0.00059,-0.00047,0.00053]},
-    {"t":3.31122, "x":5.95616, "y":3.79967, "heading":-3.14157, "vx":-0.29993, "vy":0.0013, "omega":-0.00008, "ax":0.0, "ay":0.0, "alpha":-0.00222, "fx":[0.01032,-0.03224,0.01104,0.01104], "fy":[0.0011,-0.00109,-0.00109,0.0011]},
-    {"t":3.35543, "x":5.9429, "y":3.79973, "heading":-3.14157, "vx":-0.29993, "vy":0.0013, "omega":-0.00018, "ax":0.38814, "ay":-0.00168, "alpha":-0.00036, "fx":[4.40038,4.40038,4.40245,4.40245], "fy":[-0.01832,-0.01993,-0.01971,-0.01832]},
-    {"t":3.39964, "x":5.93002, "y":3.79978, "heading":-3.14158, "vx":-0.28277, "vy":0.00123, "omega":-0.00019, "ax":0.79846, "ay":-0.00346, "alpha":0.00359, "fx":[9.03037,9.12354,9.03177,9.03177], "fy":[-0.03745,-0.04101,-0.04101,-0.03745]},
-    {"t":3.44385, "x":5.9183, "y":3.79983, "heading":-3.14159, "vx":-0.24747, "vy":0.00107, "omega":-0.00003, "ax":0.79922, "ay":-0.00346, "alpha":-0.00046, "fx":[9.06181,9.06181,9.0643,9.0643], "fy":[-0.03827,-0.04048,-0.04005,-0.03827]},
-    {"t":3.48807, "x":5.90814, "y":3.79988, "heading":-3.14159, "vx":-0.21213, "vy":0.00092, "omega":-0.00005, "ax":0.79948, "ay":-0.00346, "alpha":0.00134, "fx":[9.05442,9.09751,9.05596,9.05596], "fy":[-0.03742,-0.04114,-0.04114,-0.03742]},
-    {"t":3.53228, "x":5.89954, "y":3.79992, "heading":-3.14159, "vx":-0.17679, "vy":0.00077, "omega":0.0, "ax":0.79961, "ay":-0.00346, "alpha":-0.0005, "fx":[9.06605,9.06605,9.06878,9.06878], "fy":[-0.0382,-0.04054,-0.0402,-0.0382]},
-    {"t":3.57649, "x":5.89251, "y":3.79995, "heading":-3.14159, "vx":-0.14143, "vy":0.00061, "omega":-0.00002, "ax":0.79969, "ay":-0.00346, "alpha":0.0006, "fx":[9.06067,9.08784,9.0623,9.0623], "fy":[-0.03733,-0.04124,-0.04124,-0.03733]},
-    {"t":3.6207, "x":5.88703, "y":3.79997, "heading":-3.14159, "vx":-0.10608, "vy":0.00046, "omega":0.00001, "ax":0.79974, "ay":-0.00346, "alpha":-0.00054, "fx":[9.0674,9.0674,9.07033,9.07033], "fy":[-0.03813,-0.04061,-0.0403,-0.03813]},
-    {"t":3.66491, "x":5.88313, "y":3.79999, "heading":-3.14159, "vx":-0.07072, "vy":0.00031, "omega":-0.00001, "ax":0.79977, "ay":-0.00347, "alpha":0.00024, "fx":[9.06346,9.08325,9.0652,9.0652], "fy":[-0.03723,-0.04136,-0.04136,-0.03723]},
-    {"t":3.70912, "x":5.88078, "y":3.8, "heading":-3.14159, "vx":-0.03536, "vy":0.00015, "omega":0.0, "ax":0.7998, "ay":-0.00347, "alpha":0.0001, "fx":[9.06439,9.0816,9.06618,9.06618], "fy":[-0.03717,-0.04142,-0.04142,-0.03717]},
-    {"t":3.75334, "x":5.88, "y":3.8, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+    {"t":0.0, "x":7.2, "y":3.8, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.79963, "ay":0.00144, "alpha":-0.00002, "fx":[-9.08898,-9.02005,-9.08078,-9.08078], "fy":[0.02625,0.00639,0.00639,0.02625]},
+    {"t":0.04412, "x":7.19922, "y":3.8, "heading":3.14159, "vx":-0.03528, "vy":0.00006, "omega":0.0, "ax":-0.7998, "ay":0.00144, "alpha":-0.00131, "fx":[-9.08325,-9.04469,-9.07519,-9.07519], "fy":[0.02604,0.00661,0.00661,0.02604]},
+    {"t":0.08824, "x":7.19689, "y":3.80001, "heading":3.14159, "vx":-0.07057, "vy":0.00013, "omega":-0.00006, "ax":-0.79978, "ay":0.00144, "alpha":-0.00186, "fx":[-9.07656,-9.07656,-9.06222,-9.06222], "fy":[0.0188,0.01949,0.00819,0.0188]},
+    {"t":0.13236, "x":7.193, "y":3.80001, "heading":3.14159, "vx":-0.10585, "vy":0.00019, "omega":-0.00014, "ax":-0.79976, "ay":0.00144, "alpha":-0.00092, "fx":[-9.08463,-9.03867,-9.07668,-9.07668], "fy":[0.02585,0.00679,0.00679,0.02585]},
+    {"t":0.17648, "x":7.18755, "y":3.80002, "heading":3.14158, "vx":-0.14114, "vy":0.00025, "omega":-0.00018, "ax":-0.79974, "ay":0.00144, "alpha":-0.00184, "fx":[-9.07596,-9.07596,-9.06184,-9.06184], "fy":[0.01877,0.01949,0.00825,0.01877]},
+    {"t":0.2206, "x":7.18054, "y":3.80004, "heading":3.14158, "vx":-0.17642, "vy":0.00032, "omega":-0.00026, "ax":-0.79971, "ay":0.00144, "alpha":-0.00051, "fx":[-9.08625,-9.03145,-9.0783,-9.0783], "fy":[0.02578,0.00686,0.00686,0.02578]},
+    {"t":0.26472, "x":7.17198, "y":3.80005, "heading":3.14156, "vx":-0.2117, "vy":0.00038, "omega":-0.00029, "ax":-0.79968, "ay":0.00144, "alpha":-0.00184, "fx":[-9.07519,-9.07519,-9.06114,-9.06114], "fy":[0.01882,0.0194,0.00824,0.01882]},
+    {"t":0.30884, "x":7.16186, "y":3.80007, "heading":3.14155, "vx":-0.24698, "vy":0.00044, "omega":-0.00037, "ax":-0.79963, "ay":0.00144, "alpha":0.00021, "fx":[-9.08957,-9.01798,-9.08151,-9.08151], "fy":[0.02582,0.00681,0.00681,0.02582]},
+    {"t":0.35295, "x":7.15019, "y":3.80009, "heading":3.14154, "vx":-0.28226, "vy":0.00051, "omega":-0.00036, "ax":-0.79957, "ay":0.00144, "alpha":-0.00187, "fx":[-9.07399,-9.07399,-9.05989,-9.05989], "fy":[0.01893,0.01921,0.00819,0.01893]},
+    {"t":0.39707, "x":7.13695, "y":3.80011, "heading":3.14152, "vx":-0.31754, "vy":0.00057, "omega":-0.00044, "ax":-0.79948, "ay":0.00144, "alpha":0.00146, "fx":[-9.09528,-8.99454,-9.08701,-9.08701], "fy":[0.02596,0.00667,0.00667,0.02596]},
+    {"t":0.44119, "x":7.12217, "y":3.80014, "heading":3.1415, "vx":-0.35281, "vy":0.00063, "omega":-0.00038, "ax":-0.79935, "ay":0.00144, "alpha":-0.00193, "fx":[-9.07159,-9.07159,-9.05738,-9.05738], "fy":[0.01911,0.01892,0.00811,0.01911]},
+    {"t":0.48531, "x":7.10582, "y":3.80017, "heading":3.14148, "vx":-0.38808, "vy":0.0007, "omega":-0.00046, "ax":-0.79913, "ay":0.00144, "alpha":0.00413, "fx":[-9.10696,-8.94434,-9.09839,-9.09839], "fy":[0.02617,0.00645,0.00645,0.02617]},
+    {"t":0.52943, "x":7.08792, "y":3.8002, "heading":3.14146, "vx":-0.42334, "vy":0.00076, "omega":-0.00028, "ax":-0.7987, "ay":0.00144, "alpha":-0.00198, "fx":[-9.06419,-9.06419,-9.04996,-9.04996], "fy":[0.01931,0.01855,0.00802,0.01931]},
+    {"t":0.57355, "x":7.06847, "y":3.80024, "heading":3.14145, "vx":-0.45858, "vy":0.00083, "omega":-0.00037, "ax":-0.79738, "ay":0.00144, "alpha":0.0238, "fx":[-9.20005,-8.5875,-9.19047,-9.19047], "fy":[0.02715,0.0054,0.0054,0.02715]},
+    {"t":0.61767, "x":7.04746, "y":3.80027, "heading":3.14144, "vx":-0.49375, "vy":0.00089, "omega":0.00068, "ax":-0.13895, "ay":0.00024, "alpha":-0.00208, "fx":[-1.58234,-1.58234,-1.56896,-1.56896], "fy":[0.00653,0.00252,-0.00449,0.00653]},
+    {"t":0.66179, "x":7.02554, "y":3.80031, "heading":3.14147, "vx":-0.49989, "vy":0.0009, "omega":0.00059, "ax":0.0, "ay":0.0, "alpha":0.0101, "fx":[-0.07792,0.21555,-0.06888,-0.06888], "fy":[0.00906,-0.00899,-0.00899,0.00906]},
+    {"t":0.70591, "x":7.00349, "y":3.80035, "heading":3.14149, "vx":-0.49989, "vy":0.0009, "omega":0.00104, "ax":0.0, "ay":0.00001, "alpha":-0.0021, "fx":[-0.00567,-0.00567,0.00567,0.00567], "fy":[0.00467,-0.00414,-0.00497,0.00467]},
+    {"t":0.75003, "x":6.98143, "y":3.80039, "heading":3.14154, "vx":-0.49989, "vy":0.0009, "omega":0.00095, "ax":0.0, "ay":0.0, "alpha":-0.00188, "fx":[-0.00503,0.00201,0.00151,0.00151], "fy":[0.00675,-0.00686,-0.00686,0.00675]},
+    {"t":0.79415, "x":6.95938, "y":3.80043, "heading":3.14158, "vx":-0.49989, "vy":0.0009, "omega":0.00086, "ax":0.0, "ay":0.0, "alpha":-0.00172, "fx":[-0.00456,-0.00456,0.00456,0.00456], "fy":[0.00386,-0.00367,-0.00394,0.00386]},
+    {"t":0.83827, "x":6.93732, "y":3.80047, "heading":-3.14157, "vx":-0.49989, "vy":0.0009, "omega":0.00079, "ax":0.0, "ay":0.0, "alpha":-0.00201, "fx":[-0.00105,-0.00721,0.00413,0.00413], "fy":[0.00534,-0.00538,-0.00538,0.00534]},
+    {"t":0.88239, "x":6.91527, "y":3.80051, "heading":-3.14153, "vx":-0.49989, "vy":0.0009, "omega":0.0007, "ax":0.0, "ay":0.0, "alpha":-0.00138, "fx":[-0.00355,-0.00356,0.00355,0.00356], "fy":[0.00315,-0.00323,-0.003,0.00315]},
+    {"t":0.92651, "x":6.89321, "y":3.80055, "heading":-3.1415, "vx":-0.49989, "vy":0.0009, "omega":0.00064, "ax":0.0, "ay":0.0, "alpha":-0.00152, "fx":[-0.00098,-0.00512,0.00305,0.00305], "fy":[0.00409,-0.00412,-0.00412,0.00409]},
+    {"t":0.97063, "x":6.87116, "y":3.80059, "heading":-3.14147, "vx":-0.49989, "vy":0.0009, "omega":0.00057, "ax":0.0, "ay":0.0, "alpha":-0.00102, "fx":[-0.00265,-0.00265,0.00265,0.00265], "fy":[0.00234,-0.00229,-0.00236,0.00234]},
+    {"t":1.01475, "x":6.8491, "y":3.80063, "heading":-3.14145, "vx":-0.49989, "vy":0.0009, "omega":0.00053, "ax":0.0, "ay":0.0, "alpha":-0.00105, "fx":[-0.00102,-0.00293,0.00198,0.00198], "fy":[0.00296,-0.00297,-0.00297,0.00296]},
+    {"t":1.05886, "x":6.82705, "y":3.80067, "heading":-3.14143, "vx":-0.49989, "vy":0.0009, "omega":0.00048, "ax":0.0, "ay":0.0, "alpha":-0.00069, "fx":[-0.00183,-0.00183,0.00183,0.00183], "fy":[0.00153,-0.0012,-0.00184,0.00153]},
+    {"t":1.10298, "x":6.80499, "y":3.80071, "heading":-3.14141, "vx":-0.49989, "vy":0.0009, "omega":0.00045, "ax":0.0, "ay":0.0, "alpha":-0.00067, "fx":[-0.00081,-0.00167,0.00124,0.00124], "fy":[0.0019,-0.00191,-0.00191,0.0019]},
+    {"t":1.1471, "x":6.78294, "y":3.80075, "heading":-3.14139, "vx":-0.49989, "vy":0.0009, "omega":0.00042, "ax":0.0, "ay":0.0, "alpha":-0.00036, "fx":[-0.00105,-0.00105,0.00105,0.00105], "fy":[0.00072,-0.00002,-0.00142,0.00072]},
+    {"t":1.19122, "x":6.76089, "y":3.80079, "heading":-3.14137, "vx":-0.49989, "vy":0.0009, "omega":0.0004, "ax":0.0, "ay":0.0, "alpha":-0.00033, "fx":[-0.0005,-0.0008,0.00065,0.00065], "fy":[0.00089,-0.0009,-0.0009,0.00089]},
+    {"t":1.23534, "x":6.73883, "y":3.80083, "heading":-3.14135, "vx":-0.49989, "vy":0.0009, "omega":0.00039, "ax":0.0, "ay":0.0, "alpha":-0.00003, "fx":[-0.0003,-0.0003,0.0003,0.0003], "fy":[-0.0001,0.00128,-0.00107,-0.0001]},
+    {"t":1.27946, "x":6.71678, "y":3.80087, "heading":-3.14133, "vx":-0.49989, "vy":0.0009, "omega":0.00039, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00018,-0.00004,0.00011,0.00011], "fy":[-0.00012,0.00008,0.00008,-0.00012]},
+    {"t":1.32358, "x":6.69472, "y":3.80091, "heading":-3.14131, "vx":-0.49989, "vy":0.0009, "omega":0.00039, "ax":0.0, "ay":0.0, "alpha":0.0003, "fx":[0.00044,0.00044,-0.00044,-0.00044], "fy":[-0.0009,0.00258,-0.00072,-0.0009]},
+    {"t":1.3677, "x":6.67267, "y":3.80095, "heading":-3.1413, "vx":-0.49989, "vy":0.0009, "omega":0.0004, "ax":0.0, "ay":-0.00001, "alpha":0.00034, "fx":[0.00013,0.0008,-0.00046,-0.00046], "fy":[-0.00116,0.00104,0.00104,-0.00116]},
+    {"t":1.41182, "x":6.65061, "y":3.80099, "heading":-3.14128, "vx":-0.49989, "vy":0.0009, "omega":0.00042, "ax":0.0, "ay":0.0, "alpha":0.00055, "fx":[0.00121,0.00121,-0.00121,-0.00121], "fy":[-0.00137,0.00291,0.00001,-0.00137]},
+    {"t":1.45594, "x":6.62856, "y":3.80103, "heading":-3.14126, "vx":-0.49989, "vy":0.0009, "omega":0.00044, "ax":0.0, "ay":-0.00002, "alpha":0.00071, "fx":[0.00035,0.00195,-0.00115,-0.00115], "fy":[-0.00233,0.00197,0.00197,-0.00233]},
+    {"t":1.50006, "x":6.6065, "y":3.80107, "heading":-3.14124, "vx":-0.49989, "vy":0.0009, "omega":0.00047, "ax":0.0, "ay":0.00002, "alpha":0.00042, "fx":[0.00204,0.00204,-0.00204,-0.00204], "fy":[0.00002,-0.00175,0.00268,0.00002]},
+    {"t":1.54418, "x":6.58445, "y":3.80111, "heading":-3.14122, "vx":-0.49989, "vy":0.0009, "omega":0.00049, "ax":0.0, "ay":-0.00003, "alpha":0.00115, "fx":[0.00036,0.00385,-0.00211,-0.00211], "fy":[-0.00363,0.00287,0.00287,-0.00363]},
+    {"t":1.5883, "x":6.56239, "y":3.80115, "heading":-3.1412, "vx":-0.49989, "vy":0.0009, "omega":0.00054, "ax":0.0, "ay":0.0001, "alpha":-0.00131, "fx":[0.00299,0.00299,-0.00298,-0.00298], "fy":[0.00887,-0.02632,0.01303,0.00887]},
+    {"t":1.63242, "x":6.54034, "y":3.80119, "heading":-3.14118, "vx":-0.49989, "vy":0.0009, "omega":0.00048, "ax":0.0, "ay":-0.00016, "alpha":0.00162, "fx":[0.00022,0.00624,-0.00324,-0.00324], "fy":[-0.00623,0.00256,0.00256,-0.00623]},
+    {"t":1.67654, "x":6.51828, "y":3.80123, "heading":-3.14115, "vx":-0.49989, "vy":0.0009, "omega":0.00055, "ax":0.0, "ay":0.0002, "alpha":-0.00828, "fx":[0.00415,0.00415,-0.00415,-0.00415], "fy":[0.04033,-0.11775,0.04632,0.04033]},
+    {"t":1.72066, "x":6.49623, "y":3.80127, "heading":-3.14113, "vx":-0.49989, "vy":0.0009, "omega":0.00019, "ax":0.0, "ay":-0.00074, "alpha":0.00161, "fx":[0.00252,0.00106,-0.00182,-0.00182], "fy":[-0.01386,-0.00303,-0.00303,-0.01386]},
+    {"t":1.76477, "x":6.47418, "y":3.80131, "heading":-3.14112, "vx":-0.49989, "vy":0.00087, "omega":0.00026, "ax":0.0, "ay":0.00033, "alpha":-0.02495, "fx":[0.00546,0.00547,-0.00545,-0.00545], "fy":[0.11303,-0.33216,0.12095,0.11303]},
+    {"t":1.80889, "x":6.45212, "y":3.80134, "heading":-3.14111, "vx":-0.49989, "vy":0.00089, "omega":-0.00084, "ax":0.0, "ay":0.00068, "alpha":-0.00449, "fx":[0.03826,-0.10474,0.03327,0.03327], "fy":[0.00167,0.01368,0.01368,0.00167]},
+    {"t":1.85301, "x":6.43007, "y":3.80138, "heading":-3.14115, "vx":-0.49989, "vy":0.00092, "omega":-0.00104, "ax":0.00005, "ay":-0.05361, "alpha":0.0061, "fx":[0.00443,0.00443,-0.00319,-0.00319], "fy":[-0.63075,-0.54391,-0.62616,-0.63075]},
+    {"t":1.89713, "x":6.40801, "y":3.80137, "heading":-3.14119, "vx":-0.49988, "vy":-0.00145, "omega":-0.00077, "ax":0.54381, "ay":-0.25032, "alpha":0.02315, "fx":[6.04389,6.53977,6.04158,6.04158], "fy":[-2.84242,-2.83469,-2.83474,-2.84242]},
+    {"t":1.94125, "x":6.38649, "y":3.80106, "heading":-3.14123, "vx":-0.47589, "vy":-0.01249, "omega":0.00025, "ax":0.79742, "ay":-0.00526, "alpha":-0.00774, "fx":[9.04687,9.04687,9.03834,9.03834], "fy":[-0.02387,-0.17261,-0.01846,-0.02386]},
+    {"t":1.98537, "x":6.36627, "y":3.80051, "heading":-3.14122, "vx":-0.44071, "vy":-0.01273, "omega":-0.00009, "ax":0.79871, "ay":-0.00338, "alpha":-0.00683, "fx":[9.10705,8.91537,9.10315,9.10315], "fy":[-0.04403,-0.03273,-0.03273,-0.04403]},
+    {"t":2.02949, "x":6.3476, "y":3.79994, "heading":-3.14122, "vx":-0.40547, "vy":-0.01287, "omega":-0.00039, "ax":0.79914, "ay":-0.00275, "alpha":-0.00316, "fx":[9.06602,9.06602,9.05809,9.05809], "fy":[-0.01488,-0.08497,-0.00994,-0.01488]},
+    {"t":2.07361, "x":6.33049, "y":3.79937, "heading":-3.14124, "vx":-0.37021, "vy":-0.013, "omega":-0.00053, "ax":0.79935, "ay":-0.00243, "alpha":-0.0034, "fx":[9.09391,8.98329,9.0904,9.0904], "fy":[-0.03259,-0.02247,-0.02247,-0.03259]},
+    {"t":2.11773, "x":6.31493, "y":3.7988, "heading":-3.14126, "vx":-0.33495, "vy":-0.0131, "omega":-0.00068, "ax":0.79948, "ay":-0.00223, "alpha":-0.002, "fx":[9.06946,9.06946,9.06247,9.06247], "fy":[-0.01429,-0.06277,-0.00998,-0.01429]},
+    {"t":2.16185, "x":6.30093, "y":3.79822, "heading":-3.14129, "vx":-0.29967, "vy":-0.0132, "omega":-0.00077, "ax":0.07594, "ay":-0.75118, "alpha":0.00006, "fx":[0.86928,0.84252,0.8664,0.8664], "fy":[-8.52242,-8.51403,-8.51403,-8.52241]},
+    {"t":2.20614, "x":6.28774, "y":3.7969, "heading":-3.14132, "vx":-0.29631, "vy":-0.04647, "omega":-0.00077, "ax":0.07594, "ay":-0.40381, "alpha":0.04572, "fx":[0.86213,0.86215,0.86012,0.86012], "fy":[-4.77114,-4.00338,-4.7707,-4.77114]},
+    {"t":2.25043, "x":6.27469, "y":3.79444, "heading":-3.14136, "vx":-0.29295, "vy":-0.06436, "omega":0.00126, "ax":0.00373, "ay":-0.01687, "alpha":0.0064, "fx":[0.01462,0.13115,0.0117,0.0117], "fy":[-0.19542,-0.18729,-0.18729,-0.19542]},
+    {"t":2.29472, "x":6.26172, "y":3.79157, "heading":-3.1413, "vx":-0.29278, "vy":-0.0651, "omega":0.00154, "ax":0.00005, "ay":-0.00024, "alpha":-0.04874, "fx":[0.00646,0.00646,-0.00523,-0.00523], "fy":[0.20716,-0.64086,0.2155,0.20716]},
+    {"t":2.33901, "x":6.24875, "y":3.78869, "heading":-3.14123, "vx":-0.29278, "vy":-0.06511, "omega":-0.00062, "ax":0.00004, "ay":-0.0002, "alpha":0.00343, "fx":[-0.00913,0.03631,-0.01258,-0.01258], "fy":[-0.00691,0.00237,0.00237,-0.00691]},
+    {"t":2.3833, "x":6.23578, "y":3.78581, "heading":-3.14126, "vx":-0.29278, "vy":-0.06512, "omega":-0.00046, "ax":-0.00001, "ay":0.00006, "alpha":-0.00489, "fx":[0.00345,0.00345,-0.00376,-0.00376], "fy":[0.02404,-0.07415,0.02884,0.02404]},
+    {"t":2.42759, "x":6.22281, "y":3.78292, "heading":-3.14128, "vx":-0.29278, "vy":-0.06512, "omega":-0.00068, "ax":0.0, "ay":-0.00002, "alpha":0.00123, "fx":[0.00156,0.00149,-0.00143,-0.00143], "fy":[-0.00432,0.00388,0.00388,-0.00432]},
+    {"t":2.47188, "x":6.20985, "y":3.78004, "heading":-3.14131, "vx":-0.29278, "vy":-0.06512, "omega":-0.00063, "ax":0.0, "ay":0.00001, "alpha":0.00118, "fx":[0.0028,0.0028,-0.00283,-0.00283], "fy":[-0.00278,0.00508,0.00075,-0.00278]},
+    {"t":2.51617, "x":6.19688, "y":3.77715, "heading":-3.14134, "vx":-0.29278, "vy":-0.06512, "omega":-0.00057, "ax":0.0, "ay":0.0, "alpha":0.00092, "fx":[0.00186,-0.00071,-0.00055,-0.00055], "fy":[-0.00349,0.00341,0.00341,-0.00349]},
+    {"t":2.56046, "x":6.18391, "y":3.77427, "heading":-3.14137, "vx":-0.29278, "vy":-0.06512, "omega":-0.00053, "ax":0.0, "ay":0.0, "alpha":0.00124, "fx":[0.00235,0.00236,-0.00236,-0.00236], "fy":[-0.00344,0.00752,-0.00061,-0.00344]},
+    {"t":2.60475, "x":6.17095, "y":3.77139, "heading":-3.14139, "vx":-0.29278, "vy":-0.06512, "omega":-0.00048, "ax":0.0, "ay":0.0, "alpha":0.0008, "fx":[0.00129,0.00001,-0.00065,-0.00065], "fy":[-0.0029,0.00291,0.00291,-0.0029]},
+    {"t":2.64904, "x":6.15798, "y":3.7685, "heading":-3.14141, "vx":-0.29278, "vy":-0.06512, "omega":-0.00044, "ax":0.0, "ay":0.0, "alpha":0.00087, "fx":[0.002,0.002,-0.002,-0.002], "fy":[-0.00218,0.00425,0.00011,-0.00218]},
+    {"t":2.69333, "x":6.14501, "y":3.76562, "heading":-3.14143, "vx":-0.29278, "vy":-0.06512, "omega":-0.00041, "ax":0.0, "ay":0.0, "alpha":0.0007, "fx":[0.00088,0.00045,-0.00067,-0.00067], "fy":[-0.00245,0.00246,0.00246,-0.00245]},
+    {"t":2.73762, "x":6.13205, "y":3.76273, "heading":-3.14145, "vx":-0.29278, "vy":-0.06512, "omega":-0.00037, "ax":0.0, "ay":0.0, "alpha":0.00062, "fx":[0.0017,0.0017,-0.0017,-0.0017], "fy":[-0.00134,0.00218,0.00049,-0.00134]},
+    {"t":2.7819, "x":6.11908, "y":3.75985, "heading":-3.14146, "vx":-0.29278, "vy":-0.06512, "omega":-0.00035, "ax":0.0, "ay":0.0, "alpha":0.0006, "fx":[0.00062,0.00056,-0.0006,-0.0006], "fy":[-0.00208,0.00209,0.00209,-0.00208]},
+    {"t":2.82619, "x":6.10611, "y":3.75697, "heading":-3.14148, "vx":-0.29278, "vy":-0.06512, "omega":-0.00032, "ax":0.0, "ay":0.0, "alpha":0.00046, "fx":[0.00145,0.00145,-0.00145,-0.00145], "fy":[-0.00087,0.00116,0.00056,-0.00087]},
+    {"t":2.87048, "x":6.09314, "y":3.75408, "heading":-3.14149, "vx":-0.29278, "vy":-0.06512, "omega":-0.0003, "ax":0.0, "ay":0.0, "alpha":0.00051, "fx":[0.00045,0.00052,-0.00049,-0.00049], "fy":[-0.00178,0.00179,0.00179,-0.00178]},
+    {"t":2.91477, "x":6.08018, "y":3.7512, "heading":-3.14151, "vx":-0.29278, "vy":-0.06512, "omega":-0.00028, "ax":0.0, "ay":0.0, "alpha":0.00036, "fx":[0.00124,0.00124,-0.00124,-0.00124], "fy":[-0.0006,0.00067,0.00049,-0.0006]},
+    {"t":2.95906, "x":6.06721, "y":3.74831, "heading":-3.14152, "vx":-0.29278, "vy":-0.06512, "omega":-0.00026, "ax":0.0, "ay":0.0, "alpha":0.00043, "fx":[0.00031,0.00044,-0.00038,-0.00038], "fy":[-0.00153,0.00154,0.00154,-0.00153]},
+    {"t":3.00335, "x":6.05424, "y":3.74543, "heading":-3.14153, "vx":-0.29278, "vy":-0.06512, "omega":-0.00024, "ax":0.0, "ay":0.0, "alpha":0.00029, "fx":[0.00107,0.00108,-0.00107,-0.00107], "fy":[-0.00041,0.00043,0.00037,-0.00041]},
+    {"t":3.04764, "x":6.04128, "y":3.74254, "heading":-3.14154, "vx":-0.29278, "vy":-0.06512, "omega":-0.00023, "ax":0.0, "ay":0.0, "alpha":0.00037, "fx":[0.00018,0.00041,-0.0003,-0.0003], "fy":[-0.00132,0.00134,0.00134,-0.00132]},
+    {"t":3.09193, "x":6.02831, "y":3.73966, "heading":-3.14155, "vx":-0.29278, "vy":-0.06512, "omega":-0.00021, "ax":0.0, "ay":0.0, "alpha":0.00023, "fx":[0.00094,0.00094,-0.00094,-0.00094], "fy":[-0.00028,0.00029,0.00024,-0.00028]},
+    {"t":3.13622, "x":6.01534, "y":3.73678, "heading":-3.14156, "vx":-0.29278, "vy":-0.06512, "omega":-0.0002, "ax":0.0, "ay":0.0, "alpha":0.00033, "fx":[-0.00001,0.00058,-0.00029,-0.00029], "fy":[-0.00116,0.00117,0.00118,-0.00116]},
+    {"t":3.18051, "x":6.00237, "y":3.73389, "heading":-3.14157, "vx":-0.29278, "vy":-0.06512, "omega":-0.00019, "ax":0.0, "ay":0.0, "alpha":0.00019, "fx":[0.00083,0.00083,-0.00083,-0.00083], "fy":[-0.00017,0.0002,0.0001,-0.00017]},
+    {"t":3.2248, "x":5.98941, "y":3.73101, "heading":-3.14158, "vx":-0.29278, "vy":-0.06512, "omega":-0.00018, "ax":0.0, "ay":0.0, "alpha":0.00033, "fx":[-0.00038,0.00136,-0.00049,-0.00049], "fy":[-0.00104,0.00105,0.00105,-0.00104]},
+    {"t":3.26909, "x":5.97644, "y":3.72812, "heading":-3.14159, "vx":-0.29278, "vy":-0.06512, "omega":-0.00017, "ax":0.0, "ay":0.0, "alpha":0.00015, "fx":[0.00076,0.00076,-0.00076,-0.00076], "fy":[-0.00007,0.00014,-0.00002,-0.00007]},
+    {"t":3.31338, "x":5.96347, "y":3.72524, "heading":3.14159, "vx":-0.29278, "vy":-0.06512, "omega":-0.00016, "ax":0.0, "ay":0.0, "alpha":0.00045, "fx":[-0.00129,0.00375,-0.00123,-0.00123], "fy":[-0.00094,0.00097,0.00097,-0.00094]},
+    {"t":3.35767, "x":5.95051, "y":3.72236, "heading":3.14158, "vx":-0.29278, "vy":-0.06512, "omega":-0.00014, "ax":0.0, "ay":0.0, "alpha":0.0001, "fx":[0.00072,0.00072,-0.00071,-0.00071], "fy":[0.00008,-0.00021,-0.00005,0.00008]},
+    {"t":3.40196, "x":5.93754, "y":3.71947, "heading":3.14158, "vx":-0.29278, "vy":-0.06512, "omega":-0.00013, "ax":0.0, "ay":0.00001, "alpha":-0.00014, "fx":[0.00188,-0.00597,0.00202,0.00202], "fy":[-0.00089,0.00101,0.00101,-0.00089]},
+    {"t":3.44625, "x":5.92457, "y":3.71659, "heading":3.14157, "vx":-0.29278, "vy":-0.06512, "omega":-0.00014, "ax":0.00001, "ay":-0.00001, "alpha":-0.00094, "fx":[0.00079,0.00079,-0.00067,-0.00067], "fy":[0.00444,-0.01346,0.00426,0.00444]},
+    {"t":3.49054, "x":5.91161, "y":3.7137, "heading":3.14157, "vx":-0.29278, "vy":-0.06512, "omega":-0.00018, "ax":0.36724, "ay":0.08169, "alpha":0.01464, "fx":[4.08104,4.41216,4.08222,4.08222], "fy":[0.92622,0.9264,0.92642,0.92622]},
+    {"t":3.53483, "x":5.899, "y":3.7109, "heading":3.14156, "vx":-0.27651, "vy":-0.0615, "omega":0.00047, "ax":0.77942, "ay":0.17336, "alpha":-0.00253, "fx":[8.83951,8.83951,8.8375,8.8375], "fy":[1.97732,1.93168,1.97724,1.97732]},
+    {"t":3.57912, "x":5.88752, "y":3.70834, "heading":3.14158, "vx":-0.24199, "vy":-0.05382, "omega":0.00035, "ax":0.78017, "ay":0.17353, "alpha":-0.00575, "fx":[8.88196,8.74258,8.88168,8.88168], "fy":[1.96598,1.96957,1.96955,1.96598]},
+    {"t":3.62341, "x":5.87756, "y":3.70613, "heading":-3.14159, "vx":-0.20744, "vy":-0.04614, "omega":0.0001, "ax":0.78042, "ay":0.17358, "alpha":-0.00022, "fx":[8.85102,8.85103,8.84857,8.84857], "fy":[1.97024,1.96259,1.97053,1.97024]},
+    {"t":3.6677, "x":5.86914, "y":3.70426, "heading":-3.14159, "vx":-0.17287, "vy":-0.03845, "omega":0.00009, "ax":0.78054, "ay":0.17361, "alpha":-0.0028, "fx":[8.86996,8.79582,8.86952,8.86952], "fy":[1.96679,1.97065,1.97064,1.96679]},
+    {"t":3.71199, "x":5.86225, "y":3.70272, "heading":-3.14158, "vx":-0.1383, "vy":-0.03076, "omega":-0.00003, "ax":0.78062, "ay":0.17363, "alpha":0.00035, "fx":[8.85344,8.85344,8.85067,8.85067], "fy":[1.96847,1.96967,1.96901,1.96847]},
+    {"t":3.75628, "x":5.85689, "y":3.70153, "heading":-3.14158, "vx":-0.10373, "vy":-0.02307, "omega":-0.00002, "ax":0.78067, "ay":0.17364, "alpha":-0.00174, "fx":[8.86576,8.81435,8.86519,8.86519], "fy":[1.96696,1.9711,1.9711,1.96696]},
+    {"t":3.80057, "x":5.85306, "y":3.70068, "heading":-3.14159, "vx":-0.06916, "vy":-0.01538, "omega":-0.0001, "ax":0.7807, "ay":0.17365, "alpha":0.00061, "fx":[8.85453,8.85453,8.85151,8.85151], "fy":[1.96766,1.97278,1.96838,1.96766]},
+    {"t":3.84486, "x":5.85077, "y":3.70017, "heading":-3.14159, "vx":-0.03458, "vy":-0.00769, "omega":-0.00007, "ax":0.78073, "ay":0.17365, "alpha":0.00154, "fx":[8.86336,8.86336,8.86278,8.82381], "fy":[1.97299,1.96818,1.96818,1.9674]},
+    {"t":3.88915, "x":5.85, "y":3.7, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
   "splits":[0]
  },
  "events":[]
diff --git a/src/main/deploy/choreo/redCenterToL4G.traj b/src/main/deploy/choreo/redCenterToL4G.traj
index 5af929e4..11a141ec 100644
--- a/src/main/deploy/choreo/redCenterToL4G.traj
+++ b/src/main/deploy/choreo/redCenterToL4G.traj
@@ -3,30 +3,30 @@
  "version":1,
  "snapshot":{
   "waypoints":[
-    {"x":10.35, "y":4.2, "heading":0.0, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":11.279606819152832, "y":4.188701152801514, "heading":0.0, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
-    {"x":11.75, "y":4.2, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+    {"x":10.35, "y":4.22, "heading":0.0, "intervals":46, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":11.170971870422363, "y":4.225747585296631, "heading":0.0, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":11.65, "y":4.22, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false},
-    {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true},
-    {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":1.0}}, "enabled":true},
-    {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":0.3}}, "enabled":true}],
+    {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":0.5}}, "enabled":true},
+    {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":0.8}}, "enabled":true},
+    {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":0.25}}, "enabled":true}],
   "targetDt":0.05
  },
  "params":{
   "waypoints":[
-    {"x":{"exp":"redStart.x", "val":10.35}, "y":{"exp":"redStart.y", "val":4.2}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"11.279606819152832 m", "val":11.279606819152832}, "y":{"exp":"4.188701152801514 m", "val":4.188701152801514}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
-    {"x":{"exp":"redL4G.x", "val":11.75}, "y":{"exp":"redL4G.y", "val":4.2}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+    {"x":{"exp":"redStart.x", "val":10.35}, "y":{"exp":"4.22 m", "val":4.22}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":46, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"11.170971870422363 m", "val":11.170971870422363}, "y":{"exp":"4.225747585296631 m", "val":4.225747585296631}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":{"exp":"11.65 m", "val":11.65}, "y":{"exp":"4.22 m", "val":4.22}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false},
-    {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true},
-    {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"1 m / s ^ 2", "val":1.0}}}, "enabled":true},
-    {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"0.3 m / s", "val":0.3}}}, "enabled":true}],
+    {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"0.5 m / s", "val":0.5}}}, "enabled":true},
+    {"from":0, "to":2, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"0.8 m / s ^ 2", "val":0.8}}}, "enabled":true},
+    {"from":1, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"0.25 m / s", "val":0.25}}}, "enabled":true}],
   "targetDt":{
    "exp":"0.05 s",
    "val":0.05
@@ -34,86 +34,100 @@
  },
  "trajectory":{
   "sampleType":"Swerve",
-  "waypoints":[0.0,1.67494,3.39455],
+  "waypoints":[0.0,2.03338,4.1069],
   "samples":[
-    {"t":0.0, "x":10.35, "y":4.2, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.99944, "ay":-0.01292, "alpha":-0.00018, "fx":[11.33025,11.33965,11.3319,11.3319], "fy":[-0.14608,-0.14685,-0.14685,-0.14608]},
-    {"t":0.04295, "x":10.35092, "y":4.19999, "heading":0.0, "vx":0.04292, "vy":-0.00055, "omega":-0.00001, "ax":0.99967, "ay":-0.01291, "alpha":-0.00012, "fx":[11.33386,11.33993,11.33514,11.33514], "fy":[-0.14628,-0.14661,-0.14661,-0.14628]},
-    {"t":0.08589, "x":10.35369, "y":4.19995, "heading":0.0, "vx":0.08586, "vy":-0.00111, "omega":-0.00001, "ax":0.99965, "ay":-0.01291, "alpha":0.00003, "fx":[11.3359,11.3359,11.33586,11.33586], "fy":[-0.14623,-0.14554,-0.14751,-0.14623]},
-    {"t":0.12884, "x":10.3583, "y":4.19989, "heading":0.0, "vx":0.12879, "vy":-0.00166, "omega":-0.00001, "ax":0.99964, "ay":-0.0129, "alpha":-0.00027, "fx":[11.3338,11.34023,11.33443,11.33443], "fy":[-0.14649,-0.14612,-0.14612,-0.14649]},
-    {"t":0.17179, "x":10.36475, "y":4.19981, "heading":0.0, "vx":0.17172, "vy":-0.00222, "omega":-0.00002, "ax":0.99962, "ay":-0.01289, "alpha":-0.0001, "fx":[11.33582,11.33582,11.33528,11.33529], "fy":[-0.14646,-0.14507,-0.14691,-0.14646]},
-    {"t":0.21474, "x":10.37305, "y":4.1997, "heading":0.0, "vx":0.21465, "vy":-0.00277, "omega":-0.00003, "ax":0.99961, "ay":-0.01289, "alpha":-0.00042, "fx":[11.33364,11.34047,11.33367,11.33367], "fy":[-0.14663,-0.14563,-0.14563,-0.14663]},
-    {"t":0.25768, "x":10.38319, "y":4.19957, "heading":0.0, "vx":0.25758, "vy":-0.00333, "omega":-0.00005, "ax":0.99959, "ay":-0.01288, "alpha":-0.00023, "fx":[11.33565,11.33565,11.33465,11.33465], "fy":[-0.14661,-0.14462,-0.1463,-0.14661]},
-    {"t":0.30063, "x":10.39517, "y":4.19942, "heading":-0.00001, "vx":0.30051, "vy":-0.00388, "omega":-0.00006, "ax":0.99957, "ay":-0.01287, "alpha":-0.00056, "fx":[11.33335,11.34061,11.33283,11.33283], "fy":[-0.14673,-0.14511,-0.14511,-0.14673]},
-    {"t":0.34358, "x":10.409, "y":4.19924, "heading":-0.00001, "vx":0.34344, "vy":-0.00443, "omega":-0.00008, "ax":0.99954, "ay":-0.01286, "alpha":-0.00035, "fx":[11.33537,11.33537,11.33389,11.33389], "fy":[-0.1467,-0.14411,-0.14565,-0.1467]},
-    {"t":0.38652, "x":10.42467, "y":4.19904, "heading":-0.00001, "vx":0.38637, "vy":-0.00498, "omega":-0.00009, "ax":0.99952, "ay":-0.01284, "alpha":-0.00071, "fx":[11.33294,11.34059,11.33186,11.33186], "fy":[-0.14678,-0.1445,-0.1445,-0.14678]},
-    {"t":0.42947, "x":10.44219, "y":4.19881, "heading":-0.00002, "vx":0.42929, "vy":-0.00553, "omega":-0.00012, "ax":0.99948, "ay":-0.01283, "alpha":-0.00047, "fx":[11.33494,11.33494,11.33295,11.33295], "fy":[-0.14671,-0.14353,-0.14492,-0.14671]},
-    {"t":0.47242, "x":10.46154, "y":4.19856, "heading":-0.00002, "vx":0.47222, "vy":-0.00609, "omega":-0.00014, "ax":0.99944, "ay":-0.01281, "alpha":-0.00085, "fx":[11.33237,11.34021,11.33072,11.33073], "fy":[-0.14676,-0.14377,-0.14377,-0.14676]},
-    {"t":0.51537, "x":10.48275, "y":4.19829, "heading":-0.00003, "vx":0.51514, "vy":-0.00664, "omega":-0.00018, "ax":0.9994, "ay":-0.01279, "alpha":-0.0006, "fx":[11.33426,11.33426,11.33171,11.33172], "fy":[-0.1466,-0.1428,-0.14407,-0.1466]},
-    {"t":0.55831, "x":10.50579, "y":4.19799, "heading":-0.00003, "vx":0.55806, "vy":-0.00718, "omega":-0.00021, "ax":0.99934, "ay":-0.01276, "alpha":-0.00095, "fx":[11.33175,11.33859,11.32954,11.32954], "fy":[-0.1466,-0.14284,-0.14284,-0.1466]},
-    {"t":0.60126, "x":10.53068, "y":4.19767, "heading":-0.00004, "vx":0.60098, "vy":-0.00773, "omega":-0.00025, "ax":0.99927, "ay":-0.01273, "alpha":-0.00073, "fx":[11.33314,11.33314,11.33001,11.33001], "fy":[-0.14626,-0.14187,-0.14302,-0.14626]},
-    {"t":0.64421, "x":10.55741, "y":4.19733, "heading":-0.00005, "vx":0.6439, "vy":-0.00828, "omega":-0.00028, "ax":0.99919, "ay":-0.01269, "alpha":-0.00102, "fx":[11.33061,11.3359,11.32789,11.32789], "fy":[-0.14615,-0.14161,-0.14161,-0.14615]},
-    {"t":0.68715, "x":10.58599, "y":4.19696, "heading":-0.00007, "vx":0.68681, "vy":-0.00882, "omega":-0.00032, "ax":0.99907, "ay":-0.01263, "alpha":-0.00084, "fx":[11.33109,11.33109,11.32742,11.32742], "fy":[-0.14543,-0.14055,-0.14163,-0.14543]},
-    {"t":0.7301, "x":10.61641, "y":4.19657, "heading":-0.00008, "vx":0.72972, "vy":-0.00937, "omega":-0.00036, "ax":0.99891, "ay":-0.01256, "alpha":-0.00075, "fx":[11.32976,11.32669,11.32665,11.32665], "fy":[-0.14499,-0.13982,-0.13982,-0.145]},
-    {"t":0.77305, "x":10.64867, "y":4.19615, "heading":-0.0001, "vx":0.77262, "vy":-0.00991, "omega":-0.00039, "ax":0.99867, "ay":-0.01245, "alpha":-0.0009, "fx":[11.32675,11.32675,11.3228,11.3228], "fy":[-0.14347,-0.13817,-0.1395,-0.14347]},
-    {"t":0.816, "x":10.68277, "y":4.19572, "heading":-0.00011, "vx":0.81551, "vy":-0.01044, "omega":-0.00043, "ax":0.9983, "ay":-0.01227, "alpha":0.00169, "fx":[11.33681,11.27793,11.3336,11.3336], "fy":[-0.14183,-0.13644,-0.13643,-0.14183]},
-    {"t":0.85894, "x":10.71871, "y":4.19526, "heading":-0.00013, "vx":0.85838, "vy":-0.01097, "omega":-0.00036, "ax":0.99759, "ay":-0.01194, "alpha":-0.00087, "fx":[11.31407,11.31407,11.31081,11.31081], "fy":[-0.13782,-0.13087,-0.1349,-0.13782]},
-    {"t":0.90189, "x":10.7565, "y":4.19478, "heading":-0.00015, "vx":0.90122, "vy":-0.01148, "omega":-0.00039, "ax":0.99577, "ay":-0.01108, "alpha":0.03254, "fx":[11.48245,10.72797,11.47853,11.47853], "fy":[-0.12847,-0.12293,-0.1229,-0.12847]},
-    {"t":0.94484, "x":10.79612, "y":4.19427, "heading":-0.00016, "vx":0.94399, "vy":-0.01196, "omega":0.001, "ax":0.9813, "ay":-0.00436, "alpha":0.00095, "fx":[11.1258,11.1258,11.12969,11.12969], "fy":[-0.04686,-0.05186,-0.05207,-0.04686]},
-    {"t":0.98778, "x":10.83757, "y":4.19376, "heading":-0.00012, "vx":0.98613, "vy":-0.01214, "omega":0.00104, "ax":-0.99227, "ay":0.0164, "alpha":-0.03273, "fx":[-11.45946,-10.65534,-11.44685,-11.44686], "fy":[0.19397,0.17795,0.17788,0.19397]},
-    {"t":1.03073, "x":10.879, "y":4.19325, "heading":-0.00008, "vx":0.94352, "vy":-0.01144, "omega":-0.00036, "ax":-0.99675, "ay":0.01443, "alpha":0.00319, "fx":[-11.30805,-11.30805,-11.29792,-11.29792], "fy":[0.17328,0.14699,0.16097,0.17328]},
-    {"t":1.07368, "x":10.91861, "y":4.19277, "heading":-0.00009, "vx":0.90071, "vy":-0.01082, "omega":-0.00023, "ax":-0.99792, "ay":0.01391, "alpha":-0.00559, "fx":[-11.36714,-11.18383,-11.35703,-11.35703], "fy":[0.16478,0.15077,0.15076,0.16479]},
-    {"t":1.11663, "x":10.95637, "y":4.19232, "heading":-0.0001, "vx":0.85785, "vy":-0.01022, "omega":-0.00047, "ax":-0.99846, "ay":0.01368, "alpha":0.00229, "fx":[-11.32674,-11.32673,-11.31799,-11.31799], "fy":[0.16145,0.14713,0.1503,0.16145]},
-    {"t":1.15957, "x":10.99229, "y":4.19189, "heading":-0.00012, "vx":0.81497, "vy":-0.00963, "omega":-0.00037, "ax":-0.99877, "ay":0.01354, "alpha":0.00131, "fx":[-11.33293,-11.32,-11.32527,-11.32528], "fy":[0.1586,0.14846,0.14846,0.15861]},
-    {"t":1.20252, "x":11.02637, "y":4.19149, "heading":-0.00014, "vx":0.77208, "vy":-0.00905, "omega":-0.00031, "ax":-0.99897, "ay":0.01345, "alpha":0.00173, "fx":[-11.33132,-11.33131,-11.32498,-11.32498], "fy":[0.15744,0.14649,0.14872,0.15745]},
-    {"t":1.24547, "x":11.05861, "y":4.19112, "heading":-0.00015, "vx":0.72917, "vy":-0.00848, "omega":-0.00024, "ax":-0.99911, "ay":0.01339, "alpha":0.00153, "fx":[-11.3316,-11.33573,-11.32582,-11.32583], "fy":[0.15532,0.14831,0.14831,0.15532]},
-    {"t":1.28841, "x":11.089, "y":4.19076, "heading":-0.00016, "vx":0.68627, "vy":-0.0079, "omega":-0.00017, "ax":-0.99922, "ay":0.01334, "alpha":0.00127, "fx":[-11.33305,-11.33304,-11.32881,-11.32881], "fy":[0.15505,0.14655,0.14853,0.15505]},
-    {"t":1.33136, "x":11.11755, "y":4.19044, "heading":-0.00017, "vx":0.64335, "vy":-0.00733, "omega":-0.00012, "ax":-0.9993, "ay":0.01331, "alpha":0.00122, "fx":[-11.33214,-11.33933,-11.32794,-11.32794], "fy":[0.15312,0.14866,0.14866,0.15312]},
-    {"t":1.37431, "x":11.14426, "y":4.19013, "heading":-0.00017, "vx":0.60044, "vy":-0.00676, "omega":-0.00006, "ax":-0.99936, "ay":0.01328, "alpha":0.00087, "fx":[-11.33383,-11.33383,-11.3313,-11.3313], "fy":[0.15331,0.14698,0.14867,0.15331]},
-    {"t":1.41726, "x":11.16913, "y":4.18986, "heading":-0.00017, "vx":0.55752, "vy":-0.00619, "omega":-0.00003, "ax":-0.99941, "ay":0.01326, "alpha":0.0009, "fx":[-11.3326,-11.34059,-11.3297,-11.3297], "fy":[0.15151,0.14911,0.14911,0.15152]},
-    {"t":1.4602, "x":11.19215, "y":4.1896, "heading":-0.00018, "vx":0.51459, "vy":-0.00562, "omega":0.00001, "ax":-0.99946, "ay":0.01324, "alpha":0.00055, "fx":[-11.33422,-11.33422,-11.33306,-11.33306], "fy":[0.15198,0.14749,0.14892,0.15199]},
-    {"t":1.50315, "x":11.21333, "y":4.18937, "heading":-0.00018, "vx":0.47167, "vy":-0.00505, "omega":0.00004, "ax":-0.99949, "ay":0.01322, "alpha":0.00059, "fx":[-11.33303,-11.34071,-11.33123,-11.33123], "fy":[0.15029,0.14953,0.14953,0.15029]},
-    {"t":1.5461, "x":11.23266, "y":4.18917, "heading":-0.00017, "vx":0.42874, "vy":-0.00448, "omega":0.00006, "ax":-0.99952, "ay":0.01321, "alpha":0.00029, "fx":[-11.33442,-11.33442,-11.33437,-11.33438], "fy":[0.15095,0.14791,0.14923,0.15095]},
-    {"t":1.58904, "x":11.25015, "y":4.18899, "heading":-0.00017, "vx":0.38582, "vy":-0.00391, "omega":0.00007, "ax":-0.99955, "ay":0.01319, "alpha":0.00032, "fx":[-11.33336,-11.34041,-11.33251,-11.33252], "fy":[0.14932,0.14993,0.14993,0.14932]},
-    {"t":1.63199, "x":11.2658, "y":4.18883, "heading":-0.00017, "vx":0.34289, "vy":-0.00335, "omega":0.00009, "ax":-0.99957, "ay":0.01318, "alpha":0.00005, "fx":[-11.33451,-11.33451,-11.33542,-11.33542], "fy":[0.15006,0.14836,0.14954,0.15006]},
-    {"t":1.67494, "x":11.27961, "y":4.1887, "heading":-0.00016, "vx":0.29996, "vy":-0.00278, "omega":0.00009, "ax":-0.00239, "ay":0.21525, "alpha":-0.00036, "fx":[-0.02791,-0.02469,-0.02797,-0.02797], "fy":[2.44001,2.44177,2.44177,2.44001]},
-    {"t":1.72019, "x":11.29318, "y":4.1888, "heading":-0.00016, "vx":0.29985, "vy":0.00696, "omega":0.00007, "ax":-0.00021, "ay":0.00888, "alpha":0.00609, "fx":[-0.00136,-0.00135,-0.00345,-0.00345], "fy":[0.12714,0.02055,0.12775,0.12714]},
-    {"t":1.76544, "x":11.30675, "y":4.18912, "heading":-0.00016, "vx":0.29984, "vy":0.00736, "omega":0.00035, "ax":0.0, "ay":-0.0001, "alpha":-0.00062, "fx":[-0.00186,0.00613,-0.00208,-0.00208], "fy":[-0.00211,-0.00005,-0.00005,-0.00211]},
-    {"t":1.8107, "x":11.32032, "y":4.18945, "heading":-0.00014, "vx":0.29984, "vy":0.00736, "omega":0.00032, "ax":0.0, "ay":0.00006, "alpha":-0.00037, "fx":[0.00077,0.00077,-0.0008,-0.0008], "fy":[-0.00023,0.00324,0.00014,-0.00023]},
-    {"t":1.85595, "x":11.33389, "y":4.18979, "heading":-0.00013, "vx":0.29984, "vy":0.00736, "omega":0.0003, "ax":0.0, "ay":0.0, "alpha":-0.00031, "fx":[0.00009,0.00052,-0.0003,-0.0003], "fy":[-0.0011,0.00103,0.00103,-0.0011]},
-    {"t":1.9012, "x":11.34745, "y":4.19012, "heading":-0.00011, "vx":0.29984, "vy":0.00736, "omega":0.00029, "ax":0.0, "ay":0.0, "alpha":-0.0004, "fx":[0.0008,0.0008,-0.0008,-0.0008], "fy":[-0.0011,0.0026,-0.00051,-0.0011]},
-    {"t":1.94646, "x":11.36102, "y":4.19045, "heading":-0.0001, "vx":0.29984, "vy":0.00736, "omega":0.00027, "ax":0.0, "ay":0.0, "alpha":-0.00031, "fx":[0.00025,0.0003,-0.00027,-0.00027], "fy":[-0.0011,0.00109,0.00109,-0.00109]},
-    {"t":1.99171, "x":11.37459, "y":4.19079, "heading":-0.00009, "vx":0.29984, "vy":0.00736, "omega":0.00026, "ax":0.0, "ay":0.0, "alpha":-0.00031, "fx":[0.00082,0.00083,-0.00082,-0.00083], "fy":[-0.00069,0.00133,0.00007,-0.00069]},
-    {"t":2.03696, "x":11.38816, "y":4.19112, "heading":-0.00008, "vx":0.29984, "vy":0.00736, "omega":0.00024, "ax":0.0, "ay":0.0, "alpha":-0.00032, "fx":[0.00027,0.00042,-0.00035,-0.00035], "fy":[-0.00111,0.0011,0.0011,-0.00111]},
-    {"t":2.08221, "x":11.40173, "y":4.19145, "heading":-0.00006, "vx":0.29984, "vy":0.00736, "omega":0.00023, "ax":0.0, "ay":0.0, "alpha":-0.00028, "fx":[0.00083,0.00083,-0.00083,-0.00083], "fy":[-0.00056,0.0008,0.00032,-0.00056]},
-    {"t":2.12747, "x":11.4153, "y":4.19178, "heading":-0.00005, "vx":0.29984, "vy":0.00736, "omega":0.00022, "ax":0.0, "ay":0.0, "alpha":-0.00033, "fx":[0.0003,0.0005,-0.0004,-0.0004], "fy":[-0.00111,0.00111,0.00111,-0.00111]},
-    {"t":2.17272, "x":11.42887, "y":4.19212, "heading":-0.00004, "vx":0.29984, "vy":0.00736, "omega":0.0002, "ax":0.0, "ay":0.0, "alpha":-0.00028, "fx":[0.00084,0.00084,-0.00084,-0.00084], "fy":[-0.00056,0.00071,0.00042,-0.00056]},
-    {"t":2.21797, "x":11.44244, "y":4.19245, "heading":-0.00004, "vx":0.29984, "vy":0.00736, "omega":0.00019, "ax":0.0, "ay":0.0, "alpha":-0.00034, "fx":[0.00032,0.00056,-0.00044,-0.00044], "fy":[-0.00111,0.00111,0.00111,-0.00111]},
-    {"t":2.26323, "x":11.456, "y":4.19278, "heading":-0.00003, "vx":0.29984, "vy":0.00736, "omega":0.00017, "ax":0.0, "ay":0.0, "alpha":-0.00029, "fx":[0.00084,0.00084,-0.00084,-0.00084], "fy":[-0.0006,0.00075,0.00045,-0.0006]},
-    {"t":2.30848, "x":11.46957, "y":4.19312, "heading":-0.00002, "vx":0.29984, "vy":0.00736, "omega":0.00016, "ax":0.0, "ay":0.0, "alpha":-0.00035, "fx":[0.00034,0.00061,-0.00047,-0.00047], "fy":[-0.00112,0.00112,0.00112,-0.00112]},
-    {"t":2.35373, "x":11.48314, "y":4.19345, "heading":-0.00001, "vx":0.29984, "vy":0.00736, "omega":0.00014, "ax":0.0, "ay":0.0, "alpha":-0.00031, "fx":[0.00086,0.00086,-0.00086,-0.00086], "fy":[-0.00066,0.00084,0.00047,-0.00066]},
-    {"t":2.39898, "x":11.49671, "y":4.19378, "heading":-0.00001, "vx":0.29984, "vy":0.00736, "omega":0.00013, "ax":0.0, "ay":0.0, "alpha":-0.00037, "fx":[0.00036,0.00068,-0.00052,-0.00052], "fy":[-0.00116,0.00116,0.00116,-0.00116]},
-    {"t":2.44424, "x":11.51028, "y":4.19412, "heading":0.0, "vx":0.29984, "vy":0.00736, "omega":0.00011, "ax":0.0, "ay":0.0, "alpha":-0.00033, "fx":[0.00089,0.00089,-0.00089,-0.00089], "fy":[-0.00072,0.00094,0.0005,-0.00072]},
-    {"t":2.48949, "x":11.52385, "y":4.19445, "heading":0.00001, "vx":0.29984, "vy":0.00736, "omega":0.0001, "ax":0.0, "ay":0.0, "alpha":-0.00039, "fx":[0.00039,0.00076,-0.00057,-0.00057], "fy":[-0.00122,0.00122,0.00122,-0.00122]},
-    {"t":2.53474, "x":11.53742, "y":4.19478, "heading":0.00001, "vx":0.29984, "vy":0.00736, "omega":0.00008, "ax":0.0, "ay":0.0, "alpha":-0.00036, "fx":[0.00095,0.00095,-0.00095,-0.00095], "fy":[-0.00081,0.00107,0.00055,-0.00081]},
-    {"t":2.58, "x":11.55099, "y":4.19511, "heading":0.00001, "vx":0.29984, "vy":0.00736, "omega":0.00007, "ax":0.0, "ay":0.0, "alpha":-0.00043, "fx":[0.00043,0.00085,-0.00064,-0.00064], "fy":[-0.00132,0.00131,0.00131,-0.00132]},
-    {"t":2.62525, "x":11.56455, "y":4.19545, "heading":0.00002, "vx":0.29984, "vy":0.00736, "omega":0.00005, "ax":0.0, "ay":0.0, "alpha":-0.0004, "fx":[0.00104,0.00104,-0.00104,-0.00104], "fy":[-0.00092,0.00123,0.00061,-0.00092]},
-    {"t":2.6705, "x":11.57812, "y":4.19578, "heading":0.00002, "vx":0.29984, "vy":0.00736, "omega":0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00048, "fx":[0.0005,0.00096,-0.00073,-0.00073], "fy":[-0.00146,0.00146,0.00146,-0.00146]},
-    {"t":2.71575, "x":11.59169, "y":4.19611, "heading":0.00002, "vx":0.29984, "vy":0.00736, "omega":0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00046, "fx":[0.00117,0.00117,-0.00117,-0.00117], "fy":[-0.00106,0.00142,0.00071,-0.00106]},
-    {"t":2.76101, "x":11.60526, "y":4.19645, "heading":0.00002, "vx":0.29984, "vy":0.00736, "omega":-0.00001, "ax":0.0, "ay":0.0, "alpha":-0.00054, "fx":[0.0006,0.00106,-0.00083,-0.00083], "fy":[-0.00167,0.00167,0.00167,-0.00167]},
-    {"t":2.80626, "x":11.61883, "y":4.19678, "heading":0.00002, "vx":0.29984, "vy":0.00736, "omega":-0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00053, "fx":[0.00135,0.00135,-0.00135,-0.00135], "fy":[-0.00124,0.00165,0.00084,-0.00124]},
-    {"t":2.85151, "x":11.6324, "y":4.19711, "heading":0.00002, "vx":0.29984, "vy":0.00736, "omega":-0.00006, "ax":0.0, "ay":0.0, "alpha":-0.00064, "fx":[0.00068,0.00136,-0.00102,-0.00102], "fy":[-0.00194,0.00194,0.00194,-0.00194]},
-    {"t":2.89677, "x":11.64597, "y":4.19745, "heading":0.00002, "vx":0.29984, "vy":0.00736, "omega":-0.00009, "ax":0.0, "ay":0.0, "alpha":-0.00063, "fx":[0.00158,0.00158,-0.00158,-0.00158], "fy":[-0.00147,0.00194,0.001,-0.00147]},
-    {"t":2.94202, "x":11.65954, "y":4.19778, "heading":0.00001, "vx":0.29984, "vy":0.00736, "omega":-0.00012, "ax":0.0, "ay":0.0, "alpha":-0.00091, "fx":[-0.00005,0.00416,-0.00206,-0.00206], "fy":[-0.00229,0.00229,0.00229,-0.00229]},
-    {"t":2.98727, "x":11.6731, "y":4.19811, "heading":0.00001, "vx":0.29984, "vy":0.00736, "omega":-0.00016, "ax":0.0, "ay":0.0, "alpha":-0.00078, "fx":[0.00189,0.00189,-0.00189,-0.00189], "fy":[-0.00187,0.00262,0.0011,-0.00187]},
-    {"t":3.03252, "x":11.68667, "y":4.19845, "heading":0.0, "vx":0.29984, "vy":0.00736, "omega":-0.0002, "ax":0.0, "ay":0.0, "alpha":0.00577, "fx":[0.03932,-0.11234,0.03641,0.03641], "fy":[-0.003,0.00301,0.00301,-0.003]},
-    {"t":3.07778, "x":11.70024, "y":4.19878, "heading":-0.00001, "vx":0.29984, "vy":0.00736, "omega":0.00006, "ax":-0.63133, "ay":-0.0155, "alpha":-0.00094, "fx":[-7.15741,-7.15741,-7.16098,-7.16098], "fy":[-0.17836,-0.17042,-0.17581,-0.17836]},
-    {"t":3.12303, "x":11.71316, "y":4.1991, "heading":-0.00001, "vx":0.27127, "vy":0.00666, "omega":0.00002, "ax":-0.99824, "ay":-0.0245, "alpha":0.00085, "fx":[-11.31075,-11.34443,-11.31219,-11.31219], "fy":[-0.28004,-0.27569,-0.27569,-0.28004]},
-    {"t":3.16828, "x":11.72442, "y":4.19937, "heading":-0.00001, "vx":0.2261, "vy":0.00555, "omega":0.00006, "ax":-0.99897, "ay":-0.02452, "alpha":-0.00051, "fx":[-11.32673,-11.32673,-11.32954,-11.32954], "fy":[-0.27918,-0.27625,-0.27765,-0.27918]},
-    {"t":3.21354, "x":11.73363, "y":4.1996, "heading":0.0, "vx":0.18089, "vy":0.00444, "omega":0.00004, "ax":-0.99921, "ay":-0.02453, "alpha":-0.00008, "fx":[-11.32813,-11.33737,-11.32902,-11.32902], "fy":[-0.27987,-0.2764,-0.27639,-0.27987]},
-    {"t":3.25879, "x":11.74079, "y":4.19977, "heading":0.0, "vx":0.13568, "vy":0.00333, "omega":0.00003, "ax":-0.99933, "ay":-0.02453, "alpha":-0.00042, "fx":[-11.33107,-11.33107,-11.33345,-11.33345], "fy":[-0.27903,-0.2765,-0.27812,-0.27903]},
-    {"t":3.30404, "x":11.74591, "y":4.1999, "heading":0.0, "vx":0.09045, "vy":0.00222, "omega":0.00001, "ax":-0.99941, "ay":-0.02453, "alpha":-0.00015, "fx":[-11.33149,-11.33692,-11.33197,-11.33197], "fy":[-0.27967,-0.2767,-0.2767,-0.27967]},
-    {"t":3.34929, "x":11.74898, "y":4.19997, "heading":0.0, "vx":0.04523, "vy":0.00111, "omega":0.00001, "ax":-0.99946, "ay":-0.02453, "alpha":-0.00016, "fx":[-11.33241,-11.33674,-11.3327,-11.3327], "fy":[-0.27959,-0.27682,-0.27682,-0.27959]},
-    {"t":3.39455, "x":11.75, "y":4.2, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+    {"t":0.0, "x":10.35, "y":4.22, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.79961, "ay":0.00581, "alpha":0.00059, "fx":[9.09007,9.01384,9.08287,9.08287], "fy":[0.05675,0.07495,0.07495,0.05675]},
+    {"t":0.0442, "x":10.35078, "y":4.22001, "heading":0.0, "vx":0.03535, "vy":0.00026, "omega":0.00003, "ax":0.79978, "ay":0.00581, "alpha":-0.00085, "fx":[9.08352,9.041,9.07644,9.07644], "fy":[0.05696,0.07477,0.07477,0.05696]},
+    {"t":0.08841, "x":10.35313, "y":4.22002, "heading":0.0, "vx":0.0707, "vy":0.00051, "omega":-0.00001, "ax":0.79976, "ay":0.00581, "alpha":-0.00232, "fx":[9.0757,9.0757,9.06262,9.06262], "fy":[0.061,0.07115,0.0703,0.061]},
+    {"t":0.13261, "x":10.35703, "y":4.22005, "heading":0.0, "vx":0.10605, "vy":0.00077, "omega":-0.00011, "ax":0.79974, "ay":0.00581, "alpha":-0.00045, "fx":[9.08512,9.03444,9.07809,9.07809], "fy":[0.05708,0.07463,0.07463,0.05708]},
+    {"t":0.17682, "x":10.3625, "y":4.22009, "heading":0.0, "vx":0.1414, "vy":0.00103, "omega":-0.00013, "ax":0.79972, "ay":0.00581, "alpha":-0.0023, "fx":[9.07514,9.07514,9.06219,9.06219], "fy":[0.06103,0.07107,0.0703,0.06103]},
+    {"t":0.22102, "x":10.36953, "y":4.22014, "heading":-0.00001, "vx":0.17675, "vy":0.00128, "omega":-0.00024, "ax":0.79969, "ay":0.00581, "alpha":0.00007, "fx":[9.08753,9.02492,9.08045,9.08045], "fy":[0.05708,0.07463,0.07463,0.05708]},
+    {"t":0.26522, "x":10.37813, "y":4.2202, "heading":-0.00002, "vx":0.2121, "vy":0.00154, "omega":-0.00023, "ax":0.79965, "ay":0.00581, "alpha":-0.0023, "fx":[9.07441,9.07441,9.06145,9.06145], "fy":[0.061,0.07103,0.07037,0.061]},
+    {"t":0.30943, "x":10.38828, "y":4.22028, "heading":-0.00003, "vx":0.24745, "vy":0.0018, "omega":-0.00033, "ax":0.79961, "ay":0.00581, "alpha":0.00078, "fx":[9.09082,9.01159,9.0836,9.0836], "fy":[0.05698,0.07472,0.07472,0.05698]},
+    {"t":0.35363, "x":10.4, "y":4.22036, "heading":-0.00005, "vx":0.2828, "vy":0.00205, "omega":-0.0003, "ax":0.79955, "ay":0.00581, "alpha":-0.00233, "fx":[9.07325,9.07325,9.06014,9.06014], "fy":[0.06094,0.07097,0.07052,0.06094]},
+    {"t":0.39784, "x":10.41329, "y":4.22046, "heading":-0.00006, "vx":0.31814, "vy":0.00231, "omega":-0.0004, "ax":0.79946, "ay":0.00581, "alpha":0.00216, "fx":[9.0974,8.98558,9.08994,9.08994], "fy":[0.05676,0.07492,0.07492,0.05676]},
+    {"t":0.44204, "x":10.42813, "y":4.22057, "heading":-0.00008, "vx":0.35348, "vy":0.00257, "omega":-0.00031, "ax":0.79933, "ay":0.0058, "alpha":-0.00236, "fx":[9.07089,9.07089,9.05757,9.05757], "fy":[0.06087,0.07081,0.07075,0.06087]},
+    {"t":0.48624, "x":10.44454, "y":4.22069, "heading":-0.00009, "vx":0.38881, "vy":0.00282, "omega":-0.00041, "ax":0.79911, "ay":0.0058, "alpha":0.00517, "fx":[9.11114,8.92919,9.10334,9.10334], "fy":[0.05642,0.0752,0.0752,0.05642]},
+    {"t":0.53045, "x":10.4625, "y":4.22082, "heading":-0.00011, "vx":0.42414, "vy":0.00308, "omega":-0.00018, "ax":0.79867, "ay":0.0058, "alpha":-0.00234, "fx":[9.06351,9.06351,9.05006,9.05006], "fy":[0.06096,0.07005,0.07115,0.06096]},
+    {"t":0.57465, "x":10.48203, "y":4.22096, "heading":-0.00012, "vx":0.45944, "vy":0.00334, "omega":-0.00029, "ax":0.79734, "ay":0.00579, "alpha":0.02742, "fx":[9.21904,8.52766,9.21007,9.21007], "fy":[0.05507,0.0763,0.07629,0.05507]},
+    {"t":0.61886, "x":10.50312, "y":4.22111, "heading":-0.00013, "vx":0.49469, "vy":0.00359, "omega":0.00093, "ax":0.11729, "ay":0.00081, "alpha":-0.00312, "fx":[1.33635,1.33635,1.32378,1.32378], "fy":[0.00069,0.02457,0.01056,0.00069]},
+    {"t":0.66306, "x":10.5251, "y":4.22127, "heading":-0.00009, "vx":0.49987, "vy":0.00363, "omega":0.00079, "ax":0.0, "ay":0.00005, "alpha":0.01007, "fx":[0.07628,-0.21229,0.06805,0.06805], "fy":[-0.00805,0.00907,0.00907,-0.00805]},
+    {"t":0.70726, "x":10.5472, "y":4.22143, "heading":-0.00005, "vx":0.49987, "vy":0.00363, "omega":0.00123, "ax":0.0, "ay":0.0, "alpha":-0.00191, "fx":[0.00535,0.00535,-0.00535,-0.00535], "fy":[-0.004,0.0035,0.00462,-0.004]},
+    {"t":0.75147, "x":10.5693, "y":4.22159, "heading":0.0, "vx":0.49987, "vy":0.00363, "omega":0.00115, "ax":0.0, "ay":0.0, "alpha":-0.00176, "fx":[0.00445,-0.00177,-0.00134,-0.00134], "fy":[-0.00645,0.00635,0.00636,-0.00644]},
+    {"t":0.79567, "x":10.59139, "y":4.22175, "heading":0.00005, "vx":0.49987, "vy":0.00363, "omega":0.00107, "ax":0.0, "ay":-0.00001, "alpha":-0.00158, "fx":[0.00427,0.00427,-0.00427,-0.00427], "fy":[-0.00352,0.00349,0.00332,-0.00352]},
+    {"t":0.83988, "x":10.61349, "y":4.22191, "heading":0.0001, "vx":0.49987, "vy":0.00363, "omega":0.001, "ax":0.0, "ay":0.0, "alpha":-0.00187, "fx":[0.00063,0.00705,-0.00384,-0.00384], "fy":[-0.00496,0.00505,0.00505,-0.00496]},
+    {"t":0.88408, "x":10.63558, "y":4.22207, "heading":0.00014, "vx":0.49987, "vy":0.00363, "omega":0.00092, "ax":0.0, "ay":0.0, "alpha":-0.00122, "fx":[0.0033,0.0033,-0.0033,-0.0033], "fy":[-0.00273,0.00278,0.0025,-0.00273]},
+    {"t":0.92828, "x":10.65768, "y":4.22223, "heading":0.00018, "vx":0.49987, "vy":0.00363, "omega":0.00086, "ax":0.0, "ay":0.0, "alpha":-0.00137, "fx":[0.00065,0.00471,-0.00268,-0.00268], "fy":[-0.00374,0.00383,0.00383,-0.00374]},
+    {"t":0.97249, "x":10.67978, "y":4.22239, "heading":0.00022, "vx":0.49987, "vy":0.00363, "omega":0.0008, "ax":0.0, "ay":0.0, "alpha":-0.00089, "fx":[0.00243,0.00243,-0.00242,-0.00243], "fy":[-0.00199,0.00205,0.00178,-0.00199]},
+    {"t":1.01669, "x":10.70187, "y":4.22256, "heading":0.00026, "vx":0.49987, "vy":0.00363, "omega":0.00076, "ax":0.0, "ay":0.0, "alpha":-0.00092, "fx":[0.00072,0.00244,-0.00158,-0.00158], "fy":[-0.00264,0.00271,0.00271,-0.00264]},
+    {"t":1.0609, "x":10.72397, "y":4.22272, "heading":0.00029, "vx":0.49987, "vy":0.00363, "omega":0.00072, "ax":0.0, "ay":0.0, "alpha":-0.00059, "fx":[0.00162,0.00162,-0.00162,-0.00162], "fy":[-0.00131,0.00139,0.00112,-0.00131]},
+    {"t":1.1051, "x":10.74607, "y":4.22288, "heading":0.00032, "vx":0.49987, "vy":0.00363, "omega":0.0007, "ax":0.0, "ay":0.0, "alpha":-0.00053, "fx":[0.00054,0.00104,-0.00079,-0.00079], "fy":[-0.00162,0.00166,0.00166,-0.00162]},
+    {"t":1.1493, "x":10.76816, "y":4.22304, "heading":0.00035, "vx":0.49987, "vy":0.00363, "omega":0.00067, "ax":0.0, "ay":0.0, "alpha":-0.00031, "fx":[0.00086,0.00086,-0.00086,-0.00086], "fy":[-0.00066,0.00078,0.00049,-0.00066]},
+    {"t":1.19351, "x":10.79026, "y":4.2232, "heading":0.00038, "vx":0.49987, "vy":0.00363, "omega":0.00066, "ax":0.0, "ay":0.0, "alpha":-0.00017, "fx":[0.0003,-0.00011,-0.0001,-0.0001], "fy":[-0.00064,0.00066,0.00066,-0.00064]},
+    {"t":1.23771, "x":10.81236, "y":4.22336, "heading":0.00041, "vx":0.49987, "vy":0.00363, "omega":0.00065, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[0.00012,0.00012,-0.00012,-0.00012], "fy":[0.00001,0.00011,-0.00007,0.00001]},
+    {"t":1.28191, "x":10.83445, "y":4.22352, "heading":0.00044, "vx":0.49987, "vy":0.00363, "omega":0.00065, "ax":0.0, "ay":0.0, "alpha":0.0002, "fx":[0.00016,-0.00149,0.00067,0.00067], "fy":[0.00031,-0.00034,-0.00034,0.00031]},
+    {"t":1.32612, "x":10.85655, "y":4.22368, "heading":0.00047, "vx":0.49987, "vy":0.00363, "omega":0.00066, "ax":0.0, "ay":0.00001, "alpha":0.00029, "fx":[-0.00061,-0.00061,0.00061,0.00061], "fy":[0.00086,-0.00096,-0.00044,0.00086]},
+    {"t":1.37032, "x":10.87864, "y":4.22384, "heading":0.0005, "vx":0.49987, "vy":0.00363, "omega":0.00067, "ax":0.0, "ay":-0.00001, "alpha":0.00061, "fx":[0.00029,-0.00374,0.00173,0.00173], "fy":[0.00113,-0.00146,-0.00146,0.00113]},
+    {"t":1.41453, "x":10.90074, "y":4.224, "heading":0.00053, "vx":0.49987, "vy":0.00363, "omega":0.0007, "ax":0.0, "ay":0.00002, "alpha":0.00075, "fx":[-0.00135,-0.00135,0.00135,0.00135], "fy":[0.00239,-0.00365,-0.00017,0.00239]},
+    {"t":1.45873, "x":10.92284, "y":4.22416, "heading":0.00056, "vx":0.49987, "vy":0.00363, "omega":0.00073, "ax":0.0, "ay":-0.00006, "alpha":0.00106, "fx":[0.00046,-0.00617,0.00287,0.00287], "fy":[0.00158,-0.00305,-0.00305,0.00158]},
+    {"t":1.50293, "x":10.94493, "y":4.22432, "heading":0.00059, "vx":0.49987, "vy":0.00363, "omega":0.00078, "ax":0.0, "ay":0.00008, "alpha":0.00163, "fx":[-0.00213,-0.00213,0.00212,0.00211], "fy":[0.00625,-0.01101,0.00237,0.00625]},
+    {"t":1.54714, "x":10.96703, "y":4.22448, "heading":0.00063, "vx":0.49987, "vy":0.00363, "omega":0.00085, "ax":0.0, "ay":-0.00005, "alpha":0.00091, "fx":[-0.00299,0.00196,0.00052,0.00052], "fy":[0.00291,-0.00398,-0.00398,0.00291]},
+    {"t":1.59134, "x":10.98913, "y":4.22464, "heading":0.00067, "vx":0.49987, "vy":0.00363, "omega":0.00089, "ax":0.0, "ay":0.00005, "alpha":0.00303, "fx":[-0.00295,-0.00295,0.00294,0.00294], "fy":[0.01112,-0.02585,0.00582,0.01112]},
+    {"t":1.63555, "x":11.01122, "y":4.2248, "heading":0.00071, "vx":0.49987, "vy":0.00363, "omega":0.00103, "ax":0.00001, "ay":-0.00141, "alpha":-0.01024, "fx":[-0.07029,0.19995,-0.06463,-0.06463], "fy":[-0.01067,-0.02126,-0.02126,-0.01067]},
+    {"t":1.67975, "x":11.03332, "y":4.22496, "heading":0.00075, "vx":0.49987, "vy":0.00357, "omega":0.00057, "ax":-0.06041, "ay":-0.01326, "alpha":-0.00078, "fx":[-0.68898,-0.68898,-0.6812,-0.6812], "fy":[-0.15655,-0.12504,-0.16318,-0.15655]},
+    {"t":1.72395, "x":11.05536, "y":4.22511, "heading":0.00078, "vx":0.4972, "vy":0.00298, "omega":0.00054, "ax":-0.79729, "ay":-0.00593, "alpha":-0.03145, "fx":[-9.23415,-8.47463,-9.22777,-9.22777], "fy":[-0.06007,-0.07448,-0.07447,-0.06007]},
+    {"t":1.76816, "x":11.07656, "y":4.22523, "heading":0.0008, "vx":0.46196, "vy":0.00272, "omega":-0.00085, "ax":-0.79866, "ay":-0.00587, "alpha":0.00199, "fx":[-9.06041,-9.0604,-9.05284,-9.05284], "fy":[-0.06103,-0.07705,-0.06719,-0.06103]},
+    {"t":1.81236, "x":11.0962, "y":4.22535, "heading":0.00076, "vx":0.42666, "vy":0.00246, "omega":-0.00076, "ax":-0.7991, "ay":-0.00585, "alpha":-0.0065, "fx":[-9.10974,-8.92663,-9.10518,-9.10518], "fy":[-0.06116,-0.07153,-0.07153,-0.06116]},
+    {"t":1.85657, "x":11.11427, "y":4.22545, "heading":0.00073, "vx":0.39133, "vy":0.0022, "omega":-0.00105, "ax":-0.79932, "ay":-0.00584, "alpha":0.00148, "fx":[-9.06741,-9.06741,-9.06097,-9.06097], "fy":[-0.0624,-0.07254,-0.06757,-0.0624]},
+    {"t":1.90077, "x":11.13079, "y":4.22554, "heading":0.00068, "vx":0.356, "vy":0.00194, "omega":-0.00098, "ax":-0.79946, "ay":-0.00583, "alpha":-0.00368, "fx":[-9.09564,-8.98318,-9.09196,-9.09196], "fy":[-0.06201,-0.07031,-0.07031,-0.06201]},
+    {"t":1.94497, "x":11.14575, "y":4.22562, "heading":0.00064, "vx":0.32066, "vy":0.00169, "omega":-0.00115, "ax":-0.79954, "ay":-0.00583, "alpha":0.00116, "fx":[-9.06922,-9.06922,-9.06414,-9.06414], "fy":[-0.06315,-0.07093,-0.06723,-0.06315]},
+    {"t":1.98918, "x":11.15914, "y":4.22569, "heading":0.00059, "vx":0.28532, "vy":0.00143, "omega":-0.0011, "ax":-0.79961, "ay":-0.00583, "alpha":-0.00256, "fx":[-9.08874,-9.0089,-9.08596,-9.08596], "fy":[-0.06297,-0.0692,-0.06919,-0.06297]},
+    {"t":2.03338, "x":11.17097, "y":4.22575, "heading":0.00054, "vx":0.24997, "vy":0.00117, "omega":-0.00121, "ax":-0.00089, "ay":-0.08794, "alpha":0.00533, "fx":[0.01474,-0.08878,0.01691,0.01691], "fy":[-0.99496,-0.99949,-0.99949,-0.99496]},
+    {"t":2.07946, "x":11.18249, "y":4.22571, "heading":0.00048, "vx":0.24993, "vy":-0.00288, "omega":-0.00096, "ax":-0.00005, "ay":-0.00393, "alpha":0.00783, "fx":[-0.00182,-0.00182,0.00076,0.00076], "fy":[-0.01254,-0.13857,-0.01476,-0.01254]},
+    {"t":2.12554, "x":11.19401, "y":4.22557, "heading":0.00044, "vx":0.24993, "vy":-0.00306, "omega":-0.0006, "ax":0.0, "ay":0.00025, "alpha":0.00089, "fx":[0.0008,-0.00573,0.00254,0.00254], "fy":[0.00475,0.00097,0.00097,0.00475]},
+    {"t":2.17162, "x":11.20552, "y":4.22543, "heading":0.00041, "vx":0.24993, "vy":-0.00305, "omega":-0.00056, "ax":0.0, "ay":-0.00005, "alpha":0.00228, "fx":[-0.00124,-0.00124,0.00123,0.00123], "fy":[0.00814,-0.02461,0.0061,0.00814]},
+    {"t":2.2177, "x":11.21704, "y":4.22529, "heading":0.00039, "vx":0.24993, "vy":-0.00305, "omega":-0.00046, "ax":0.0, "ay":-0.00002, "alpha":0.00057, "fx":[-0.00045,-0.00166,0.00105,0.00105], "fy":[0.00134,-0.00189,-0.00189,0.00134]},
+    {"t":2.26377, "x":11.22855, "y":4.22515, "heading":0.00036, "vx":0.24993, "vy":-0.00305, "omega":-0.00043, "ax":0.0, "ay":0.0, "alpha":0.00043, "fx":[-0.00114,-0.00114,0.00114,0.00114], "fy":[0.00099,-0.00106,-0.00088,0.00099]},
+    {"t":2.30985, "x":11.24007, "y":4.22501, "heading":0.00035, "vx":0.24993, "vy":-0.00305, "omega":-0.00041, "ax":0.0, "ay":0.0, "alpha":0.00046, "fx":[-0.0006,-0.00081,0.0007,0.0007], "fy":[0.00142,-0.00141,-0.00141,0.00142]},
+    {"t":2.35593, "x":11.25159, "y":4.22487, "heading":0.00033, "vx":0.24993, "vy":-0.00305, "omega":-0.00039, "ax":0.0, "ay":0.0, "alpha":0.00033, "fx":[-0.001,-0.001,0.001,0.001], "fy":[0.00063,-0.00025,-0.001,0.00063]},
+    {"t":2.40201, "x":11.2631, "y":4.22473, "heading":0.00031, "vx":0.24993, "vy":-0.00305, "omega":-0.00037, "ax":0.0, "ay":0.0, "alpha":0.0004, "fx":[-0.00056,-0.00058,0.00057,0.00057], "fy":[0.00124,-0.00125,-0.00125,0.00124]},
+    {"t":2.44809, "x":11.27462, "y":4.22459, "heading":0.00029, "vx":0.24993, "vy":-0.00305, "omega":-0.00036, "ax":0.0, "ay":0.0, "alpha":0.00031, "fx":[-0.00087,-0.00087,0.00087,0.00087], "fy":[0.00064,-0.0005,-0.00077,0.00064]},
+    {"t":2.49417, "x":11.28614, "y":4.22445, "heading":0.00027, "vx":0.24993, "vy":-0.00305, "omega":-0.00034, "ax":0.0, "ay":0.0, "alpha":0.00035, "fx":[-0.00049,-0.00048,0.00049,0.00049], "fy":[0.00109,-0.00109,-0.00109,0.00109]},
+    {"t":2.54024, "x":11.29765, "y":4.2243, "heading":0.00026, "vx":0.24993, "vy":-0.00305, "omega":-0.00033, "ax":0.0, "ay":0.0, "alpha":0.00028, "fx":[-0.00077,-0.00077,0.00077,0.00077], "fy":[0.00059,-0.00057,-0.00061,0.00059]},
+    {"t":2.58632, "x":11.30917, "y":4.22416, "heading":0.00024, "vx":0.24993, "vy":-0.00305, "omega":-0.00031, "ax":0.0, "ay":0.0, "alpha":0.0003, "fx":[-0.00042,-0.00042,0.00042,0.00042], "fy":[0.00096,-0.00096,-0.00096,0.00096]},
+    {"t":2.6324, "x":11.32068, "y":4.22402, "heading":0.00023, "vx":0.24993, "vy":-0.00305, "omega":-0.0003, "ax":0.0, "ay":0.0, "alpha":0.00025, "fx":[-0.00068,-0.00068,0.00068,0.00068], "fy":[0.00052,-0.00054,-0.00051,0.00052]},
+    {"t":2.67848, "x":11.3322, "y":4.22388, "heading":0.00022, "vx":0.24993, "vy":-0.00305, "omega":-0.00029, "ax":0.0, "ay":0.0, "alpha":0.00027, "fx":[-0.00036,-0.00036,0.00036,0.00036], "fy":[0.00086,-0.00086,-0.00086,0.00086]},
+    {"t":2.72456, "x":11.34372, "y":4.22374, "heading":0.0002, "vx":0.24993, "vy":-0.00305, "omega":-0.00028, "ax":0.0, "ay":0.0, "alpha":0.00021, "fx":[-0.00061,-0.00061,0.00061,0.00061], "fy":[0.00045,-0.00047,-0.00043,0.00045]},
+    {"t":2.77063, "x":11.35523, "y":4.2236, "heading":0.00019, "vx":0.24993, "vy":-0.00305, "omega":-0.00027, "ax":0.0, "ay":0.0, "alpha":0.00024, "fx":[-0.0003,-0.0003,0.0003,0.0003], "fy":[0.00077,-0.00077,-0.00077,0.00077]},
+    {"t":2.81671, "x":11.36675, "y":4.22346, "heading":0.00018, "vx":0.24993, "vy":-0.00305, "omega":-0.00025, "ax":0.0, "ay":0.0, "alpha":0.00019, "fx":[-0.00055,-0.00055,0.00055,0.00055], "fy":[0.00037,-0.00039,-0.00035,0.00037]},
+    {"t":2.86279, "x":11.37827, "y":4.22332, "heading":0.00017, "vx":0.24993, "vy":-0.00305, "omega":-0.00025, "ax":0.0, "ay":0.0, "alpha":0.00021, "fx":[-0.00024,-0.00025,0.00024,0.00024], "fy":[0.00069,-0.00069,-0.00069,0.00069]},
+    {"t":2.90887, "x":11.38978, "y":4.22318, "heading":0.00015, "vx":0.24993, "vy":-0.00305, "omega":-0.00024, "ax":0.0, "ay":0.0, "alpha":0.00016, "fx":[-0.0005,-0.0005,0.0005,0.0005], "fy":[0.0003,-0.00032,-0.00028,0.0003]},
+    {"t":2.95495, "x":11.4013, "y":4.22304, "heading":0.00014, "vx":0.24993, "vy":-0.00305, "omega":-0.00023, "ax":0.0, "ay":0.0, "alpha":0.00018, "fx":[-0.00019,-0.0002,0.00019,0.00019], "fy":[0.00063,-0.00063,-0.00063,0.00063]},
+    {"t":3.00103, "x":11.41281, "y":4.2229, "heading":0.00013, "vx":0.24993, "vy":-0.00305, "omega":-0.00022, "ax":0.0, "ay":0.0, "alpha":0.00014, "fx":[-0.00045,-0.00045,0.00045,0.00045], "fy":[0.00023,-0.00025,-0.00021,0.00023]},
+    {"t":3.0471, "x":11.42433, "y":4.22276, "heading":0.00012, "vx":0.24993, "vy":-0.00305, "omega":-0.00021, "ax":0.0, "ay":0.0, "alpha":0.00016, "fx":[-0.00014,-0.00015,0.00014,0.00014], "fy":[0.00058,-0.00058,-0.00058,0.00058]},
+    {"t":3.09318, "x":11.43585, "y":4.22262, "heading":0.00011, "vx":0.24993, "vy":-0.00305, "omega":-0.00021, "ax":0.0, "ay":0.0, "alpha":0.00011, "fx":[-0.00042,-0.00042,0.00042,0.00042], "fy":[0.00016,-0.00018,-0.00014,0.00016]},
+    {"t":3.13926, "x":11.44736, "y":4.22248, "heading":0.0001, "vx":0.24993, "vy":-0.00305, "omega":-0.0002, "ax":0.0, "ay":0.0, "alpha":0.00014, "fx":[-0.00008,-0.0001,0.00009,0.00009], "fy":[0.00054,-0.00054,-0.00054,0.00054]},
+    {"t":3.18534, "x":11.45888, "y":4.22234, "heading":0.00009, "vx":0.24993, "vy":-0.00305, "omega":-0.0002, "ax":0.0, "ay":0.0, "alpha":0.00009, "fx":[-0.00039,-0.00039,0.00039,0.00039], "fy":[0.00009,-0.0001,-0.00007,0.00009]},
+    {"t":3.23142, "x":11.47039, "y":4.22219, "heading":0.00009, "vx":0.24993, "vy":-0.00305, "omega":-0.00019, "ax":0.0, "ay":0.0, "alpha":0.00012, "fx":[-0.00002,-0.00004,0.00003,0.00003], "fy":[0.0005,-0.0005,-0.0005,0.0005]},
+    {"t":3.2775, "x":11.48191, "y":4.22205, "heading":0.00008, "vx":0.24993, "vy":-0.00305, "omega":-0.00019, "ax":0.0, "ay":0.0, "alpha":0.00007, "fx":[-0.00037,-0.00037,0.00037,0.00037], "fy":[0.00001,-0.00003,0.00001,0.00001]},
+    {"t":3.32357, "x":11.49343, "y":4.22191, "heading":0.00007, "vx":0.24993, "vy":-0.00305, "omega":-0.00018, "ax":0.0, "ay":0.0, "alpha":0.00011, "fx":[0.00004,0.0,-0.00002,-0.00002], "fy":[0.00047,-0.00047,-0.00047,0.00047]},
+    {"t":3.36965, "x":11.50494, "y":4.22177, "heading":0.00006, "vx":0.24993, "vy":-0.00305, "omega":-0.00018, "ax":0.0, "ay":0.0, "alpha":0.00004, "fx":[-0.00035,-0.00035,0.00035,0.00035], "fy":[-0.00007,0.00006,0.0001,-0.00007]},
+    {"t":3.41573, "x":11.51646, "y":4.22163, "heading":0.00005, "vx":0.24993, "vy":-0.00305, "omega":-0.00018, "ax":0.0, "ay":0.0, "alpha":0.0001, "fx":[0.00015,-0.00004,-0.00005,-0.00005], "fy":[0.00044,-0.00045,-0.00045,0.00044]},
+    {"t":3.46181, "x":11.52798, "y":4.22149, "heading":0.00004, "vx":0.24993, "vy":-0.00305, "omega":-0.00017, "ax":0.0, "ay":0.0, "alpha":0.00002, "fx":[-0.00033,-0.00033,0.00033,0.00033], "fy":[-0.00017,0.00015,0.00019,-0.00017]},
+    {"t":3.50789, "x":11.53949, "y":4.22135, "heading":0.00004, "vx":0.24993, "vy":-0.00305, "omega":-0.00017, "ax":0.0, "ay":0.0, "alpha":0.00011, "fx":[0.00039,-0.00046,0.00003,0.00003], "fy":[0.00042,-0.00043,-0.00043,0.00042]},
+    {"t":3.55397, "x":11.55101, "y":4.22121, "heading":0.00003, "vx":0.24993, "vy":-0.00305, "omega":-0.00016, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00032,-0.00032,0.00032,0.00032], "fy":[-0.00028,0.00026,0.00031,-0.00028]},
+    {"t":3.60004, "x":11.56252, "y":4.22107, "heading":0.00002, "vx":0.24993, "vy":-0.00305, "omega":-0.00017, "ax":0.0, "ay":0.0, "alpha":0.00021, "fx":[0.0012,-0.00251,0.00065,0.00065], "fy":[0.0004,-0.0004,-0.0004,0.0004]},
+    {"t":3.64612, "x":11.57404, "y":4.22093, "heading":0.00001, "vx":0.24993, "vy":-0.00305, "omega":-0.00016, "ax":0.0, "ay":0.0, "alpha":-0.00004, "fx":[-0.00031,-0.00031,0.00031,0.00031], "fy":[-0.00039,0.00034,0.00044,-0.00039]},
+    {"t":3.6922, "x":11.58556, "y":4.22079, "heading":0.00001, "vx":0.24993, "vy":-0.00305, "omega":-0.00016, "ax":0.0, "ay":0.0, "alpha":-0.00014, "fx":[-0.00063,0.00332,-0.00135,-0.00135], "fy":[0.00041,-0.00041,-0.00041,0.00041]},
+    {"t":3.73828, "x":11.59707, "y":4.22065, "heading":0.0, "vx":0.24993, "vy":-0.00305, "omega":-0.00016, "ax":-0.00001, "ay":0.0, "alpha":0.0, "fx":[-0.00037,-0.00037,0.00021,0.00021], "fy":[-0.00022,-0.00046,0.00091,-0.00022]},
+    {"t":3.78436, "x":11.60859, "y":4.22051, "heading":-0.00001, "vx":0.24993, "vy":-0.00305, "omega":-0.00016, "ax":-0.62718, "ay":0.00766, "alpha":0.00887, "fx":[-7.06051,-7.26376,-7.06217,-7.06217], "fy":[0.08681,0.08699,0.08699,0.08681]},
+    {"t":3.83043, "x":11.61944, "y":4.22037, "heading":-0.00002, "vx":0.22103, "vy":-0.0027, "omega":0.00024, "ax":-0.79878, "ay":0.00976, "alpha":0.00006, "fx":[-9.05844,-9.05844,-9.05752,-9.05752], "fy":[0.11057,0.10996,0.11158,0.11057]},
+    {"t":3.87651, "x":11.62878, "y":4.22026, "heading":-0.00001, "vx":0.18422, "vy":-0.00225, "omega":0.00025, "ax":-0.79936, "ay":0.00977, "alpha":-0.00487, "fx":[-9.09306,-8.97831,-9.09342,-9.09342], "fy":[0.11187,0.10964,0.10963,0.11187]},
+    {"t":3.92259, "x":11.63642, "y":4.22017, "heading":0.00001, "vx":0.14739, "vy":-0.0018, "omega":0.00002, "ax":-0.79955, "ay":0.00977, "alpha":0.00006, "fx":[-9.06749,-9.06749,-9.06602,-9.06602], "fy":[0.11048,0.11111,0.11104,0.11048]},
+    {"t":3.96867, "x":11.64236, "y":4.22009, "heading":0.00001, "vx":0.11055, "vy":-0.00135, "omega":0.00003, "ax":-0.79965, "ay":0.00977, "alpha":-0.00244, "fx":[-9.08302,-9.02197,-9.08322,-9.08322], "fy":[0.11203,0.10955,0.10955,0.11203]},
+    {"t":4.01475, "x":11.6466, "y":4.22004, "heading":0.00001, "vx":0.0737, "vy":-0.0009, "omega":-0.00009, "ax":-0.79971, "ay":0.00977, "alpha":0.00009, "fx":[-9.0694,-9.0694,-9.06764,-9.06764], "fy":[0.11053,0.1113,0.11084,0.11053]},
+    {"t":4.06083, "x":11.64915, "y":4.22001, "heading":0.0, "vx":0.03685, "vy":-0.00045, "omega":-0.00008, "ax":-0.79975, "ay":0.00977, "alpha":0.00179, "fx":[-9.07904,-9.07904,-9.07928,-9.03848], "fy":[0.11108,0.11082,0.11082,0.1105]},
+    {"t":4.1069, "x":11.65, "y":4.22, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
   "splits":[0]
  },
  "events":[]
diff --git a/src/main/java/frc/game/Reef.java b/src/main/java/frc/game/Reef.java
index caf253d1..b28cd275 100644
--- a/src/main/java/frc/game/Reef.java
+++ b/src/main/java/frc/game/Reef.java
@@ -18,8 +18,7 @@
  * to a pose.
  */
 public enum Reef {
-  // TODO remove reef ceter offset
-  Blue(new Pose2d(Inches.of(176.75), Inches.of(158.5 + 4.5), Rotation2d.kZero)),
+  Blue(new Pose2d(Inches.of(176.75), Inches.of(158.5), Rotation2d.kZero)),
   Red(new Pose2d(Inches.of(514.125), Inches.of(158.5), Rotation2d.kPi));
 
   /** The radius of the reef hexagon */
@@ -75,10 +74,10 @@ public int getSector(Pose2d atPose) {
     var delta = centerPose.getTranslation().minus(atPose.getTranslation());
     int vectorAngle = (int) delta.getAngle().getDegrees();
     int rayAngle = (vectorAngle + 30 + 360 + (int) centerPose.getRotation().getDegrees()) % 360;
-    SmartDashboard.putNumber("Sector delta X", delta.getX());
-    SmartDashboard.putNumber("Sector delta Y", delta.getY());
-    SmartDashboard.putNumber("Sector vector delta", vectorAngle);
-    SmartDashboard.putNumber("Sector ray angle", rayAngle);
+    // SmartDashboard.putNumber("Sector delta X", delta.getX());
+    // SmartDashboard.putNumber("Sector delta Y", delta.getY());
+    // SmartDashboard.putNumber("Sector vector delta", vectorAngle);
+    // SmartDashboard.putNumber("Sector ray angle", rayAngle);
     return rayAngle / 60;
   }
 
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 3b4a4dc4..71b94890 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -14,7 +14,6 @@
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.units.measure.LinearVelocity;
 import edu.wpi.first.units.measure.Time;
-import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.util.Color;
 import frc.game.FeederStation;
@@ -39,9 +38,6 @@ public static final class NEOVortexConstants {
   }
 
   public static final class VisionConstants {
-    public static final String kAprilTagLayoutPath =
-        Filesystem.getDeployDirectory() + "/" + "stemgym.json";
-
     // Define the standard deviations for the pose estimator, which determine how fast the pose
     // estimate converges to the vision measurement. This should depend on the vision measurement
     // noise and how many or how frequently vision measurements are applied to the pose estimator.
@@ -228,8 +224,8 @@ public static final class RotationControllerGains {
   public static final class ClimberConstants {
     public static final int kClimberPort = 17;
     public static final int kRatchetServoPort = 1;
-    public static final double kEngagedPosition = 0 / 1024.0;
-    public static final double kDisengedPosition = 1024.0 / 1024.0;
+    public static final double kEngagedPosition = 800 / 1024.0;
+    public static final double kDisengedPosition = 950 / 1024.0;
 
     public static final int kCageSensorPort = 6;
 
diff --git a/src/main/java/frc/robot/LEDs/LEDs.java b/src/main/java/frc/robot/LEDs/LEDs.java
index cc700d95..2799dbce 100644
--- a/src/main/java/frc/robot/LEDs/LEDs.java
+++ b/src/main/java/frc/robot/LEDs/LEDs.java
@@ -326,10 +326,10 @@ public void displayPoseSeek(Pose2d currentPose, Pose2d targetPose) {
         Segments.MIDDLE);
 
     var x = delta.getTranslation().getMeasureX().in(Centimeters);
-    fill(Math.abs(x) < 3 ? Color.kWhite : x > 0 ? Color.kGreen : Color.kRed, Segments.TOP);
+    fill(Math.abs(x) < 5 ? Color.kWhite : x > 0 ? Color.kGreen : Color.kRed, Segments.TOP);
 
     var y = delta.getTranslation().getMeasureY().in(Centimeters);
-    if (Math.abs(y) < 3) {
+    if (Math.abs(y) < 6) {
       fill(Color.kWhite, Segments.BOTTOM);
     } else {
       fill(Color.kGreen, y > 0 ? Segments.leftBottom : Segments.rightBottom);
@@ -371,18 +371,6 @@ public Command createStandardDisplayCommand(
         () -> displayDefaultInfo(isAlgeMode.getAsBoolean(), Optional.ofNullable(gamepiece.get())));
   }
 
-  /**
-   * Create a command to display pose seeking information on the LEDs.
-   *
-   * @param targetPoseSupplier provides the target pose
-   * @param currentPoseSupplier provides the current pose
-   * @return a command to display pose seeking information
-   */
-  public Command createPoseSeekingCommand(
-      Supplier<Pose2d> targetPoseSupplier, Supplier<Pose2d> currentPoseSupplier) {
-    return newCommand(() -> displayPoseSeek(currentPoseSupplier.get(), targetPoseSupplier.get()));
-  }
-
   /**
    * Create a pattern to display stacked blocks on the left and right strips.
    *
@@ -446,8 +434,10 @@ public void displayAutoMode(
          */
         auto -> {
           var hasInitialPose = auto.getInitialPose().isPresent();
-          if (hasInitialPose) {
+          if (hasInitialPose && currentPose != null) {
             displayPoseSeek(currentPose, auto.getInitialPose().get());
+          } else if (currentPose == null) {
+            fill(Color.kDarkGray, Segments.ALL);
           }
           displayAutoSelection(
               auto.getAllianceColor(), auto.getOptionNumber(), agreement, hasInitialPose);
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 903a0f89..916822b0 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -1,5 +1,7 @@
 package frc.robot;
 
+import static edu.wpi.first.units.Units.Seconds;
+
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.networktables.StructArrayPublisher;
@@ -115,7 +117,15 @@ public Gamepiece get() {
         "Align Encoders",
         new InstantCommand(() -> swerve.zeroAbsTurningEncoderOffsets()).ignoringDisable(true));
 
-    addPeriodic(() -> swerve.refreshRelativeTurningEncoder(), 0.1);
+    addPeriodic(() -> swerve.refreshRelativeTurningEncoder(), Seconds.of(0.1));
+    // TODO: see what happens with and without this odometry update
+    // addPeriodic(() -> updateOdometry(), Seconds.of(1));
+  }
+
+  public void updateOdometry() {
+    vision
+        .getEstimatedGlobalPose()
+        .ifPresent(pose -> swerve.resetOdometry(pose.estimatedPose.toPose2d()));
   }
 
   @Override
@@ -125,9 +135,6 @@ public void robotInit() {
     DataLogManager.start();
     DriverStation.startDataLog(DataLogManager.getLog());
 
-    swerve.setDefaultCommand(
-        new ZorroDriveCommand(swerve, DriveConstants.kDriveKinematics, driver.getHID()));
-
     reefTargetPositionsPublisher.set(DriveConstants.kReefTargetPoses);
   }
 
@@ -166,7 +173,10 @@ public void disabledInit() {
     leds.replaceDefaultCommandImmediately(
         leds.createAutoOptionDisplayCommand(
                 autoSelector,
-                () -> swerve.getPose(),
+                () ->
+                    vision.getEstimatedGlobalPose().isPresent()
+                        ? vision.getEstimatedGlobalPose().get().estimatedPose.toPose2d()
+                        : null,
                 allianceSelector.getAgreementInAllianceColor())
             .ignoringDisable(true));
 
@@ -199,6 +209,7 @@ public void disabledPeriodic() {
   @Override
   public void autonomousInit() {
     autoSelector.scheduleAuto();
+    swerve.setDefaultCommand(swerve.createStopCommand());
     lifter.setDefaultCommand(lifter.createRemainAtCurrentHeightCommand());
     leds.replaceDefaultCommandImmediately(
         leds.createStandardDisplayCommand(algaeModeSupplier, gamepieceSupplier));
@@ -210,7 +221,13 @@ public void autonomousPeriodic() {}
   @Override
   public void teleopInit() {
     autoSelector.cancelAuto();
+    swerve.setDefaultCommand(
+        new ZorroDriveCommand(swerve, DriveConstants.kDriveKinematics, driver.getHID()));
+
+    lifter.matchHeight();
+    lifter.resetController();
     lifter.setDefaultCommand(lifter.createJoystickControlCommand(operator.getHID()));
+    // lifter.setDefaultCommand(lifter.createJoystickControlCommand(operator.getHID()));
     leds.replaceDefaultCommandImmediately(
         leds.createStandardDisplayCommand(algaeModeSupplier, gamepieceSupplier));
 
@@ -343,7 +360,7 @@ public boolean getAsBoolean() {
     // Force joystick operation of the elevator
     Trigger elevatorTriggerHigh = operator.axisGreaterThan(Axis.kLeftY.value, 0.9, loop).debounce(0.1);
     Trigger elevatorTriggerLow = operator.axisGreaterThan(Axis.kLeftY.value, -0.9, loop).debounce(0.1);
-    elevatorTriggerHigh.or(elevatorTriggerLow).onTrue(lifter.createJoystickControlCommand(operator.getHID()));
+    // elevatorTriggerHigh.or(elevatorTriggerLow).onTrue(lifter.createJoystickControlCommand(operator.getHID()));
 
     // Actuate climber winch
     // Trigger climbTrigger = operator.axisGreaterThan(Axis.kRightY.value, -0.9, loop).debounce(0.1);
@@ -383,8 +400,8 @@ private void configureEventBindings() {
 
     algaeRoller.hasAlgae
         .whileTrue(algaeRoller.createHoldAlgaeCommand());
-    algaeRoller.hasAlgae
-        .onTrue(algaeWrist.createSetAngleCommand(AlgaeWristState.Barge));
+    // algaeRoller.hasAlgae
+    //     .onTrue(algaeWrist.createSetAngleCommand(AlgaeWristState.Barge));
     coralRoller.isRolling.or(algaeRoller.isRolling).whileTrue(createRollerAnimationCommand());
   }
 
@@ -429,6 +446,7 @@ private synchronized StructPublisher<Pose2d> getPose2dPublisher(String name) {
   }
 
   protected void checkVision() {
+
     vision
         .getPoseEstimates()
         .forEach(
diff --git a/src/main/java/frc/robot/auto/AutoMode.java b/src/main/java/frc/robot/auto/AutoMode.java
index d44bb918..6e85198f 100644
--- a/src/main/java/frc/robot/auto/AutoMode.java
+++ b/src/main/java/frc/robot/auto/AutoMode.java
@@ -7,15 +7,13 @@
 import java.util.Optional;
 
 public abstract class AutoMode {
-  private final Drivetrain drivetrain;
   private final AutoFactory autoFactory;
 
-  public AutoMode(Drivetrain dt) {
-    this.drivetrain = dt;
+  public AutoMode(Drivetrain drivetrain) {
     autoFactory =
         new AutoFactory(
             drivetrain::getPose,
-            drivetrain::setPose,
+            drivetrain::resetOdometry,
             drivetrain::followTrajectory,
             false,
             drivetrain);
diff --git a/src/main/java/frc/robot/auto/BlueL4Auto.java b/src/main/java/frc/robot/auto/BlueL4Auto.java
index 80c97da6..92892d44 100644
--- a/src/main/java/frc/robot/auto/BlueL4Auto.java
+++ b/src/main/java/frc/robot/auto/BlueL4Auto.java
@@ -48,9 +48,7 @@ public AutoRoutine getAutoRoutine() {
     blueL4AutoRoutine.active().onTrue(
       Commands.parallel(
         blueCenterToL4G.cmd(),
-        Commands.sequence(
-          Commands.waitSeconds(1.0),
-          elevator.coralL4PositionCG())));
+        elevator.coralL4PositionCG().withTimeout(2.0)));
 
     blueCenterToL4G.done().onTrue(
         Commands.sequence(
diff --git a/src/main/java/frc/robot/auto/RedL4Auto.java b/src/main/java/frc/robot/auto/RedL4Auto.java
index 4ae8563a..f390a331 100644
--- a/src/main/java/frc/robot/auto/RedL4Auto.java
+++ b/src/main/java/frc/robot/auto/RedL4Auto.java
@@ -45,13 +45,15 @@ public Optional<Pose2d> getInitialPose() {
   public AutoRoutine getAutoRoutine() {
 
     // spotless:off
-    redL4AutoRoutine.active().onTrue(redCenterToL4G.cmd());
+    redL4AutoRoutine.active().onTrue(
+      Commands.parallel(
+        redCenterToL4G.cmd(),
+        elevator.coralL4PositionCG().withTimeout(2.0)));
+        
 
     redCenterToL4G.done().onTrue(
         Commands.sequence(
-            Commands.waitSeconds(0.1),
-            elevator.coralL4PositionCG(),
-            Commands.waitSeconds(0.1),
+            Commands.waitSeconds(0.1), 
             coralRoller.createOuttakeCommand().withTimeout(0.2),
             Commands.waitSeconds(0.2)));
 
diff --git a/src/main/java/frc/robot/drivetrain/Drivetrain.java b/src/main/java/frc/robot/drivetrain/Drivetrain.java
index a07fbc5a..7989eaa5 100644
--- a/src/main/java/frc/robot/drivetrain/Drivetrain.java
+++ b/src/main/java/frc/robot/drivetrain/Drivetrain.java
@@ -11,6 +11,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.numbers.N1;
@@ -47,6 +48,7 @@ public class Drivetrain extends SubsystemBase {
 
   // The robot pose estimator for tracking swerve odometry and applying vision corrections.
   private final SwerveDrivePoseEstimator poseEstimator;
+  private final SwerveDriveOdometry odometry;
 
   private final PIDController xController =
       new PIDController(TranslationControllerGains.kP, 0.0, 0.0);
@@ -58,8 +60,10 @@ public class Drivetrain extends SubsystemBase {
   private final Canandgyro canandgyro = new Canandgyro(0);
   private Rotation2d headingOffset = Rotation2d.kZero;
 
-  private StructPublisher<Pose2d> m_visionPosePublisher =
+  private StructPublisher<Pose2d> visionPosePublisher =
       NetworkTableInstance.getDefault().getStructTopic("Vision", Pose2d.struct).publish();
+  private StructPublisher<Pose2d> odometryPosePublisher =
+      NetworkTableInstance.getDefault().getStructTopic("Odometry", Pose2d.struct).publish();
 
   public Drivetrain(BooleanSupplier fieldRotated, Supplier<Dimensionless> elevatorHeight) {
     this.fieldRotatedSupplier = fieldRotated;
@@ -81,6 +85,11 @@ public Drivetrain(BooleanSupplier fieldRotated, Supplier<Dimensionless> elevator
             Pose2d.kZero,
             DriveConstants.kStateStdDevs,
             VisionConstants.kMultiTagStdDevs);
+    odometry =
+        new SwerveDriveOdometry(
+            DriveConstants.kDriveKinematics,
+            canandgyro.getRotation2d(),
+            getSwerveModulePositions());
 
     thetaController.enableContinuousInput(-Math.PI, Math.PI);
   }
@@ -88,7 +97,9 @@ public Drivetrain(BooleanSupplier fieldRotated, Supplier<Dimensionless> elevator
   @Override
   public void periodic() {
     poseEstimator.update(canandgyro.getRotation2d(), getSwerveModulePositions());
-    m_visionPosePublisher.set(poseEstimator.getEstimatedPosition());
+    odometry.update(canandgyro.getRotation2d(), getSwerveModulePositions());
+    visionPosePublisher.set(poseEstimator.getEstimatedPosition());
+    odometryPosePublisher.set(odometry.getPoseMeters());
 
     for (SwerveModule module : SwerveModule.values()) {
       SmartDashboard.putNumber(
@@ -169,8 +180,20 @@ public Pose2d getPose() {
   /**
    * @param pose The robot pose
    */
-  public void setPose(Pose2d pose) {
+  public void resetOdometry(Pose2d pose) {
+    resetOdometry(pose, false);
+  }
+
+  /**
+   * Reset the swerve drive's field pose.
+   *
+   * @param pose New robot pose.
+   * @param resetSimPose If the simulated robot pose should also be reset. This effectively
+   *     teleports the robot and should only be used during the setup of the simulation world.
+   */
+  public void resetOdometry(Pose2d pose, boolean resetSimPose) {
     poseEstimator.resetPosition(canandgyro.getRotation2d(), getSwerveModulePositions(), pose);
+    odometry.resetPosition(canandgyro.getRotation2d(), getSwerveModulePositions(), pose);
   }
 
   public void initializeRelativeTurningEncoder() {
@@ -192,6 +215,10 @@ public void setHeadingOffset() {
         fieldRotatedSupplier.getAsBoolean() ? getHeading().rotateBy(Rotation2d.kPi) : getHeading();
   }
 
+  public Command createStopCommand() {
+    return this.run(() -> setRobotRelativeChassisSpeeds(new ChassisSpeeds()));
+  }
+
   /**
    * @return Array of swerve module positions
    */
@@ -232,17 +259,6 @@ public void addVisionMeasurement(
     poseEstimator.addVisionMeasurement(visionMeasurement, timestampSeconds, stdDevs);
   }
 
-  /**
-   * Reset the estimated pose of the swerve drive on the field.
-   *
-   * @param pose New robot pose.
-   * @param resetSimPose If the simulated robot pose should also be reset. This effectively
-   *     teleports the robot and should only be used during the setup of the simulation world.
-   */
-  public void resetPose(Pose2d pose, boolean resetSimPose) {
-    poseEstimator.resetPosition(canandgyro.getRotation2d(), getSwerveModulePositions(), pose);
-  }
-
   public void followTrajectory(SwerveSample sample) {
     Pose2d pose = getPose();
 
diff --git a/src/main/java/frc/robot/elevator/AlgaeRoller.java b/src/main/java/frc/robot/elevator/AlgaeRoller.java
index 7ada5c6a..be6204e7 100644
--- a/src/main/java/frc/robot/elevator/AlgaeRoller.java
+++ b/src/main/java/frc/robot/elevator/AlgaeRoller.java
@@ -67,8 +67,8 @@ public AlgaeRoller() {
   @Override
   public void periodic() {
     SmartDashboard.putNumber("Algae Roller/Velocity", encoder.getVelocity());
-    // SmartDashboard.putNumber("Algae Roller/Applied Duty Cycle", leaderMotor.getAppliedOutput());
-    // SmartDashboard.putNumber("Algae Roller/Current", leaderMotor.getOutputCurrent());
+    SmartDashboard.putNumber("Algae Roller/Applied Duty Cycle", leaderMotor.getAppliedOutput());
+    SmartDashboard.putNumber("Algae Roller/Current", leaderMotor.getOutputCurrent());
     SmartDashboard.putBoolean("Algae Loaded", hasAlgae.getAsBoolean());
     SmartDashboard.putBoolean("Algae isRolling", isRolling.getAsBoolean());
   }
diff --git a/src/main/java/frc/robot/elevator/Elevator.java b/src/main/java/frc/robot/elevator/Elevator.java
index db610525..44000b55 100644
--- a/src/main/java/frc/robot/elevator/Elevator.java
+++ b/src/main/java/frc/robot/elevator/Elevator.java
@@ -49,6 +49,7 @@ public AlgaeWrist getAlgaeWrist() {
   public Command resetPositionControllers() {
     return new InstantCommand(
         () -> {
+          lifter.matchHeight();
           lifter.resetController();
           coralWrist.resetController();
           algaeWrist.resetController();
diff --git a/src/main/java/frc/robot/elevator/ElevatorConstants.java b/src/main/java/frc/robot/elevator/ElevatorConstants.java
index a2ddb688..b18238a0 100644
--- a/src/main/java/frc/robot/elevator/ElevatorConstants.java
+++ b/src/main/java/frc/robot/elevator/ElevatorConstants.java
@@ -73,7 +73,7 @@ public static enum LifterState {
       CoralIntake(11.7),
       AlgaeProcessor(12.0),
       AlgaeL2(29.5),
-      AlgaeL3(45),
+      AlgaeL3(43),
       AlgaeBarge(67.8),
       Max(68.3);
 
diff --git a/src/main/java/frc/robot/elevator/Lifter.java b/src/main/java/frc/robot/elevator/Lifter.java
index 8f8cf789..56e039ef 100644
--- a/src/main/java/frc/robot/elevator/Lifter.java
+++ b/src/main/java/frc/robot/elevator/Lifter.java
@@ -181,12 +181,16 @@ public Command createSetHeightCommand(LifterState state) {
         this);
   }
 
+  public void matchHeight() {
+    feedback.setGoal(encoder.getPosition());
+  }
+
   public Command createRemainAtCurrentHeightCommand() {
     return new FunctionalCommand(
         // initialize
         () -> {
           if (targetState == LifterState.Initial) {
-            feedback.setGoal(encoder.getPosition());
+            matchHeight();
             // Users should call reset() when they first start running the controller to avoid
             // unwanted behavior.
             resetController();
@@ -209,8 +213,14 @@ private Boolean isInRange(Distance height) {
   }
 
   public Command createJoystickControlCommand(XboxController gamepad) {
-    return this.run(
-        () -> {
+    return new FunctionalCommand(
+      // initialize
+      () -> { 
+        // matchHeight();
+        // resetController();
+      },
+      // execute
+      () -> {
           Distance targetPosition = Meters.of(feedback.getGoal().position);
 
           double joystickInput = MathUtil.applyDeadband(-gamepad.getLeftY(), 0.05);
@@ -220,7 +230,13 @@ public Command createJoystickControlCommand(XboxController gamepad) {
 
           if (isInRange(targetPosition)) feedback.setGoal(targetPosition.in(Meters));
           control();
-        });
+        },
+      // end
+      interrupted -> {},
+      // isFinished
+      () -> false,
+      // requirements
+      this);
   }
 
   public Command createJoystickVoltageCommand(XboxController gamepad) {
diff --git a/src/main/java/frc/robot/vision/Camera.java b/src/main/java/frc/robot/vision/Camera.java
index dae4b616..a6d99970 100644
--- a/src/main/java/frc/robot/vision/Camera.java
+++ b/src/main/java/frc/robot/vision/Camera.java
@@ -9,9 +9,8 @@
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.wpilibj2.command.Subsystem;
 import frc.robot.Constants;
-import frc.robot.Constants.VisionConstants;
-import java.io.IOException;
 import java.util.List;
 import java.util.Optional;
 import org.photonvision.EstimatedRobotPose;
@@ -20,7 +19,7 @@
 import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 import org.photonvision.targeting.PhotonTrackedTarget;
 
-public enum Camera {
+public enum Camera implements Subsystem {
   FrontRight("OV2311_TH_8", new Translation3d(0.248, -0.318, 0.513), new Rotation3d(0.0, 0.0, 0.0)),
   FrontLeft(
       "OV2311_TH_5", new Translation3d(0.222, 0.331, 0.513), new Rotation3d(0.0, 0, Math.PI / 2.0)),
@@ -34,33 +33,53 @@ public enum Camera {
   public final String name;
   public final Transform3d transform;
   public final PhotonCamera device;
-  public PhotonPoseEstimator pose;
+
+  private Optional<EstimatedRobotPose> pose;
+  private final PhotonPoseEstimator poseEstimator;
   private Matrix<N3, N1> curStdDevs;
 
   private Camera(String name, Translation3d translation, Rotation3d rotation) {
     this.name = name;
     this.transform = new Transform3d(translation, rotation);
     this.device = new PhotonCamera(name);
+    this.pose = Optional.empty();
 
-    // TODO: switch back to official field layout
-    AprilTagFieldLayout tagLayout;
-    try {
-      tagLayout = new AprilTagFieldLayout(VisionConstants.kAprilTagLayoutPath);
-    } catch (IOException e) {
-      System.err.println("Error loading custom AprilTag layout: " + e.getMessage());
-      tagLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
-    }
+    // // TODO: switch back to official field layout
+    // AprilTagFieldLayout tagLayout;
+    // try {
+    //   tagLayout = new AprilTagFieldLayout(VisionConstants.kAprilTagLayoutPath);
+    // } catch (IOException e) {
+    //   System.err.println("Error loading custom AprilTag layout: " + e.getMessage());
+    //   tagLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
+    // }
 
-    // AprilTagFieldLayout tagLayout =
-    //     AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeAndyMark);
+    AprilTagFieldLayout tagLayout =
+        AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeAndyMark);
 
-    this.pose =
+    this.poseEstimator =
         new PhotonPoseEstimator(tagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, transform);
+    register();
+  }
+
+  /** Updates the pose estimate for this camera */
+  @Override
+  public void periodic() {
+    device
+        .getAllUnreadResults()
+        .forEach(
+            change -> {
+              pose = poseEstimator.update(change);
+              updateEstimationStdDevs(pose, change.getTargets());
+            });
+  }
+
+  @Override
+  public String getName() {
+    return "Camera." + toString();
   }
 
   /**
-   * The latest estimated robot pose on the field from vision data. This may be empty. This should
-   * only be called once per loop.
+   * The latest estimated robot pose on the field from vision data. This may be empty.
    *
    * <p>Also includes updates for the standard deviations, which can (optionally) be retrieved with
    * {@link getEstimationStdDevs}
@@ -69,12 +88,17 @@ private Camera(String name, Translation3d translation, Rotation3d rotation) {
    *     used for estimation.
    */
   public Optional<EstimatedRobotPose> getEstimatedGlobalPose() {
-    Optional<EstimatedRobotPose> est = Optional.empty();
-    for (var change : device.getAllUnreadResults()) {
-      est = pose.update(change);
-      updateEstimationStdDevs(est, change.getTargets());
-    }
-    return est;
+    return pose;
+  }
+
+  /**
+   * Returns the latest standard deviations of the estimated pose from {@link
+   * #getEstimatedGlobalPose()}, for use with {@link
+   * edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should
+   * only be used when there are targets visible.
+   */
+  public Matrix<N3, N1> getEstimationStdDevs() {
+    return curStdDevs;
   }
 
   /**
@@ -98,7 +122,7 @@ private void updateEstimationStdDevs(
 
       // Precalculation - see how many tags we found, and calculate an average-distance metric
       for (var tgt : targets) {
-        var tagPose = pose.getFieldTags().getTagPose(tgt.getFiducialId());
+        var tagPose = poseEstimator.getFieldTags().getTagPose(tgt.getFiducialId());
         if (tagPose.isEmpty()) continue;
         numTags++;
         avgDist +=
@@ -125,14 +149,4 @@ private void updateEstimationStdDevs(
       }
     }
   }
-
-  /**
-   * Returns the latest standard deviations of the estimated pose from {@link
-   * #getEstimatedGlobalPose()}, for use with {@link
-   * edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should
-   * only be used when there are targets visible.
-   */
-  public Matrix<N3, N1> getEstimationStdDevs() {
-    return curStdDevs;
-  }
 }
diff --git a/src/main/java/frc/robot/vision/Vision.java b/src/main/java/frc/robot/vision/Vision.java
index 44897e3c..abbd2a74 100644
--- a/src/main/java/frc/robot/vision/Vision.java
+++ b/src/main/java/frc/robot/vision/Vision.java
@@ -3,12 +3,29 @@
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import java.util.Arrays;
 import java.util.List;
+import java.util.Optional;
 import java.util.stream.Collectors;
 import org.photonvision.EstimatedRobotPose;
 
-public class Vision {
+public class Vision extends SubsystemBase {
+
+  /** Choose the pose estimate with the lowest maximum stdev. */
+  public Optional<EstimatedRobotPose> getEstimatedGlobalPose() {
+    return Arrays.stream(Camera.values())
+        .sorted(
+            (lhs, rhs) ->
+                (int)
+                    Math.signum(
+                        lhs.getEstimationStdDevs().max() - rhs.getEstimationStdDevs().max()))
+        .map(cam -> cam.getEstimatedGlobalPose())
+        .filter(Optional::isPresent)
+        .map(Optional::get)
+        .findFirst();
+  }
+
   public record EstimatedPoseWithStdevs(
       EstimatedRobotPose pose, Matrix<N3, N1> stdev, String name) {}