diff --git a/src/main/deploy/choreo/ExamplePathOne.traj b/src/main/deploy/choreo/ExamplePathOne.traj
new file mode 100644
index 00000000..f0d9fe67
--- /dev/null
+++ b/src/main/deploy/choreo/ExamplePathOne.traj
@@ -0,0 +1,219 @@
+{
+ "name":"ExamplePathOne",
+ "version":3,
+ "snapshot":{
+  "waypoints":[
+    {"x":2.3845205307006836, "y":5.301450252532959, "heading":0.19634954084936207, "intervals":73, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":2.306560516357422, "y":5.98360013961792, "heading":0.0, "intervals":84, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":2.8327901363372803, "y":6.531269073486328, "heading":-0.7853977386450895, "intervals":29, "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":16.541, "h":8.0692}}, "enabled":false},
+    {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":0.2}}, "enabled":true}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"2.3845205307006836 m", "val":2.3845205307006836}, "y":{"exp":"5.301450252532959 m", "val":5.301450252532959}, "heading":{"exp":"0.19634954084936207 rad", "val":0.19634954084936207}, "intervals":73, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"2.306560516357422 m", "val":2.306560516357422}, "y":{"exp":"5.98360013961792 m", "val":5.98360013961792}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":84, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"2.8327901363372803 m", "val":2.8327901363372803}, "y":{"exp":"6.531269073486328 m", "val":6.531269073486328}, "heading":{"exp":"-0.7853977386450895 rad", "val":-0.7853977386450895}, "intervals":29, "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":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false},
+    {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"0.2 m / s", "val":0.2}}}, "enabled":true}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "config":{
+   "frontLeft":{
+    "x":0.2794,
+    "y":0.2794
+   },
+   "backLeft":{
+    "x":-0.2794,
+    "y":0.2794
+   },
+   "mass":68.0388555,
+   "inertia":6.0,
+   "gearing":6.5,
+   "radius":0.0508,
+   "vmax":628.3185307179587,
+   "tmax":1.2,
+   "cof":1.5,
+   "bumper":{
+    "front":0.4064,
+    "side":0.4064,
+    "back":0.4064
+   },
+   "differentialTrackWidth":0.5588
+  },
+  "sampleType":"Swerve",
+  "waypoints":[0.0,3.45954,7.28297],
+  "samples":[
+    {"t":0.0, "x":2.38452, "y":5.30145, "heading":0.19635, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-0.49122, "ay":4.19064, "alpha":-0.11067, "fx":[-7.94317,-8.05555,-8.76668,-8.65686], "fy":[71.1155,71.56831,71.44782,70.99456]},
+    {"t":0.04739, "x":2.38397, "y":5.30616, "heading":0.19635, "vx":-0.02328, "vy":0.1986, "omega":-0.00524, "ax":0.0, "ay":0.0, "alpha":-0.14757, "fx":[0.4658,0.31124,-0.4658,-0.31124], "fy":[-0.31123,0.4658,0.31124,-0.46579]},
+    {"t":0.09478, "x":2.38287, "y":5.31557, "heading":0.1961, "vx":-0.02328, "vy":0.1986, "omega":-0.01224, "ax":0.0, "ay":0.0, "alpha":-0.12648, "fx":[0.39916,0.26686,-0.39916,-0.26686], "fy":[-0.26686,0.39916,0.26686,-0.39916]},
+    {"t":0.14217, "x":2.38176, "y":5.32498, "heading":0.19552, "vx":-0.02328, "vy":0.1986, "omega":-0.01823, "ax":0.0, "ay":0.0, "alpha":-0.10841, "fx":[0.34198,0.22892,-0.34198,-0.22892], "fy":[-0.22892,0.34198,0.22892,-0.34198]},
+    {"t":0.18956, "x":2.38066, "y":5.33439, "heading":0.19466, "vx":-0.02328, "vy":0.1986, "omega":-0.02337, "ax":0.0, "ay":0.0, "alpha":-0.09291, "fx":[0.29294,0.19646,-0.29294,-0.19646], "fy":[-0.19646,0.29294,0.19646,-0.29294]},
+    {"t":0.23695, "x":2.37956, "y":5.3438, "heading":0.19355, "vx":-0.02328, "vy":0.1986, "omega":-0.02777, "ax":0.0, "ay":0.0, "alpha":-0.07964, "fx":[0.25089,0.16866,-0.25089,-0.16866], "fy":[-0.16866,0.25089,0.16866,-0.25089]},
+    {"t":0.28435, "x":2.37845, "y":5.35321, "heading":0.19223, "vx":-0.02328, "vy":0.1986, "omega":-0.03155, "ax":0.0, "ay":0.0, "alpha":-0.06826, "fx":[0.21485,0.14485,-0.21485,-0.14485], "fy":[-0.14485,0.21485,0.14485,-0.21485]},
+    {"t":0.33174, "x":2.37735, "y":5.36263, "heading":0.19074, "vx":-0.02328, "vy":0.1986, "omega":-0.03478, "ax":0.0, "ay":0.0, "alpha":-0.0585, "fx":[0.18397,0.12443,-0.18397,-0.12443], "fy":[-0.12443,0.18397,0.12443,-0.18397]},
+    {"t":0.37913, "x":2.37625, "y":5.37204, "heading":0.18909, "vx":-0.02328, "vy":0.1986, "omega":-0.03755, "ax":0.0, "ay":0.0, "alpha":-0.05015, "fx":[0.15751,0.10691,-0.15751,-0.10691], "fy":[-0.10691,0.15751,0.10691,-0.15751]},
+    {"t":0.42652, "x":2.37514, "y":5.38145, "heading":0.18731, "vx":-0.02328, "vy":0.1986, "omega":-0.03993, "ax":0.0, "ay":0.0, "alpha":-0.04298, "fx":[0.13485,0.09188,-0.13485,-0.09188], "fy":[-0.09188,0.13485,0.09188,-0.13485]},
+    {"t":0.47391, "x":2.37404, "y":5.39086, "heading":0.18542, "vx":-0.02328, "vy":0.1986, "omega":-0.04197, "ax":0.0, "ay":0.0, "alpha":-0.03685, "fx":[0.11545,0.07898,-0.11545,-0.07898], "fy":[-0.07898,0.11545,0.07898,-0.11545]},
+    {"t":0.5213, "x":2.37294, "y":5.40027, "heading":0.18343, "vx":-0.02328, "vy":0.1986, "omega":-0.04371, "ax":0.0, "ay":0.0, "alpha":-0.03159, "fx":[0.09883,0.0679,-0.09883,-0.0679], "fy":[-0.0679,0.09883,0.0679,-0.09883]},
+    {"t":0.56869, "x":2.37183, "y":5.40969, "heading":0.18136, "vx":-0.02328, "vy":0.1986, "omega":-0.04521, "ax":0.0, "ay":0.0, "alpha":-0.02708, "fx":[0.0846,0.05839,-0.0846,-0.05839], "fy":[-0.05839,0.0846,0.05839,-0.0846]},
+    {"t":0.61608, "x":2.37073, "y":5.4191, "heading":0.17921, "vx":-0.02328, "vy":0.1986, "omega":-0.04649, "ax":0.0, "ay":0.0, "alpha":-0.02322, "fx":[0.07243,0.05021,-0.07243,-0.05021], "fy":[-0.05021,0.07243,0.05021,-0.07243]},
+    {"t":0.66347, "x":2.36963, "y":5.42851, "heading":0.17701, "vx":-0.02328, "vy":0.1986, "omega":-0.04759, "ax":0.0, "ay":0.0, "alpha":-0.01991, "fx":[0.06202,0.0432,-0.06202,-0.0432], "fy":[-0.0432,0.06202,0.0432,-0.06202]},
+    {"t":0.71086, "x":2.36852, "y":5.43792, "heading":0.17476, "vx":-0.02328, "vy":0.1986, "omega":-0.04854, "ax":0.0, "ay":0.0, "alpha":-0.01708, "fx":[0.05311,0.03717,-0.05311,-0.03717], "fy":[-0.03717,0.05311,0.03717,-0.05311]},
+    {"t":0.75826, "x":2.36742, "y":5.44733, "heading":0.17246, "vx":-0.02328, "vy":0.1986, "omega":-0.04935, "ax":0.0, "ay":0.0, "alpha":-0.01465, "fx":[0.04549,0.03199,-0.04549,-0.03199], "fy":[-0.03199,0.04549,0.03199,-0.04549]},
+    {"t":0.80565, "x":2.36632, "y":5.45674, "heading":0.17012, "vx":-0.02328, "vy":0.1986, "omega":-0.05004, "ax":0.0, "ay":0.0, "alpha":-0.01257, "fx":[0.03897,0.02755,-0.03897,-0.02755], "fy":[-0.02755,0.03897,0.02755,-0.03897]},
+    {"t":0.85304, "x":2.36521, "y":5.46616, "heading":0.16775, "vx":-0.02328, "vy":0.1986, "omega":-0.05064, "ax":0.0, "ay":0.0, "alpha":-0.01079, "fx":[0.03341,0.02373,-0.03341,-0.02373], "fy":[-0.02373,0.03341,0.02373,-0.03341]},
+    {"t":0.90043, "x":2.36411, "y":5.47557, "heading":0.16535, "vx":-0.02328, "vy":0.1986, "omega":-0.05115, "ax":0.0, "ay":0.0, "alpha":-0.00927, "fx":[0.02865,0.02046,-0.02865,-0.02046], "fy":[-0.02046,0.02865,0.02046,-0.02865]},
+    {"t":0.94782, "x":2.36301, "y":5.48498, "heading":0.16292, "vx":-0.02328, "vy":0.1986, "omega":-0.05159, "ax":0.0, "ay":0.0, "alpha":-0.00797, "fx":[0.02459,0.01765,-0.02459,-0.01765], "fy":[-0.01765,0.02459,0.01765,-0.02459]},
+    {"t":0.99521, "x":2.3619, "y":5.49439, "heading":0.16048, "vx":-0.02328, "vy":0.1986, "omega":-0.05197, "ax":0.0, "ay":0.0, "alpha":-0.00686, "fx":[0.02113,0.01524,-0.02113,-0.01524], "fy":[-0.01524,0.02113,0.01524,-0.02113]},
+    {"t":1.0426, "x":2.3608, "y":5.5038, "heading":0.15801, "vx":-0.02328, "vy":0.1986, "omega":-0.05229, "ax":0.0, "ay":0.0, "alpha":-0.00592, "fx":[0.01818,0.01319,-0.01818,-0.01319], "fy":[-0.01319,0.01818,0.01319,-0.01818]},
+    {"t":1.08999, "x":2.3597, "y":5.51322, "heading":0.15554, "vx":-0.02328, "vy":0.1986, "omega":-0.05257, "ax":0.0, "ay":0.0, "alpha":-0.00511, "fx":[0.01568,0.01143,-0.01568,-0.01143], "fy":[-0.01143,0.01568,0.01143,-0.01568]},
+    {"t":1.13738, "x":2.35859, "y":5.52263, "heading":0.15304, "vx":-0.02328, "vy":0.1986, "omega":-0.05281, "ax":0.0, "ay":0.0, "alpha":-0.00443, "fx":[0.01356,0.00994,-0.01356,-0.00994], "fy":[-0.00994,0.01356,0.00994,-0.01356]},
+    {"t":1.18477, "x":2.35749, "y":5.53204, "heading":0.15054, "vx":-0.02328, "vy":0.1986, "omega":-0.05302, "ax":0.0, "ay":0.0, "alpha":-0.00385, "fx":[0.01177,0.00867,-0.01177,-0.00867], "fy":[-0.00867,0.01177,0.00867,-0.01177]},
+    {"t":1.23217, "x":2.35639, "y":5.54145, "heading":0.14803, "vx":-0.02328, "vy":0.1986, "omega":-0.05321, "ax":0.0, "ay":0.0, "alpha":-0.00336, "fx":[0.01026,0.0076,-0.01026,-0.0076], "fy":[-0.0076,0.01026,0.0076,-0.01026]},
+    {"t":1.27956, "x":2.35528, "y":5.55086, "heading":0.14551, "vx":-0.02328, "vy":0.1986, "omega":-0.05337, "ax":0.0, "ay":0.0, "alpha":-0.00296, "fx":[0.00901,0.00671,-0.00901,-0.00671], "fy":[-0.00671,0.00901,0.00671,-0.00901]},
+    {"t":1.32695, "x":2.35418, "y":5.56027, "heading":0.14298, "vx":-0.02328, "vy":0.1986, "omega":-0.05351, "ax":0.0, "ay":0.0, "alpha":-0.00262, "fx":[0.00797,0.00596,-0.00797,-0.00596], "fy":[-0.00596,0.00797,0.00596,-0.00797]},
+    {"t":1.37434, "x":2.35308, "y":5.56969, "heading":0.14044, "vx":-0.02328, "vy":0.1986, "omega":-0.05363, "ax":0.0, "ay":0.0, "alpha":-0.00235, "fx":[0.00712,0.00536,-0.00712,-0.00536], "fy":[-0.00536,0.00712,0.00536,-0.00712]},
+    {"t":1.42173, "x":2.35197, "y":5.5791, "heading":0.1379, "vx":-0.02328, "vy":0.1986, "omega":-0.05374, "ax":0.0, "ay":0.0, "alpha":-0.00213, "fx":[0.00645,0.00488,-0.00645,-0.00488], "fy":[-0.00488,0.00645,0.00488,-0.00645]},
+    {"t":1.46912, "x":2.35087, "y":5.58851, "heading":0.13535, "vx":-0.02328, "vy":0.1986, "omega":-0.05384, "ax":0.0, "ay":0.0, "alpha":-0.00196, "fx":[0.00594,0.00452,-0.00594,-0.00452], "fy":[-0.00452,0.00594,0.00452,-0.00594]},
+    {"t":1.51651, "x":2.34977, "y":5.59792, "heading":0.1328, "vx":-0.02328, "vy":0.1986, "omega":-0.05394, "ax":0.0, "ay":0.0, "alpha":-0.00185, "fx":[0.00557,0.00426,-0.00557,-0.00426], "fy":[-0.00426,0.00557,0.00426,-0.00557]},
+    {"t":1.5639, "x":2.34867, "y":5.60733, "heading":0.13025, "vx":-0.02328, "vy":0.1986, "omega":-0.05402, "ax":0.0, "ay":0.0, "alpha":-0.00177, "fx":[0.00533,0.0041,-0.00533,-0.0041], "fy":[-0.0041,0.00533,0.0041,-0.00533]},
+    {"t":1.61129, "x":2.34756, "y":5.61674, "heading":0.12769, "vx":-0.02328, "vy":0.1986, "omega":-0.05411, "ax":0.0, "ay":0.0, "alpha":-0.00174, "fx":[0.00522,0.00404,-0.00522,-0.00404], "fy":[-0.00404,0.00522,0.00404,-0.00522]},
+    {"t":1.65868, "x":2.34646, "y":5.62616, "heading":0.12512, "vx":-0.02328, "vy":0.1986, "omega":-0.05419, "ax":0.0, "ay":0.0, "alpha":-0.00175, "fx":[0.00524,0.00407,-0.00524,-0.00407], "fy":[-0.00407,0.00524,0.00407,-0.00524]},
+    {"t":1.70607, "x":2.34536, "y":5.63557, "heading":0.12255, "vx":-0.02328, "vy":0.1986, "omega":-0.05427, "ax":0.0, "ay":0.0, "alpha":-0.0018, "fx":[0.00538,0.00421,-0.00538,-0.00421], "fy":[-0.00421,0.00538,0.00421,-0.00538]},
+    {"t":1.75347, "x":2.34425, "y":5.64498, "heading":0.11998, "vx":-0.02328, "vy":0.1986, "omega":-0.05436, "ax":0.0, "ay":0.0, "alpha":-0.00189, "fx":[0.00566,0.00444,-0.00566,-0.00444], "fy":[-0.00444,0.00566,0.00444,-0.00566]},
+    {"t":1.80086, "x":2.34315, "y":5.65439, "heading":0.11741, "vx":-0.02328, "vy":0.1986, "omega":-0.05445, "ax":0.0, "ay":0.0, "alpha":-0.00203, "fx":[0.00606,0.00479,-0.00606,-0.00479], "fy":[-0.00479,0.00606,0.00479,-0.00606]},
+    {"t":1.84825, "x":2.34205, "y":5.6638, "heading":0.11483, "vx":-0.02328, "vy":0.1986, "omega":-0.05454, "ax":0.0, "ay":0.0, "alpha":-0.00222, "fx":[0.00661,0.00525,-0.00661,-0.00525], "fy":[-0.00525,0.00661,0.00525,-0.00661]},
+    {"t":1.89564, "x":2.34094, "y":5.67322, "heading":0.11224, "vx":-0.02328, "vy":0.1986, "omega":-0.05465, "ax":0.0, "ay":0.0, "alpha":-0.00247, "fx":[0.00732,0.00584,-0.00732,-0.00584], "fy":[-0.00584,0.00732,0.00584,-0.00732]},
+    {"t":1.94303, "x":2.33984, "y":5.68263, "heading":0.10965, "vx":-0.02328, "vy":0.1986, "omega":-0.05477, "ax":0.0, "ay":0.0, "alpha":-0.00277, "fx":[0.00819,0.00657,-0.00819,-0.00657], "fy":[-0.00657,0.00819,0.00657,-0.00819]},
+    {"t":1.99042, "x":2.33874, "y":5.69204, "heading":0.10706, "vx":-0.02328, "vy":0.1986, "omega":-0.0549, "ax":0.0, "ay":0.0, "alpha":-0.00313, "fx":[0.00926,0.00747,-0.00926,-0.00747], "fy":[-0.00747,0.00926,0.00747,-0.00926]},
+    {"t":2.03781, "x":2.33763, "y":5.70145, "heading":0.10445, "vx":-0.02328, "vy":0.1986, "omega":-0.05505, "ax":0.0, "ay":0.0, "alpha":-0.00358, "fx":[0.01054,0.00855,-0.01054,-0.00855], "fy":[-0.00855,0.01054,0.00855,-0.01054]},
+    {"t":2.0852, "x":2.33653, "y":5.71086, "heading":0.10184, "vx":-0.02328, "vy":0.1986, "omega":-0.05522, "ax":0.0, "ay":0.0, "alpha":-0.0041, "fx":[0.01207,0.00984,-0.01207,-0.00984], "fy":[-0.00984,0.01207,0.00984,-0.01207]},
+    {"t":2.13259, "x":2.33543, "y":5.72027, "heading":0.09923, "vx":-0.02328, "vy":0.1986, "omega":-0.05541, "ax":0.0, "ay":0.0, "alpha":-0.00473, "fx":[0.01388,0.01138,-0.01388,-0.01138], "fy":[-0.01138,0.01388,0.01138,-0.01388]},
+    {"t":2.17998, "x":2.33432, "y":5.72969, "heading":0.0966, "vx":-0.02328, "vy":0.1986, "omega":-0.05563, "ax":0.0, "ay":0.0, "alpha":-0.00547, "fx":[0.01602,0.0132,-0.01602,-0.0132], "fy":[-0.0132,0.01602,0.0132,-0.01602]},
+    {"t":2.22738, "x":2.33322, "y":5.7391, "heading":0.09397, "vx":-0.02328, "vy":0.1986, "omega":-0.05589, "ax":0.0, "ay":0.0, "alpha":-0.00634, "fx":[0.01853,0.01534,-0.01853,-0.01534], "fy":[-0.01534,0.01853,0.01534,-0.01853]},
+    {"t":2.27477, "x":2.33212, "y":5.74851, "heading":0.09132, "vx":-0.02328, "vy":0.1986, "omega":-0.05619, "ax":0.0, "ay":0.0, "alpha":-0.00736, "fx":[0.02147,0.01787,-0.02147,-0.01787], "fy":[-0.01787,0.02147,0.01787,-0.02147]},
+    {"t":2.32216, "x":2.33101, "y":5.75792, "heading":0.08865, "vx":-0.02328, "vy":0.1986, "omega":-0.05654, "ax":0.0, "ay":0.0, "alpha":-0.00855, "fx":[0.0249,0.02085,-0.0249,-0.02085], "fy":[-0.02085,0.0249,0.02085,-0.0249]},
+    {"t":2.36955, "x":2.32991, "y":5.76733, "heading":0.08597, "vx":-0.02328, "vy":0.1986, "omega":-0.05695, "ax":0.0, "ay":0.0, "alpha":-0.00996, "fx":[0.02892,0.02434,-0.02892,-0.02434], "fy":[-0.02434,0.02892,0.02434,-0.02892]},
+    {"t":2.41694, "x":2.32881, "y":5.77674, "heading":0.08328, "vx":-0.02328, "vy":0.1986, "omega":-0.05742, "ax":0.0, "ay":0.0, "alpha":-0.0116, "fx":[0.0336,0.02844,-0.0336,-0.02844], "fy":[-0.02844,0.0336,0.02844,-0.0336]},
+    {"t":2.46433, "x":2.3277, "y":5.78616, "heading":0.08055, "vx":-0.02328, "vy":0.1986, "omega":-0.05797, "ax":0.0, "ay":0.0, "alpha":-0.01351, "fx":[0.03907,0.03324,-0.03907,-0.03324], "fy":[-0.03324,0.03907,0.03324,-0.03907]},
+    {"t":2.51172, "x":2.3266, "y":5.79557, "heading":0.07781, "vx":-0.02328, "vy":0.1986, "omega":-0.05861, "ax":0.0, "ay":0.0, "alpha":-0.01575, "fx":[0.04544,0.03888,-0.04544,-0.03888], "fy":[-0.03888,0.04544,0.03888,-0.04544]},
+    {"t":2.55911, "x":2.3255, "y":5.80498, "heading":0.07503, "vx":-0.02328, "vy":0.1986, "omega":-0.05936, "ax":0.0, "ay":0.0, "alpha":-0.01837, "fx":[0.05286,0.04548,-0.05286,-0.04548], "fy":[-0.04548,0.05286,0.04548,-0.05286]},
+    {"t":2.6065, "x":2.32439, "y":5.81439, "heading":0.07222, "vx":-0.02328, "vy":0.1986, "omega":-0.06023, "ax":0.0, "ay":0.0, "alpha":-0.02142, "fx":[0.0615,0.05321,-0.0615,-0.05321], "fy":[-0.05321,0.0615,0.05321,-0.0615]},
+    {"t":2.65389, "x":2.32329, "y":5.8238, "heading":0.06936, "vx":-0.02328, "vy":0.1986, "omega":-0.06124, "ax":0.0, "ay":0.0, "alpha":-0.02499, "fx":[0.07155,0.06227,-0.07155,-0.06227], "fy":[-0.06227,0.07155,0.06227,-0.07155]},
+    {"t":2.70128, "x":2.32219, "y":5.83322, "heading":0.06646, "vx":-0.02328, "vy":0.1986, "omega":-0.06243, "ax":0.0, "ay":0.0, "alpha":-0.02915, "fx":[0.08325,0.07287,-0.08325,-0.07287], "fy":[-0.07287,0.08325,0.07287,-0.08325]},
+    {"t":2.74868, "x":2.32108, "y":5.84263, "heading":0.0635, "vx":-0.02328, "vy":0.1986, "omega":-0.06381, "ax":0.0, "ay":0.0, "alpha":-0.034, "fx":[0.09687,0.0853,-0.09687,-0.0853], "fy":[-0.0853,0.09687,0.0853,-0.09687]},
+    {"t":2.79607, "x":2.31998, "y":5.85204, "heading":0.06048, "vx":-0.02328, "vy":0.1986, "omega":-0.06542, "ax":0.0, "ay":0.0, "alpha":-0.03967, "fx":[0.11271,0.09986,-0.11271,-0.09986], "fy":[-0.09986,0.11271,0.09986,-0.11271]},
+    {"t":2.84346, "x":2.31888, "y":5.86145, "heading":0.05738, "vx":-0.02328, "vy":0.1986, "omega":-0.0673, "ax":0.0, "ay":0.0, "alpha":-0.04628, "fx":[0.13114,0.11691,-0.13114,-0.11691], "fy":[-0.11691,0.13114,0.11691,-0.13114]},
+    {"t":2.89085, "x":2.31777, "y":5.87086, "heading":0.05419, "vx":-0.02328, "vy":0.1986, "omega":-0.06949, "ax":0.0, "ay":0.0, "alpha":-0.054, "fx":[0.15257,0.13689,-0.15257,-0.13689], "fy":[-0.13689,0.15257,0.13689,-0.15257]},
+    {"t":2.93824, "x":2.31667, "y":5.88027, "heading":0.0509, "vx":-0.02328, "vy":0.1986, "omega":-0.07205, "ax":0.0, "ay":0.0, "alpha":-0.063, "fx":[0.17749,0.1603,-0.17749,-0.1603], "fy":[-0.1603,0.17749,0.1603,-0.17749]},
+    {"t":2.98563, "x":2.31557, "y":5.88969, "heading":0.04748, "vx":-0.02328, "vy":0.1986, "omega":-0.07504, "ax":0.0, "ay":0.0, "alpha":-0.07351, "fx":[0.20645,0.18774,-0.20645,-0.18774], "fy":[-0.18774,0.20645,0.18774,-0.20645]},
+    {"t":3.03302, "x":2.31446, "y":5.8991, "heading":0.04392, "vx":-0.02328, "vy":0.1986, "omega":-0.07852, "ax":0.0, "ay":0.0, "alpha":-0.08577, "fx":[0.24011,0.2199,-0.24011,-0.2199], "fy":[-0.2199,0.24011,0.2199,-0.24011]},
+    {"t":3.08041, "x":2.31336, "y":5.90851, "heading":0.0402, "vx":-0.02328, "vy":0.1986, "omega":-0.08258, "ax":0.0, "ay":0.0, "alpha":-0.10007, "fx":[0.2792,0.25762,-0.2792,-0.25762], "fy":[-0.25762,0.2792,0.25762,-0.2792]},
+    {"t":3.1278, "x":2.31226, "y":5.91792, "heading":0.03629, "vx":-0.02328, "vy":0.1986, "omega":-0.08733, "ax":0.0, "ay":0.0, "alpha":-0.11676, "fx":[0.32459,0.30186,-0.32459,-0.30186], "fy":[-0.30186,0.32459,0.30186,-0.32459]},
+    {"t":3.17519, "x":2.31116, "y":5.92733, "heading":0.03215, "vx":-0.02328, "vy":0.1986, "omega":-0.09286, "ax":0.0, "ay":0.0, "alpha":-0.13624, "fx":[0.37727,0.35377,-0.37727,-0.35377], "fy":[-0.35377,0.37727,0.35377,-0.37727]},
+    {"t":3.22259, "x":2.31005, "y":5.93674, "heading":0.02775, "vx":-0.02328, "vy":0.1986, "omega":-0.09932, "ax":0.0, "ay":0.0, "alpha":-0.15896, "fx":[0.43838,0.41471,-0.43838,-0.41471], "fy":[-0.41471,0.43838,0.41471,-0.43838]},
+    {"t":3.26998, "x":2.30895, "y":5.94616, "heading":0.02304, "vx":-0.02328, "vy":0.1986, "omega":-0.10685, "ax":0.0, "ay":0.0, "alpha":-0.18548, "fx":[0.50922,0.48629,-0.50922,-0.48629], "fy":[-0.48629,0.50922,0.48629,-0.50922]},
+    {"t":3.31737, "x":2.30785, "y":5.95557, "heading":0.01798, "vx":-0.02328, "vy":0.1986, "omega":-0.11564, "ax":0.0, "ay":0.0, "alpha":-0.21642, "fx":[0.59129,0.57041,-0.59128,-0.57041], "fy":[-0.57041,0.59129,0.57041,-0.59129]},
+    {"t":3.36476, "x":2.30674, "y":5.96498, "heading":0.0125, "vx":-0.02328, "vy":0.1986, "omega":-0.1259, "ax":0.00067, "ay":0.00008, "alpha":-0.25252, "fx":[0.69765,0.68071,-0.67488,-0.65794], "fy":[-0.66799,0.6876,0.67066,-0.68494]},
+    {"t":3.41215, "x":2.30564, "y":5.97439, "heading":0.00653, "vx":-0.02325, "vy":0.1986, "omega":-0.13786, "ax":1.80117, "ay":-0.18127, "alpha":-0.27212, "fx":[31.33798,31.34092,29.93652,29.93406], "fy":[-3.82985,-2.31507,-2.33405,-3.85453]},
+    {"t":3.45954, "x":2.30656, "y":5.9836, "heading":0.0, "vx":0.06211, "vy":0.19001, "omega":-0.15076, "ax":1.68681, "ay":-1.01421, "alpha":-0.22252, "fx":[29.25113,29.3014,28.13253,28.08388], "fy":[-17.83657,-16.61702,-16.66478,-17.88763]},
+    {"t":3.50506, "x":2.31114, "y":5.9912, "heading":-0.00686, "vx":0.13889, "vy":0.14385, "omega":-0.16089, "ax":0.0006, "ay":-0.00058, "alpha":-0.21076, "fx":[0.57205,0.57981,-0.55168,-0.55944], "fy":[-0.57946,0.55203,0.55979,-0.5717]},
+    {"t":3.55057, "x":2.31746, "y":5.99775, "heading":-0.01419, "vx":0.13892, "vy":0.14382, "omega":-0.17048, "ax":0.0, "ay":0.0, "alpha":-0.18172, "fx":[0.48084,0.49467,-0.48083,-0.49467], "fy":[-0.49467,0.48083,0.49467,-0.48084]},
+    {"t":3.59609, "x":2.32378, "y":6.00429, "heading":-0.02195, "vx":0.13892, "vy":0.14382, "omega":-0.17875, "ax":0.0, "ay":0.0, "alpha":-0.15668, "fx":[0.41125,0.42971,-0.41125,-0.42971], "fy":[-0.42971,0.41125,0.42971,-0.41125]},
+    {"t":3.64161, "x":2.3301, "y":6.01084, "heading":-0.03008, "vx":0.13892, "vy":0.14382, "omega":-0.18588, "ax":0.0, "ay":0.0, "alpha":-0.13509, "fx":[0.35156,0.37337,-0.35156,-0.37337], "fy":[-0.37337,0.35156,0.37337,-0.35156]},
+    {"t":3.68713, "x":2.33643, "y":6.01738, "heading":-0.03854, "vx":0.13892, "vy":0.14382, "omega":-0.19203, "ax":0.0, "ay":0.0, "alpha":-0.11648, "fx":[0.30038,0.32447,-0.30038,-0.32447], "fy":[-0.32447,0.30038,0.32447,-0.30038]},
+    {"t":3.73264, "x":2.34275, "y":6.02393, "heading":-0.04728, "vx":0.13892, "vy":0.14382, "omega":-0.19734, "ax":0.0, "ay":0.0, "alpha":-0.10043, "fx":[0.25653,0.28202,-0.25653,-0.28202], "fy":[-0.28202,0.25653,0.28202,-0.25653]},
+    {"t":3.77816, "x":2.34907, "y":6.03048, "heading":-0.05627, "vx":0.13892, "vy":0.14382, "omega":-0.20191, "ax":0.0, "ay":0.0, "alpha":-0.08659, "fx":[0.21899,0.24513,-0.21899,-0.24513], "fy":[-0.24513,0.21899,0.24513,-0.21899]},
+    {"t":3.82368, "x":2.3554, "y":6.03702, "heading":-0.06546, "vx":0.13892, "vy":0.14382, "omega":-0.20585, "ax":0.0, "ay":0.0, "alpha":-0.07465, "fx":[0.18686,0.21307,-0.18686,-0.21307], "fy":[-0.21307,0.18686,0.21307,-0.18686]},
+    {"t":3.86919, "x":2.36172, "y":6.04357, "heading":-0.07482, "vx":0.13892, "vy":0.14382, "omega":-0.20925, "ax":0.0, "ay":0.0, "alpha":-0.06436, "fx":[0.15937,0.18521,-0.15937,-0.18521], "fy":[-0.18521,0.15937,0.18521,-0.15937]},
+    {"t":3.91471, "x":2.36804, "y":6.05012, "heading":-0.08435, "vx":0.13892, "vy":0.14382, "omega":-0.21217, "ax":0.0, "ay":0.0, "alpha":-0.05549, "fx":[0.13588,0.16098,-0.13588,-0.16098], "fy":[-0.16098,0.13588,0.16098,-0.13588]},
+    {"t":3.96023, "x":2.37437, "y":6.05666, "heading":-0.09401, "vx":0.13892, "vy":0.14382, "omega":-0.2147, "ax":0.0, "ay":0.0, "alpha":-0.04784, "fx":[0.1158,0.13991,-0.1158,-0.13991], "fy":[-0.13991,0.1158,0.13991,-0.1158]},
+    {"t":4.00574, "x":2.38069, "y":6.06321, "heading":-0.10378, "vx":0.13892, "vy":0.14382, "omega":-0.21688, "ax":0.0, "ay":0.0, "alpha":-0.04125, "fx":[0.09865,0.12159,-0.09865,-0.12159], "fy":[-0.12159,0.09865,0.12159,-0.09865]},
+    {"t":4.05126, "x":2.38701, "y":6.06975, "heading":-0.11365, "vx":0.13892, "vy":0.14382, "omega":-0.21876, "ax":0.0, "ay":0.0, "alpha":-0.03556, "fx":[0.084,0.10566,-0.084,-0.10566], "fy":[-0.10566,0.084,0.10566,-0.084]},
+    {"t":4.09678, "x":2.39333, "y":6.0763, "heading":-0.12361, "vx":0.13892, "vy":0.14382, "omega":-0.22037, "ax":0.0, "ay":0.0, "alpha":-0.03065, "fx":[0.0715,0.09179,-0.0715,-0.09179], "fy":[-0.09179,0.0715,0.09179,-0.0715]},
+    {"t":4.1423, "x":2.39966, "y":6.08285, "heading":-0.13364, "vx":0.13892, "vy":0.14382, "omega":-0.22177, "ax":0.0, "ay":0.0, "alpha":-0.02642, "fx":[0.06083,0.07974,-0.06083,-0.07974], "fy":[-0.07974,0.06083,0.07974,-0.06083]},
+    {"t":4.18781, "x":2.40598, "y":6.08939, "heading":-0.14373, "vx":0.13892, "vy":0.14382, "omega":-0.22297, "ax":0.0, "ay":0.0, "alpha":-0.02277, "fx":[0.05173,0.06925,-0.05173,-0.06925], "fy":[-0.06925,0.05173,0.06925,-0.05173]},
+    {"t":4.23333, "x":2.4123, "y":6.09594, "heading":-0.15388, "vx":0.13892, "vy":0.14382, "omega":-0.22401, "ax":0.0, "ay":0.0, "alpha":-0.01962, "fx":[0.04397,0.06012,-0.04397,-0.06012], "fy":[-0.06012,0.04397,0.06012,-0.04397]},
+    {"t":4.27885, "x":2.41863, "y":6.10249, "heading":-0.16408, "vx":0.13892, "vy":0.14382, "omega":-0.2249, "ax":0.0, "ay":0.0, "alpha":-0.0169, "fx":[0.03735,0.05218,-0.03735,-0.05218], "fy":[-0.05218,0.03735,0.05218,-0.03735]},
+    {"t":4.32436, "x":2.42495, "y":6.10903, "heading":-0.17431, "vx":0.13892, "vy":0.14382, "omega":-0.22567, "ax":0.0, "ay":0.0, "alpha":-0.01456, "fx":[0.03171,0.04527,-0.03171,-0.04527], "fy":[-0.04527,0.03171,0.04527,-0.03171]},
+    {"t":4.36988, "x":2.43127, "y":6.11558, "heading":-0.18459, "vx":0.13892, "vy":0.14382, "omega":-0.22633, "ax":0.0, "ay":0.0, "alpha":-0.01254, "fx":[0.0269,0.03926,-0.0269,-0.03926], "fy":[-0.03926,0.0269,0.03926,-0.0269]},
+    {"t":4.4154, "x":2.4376, "y":6.12212, "heading":-0.19489, "vx":0.13892, "vy":0.14382, "omega":-0.2269, "ax":0.0, "ay":0.0, "alpha":-0.01079, "fx":[0.0228,0.03402,-0.0228,-0.03402], "fy":[-0.03402,0.0228,0.03402,-0.0228]},
+    {"t":4.46091, "x":2.44392, "y":6.12867, "heading":-0.20522, "vx":0.13892, "vy":0.14382, "omega":-0.2274, "ax":0.0, "ay":0.0, "alpha":-0.00928, "fx":[0.01931,0.02946,-0.01931,-0.02946], "fy":[-0.02946,0.01931,0.02946,-0.01931]},
+    {"t":4.50643, "x":2.45024, "y":6.13522, "heading":-0.21557, "vx":0.13892, "vy":0.14382, "omega":-0.22782, "ax":0.0, "ay":0.0, "alpha":-0.00797, "fx":[0.01633,0.02549,-0.01633,-0.02549], "fy":[-0.02549,0.01633,0.02549,-0.01633]},
+    {"t":4.55195, "x":2.45657, "y":6.14176, "heading":-0.22594, "vx":0.13892, "vy":0.14382, "omega":-0.22818, "ax":0.0, "ay":0.0, "alpha":-0.00684, "fx":[0.01379,0.02202,-0.01379,-0.02202], "fy":[-0.02202,0.01379,0.02202,-0.01379]},
+    {"t":4.59747, "x":2.46289, "y":6.14831, "heading":-0.23632, "vx":0.13892, "vy":0.14382, "omega":-0.22849, "ax":0.0, "ay":0.0, "alpha":-0.00586, "fx":[0.01162,0.01899,-0.01162,-0.01899], "fy":[-0.01899,0.01162,0.01899,-0.01162]},
+    {"t":4.64298, "x":2.46921, "y":6.15486, "heading":-0.24672, "vx":0.13892, "vy":0.14382, "omega":-0.22876, "ax":0.0, "ay":0.0, "alpha":-0.00502, "fx":[0.00976,0.01634,-0.00976,-0.01634], "fy":[-0.01634,0.00976,0.01634,-0.00976]},
+    {"t":4.6885, "x":2.47553, "y":6.1614, "heading":-0.25714, "vx":0.13892, "vy":0.14382, "omega":-0.22899, "ax":0.0, "ay":0.0, "alpha":-0.00428, "fx":[0.00818,0.01402,-0.00818,-0.01402], "fy":[-0.01402,0.00818,0.01402,-0.00818]},
+    {"t":4.73402, "x":2.48186, "y":6.16795, "heading":-0.26756, "vx":0.13892, "vy":0.14382, "omega":-0.22918, "ax":0.0, "ay":0.0, "alpha":-0.00363, "fx":[0.00682,0.01198,-0.00682,-0.01198], "fy":[-0.01198,0.00682,0.01198,-0.00682]},
+    {"t":4.77953, "x":2.48818, "y":6.1745, "heading":-0.27799, "vx":0.13892, "vy":0.14382, "omega":-0.22935, "ax":0.0, "ay":0.0, "alpha":-0.00307, "fx":[0.00565,0.01018,-0.00565,-0.01018], "fy":[-0.01018,0.00565,0.01018,-0.00565]},
+    {"t":4.82505, "x":2.4945, "y":6.18104, "heading":-0.28843, "vx":0.13892, "vy":0.14382, "omega":-0.22949, "ax":0.0, "ay":0.0, "alpha":-0.00257, "fx":[0.00465,0.00857,-0.00465,-0.00857], "fy":[-0.00857,0.00465,0.00857,-0.00465]},
+    {"t":4.87057, "x":2.50083, "y":6.18759, "heading":-0.29887, "vx":0.13892, "vy":0.14382, "omega":-0.2296, "ax":0.0, "ay":0.0, "alpha":-0.00213, "fx":[0.00377,0.00714,-0.00377,-0.00714], "fy":[-0.00714,0.00377,0.00714,-0.00377]},
+    {"t":4.91608, "x":2.50715, "y":6.19413, "heading":-0.30933, "vx":0.13892, "vy":0.14382, "omega":-0.2297, "ax":0.0, "ay":0.0, "alpha":-0.00174, "fx":[0.00302,0.00586,-0.00302,-0.00586], "fy":[-0.00586,0.00302,0.00586,-0.00302]},
+    {"t":4.9616, "x":2.51347, "y":6.20068, "heading":-0.31978, "vx":0.13892, "vy":0.14382, "omega":-0.22978, "ax":0.0, "ay":0.0, "alpha":-0.00138, "fx":[0.00235,0.00469,-0.00235,-0.00469], "fy":[-0.00469,0.00235,0.00469,-0.00235]},
+    {"t":5.00712, "x":2.5198, "y":6.20723, "heading":-0.33024, "vx":0.13892, "vy":0.14382, "omega":-0.22984, "ax":0.0, "ay":0.0, "alpha":-0.00106, "fx":[0.00176,0.00361,-0.00176,-0.00361], "fy":[-0.00361,0.00176,0.00361,-0.00176]},
+    {"t":5.05264, "x":2.52612, "y":6.21377, "heading":-0.3407, "vx":0.13892, "vy":0.14382, "omega":-0.22989, "ax":0.0, "ay":0.0, "alpha":-0.00076, "fx":[0.00123,0.0026,-0.00123,-0.0026], "fy":[-0.0026,0.00123,0.0026,-0.00123]},
+    {"t":5.09815, "x":2.53244, "y":6.22032, "heading":-0.35117, "vx":0.13892, "vy":0.14382, "omega":-0.22992, "ax":0.0, "ay":0.0, "alpha":-0.00047, "fx":[0.00075,0.00164,-0.00075,-0.00164], "fy":[-0.00164,0.00075,0.00164,-0.00075]},
+    {"t":5.14367, "x":2.53877, "y":6.22687, "heading":-0.36163, "vx":0.13892, "vy":0.14382, "omega":-0.22995, "ax":0.0, "ay":0.0, "alpha":-0.0002, "fx":[0.00031,0.0007,-0.00031,-0.0007], "fy":[-0.0007,0.00031,0.0007,-0.00031]},
+    {"t":5.18919, "x":2.54509, "y":6.23341, "heading":-0.3721, "vx":0.13892, "vy":0.14382, "omega":-0.22996, "ax":0.0, "ay":0.0, "alpha":0.00006, "fx":[-0.0001,-0.00022,0.0001,0.00022], "fy":[0.00022,-0.0001,-0.00022,0.0001]},
+    {"t":5.2347, "x":2.55141, "y":6.23996, "heading":-0.38256, "vx":0.13892, "vy":0.14382, "omega":-0.22995, "ax":0.0, "ay":0.0, "alpha":0.00033, "fx":[-0.0005,-0.00115,0.0005,0.00115], "fy":[0.00115,-0.0005,-0.00115,0.0005]},
+    {"t":5.28022, "x":2.55774, "y":6.2465, "heading":-0.39303, "vx":0.13892, "vy":0.14382, "omega":-0.22994, "ax":0.0, "ay":0.0, "alpha":0.00061, "fx":[-0.00088,-0.00212,0.00088,0.00212], "fy":[0.00212,-0.00088,-0.00212,0.00088]},
+    {"t":5.32574, "x":2.56406, "y":6.25305, "heading":-0.4035, "vx":0.13892, "vy":0.14382, "omega":-0.22991, "ax":0.0, "ay":0.0, "alpha":0.00089, "fx":[-0.00127,-0.00315,0.00127,0.00315], "fy":[0.00315,-0.00127,-0.00315,0.00127]},
+    {"t":5.37125, "x":2.57038, "y":6.2596, "heading":-0.41396, "vx":0.13892, "vy":0.14382, "omega":-0.22987, "ax":0.0, "ay":0.0, "alpha":0.0012, "fx":[-0.00167,-0.00426,0.00167,0.00426], "fy":[0.00426,-0.00167,-0.00426,0.00167]},
+    {"t":5.41677, "x":2.5767, "y":6.26614, "heading":-0.42442, "vx":0.13892, "vy":0.14382, "omega":-0.22981, "ax":0.0, "ay":0.0, "alpha":0.00154, "fx":[-0.00207,-0.00547,0.00207,0.00547], "fy":[0.00547,-0.00207,-0.00547,0.00207]},
+    {"t":5.46229, "x":2.58303, "y":6.27269, "heading":-0.43489, "vx":0.13892, "vy":0.14382, "omega":-0.22974, "ax":0.0, "ay":0.0, "alpha":0.00191, "fx":[-0.00249,-0.00681,0.00249,0.00681], "fy":[0.00681,-0.00249,-0.00681,0.00249]},
+    {"t":5.50781, "x":2.58935, "y":6.27924, "heading":-0.44534, "vx":0.13892, "vy":0.14382, "omega":-0.22966, "ax":0.0, "ay":0.0, "alpha":0.00232, "fx":[-0.00295,-0.00831,0.00295,0.00831], "fy":[0.00831,-0.00295,-0.00831,0.00295]},
+    {"t":5.55332, "x":2.59567, "y":6.28578, "heading":-0.4558, "vx":0.13892, "vy":0.14382, "omega":-0.22955, "ax":0.0, "ay":0.0, "alpha":0.00279, "fx":[-0.00343,-0.01001,0.00343,0.01001], "fy":[0.01001,-0.00343,-0.01001,0.00343]},
+    {"t":5.59884, "x":2.602, "y":6.29233, "heading":-0.46624, "vx":0.13892, "vy":0.14382, "omega":-0.22943, "ax":0.0, "ay":0.0, "alpha":0.00331, "fx":[-0.00395,-0.01193,0.00395,0.01193], "fy":[0.01193,-0.00395,-0.01193,0.00395]},
+    {"t":5.64436, "x":2.60832, "y":6.29888, "heading":-0.47669, "vx":0.13892, "vy":0.14382, "omega":-0.22927, "ax":0.0, "ay":0.0, "alpha":0.00391, "fx":[-0.00451,-0.01413,0.00451,0.01413], "fy":[0.01413,-0.00451,-0.01413,0.00451]},
+    {"t":5.68987, "x":2.61464, "y":6.30542, "heading":-0.48712, "vx":0.13892, "vy":0.14382, "omega":-0.2291, "ax":0.0, "ay":0.0, "alpha":0.00459, "fx":[-0.00513,-0.01666,0.00513,0.01666], "fy":[0.01666,-0.00513,-0.01666,0.00513]},
+    {"t":5.73539, "x":2.62097, "y":6.31197, "heading":-0.49755, "vx":0.13892, "vy":0.14382, "omega":-0.22889, "ax":0.0, "ay":0.0, "alpha":0.00537, "fx":[-0.0058,-0.01956,0.0058,0.01956], "fy":[0.01956,-0.0058,-0.01956,0.0058]},
+    {"t":5.78091, "x":2.62729, "y":6.31851, "heading":-0.50797, "vx":0.13892, "vy":0.14382, "omega":-0.22864, "ax":0.0, "ay":0.0, "alpha":0.00628, "fx":[-0.00653,-0.02291,0.00653,0.02291], "fy":[0.02291,-0.00653,-0.02291,0.00653]},
+    {"t":5.82642, "x":2.63361, "y":6.32506, "heading":-0.51838, "vx":0.13892, "vy":0.14382, "omega":-0.22836, "ax":0.0, "ay":0.0, "alpha":0.00732, "fx":[-0.00734,-0.0268,0.00734,0.0268], "fy":[0.0268,-0.00734,-0.0268,0.00734]},
+    {"t":5.87194, "x":2.63994, "y":6.33161, "heading":-0.52877, "vx":0.13892, "vy":0.14382, "omega":-0.22802, "ax":0.0, "ay":0.0, "alpha":0.00852, "fx":[-0.00822,-0.03129,0.00822,0.03129], "fy":[0.03129,-0.00822,-0.03129,0.00822]},
+    {"t":5.91746, "x":2.64626, "y":6.33815, "heading":-0.53915, "vx":0.13892, "vy":0.14382, "omega":-0.22764, "ax":0.0, "ay":0.0, "alpha":0.00992, "fx":[-0.00918,-0.03651,0.00918,0.03651], "fy":[0.03651,-0.00918,-0.03651,0.00918]},
+    {"t":5.96298, "x":2.65258, "y":6.3447, "heading":-0.54951, "vx":0.13892, "vy":0.14382, "omega":-0.22718, "ax":0.0, "ay":0.0, "alpha":0.01153, "fx":[-0.01023,-0.04254,0.01023,0.04254], "fy":[0.04254,-0.01023,-0.04254,0.01023]},
+    {"t":6.00849, "x":2.6589, "y":6.35125, "heading":-0.55985, "vx":0.13892, "vy":0.14382, "omega":-0.22666, "ax":0.0, "ay":0.0, "alpha":0.01339, "fx":[-0.01138,-0.04955,0.01138,0.04955], "fy":[0.04955,-0.01138,-0.04955,0.01138]},
+    {"t":6.05401, "x":2.66523, "y":6.35779, "heading":-0.57017, "vx":0.13892, "vy":0.14382, "omega":-0.22605, "ax":0.0, "ay":0.0, "alpha":0.01555, "fx":[-0.01262,-0.05768,0.01262,0.05768], "fy":[0.05768,-0.01262,-0.05768,0.01262]},
+    {"t":6.09953, "x":2.67155, "y":6.36434, "heading":-0.58046, "vx":0.13892, "vy":0.14382, "omega":-0.22534, "ax":0.0, "ay":0.0, "alpha":0.01805, "fx":[-0.01396,-0.0671,0.01396,0.0671], "fy":[0.0671,-0.01396,-0.0671,0.01396]},
+    {"t":6.14504, "x":2.67787, "y":6.37088, "heading":-0.59071, "vx":0.13892, "vy":0.14382, "omega":-0.22452, "ax":0.0, "ay":0.0, "alpha":0.02095, "fx":[-0.0154,-0.07804,0.0154,0.07804], "fy":[0.07804,-0.0154,-0.07804,0.0154]},
+    {"t":6.19056, "x":2.6842, "y":6.37743, "heading":-0.60093, "vx":0.13892, "vy":0.14382, "omega":-0.22357, "ax":0.0, "ay":0.0, "alpha":0.02431, "fx":[-0.01694,-0.09074,0.01694,0.09074], "fy":[0.09074,-0.01694,-0.09074,0.01694]},
+    {"t":6.23608, "x":2.69052, "y":6.38398, "heading":-0.61111, "vx":0.13892, "vy":0.14382, "omega":-0.22246, "ax":0.0, "ay":0.0, "alpha":0.02821, "fx":[-0.01858,-0.10548,0.01858,0.10548], "fy":[0.10548,-0.01858,-0.10548,0.01858]},
+    {"t":6.28159, "x":2.69684, "y":6.39052, "heading":-0.62124, "vx":0.13892, "vy":0.14382, "omega":-0.22118, "ax":0.0, "ay":0.0, "alpha":0.03273, "fx":[-0.02032,-0.12259,0.02032,0.12259], "fy":[0.12259,-0.02032,-0.12259,0.02032]},
+    {"t":6.32711, "x":2.70317, "y":6.39707, "heading":-0.6313, "vx":0.13892, "vy":0.14382, "omega":-0.21969, "ax":0.0, "ay":0.0, "alpha":0.03797, "fx":[-0.02214,-0.14245,0.02214,0.14245], "fy":[0.14245,-0.02214,-0.14245,0.02214]},
+    {"t":6.37263, "x":2.70949, "y":6.40362, "heading":-0.6413, "vx":0.13892, "vy":0.14382, "omega":-0.21796, "ax":0.0, "ay":0.0, "alpha":0.04405, "fx":[-0.02403,-0.1655,0.02403,0.1655], "fy":[0.1655,-0.02403,-0.1655,0.02403]},
+    {"t":6.41815, "x":2.71581, "y":6.41016, "heading":-0.65122, "vx":0.13892, "vy":0.14382, "omega":-0.21595, "ax":0.0, "ay":0.0, "alpha":0.0511, "fx":[-0.02596,-0.19224,0.02596,0.19224], "fy":[0.19224,-0.02596,-0.19224,0.02596]},
+    {"t":6.46366, "x":2.72214, "y":6.41671, "heading":-0.66105, "vx":0.13892, "vy":0.14382, "omega":-0.21363, "ax":0.0, "ay":0.0, "alpha":0.05927, "fx":[-0.02792,-0.22327,0.02792,0.22327], "fy":[0.22327,-0.02792,-0.22327,0.02792]},
+    {"t":6.50918, "x":2.72846, "y":6.42326, "heading":-0.67078, "vx":0.13892, "vy":0.14382, "omega":-0.21093, "ax":0.0, "ay":0.0, "alpha":0.06875, "fx":[-0.02987,-0.25927,0.02987,0.25927], "fy":[0.25927,-0.02987,-0.25927,0.02987]},
+    {"t":6.5547, "x":2.73478, "y":6.4298, "heading":-0.68038, "vx":0.13892, "vy":0.14382, "omega":-0.2078, "ax":0.0, "ay":0.0, "alpha":0.07973, "fx":[-0.03175,-0.30102,0.03175,0.30102], "fy":[0.30102,-0.03175,-0.30102,0.03175]},
+    {"t":6.60021, "x":2.74111, "y":6.43635, "heading":-0.68984, "vx":0.13892, "vy":0.14382, "omega":-0.20417, "ax":0.0, "ay":0.0, "alpha":0.09247, "fx":[-0.03352,-0.34944,0.03352,0.34944], "fy":[0.34944,-0.03352,-0.34944,0.03352]},
+    {"t":6.64573, "x":2.74743, "y":6.44289, "heading":-0.69913, "vx":0.13892, "vy":0.14382, "omega":-0.19996, "ax":0.0, "ay":0.0, "alpha":0.10725, "fx":[-0.03511,-0.40562,0.03511,0.40562], "fy":[0.40562,-0.03511,-0.40562,0.03511]},
+    {"t":6.69125, "x":2.75375, "y":6.44944, "heading":-0.70823, "vx":0.13892, "vy":0.14382, "omega":-0.19508, "ax":0.0, "ay":0.0, "alpha":0.12438, "fx":[-0.03643,-0.47077,0.03643,0.47077], "fy":[0.47077,-0.03643,-0.47077,0.03643]},
+    {"t":6.73676, "x":2.76007, "y":6.45599, "heading":-0.71711, "vx":0.13892, "vy":0.14382, "omega":-0.18942, "ax":0.0, "ay":0.0, "alpha":0.14425, "fx":[-0.0374,-0.54634,0.0374,0.54634], "fy":[0.54634,-0.0374,-0.54634,0.0374]},
+    {"t":6.78228, "x":2.7664, "y":6.46253, "heading":-0.72573, "vx":0.13892, "vy":0.14382, "omega":-0.18285, "ax":0.0, "ay":0.0, "alpha":0.1673, "fx":[-0.03792,-0.63398,0.03792,0.63398], "fy":[0.63398,-0.03792,-0.63398,0.03792]},
+    {"t":6.8278, "x":2.77272, "y":6.46908, "heading":-0.73405, "vx":0.13892, "vy":0.14382, "omega":-0.17524, "ax":0.0, "ay":0.0, "alpha":0.19403, "fx":[-0.03785,-0.73559,0.03785,0.73559], "fy":[0.73559,-0.03785,-0.73559,0.03785]},
+    {"t":6.87332, "x":2.77904, "y":6.47563, "heading":-0.74203, "vx":0.13892, "vy":0.14382, "omega":-0.16641, "ax":0.0, "ay":0.0, "alpha":0.22502, "fx":[-0.03709,-0.85342,0.03709,0.85342], "fy":[0.85342,-0.03709,-0.85342,0.03709]},
+    {"t":6.91883, "x":2.78537, "y":6.48217, "heading":-0.74961, "vx":0.13892, "vy":0.14382, "omega":-0.15616, "ax":0.0, "ay":0.0, "alpha":0.26096, "fx":[-0.03551,-0.99002,0.03551,0.99002], "fy":[0.99002,-0.03551,-0.99002,0.03551]},
+    {"t":6.96435, "x":2.79169, "y":6.48872, "heading":-0.75671, "vx":0.13892, "vy":0.14382, "omega":-0.14429, "ax":0.0, "ay":0.0, "alpha":0.30264, "fx":[-0.03302,-1.1484,0.03302,1.1484], "fy":[1.1484,-0.03302,-1.1484,0.03302]},
+    {"t":7.00987, "x":2.79801, "y":6.49526, "heading":-0.76328, "vx":0.13892, "vy":0.14382, "omega":-0.13051, "ax":0.0, "ay":0.0, "alpha":0.35097, "fx":[-0.02953,-1.33203,0.02953,1.33203], "fy":[1.33203,-0.02953,-1.33203,0.02953]},
+    {"t":7.05538, "x":2.80434, "y":6.50181, "heading":-0.76922, "vx":0.13892, "vy":0.14382, "omega":-0.11453, "ax":0.0, "ay":0.0, "alpha":0.40701, "fx":[-0.02506,-1.54491,0.02506,1.54491], "fy":[1.54491,-0.02506,-1.54491,0.02506]},
+    {"t":7.1009, "x":2.81066, "y":6.50836, "heading":-0.77443, "vx":0.13892, "vy":0.14382, "omega":-0.09601, "ax":0.0, "ay":0.0, "alpha":0.472, "fx":[-0.01971,-1.79171,0.01971,1.79171], "fy":[1.79171,-0.01971,-1.79171,0.01971]},
+    {"t":7.14642, "x":2.81698, "y":6.5149, "heading":-0.7788, "vx":0.13892, "vy":0.14382, "omega":-0.07452, "ax":0.0, "ay":0.0, "alpha":0.54736, "fx":[-0.01376,-2.07784,0.01376,2.07784], "fy":[2.07784,-0.01376,-2.07784,0.01376]},
+    {"t":7.19193, "x":2.82331, "y":6.52145, "heading":-0.7822, "vx":0.13892, "vy":0.14382, "omega":-0.04961, "ax":0.0, "ay":0.0, "alpha":0.63473, "fx":[-0.00779,-2.40954,0.00776,2.40954], "fy":[2.40954,-0.00776,-2.40954,0.00774]},
+    {"t":7.23745, "x":2.82963, "y":6.528, "heading":-0.78445, "vx":0.13892, "vy":0.14382, "omega":-0.02072, "ax":-3.05199, "ay":-3.15972, "alpha":0.45521, "fx":[-52.33257,-53.64695,-51.51105,-50.1632], "fy":[-52.02346,-53.34676,-55.45202,-54.16171]},
+    {"t":7.28297, "x":2.83279, "y":6.53127, "heading":-0.7854, "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/ExamplePathTwo.traj b/src/main/deploy/choreo/ExamplePathTwo.traj
new file mode 100644
index 00000000..475863ca
--- /dev/null
+++ b/src/main/deploy/choreo/ExamplePathTwo.traj
@@ -0,0 +1,215 @@
+{
+ "name":"ExamplePathTwo",
+ "version":3,
+ "snapshot":{
+  "waypoints":[
+    {"x":2.8327901363372803, "y":6.531269073486328, "heading":-0.7853977386450895, "intervals":71, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":2.3845205307006836, "y":6.042070388793945, "heading":-0.9342889335095604, "intervals":82, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":1.8777804374694824, "y":5.51584005355835, "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":16.541, "h":8.0692}}, "enabled":false},
+    {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":0.2}}, "enabled":true}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"2.8327901363372803 m", "val":2.8327901363372803}, "y":{"exp":"6.531269073486328 m", "val":6.531269073486328}, "heading":{"exp":"-0.7853977386450895 rad", "val":-0.7853977386450895}, "intervals":71, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"2.3845205307006836 m", "val":2.3845205307006836}, "y":{"exp":"6.042070388793945 m", "val":6.042070388793945}, "heading":{"exp":"-0.9342889335095603 rad", "val":-0.9342889335095604}, "intervals":82, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"1.8777804374694824 m", "val":1.8777804374694824}, "y":{"exp":"5.51584005355835 m", "val":5.51584005355835}, "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":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false},
+    {"from":0, "to":2, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"0.2 m / s", "val":0.2}}}, "enabled":true}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "config":{
+   "frontLeft":{
+    "x":0.2794,
+    "y":0.2794
+   },
+   "backLeft":{
+    "x":-0.2794,
+    "y":0.2794
+   },
+   "mass":68.0388555,
+   "inertia":6.0,
+   "gearing":6.5,
+   "radius":0.0508,
+   "vmax":628.3185307179587,
+   "tmax":1.2,
+   "cof":1.5,
+   "bumper":{
+    "front":0.4064,
+    "side":0.4064,
+    "back":0.4064
+   },
+   "differentialTrackWidth":0.5588
+  },
+  "sampleType":"Swerve",
+  "waypoints":[0.0,3.34185,7.01783],
+  "samples":[
+    {"t":0.0, "x":2.83279, "y":6.53127, "heading":-0.7854, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-2.86981, "ay":-3.13238, "alpha":-0.13968, "fx":[-48.6989,-48.27344,-48.93189,-49.35434], "fy":[-53.80017,-53.39809,-52.76011,-53.16511]},
+    {"t":0.04707, "x":2.82961, "y":6.5278, "heading":-0.7854, "vx":-0.13508, "vy":-0.14744, "omega":-0.00657, "ax":0.0, "ay":0.0, "alpha":-0.18695, "fx":[-0.00002,0.70971,0.00001,-0.70972], "fy":[-0.70972,0.00001,0.70971,-0.00002]},
+    {"t":0.09414, "x":2.82325, "y":6.52086, "heading":-0.78571, "vx":-0.13508, "vy":-0.14744, "omega":-0.01537, "ax":0.0, "ay":0.0, "alpha":-0.16042, "fx":[-0.00019,0.60897,0.00019,-0.60897], "fy":[-0.60897,-0.00019,0.60897,0.00019]},
+    {"t":0.1412, "x":2.8169, "y":6.51392, "heading":-0.78643, "vx":-0.13508, "vy":-0.14744, "omega":-0.02292, "ax":0.0, "ay":0.0, "alpha":-0.13765, "fx":[-0.00054,0.52253,0.00054,-0.52253], "fy":[-0.52253,-0.00054,0.52253,0.00054]},
+    {"t":0.18827, "x":2.81054, "y":6.50698, "heading":-0.78751, "vx":-0.13508, "vy":-0.14744, "omega":-0.0294, "ax":0.0, "ay":0.0, "alpha":-0.11811, "fx":[-0.00095,0.44836,0.00095,-0.44836], "fy":[-0.44836,-0.00095,0.44836,0.00095]},
+    {"t":0.23534, "x":2.80418, "y":6.50004, "heading":-0.78889, "vx":-0.13508, "vy":-0.14744, "omega":-0.03496, "ax":0.0, "ay":0.0, "alpha":-0.10134, "fx":[-0.00135,0.38471,0.00135,-0.38471], "fy":[-0.38471,-0.00135,0.38471,0.00135]},
+    {"t":0.28241, "x":2.79782, "y":6.4931, "heading":-0.79054, "vx":-0.13508, "vy":-0.14744, "omega":-0.03973, "ax":0.0, "ay":0.0, "alpha":-0.08695, "fx":[-0.0017,0.33009,0.0017,-0.33009], "fy":[-0.33009,-0.0017,0.33009,0.0017]},
+    {"t":0.32948, "x":2.79146, "y":6.48616, "heading":-0.79241, "vx":-0.13508, "vy":-0.14744, "omega":-0.04383, "ax":0.0, "ay":0.0, "alpha":-0.07461, "fx":[-0.00199,0.28321,0.00199,-0.28321], "fy":[-0.28321,-0.00199,0.28321,0.00199]},
+    {"t":0.37655, "x":2.78511, "y":6.47922, "heading":-0.79447, "vx":-0.13508, "vy":-0.14744, "omega":-0.04734, "ax":0.0, "ay":0.0, "alpha":-0.06401, "fx":[-0.00221,0.24299,0.00221,-0.24299], "fy":[-0.24299,-0.00221,0.24299,0.00221]},
+    {"t":0.42361, "x":2.77875, "y":6.47228, "heading":-0.7967, "vx":-0.13508, "vy":-0.14744, "omega":-0.05035, "ax":0.0, "ay":0.0, "alpha":-0.05492, "fx":[-0.00236,0.20846,0.00236,-0.20846], "fy":[-0.20846,-0.00236,0.20846,0.00236]},
+    {"t":0.47068, "x":2.77239, "y":6.46534, "heading":-0.79907, "vx":-0.13508, "vy":-0.14744, "omega":-0.05293, "ax":0.0, "ay":0.0, "alpha":-0.04711, "fx":[-0.00245,0.17882,0.00245,-0.17882], "fy":[-0.17882,-0.00245,0.17882,0.00245]},
+    {"t":0.51775, "x":2.76603, "y":6.4584, "heading":-0.80156, "vx":-0.13508, "vy":-0.14744, "omega":-0.05515, "ax":0.0, "ay":0.0, "alpha":-0.04041, "fx":[-0.00248,0.15338,0.00248,-0.15338], "fy":[-0.15338,-0.00248,0.15338,0.00248]},
+    {"t":0.56482, "x":2.75967, "y":6.45146, "heading":-0.80416, "vx":-0.13508, "vy":-0.14744, "omega":-0.05705, "ax":0.0, "ay":0.0, "alpha":-0.03465, "fx":[-0.00247,0.13153,0.00247,-0.13153], "fy":[-0.13153,-0.00247,0.13153,0.00247]},
+    {"t":0.61189, "x":2.75332, "y":6.44452, "heading":-0.80684, "vx":-0.13508, "vy":-0.14744, "omega":-0.05868, "ax":0.0, "ay":0.0, "alpha":-0.02971, "fx":[-0.00242,0.11277,0.00242,-0.11277], "fy":[-0.11277,-0.00242,0.11277,0.00242]},
+    {"t":0.65896, "x":2.74696, "y":6.43759, "heading":-0.80961, "vx":-0.13508, "vy":-0.14744, "omega":-0.06008, "ax":0.0, "ay":0.0, "alpha":-0.02547, "fx":[-0.00234,0.09666,0.00234,-0.09666], "fy":[-0.09666,-0.00234,0.09666,0.00234]},
+    {"t":0.70602, "x":2.7406, "y":6.43065, "heading":-0.81243, "vx":-0.13508, "vy":-0.14744, "omega":-0.06128, "ax":0.0, "ay":0.0, "alpha":-0.02182, "fx":[-0.00224,0.08281,0.00224,-0.08281], "fy":[-0.08281,-0.00224,0.08281,0.00224]},
+    {"t":0.75309, "x":2.73424, "y":6.42371, "heading":-0.81532, "vx":-0.13508, "vy":-0.14744, "omega":-0.06231, "ax":0.0, "ay":0.0, "alpha":-0.01869, "fx":[-0.00212,0.0709,0.00212,-0.0709], "fy":[-0.0709,-0.00212,0.0709,0.00212]},
+    {"t":0.80016, "x":2.72789, "y":6.41677, "heading":-0.81825, "vx":-0.13508, "vy":-0.14744, "omega":-0.06319, "ax":0.0, "ay":0.0, "alpha":-0.01599, "fx":[-0.00199,0.06066,0.00199,-0.06066], "fy":[-0.06066,-0.00199,0.06066,0.00199]},
+    {"t":0.84723, "x":2.72153, "y":6.40983, "heading":-0.82122, "vx":-0.13508, "vy":-0.14744, "omega":-0.06394, "ax":0.0, "ay":0.0, "alpha":-0.01366, "fx":[-0.00186,0.05184,0.00186,-0.05184], "fy":[-0.05184,-0.00186,0.05184,0.00186]},
+    {"t":0.8943, "x":2.71517, "y":6.40289, "heading":-0.82423, "vx":-0.13508, "vy":-0.14744, "omega":-0.06458, "ax":0.0, "ay":0.0, "alpha":-0.01166, "fx":[-0.00172,0.04423,0.00172,-0.04423], "fy":[-0.04423,-0.00172,0.04423,0.00172]},
+    {"t":0.94137, "x":2.70881, "y":6.39595, "heading":-0.82727, "vx":-0.13508, "vy":-0.14744, "omega":-0.06513, "ax":0.0, "ay":0.0, "alpha":-0.00993, "fx":[-0.00158,0.03766,0.00158,-0.03766], "fy":[-0.03766,-0.00158,0.03766,0.00158]},
+    {"t":0.98843, "x":2.70245, "y":6.38901, "heading":-0.83034, "vx":-0.13508, "vy":-0.14744, "omega":-0.0656, "ax":0.0, "ay":0.0, "alpha":-0.00843, "fx":[-0.00144,0.03198,0.00144,-0.03198], "fy":[-0.03198,-0.00144,0.03198,0.00144]},
+    {"t":1.0355, "x":2.6961, "y":6.38207, "heading":-0.83343, "vx":-0.13508, "vy":-0.14744, "omega":-0.066, "ax":0.0, "ay":0.0, "alpha":-0.00713, "fx":[-0.0013,0.02704,0.0013,-0.02704], "fy":[-0.02704,-0.0013,0.02704,0.0013]},
+    {"t":1.08257, "x":2.68974, "y":6.37513, "heading":-0.83653, "vx":-0.13508, "vy":-0.14744, "omega":-0.06633, "ax":0.0, "ay":0.0, "alpha":-0.006, "fx":[-0.00116,0.02274,0.00116,-0.02274], "fy":[-0.02274,-0.00116,0.02274,0.00116]},
+    {"t":1.12964, "x":2.68338, "y":6.36819, "heading":-0.83966, "vx":-0.13508, "vy":-0.14744, "omega":-0.06662, "ax":0.0, "ay":0.0, "alpha":-0.00501, "fx":[-0.00103,0.01898,0.00103,-0.01898], "fy":[-0.01898,-0.00103,0.01898,0.00103]},
+    {"t":1.17671, "x":2.67702, "y":6.36125, "heading":-0.84279, "vx":-0.13508, "vy":-0.14744, "omega":-0.06685, "ax":0.0, "ay":0.0, "alpha":-0.00413, "fx":[-0.0009,0.01566,0.0009,-0.01566], "fy":[-0.01566,-0.0009,0.01566,0.0009]},
+    {"t":1.22378, "x":2.67066, "y":6.35431, "heading":-0.84594, "vx":-0.13508, "vy":-0.14744, "omega":-0.06705, "ax":0.0, "ay":0.0, "alpha":-0.00335, "fx":[-0.00077,0.01271,0.00077,-0.01271], "fy":[-0.01271,-0.00077,0.01271,0.00077]},
+    {"t":1.27084, "x":2.66431, "y":6.34737, "heading":-0.84909, "vx":-0.13508, "vy":-0.14744, "omega":-0.0672, "ax":0.0, "ay":0.0, "alpha":-0.00265, "fx":[-0.00064,0.01005,0.00064,-0.01005], "fy":[-0.01005,-0.00064,0.01005,0.00064]},
+    {"t":1.31791, "x":2.65795, "y":6.34043, "heading":-0.85226, "vx":-0.13508, "vy":-0.14744, "omega":-0.06733, "ax":0.0, "ay":0.0, "alpha":-0.00202, "fx":[-0.00051,0.00764,0.00051,-0.00764], "fy":[-0.00764,-0.00051,0.00764,0.00051]},
+    {"t":1.36498, "x":2.65159, "y":6.33349, "heading":-0.85543, "vx":-0.13508, "vy":-0.14744, "omega":-0.06742, "ax":0.0, "ay":0.0, "alpha":-0.00143, "fx":[-0.00038,0.00541,0.00038,-0.00541], "fy":[-0.00541,-0.00038,0.00541,0.00038]},
+    {"t":1.41205, "x":2.64523, "y":6.32655, "heading":-0.8586, "vx":-0.13508, "vy":-0.14744, "omega":-0.06749, "ax":0.0, "ay":0.0, "alpha":-0.00087, "fx":[-0.00024,0.00331,0.00024,-0.00331], "fy":[-0.00331,-0.00024,0.00331,0.00024]},
+    {"t":1.45912, "x":2.63888, "y":6.31961, "heading":-0.86178, "vx":-0.13508, "vy":-0.14744, "omega":-0.06753, "ax":0.0, "ay":0.0, "alpha":-0.00034, "fx":[-0.0001,0.00129,0.0001,-0.00129], "fy":[-0.00129,-0.0001,0.00129,0.0001]},
+    {"t":1.50619, "x":2.63252, "y":6.31267, "heading":-0.86495, "vx":-0.13508, "vy":-0.14744, "omega":-0.06755, "ax":0.0, "ay":0.0, "alpha":0.00018, "fx":[0.00005,-0.00069,-0.00005,0.00069], "fy":[0.00069,0.00005,-0.00069,-0.00005]},
+    {"t":1.55325, "x":2.62616, "y":6.30573, "heading":-0.86813, "vx":-0.13508, "vy":-0.14744, "omega":-0.06754, "ax":0.0, "ay":0.0, "alpha":0.00071, "fx":[0.00022,-0.00268,-0.00022,0.00268], "fy":[0.00268,0.00022,-0.00268,-0.00022]},
+    {"t":1.60032, "x":2.6198, "y":6.29879, "heading":-0.87131, "vx":-0.13508, "vy":-0.14744, "omega":-0.06751, "ax":0.0, "ay":0.0, "alpha":0.00125, "fx":[0.0004,-0.00473,-0.0004,0.00473], "fy":[0.00473,0.0004,-0.00473,-0.0004]},
+    {"t":1.64739, "x":2.61344, "y":6.29185, "heading":-0.87449, "vx":-0.13508, "vy":-0.14744, "omega":-0.06745, "ax":0.0, "ay":0.0, "alpha":0.00182, "fx":[0.00061,-0.0069,-0.00061,0.0069], "fy":[0.0069,0.00061,-0.0069,-0.00061]},
+    {"t":1.69446, "x":2.60709, "y":6.28491, "heading":-0.87767, "vx":-0.13508, "vy":-0.14744, "omega":-0.06736, "ax":0.0, "ay":0.0, "alpha":0.00244, "fx":[0.00084,-0.00922,-0.00084,0.00922], "fy":[0.00922,0.00084,-0.00922,-0.00084]},
+    {"t":1.74153, "x":2.60073, "y":6.27798, "heading":-0.88084, "vx":-0.13508, "vy":-0.14744, "omega":-0.06725, "ax":0.0, "ay":0.0, "alpha":0.00311, "fx":[0.00111,-0.01174,-0.00111,0.01174], "fy":[0.01174,0.00111,-0.01174,-0.00111]},
+    {"t":1.7886, "x":2.59437, "y":6.27104, "heading":-0.884, "vx":-0.13508, "vy":-0.14744, "omega":-0.0671, "ax":0.0, "ay":0.0, "alpha":0.00385, "fx":[0.00143,-0.01454,-0.00143,0.01454], "fy":[0.01454,0.00143,-0.01454,-0.00143]},
+    {"t":1.83566, "x":2.58801, "y":6.2641, "heading":-0.88716, "vx":-0.13508, "vy":-0.14744, "omega":-0.06692, "ax":0.0, "ay":0.0, "alpha":0.00468, "fx":[0.00179,-0.01767,-0.00179,0.01767], "fy":[0.01767,0.00179,-0.01767,-0.00179]},
+    {"t":1.88273, "x":2.58166, "y":6.25716, "heading":-0.89031, "vx":-0.13508, "vy":-0.14744, "omega":-0.0667, "ax":0.0, "ay":0.0, "alpha":0.00562, "fx":[0.00222,-0.02122,-0.00222,0.02122], "fy":[0.02122,0.00222,-0.02122,-0.00222]},
+    {"t":1.9298, "x":2.5753, "y":6.25022, "heading":-0.89345, "vx":-0.13508, "vy":-0.14744, "omega":-0.06643, "ax":0.0, "ay":0.0, "alpha":0.00669, "fx":[0.00272,-0.02525,-0.00272,0.02525], "fy":[0.02525,0.00272,-0.02525,-0.00272]},
+    {"t":1.97687, "x":2.56894, "y":6.24328, "heading":-0.89658, "vx":-0.13508, "vy":-0.14744, "omega":-0.06612, "ax":0.0, "ay":0.0, "alpha":0.00792, "fx":[0.00331,-0.02987,-0.00331,0.02987], "fy":[0.02987,0.00331,-0.02987,-0.00331]},
+    {"t":2.02394, "x":2.56258, "y":6.23634, "heading":-0.89969, "vx":-0.13508, "vy":-0.14744, "omega":-0.06575, "ax":0.0, "ay":0.0, "alpha":0.00933, "fx":[0.00402,-0.03519,-0.00402,0.03519], "fy":[0.03519,0.00402,-0.03519,-0.00402]},
+    {"t":2.07101, "x":2.55622, "y":6.2294, "heading":-0.90278, "vx":-0.13508, "vy":-0.14744, "omega":-0.06531, "ax":0.0, "ay":0.0, "alpha":0.01096, "fx":[0.00485,-0.04133,-0.00485,0.04133], "fy":[0.04133,0.00485,-0.04133,-0.00485]},
+    {"t":2.11807, "x":2.54987, "y":6.22246, "heading":-0.90586, "vx":-0.13508, "vy":-0.14744, "omega":-0.06479, "ax":0.0, "ay":0.0, "alpha":0.01285, "fx":[0.00583,-0.04843,-0.00583,0.04843], "fy":[0.04843,0.00583,-0.04843,-0.00583]},
+    {"t":2.16514, "x":2.54351, "y":6.21552, "heading":-0.90891, "vx":-0.13508, "vy":-0.14744, "omega":-0.06419, "ax":0.0, "ay":0.0, "alpha":0.01504, "fx":[0.007,-0.05666,-0.007,0.05666], "fy":[0.05666,0.007,-0.05666,-0.007]},
+    {"t":2.21221, "x":2.53715, "y":6.20858, "heading":-0.91193, "vx":-0.13508, "vy":-0.14744, "omega":-0.06348, "ax":0.0, "ay":0.0, "alpha":0.01758, "fx":[0.00839,-0.06621,-0.00839,0.06621], "fy":[0.06621,0.00839,-0.06621,-0.00839]},
+    {"t":2.25928, "x":2.53079, "y":6.20164, "heading":-0.91491, "vx":-0.13508, "vy":-0.14744, "omega":-0.06265, "ax":0.0, "ay":0.0, "alpha":0.02053, "fx":[0.01003,-0.0773,-0.01003,0.0773], "fy":[0.0773,0.01003,-0.0773,-0.01003]},
+    {"t":2.30635, "x":2.52443, "y":6.1947, "heading":-0.91786, "vx":-0.13508, "vy":-0.14744, "omega":-0.06168, "ax":0.0, "ay":0.0, "alpha":0.02397, "fx":[0.01198,-0.0902,-0.01198,0.0902], "fy":[0.0902,0.01198,-0.0902,-0.01198]},
+    {"t":2.35342, "x":2.51808, "y":6.18776, "heading":-0.92077, "vx":-0.13508, "vy":-0.14744, "omega":-0.06056, "ax":0.0, "ay":0.0, "alpha":0.02796, "fx":[0.01429,-0.10519,-0.01429,0.10519], "fy":[0.10519,0.01429,-0.10519,-0.01429]},
+    {"t":2.40048, "x":2.51172, "y":6.18082, "heading":-0.92362, "vx":-0.13508, "vy":-0.14744, "omega":-0.05924, "ax":0.0, "ay":0.0, "alpha":0.03261, "fx":[0.01702,-0.12264,-0.01702,0.12264], "fy":[0.12264,0.01702,-0.12264,-0.01702]},
+    {"t":2.44755, "x":2.50536, "y":6.17388, "heading":-0.92641, "vx":-0.13508, "vy":-0.14744, "omega":-0.05771, "ax":0.0, "ay":0.0, "alpha":0.03803, "fx":[0.02024,-0.14293,-0.02024,0.14293], "fy":[0.14293,0.02024,-0.14293,-0.02024]},
+    {"t":2.49462, "x":2.499, "y":6.16694, "heading":-0.92912, "vx":-0.13508, "vy":-0.14744, "omega":-0.05592, "ax":0.0, "ay":0.0, "alpha":0.04434, "fx":[0.02406,-0.16658,-0.02406,0.16658], "fy":[0.16658,0.02406,-0.16658,-0.02406]},
+    {"t":2.54169, "x":2.49265, "y":6.16, "heading":-0.93175, "vx":-0.13508, "vy":-0.14744, "omega":-0.05383, "ax":0.0, "ay":0.0, "alpha":0.05168, "fx":[0.02856,-0.19411,-0.02856,0.19411], "fy":[0.19411,0.02856,-0.19411,-0.02856]},
+    {"t":2.58876, "x":2.48629, "y":6.15306, "heading":-0.93429, "vx":-0.13508, "vy":-0.14744, "omega":-0.0514, "ax":0.0, "ay":0.0, "alpha":0.06024, "fx":[0.03387,-0.22618,-0.03387,0.22618], "fy":[0.22618,0.03387,-0.22618,-0.03387]},
+    {"t":2.63582, "x":2.47993, "y":6.14612, "heading":-0.93671, "vx":-0.13508, "vy":-0.14744, "omega":-0.04856, "ax":0.0, "ay":0.0, "alpha":0.07022, "fx":[0.04012,-0.26354,-0.04012,0.26354], "fy":[0.26354,0.04012,-0.26354,-0.04012]},
+    {"t":2.68289, "x":2.47357, "y":6.13918, "heading":-0.93899, "vx":-0.13508, "vy":-0.14744, "omega":-0.04526, "ax":0.0, "ay":0.0, "alpha":0.08184, "fx":[0.04747,-0.30704,-0.04747,0.30704], "fy":[0.30704,0.04747,-0.30704,-0.04747]},
+    {"t":2.72996, "x":2.46721, "y":6.13224, "heading":-0.94112, "vx":-0.13508, "vy":-0.14744, "omega":-0.0414, "ax":0.0, "ay":0.0, "alpha":0.09538, "fx":[0.0561,-0.35773,-0.0561,0.35773], "fy":[0.35773,0.0561,-0.35773,-0.0561]},
+    {"t":2.77703, "x":2.46086, "y":6.12531, "heading":-0.94307, "vx":-0.13508, "vy":-0.14744, "omega":-0.03691, "ax":0.0, "ay":0.0, "alpha":0.11116, "fx":[0.0662,-0.41678,-0.0662,0.41678], "fy":[0.41678,0.0662,-0.41678,-0.0662]},
+    {"t":2.8241, "x":2.4545, "y":6.11837, "heading":-0.94481, "vx":-0.13508, "vy":-0.14744, "omega":-0.03168, "ax":0.0, "ay":0.0, "alpha":0.12955, "fx":[0.078,-0.48559,-0.078,0.48559], "fy":[0.48559,0.078,-0.48559,-0.078]},
+    {"t":2.87117, "x":2.44814, "y":6.11143, "heading":-0.9463, "vx":-0.13508, "vy":-0.14744, "omega":-0.02558, "ax":0.0, "ay":0.0, "alpha":0.15098, "fx":[0.09175,-0.56577,-0.09175,0.56577], "fy":[0.56577,0.09175,-0.56577,-0.09175]},
+    {"t":2.91823, "x":2.44178, "y":6.10449, "heading":-0.9475, "vx":-0.13508, "vy":-0.14744, "omega":-0.01848, "ax":0.0, "ay":0.0, "alpha":0.17595, "fx":[0.10773,-0.65922,-0.10773,0.65922], "fy":[0.65922,0.10773,-0.65922,-0.10773]},
+    {"t":2.9653, "x":2.43542, "y":6.09755, "heading":-0.94837, "vx":-0.13508, "vy":-0.14744, "omega":-0.01019, "ax":0.0, "ay":0.0, "alpha":0.20506, "fx":[0.12623,-0.76814,-0.12623,0.76814], "fy":[0.76814,0.12623,-0.76814,-0.12623]},
+    {"t":3.01237, "x":2.42907, "y":6.09061, "heading":-0.94885, "vx":-0.13508, "vy":-0.14744, "omega":-0.00054, "ax":0.0, "ay":0.0, "alpha":0.23897, "fx":[0.14755,-0.89512,-0.14755,0.89512], "fy":[0.89512,0.14755,-0.89512,-0.14755]},
+    {"t":3.05944, "x":2.42271, "y":6.08367, "heading":-0.94888, "vx":-0.13508, "vy":-0.14744, "omega":0.0107, "ax":0.0, "ay":0.0, "alpha":0.2785, "fx":[0.172,-1.04317,-0.172,1.04317], "fy":[1.04317,0.172,-1.04317,-0.172]},
+    {"t":3.10651, "x":2.41635, "y":6.07673, "heading":-0.94837, "vx":-0.13508, "vy":-0.14744, "omega":0.02381, "ax":0.0, "ay":0.0, "alpha":0.32457, "fx":[0.19985,-1.21582,-0.19985,1.21582], "fy":[1.21582,0.19985,-1.21582,-0.19985]},
+    {"t":3.15358, "x":2.40999, "y":6.06979, "heading":-0.94725, "vx":-0.13508, "vy":-0.14744, "omega":0.03909, "ax":0.0, "ay":0.0, "alpha":0.37825, "fx":[0.23133,-1.41717,-0.23134,1.41717], "fy":[1.41717,0.23134,-1.41717,-0.23133]},
+    {"t":3.20064, "x":2.40364, "y":6.06285, "heading":-0.94541, "vx":-0.13508, "vy":-0.14744, "omega":0.05689, "ax":0.0, "ay":0.0, "alpha":0.44082, "fx":[0.26658,-1.65206,-0.26658,1.65207], "fy":[1.65206,0.26658,-1.65207,-0.26658]},
+    {"t":3.24771, "x":2.39728, "y":6.05591, "heading":-0.94274, "vx":-0.13508, "vy":-0.14744, "omega":0.07764, "ax":-0.00001, "ay":0.00001, "alpha":0.51373, "fx":[0.30529,-1.92637,-0.30578,1.92588], "fy":[1.92635,0.30576,-1.9259,-0.30531]},
+    {"t":3.29478, "x":2.39092, "y":6.04897, "heading":-0.93908, "vx":-0.13508, "vy":-0.14744, "omega":0.10182, "ax":-0.03721, "ay":0.03579, "alpha":0.59857, "fx":[-0.28547,-2.87833,-0.98054,1.61286], "fy":[2.85404,0.95705,-1.63681,0.26078]},
+    {"t":3.34185, "x":2.38452, "y":6.04207, "heading":-0.93429, "vx":-0.13683, "vy":-0.14575, "omega":0.13, "ax":-0.04192, "ay":0.03856, "alpha":0.49302, "fx":[-0.43573,-2.56376,-0.9905,1.13812], "fy":[2.50672,0.93376,-1.19498,0.37803]},
+    {"t":3.38668, "x":2.37834, "y":6.03558, "heading":-0.92846, "vx":-0.13871, "vy":-0.14402, "omega":0.1521, "ax":-0.00002, "ay":0.00002, "alpha":0.42617, "fx":[0.23038,-1.60162,-0.231,1.601], "fy":[1.6016,0.23099,-1.60101,-0.23039]},
+    {"t":3.43151, "x":2.37213, "y":6.02912, "heading":-0.92164, "vx":-0.13871, "vy":-0.14402, "omega":0.1712, "ax":0.0, "ay":0.0, "alpha":0.36828, "fx":[0.18993,-1.38509,-0.18992,1.3851], "fy":[1.38509,0.18992,-1.3851,-0.18993]},
+    {"t":3.47634, "x":2.36591, "y":6.02266, "heading":-0.91397, "vx":-0.13871, "vy":-0.14402, "omega":0.18771, "ax":0.0, "ay":0.0, "alpha":0.31825, "fx":[0.15493,-1.19816,-0.15493,1.19816], "fy":[1.19816,0.15493,-1.19816,-0.15493]},
+    {"t":3.52117, "x":2.35969, "y":6.01621, "heading":-0.90555, "vx":-0.13871, "vy":-0.14402, "omega":0.20198, "ax":0.0, "ay":0.0, "alpha":0.27502, "fx":[0.12517,-1.0365,-0.12517,1.0365], "fy":[1.0365,0.12517,-1.0365,-0.12517]},
+    {"t":3.56599, "x":2.35347, "y":6.00975, "heading":-0.8965, "vx":-0.13871, "vy":-0.14402, "omega":0.21431, "ax":0.0, "ay":0.0, "alpha":0.23766, "fx":[0.10005,-0.89666,-0.10005,0.89666], "fy":[0.89666,0.10005,-0.89666,-0.10005]},
+    {"t":3.61082, "x":2.34725, "y":6.00329, "heading":-0.88689, "vx":-0.13871, "vy":-0.14402, "omega":0.22496, "ax":0.0, "ay":0.0, "alpha":0.20539, "fx":[0.07901,-0.77567,-0.07901,0.77567], "fy":[0.77567,0.07901,-0.77567,-0.07901]},
+    {"t":3.65565, "x":2.34104, "y":5.99684, "heading":-0.87681, "vx":-0.13871, "vy":-0.14402, "omega":0.23417, "ax":0.0, "ay":0.0, "alpha":0.17749, "fx":[0.06151,-0.67098,-0.06151,0.67098], "fy":[0.67098,0.06151,-0.67098,-0.06151]},
+    {"t":3.70048, "x":2.33482, "y":5.99038, "heading":-0.86631, "vx":-0.13871, "vy":-0.14402, "omega":0.24213, "ax":0.0, "ay":0.0, "alpha":0.15339, "fx":[0.04706,-0.58039,-0.04706,0.58039], "fy":[0.58039,0.04706,-0.58039,-0.04706]},
+    {"t":3.74531, "x":2.3286, "y":5.98392, "heading":-0.85545, "vx":-0.13871, "vy":-0.14402, "omega":0.249, "ax":0.0, "ay":0.0, "alpha":0.13256, "fx":[0.03522,-0.50199,-0.03522,0.50199], "fy":[0.50199,0.03522,-0.50199,-0.03522]},
+    {"t":3.79014, "x":2.32238, "y":5.97747, "heading":-0.84429, "vx":-0.13871, "vy":-0.14402, "omega":0.25495, "ax":0.0, "ay":0.0, "alpha":0.11456, "fx":[0.02559,-0.43413,-0.02559,0.43413], "fy":[0.43413,0.02559,-0.43413,-0.02559]},
+    {"t":3.83497, "x":2.31616, "y":5.97101, "heading":-0.83286, "vx":-0.13871, "vy":-0.14402, "omega":0.26008, "ax":0.0, "ay":0.0, "alpha":0.099, "fx":[0.01782,-0.3754,-0.01782,0.3754], "fy":[0.3754,0.01782,-0.3754,-0.01782]},
+    {"t":3.8798, "x":2.30994, "y":5.96456, "heading":-0.8212, "vx":-0.13871, "vy":-0.14402, "omega":0.26452, "ax":0.0, "ay":0.0, "alpha":0.08555, "fx":[0.01161,-0.32457,-0.01161,0.32457], "fy":[0.32457,0.01161,-0.32457,-0.01161]},
+    {"t":3.92463, "x":2.30373, "y":5.9581, "heading":-0.80935, "vx":-0.13871, "vy":-0.14402, "omega":0.26835, "ax":0.0, "ay":0.0, "alpha":0.07393, "fx":[0.0067,-0.28057,-0.0067,0.28057], "fy":[0.28057,0.0067,-0.28057,-0.0067]},
+    {"t":3.96946, "x":2.29751, "y":5.95164, "heading":-0.79732, "vx":-0.13871, "vy":-0.14402, "omega":0.27167, "ax":0.0, "ay":0.0, "alpha":0.06388, "fx":[0.00287,-0.2425,-0.00287,0.2425], "fy":[0.2425,0.00287,-0.2425,-0.00287]},
+    {"t":4.01429, "x":2.29129, "y":5.94519, "heading":-0.78514, "vx":-0.13871, "vy":-0.14402, "omega":0.27453, "ax":0.0, "ay":0.0, "alpha":0.0552, "fx":[-0.00007,-0.20955,0.00007,0.20955], "fy":[0.20955,-0.00007,-0.20955,0.00007]},
+    {"t":4.05911, "x":2.28507, "y":5.93873, "heading":-0.77283, "vx":-0.13871, "vy":-0.14402, "omega":0.27701, "ax":0.0, "ay":0.0, "alpha":0.04769, "fx":[-0.00229,-0.18103,0.00229,0.18103], "fy":[0.18103,-0.00229,-0.18103,0.00229]},
+    {"t":4.10394, "x":2.27885, "y":5.93227, "heading":-0.76041, "vx":-0.13871, "vy":-0.14402, "omega":0.27914, "ax":0.0, "ay":0.0, "alpha":0.0412, "fx":[-0.00392,-0.15635,0.00392,0.15635], "fy":[0.15635,-0.00392,-0.15635,0.00392]},
+    {"t":4.14877, "x":2.27264, "y":5.92582, "heading":-0.7479, "vx":-0.13871, "vy":-0.14402, "omega":0.28099, "ax":0.0, "ay":0.0, "alpha":0.03559, "fx":[-0.00508,-0.135,0.00508,0.135], "fy":[0.135,-0.00508,-0.135,0.00508]},
+    {"t":4.1936, "x":2.26642, "y":5.91936, "heading":-0.7353, "vx":-0.13871, "vy":-0.14402, "omega":0.28259, "ax":0.0, "ay":0.0, "alpha":0.03073, "fx":[-0.00586,-0.11651,0.00586,0.11651], "fy":[0.11651,-0.00586,-0.11651,0.00586]},
+    {"t":4.23843, "x":2.2602, "y":5.9129, "heading":-0.72263, "vx":-0.13871, "vy":-0.14402, "omega":0.28396, "ax":0.0, "ay":0.0, "alpha":0.02653, "fx":[-0.00633,-0.10051,0.00633,0.10051], "fy":[0.10051,-0.00633,-0.10051,0.00633]},
+    {"t":4.28326, "x":2.25398, "y":5.90645, "heading":-0.7099, "vx":-0.13871, "vy":-0.14402, "omega":0.28515, "ax":0.0, "ay":0.0, "alpha":0.02289, "fx":[-0.00657,-0.08665,0.00657,0.08665], "fy":[0.08665,-0.00657,-0.08665,0.00657]},
+    {"t":4.32809, "x":2.24776, "y":5.89999, "heading":-0.69712, "vx":-0.13871, "vy":-0.14402, "omega":0.28618, "ax":0.0, "ay":0.0, "alpha":0.01974, "fx":[-0.00662,-0.07465,0.00662,0.07465], "fy":[0.07465,-0.00662,-0.07465,0.00662]},
+    {"t":4.37292, "x":2.24154, "y":5.89354, "heading":-0.68429, "vx":-0.13871, "vy":-0.14402, "omega":0.28707, "ax":0.0, "ay":0.0, "alpha":0.01701, "fx":[-0.00653,-0.06426,0.00653,0.06426], "fy":[0.06426,-0.00653,-0.06426,0.00653]},
+    {"t":4.41775, "x":2.23533, "y":5.88708, "heading":-0.67142, "vx":-0.13871, "vy":-0.14402, "omega":0.28783, "ax":0.0, "ay":0.0, "alpha":0.01465, "fx":[-0.00634,-0.05524,0.00634,0.05524], "fy":[0.05524,-0.00634,-0.05524,0.00634]},
+    {"t":4.46258, "x":2.22911, "y":5.88062, "heading":-0.65852, "vx":-0.13871, "vy":-0.14402, "omega":0.28848, "ax":0.0, "ay":0.0, "alpha":0.01259, "fx":[-0.00606,-0.04742,0.00606,0.04742], "fy":[0.04742,-0.00606,-0.04742,0.00606]},
+    {"t":4.5074, "x":2.22289, "y":5.87417, "heading":-0.64559, "vx":-0.13871, "vy":-0.14402, "omega":0.28905, "ax":0.0, "ay":0.0, "alpha":0.01081, "fx":[-0.00573,-0.04062,0.00573,0.04062], "fy":[0.04062,-0.00573,-0.04062,0.00573]},
+    {"t":4.55223, "x":2.21667, "y":5.86771, "heading":-0.63263, "vx":-0.13871, "vy":-0.14402, "omega":0.28953, "ax":0.0, "ay":0.0, "alpha":0.00925, "fx":[-0.00535,-0.0347,0.00535,0.0347], "fy":[0.0347,-0.00535,-0.0347,0.00535]},
+    {"t":4.59706, "x":2.21045, "y":5.86125, "heading":-0.61965, "vx":-0.13871, "vy":-0.14402, "omega":0.28995, "ax":0.0, "ay":0.0, "alpha":0.00789, "fx":[-0.00495,-0.02954,0.00495,0.02954], "fy":[0.02954,-0.00495,-0.02954,0.00495]},
+    {"t":4.64189, "x":2.20424, "y":5.8548, "heading":-0.60665, "vx":-0.13871, "vy":-0.14402, "omega":0.2903, "ax":0.0, "ay":0.0, "alpha":0.0067, "fx":[-0.00453,-0.02502,0.00453,0.02502], "fy":[0.02502,-0.00453,-0.02502,0.00453]},
+    {"t":4.68672, "x":2.19802, "y":5.84834, "heading":-0.59364, "vx":-0.13871, "vy":-0.14402, "omega":0.2906, "ax":0.0, "ay":0.0, "alpha":0.00565, "fx":[-0.0041,-0.02105,0.0041,0.02105], "fy":[0.02105,-0.0041,-0.02105,0.0041]},
+    {"t":4.73155, "x":2.1918, "y":5.84189, "heading":-0.58061, "vx":-0.13871, "vy":-0.14402, "omega":0.29086, "ax":0.0, "ay":0.0, "alpha":0.00472, "fx":[-0.00365,-0.01754,0.00365,0.01754], "fy":[0.01754,-0.00365,-0.01754,0.00365]},
+    {"t":4.77638, "x":2.18558, "y":5.83543, "heading":-0.56757, "vx":-0.13871, "vy":-0.14402, "omega":0.29107, "ax":0.0, "ay":0.0, "alpha":0.00389, "fx":[-0.0032,-0.01442,0.0032,0.01442], "fy":[0.01442,-0.0032,-0.01442,0.0032]},
+    {"t":4.82121, "x":2.17936, "y":5.82897, "heading":-0.55452, "vx":-0.13871, "vy":-0.14402, "omega":0.29124, "ax":0.0, "ay":0.0, "alpha":0.00315, "fx":[-0.00274,-0.01163,0.00274,0.01163], "fy":[0.01163,-0.00274,-0.01163,0.00274]},
+    {"t":4.86604, "x":2.17314, "y":5.82252, "heading":-0.54147, "vx":-0.13871, "vy":-0.14402, "omega":0.29138, "ax":0.0, "ay":0.0, "alpha":0.00247, "fx":[-0.00227,-0.0091,0.00227,0.0091], "fy":[0.0091,-0.00227,-0.0091,0.00227]},
+    {"t":4.91087, "x":2.16693, "y":5.81606, "heading":-0.5284, "vx":-0.13871, "vy":-0.14402, "omega":0.29149, "ax":0.0, "ay":0.0, "alpha":0.00185, "fx":[-0.00179,-0.00678,0.00179,0.00678], "fy":[0.00678,-0.00179,-0.00678,0.00179]},
+    {"t":4.9557, "x":2.16071, "y":5.8096, "heading":-0.51534, "vx":-0.13871, "vy":-0.14402, "omega":0.29158, "ax":0.0, "ay":0.0, "alpha":0.00126, "fx":[-0.00128,-0.00461,0.00128,0.00461], "fy":[0.00461,-0.00128,-0.00461,0.00128]},
+    {"t":5.00052, "x":2.15449, "y":5.80315, "heading":-0.50227, "vx":-0.13871, "vy":-0.14402, "omega":0.29163, "ax":0.0, "ay":0.0, "alpha":0.0007, "fx":[-0.00075,-0.00255,0.00075,0.00255], "fy":[0.00255,-0.00075,-0.00255,0.00075]},
+    {"t":5.04535, "x":2.14827, "y":5.79669, "heading":-0.48919, "vx":-0.13871, "vy":-0.14402, "omega":0.29166, "ax":0.0, "ay":0.0, "alpha":0.00016, "fx":[-0.00018,-0.00057,0.00018,0.00057], "fy":[0.00057,-0.00018,-0.00057,0.00018]},
+    {"t":5.09018, "x":2.14205, "y":5.79023, "heading":-0.47612, "vx":-0.13871, "vy":-0.14402, "omega":0.29167, "ax":0.0, "ay":0.0, "alpha":-0.00038, "fx":[0.00044,0.00138,-0.00044,-0.00138], "fy":[-0.00138,0.00044,0.00138,-0.00044]},
+    {"t":5.13501, "x":2.13584, "y":5.78378, "heading":-0.46304, "vx":-0.13871, "vy":-0.14402, "omega":0.29165, "ax":0.0, "ay":0.0, "alpha":-0.00093, "fx":[0.00111,0.00335,-0.00111,-0.00335], "fy":[-0.00335,0.00111,0.00335,-0.00111]},
+    {"t":5.17984, "x":2.12962, "y":5.77732, "heading":-0.44997, "vx":-0.13871, "vy":-0.14402, "omega":0.29161, "ax":0.0, "ay":0.0, "alpha":-0.0015, "fx":[0.00186,0.00536,-0.00186,-0.00536], "fy":[-0.00536,0.00186,0.00536,-0.00186]},
+    {"t":5.22467, "x":2.1234, "y":5.77087, "heading":-0.4369, "vx":-0.13871, "vy":-0.14402, "omega":0.29154, "ax":0.0, "ay":0.0, "alpha":-0.00209, "fx":[0.00271,0.00748,-0.00271,-0.00748], "fy":[-0.00748,0.00271,0.00748,-0.00271]},
+    {"t":5.2695, "x":2.11718, "y":5.76441, "heading":-0.42383, "vx":-0.13871, "vy":-0.14402, "omega":0.29145, "ax":0.0, "ay":0.0, "alpha":-0.00274, "fx":[0.00367,0.00972,-0.00367,-0.00972], "fy":[-0.00972,0.00367,0.00972,-0.00367]},
+    {"t":5.31433, "x":2.11096, "y":5.75795, "heading":-0.41076, "vx":-0.13871, "vy":-0.14402, "omega":0.29133, "ax":0.0, "ay":0.0, "alpha":-0.00344, "fx":[0.00478,0.01216,-0.00478,-0.01216], "fy":[-0.01216,0.00478,0.01216,-0.00478]},
+    {"t":5.35916, "x":2.10474, "y":5.7515, "heading":-0.3977, "vx":-0.13871, "vy":-0.14402, "omega":0.29117, "ax":0.0, "ay":0.0, "alpha":-0.00422, "fx":[0.00605,0.01482,-0.00605,-0.01482], "fy":[-0.01482,0.00605,0.01482,-0.00605]},
+    {"t":5.40399, "x":2.09853, "y":5.74504, "heading":-0.38465, "vx":-0.13871, "vy":-0.14402, "omega":0.29098, "ax":0.0, "ay":0.0, "alpha":-0.00508, "fx":[0.00753,0.01777,-0.00753,-0.01777], "fy":[-0.01777,0.00753,0.01777,-0.00753]},
+    {"t":5.44882, "x":2.09231, "y":5.73858, "heading":-0.3716, "vx":-0.13871, "vy":-0.14402, "omega":0.29076, "ax":0.0, "ay":0.0, "alpha":-0.00606, "fx":[0.00924,0.02105,-0.00924,-0.02105], "fy":[-0.02105,0.00924,0.02105,-0.00924]},
+    {"t":5.49364, "x":2.08609, "y":5.73213, "heading":-0.35857, "vx":-0.13871, "vy":-0.14402, "omega":0.29049, "ax":0.0, "ay":0.0, "alpha":-0.00716, "fx":[0.01125,0.02474,-0.01125,-0.02474], "fy":[-0.02474,0.01125,0.02474,-0.01125]},
+    {"t":5.53847, "x":2.07987, "y":5.72567, "heading":-0.34555, "vx":-0.13871, "vy":-0.14402, "omega":0.29016, "ax":0.0, "ay":0.0, "alpha":-0.00842, "fx":[0.01361,0.02892,-0.01361,-0.02892], "fy":[-0.02892,0.01361,0.02892,-0.01361]},
+    {"t":5.5833, "x":2.07365, "y":5.71921, "heading":-0.33254, "vx":-0.13871, "vy":-0.14402, "omega":0.28979, "ax":0.0, "ay":0.0, "alpha":-0.00986, "fx":[0.01637,0.03365,-0.01637,-0.03365], "fy":[-0.03365,0.01637,0.03365,-0.01637]},
+    {"t":5.62813, "x":2.06744, "y":5.71276, "heading":-0.31955, "vx":-0.13871, "vy":-0.14402, "omega":0.28935, "ax":0.0, "ay":0.0, "alpha":-0.0115, "fx":[0.01961,0.03902,-0.01961,-0.03902], "fy":[-0.03902,0.01961,0.03902,-0.01961]},
+    {"t":5.67296, "x":2.06122, "y":5.7063, "heading":-0.30658, "vx":-0.13871, "vy":-0.14402, "omega":0.28883, "ax":0.0, "ay":0.0, "alpha":-0.0134, "fx":[0.02343,0.04513,-0.02343,-0.04513], "fy":[-0.04513,0.02343,0.04513,-0.02343]},
+    {"t":5.71779, "x":2.055, "y":5.69985, "heading":-0.29363, "vx":-0.13871, "vy":-0.14402, "omega":0.28823, "ax":0.0, "ay":0.0, "alpha":-0.01557, "fx":[0.02791,0.05211,-0.02791,-0.05211], "fy":[-0.05211,0.02791,0.05211,-0.02791]},
+    {"t":5.76262, "x":2.04878, "y":5.69339, "heading":-0.28071, "vx":-0.13871, "vy":-0.14402, "omega":0.28753, "ax":0.0, "ay":0.0, "alpha":-0.01808, "fx":[0.03319,0.06008,-0.03319,-0.06008], "fy":[-0.06008,0.03319,0.06008,-0.03319]},
+    {"t":5.80745, "x":2.04256, "y":5.68693, "heading":-0.26782, "vx":-0.13871, "vy":-0.14402, "omega":0.28672, "ax":0.0, "ay":0.0, "alpha":-0.02098, "fx":[0.0394,0.0692,-0.0394,-0.0692], "fy":[-0.0692,0.0394,0.0692,-0.0394]},
+    {"t":5.85228, "x":2.03634, "y":5.68048, "heading":-0.25496, "vx":-0.13871, "vy":-0.14402, "omega":0.28578, "ax":0.0, "ay":0.0, "alpha":-0.02432, "fx":[0.0467,0.07962,-0.0467,-0.07962], "fy":[-0.07962,0.0467,0.07962,-0.0467]},
+    {"t":5.89711, "x":2.03013, "y":5.67402, "heading":-0.24215, "vx":-0.13871, "vy":-0.14402, "omega":0.28469, "ax":0.0, "ay":0.0, "alpha":-0.02817, "fx":[0.05528,0.09155,-0.05528,-0.09155], "fy":[-0.09155,0.05528,0.09155,-0.05528]},
+    {"t":5.94193, "x":2.02391, "y":5.66756, "heading":-0.22939, "vx":-0.13871, "vy":-0.14402, "omega":0.28343, "ax":0.0, "ay":0.0, "alpha":-0.03263, "fx":[0.06538,0.10521,-0.06538,-0.10521], "fy":[-0.10521,0.06538,0.10521,-0.06538]},
+    {"t":5.98676, "x":2.01769, "y":5.66111, "heading":-0.21668, "vx":-0.13871, "vy":-0.14402, "omega":0.28196, "ax":0.0, "ay":0.0, "alpha":-0.03778, "fx":[0.07723,0.12084,-0.07723,-0.12084], "fy":[-0.12084,0.07723,0.12084,-0.07723]},
+    {"t":6.03159, "x":2.01147, "y":5.65465, "heading":-0.20404, "vx":-0.13871, "vy":-0.14402, "omega":0.28027, "ax":0.0, "ay":0.0, "alpha":-0.04373, "fx":[0.09117,0.13875,-0.09117,-0.13875], "fy":[-0.13875,0.09117,0.13875,-0.09117]},
+    {"t":6.07642, "x":2.00525, "y":5.64819, "heading":-0.19148, "vx":-0.13871, "vy":-0.14402, "omega":0.27831, "ax":0.0, "ay":0.0, "alpha":-0.05062, "fx":[0.10753,0.15926,-0.10753,-0.15926], "fy":[-0.15926,0.10753,0.15926,-0.10753]},
+    {"t":6.12125, "x":1.99904, "y":5.64174, "heading":-0.179, "vx":-0.13871, "vy":-0.14402, "omega":0.27604, "ax":0.0, "ay":0.0, "alpha":-0.05858, "fx":[0.12674,0.18275,-0.12674,-0.18275], "fy":[-0.18275,0.12674,0.18275,-0.12674]},
+    {"t":6.16608, "x":1.99282, "y":5.63528, "heading":-0.16663, "vx":-0.13871, "vy":-0.14402, "omega":0.27341, "ax":0.0, "ay":0.0, "alpha":-0.06779, "fx":[0.14927,0.20965,-0.14927,-0.20965], "fy":[-0.20965,0.14927,0.20965,-0.14927]},
+    {"t":6.21091, "x":1.9866, "y":5.62883, "heading":-0.15437, "vx":-0.13871, "vy":-0.14402, "omega":0.27038, "ax":0.0, "ay":0.0, "alpha":-0.07845, "fx":[0.17568,0.24046,-0.17568,-0.24046], "fy":[-0.24046,0.17568,0.24046,-0.17568]},
+    {"t":6.25574, "x":1.98038, "y":5.62237, "heading":-0.14225, "vx":-0.13871, "vy":-0.14402, "omega":0.26686, "ax":0.0, "ay":0.0, "alpha":-0.09077, "fx":[0.20662,0.27574,-0.20662,-0.27574], "fy":[-0.27574,0.20662,0.27574,-0.20662]},
+    {"t":6.30057, "x":1.97416, "y":5.61591, "heading":-0.13029, "vx":-0.13871, "vy":-0.14402, "omega":0.26279, "ax":0.0, "ay":0.0, "alpha":-0.10502, "fx":[0.24286,0.31615,-0.24286,-0.31615], "fy":[-0.31615,0.24286,0.31615,-0.24286]},
+    {"t":6.3454, "x":1.96794, "y":5.60946, "heading":-0.11851, "vx":-0.13871, "vy":-0.14402, "omega":0.25808, "ax":0.0, "ay":0.0, "alpha":-0.1215, "fx":[0.28527,0.36244,-0.28527,-0.36244], "fy":[-0.36244,0.28527,0.36244,-0.28527]},
+    {"t":6.39023, "x":1.96173, "y":5.603, "heading":-0.10694, "vx":-0.13871, "vy":-0.14402, "omega":0.25264, "ax":0.0, "ay":0.0, "alpha":-0.14057, "fx":[0.33488,0.41549,-0.33488,-0.41549], "fy":[-0.41549,0.33488,0.41549,-0.33488]},
+    {"t":6.43505, "x":1.95551, "y":5.59654, "heading":-0.09561, "vx":-0.13871, "vy":-0.14402, "omega":0.24633, "ax":0.0, "ay":0.0, "alpha":-0.16263, "fx":[0.39285,0.47628,-0.39285,-0.47628], "fy":[-0.47628,0.39285,0.47628,-0.39285]},
+    {"t":6.47988, "x":1.94929, "y":5.59009, "heading":-0.08457, "vx":-0.13871, "vy":-0.14402, "omega":0.23904, "ax":0.0, "ay":0.0, "alpha":-0.18816, "fx":[0.46056,0.54598,-0.46056,-0.54598], "fy":[-0.54598,0.46056,0.54598,-0.46056]},
+    {"t":6.52471, "x":1.94307, "y":5.58363, "heading":-0.07385, "vx":-0.13871, "vy":-0.14402, "omega":0.23061, "ax":0.0, "ay":0.0, "alpha":-0.21769, "fx":[0.53958,0.62593,-0.53958,-0.62593], "fy":[-0.62593,0.53958,0.62593,-0.53958]},
+    {"t":6.56954, "x":1.93685, "y":5.57718, "heading":-0.06352, "vx":-0.13871, "vy":-0.14402, "omega":0.22085, "ax":0.0, "ay":0.0, "alpha":-0.25186, "fx":[0.63172,0.71767,-0.63172,-0.71767], "fy":[-0.71767,0.63172,0.71767,-0.63172]},
+    {"t":6.61437, "x":1.93064, "y":5.57072, "heading":-0.05361, "vx":-0.13871, "vy":-0.14402, "omega":0.20956, "ax":0.0, "ay":0.0, "alpha":-0.29138, "fx":[0.73905,0.82303,-0.73905,-0.82303], "fy":[-0.82303,0.73905,0.82303,-0.73905]},
+    {"t":6.6592, "x":1.92442, "y":5.56426, "heading":-0.04422, "vx":-0.13871, "vy":-0.14402, "omega":0.1965, "ax":0.0, "ay":0.0, "alpha":-0.33711, "fx":[0.86395,0.94412,-0.86395,-0.94412], "fy":[-0.94412,0.86395,0.94412,-0.86395]},
+    {"t":6.70403, "x":1.9182, "y":5.55781, "heading":-0.03541, "vx":-0.13871, "vy":-0.14402, "omega":0.18138, "ax":0.0, "ay":0.0, "alpha":-0.39002, "fx":[1.00913,1.08343,-1.00913,-1.08343], "fy":[-1.08343,1.00913,1.08343,-1.00913]},
+    {"t":6.74886, "x":1.91198, "y":5.55135, "heading":-0.02728, "vx":-0.13871, "vy":-0.14402, "omega":0.1639, "ax":0.0, "ay":0.0, "alpha":-0.45123, "fx":[1.17765,1.24391,-1.17765,-1.24391], "fy":[-1.24391,1.17765,1.24391,-1.17765]},
+    {"t":6.79369, "x":1.90576, "y":5.54489, "heading":-0.01993, "vx":-0.13871, "vy":-0.14402, "omega":0.14367, "ax":0.0, "ay":0.0, "alpha":-0.52203, "fx":[1.373,1.42905,-1.373,-1.42905], "fy":[-1.42905,1.373,1.42905,-1.373]},
+    {"t":6.83852, "x":1.89954, "y":5.53844, "heading":-0.01349, "vx":-0.13871, "vy":-0.14402, "omega":0.12027, "ax":0.0, "ay":0.0, "alpha":-0.60394, "fx":[1.59906,1.64299,-1.59906,-1.64299], "fy":[-1.64299,1.59906,1.64299,-1.59906]},
+    {"t":6.88335, "x":1.89333, "y":5.53198, "heading":-0.0081, "vx":-0.13871, "vy":-0.14402, "omega":0.0932, "ax":0.0, "ay":0.0, "alpha":-0.69869, "fx":[1.86017,1.89072,-1.86017,-1.89072], "fy":[-1.89072,1.86017,1.89072,-1.86017]},
+    {"t":6.92817, "x":1.88711, "y":5.52552, "heading":-0.00392, "vx":-0.13871, "vy":-0.14402, "omega":0.06187, "ax":0.0, "ay":0.0, "alpha":-0.80831, "fx":[2.1611,2.17843,-2.1611,-2.1784], "fy":[-2.17826,2.16127,2.17827,-2.16125]},
+    {"t":6.973, "x":1.88089, "y":5.51907, "heading":-0.00115, "vx":-0.13871, "vy":-0.14402, "omega":0.02564, "ax":3.09417, "ay":3.21268, "alpha":-0.5719, "fx":[54.55646,53.80479,50.71441,51.44826], "fy":[52.75458,55.78891,56.54588,53.49742]},
+    {"t":7.01783, "x":1.87778, "y":5.51584, "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/deploy/choreo/path.chor b/src/main/deploy/choreo/path.chor
new file mode 100644
index 00000000..fdb671e9
--- /dev/null
+++ b/src/main/deploy/choreo/path.chor
@@ -0,0 +1,84 @@
+{
+ "name":"path",
+ "version":2,
+ "type":"Swerve",
+ "variables":{
+  "expressions":{},
+  "poses":{}
+ },
+ "config":{
+  "frontLeft":{
+   "x":{
+    "exp":"11 in",
+    "val":0.2794
+   },
+   "y":{
+    "exp":"11 in",
+    "val":0.2794
+   }
+  },
+  "backLeft":{
+   "x":{
+    "exp":"-11 in",
+    "val":-0.2794
+   },
+   "y":{
+    "exp":"11 in",
+    "val":0.2794
+   }
+  },
+  "mass":{
+   "exp":"150 lbs",
+   "val":68.0388555
+  },
+  "inertia":{
+   "exp":"6 kg m ^ 2",
+   "val":6.0
+  },
+  "gearing":{
+   "exp":"6.5",
+   "val":6.5
+  },
+  "radius":{
+   "exp":"2 in",
+   "val":0.0508
+  },
+  "vmax":{
+   "exp":"6000 RPM",
+   "val":628.3185307179587
+  },
+  "tmax":{
+   "exp":"1.2 N * m",
+   "val":1.2
+  },
+  "cof":{
+   "exp":"1.5",
+   "val":1.5
+  },
+  "bumper":{
+   "front":{
+    "exp":"16 in",
+    "val":0.4064
+   },
+   "side":{
+    "exp":"16 in",
+    "val":0.4064
+   },
+   "back":{
+    "exp":"16 in",
+    "val":0.4064
+   }
+  },
+  "differentialTrackWidth":{
+   "exp":"22 in",
+   "val":0.5588
+  }
+ },
+ "generationFeatures":[],
+ "codegen":{
+  "root":"Users/michael/Developer/FRC2026_Java/src/main/deploy/choreo",
+  "genVars":true,
+  "genTrajData":true,
+  "useChoreoLib":true
+ }
+}
diff --git a/src/main/java/frc/robot/Drive.java b/src/main/java/frc/robot/Drive.java
new file mode 100644
index 00000000..dbfef819
--- /dev/null
+++ b/src/main/java/frc/robot/Drive.java
@@ -0,0 +1,34 @@
+package frc.robot;
+
+import choreo.trajectory.SwerveSample;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.subsystems.swervedrive.SwerveSubsystem;
+
+
+public class Drive extends SubsystemBase{
+    private final PIDController xController = new PIDController(.01, 0.0, 0.0);
+    private final PIDController yController = new PIDController(.01, 0.0, 0.0);
+    private final PIDController headingController = new PIDController(.01, 0.0, 0.0);
+    private SwerveSubsystem subsystem;
+
+    public Drive(SwerveSubsystem subsystem) {
+        headingController.enableContinuousInput(-Math.PI, Math.PI);
+        this.subsystem = subsystem;
+    }
+
+    public void followTrajectory(SwerveSample sample) {
+        Pose2d pose = subsystem.getPose();
+
+        ChassisSpeeds speeds = new ChassisSpeeds(
+            sample.vx + xController.calculate(pose.getX(), sample.x),
+            sample.vy + yController.calculate(pose.getX(), sample.y),
+            sample.omega + headingController.calculate(pose.getRotation().getRadians(), sample.heading)
+            );
+
+            subsystem.drive(speeds);
+
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 7b9ac010..42d3bfc5 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -28,6 +28,7 @@
 import frc.robot.autochooser.AutoChooser;
 import frc.robot.commands.angler.AimAngler;
 import frc.robot.commands.angler.RunAnglerToReverseLimit;
+import frc.robot.commands.auto.ExampleAuto;
 import frc.robot.commands.shooter.SetShootingState;
 import frc.robot.commands.turret.RunTurretToFwdLimit;
 import frc.robot.commands.turret.RunTurretToRevLimit;
@@ -64,6 +65,14 @@
 
 import java.io.File;
 
+import choreo.auto.AutoFactory;
+import choreo.auto.AutoRoutine;
+import choreo.auto.AutoTrajectory;
+
+import choreo.auto.AutoFactory;
+import choreo.auto.AutoRoutine;
+import choreo.auto.AutoTrajectory;
+
 /**
  * This class is where the bulk of the robot should be declared. Since
  * Command-based is a
@@ -78,7 +87,6 @@ public class RobotContainer {
         private final AutoChooser autoChooser = new AutoChooser();
         // The robot's subsystems and commands are defined here...
         // private final TiltSubsystem tiltSubsystem;
-        private final ClimberSubsystem climberSubsystem;
         private final AnglerSubsystem anglerSubsystem;
         private final IntakeSubsystem intakeSubsystem;
         private final FeederSubsystem feederSubsystem;
@@ -86,6 +94,7 @@ public class RobotContainer {
         private final ShooterSubsystem shooterSubsystem;
         private RobotVisualizer robotVisualizer = null;
         private final HopperSubsystem hopperSubsystem;
+        private final ClimberSubsystem climberSubsystem;
     private final TurretSubsystem turretSubsystem;
         private final IntakeDeployerSubsystem intakeDeployer;
         private SwerveSubsystem drivebase = null;
@@ -93,6 +102,10 @@ public class RobotContainer {
         private final CommandJoystick driveJoystick = new CommandJoystick(Constants.DRIVE_JOYSTICK_PORT);
         private final CommandJoystick steerJoystick = new CommandJoystick(Constants.STEER_JOYSTICK_PORT);
         private ShootingState shootState = new ShootingState(ShootState.STOPPED);
+        private Drive drive;
+        private AutoFactory autoFactory;
+        private static AutoRoutine straightRoutine;
+        private static AutoTrajectory straightTrajectory;
 
         // Replace with CommandPS4Controller or CommandJoystick if needed
         // new CommandXboxController(OperatorConstants.kDriverControllerPort);private
@@ -162,7 +175,8 @@ public RobotContainer() {
                                 turretSubsystem = new TurretSubsystem(TurretSubsystem.createSimIo(robotVisualizer));
                 apriltagSubsystem = new ApriltagSubsystem(ApriltagSubsystem.createSimIo());
                                 shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createSimIo(robotVisualizer));
-                                intakeDeployer = new IntakeDeployerSubsystem(IntakeDeployerSubsystem.createSimIo(robotVisualizer));
+                                intakeDeployer = new IntakeDeployerSubsystem(
+                                                IntakeDeployerSubsystem.createSimIo(robotVisualizer));
 
                                 // No GyroSubsystem in REPLAY for now
                                 // create the drive subsystem with null gyro (use default json)
@@ -177,6 +191,7 @@ public RobotContainer() {
 
                 configureBindings();
                 putShuffleboardCommands();
+                setUpAutoFactory();
         }
 
         /**
@@ -193,6 +208,68 @@ public RobotContainer() {
          * {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight
          * joysticks}.
          */
+        private void setUpAutoFactory() {
+
+                drive = new Drive(drivebase);
+
+                // Sets up Choreo with pose, odometry, drivebase, and a follow trajectory
+                // command
+                autoFactory = new AutoFactory(drivebase::getPose,
+                                drivebase::resetOdometry,
+                                drive::followTrajectory,
+                                true,
+                                drivebase);
+
+                // example implementation of autoRoutine
+                if (false) {
+                        // Uses autofactory to create a new routine
+                        straightRoutine = autoFactory.newRoutine("StraightRoutine");
+
+                        /*
+                         * Loads a trajectory created in Choreo given the name
+                         * Can load multiple trajectories from the same routine
+                         * 
+                         * i.e.
+                         * AutoRoutine routine = autoFactory.newRoutine("grabAndScore");
+                         * AutoTrajectory grabTraj = routine.trajectory("grabPiece");
+                         * AutoTrajectory scoreTraj = routine.trajectory("scorePiece");
+                         */
+                        straightTrajectory = straightRoutine.trajectory("StraightPath");
+
+                        /*
+                         * .active() is a trigger that becomes true when the routine is running
+                         * .onTrue() starts a command when the trigger becomes true (i.e. when the
+                         * routine starts)
+                         * 
+                         * Use commands.sequence() to sequence multiple commands (i.e. reset odometry,
+                         * then follow trajectory)
+                         */
+                        straightRoutine.active().onTrue(
+                                        straightTrajectory.resetOdometry()
+                                                        .andThen(straightTrajectory.cmd()));
+
+                        /*
+                         * -----------------------------------------------------------------------------
+                         * -------------------
+                         * Trajectory Triggers (read more on docs page
+                         * https://choreo.autos/choreolib/auto-factory/):
+                         * -----------------------------------------------------------------------------
+                         * -------------------
+                         * 
+                         * trajectory.atTime(String)
+                         * trajectory.atTime(double time)
+                         * trajectory.done()
+                         * trajectory.active()
+                         * trajectory.inactive()
+                         * trajectory.atPose(String, double, double)
+                         * trajectory.atPose(Pose2d, double, double)
+                         * trajectory.doneDelayed(int)
+                         * trajectory.doneFor(int)
+                         * trajectory.recentlyDone()
+                         */
+                }
+        }
+
         private void configureBindings() {
                 // Schedule `ExampleCommand` when `exampleCondition` changes to `true`
                 // new Trigger(m_exampleSubsystem::exampleCondition)
@@ -401,12 +478,14 @@ public void putShuffleboardCommands() {
          * @return the command to run in autonomous
          */
         public Command getAutonomousCommand() {
-                return autoChooser.getCommand();
+                // return autoChooser.getCommand();
+                // return straightRoutine.cmd(straightTrajectory.done());
+                return new ExampleAuto(drivebase, autoFactory);
         }
 
-  public ClimberSubsystem getClimberSubsystem() {
-    return climberSubsystem;
-  }
+        public ClimberSubsystem getClimberSubsystem() {
+                return climberSubsystem;
+        }
 
         public RobotVisualizer getRobotVisualizer() {
                 return robotVisualizer;
diff --git a/src/main/java/frc/robot/commands/PrintCommand.java b/src/main/java/frc/robot/commands/PrintCommand.java
new file mode 100644
index 00000000..2865b1ee
--- /dev/null
+++ b/src/main/java/frc/robot/commands/PrintCommand.java
@@ -0,0 +1,36 @@
+package frc.robot.commands;
+
+import frc.robot.utils.logging.commands.LoggableCommand;
+
+/**
+ * A command that prints a string when initialized.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
+ */
+public class PrintCommand extends LoggableCommand {
+  private final String message;
+
+  /**
+   * Creates a new a PrintCommand.
+   *
+   * @param message the message to print
+   */
+  public PrintCommand(String message) {
+    this.message = message;
+  }
+
+  @Override
+  public void initialize() {
+    System.out.println(message);
+  }
+
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return true;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/auto/ExampleAuto.java b/src/main/java/frc/robot/commands/auto/ExampleAuto.java
new file mode 100644
index 00000000..bece64d4
--- /dev/null
+++ b/src/main/java/frc/robot/commands/auto/ExampleAuto.java
@@ -0,0 +1,27 @@
+package frc.robot.commands.auto;
+
+import choreo.auto.AutoFactory;
+import frc.robot.commands.PrintCommand;
+import frc.robot.subsystems.swervedrive.SwerveSubsystem;
+import frc.robot.utils.logging.commands.LoggableCommandWrapper;
+import frc.robot.utils.logging.commands.LoggableParallelCommandGroup;
+import frc.robot.utils.logging.commands.LoggableSequentialCommandGroup;
+
+public class ExampleAuto extends LoggableSequentialCommandGroup{
+    public ExampleAuto(SwerveSubsystem subsystem, AutoFactory auto) {
+        super(
+                LoggableCommandWrapper.wrap(auto.resetOdometry("ExamplePathOne")),
+                new LoggableParallelCommandGroup(
+                        LoggableCommandWrapper.wrap((auto.trajectoryCmd("ExamplePathOne"))),
+                        new PrintCommand("Started ExamplePathOne")
+                ),
+                new PrintCommand("Finished ExamplePathOne"),
+
+                new LoggableParallelCommandGroup(
+                        LoggableCommandWrapper.wrap((auto.trajectoryCmd("ExamplePathTwo"))),
+                        new PrintCommand("Started ExamplePathTwo")
+                ),
+                new PrintCommand("Finished ExamplePathTwo")
+        );
+    }
+}
diff --git a/src/main/java/frc/robot/utils/logging/commands/LoggableCommandWrapper.java b/src/main/java/frc/robot/utils/logging/commands/LoggableCommandWrapper.java
index 21224e67..70c7c06c 100644
--- a/src/main/java/frc/robot/utils/logging/commands/LoggableCommandWrapper.java
+++ b/src/main/java/frc/robot/utils/logging/commands/LoggableCommandWrapper.java
@@ -50,4 +50,4 @@ public InterruptionBehavior getInterruptionBehavior() {
   public boolean runsWhenDisabled() {
     return wrap.runsWhenDisabled();
   }
-}
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/logging/commands/LoggableDeadlineCommandGroup.java b/src/main/java/frc/robot/utils/logging/commands/LoggableDeadlineCommandGroup.java
index 82a08460..23c0ec45 100644
--- a/src/main/java/frc/robot/utils/logging/commands/LoggableDeadlineCommandGroup.java
+++ b/src/main/java/frc/robot/utils/logging/commands/LoggableDeadlineCommandGroup.java
@@ -45,4 +45,4 @@ public LoggableDeadlineCommandGroup withBasicName(String name) {
     basicName = name;
     return this;
   }
-}
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/logging/commands/LoggableParallelCommandGroup.java b/src/main/java/frc/robot/utils/logging/commands/LoggableParallelCommandGroup.java
index ad5b2872..e080e167 100644
--- a/src/main/java/frc/robot/utils/logging/commands/LoggableParallelCommandGroup.java
+++ b/src/main/java/frc/robot/utils/logging/commands/LoggableParallelCommandGroup.java
@@ -42,4 +42,4 @@ public LoggableParallelCommandGroup withBasicName(String name) {
     this.basicName = name;
     return this;
   }
-}
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/logging/commands/LoggableRaceCommandGroup.java b/src/main/java/frc/robot/utils/logging/commands/LoggableRaceCommandGroup.java
index cd4f50dd..3af9f4bc 100644
--- a/src/main/java/frc/robot/utils/logging/commands/LoggableRaceCommandGroup.java
+++ b/src/main/java/frc/robot/utils/logging/commands/LoggableRaceCommandGroup.java
@@ -42,4 +42,4 @@ public LoggableRaceCommandGroup withBasicName(String name) {
     this.basicName = name;
     return this;
   }
-}
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/logging/commands/LoggableSequentialCommandGroup.java b/src/main/java/frc/robot/utils/logging/commands/LoggableSequentialCommandGroup.java
index 92363977..1e19fa4d 100644
--- a/src/main/java/frc/robot/utils/logging/commands/LoggableSequentialCommandGroup.java
+++ b/src/main/java/frc/robot/utils/logging/commands/LoggableSequentialCommandGroup.java
@@ -41,4 +41,4 @@ public LoggableSequentialCommandGroup withBasicName(String name) {
     this.basicName = name;
     return this;
   }
-}
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/logging/commands/LoggableWaitCommand.java b/src/main/java/frc/robot/utils/logging/commands/LoggableWaitCommand.java
index 6f43376b..5800da9d 100644
--- a/src/main/java/frc/robot/utils/logging/commands/LoggableWaitCommand.java
+++ b/src/main/java/frc/robot/utils/logging/commands/LoggableWaitCommand.java
@@ -50,4 +50,4 @@ public boolean equals(Object o) {
   public int hashCode() {
     return Objects.hash(basicName, parent);
   }
-}
+}
\ No newline at end of file
diff --git a/vendordeps/ChoreoLib2026.json b/vendordeps/ChoreoLib2026.json
new file mode 100644
index 00000000..322c9e20
--- /dev/null
+++ b/vendordeps/ChoreoLib2026.json
@@ -0,0 +1,44 @@
+{
+    "fileName": "ChoreoLib2026.json",
+    "name": "ChoreoLib",
+    "version": "2026.0.1",
+    "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca",
+    "frcYear": "2026",
+    "mavenUrls": [
+        "https://frcmaven.wpi.edu/artifactory/sleipnirgroup-mvn-release/",
+        "https://repo1.maven.org/maven2"
+    ],
+    "jsonUrl": "https://choreo.autos/lib/ChoreoLib2026.json",
+    "javaDependencies": [
+        {
+            "groupId": "choreo",
+            "artifactId": "ChoreoLib-java",
+            "version": "2026.0.1"
+        },
+        {
+            "groupId": "com.google.code.gson",
+            "artifactId": "gson",
+            "version": "2.11.0"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "choreo",
+            "artifactId": "ChoreoLib-cpp",
+            "version": "2026.0.1",
+            "libName": "ChoreoLib",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal",
+                "linuxathena",
+                "linuxarm32",
+                "linuxarm64"
+            ]
+        }
+    ]
+}
\ No newline at end of file