diff --git a/src/main/deploy/choreo/2025-project.chor b/src/main/deploy/choreo/2025-project.chor
index 1b20df9d..b1f23deb 100644
--- a/src/main/deploy/choreo/2025-project.chor
+++ b/src/main/deploy/choreo/2025-project.chor
@@ -3,9 +3,213 @@
  "version":1,
  "type":"Swerve",
  "variables":{
-  "expressions":{},
+  "expressions":{
+   "endEffectorOffset":{
+    "dimension":"Number",
+    "var":{
+     "exp":"9.408839",
+     "val":9.408839
+    }
+   }
+  },
   "poses":{
-   "startpos":{
+   "A":{
+    "x":{
+     "exp":"3.2654999999999994 m",
+     "val":3.2654999999999994
+    },
+    "y":{
+     "exp":"4.3309 m",
+     "val":4.3309
+    },
+    "heading":{
+     "exp":"0 deg",
+     "val":0.0
+    }
+   },
+   "B":{
+    "x":{
+     "exp":"3.2654999999999994 m",
+     "val":3.2654999999999994
+    },
+    "y":{
+     "exp":"4.0009 m",
+     "val":4.0009
+    },
+    "heading":{
+     "exp":"0 deg",
+     "val":0.0
+    }
+   },
+   "C":{
+    "x":{
+     "exp":"3.6132737518457456 m",
+     "val":3.6132737518457456
+    },
+    "y":{
+     "exp":"3.1185381922643174 m",
+     "val":3.1185381922643174
+    },
+    "heading":{
+     "exp":"60 deg",
+     "val":1.0471975511965976
+    }
+   },
+   "D":{
+    "x":{
+     "exp":"3.89906213509461 m",
+     "val":3.89906213509461
+    },
+    "y":{
+     "exp":"2.953538192264317 m",
+     "val":2.953538192264317
+    },
+    "heading":{
+     "exp":"60 deg",
+     "val":1.0471975511965976
+    }
+   },
+   "E":{
+    "x":{
+     "exp":"4.837096751845746 m",
+     "val":4.837096751845746
+    },
+    "y":{
+     "exp":"2.813538192264317 m",
+     "val":2.813538192264317
+    },
+    "heading":{
+     "exp":"120 deg",
+     "val":2.0943951023931953
+    }
+   },
+   "F":{
+    "x":{
+     "exp":"5.122885135094611 m",
+     "val":5.122885135094611
+    },
+    "y":{
+     "exp":"2.978538192264317 m",
+     "val":2.978538192264317
+    },
+    "heading":{
+     "exp":"120 deg",
+     "val":2.0943951023931953
+    }
+   },
+   "G":{
+    "x":{
+     "exp":"5.713146 m",
+     "val":5.713146
+    },
+    "y":{
+     "exp":"3.7209 m",
+     "val":3.7209
+    },
+    "heading":{
+     "exp":"180 deg",
+     "val":3.141592653589793
+    }
+   },
+   "H":{
+    "x":{
+     "exp":"5.713146 m",
+     "val":5.713146
+    },
+    "y":{
+     "exp":"4.0509 m",
+     "val":4.0509
+    },
+    "heading":{
+     "exp":"180 deg",
+     "val":3.141592653589793
+    }
+   },
+   "I":{
+    "x":{
+     "exp":"5.365372248154253 m",
+     "val":5.365372248154253
+    },
+    "y":{
+     "exp":"4.933261807735684 m",
+     "val":4.933261807735684
+    },
+    "heading":{
+     "exp":"240 deg",
+     "val":4.1887902047863905
+    }
+   },
+   "J":{
+    "x":{
+     "exp":"5.079583864905389 m",
+     "val":5.079583864905389
+    },
+    "y":{
+     "exp":"5.098261807735684 m",
+     "val":5.098261807735684
+    },
+    "heading":{
+     "exp":"240 deg",
+     "val":4.1887902047863905
+    }
+   },
+   "K":{
+    "x":{
+     "exp":"4.141549248154255 m",
+     "val":4.141549248154255
+    },
+    "y":{
+     "exp":"5.238261807735684 m",
+     "val":5.238261807735684
+    },
+    "heading":{
+     "exp":"300 deg",
+     "val":5.235987755982989
+    }
+   },
+   "L":{
+    "x":{
+     "exp":"3.85576086490539 m",
+     "val":3.85576086490539
+    },
+    "y":{
+     "exp":"5.073261807735684 m",
+     "val":5.073261807735684
+    },
+    "heading":{
+     "exp":"300 deg",
+     "val":5.235987755982989
+    }
+   },
+   "barge":{
+    "x":{
+     "exp":"305.5 in",
+     "val":7.7597
+    },
+    "y":{
+     "exp":"4.3597 m",
+     "val":4.3597
+    },
+    "heading":{
+     "exp":"180 deg",
+     "val":3.141592653589793
+    }
+   },
+   "fetch":{
+    "x":{
+     "exp":"1.58 m",
+     "val":1.58
+    },
+    "y":{
+     "exp":"7.3 m",
+     "val":7.3
+    },
+    "heading":{
+     "exp":"-52.8554 deg",
+     "val":-0.9225007574586108
+    }
+   },
+   "start":{
     "x":{
      "exp":"7.1008875 m",
      "val":7.1008875
@@ -18,6 +222,48 @@
      "exp":"180 deg",
      "val":3.141592653589793
     }
+   },
+   "startDeep":{
+    "x":{
+     "exp":"7.1008875 m",
+     "val":7.1008875
+    },
+    "y":{
+     "exp":"7.2570308 m",
+     "val":7.2570308
+    },
+    "heading":{
+     "exp":"180 deg",
+     "val":3.141592653589793
+    }
+   },
+   "startG":{
+    "x":{
+     "exp":"7.1008875 m",
+     "val":7.1008875
+    },
+    "y":{
+     "exp":"142.621161 in",
+     "val":3.6225774894
+    },
+    "heading":{
+     "exp":"180 deg",
+     "val":3.141592653589793
+    }
+   },
+   "startH":{
+    "x":{
+     "exp":"7.1008875 m",
+     "val":7.1008875
+    },
+    "y":{
+     "exp":"155.561161 in",
+     "val":3.9512534894
+    },
+    "heading":{
+     "exp":"180 deg",
+     "val":3.141592653589793
+    }
    }
   }
  },
@@ -43,20 +289,20 @@
    }
   },
   "mass":{
-   "exp":"150 lbs",
-   "val":68.0388555
+   "exp":"147 lbs",
+   "val":66.67807839000001
   },
   "inertia":{
    "exp":"6 kg m ^ 2",
    "val":6.0
   },
   "gearing":{
-   "exp":"6.5",
-   "val":6.5
+   "exp":"7.09",
+   "val":7.09
   },
   "radius":{
-   "exp":"1.75 in",
-   "val":0.04445
+   "exp":"1.68 in",
+   "val":0.042671999999999995
   },
   "vmax":{
    "exp":"6000 RPM",
@@ -72,16 +318,16 @@
   },
   "bumper":{
    "front":{
-    "exp":"17.9375 in",
-    "val":0.4556125
+    "exp":"18.5 in",
+    "val":0.4699
    },
    "side":{
-    "exp":"17.9375 in",
-    "val":0.4556125
+    "exp":"18.5 in",
+    "val":0.4699
    },
    "back":{
-    "exp":"17.9375 in",
-    "val":0.4556125
+    "exp":"18.5 in",
+    "val":0.4699
    }
   },
   "differentialTrackWidth":{
diff --git a/src/main/deploy/choreo/ATofetch.traj b/src/main/deploy/choreo/ATofetch.traj
new file mode 100644
index 00000000..565460f6
--- /dev/null
+++ b/src/main/deploy/choreo/ATofetch.traj
@@ -0,0 +1,69 @@
+{
+ "name":"ATofetch",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":3.2654999999999994, "y":4.3309, "heading":0.0, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false},
+    {"from":1, "to":0, "data":{"type":"KeepOutCircle", "props":{"x":1.2161137368530035, "y":5.864292200654745, "r":0.3248109411104518}}, "enabled":true}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"A.x", "val":3.2654999999999994}, "y":{"exp":"A.y", "val":4.3309}, "heading":{"exp":"A.heading", "val":0.0}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false},
+    {"from":1, "to":0, "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"1.2161137368530035 m", "val":1.2161137368530035}, "y":{"exp":"5.864292200654745 m", "val":5.864292200654745}, "r":{"exp":"0.3248109411104518 m", "val":0.3248109411104518}}}, "enabled":true}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,1.23713],
+  "samples":[
+    {"t":0.0, "x":3.2655, "y":4.3309, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.30896, "ay":9.88198, "alpha":-14.2027, "fx":[-49.79715,-21.5846,-112.85386,-169.75595], "fy":[192.60411,198.01611,164.17484,104.11653]},
+    {"t":0.03866, "x":3.26153, "y":4.33828, "heading":0.0, "vx":-0.20525, "vy":0.38204, "omega":-0.54908, "ax":-5.39586, "ay":9.93662, "alpha":-13.281, "fx":[-55.53562,-25.77834,-112.19147,-166.2803], "fy":[190.97923,197.47805,164.59131,109.50615]},
+    {"t":0.07732, "x":3.24957, "y":4.36048, "heading":-0.02123, "vx":-0.41385, "vy":0.7662, "omega":-1.06253, "ax":-5.5072, "ay":10.01397, "alpha":-11.89483, "fx":[-65.17468,-31.06793,-110.16236,-160.80484], "fy":[187.84378,196.66126,165.90257,117.30477]},
+    {"t":0.11598, "x":3.22945, "y":4.39759, "heading":-0.06231, "vx":-0.62676, "vy":1.15334, "omega":-1.52239, "ax":-5.63373, "ay":10.11742, "alpha":-9.90408, "fx":[-77.20491,-39.6519,-106.8783,-151.91119], "fy":[183.14628,195.02466,167.95128,128.48823]},
+    {"t":0.15464, "x":3.20101, "y":4.44974, "heading":-0.12116, "vx":-0.84457, "vy":1.54449, "omega":-1.90529, "ax":-5.76169, "ay":10.25597, "alpha":-6.74921, "fx":[-89.14902,-56.67983,-102.54631,-135.80334], "fy":[177.53836,190.61584,170.47022,145.22379]},
+    {"t":0.1933, "x":3.16405, "y":4.51711, "heading":-0.19482, "vx":-1.06732, "vy":1.94099, "omega":-2.16622, "ax":-5.85423, "ay":10.37353, "alpha":-0.31926, "fx":[-97.43777,-95.68462,-97.73927,-99.48724], "fy":[173.00259,173.98955,172.85305,171.84194]},
+    {"t":0.23196, "x":3.11841, "y":4.5999, "heading":-0.27857, "vx":-1.29364, "vy":2.34203, "omega":-2.17856, "ax":-5.42083, "ay":9.46516, "alpha":15.71702, "fx":[-97.44713,-174.89501,-99.69209,10.58371], "fy":[172.8305,92.9213,167.61559,197.75099]},
+    {"t":0.27062, "x":3.06435, "y":4.69752, "heading":-0.36279, "vx":-1.50322, "vy":2.70796, "omega":-1.57093, "ax":-5.65468, "ay":8.01707, "alpha":22.30033, "fx":[-89.35516,-182.61572,-152.2676,47.19515], "fy":[176.21693,73.75,93.61352,190.98216]},
+    {"t":0.30928, "x":3.00201, "y":4.8082, "heading":-0.42353, "vx":-1.72183, "vy":3.0179, "omega":-0.70879, "ax":-3.34642, "ay":7.0843, "alpha":18.15978, "fx":[-66.21869,-133.09216,-50.597,26.77517], "fy":[154.31283,86.08343,78.21784,153.75344]},
+    {"t":0.34794, "x":2.93294, "y":4.93017, "heading":-0.45093, "vx":-1.8512, "vy":3.29178, "omega":-0.00672, "ax":0.1361, "ay":0.08415, "alpha":0.01598, "fx":[2.25059,2.21669,2.28673,2.32063], "fy":[1.45468,1.38464,1.35071,1.42075]},
+    {"t":0.3866, "x":2.86147, "y":5.05749, "heading":-0.45119, "vx":-1.84594, "vy":3.29504, "omega":-0.00611, "ax":-0.00182, "ay":-0.00096, "alpha":0.00009, "fx":[-0.03046,-0.03065,-0.03026,-0.03006], "fy":[-0.01575,-0.01615,-0.01634,-0.01594]},
+    {"t":0.42527, "x":2.79011, "y":5.18488, "heading":-0.45142, "vx":-1.84601, "vy":3.295, "omega":-0.0061, "ax":-0.01944, "ay":-0.01084, "alpha":0.00007, "fx":[-0.32418,-0.32433,-0.32402,-0.32388], "fy":[-0.18052,-0.18082,-0.18097,-0.18067]},
+    {"t":0.46393, "x":2.71873, "y":5.31226, "heading":-0.45166, "vx":-1.84676, "vy":3.29458, "omega":-0.0061, "ax":-0.02407, "ay":-0.01345, "alpha":0.00005, "fx":[-0.40128,-0.40138,-0.40116,-0.40106], "fy":[-0.224,-0.22422,-0.22433,-0.22411]},
+    {"t":0.50259, "x":2.64731, "y":5.43962, "heading":-0.4519, "vx":-1.84769, "vy":3.29406, "omega":-0.0061, "ax":-0.02792, "ay":-0.01562, "alpha":0.00002, "fx":[-0.46538,-0.46543,-0.46533,-0.46529], "fy":[-0.26035,-0.26044,-0.26049,-0.26039]},
+    {"t":0.54125, "x":2.57586, "y":5.56696, "heading":-0.45213, "vx":-1.84877, "vy":3.29346, "omega":-0.0061, "ax":-0.03281, "ay":-0.01839, "alpha":-0.00002, "fx":[-0.54691,-0.54687,-0.54695,-0.547], "fy":[-0.30668,-0.30659,-0.30655,-0.30663]},
+    {"t":0.57991, "x":2.50436, "y":5.69427, "heading":-0.45237, "vx":-1.85004, "vy":3.29275, "omega":-0.0061, "ax":-0.04041, "ay":-0.0227, "alpha":-0.00007, "fx":[-0.67354,-0.67338,-0.67371,-0.67386], "fy":[-0.37858,-0.37825,-0.3781,-0.37842]},
+    {"t":0.61857, "x":2.43281, "y":5.82155, "heading":-0.4526, "vx":-1.8516, "vy":3.29187, "omega":-0.0061, "ax":-0.0548, "ay":-0.03083, "alpha":-0.00011, "fx":[-0.91341,-0.91317,-0.91367,-0.91392], "fy":[-0.51436,-0.51386,-0.51361,-0.51412]},
+    {"t":0.65723, "x":2.36118, "y":5.94879, "heading":-0.45284, "vx":-1.85372, "vy":3.29068, "omega":-0.00611, "ax":-0.08393, "ay":-0.04731, "alpha":-0.00012, "fx":[-1.39887,-1.39861,-1.39914,-1.3994], "fy":[-0.78905,-0.78852,-0.78827,-0.78879]},
+    {"t":0.69589, "x":2.28945, "y":6.07598, "heading":-0.45307, "vx":-1.85697, "vy":3.28885, "omega":-0.00611, "ax":-0.13391, "ay":-0.07567, "alpha":0.00009, "fx":[-2.23234,-2.23254,-2.23213,-2.23193], "fy":[-1.26101,-1.26142,-1.26162,-1.26121]},
+    {"t":0.73455, "x":2.21756, "y":6.20307, "heading":-0.45331, "vx":-1.86214, "vy":3.28592, "omega":-0.00611, "ax":-0.16679, "ay":-0.09458, "alpha":0.00047, "fx":[-2.78092,-2.78193,-2.77985,-2.77885], "fy":[-1.57504,-1.57711,-1.57812,-1.57605]},
+    {"t":0.77321, "x":2.14544, "y":6.33003, "heading":-0.45355, "vx":-1.86859, "vy":3.28227, "omega":-0.00609, "ax":-0.14307, "ay":-0.08153, "alpha":0.00035, "fx":[-2.38528,-2.38602,-2.38449,-2.38375], "fy":[-1.35794,-1.35946,-1.3602,-1.35868]},
+    {"t":0.81187, "x":2.0731, "y":6.45687, "heading":-0.45378, "vx":-1.87412, "vy":3.27911, "omega":-0.00608, "ax":-0.13178, "ay":-0.07546, "alpha":0.00012, "fx":[-2.1969,-2.19714,-2.19664,-2.19639], "fy":[-1.25744,-1.25795,-1.2582,-1.25769]},
+    {"t":0.85053, "x":2.00054, "y":6.58358, "heading":-0.45402, "vx":-1.87922, "vy":3.2762, "omega":-0.00607, "ax":-0.44204, "ay":-0.26348, "alpha":-0.01728, "fx":[-7.34912,-7.31237,-7.38799,-7.42473], "fy":[-4.44847,-4.37285,-4.33579,-4.41141]},
+    {"t":0.88919, "x":1.92756, "y":6.71005, "heading":-0.45425, "vx":-1.89631, "vy":3.26601, "omega":-0.00674, "ax":2.69567, "ay":-7.51981, "alpha":-18.40304, "fx":[56.53034,128.80933,34.33934,-39.93669], "fy":[-160.75486,-96.01529,-89.40517,-155.23087]},
+    {"t":0.92785, "x":1.85627, "y":6.83069, "heading":-0.45451, "vx":-1.79209, "vy":2.97529, "omega":-0.71821, "ax":5.70514, "ay":-7.9955, "alpha":-22.9634, "fx":[81.90445,177.44468,169.4511,-48.39244], "fy":[-179.79677,-85.66873,-77.10406,-190.55508]},
+    {"t":0.96651, "x":1.79125, "y":6.93974, "heading":-0.48228, "vx":-1.57153, "vy":2.66618, "omega":-1.60598, "ax":5.767, "ay":-9.23442, "alpha":-16.33864, "fx":[85.17977,166.78278,144.74787,-12.17788], "fy":[-179.19053,-107.02025,-131.99584,-197.52651]},
+    {"t":1.00517, "x":1.7348, "y":7.03592, "heading":-0.54437, "vx":-1.34857, "vy":2.30917, "omega":-2.23764, "ax":5.97116, "ay":-10.30787, "alpha":0.17994, "fx":[99.83586,98.5077,99.24201,100.55985], "fy":[-171.65007,-172.41967,-172.00515,-171.23415]},
+    {"t":1.04383, "x":1.68713, "y":7.11749, "heading":-0.63088, "vx":-1.11773, "vy":1.91067, "omega":-2.23069, "ax":5.90684, "ay":-10.19527, "alpha":6.39024, "fx":[116.11521,59.72799,88.04782,129.96561], "fy":[-161.24442,-189.62146,-178.3856,-150.54927]},
+    {"t":1.08249, "x":1.64833, "y":7.18374, "heading":-0.71711, "vx":-0.88936, "vy":1.51651, "omega":-1.98364, "ax":5.8396, "ay":-10.0207, "alpha":9.81045, "fx":[132.11588,37.8889,79.86673,139.5017], "fy":[-148.56542,-195.28709,-182.34303,-141.96529]},
+    {"t":1.12115, "x":1.61831, "y":7.23488, "heading":-0.7938, "vx":-0.6636, "vy":1.12911, "omega":-1.60436, "ax":5.78055, "ay":-9.85858, "alpha":12.20174, "fx":[145.26601,24.24682,73.06989,142.85302], "fy":[-135.87087,-197.52027,-185.24836,-138.71147]},
+    {"t":1.15981, "x":1.59697, "y":7.27116, "heading":-0.85583, "vx":-0.44012, "vy":0.74797, "omega":-1.13264, "ax":5.72047, "ay":-9.72537, "alpha":13.98608, "fx":[154.85325,15.08528,67.55032,143.94117], "fy":[-124.9526,-198.48087,-187.37875,-137.65698]},
+    {"t":1.19847, "x":1.58423, "y":7.29281, "heading":-0.89962, "vx":-0.21897, "vy":0.37198, "omega":-0.59193, "ax":5.66388, "ay":-9.62184, "alpha":15.31093, "fx":[161.36875,8.53205,63.42083,144.3352], "fy":[-116.51418,-198.90836,-188.84852,-137.29443]},
+    {"t":1.23713, "x":1.58, "y":7.3, "heading":-0.9225, "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/BTofetch.traj b/src/main/deploy/choreo/BTofetch.traj
new file mode 100644
index 00000000..bd94a4c9
--- /dev/null
+++ b/src/main/deploy/choreo/BTofetch.traj
@@ -0,0 +1,71 @@
+{
+ "name":"BTofetch",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":3.2654999999999994, "y":4.0009, "heading":0.0, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false},
+    {"from":1, "to":0, "data":{"type":"KeepOutCircle", "props":{"x":1.2161137368530035, "y":5.864292200654745, "r":0.3248109411104518}}, "enabled":true}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"B.x", "val":3.2654999999999994}, "y":{"exp":"B.y", "val":4.0009}, "heading":{"exp":"A.heading", "val":0.0}, "intervals":34, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false},
+    {"from":1, "to":0, "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"1.2161137368530035 m", "val":1.2161137368530035}, "y":{"exp":"5.864292200654745 m", "val":5.864292200654745}, "r":{"exp":"0.3248109411104518 m", "val":0.3248109411104518}}}, "enabled":true}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,1.31413],
+  "samples":[
+    {"t":0.0, "x":3.2655, "y":4.0009, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.83505, "ay":10.10411, "alpha":-14.45292, "fx":[-34.56375,-15.21964,-107.65803,-164.95009], "fy":[195.92378,198.60886,167.62664,111.56328]},
+    {"t":0.03865, "x":3.26189, "y":4.00845, "heading":0.0, "vx":-0.18688, "vy":0.39053, "omega":-0.55862, "ax":-4.92256, "ay":10.16731, "alpha":-13.46854, "fx":[-40.99843,-19.27311,-106.85522,-161.09988], "fy":[194.63015,198.2211,168.10316,116.98228]},
+    {"t":0.0773, "x":3.25099, "y":4.03114, "heading":-0.02159, "vx":-0.37714, "vy":0.78351, "omega":-1.07919, "ax":-5.0326, "ay":10.2558, "alpha":-11.99808, "fx":[-51.51855,-24.33421,-104.58499,-155.12615], "fy":[192.04903,197.61027,169.47202,124.70587]},
+    {"t":0.11595, "x":3.23265, "y":4.06908, "heading":-0.0633, "vx":-0.57165, "vy":1.17991, "omega":-1.54293, "ax":-5.15731, "ay":10.37043, "alpha":-9.90611, "fx":[-64.84177,-32.59414,-100.91548,-145.52837], "fy":[187.88769,196.3308,171.59803,135.6639]},
+    {"t":0.1546, "x":3.20671, "y":4.12243, "heading":-0.12294, "vx":-0.77099, "vy":1.58073, "omega":-1.92581, "ax":-5.2812, "ay":10.51695, "alpha":-6.65428, "fx":[-78.63141,-49.24899,-95.9173,-128.34266], "fy":[182.44602,192.67269,174.2831,151.84829]},
+    {"t":0.19325, "x":3.17296, "y":4.19138, "heading":-0.19737, "vx":-0.97511, "vy":1.98722, "omega":-2.183, "ax":-5.36431, "ay":10.63536, "alpha":-0.16255, "fx":[-89.29833,-88.43237,-89.54254,-90.40846], "fy":[177.34455,177.78369,177.23158,176.78574]},
+    {"t":0.23191, "x":3.13127, "y":4.27613, "heading":-0.28175, "vx":-1.18245, "vy":2.39829, "omega":-2.18928, "ax":-4.84959, "ay":9.74012, "alpha":15.92924, "fx":[-92.22034,-172.20512,-77.55869,18.6228], "fy":[175.67916,97.76778,178.81929,197.18623]},
+    {"t":0.27056, "x":3.08194, "y":4.37611, "heading":-0.36637, "vx":-1.36989, "vy":2.77475, "omega":-1.5736, "ax":-5.10699, "ay":8.31268, "alpha":22.21564, "fx":[-84.85417,-181.87253,-129.52164,55.72403], "fy":[178.44967,75.50588,111.56567,188.75227]},
+    {"t":0.30921, "x":3.02518, "y":4.48956, "heading":-0.42719, "vx":-1.56728, "vy":3.09605, "omega":-0.71495, "ax":-3.30428, "ay":7.21347, "alpha":18.32308, "fx":[-65.50741,-134.11262,-49.84902,29.14605], "fy":[156.32275,87.87055,81.24969,155.53712]},
+    {"t":0.34786, "x":2.96213, "y":4.61462, "heading":-0.45482, "vx":-1.69499, "vy":3.37485, "omega":-0.00674, "ax":0.0758, "ay":0.04606, "alpha":0.01684, "fx":[1.24478,1.20877,1.28244,1.31845], "fy":[0.82271,0.74904,0.71298,0.78663]},
+    {"t":0.38651, "x":2.89668, "y":4.74509, "heading":-0.45508, "vx":-1.69206, "vy":3.37663, "omega":-0.00609, "ax":-0.00577, "ay":-0.00283, "alpha":0.00012, "fx":[-0.09629,-0.09654,-0.09603,-0.09578], "fy":[-0.04678,-0.04728,-0.04753,-0.04703]},
+    {"t":0.42516, "x":2.83127, "y":4.8756, "heading":-0.45532, "vx":-1.69229, "vy":3.37652, "omega":-0.00609, "ax":-0.01682, "ay":-0.00837, "alpha":0.0001, "fx":[-0.28055,-0.28077,-0.28032,-0.28009], "fy":[-0.13925,-0.13971,-0.13993,-0.13947]},
+    {"t":0.46381, "x":2.76585, "y":5.0061, "heading":-0.45555, "vx":-1.69294, "vy":3.3762, "omega":-0.00609, "ax":-0.02018, "ay":-0.01006, "alpha":0.0001, "fx":[-0.33647,-0.33668,-0.33626,-0.33605], "fy":[-0.16742,-0.16784,-0.16805,-0.16762]},
+    {"t":0.50246, "x":2.7004, "y":5.13658, "heading":-0.45579, "vx":-1.69372, "vy":3.37581, "omega":-0.00608, "ax":-0.02307, "ay":-0.01152, "alpha":0.00008, "fx":[-0.38467,-0.38485,-0.38448,-0.3843], "fy":[-0.19179,-0.19216,-0.19234,-0.19197]},
+    {"t":0.54111, "x":2.63492, "y":5.26705, "heading":-0.45602, "vx":-1.69461, "vy":3.37537, "omega":-0.00608, "ax":-0.02638, "ay":-0.0132, "alpha":0.00006, "fx":[-0.43981,-0.43995,-0.43966,-0.43953], "fy":[-0.21976,-0.22004,-0.22018,-0.2199]},
+    {"t":0.57976, "x":2.5694, "y":5.3975, "heading":-0.45626, "vx":-1.69563, "vy":3.37486, "omega":-0.00608, "ax":-0.03043, "ay":-0.01525, "alpha":0.00003, "fx":[-0.50727,-0.50734,-0.5072,-0.50713], "fy":[-0.25409,-0.25424,-0.25431,-0.25416]},
+    {"t":0.61841, "x":2.50384, "y":5.52793, "heading":-0.45649, "vx":-1.6968, "vy":3.37427, "omega":-0.00607, "ax":-0.0359, "ay":-0.01803, "alpha":-0.00002, "fx":[-0.59848,-0.59845,-0.59852,-0.59855], "fy":[-0.3006,-0.30053,-0.30049,-0.30056]},
+    {"t":0.65707, "x":2.43823, "y":5.65834, "heading":-0.45673, "vx":-1.69819, "vy":3.37357, "omega":-0.00608, "ax":-0.04499, "ay":-0.02264, "alpha":-0.00009, "fx":[-0.74983,-0.74963,-0.75004,-0.75024], "fy":[-0.37777,-0.37736,-0.37716,-0.37757]},
+    {"t":0.69572, "x":2.37256, "y":5.78871, "heading":-0.45696, "vx":-1.69993, "vy":3.37269, "omega":-0.00608, "ax":-0.06606, "ay":-0.03333, "alpha":-0.00019, "fx":[-1.10105,-1.10065,-1.10147,-1.10188], "fy":[-0.55622,-0.55539,-0.55499,-0.55581]},
+    {"t":0.73437, "x":2.30681, "y":5.91905, "heading":-0.4572, "vx":-1.70248, "vy":3.37141, "omega":-0.00609, "ax":-0.12937, "ay":-0.06546, "alpha":-0.0003, "fx":[-2.15623,-2.15559,-2.15688,-2.15752], "fy":[-1.09219,-1.09089,-1.09026,-1.09155]},
+    {"t":0.77302, "x":2.24091, "y":6.04931, "heading":-0.45743, "vx":-1.70748, "vy":3.36888, "omega":-0.0061, "ax":-0.3243, "ay":-0.165, "alpha":0.0001, "fx":[-5.40607,-5.40627,-5.40586,-5.40565], "fy":[-2.75013,-2.75055,-2.75075,-2.75034]},
+    {"t":0.81167, "x":2.17467, "y":6.17939, "heading":-0.45767, "vx":-1.72002, "vy":3.3625, "omega":-0.00609, "ax":-0.43159, "ay":-0.22171, "alpha":0.00131, "fx":[-7.19587,-7.19869,-7.19296,-7.19014], "fy":[-3.69155,-3.69728,-3.70012,-3.69438]},
+    {"t":0.85032, "x":2.10787, "y":6.30919, "heading":-0.4579, "vx":-1.7367, "vy":3.35393, "omega":-0.00604, "ax":-0.27618, "ay":-0.14353, "alpha":0.00036, "fx":[-4.60418,-4.60496,-4.60337,-4.60259], "fy":[-2.3914,-2.39298,-2.39377,-2.39218]},
+    {"t":0.88897, "x":2.04054, "y":6.43872, "heading":-0.45814, "vx":-1.74737, "vy":3.34838, "omega":-0.00603, "ax":-0.15551, "ay":-0.08135, "alpha":0.0001, "fx":[-2.59231,-2.59252,-2.5921,-2.59189], "fy":[-1.35583,-1.35625,-1.35646,-1.35604]},
+    {"t":0.92762, "x":1.97289, "y":6.56807, "heading":-0.45837, "vx":-1.75338, "vy":3.34524, "omega":-0.00603, "ax":-0.43111, "ay":-0.23536, "alpha":-0.01671, "fx":[-7.16783,-7.13199,-7.20494,-7.2408], "fy":[-3.97796,-3.90492,-3.86879,-3.94182]},
+    {"t":0.96627, "x":1.90479, "y":6.69719, "heading":-0.4586, "vx":-1.77005, "vy":3.33614, "omega":-0.00667, "ax":2.44237, "ay":-7.5854, "alpha":-18.26497, "fx":[53.19878,125.51112,26.91932,-42.77676], "fy":[-161.32407,-98.55389,-91.70258,-154.19917]},
+    {"t":1.00492, "x":1.8382, "y":6.82047, "heading":-0.45886, "vx":-1.67565, "vy":3.04296, "omega":-0.71263, "ax":5.39179, "ay":-8.19331, "alpha":-22.79072, "fx":[77.49306,176.14536,161.73383,-55.85825], "fy":[-181.75667,-88.26156,-87.7235,-188.57261]},
+    {"t":1.04357, "x":1.77747, "y":6.93197, "heading":-0.4864, "vx":-1.46725, "vy":2.72628, "omega":-1.59351, "ax":5.37107, "ay":-9.49124, "alpha":-16.08276, "fx":[80.20573,163.86412,133.69464,-19.63209], "fy":[-181.47639,-111.4136,-143.01155,-196.9558]},
+    {"t":1.08223, "x":1.72477, "y":7.03025, "heading":-0.54799, "vx":-1.25965, "vy":2.35943, "omega":-2.21513, "ax":5.55987, "ay":-10.53544, "alpha":0.12451, "fx":[92.86545,91.94652,92.49746,93.41182], "fy":[-175.5202,-176.00616,-175.72197,-175.23481]},
+    {"t":1.12088, "x":1.68023, "y":7.11357, "heading":-0.63361, "vx":-1.04476, "vy":1.95223, "omega":-2.21031, "ax":5.50575, "ay":-10.41939, "alpha":6.34296, "fx":[108.1843,52.2276,82.20715,124.49391], "fy":[-166.66591,-191.82744,-181.15169,-155.09988]},
+    {"t":1.15953, "x":1.64397, "y":7.18125, "heading":-0.71904, "vx":-0.83196, "vy":1.54951, "omega":-1.96515, "ax":5.45047, "ay":-10.24272, "alpha":9.7209, "fx":[124.14623,30.05073,74.37517,134.85473], "fy":[-155.27904,-196.65146,-184.65178,-146.38259]},
+    {"t":1.19818, "x":1.61588, "y":7.23349, "heading":-0.795, "vx":-0.62129, "vy":1.15362, "omega":-1.58943, "ax":5.40501, "ay":-10.07747, "alpha":12.08032, "fx":[137.8136,16.18972,67.76085,138.63147], "fy":[-143.41325,-198.34926,-187.25638,-142.92733]},
+    {"t":1.23683, "x":1.5959, "y":7.27055, "heading":-0.85643, "vx":-0.41238, "vy":0.76412, "omega":-1.12251, "ax":5.35774, "ay":-9.93922, "alpha":13.85719, "fx":[148.06592,6.86279,62.35789,139.957], "fy":[-132.91268,-198.94101,-189.17097,-141.70315]},
+    {"t":1.27548, "x":1.58397, "y":7.29266, "heading":-0.89982, "vx":-0.2053, "vy":0.37996, "omega":-0.58692, "ax":5.31169, "ay":-9.83046, "alpha":15.18518, "fx":[155.14497,0.22017,58.31677,140.49104], "fy":[-124.66813,-199.09708,-190.48734,-141.22339]},
+    {"t":1.31413, "x":1.58, "y":7.3, "heading":-0.9225, "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/ConfusingTestPath.traj b/src/main/deploy/choreo/ConfusingTestPath.traj
new file mode 100644
index 00000000..69951c22
--- /dev/null
+++ b/src/main/deploy/choreo/ConfusingTestPath.traj
@@ -0,0 +1,152 @@
+{
+ "name":"ConfusingTestPath",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":0.44632652401924133, "y":4.026000499725342, "heading":0.0, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":1.6765512228012085, "y":5.0101799964904785, "heading":-0.4124102660645864, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":2.8878490924835205, "y":5.729388236999512, "heading":-0.9827935848181404, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":4.43982458114624, "y":5.994359493255615, "heading":-1.5120409384606408, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":6.029653072357178, "y":5.540122985839844, "heading":-2.2686992417323877, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":6.881347179412842, "y":4.290971755981445, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false},
+    {"from":0, "to":0, "data":{"type":"MaxAcceleration", "props":{"max":5.0}}, "enabled":true},
+    {"from":1, "to":5, "data":{"type":"PointAt", "props":{"x":4.4726667404174805, "y":3.991537094116211, "tolerance":0.017453292519943295, "flip":false}}, "enabled":true}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"0.44632652401924133 m", "val":0.44632652401924133}, "y":{"exp":"4.026000499725342 m", "val":4.026000499725342}, "heading":{"exp":"0 rad", "val":0.0}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"1.6765512228012085 m", "val":1.6765512228012085}, "y":{"exp":"5.0101799964904785 m", "val":5.0101799964904785}, "heading":{"exp":"-0.4124102660645864 rad", "val":-0.4124102660645864}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":{"exp":"2.8878490924835205 m", "val":2.8878490924835205}, "y":{"exp":"5.729388236999512 m", "val":5.729388236999512}, "heading":{"exp":"-0.9827935848181404 rad", "val":-0.9827935848181404}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":{"exp":"4.43982458114624 m", "val":4.43982458114624}, "y":{"exp":"5.994359493255615 m", "val":5.994359493255615}, "heading":{"exp":"-1.5120409384606408 rad", "val":-1.5120409384606408}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":{"exp":"6.029653072357178 m", "val":6.029653072357178}, "y":{"exp":"5.540122985839844 m", "val":5.540122985839844}, "heading":{"exp":"-2.2686992417323877 rad", "val":-2.2686992417323877}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":{"exp":"6.881347179412842 m", "val":6.881347179412842}, "y":{"exp":"4.290971755981445 m", "val":4.290971755981445}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false},
+    {"from":0, "to":0, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"5 m / s ^ 2", "val":5.0}}}, "enabled":true},
+    {"from":1, "to":5, "data":{"type":"PointAt", "props":{"x":{"exp":"4.4726667404174805 m", "val":4.4726667404174805}, "y":{"exp":"3.991537094116211 m", "val":3.991537094116211}, "tolerance":{"exp":"1 deg", "val":0.017453292519943295}, "flip":false}}, "enabled":true}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,0.59344,1.01952,1.50775,2.02236,2.6234],
+  "samples":[
+    {"t":0.0, "x":0.44633, "y":4.026, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.88913, "ay":3.13608, "alpha":-42.99403, "fx":[190.04369,138.4564,-8.60736,-60.57299], "fy":[-13.85691,134.43823,190.08233,-101.55616]},
+    {"t":0.02967, "x":0.44804, "y":4.02738, "heading":0.0, "vx":0.1154, "vy":0.09305, "omega":-1.27572, "ax":9.28618, "ay":7.50996, "alpha":-0.45841, "fx":[156.75556,154.60388,152.81142,155.01402], "fy":[122.74622,125.45176,127.62144,124.93025]},
+    {"t":0.05934, "x":0.45555, "y":4.03345, "heading":-0.03785, "vx":0.39094, "vy":0.31589, "omega":-1.28932, "ax":9.285, "ay":7.50906, "alpha":-0.26904, "fx":[155.93346,154.70436,153.61154,154.85673], "fy":[123.73548,125.27313,126.60588,125.0753]},
+    {"t":0.08902, "x":0.47124, "y":4.04613, "heading":-0.07611, "vx":0.66644, "vy":0.5387, "omega":-1.2973, "ax":9.28323, "ay":7.5077, "alpha":-0.0307, "fx":[154.87963,154.74347,154.61429,154.75064], "fy":[124.98572,125.15484,125.31377,125.14477]},
+    {"t":0.11869, "x":0.4951, "y":4.06542, "heading":-0.1146, "vx":0.94189, "vy":0.76147, "omega":-1.29821, "ax":9.28046, "ay":7.50556, "alpha":0.27829, "fx":[153.49461,154.69008,155.90024,154.71855], "fy":[126.59832,125.12829,123.6237,125.10576]},
+    {"t":0.14836, "x":0.52713, "y":4.09131, "heading":-0.15312, "vx":1.21726, "vy":0.98417, "omega":-1.28996, "ax":9.27589, "ay":7.50198, "alpha":0.69451, "fx":[151.60917,154.49066,157.60299,154.79576], "fy":[128.73909,125.24654,121.32579,124.90645]},
+    {"t":0.17803, "x":0.56733, "y":4.12382, "heading":-0.1914, "vx":1.4925, "vy":1.20677, "omega":-1.26935, "ax":9.26773, "ay":7.49557, "alpha":1.28494, "fx":[148.90988,154.04313,159.96083,155.04083], "fy":[131.69914,125.60873,118.02357,124.45876]},
+    {"t":0.2077, "x":0.6157, "y":4.16293, "heading":-0.22906, "vx":1.76749, "vy":1.42918, "omega":-1.23122, "ax":9.25146, "ay":7.48274, "alpha":2.18655, "fx":[144.74636,153.12234,163.44036,155.56038], "fy":[136.03897,126.42771,112.87092,123.59733]},
+    {"t":0.23738, "x":0.67222, "y":4.20863, "heading":-0.2656, "vx":2.042, "vy":1.65121, "omega":-1.16634, "ax":9.21263, "ay":7.4525, "alpha":3.7288, "fx":[137.52827,151.10547,169.07141,156.57503], "fy":[142.9773,128.26937,103.70635,121.96549]},
+    {"t":0.26705, "x":0.73686, "y":4.2609, "heading":-0.3002, "vx":2.31536, "vy":1.87234, "omega":-1.0557, "ax":9.08315, "ay":7.35928, "alpha":6.94154, "fx":[122.2368,145.37059,179.40567,158.63399], "fy":[155.60473,133.33996,83.14283,118.61481]},
+    {"t":0.29672, "x":0.80956, "y":4.3197, "heading":-0.33153, "vx":2.58487, "vy":2.0907, "omega":-0.84973, "ax":7.96436, "ay":6.98981, "alpha":17.60624, "fx":[75.43166,96.61161,195.52412,163.48074], "fy":[181.41985,163.89613,10.72895,110.02246]},
+    {"t":0.32639, "x":0.88977, "y":4.38481, "heading":-0.35674, "vx":2.82119, "vy":2.2981, "omega":-0.32732, "ax":3.80804, "ay":2.68034, "alpha":11.00204, "fx":[44.65665,30.93334,86.45098,91.87211], "fy":[82.70079,34.36279,5.88361,55.77279]},
+    {"t":0.35606, "x":0.97515, "y":4.45418, "heading":-0.36645, "vx":2.93418, "vy":2.37763, "omega":-0.00087, "ax":0.07708, "ay":-0.09132, "alpha":0.00355, "fx":[1.20377,1.32146,1.38427,1.23036], "fy":[-1.5111,-1.58333,-1.5249,-1.46989]},
+    {"t":0.38574, "x":1.06225, "y":4.52469, "heading":-0.36648, "vx":2.93647, "vy":2.37492, "omega":-0.00076, "ax":0.01796, "ay":-0.0222, "alpha":0.0, "fx":[0.27915,0.31037,0.31992,0.28777], "fy":[-0.37615,-0.38968,-0.36348,-0.35121]},
+    {"t":0.41541, "x":1.14939, "y":4.59515, "heading":-0.3665, "vx":2.937, "vy":2.37427, "omega":-0.00076, "ax":-0.00615, "ay":0.00761, "alpha":0.0, "fx":[-0.10851,-0.09935,-0.0964,-0.1057], "fy":[0.12451,0.12044,0.12914,0.13299]},
+    {"t":0.44508, "x":1.23653, "y":4.6656, "heading":-0.36653, "vx":2.93682, "vy":2.37449, "omega":-0.00076, "ax":-0.01549, "ay":0.01916, "alpha":0.0, "fx":[-0.25678,-0.25891,-0.25971,-0.25758], "fy":[0.31997,0.3208,0.31869,0.31787]},
+    {"t":0.47475, "x":1.32367, "y":4.73606, "heading":-0.36655, "vx":2.93636, "vy":2.37506, "omega":-0.00076, "ax":0.04631, "ay":-0.05729, "alpha":0.0, "fx":[0.7787,0.76943,0.76509,0.77453], "fy":[-0.95127,-0.948,-0.95847,-0.96203]},
+    {"t":0.50442, "x":1.41082, "y":4.80651, "heading":-0.36657, "vx":2.93773, "vy":2.37336, "omega":-0.00076, "ax":0.42511, "ay":-0.52909, "alpha":0.0, "fx":[7.09941,7.08523,7.07164,7.0889], "fy":[-8.80875,-8.80644,-8.82969,-8.83409]},
+    {"t":0.5341, "x":1.49817, "y":4.8767, "heading":-0.36659, "vx":2.95035, "vy":2.35766, "omega":-0.00076, "ax":1.79709, "ay":-2.30612, "alpha":-0.00377, "fx":[29.98309,30.07956,29.89906,29.86466], "fy":[-38.31165,-38.41257,-38.54516,-38.49821]},
+    {"t":0.56377, "x":1.58651, "y":4.94564, "heading":-0.36662, "vx":3.00367, "vy":2.28923, "omega":-0.00087, "ax":2.09246, "ay":-7.69605, "alpha":-6.71571, "fx":[46.10063,66.51467,17.52132,9.38468], "fy":[-139.35094,-120.50752,-115.5918,-137.70734]},
+    {"t":0.59344, "x":1.67655, "y":5.01018, "heading":-0.36664, "vx":3.06576, "vy":2.06088, "omega":-0.20014, "ax":-2.81595, "ay":-8.26016, "alpha":-15.85793, "fx":[-12.88208,28.60845,-106.66565,-96.82272], "fy":[-170.15251,-143.904,-96.54452,-140.1703]},
+    {"t":0.61474, "x":1.74123, "y":5.05221, "heading":-0.37091, "vx":3.00577, "vy":1.8849, "omega":-0.53798, "ax":-3.21784, "ay":-6.12325, "alpha":-13.7128, "fx":[-28.87914,-2.84596,-87.88781,-94.94649], "fy":[-141.06139,-95.34012,-60.0147,-111.87002]},
+    {"t":0.63605, "x":1.80453, "y":5.09098, "heading":-0.38237, "vx":2.93721, "vy":1.75445, "omega":-0.83012, "ax":-2.48369, "ay":-3.30284, "alpha":-8.87435, "fx":[-28.49885,-12.27197,-55.92649,-68.91041], "fy":[-82.89093,-45.19246,-26.88037,-65.26303]},
+    {"t":0.65735, "x":1.86654, "y":5.1276, "heading":-0.40005, "vx":2.8843, "vy":1.68409, "omega":-1.01918, "ax":-1.37907, "ay":-1.11257, "alpha":-4.19162, "fx":[-17.52455,-9.47973,-28.64527,-36.30413], "fy":[-31.92332,-13.40527,-5.13115,-23.72409]},
+    {"t":0.67866, "x":1.92768, "y":5.16323, "heading":-0.42176, "vx":2.85492, "vy":1.66038, "omega":-1.10848, "ax":-0.61844, "ay":0.03244, "alpha":-1.41262, "fx":[-8.60723,-5.73337,-12.02965,-14.86609], "fy":[-4.00881,2.22429,5.09459,-1.14727]},
+    {"t":0.69996, "x":1.98836, "y":5.19861, "heading":-0.44538, "vx":2.84175, "vy":1.66108, "omega":-1.13857, "ax":-0.24479, "ay":0.49407, "alpha":-0.17408, "fx":[-3.89898,-3.50331,-4.26468,-4.65517], "fy":[7.67045,8.41947,8.80273,8.05129]},
+    {"t":0.72126, "x":2.04884, "y":5.23411, "heading":-0.46964, "vx":2.83653, "vy":1.6716, "omega":-1.14228, "ax":-0.14847, "ay":0.60934, "alpha":0.12037, "fx":[-2.61681,-2.86169,-2.334,-2.08735], "fy":[10.5514,10.019,9.76388,10.2952]},
+    {"t":0.74257, "x":2.10924, "y":5.26986, "heading":-0.49397, "vx":2.83337, "vy":1.68458, "omega":-1.13972, "ax":-0.22023, "ay":0.58196, "alpha":-0.08924, "fx":[-3.59418,-3.37018,-3.74873,-3.97134], "fy":[9.40784,9.7834,9.99431,9.6183]},
+    {"t":0.76387, "x":2.16955, "y":5.30588, "heading":-0.51825, "vx":2.82868, "vy":1.69698, "omega":-1.14162, "ax":-0.3764, "ay":0.51965, "alpha":-0.51926, "fx":[-5.81409,-4.54258,-6.73559,-8.0051], "fy":[6.94003,9.13311,10.38417,8.19209]},
+    {"t":0.78518, "x":2.22973, "y":5.34215, "heading":-0.54257, "vx":2.82066, "vy":1.70805, "omega":-1.15268, "ax":-0.55766, "ay":0.47222, "alpha":-0.99678, "fx":[-8.48522,-5.95643,-10.10859,-12.63326], "fy":[4.54222,8.70126,11.19939,7.04413]},
+    {"t":0.80648, "x":2.28969, "y":5.37865, "heading":-0.56713, "vx":2.80878, "vy":1.71811, "omega":-1.17392, "ax":-0.72099, "ay":0.45503, "alpha":-1.41787, "fx":[-10.98077,-7.24335,-13.06007,-16.78971], "fy":[2.8198,8.65167,12.34706,6.522]},
+    {"t":0.82778, "x":2.34937, "y":5.41535, "heading":-0.59214, "vx":2.79342, "vy":1.72781, "omega":-1.20412, "ax":-0.82848, "ay":0.45994, "alpha":-1.71277, "fx":[-12.70194,-8.01199,-14.92432,-19.60323], "fy":[1.88049,8.81252,13.44934,6.52528]},
+    {"t":0.84909, "x":2.40869, "y":5.45227, "heading":-0.61779, "vx":2.77577, "vy":1.7376, "omega":-1.24061, "ax":-0.83537, "ay":0.45377, "alpha":-1.82233, "fx":[-12.90438,-7.72416,-14.95141,-20.12076], "fy":[1.38318,8.62514,13.74271,6.50558]},
+    {"t":0.87039, "x":2.46764, "y":5.48939, "heading":-0.64422, "vx":2.75797, "vy":1.74727, "omega":-1.27944, "ax":-0.67788, "ay":0.36096, "alpha":-1.69099, "fx":[-10.50483,-5.51509,-12.09794,-17.08192], "fy":[0.26858,6.85107,11.76715,5.18136]},
+    {"t":0.8917, "x":2.52624, "y":5.52669, "heading":-0.67148, "vx":2.74353, "vy":1.75496, "omega":-1.31546, "ax":-0.26179, "ay":0.02288, "alpha":-1.28537, "fx":[-3.88769,0.06415,-4.83928,-8.79285], "fy":[-3.98334,0.90766,4.75237,-0.15135]},
+    {"t":0.913, "x":2.58463, "y":5.56409, "heading":-0.6995, "vx":2.73795, "vy":1.75545, "omega":-1.34284, "ax":0.54223, "ay":-0.87281, "alpha":-0.64888, "fx":[9.17997,11.31731,8.90066,6.75667], "fy":[-16.71724,-14.30315,-12.37305,-14.80395]},
+    {"t":0.93431, "x":2.64308, "y":5.60129, "heading":-0.72811, "vx":2.7495, "vy":1.73685, "omega":-1.35667, "ax":1.85347, "ay":-2.8488, "alpha":0.0153, "fx":[30.80664,30.93067,30.9906,30.85766], "fy":[-47.34181,-47.39197,-47.6268,-47.59203]},
+    {"t":0.95561, "x":2.70208, "y":5.63764, "heading":-0.75701, "vx":2.78899, "vy":1.67616, "omega":-1.35634, "ax":3.45108, "ay":-6.19927, "alpha":0.33412, "fx":[57.4897,56.42198,57.57188,58.62785], "fy":[-102.13646,-103.34314,-104.53288,-103.34321]},
+    {"t":0.97691, "x":2.76228, "y":5.67195, "heading":-0.78591, "vx":2.86251, "vy":1.54409, "omega":-1.34922, "ax":4.0425, "ay":-8.88375, "alpha":0.0938, "fx":[67.52031,66.96246,67.25636,67.80678], "fy":[-147.86284,-148.20628,-148.31068,-147.97141]},
+    {"t":0.99822, "x":2.82418, "y":5.70282, "heading":-0.81465, "vx":2.94863, "vy":1.35483, "omega":-1.34723, "ax":3.76573, "ay":-10.13418, "alpha":-0.00864, "fx":[62.78049,62.79677,62.76657,62.74774], "fy":[-168.96669,-168.93833,-168.8966,-168.92602]},
+    {"t":1.01952, "x":2.88785, "y":5.72939, "heading":-0.84336, "vx":3.02886, "vy":1.13893, "omega":-1.34741, "ax":3.90571, "ay":-10.10441, "alpha":2.1371, "fx":[70.23562,53.17271,60.68962,76.32708], "fy":[-165.30813,-172.32988,-171.54057,-164.56416]},
+    {"t":1.04171, "x":2.95603, "y":5.75218, "heading":-0.87326, "vx":3.11553, "vy":0.9147, "omega":-1.29998, "ax":3.29079, "ay":-9.19937, "alpha":2.80793, "fx":[60.56947,40.26375,50.01039,68.5799], "fy":[-148.18801,-156.97076,-158.40189,-149.83569]},
+    {"t":1.06391, "x":3.02598, "y":5.77021, "heading":-0.90211, "vx":3.18856, "vy":0.71054, "omega":-1.23767, "ax":2.06397, "ay":-6.74238, "alpha":2.02394, "fx":[36.0938,26.45206,32.80136,42.27428], "fy":[-106.38754,-112.48678,-118.25773,-112.43693]},
+    {"t":1.0861, "x":3.09725, "y":5.78432, "heading":-0.92957, "vx":3.23437, "vy":0.56092, "omega":-1.19275, "ax":0.47965, "ay":-3.18292, "alpha":-0.15849, "fx":[7.87814,8.62907,8.11686,7.35815], "fy":[-53.50266,-53.08006,-52.60778,-53.0403]},
+    {"t":1.10829, "x":3.16914, "y":5.79598, "heading":-0.95604, "vx":3.24501, "vy":0.49028, "omega":-1.19627, "ax":-0.47904, "ay":-0.90512, "alpha":-1.78745, "fx":[-9.0406,-1.8271,-6.92478,-14.1489], "fy":[-21.05245,-16.11427,-9.10226,-14.08266]},
+    {"t":1.13048, "x":3.24104, "y":5.80664, "heading":-0.98259, "vx":3.23438, "vy":0.4702, "omega":-1.23594, "ax":-0.89605, "ay":0.24562, "alpha":-2.58523, "fx":[-16.70302,-6.16548,-13.18337,-23.69523], "fy":[-4.59417,2.35576,12.79928,5.81649]},
+    {"t":1.15267, "x":3.31259, "y":5.81713, "heading":-1.01002, "vx":3.2145, "vy":0.47565, "omega":-1.29331, "ax":-0.98961, "ay":0.78086, "alpha":-2.80713, "fx":[-18.71397,-7.04888,-14.30892,-25.91345], "fy":[3.60593,10.88756,22.43017,15.14227]},
+    {"t":1.17487, "x":3.38369, "y":5.82788, "heading":-1.03872, "vx":3.19253, "vy":0.49298, "omega":-1.35561, "ax":-0.91059, "ay":1.01926, "alpha":-2.70283, "fx":[-17.60151,-6.14617,-12.79274,-24.17619], "fy":[7.98945,14.69448,25.98547,19.29301]},
+    {"t":1.19706, "x":3.45431, "y":5.83907, "heading":-1.0688, "vx":3.17233, "vy":0.51559, "omega":-1.41559, "ax":-0.75343, "ay":1.12111, "alpha":-2.4496, "fx":[-15.00888,-4.43519,-10.14197,-20.65126], "fy":[10.60414,16.36554,26.76231,21.02122]},
+    {"t":1.21925, "x":3.52453, "y":5.85079, "heading":-1.10022, "vx":3.15561, "vy":0.54047, "omega":-1.46995, "ax":-0.57212, "ay":1.15656, "alpha":-2.15567, "fx":[-11.91187,-2.45229,-7.1862,-16.59735], "fy":[12.24155,17.00852,26.30667,21.56019]},
+    {"t":1.24144, "x":3.59441, "y":5.86307, "heading":-1.13284, "vx":3.14291, "vy":0.56614, "omega":-1.51779, "ax":-0.38879, "ay":1.14683, "alpha":-1.86629, "fx":[-8.72453,-0.41415,-4.25279,-12.53236], "fy":[13.0975,16.946,25.1283,21.29641]},
+    {"t":1.26363, "x":3.66407, "y":5.87592, "heading":-1.16652, "vx":3.13428, "vy":0.59159, "omega":-1.5592, "ax":-0.20047, "ay":1.08244, "alpha":-1.57189, "fx":[-5.38913,1.70147,-1.30195,-8.37758], "fy":[13.04036,16.03769,23.04173,20.05533]},
+    {"t":1.28583, "x":3.73357, "y":5.88931, "heading":-1.20112, "vx":3.12983, "vy":0.61561, "omega":-1.59409, "ax":0.01277, "ay":0.92681, "alpha":-1.22219, "fx":[-1.50394,4.0742,1.92818,-3.64707], "fy":[11.61467,13.75063,19.28168,17.15076]},
+    {"t":1.30802, "x":3.80303, "y":5.9032, "heading":-1.2365, "vx":3.13012, "vy":0.63618, "omega":-1.62121, "ax":0.27599, "ay":0.61029, "alpha":-0.75165, "fx":[3.46283,6.93432,5.74007,2.26499], "fy":[7.85293,9.04558,12.49342,11.30122]},
+    {"t":1.33021, "x":3.87256, "y":5.91747, "heading":-1.27248, "vx":3.13624, "vy":0.64972, "omega":-1.63789, "ax":0.5998, "ay":0.02009, "alpha":-0.12723, "fx":[9.78239,10.38901,10.21663,9.60557], "fy":[-0.04475,0.13639,0.71519,0.53276]},
+    {"t":1.3524, "x":3.94231, "y":5.93189, "heading":-1.30883, "vx":3.14955, "vy":0.65017, "omega":-1.64072, "ax":0.94542, "ay":-0.99372, "alpha":0.56532, "fx":[16.72675,14.0823,14.79569,17.4339], "fy":[-14.86583,-15.58786,-18.26364,-17.54215]},
+    {"t":1.37459, "x":4.01244, "y":5.94608, "heading":-1.34524, "vx":3.17053, "vy":0.62812, "omega":-1.62817, "ax":1.17222, "ay":-2.50285, "alpha":1.01159, "fx":[21.42748,16.58587,17.66132,22.48659], "fy":[-38.76545,-39.90731,-44.67662,-43.5357]},
+    {"t":1.39679, "x":4.08309, "y":5.9594, "heading":-1.38137, "vx":3.19655, "vy":0.57257, "omega":-1.60572, "ax":1.03957, "ay":-4.03265, "alpha":0.7055, "fx":[18.74148,15.29571,15.92247,19.35703], "fy":[-65.23109,-65.90432,-69.2128,-68.54123]},
+    {"t":1.41898, "x":4.15428, "y":5.97111, "heading":-1.417, "vx":3.21962, "vy":0.48308, "omega":-1.59006, "ax":0.46519, "ay":-4.69731, "alpha":-0.5667, "fx":[6.44519,9.48097,9.077,6.01469], "fy":[-79.72152,-79.28694,-76.87947,-77.31967]},
+    {"t":1.44117, "x":4.22585, "y":5.98068, "heading":-1.45229, "vx":3.22994, "vy":0.37884, "omega":-1.60264, "ax":-0.27649, "ay":-4.94195, "alpha":-2.22119, "fx":[-9.67414,1.90988,0.56274,-11.23426], "fy":[-87.8149,-86.73909,-76.91333,-78.05246]},
+    {"t":1.46336, "x":4.29746, "y":5.98787, "heading":-1.48786, "vx":3.2238, "vy":0.26917, "omega":-1.65193, "ax":-0.77691, "ay":-5.1899, "alpha":-3.16274, "fx":[-20.37641,-3.84097,-5.31974,-22.26545], "fy":[-93.885,-93.10528,-79.07583,-79.98677]},
+    {"t":1.48555, "x":4.36881, "y":5.99256, "heading":-1.52452, "vx":3.20656, "vy":0.15399, "omega":-1.72212, "ax":-0.58565, "ay":-6.58257, "alpha":-1.99068, "fx":[-14.99097,-3.80075,-4.40976,-15.84834], "fy":[-113.8176,-113.69596,-105.61811,-105.78121]},
+    {"t":1.50775, "x":4.43982, "y":5.99436, "heading":-1.56273, "vx":3.19357, "vy":0.00791, "omega":-1.7663, "ax":1.30317, "ay":-7.90379, "alpha":5.0801, "fx":[39.78702,5.61972,5.2799,36.20625], "fy":[-122.05663,-124.51233,-141.51408,-138.92633]},
+    {"t":1.53114, "x":4.51488, "y":5.99238, "heading":-1.60405, "vx":3.22405, "vy":-0.17697, "omega":-1.64747, "ax":1.11293, "ay":-5.06677, "alpha":4.05446, "fx":[30.34868,8.17813,7.19352,28.48753], "fy":[-75.55582,-75.62227,-93.48234,-93.18182]},
+    {"t":1.55453, "x":4.5906, "y":5.98686, "heading":-1.64259, "vx":3.25008, "vy":-0.29549, "omega":-1.55263, "ax":0.58744, "ay":-3.96961, "alpha":2.44891, "fx":[16.72095,3.94735,2.99268,15.50854], "fy":[-60.91071,-60.28095,-71.46958,-72.02463]},
+    {"t":1.57792, "x":4.66679, "y":5.97886, "heading":-1.6789, "vx":3.26382, "vy":-0.38835, "omega":-1.49534, "ax":0.02565, "ay":-4.14796, "alpha":1.41483, "fx":[4.48056,-2.83237,-3.58278,3.64456], "fy":[-66.27813,-65.57894,-72.02047,-72.70022]},
+    {"t":1.60131, "x":4.74314, "y":5.96864, "heading":-1.71388, "vx":3.26442, "vy":-0.48537, "omega":-1.46225, "ax":-0.56882, "ay":-4.36266, "alpha":0.34278, "fx":[-8.49638,-10.23656,-10.46515,-8.72944], "fy":[-72.06528,-71.8154,-73.38211,-73.63131]},
+    {"t":1.6247, "x":4.81935, "y":5.95609, "heading":-1.74809, "vx":3.25112, "vy":-0.58742, "omega":-1.45423, "ax":-1.18922, "ay":-4.32651, "alpha":-0.96482, "fx":[-22.65033,-17.70048,-16.97278,-21.97121], "fy":[-73.77708,-74.73972,-70.46676,-69.5]},
+    {"t":1.6481, "x":4.89507, "y":5.94117, "heading":-1.7821, "vx":3.2233, "vy":-0.68863, "omega":-1.4768, "ax":-1.78969, "ay":-4.21174, "alpha":-2.25079, "fx":[-36.41268,-25.11595,-23.13284,-34.67141], "fy":[-73.82847,-76.59891,-66.58982,-63.81385]},
+    {"t":1.67149, "x":4.96998, "y":5.92391, "heading":-1.81665, "vx":3.18144, "vy":-0.78715, "omega":-1.52945, "ax":-2.23334, "ay":-3.99839, "alpha":-3.10223, "fx":[-46.32874,-31.14972,-27.93007,-43.50648], "fy":[-71.41586,-75.80617,-61.88213,-57.50082]},
+    {"t":1.69488, "x":5.04379, "y":5.9044, "heading":-1.85242, "vx":3.1292, "vy":-0.88068, "omega":-1.60201, "ax":-2.14127, "ay":-3.0631, "alpha":-2.77981, "fx":[-43.98826,-30.84025,-27.30599,-40.64109], "fy":[-55.37415,-59.50478,-46.74122,-42.62126]},
+    {"t":1.71827, "x":5.1164, "y":5.88296, "heading":-1.8899, "vx":3.07911, "vy":-0.95233, "omega":-1.66704, "ax":-1.36697, "ay":-1.38729, "alpha":-1.13386, "fx":[-26.25157,-21.03012,-19.31361,-24.55164], "fy":[-24.87155,-26.63482,-21.37951,-19.6159]},
+    {"t":1.74166, "x":5.18805, "y":5.86031, "heading":-1.92889, "vx":3.04713, "vy":-0.98478, "omega":-1.69356, "ax":-0.42172, "ay":-0.04256, "alpha":0.79043, "fx":[-4.53871,-8.17185,-9.51655,-5.89217], "fy":[0.39544,1.76209,-1.81512,-3.1801]},
+    {"t":1.76505, "x":5.25921, "y":5.83726, "heading":-1.96851, "vx":3.03727, "vy":-0.98577, "omega":-1.67507, "ax":0.37736, "ay":0.74755, "alpha":2.34564, "fx":[13.78292,3.21166,-1.20995,9.37675], "fy":[15.48639,19.92179,9.43869,4.99812]},
+    {"t":1.78845, "x":5.33036, "y":5.81441, "heading":-2.00769, "vx":3.04609, "vy":-0.96829, "omega":-1.6202, "ax":0.9223, "ay":1.09868, "alpha":3.31804, "fx":[26.10539,11.40783,4.60377,19.38031], "fy":[22.16172,29.03194,14.47284,7.59126]},
+    {"t":1.81184, "x":5.40187, "y":5.79206, "heading":-2.04559, "vx":3.06767, "vy":-0.94259, "omega":-1.54259, "ax":1.1718, "ay":1.11182, "alpha":3.65689, "fx":[31.50045,15.59073,7.5107,23.53182], "fy":[22.31412,30.50363,14.75648,6.55965]},
+    {"t":1.83523, "x":5.47394, "y":5.77031, "heading":-2.08167, "vx":3.09508, "vy":-0.91658, "omega":-1.45705, "ax":1.07189, "ay":0.76632, "alpha":3.31647, "fx":[28.86821,14.68013,6.83499,21.08847], "fy":[15.81511,23.73355,9.72682,1.82145]},
+    {"t":1.85862, "x":5.54664, "y":5.74908, "heading":-2.11576, "vx":3.12015, "vy":-0.89865, "omega":-1.37947, "ax":0.51514, "ay":-0.16351, "alpha":2.21272, "fx":[16.06591,6.6788,1.1134,10.49057], "fy":[-0.97356,4.60013,-4.49435,-10.0346]},
+    {"t":1.88201, "x":5.61976, "y":5.72802, "heading":-2.14802, "vx":3.1322, "vy":-0.90248, "omega":-1.32771, "ax":-0.66751, "ay":-2.19579, "alpha":0.32332, "fx":[-9.92636,-11.47846,-12.31982,-10.78336], "fy":[-36.48504,-35.61903,-36.72738,-37.57986]},
+    {"t":1.9054, "x":5.69285, "y":5.7063, "heading":-2.17908, "vx":3.11659, "vy":-0.95384, "omega":-1.32015, "ax":-2.53736, "ay":-5.81535, "alpha":-1.81333, "fx":[-48.80048,-40.66043,-35.74264,-43.98261], "fy":[-97.50721,-102.67577,-96.4295,-91.14383]},
+    {"t":1.92879, "x":5.76506, "y":5.6824, "heading":-2.20996, "vx":3.05724, "vy":-1.08987, "omega":-1.36256, "ax":-3.99323, "ay":-8.79389, "alpha":-2.21646, "fx":[-76.45554,-61.63658,-56.08405,-72.08499], "fy":[-143.70038,-151.40562,-149.68201,-141.57186]},
+    {"t":1.95219, "x":5.83548, "y":5.6545, "heading":-2.24183, "vx":2.96383, "vy":-1.29558, "omega":-1.41441, "ax":-4.98699, "ay":-9.62796, "alpha":-2.12276, "fx":[-92.86034,-77.37645,-72.59367,-89.6924], "fy":[-156.19491,-164.74497,-165.06152,-155.97256]},
+    {"t":1.97558, "x":5.90344, "y":5.62156, "heading":-2.27492, "vx":2.84717, "vy":-1.52079, "omega":-1.46407, "ax":-6.18377, "ay":-9.47657, "alpha":-3.03555, "fx":[-115.69569,-94.24117,-88.77234,-113.61245], "fy":[-150.45939,-164.91971,-166.32808,-150.17242]},
+    {"t":1.99897, "x":5.96835, "y":5.5834, "heading":-2.30917, "vx":2.70253, "vy":-1.74246, "omega":-1.53507, "ax":-6.99194, "ay":-9.18735, "alpha":-3.04158, "fx":[-128.31441,-107.40592,-103.10309,-127.38571], "fy":[-144.72247,-160.92987,-162.65166,-144.29075]},
+    {"t":2.02236, "x":6.02965, "y":5.54012, "heading":-2.34507, "vx":2.53897, "vy":-1.95737, "omega":-1.60622, "ax":-7.05986, "ay":-9.21587, "alpha":-0.38118, "fx":[-119.29243,-116.48686,-116.04961,-118.90898], "fy":[-152.44984,-154.62004,-154.81435,-152.61213]},
+    {"t":2.05098, "x":6.09943, "y":5.48033, "heading":-2.39105, "vx":2.33691, "vy":-2.22113, "omega":-1.61713, "ax":-7.48953, "ay":-8.22329, "alpha":0.10134, "fx":[-124.4484,-125.16255,-125.23817,-124.53834], "fy":[-137.42533,-136.77268,-136.7374,-137.37765]},
+    {"t":2.0796, "x":6.16325, "y":5.41339, "heading":-2.43733, "vx":2.12256, "vy":-2.45649, "omega":-1.61423, "ax":-3.69481, "ay":-5.67025, "alpha":5.70051, "fx":[-40.01545,-64.18298,-82.52243,-59.64179], "fy":[-100.43521,-76.03406,-89.79229,-111.8199]},
+    {"t":2.10822, "x":6.22248, "y":5.34076, "heading":-2.48353, "vx":2.01681, "vy":-2.61878, "omega":-1.45108, "ax":-3.1408, "ay":-3.48921, "alpha":2.71561, "fx":[-42.89826,-52.07669,-61.731,-52.71626], "fy":[-60.26866,-48.94724,-56.20949,-67.22834]},
+    {"t":2.13684, "x":6.27892, "y":5.26438, "heading":-2.52506, "vx":1.92692, "vy":-2.71864, "omega":-1.37335, "ax":-3.69051, "ay":-2.18649, "alpha":-0.76001, "fx":[-64.05217,-61.83891,-58.97929,-61.20584], "fy":[-35.89542,-39.06895,-37.00821,-33.81863]},
+    {"t":2.16546, "x":6.33256, "y":5.18567, "heading":-2.56437, "vx":1.82129, "vy":-2.78122, "omega":-1.3951, "ax":-4.04206, "ay":-1.33536, "alpha":-2.83698, "fx":[-76.52385,-69.04806,-58.12171,-65.82312], "fy":[-19.87687,-32.12466,-24.74548,-12.29207]},
+    {"t":2.19409, "x":6.38303, "y":5.10553, "heading":-2.6043, "vx":1.70561, "vy":-2.81944, "omega":-1.4763, "ax":-3.16304, "ay":-0.68569, "alpha":-2.29294, "fx":[-60.22428,-54.63693,-45.18481,-50.85957], "fy":[-9.46884,-19.19968,-13.4361,-3.61591]},
+    {"t":2.22271, "x":6.43055, "y":5.02455, "heading":-2.64655, "vx":1.61508, "vy":-2.83906, "omega":-1.54193, "ax":-0.59129, "ay":-0.2096, "alpha":1.17644, "fx":[-5.96118,-8.72711,-13.74553,-10.99245], "fy":[-4.76746,0.37668,-2.25809,-7.3269]},
+    {"t":2.25133, "x":6.47653, "y":4.94321, "heading":-2.69068, "vx":1.59815, "vy":-2.84506, "omega":-1.50826, "ax":1.4253, "ay":-0.34301, "alpha":4.53234, "fx":[38.37295,28.98774,9.03112,18.64461], "fy":[-10.86828,9.0666,-0.55637,-20.5135]},
+    {"t":2.27995, "x":6.52285, "y":4.86164, "heading":-2.73385, "vx":1.63895, "vy":-2.85488, "omega":-1.37854, "ax":2.12974, "ay":-0.3244, "alpha":5.16553, "fx":[51.72309,42.1461,19.10584,29.03166], "fy":[-11.92854,11.28754,1.23594,-22.22502]},
+    {"t":2.30857, "x":6.57063, "y":4.7798, "heading":-2.7733, "vx":1.6999, "vy":-2.86416, "omega":-1.2307, "ax":1.13667, "ay":1.79445, "alpha":-1.21695, "fx":[15.10083,16.54493,23.14966,20.99568], "fy":[31.1636,26.22018,28.78485,33.48191]},
+    {"t":2.33719, "x":6.61975, "y":4.69856, "heading":-2.80853, "vx":1.73243, "vy":-2.8128, "omega":-1.26553, "ax":-2.83623, "ay":8.93876, "alpha":-0.74547, "fx":[-50.36804,-49.29299,-43.96503,-45.48827], "fy":[148.98588,147.36743,149.08471,150.58149]},
+    {"t":2.36581, "x":6.66817, "y":4.62172, "heading":-2.84475, "vx":1.65126, "vy":-2.55697, "omega":-1.28686, "ax":-6.3372, "ay":9.74433, "alpha":7.03636, "fx":[-82.26444,-75.406,-122.80609,-142.07581], "fy":[177.90499,181.99018,154.22831,135.60948]},
+    {"t":2.39443, "x":6.71284, "y":4.55253, "heading":-2.88158, "vx":1.46988, "vy":-2.27808, "omega":-1.08548, "ax":-6.38485, "ay":9.85009, "alpha":6.31674, "fx":[-87.03652,-77.65451,-121.1212,-139.91723], "fy":[177.53571,182.3194,156.9492,139.98049]},
+    {"t":2.42305, "x":6.75229, "y":4.49136, "heading":-2.91264, "vx":1.28715, "vy":-1.99616, "omega":-0.90469, "ax":-6.40409, "ay":9.89787, "alpha":5.76208, "fx":[-90.31946,-79.43089,-119.5432,-137.71879], "fy":[176.5296,181.9835,158.61732,142.84053]},
+    {"t":2.45167, "x":6.78651, "y":4.43828, "heading":-2.93854, "vx":1.10386, "vy":-1.71288, "omega":-0.73977, "ax":-6.41526, "ay":9.92841, "alpha":5.29537, "fx":[-92.81906,-81.07007,-118.19905,-135.66893], "fy":[175.54955,181.47691,159.85185,145.1291]},
+    {"t":2.48029, "x":6.81547, "y":4.39332, "heading":-2.95971, "vx":0.92025, "vy":-1.42872, "omega":-0.58821, "ax":-6.42272, "ay":9.95121, "alpha":4.87613, "fx":[-94.8155,-82.69122,-117.04735,-133.70039], "fy":[174.67266,180.87589,160.8346,147.14446]},
+    {"t":2.50891, "x":6.83918, "y":4.35651, "heading":-2.97654, "vx":0.73642, "vy":-1.14391, "omega":-0.44866, "ax":-6.42802, "ay":9.96981, "alpha":4.48187, "fx":[-96.45919,-84.34923,-116.04454,-131.75482], "fy":[173.89965,180.19744,161.6508,149.01974]},
+    {"t":2.53753, "x":6.85763, "y":4.32785, "heading":-2.98939, "vx":0.55245, "vy":-0.85857, "omega":-0.32038, "ax":-6.43183, "ay":9.9858, "alpha":4.10032, "fx":[-97.84158,-86.06486,-115.15641,-129.79921], "fy":[173.21831,179.44838,162.34941,150.81785]},
+    {"t":2.56616, "x":6.8708, "y":4.30737, "heading":-2.99855, "vx":0.36837, "vy":-0.57277, "omega":-0.20303, "ax":-6.43447, "ay":9.99991, "alpha":3.72736, "fx":[-99.01871,-87.82807,-114.35977,-127.83164], "fy":[172.61777,178.64051,162.95982,152.55673]},
+    {"t":2.59478, "x":6.87871, "y":4.29507, "heading":-3.00437, "vx":0.18421, "vy":-0.28656, "omega":-0.09635, "ax":-6.43611, "ay":10.01239, "alpha":3.36636, "fx":[-100.02185,-89.59719,-113.64325,-125.88484], "fy":[172.09263,177.7979,163.49769,154.21886]},
+    {"t":2.6234, "x":6.88135, "y":4.29097, "heading":-3.00712, "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/FiveMeterTestPath.traj b/src/main/deploy/choreo/FiveMeterTestPath.traj
new file mode 100644
index 00000000..8ab66ea4
--- /dev/null
+++ b/src/main/deploy/choreo/FiveMeterTestPath.traj
@@ -0,0 +1,157 @@
+{
+ "name":"FiveMeterTestPath",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":0.0, "y":0.0, "heading":0.0, "intervals":120, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":5.0, "y":0.0, "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":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":1.0}}, "enabled":true},
+    {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":1.0}}, "enabled":true}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":120, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"5 m", "val":5.0}, "y":{"exp":"0 m", "val":0.0}, "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":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"1 m / s", "val":1.0}}}, "enabled":true},
+    {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"1 m / s ^ 2", "val":1.0}}}, "enabled":true}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,6.00122],
+  "samples":[
+    {"t":0.0, "x":0.0, "y":0.0, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.99959, "ay":0.0, "alpha":0.00745, "fx":[16.77217,16.43266,16.72304,16.72304], "fy":[-0.02404,0.02404,0.02404,-0.02404]},
+    {"t":0.05001, "x":0.00125, "y":0.0, "heading":0.0, "vx":0.04999, "vy":0.0, "omega":0.00037, "ax":0.99979, "ay":0.0, "alpha":0.00019, "fx":[16.73708,16.54679,16.68995,16.68995], "fy":[-0.02306,0.02306,0.02306,-0.02306]},
+    {"t":0.10002, "x":0.005, "y":0.0, "heading":0.00002, "vx":0.09999, "vy":0.0, "omega":0.00038, "ax":0.99977, "ay":0.0, "alpha":-0.00911, "fx":[16.68767,16.68767,16.64381,16.64381], "fy":[-0.02246,0.02247,0.02246,-0.02246]},
+    {"t":0.15003, "x":0.01125, "y":0.0, "heading":0.00004, "vx":0.14999, "vy":0.0, "omega":-0.00007, "ax":0.99976, "ay":0.0, "alpha":0.00185, "fx":[16.74202,16.52663,16.6967,16.6967], "fy":[-0.02217,0.02217,0.02217,-0.02217]},
+    {"t":0.20004, "x":0.02, "y":0.0, "heading":0.00003, "vx":0.19999, "vy":0.0, "omega":0.00002, "ax":0.99974, "ay":0.0, "alpha":-0.00885, "fx":[16.68655,16.68655,16.64397,16.64397], "fy":[-0.02181,0.02181,0.02181,-0.02181]},
+    {"t":0.25005, "x":0.03125, "y":0.0, "heading":0.00004, "vx":0.24998, "vy":0.0, "omega":-0.00042, "ax":0.99973, "ay":0.0, "alpha":0.00355, "fx":[16.74892,16.50262,16.70417,16.70417], "fy":[-0.02189,0.02189,0.02189,-0.02189]},
+    {"t":0.30006, "x":0.04501, "y":0.0, "heading":0.00001, "vx":0.29998, "vy":0.0, "omega":-0.00025, "ax":0.99971, "ay":0.0, "alpha":-0.0088, "fx":[16.6858,16.6858,16.64347,16.64347], "fy":[-0.02168,0.02168,0.02168,-0.02168]},
+    {"t":0.35007, "x":0.06126, "y":0.0, "heading":0.0, "vx":0.34998, "vy":0.0, "omega":-0.00069, "ax":0.99968, "ay":0.0, "alpha":0.00549, "fx":[16.75816,16.47274,16.71305,16.71305], "fy":[-0.02207,0.02207,0.02207,-0.02207]},
+    {"t":0.40008, "x":0.08001, "y":0.0, "heading":-0.00003, "vx":0.39997, "vy":0.0, "omega":-0.00041, "ax":0.99966, "ay":0.0, "alpha":-0.00889, "fx":[16.68519,16.68519,16.64241,16.64241], "fy":[-0.0219,0.02191,0.0219,-0.0219]},
+    {"t":0.45009, "x":0.10126, "y":0.0, "heading":-0.00005, "vx":0.44996, "vy":0.0, "omega":-0.00086, "ax":0.99962, "ay":0.0, "alpha":0.00796, "fx":[16.77062,16.43332,16.72456,16.72456], "fy":[-0.02254,0.02254,0.02254,-0.02254]},
+    {"t":0.5001, "x":0.12502, "y":0.0, "heading":-0.0001, "vx":0.49995, "vy":0.0, "omega":-0.00046, "ax":0.99959, "ay":0.0, "alpha":-0.00904, "fx":[16.68437,16.68437,16.64087,16.64087], "fy":[-0.02227,0.02228,0.02227,-0.02227]},
+    {"t":0.55011, "x":0.15127, "y":0.0, "heading":-0.00012, "vx":0.54994, "vy":0.0, "omega":-0.00091, "ax":0.99954, "ay":0.0, "alpha":0.01125, "fx":[16.7868,16.38118,16.73965,16.73965], "fy":[-0.02307,0.02307,0.02307,-0.02307]},
+    {"t":0.60012, "x":0.18002, "y":0.0, "heading":-0.00016, "vx":0.59993, "vy":0.0, "omega":-0.00035, "ax":0.99948, "ay":0.0, "alpha":-0.00914, "fx":[16.68282,16.68282,16.63883,16.63883], "fy":[-0.02252,0.02253,0.02251,-0.02252]},
+    {"t":0.65013, "x":0.21127, "y":0.0, "heading":-0.00018, "vx":0.64991, "vy":0.0, "omega":-0.0008, "ax":0.9994, "ay":0.0, "alpha":0.01574, "fx":[16.80731,16.31171,16.75954,16.75954], "fy":[-0.02338,0.02338,0.02338,-0.02338]},
+    {"t":0.70014, "x":0.24503, "y":0.0, "heading":-0.00022, "vx":0.6999, "vy":0.0, "omega":-0.00002, "ax":0.9993, "ay":0.0, "alpha":-0.00903, "fx":[16.67951,16.67951,16.63602,16.63602], "fy":[-0.02226,0.02227,0.02225,-0.02226]},
+    {"t":0.75015, "x":0.28128, "y":0.0, "heading":-0.00022, "vx":0.74987, "vy":0.0, "omega":-0.00047, "ax":0.99914, "ay":0.0, "alpha":0.0219, "fx":[16.83194,16.21926,16.78492,16.78492], "fy":[-0.02301,0.02301,0.02301,-0.02301]},
+    {"t":0.80016, "x":0.32003, "y":0.0, "heading":-0.00025, "vx":0.79984, "vy":0.0, "omega":0.00063, "ax":0.99891, "ay":0.0, "alpha":-0.00849, "fx":[16.67184,16.67184,16.63097,16.63097], "fy":[-0.02091,0.02092,0.0209,-0.02091]},
+    {"t":0.85017, "x":0.36128, "y":0.0, "heading":-0.00022, "vx":0.84979, "vy":0.0, "omega":0.0002, "ax":0.99851, "ay":0.0, "alpha":0.0287, "fx":[16.84907,16.1183,16.80567,16.80567], "fy":[-0.02124,0.02124,0.02124,-0.02124]},
+    {"t":0.90018, "x":0.40502, "y":0.0, "heading":-0.00021, "vx":0.89973, "vy":0.0, "omega":0.00164, "ax":0.99763, "ay":0.0, "alpha":-0.0072, "fx":[16.64737,16.64737,16.61271,16.61272], "fy":[-0.01773,0.01774,0.01771,-0.01773]},
+    {"t":0.95019, "x":0.45127, "y":0.0, "heading":-0.00012, "vx":0.94962, "vy":0.0, "omega":0.00128, "ax":0.99423, "ay":0.0, "alpha":0.02853, "fx":[16.7637,16.0711,16.72914,16.72914], "fy":[-0.01692,0.01693,0.01693,-0.01692]},
+    {"t":1.0002, "x":0.5, "y":0.0, "heading":-0.00006, "vx":0.99934, "vy":0.0, "omega":0.0027, "ax":0.00915, "ay":0.0, "alpha":-0.00505, "fx":[0.16466,0.16466,0.14036,0.14036], "fy":[-0.01247,0.01241,0.01237,-0.01247]},
+    {"t":1.05021, "x":0.54999, "y":0.0, "heading":0.00008, "vx":0.9998, "vy":0.0, "omega":0.00245, "ax":0.0, "ay":0.0, "alpha":-0.00909, "fx":[-0.01709,0.08766,-0.03538,-0.03538], "fy":[-0.00888,0.00903,0.00903,-0.00888]},
+    {"t":1.10022, "x":0.59999, "y":0.0, "heading":0.0002, "vx":0.9998, "vy":0.0, "omega":0.002, "ax":0.0, "ay":0.0, "alpha":-0.00279, "fx":[0.00672,0.00672,-0.00671,-0.00672], "fy":[-0.00694,0.00681,0.00679,-0.00694]},
+    {"t":1.15023, "x":0.64999, "y":0.0, "heading":0.0003, "vx":0.9998, "vy":0.0, "omega":0.00186, "ax":0.0, "ay":0.0, "alpha":-0.02098, "fx":[-0.09134,0.28866,-0.09866,-0.09866], "fy":[-0.00351,0.00364,0.00364,-0.00351]},
+    {"t":1.20024, "x":0.69999, "y":0.0, "heading":0.00039, "vx":0.9998, "vy":0.0, "omega":0.00081, "ax":0.0, "ay":0.0, "alpha":-0.00132, "fx":[0.00317,0.00318,-0.00317,-0.00318], "fy":[-0.00332,0.00319,0.00318,-0.00332]},
+    {"t":1.25025, "x":0.74999, "y":0.0, "heading":0.00043, "vx":0.9998, "vy":0.0, "omega":0.00074, "ax":0.0, "ay":0.0, "alpha":-0.0107, "fx":[-0.04727,0.14836,-0.05054,-0.05055], "fy":[-0.00154,0.00166,0.00166,-0.00154]},
+    {"t":1.30026, "x":0.79999, "y":0.0, "heading":0.00047, "vx":0.9998, "vy":0.0, "omega":0.00021, "ax":0.0, "ay":0.0, "alpha":-0.0006, "fx":[0.00144,0.00144,-0.00144,-0.00144], "fy":[-0.00154,0.00143,0.00142,-0.00154]},
+    {"t":1.35027, "x":0.84999, "y":0.0, "heading":0.00048, "vx":0.9998, "vy":0.0, "omega":0.00018, "ax":0.0, "ay":0.0, "alpha":-0.00413, "fx":[-0.01783,0.05655,-0.01936,-0.01936], "fy":[-0.00069,0.0008,0.0008,-0.00069]},
+    {"t":1.40028, "x":0.89999, "y":0.0, "heading":0.00049, "vx":0.9998, "vy":0.0, "omega":-0.00003, "ax":0.0, "ay":0.0, "alpha":-0.00025, "fx":[0.0006,0.0006,-0.0006,-0.0006], "fy":[-0.00068,0.00058,0.00057,-0.00068]},
+    {"t":1.45029, "x":0.94999, "y":0.0, "heading":0.00049, "vx":0.9998, "vy":0.0, "omega":-0.00004, "ax":0.0, "ay":0.0, "alpha":-0.00149, "fx":[-0.00639,0.02032,-0.00696,-0.00697], "fy":[-0.00022,0.00033,0.00033,-0.00022]},
+    {"t":1.5003, "x":0.99999, "y":0.0, "heading":0.00048, "vx":0.9998, "vy":0.0, "omega":-0.00012, "ax":0.0, "ay":0.0, "alpha":-0.00007, "fx":[0.00016,0.00016,-0.00016,-0.00016], "fy":[-0.00022,0.00012,0.00012,-0.00022]},
+    {"t":1.55032, "x":1.04999, "y":0.0, "heading":0.00048, "vx":0.9998, "vy":0.0, "omega":-0.00012, "ax":0.0, "ay":0.0, "alpha":-0.00054, "fx":[-0.00266,0.00794,-0.00264,-0.00264], "fy":[0.00006,0.00003,0.00004,0.00006]},
+    {"t":1.60033, "x":1.09999, "y":0.0, "heading":0.00047, "vx":0.9998, "vy":0.0, "omega":-0.00015, "ax":0.0, "ay":0.0, "alpha":0.00004, "fx":[-0.0001,-0.0001,0.0001,0.0001], "fy":[0.00004,-0.00013,-0.00014,0.00004]},
+    {"t":1.65034, "x":1.14999, "y":0.0, "heading":0.00046, "vx":0.9998, "vy":0.0, "omega":-0.00015, "ax":0.0, "ay":0.0, "alpha":-0.00019, "fx":[-0.00152,0.00378,-0.00113,-0.00113], "fy":[0.00024,-0.00015,-0.00015,0.00025]},
+    {"t":1.70035, "x":1.19999, "y":0.0, "heading":0.00046, "vx":0.9998, "vy":0.0, "omega":-0.00015, "ax":0.0, "ay":0.0, "alpha":0.0001, "fx":[-0.00025,-0.00025,0.00025,0.00025], "fy":[0.0002,-0.00029,-0.00029,0.0002]},
+    {"t":1.75036, "x":1.24999, "y":0.0, "heading":0.00045, "vx":0.9998, "vy":0.0, "omega":-0.00015, "ax":0.0, "ay":0.0, "alpha":-0.00005, "fx":[-0.00115,0.00222,-0.00054,-0.00054], "fy":[0.00035,-0.00026,-0.00026,0.00035]},
+    {"t":1.80037, "x":1.29999, "y":0.0, "heading":0.00044, "vx":0.9998, "vy":0.0, "omega":-0.00015, "ax":0.0, "ay":0.0, "alpha":0.00013, "fx":[-0.00033,-0.00033,0.00033,0.00033], "fy":[0.00028,-0.00037,-0.00038,0.00028]},
+    {"t":1.85038, "x":1.34999, "y":0.0, "heading":0.00043, "vx":0.9998, "vy":0.0, "omega":-0.00015, "ax":0.0, "ay":0.0, "alpha":0.00002, "fx":[-0.00097,0.00146,-0.00024,-0.00025], "fy":[0.0004,-0.00031,-0.00031,0.0004]},
+    {"t":1.90039, "x":1.39999, "y":0.0, "heading":0.00043, "vx":0.9998, "vy":0.0, "omega":-0.00014, "ax":0.0, "ay":0.0, "alpha":0.00015, "fx":[-0.00036,-0.00036,0.00036,0.00036], "fy":[0.00032,-0.0004,-0.00041,0.00032]},
+    {"t":1.9504, "x":1.44999, "y":0.0, "heading":0.00042, "vx":0.9998, "vy":0.0, "omega":-0.00014, "ax":0.0, "ay":0.0, "alpha":0.00006, "fx":[-0.00083,0.00098,-0.00007,-0.00008], "fy":[0.00041,-0.00033,-0.00033,0.00041]},
+    {"t":2.00041, "x":1.49999, "y":0.0, "heading":0.00041, "vx":0.9998, "vy":0.0, "omega":-0.00013, "ax":0.0, "ay":0.0, "alpha":0.00015, "fx":[-0.00036,-0.00036,0.00036,0.00036], "fy":[0.00032,-0.0004,-0.00041,0.00032]},
+    {"t":2.05042, "x":1.54999, "y":0.0, "heading":0.00041, "vx":0.9998, "vy":0.0, "omega":-0.00013, "ax":0.0, "ay":0.0, "alpha":0.00008, "fx":[-0.00069,0.00063,0.00003,0.00003], "fy":[0.0004,-0.00032,-0.00032,0.0004]},
+    {"t":2.10043, "x":1.59999, "y":0.0, "heading":0.0004, "vx":0.9998, "vy":0.0, "omega":-0.00012, "ax":0.0, "ay":0.0, "alpha":0.00014, "fx":[-0.00034,-0.00034,0.00034,0.00034], "fy":[0.0003,-0.00038,-0.00039,0.0003]},
+    {"t":2.15044, "x":1.64999, "y":0.0, "heading":0.00039, "vx":0.9998, "vy":0.0, "omega":-0.00012, "ax":0.0, "ay":0.0, "alpha":0.00009, "fx":[-0.00056,0.00036,0.0001,0.0001], "fy":[0.00036,-0.00028,-0.00028,0.00036]},
+    {"t":2.20045, "x":1.69999, "y":0.0, "heading":0.00039, "vx":0.9998, "vy":0.0, "omega":-0.00011, "ax":0.0, "ay":0.0, "alpha":0.00012, "fx":[-0.0003,-0.0003,0.0003,0.0003], "fy":[0.00026,-0.00034,-0.00035,0.00026]},
+    {"t":2.25046, "x":1.74999, "y":0.0, "heading":0.00038, "vx":0.9998, "vy":0.0, "omega":-0.0001, "ax":0.0, "ay":0.0, "alpha":0.00008, "fx":[-0.00044,0.00018,0.00013,0.00013], "fy":[0.00031,-0.00024,-0.00024,0.00032]},
+    {"t":2.30047, "x":1.79999, "y":0.0, "heading":0.00038, "vx":0.9998, "vy":0.0, "omega":-0.0001, "ax":0.0, "ay":0.0, "alpha":0.0001, "fx":[-0.00025,-0.00025,0.00025,0.00025], "fy":[0.00021,-0.00029,-0.00029,0.00021]},
+    {"t":2.35048, "x":1.84999, "y":0.0, "heading":0.00037, "vx":0.9998, "vy":0.0, "omega":-0.0001, "ax":0.0, "ay":0.0, "alpha":0.00007, "fx":[-0.00033,0.00007,0.00013,0.00013], "fy":[0.00026,-0.00018,-0.00018,0.00026]},
+    {"t":2.40049, "x":1.89999, "y":0.0, "heading":0.00037, "vx":0.9998, "vy":0.0, "omega":-0.00009, "ax":0.0, "ay":0.0, "alpha":0.00008, "fx":[-0.00019,-0.00019,0.00019,0.00019], "fy":[0.00016,-0.00023,-0.00023,0.00016]},
+    {"t":2.4505, "x":1.94999, "y":0.0, "heading":0.00036, "vx":0.9998, "vy":0.0, "omega":-0.00009, "ax":0.0, "ay":0.0, "alpha":0.00006, "fx":[-0.00022,0.0,0.00011,0.00011], "fy":[0.0002,-0.00012,-0.00012,0.0002]},
+    {"t":2.50051, "x":1.99999, "y":0.0, "heading":0.00036, "vx":0.9998, "vy":0.0, "omega":-0.00009, "ax":0.0, "ay":0.0, "alpha":0.00005, "fx":[-0.00013,-0.00013,0.00013,0.00013], "fy":[0.0001,-0.00016,-0.00017,0.0001]},
+    {"t":2.55052, "x":2.04999, "y":0.0, "heading":0.00035, "vx":0.9998, "vy":0.0, "omega":-0.00008, "ax":0.0, "ay":0.0, "alpha":0.00003, "fx":[-0.00013,-0.00001,0.00007,0.00007], "fy":[0.00013,-0.00006,-0.00006,0.00013]},
+    {"t":2.60053, "x":2.09999, "y":0.0, "heading":0.00035, "vx":0.9998, "vy":0.0, "omega":-0.00008, "ax":0.0, "ay":0.0, "alpha":0.00003, "fx":[-0.00006,-0.00006,0.00006,0.00006], "fy":[0.00003,-0.0001,-0.0001,0.00003]},
+    {"t":2.65054, "x":2.14999, "y":0.0, "heading":0.00035, "vx":0.9998, "vy":0.0, "omega":-0.00008, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[-0.00005,0.00001,0.00002,0.00002], "fy":[0.00006,0.0,0.0,0.00006]},
+    {"t":2.70055, "x":2.19999, "y":0.0, "heading":0.00034, "vx":0.9998, "vy":0.0, "omega":-0.00008, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-0.00003,-0.00003,-0.00004,-0.00003]},
+    {"t":2.75056, "x":2.24999, "y":0.0, "heading":0.00034, "vx":0.9998, "vy":0.0, "omega":-0.00008, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[0.00002,0.00005,-0.00004,-0.00004], "fy":[0.0,0.00007,0.00007,0.0]},
+    {"t":2.80057, "x":2.29999, "y":0.0, "heading":0.00033, "vx":0.9998, "vy":0.0, "omega":-0.00008, "ax":0.0, "ay":0.0, "alpha":-0.00003, "fx":[0.00007,0.00007,-0.00007,-0.00007], "fy":[-0.0001,0.00004,0.00003,-0.0001]},
+    {"t":2.85058, "x":2.34999, "y":0.0, "heading":0.00033, "vx":0.9998, "vy":0.0, "omega":-0.00008, "ax":0.0, "ay":0.0, "alpha":-0.00004, "fx":[0.0001,0.00009,-0.0001,-0.0001], "fy":[-0.00007,0.00014,0.00014,-0.00007]},
+    {"t":2.90059, "x":2.39999, "y":0.0, "heading":0.00033, "vx":0.9998, "vy":0.0, "omega":-0.00008, "ax":0.0, "ay":0.0, "alpha":-0.00006, "fx":[0.00014,0.00014,-0.00014,-0.00014], "fy":[-0.00017,0.00011,0.0001,-0.00016]},
+    {"t":2.9506, "x":2.44999, "y":0.0, "heading":0.00032, "vx":0.9998, "vy":0.0, "omega":-0.00009, "ax":0.0, "ay":0.0, "alpha":-0.00007, "fx":[0.00017,0.00015,-0.00016,-0.00016], "fy":[-0.00014,0.0002,0.0002,-0.00014]},
+    {"t":3.00061, "x":2.49999, "y":0.0, "heading":0.00032, "vx":0.9998, "vy":0.0, "omega":-0.00009, "ax":0.0, "ay":0.0, "alpha":-0.00008, "fx":[0.0002,0.0002,-0.0002,-0.0002], "fy":[-0.00023,0.00018,0.00017,-0.00023]},
+    {"t":3.05062, "x":2.55, "y":0.0, "heading":0.00031, "vx":0.9998, "vy":0.0, "omega":-0.00009, "ax":0.0, "ay":0.0, "alpha":-0.00009, "fx":[0.00024,0.0002,-0.00022,-0.00022], "fy":[-0.00021,0.00027,0.00027,-0.00021]},
+    {"t":3.10063, "x":2.6, "y":0.0, "heading":0.00031, "vx":0.9998, "vy":0.0, "omega":-0.0001, "ax":0.0, "ay":0.0, "alpha":-0.00011, "fx":[0.00027,0.00027,-0.00027,-0.00027], "fy":[-0.0003,0.00024,0.00023,-0.0003]},
+    {"t":3.15064, "x":2.65, "y":0.0, "heading":0.0003, "vx":0.9998, "vy":0.0, "omega":-0.0001, "ax":0.0, "ay":0.0, "alpha":-0.00012, "fx":[0.00032,0.00024,-0.00028,-0.00028], "fy":[-0.00028,0.00033,0.00033,-0.00027]},
+    {"t":3.20065, "x":2.7, "y":0.0, "heading":0.0003, "vx":0.9998, "vy":0.0, "omega":-0.00011, "ax":0.0, "ay":0.0, "alpha":-0.00014, "fx":[0.00034,0.00034,-0.00034,-0.00034], "fy":[-0.00036,0.00031,0.0003,-0.00036]},
+    {"t":3.25066, "x":2.75, "y":0.0, "heading":0.00029, "vx":0.9998, "vy":0.0, "omega":-0.00012, "ax":0.0, "ay":0.0, "alpha":-0.00014, "fx":[0.00039,0.00027,-0.00033,-0.00033], "fy":[-0.00034,0.0004,0.0004,-0.00034]},
+    {"t":3.30067, "x":2.8, "y":0.0, "heading":0.00029, "vx":0.9998, "vy":0.0, "omega":-0.00012, "ax":0.0, "ay":0.0, "alpha":-0.00016, "fx":[0.0004,0.0004,-0.0004,-0.0004], "fy":[-0.00042,0.00037,0.00037,-0.00042]},
+    {"t":3.35068, "x":2.85, "y":0.0, "heading":0.00028, "vx":0.9998, "vy":0.0, "omega":-0.00013, "ax":0.0, "ay":0.0, "alpha":-0.00016, "fx":[0.00048,0.00026,-0.00037,-0.00037], "fy":[-0.00041,0.00046,0.00046,-0.00041]},
+    {"t":3.40069, "x":2.9, "y":0.0, "heading":0.00028, "vx":0.9998, "vy":0.0, "omega":-0.00014, "ax":0.0, "ay":0.0, "alpha":-0.00019, "fx":[0.00046,0.00046,-0.00046,-0.00046], "fy":[-0.00049,0.00044,0.00043,-0.00049]},
+    {"t":3.4507, "x":2.95, "y":0.0, "heading":0.00027, "vx":0.9998, "vy":0.0, "omega":-0.00015, "ax":0.0, "ay":0.0, "alpha":-0.00018, "fx":[0.00058,0.0002,-0.00039,-0.00039], "fy":[-0.00047,0.00052,0.00052,-0.00047]},
+    {"t":3.50071, "x":3.0, "y":0.0, "heading":0.00026, "vx":0.9998, "vy":0.0, "omega":-0.00016, "ax":0.0, "ay":0.0, "alpha":-0.00021, "fx":[0.00052,0.00052,-0.00052,-0.00052], "fy":[-0.00054,0.0005,0.00049,-0.00054]},
+    {"t":3.55072, "x":3.05, "y":0.0, "heading":0.00025, "vx":0.9998, "vy":0.0, "omega":-0.00017, "ax":0.0, "ay":0.0, "alpha":-0.00019, "fx":[0.0007,0.0001,-0.0004,-0.0004], "fy":[-0.00053,0.00058,0.00058,-0.00053]},
+    {"t":3.60073, "x":3.1, "y":0.0, "heading":0.00024, "vx":0.9998, "vy":0.0, "omega":-0.00018, "ax":0.0, "ay":0.0, "alpha":-0.00024, "fx":[0.00058,0.00058,-0.00058,-0.00058], "fy":[-0.0006,0.00055,0.00054,-0.0006]},
+    {"t":3.65074, "x":3.15, "y":0.0, "heading":0.00024, "vx":0.9998, "vy":0.0, "omega":-0.00019, "ax":0.0, "ay":0.0, "alpha":-0.0002, "fx":[0.00081,-0.00004,-0.00039,-0.00039], "fy":[-0.00058,0.00063,0.00063,-0.00058]},
+    {"t":3.70075, "x":3.2, "y":0.0, "heading":0.00023, "vx":0.9998, "vy":0.0, "omega":-0.0002, "ax":0.0, "ay":0.0, "alpha":-0.00026, "fx":[0.00062,0.00062,-0.00062,-0.00062], "fy":[-0.00064,0.0006,0.00059,-0.00064]},
+    {"t":3.75076, "x":3.25, "y":0.0, "heading":0.00022, "vx":0.9998, "vy":0.0, "omega":-0.00021, "ax":0.0, "ay":0.0, "alpha":-0.00021, "fx":[0.00093,-0.00022,-0.00035,-0.00035], "fy":[-0.00062,0.00067,0.00067,-0.00062]},
+    {"t":3.80077, "x":3.3, "y":0.0, "heading":0.0002, "vx":0.9998, "vy":0.0, "omega":-0.00022, "ax":0.0, "ay":0.0, "alpha":-0.00027, "fx":[0.00066,0.00066,-0.00066,-0.00066], "fy":[-0.00068,0.00064,0.00063,-0.00068]},
+    {"t":3.85078, "x":3.35, "y":0.0, "heading":0.00019, "vx":0.9998, "vy":0.0, "omega":-0.00024, "ax":0.0, "ay":0.0, "alpha":-0.0002, "fx":[0.00105,-0.00047,-0.00029,-0.00029], "fy":[-0.00066,0.0007,0.0007,-0.00066]},
+    {"t":3.90079, "x":3.4, "y":0.0, "heading":0.00018, "vx":0.9998, "vy":0.0, "omega":-0.00025, "ax":0.0, "ay":0.0, "alpha":-0.00028, "fx":[0.00068,0.00068,-0.00068,-0.00068], "fy":[-0.0007,0.00066,0.00065,-0.0007]},
+    {"t":3.9508, "x":3.45, "y":0.0, "heading":0.00017, "vx":0.9998, "vy":0.0, "omega":-0.00026, "ax":0.0, "ay":0.0, "alpha":-0.00018, "fx":[0.00117,-0.00078,-0.0002,-0.0002], "fy":[-0.00067,0.00071,0.00071,-0.00067]},
+    {"t":4.00081, "x":3.5, "y":0.0, "heading":0.00016, "vx":0.9998, "vy":0.0, "omega":-0.00027, "ax":0.0, "ay":0.0, "alpha":-0.00028, "fx":[0.00068,0.00068,-0.00068,-0.00068], "fy":[-0.00069,0.00066,0.00065,-0.00069]},
+    {"t":4.05082, "x":3.55, "y":0.0, "heading":0.00014, "vx":0.9998, "vy":0.0, "omega":-0.00028, "ax":0.0, "ay":0.0, "alpha":-0.00015, "fx":[0.00128,-0.00117,-0.00005,-0.00005], "fy":[-0.00066,0.00069,0.00069,-0.00066]},
+    {"t":4.10083, "x":3.6, "y":0.0, "heading":0.00013, "vx":0.9998, "vy":0.0, "omega":-0.00029, "ax":0.0, "ay":0.0, "alpha":-0.00026, "fx":[0.00065,0.00065,-0.00065,-0.00065], "fy":[-0.00066,0.00063,0.00062,-0.00066]},
+    {"t":4.15084, "x":3.65, "y":0.0, "heading":0.00011, "vx":0.9998, "vy":0.0, "omega":-0.00031, "ax":0.0, "ay":0.0, "alpha":-0.00009, "fx":[0.00139,-0.00172,0.00016,0.00016], "fy":[-0.00061,0.00064,0.00064,-0.00061]},
+    {"t":4.20085, "x":3.7, "y":0.0, "heading":0.0001, "vx":0.9998, "vy":0.0, "omega":-0.00031, "ax":0.0, "ay":0.0, "alpha":-0.00023, "fx":[0.00057,0.00057,-0.00057,-0.00057], "fy":[-0.00058,0.00055,0.00054,-0.00058]},
+    {"t":4.25086, "x":3.75, "y":0.0, "heading":0.00008, "vx":0.9998, "vy":0.0, "omega":-0.00032, "ax":0.0, "ay":0.0, "alpha":0.00001, "fx":[0.00159,-0.00272,0.00056,0.00056], "fy":[-0.00051,0.00054,0.00055,-0.00051]},
+    {"t":4.30087, "x":3.8, "y":0.0, "heading":0.00007, "vx":0.9998, "vy":0.0, "omega":-0.00032, "ax":0.0, "ay":0.0, "alpha":-0.00018, "fx":[0.00044,0.00044,-0.00044,-0.00044], "fy":[-0.00044,0.00042,0.0004,-0.00044]},
+    {"t":4.35088, "x":3.85, "y":0.0, "heading":0.00005, "vx":0.9998, "vy":0.0, "omega":-0.00033, "ax":0.0, "ay":0.0, "alpha":0.00023, "fx":[0.00216,-0.00509,0.00147,0.00147], "fy":[-0.00035,0.00038,0.00038,-0.00035]},
+    {"t":4.40089, "x":3.9, "y":0.0, "heading":0.00003, "vx":0.9998, "vy":0.0, "omega":-0.00032, "ax":0.0, "ay":0.0, "alpha":-0.00009, "fx":[0.00022,0.00022,-0.00022,-0.00022], "fy":[-0.00022,0.0002,0.00018,-0.00022]},
+    {"t":4.4509, "x":3.95, "y":0.0, "heading":0.00002, "vx":0.9998, "vy":0.0, "omega":-0.00032, "ax":0.0, "ay":0.0, "alpha":0.00078, "fx":[0.00408,-0.01188,0.0039,0.0039], "fy":[-0.0001,0.00013,0.00013,-0.0001]},
+    {"t":4.50091, "x":4.0, "y":0.0, "heading":0.0, "vx":0.9998, "vy":0.0, "omega":-0.00028, "ax":0.0, "ay":0.0, "alpha":0.00006, "fx":[-0.00013,-0.00013,0.00013,0.00013], "fy":[0.00014,-0.00017,-0.00018,0.00014]},
+    {"t":4.55093, "x":4.05, "y":0.0, "heading":-0.00001, "vx":0.9998, "vy":0.0, "omega":-0.00028, "ax":0.0, "ay":0.0, "alpha":0.00231, "fx":[0.01035,-0.03229,0.01097,0.01097], "fy":[0.00029,-0.00026,-0.00026,0.00029]},
+    {"t":4.60094, "x":4.1, "y":0.0, "heading":-0.00003, "vx":0.9998, "vy":0.0, "omega":-0.00017, "ax":0.0, "ay":0.0, "alpha":0.00032, "fx":[-0.00075,-0.00075,0.00075,0.00075], "fy":[0.00078,-0.00079,-0.00081,0.00078]},
+    {"t":4.65095, "x":4.15, "y":0.0, "heading":-0.00003, "vx":0.9998, "vy":0.0, "omega":-0.00015, "ax":0.0, "ay":0.0, "alpha":0.00642, "fx":[0.02846,-0.08921,0.03038,0.03037], "fy":[0.00092,-0.0009,-0.0009,0.00092]},
+    {"t":4.70096, "x":4.2, "y":0.0, "heading":-0.00004, "vx":0.9998, "vy":0.0, "omega":0.00017, "ax":0.0, "ay":0.0, "alpha":0.00081, "fx":[-0.00194,-0.00194,0.00194,0.00194], "fy":[0.002,-0.00201,-0.00202,0.002]},
+    {"t":4.75097, "x":4.25, "y":0.0, "heading":-0.00003, "vx":0.9998, "vy":0.0, "omega":0.00021, "ax":0.0, "ay":0.0, "alpha":0.01524, "fx":[0.06758,-0.21167,0.07205,0.07205], "fy":[0.00217,-0.00216,-0.00216,0.00217]},
+    {"t":4.80098, "x":4.3, "y":0.0, "heading":-0.00002, "vx":0.9998, "vy":0.0, "omega":0.00097, "ax":0.0, "ay":0.0, "alpha":0.00183, "fx":[-0.00441,-0.00441,0.00441,0.00441], "fy":[0.00452,-0.00452,-0.00453,0.00452]},
+    {"t":4.85099, "x":4.35, "y":0.0, "heading":0.00003, "vx":0.9998, "vy":0.0, "omega":0.00107, "ax":0.0, "ay":0.0, "alpha":0.02102, "fx":[0.08606,-0.28009,0.09701,0.09701], "fy":[0.00534,-0.00534,-0.00534,0.00534]},
+    {"t":4.901, "x":4.4, "y":0.0, "heading":0.00008, "vx":0.9998, "vy":0.0, "omega":0.00212, "ax":0.0, "ay":0.0, "alpha":0.00372, "fx":[-0.00892,-0.00892,0.00898,0.00898], "fy":[0.00917,-0.00917,-0.00916,0.00917]},
+    {"t":4.95101, "x":4.45, "y":0.0, "heading":0.00019, "vx":0.9998, "vy":0.0, "omega":0.0023, "ax":-0.00899, "ay":0.0, "alpha":-0.00661, "fx":[-0.2199,0.00956,-0.19446,-0.19446], "fy":[0.01242,-0.01243,-0.01243,0.01242]},
+    {"t":5.00102, "x":4.49999, "y":0.0, "heading":0.0003, "vx":0.99935, "vy":0.0, "omega":0.00197, "ax":-0.99419, "ay":0.0, "alpha":0.006, "fx":[-16.58714,-16.58714,-16.55829,-16.55829], "fy":[0.01477,-0.01478,-0.01476,0.01477]},
+    {"t":5.05103, "x":4.54872, "y":0.0, "heading":0.0004, "vx":0.94963, "vy":0.0, "omega":0.00227, "ax":-0.99763, "ay":0.0, "alpha":-0.02999, "fx":[-16.83337,-16.09669,-16.7949,-16.7949], "fy":[0.01881,-0.01881,-0.01881,0.01881]},
+    {"t":5.10104, "x":4.59497, "y":0.0, "heading":0.00051, "vx":0.89974, "vy":0.0, "omega":0.00077, "ax":-0.99851, "ay":0.0, "alpha":0.00763, "fx":[-16.66302,-16.66302,-16.62628,-16.62628], "fy":[0.01882,-0.01883,-0.0188,0.01882]},
+    {"t":5.15105, "x":4.63871, "y":0.0, "heading":0.00055, "vx":0.8498, "vy":0.0, "omega":0.00115, "ax":-0.99891, "ay":0.0, "alpha":-0.02646, "fx":[-16.84564,-16.1563,-16.80181,-16.80181], "fy":[0.02143,-0.02143,-0.02143,0.02143]},
+    {"t":5.20106, "x":4.67996, "y":0.0, "heading":0.00061, "vx":0.79985, "vy":0.0, "omega":-0.00017, "ax":-0.99914, "ay":0.0, "alpha":0.00835, "fx":[-16.67535,-16.67535,-16.63516,-16.63516], "fy":[0.02059,-0.0206,-0.02058,0.02059]},
+    {"t":5.25107, "x":4.71872, "y":0.0, "heading":0.0006, "vx":0.74988, "vy":0.0, "omega":0.00025, "ax":-0.99929, "ay":0.0, "alpha":-0.01953, "fx":[-16.81976,-16.26147,-16.77491,-16.77491], "fy":[0.02193,-0.02193,-0.02193,0.02193]},
+    {"t":5.30108, "x":4.75497, "y":0.0, "heading":0.00061, "vx":0.69991, "vy":0.0, "omega":-0.00073, "ax":-0.9994, "ay":0.0, "alpha":0.00844, "fx":[-16.67982,-16.67982,-16.63922,-16.63922], "fy":[0.0208,-0.0208,-0.02079,0.0208]},
+    {"t":5.35109, "x":4.78872, "y":0.0, "heading":0.00058, "vx":0.64993, "vy":0.0, "omega":-0.00031, "ax":-0.99948, "ay":0.0, "alpha":-0.0144, "fx":[-16.79597,-16.34261,-16.75235,-16.75235], "fy":[0.02133,-0.02133,-0.02133,0.02133]},
+    {"t":5.4011, "x":4.81997, "y":0.0, "heading":0.00056, "vx":0.59994, "vy":0.0, "omega":-0.00103, "ax":-0.99954, "ay":0.0, "alpha":0.00815, "fx":[-16.68144,-16.68144,-16.64221,-16.64221], "fy":[0.0201,-0.02011,-0.0201,0.0201]},
+    {"t":5.45111, "x":4.84873, "y":0.0, "heading":0.00051, "vx":0.54995, "vy":0.0, "omega":-0.00062, "ax":-0.99959, "ay":0.0, "alpha":-0.01077, "fx":[-16.77648,-16.40346,-16.73526,-16.73526], "fy":[0.02015,-0.02015,-0.02015,0.02015]},
+    {"t":5.50112, "x":4.87498, "y":0.0, "heading":0.00048, "vx":0.49996, "vy":0.0, "omega":-0.00116, "ax":-0.99962, "ay":0.0, "alpha":0.00768, "fx":[-16.68174,-16.68174,-16.64478,-16.64478], "fy":[0.01895,-0.01895,-0.01894,0.01895]},
+    {"t":5.55113, "x":4.89873, "y":0.0, "heading":0.00042, "vx":0.44997, "vy":0.0, "omega":-0.00077, "ax":-0.99966, "ay":0.0, "alpha":-0.00829, "fx":[-16.76126,-16.44805,-16.72294,-16.72294], "fy":[0.01873,-0.01873,-0.01873,0.01873]},
+    {"t":5.60114, "x":4.91999, "y":0.0, "heading":0.00038, "vx":0.39998, "vy":0.0, "omega":-0.00119, "ax":-0.99968, "ay":0.0, "alpha":0.00714, "fx":[-16.68141,-16.68141,-16.64709,-16.64709], "fy":[0.0176,-0.0176,-0.0176,0.0176]},
+    {"t":5.65115, "x":4.93874, "y":0.0, "heading":0.00032, "vx":0.34999, "vy":0.0, "omega":-0.00083, "ax":-0.99971, "ay":0.0, "alpha":-0.00654, "fx":[-16.74906,-16.48199,-16.71375,-16.71375], "fy":[0.01725,-0.01725,-0.01725,0.01725]},
+    {"t":5.70116, "x":4.95499, "y":0.0, "heading":0.00028, "vx":0.29999, "vy":0.0, "omega":-0.00116, "ax":-0.99973, "ay":0.0, "alpha":0.00658, "fx":[-16.68079,-16.68079,-16.64915,-16.64915], "fy":[0.01623,-0.01623,-0.01623,0.01623]},
+    {"t":5.75117, "x":4.96874, "y":0.0, "heading":0.00022, "vx":0.24999, "vy":0.0, "omega":-0.00083, "ax":-0.99974, "ay":0.0, "alpha":-0.00531, "fx":[-16.73944,-16.5076,-16.707,-16.707], "fy":[0.01585,-0.01585,-0.01585,0.01585]},
+    {"t":5.80118, "x":4.98, "y":0.0, "heading":0.00018, "vx":0.2, "vy":0.0, "omega":-0.00109, "ax":-0.99976, "ay":0.0, "alpha":0.00606, "fx":[-16.68008,-16.68008,-16.65095,-16.65095], "fy":[0.01495,-0.01495,-0.01494,0.01495]},
+    {"t":5.85119, "x":4.98875, "y":0.0, "heading":0.00013, "vx":0.15, "vy":0.0, "omega":-0.00079, "ax":-0.99977, "ay":0.0, "alpha":-0.00444, "fx":[-16.73178,-16.52731,-16.70193,-16.70193], "fy":[0.01458,-0.01458,-0.01458,0.01458]},
+    {"t":5.9012, "x":4.995, "y":0.0, "heading":0.00009, "vx":0.1, "vy":0.0, "omega":-0.00101, "ax":-0.99979, "ay":0.0, "alpha":0.0056, "fx":[-16.67939,-16.67939,-16.65249,-16.65249], "fy":[0.01381,-0.01381,-0.01381,0.01381]},
+    {"t":5.95121, "x":4.99875, "y":0.0, "heading":0.00004, "vx":0.05, "vy":0.0, "omega":-0.00073, "ax":-0.9998, "ay":0.0, "alpha":0.01467, "fx":[-16.72458,-16.72458,-16.69857,-16.51676], "fy":[0.01301,-0.01301,-0.01301,0.013]},
+    {"t":6.00122, "x":5.0, "y":0.0, "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/GToBarge.traj b/src/main/deploy/choreo/GToBarge.traj
new file mode 100644
index 00000000..1256a3e6
--- /dev/null
+++ b/src/main/deploy/choreo/GToBarge.traj
@@ -0,0 +1,54 @@
+{
+ "name":"GToBarge",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":5.7769125, "y":3.6225774894, "heading":3.141592653589793, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":7.7597, "y":4.3597, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"G.x", "val":5.713146}, "y":{"exp":"G.y", "val":3.7209}, "heading":{"exp":"G.heading", "val":3.141592653589793}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"barge.x", "val":7.7597}, "y":{"exp":"barge.y", "val":4.3597}, "heading":{"exp":"barge.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,0.9107],
+  "samples":[
+    {"t":0.0, "x":5.77691, "y":3.62258, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.6612, "ay":3.59165, "alpha":0.0, "fx":[164.33422,164.33422,164.33422,164.33422], "fy":[61.09301,61.09301,61.09301,61.09301]},
+    {"t":0.04793, "x":5.78801, "y":3.6267, "heading":3.14159, "vx":0.46308, "vy":0.17215, "omega":0.0, "ax":9.65998, "ay":3.5912, "alpha":0.0, "fx":[164.31345,164.31345,164.31345,164.31345], "fy":[61.08529,61.08529,61.08529,61.08529]},
+    {"t":0.09586, "x":5.8213, "y":3.63908, "heading":3.14159, "vx":0.9261, "vy":0.34429, "omega":0.0, "ax":9.65835, "ay":3.5906, "alpha":0.0, "fx":[164.28577,164.28577,164.28577,164.28577], "fy":[61.075,61.075,61.075,61.075]},
+    {"t":0.1438, "x":5.87679, "y":3.65971, "heading":3.14159, "vx":1.38904, "vy":0.51639, "omega":0.0, "ax":9.65607, "ay":3.58975, "alpha":0.0, "fx":[164.24704,164.24704,164.24704,164.24704], "fy":[61.0606,61.0606,61.0606,61.0606]},
+    {"t":0.19173, "x":5.95446, "y":3.68858, "heading":3.14159, "vx":1.85187, "vy":0.68845, "omega":0.0, "ax":9.65266, "ay":3.58848, "alpha":0.0, "fx":[164.18901,164.18901,164.18901,164.18901], "fy":[61.03903,61.03903,61.03903,61.03903]},
+    {"t":0.23966, "x":6.05431, "y":3.7257, "heading":3.14159, "vx":2.31454, "vy":0.86045, "omega":0.0, "ax":9.64699, "ay":3.58637, "alpha":0.0, "fx":[164.09249,164.09249,164.09249,164.09249], "fy":[61.00314,61.00314,61.00314,61.00314]},
+    {"t":0.28759, "x":6.17633, "y":3.77107, "heading":3.14159, "vx":2.77693, "vy":1.03236, "omega":0.0, "ax":9.63568, "ay":3.58217, "alpha":0.0, "fx":[163.90022,163.90022,163.90022,163.90022], "fy":[60.93166,60.93166,60.93166,60.93166]},
+    {"t":0.33552, "x":6.3205, "y":3.82466, "heading":3.14159, "vx":3.23879, "vy":1.20405, "omega":0.0, "ax":9.60219, "ay":3.56972, "alpha":0.0, "fx":[163.33057,163.33057,163.33057,163.33057], "fy":[60.71989,60.71989,60.71989,60.71989]},
+    {"t":0.38345, "x":6.48677, "y":3.88648, "heading":3.14159, "vx":3.69904, "vy":1.37516, "omega":0.0, "ax":6.78138, "ay":2.52105, "alpha":0.0, "fx":[115.3494,115.3494,115.3494,115.3494], "fy":[42.88238,42.88238,42.88238,42.88238]},
+    {"t":0.43139, "x":6.67187, "y":3.95529, "heading":3.14159, "vx":4.02408, "vy":1.496, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.47932, "x":6.86475, "y":4.02699, "heading":3.14159, "vx":4.02408, "vy":1.496, "omega":0.0, "ax":-6.78138, "ay":-2.52105, "alpha":0.0, "fx":[-115.3494,-115.3494,-115.3494,-115.3494], "fy":[-42.88238,-42.88238,-42.88238,-42.88238]},
+    {"t":0.52725, "x":7.04984, "y":4.0958, "heading":3.14159, "vx":3.69904, "vy":1.37516, "omega":0.0, "ax":-9.60219, "ay":-3.56972, "alpha":0.0, "fx":[-163.33057,-163.33057,-163.33057,-163.33057], "fy":[-60.71989,-60.71989,-60.71989,-60.71989]},
+    {"t":0.57518, "x":7.21611, "y":4.15761, "heading":3.14159, "vx":3.23879, "vy":1.20405, "omega":0.0, "ax":-9.63568, "ay":-3.58217, "alpha":0.0, "fx":[-163.90022,-163.90022,-163.90022,-163.90022], "fy":[-60.93166,-60.93166,-60.93166,-60.93166]},
+    {"t":0.62311, "x":7.36028, "y":4.21121, "heading":3.14159, "vx":2.77693, "vy":1.03236, "omega":0.0, "ax":-9.64699, "ay":-3.58637, "alpha":0.0, "fx":[-164.09249,-164.09249,-164.09249,-164.09249], "fy":[-61.00314,-61.00314,-61.00314,-61.00314]},
+    {"t":0.67104, "x":7.4823, "y":4.25657, "heading":3.14159, "vx":2.31454, "vy":0.86045, "omega":0.0, "ax":-9.65266, "ay":-3.58848, "alpha":0.0, "fx":[-164.18901,-164.18901,-164.18901,-164.18901], "fy":[-61.03903,-61.03903,-61.03903,-61.03903]},
+    {"t":0.71898, "x":7.58215, "y":4.2937, "heading":3.14159, "vx":1.85187, "vy":0.68845, "omega":0.0, "ax":-9.65607, "ay":-3.58975, "alpha":0.0, "fx":[-164.24704,-164.24704,-164.24704,-164.24704], "fy":[-61.0606,-61.0606,-61.0606,-61.0606]},
+    {"t":0.76691, "x":7.65983, "y":4.32257, "heading":3.14159, "vx":1.38904, "vy":0.51639, "omega":0.0, "ax":-9.65835, "ay":-3.5906, "alpha":0.0, "fx":[-164.28577,-164.28577,-164.28577,-164.28577], "fy":[-61.075,-61.075,-61.075,-61.075]},
+    {"t":0.81484, "x":7.71531, "y":4.3432, "heading":3.14159, "vx":0.9261, "vy":0.34429, "omega":0.0, "ax":-9.65998, "ay":-3.5912, "alpha":0.0, "fx":[-164.31345,-164.31345,-164.31345,-164.31345], "fy":[-61.08529,-61.08529,-61.08529,-61.08529]},
+    {"t":0.86277, "x":7.7486, "y":4.35557, "heading":3.14159, "vx":0.46308, "vy":0.17215, "omega":0.0, "ax":-9.6612, "ay":-3.59165, "alpha":0.0, "fx":[-164.33422,-164.33422,-164.33422,-164.33422], "fy":[-61.09301,-61.09301,-61.09301,-61.09301]},
+    {"t":0.9107, "x":7.7597, "y":4.3597, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+  "splits":[0]
+ },
+ "events":[]
+}
diff --git a/src/main/deploy/choreo/GTofetch.traj b/src/main/deploy/choreo/GTofetch.traj
new file mode 100644
index 00000000..14545995
--- /dev/null
+++ b/src/main/deploy/choreo/GTofetch.traj
@@ -0,0 +1,121 @@
+{
+ "name":"GTofetch",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":5.713146, "y":3.7209, "heading":3.141592653589793, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":6.068316459655762, "y":4.486484527587891, "heading":0.0, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":5.048557281494141, "y":5.589489936828613, "heading":0.0, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":2.7798967361450195, "y":6.786823749542236, "heading":-0.9225007574586108, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"G.x", "val":5.713146}, "y":{"exp":"G.y", "val":3.7209}, "heading":{"exp":"G.heading", "val":3.141592653589793}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"6.068316459655762 m", "val":6.068316459655762}, "y":{"exp":"4.486484527587891 m", "val":4.486484527587891}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":{"exp":"5.048557281494141 m", "val":5.048557281494141}, "y":{"exp":"5.589489936828613 m", "val":5.589489936828613}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":{"exp":"2.7798967361450195 m", "val":2.7798967361450195}, "y":{"exp":"6.786823749542236 m", "val":6.786823749542236}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,0.48762,0.94415,1.62519,2.12926],
+  "samples":[
+    {"t":0.0, "x":5.71315, "y":3.7209, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":1.87481, "ay":4.52134, "alpha":47.80775, "fx":[199.06009,135.69912,-17.24657,-192.50424], "fy":[-3.76206,145.81172,198.36807,-38.94351]},
+    {"t":0.02709, "x":5.71383, "y":3.72256, "heading":3.14159, "vx":0.05079, "vy":0.12248, "omega":1.2951, "ax":1.89954, "ay":5.17512, "alpha":45.50636, "fx":[199.04786,135.81052,-13.3846,-194.81589], "fy":[0.05386,145.66319,198.62064,0.72931]},
+    {"t":0.05418, "x":5.71591, "y":3.72778, "heading":-3.10651, "vx":0.10225, "vy":0.26268, "omega":2.52786, "ax":2.59085, "ay":6.95399, "alpha":36.58248, "fx":[198.7419,133.36554,-10.58214,-148.77242], "fy":[9.86664,147.84157,198.71781,107.25284]},
+    {"t":0.08127, "x":5.71963, "y":3.73744, "heading":-3.03803, "vx":0.17243, "vy":0.45106, "omega":3.51888, "ax":7.41353, "ay":6.81703, "alpha":21.44169, "fx":[197.35101,128.80461,-7.21228,175.37656], "fy":[24.79746,151.73817,198.75152,79.25912]},
+    {"t":0.10836, "x":5.72702, "y":3.75216, "heading":-2.9427, "vx":0.37326, "vy":0.63573, "omega":4.09973, "ax":7.54381, "ay":7.04127, "alpha":20.5181, "fx":[193.57105,123.36444,2.75211,183.31936], "fy":[45.16021,156.03833,198.65595,69.64374]},
+    {"t":0.13545, "x":5.7399, "y":3.77197, "heading":-2.83164, "vx":0.57762, "vy":0.82648, "omega":4.65556, "ax":7.69808, "ay":7.80898, "alpha":16.03514, "fx":[184.33855,120.39847,35.30655,173.24953], "fy":[73.64583,158.02056,195.10349,93.91814]},
+    {"t":0.16254, "x":5.75837, "y":3.79722, "heading":-2.70552, "vx":0.78616, "vy":1.03802, "omega":5.08995, "ax":8.33681, "ay":8.37434, "alpha":3.45658, "fx":[153.13645,133.11266,122.693,146.9403], "fy":[125.15733,146.37868,154.90574,131.94317]},
+    {"t":0.18963, "x":5.78273, "y":3.82842, "heading":-2.56764, "vx":1.01201, "vy":1.26488, "omega":5.18359, "ax":7.74355, "ay":7.59173, "alpha":-16.90046, "fx":[45.62291,184.46597,176.6839,109.55241], "fy":[190.73306,63.25545,87.89108,164.32231]},
+    {"t":0.21672, "x":5.81298, "y":3.86547, "heading":-2.42722, "vx":1.22178, "vy":1.47054, "omega":4.72576, "ax":5.8403, "ay":8.93467, "alpha":-17.76608, "fx":[-6.37844,164.95639,157.32065,73.52134], "fy":[195.11023,100.00344,117.95035,182.6826]},
+    {"t":0.24381, "x":5.84822, "y":3.90858, "heading":-2.2992, "vx":1.37999, "vy":1.71258, "omega":4.24448, "ax":1.59343, "ay":10.17101, "alpha":-18.89311, "fx":[-104.13862,82.4896,120.57594,7.31981], "fy":[164.51466,163.82625,153.71019,196.13238]},
+    {"t":0.2709, "x":5.88619, "y":3.95871, "heading":-2.18421, "vx":1.42316, "vy":1.98811, "omega":3.73267, "ax":1.50526, "ay":9.10582, "alpha":-24.69419, "fx":[-126.75458,120.11283,116.49874,-9.48931], "fy":[143.22332,116.27783,153.68229,193.97538]},
+    {"t":0.29799, "x":5.9253, "y":4.01591, "heading":-2.0831, "vx":1.46393, "vy":2.23478, "omega":3.0637, "ax":0.17244, "ay":9.02743, "alpha":-21.83478, "fx":[-124.89815,71.76599,90.81638,-26.18634], "fy":[130.953,124.72393,160.62013,185.63446]},
+    {"t":0.32508, "x":5.96502, "y":4.07976, "heading":-2.0001, "vx":1.4686, "vy":2.47933, "omega":2.4722, "ax":-9.09843, "ay":3.69371, "alpha":3.91571, "fx":[-142.47752,-148.63286,-159.98465,-155.57089], "fy":[75.62452,76.93398,49.82913,43.9021]},
+    {"t":0.35217, "x":6.00146, "y":4.14828, "heading":-1.93313, "vx":1.22213, "vy":2.5794, "omega":2.57828, "ax":-10.14427, "ay":-1.47543, "alpha":14.48461, "fx":[-169.58754,-180.62563,-182.85799,-143.32937], "fy":[16.30248,46.09192,-46.26932,-114.5039]},
+    {"t":0.37926, "x":6.03085, "y":4.21761, "heading":-1.86328, "vx":0.94732, "vy":2.53943, "omega":2.97066, "ax":-10.64955, "ay":-1.68025, "alpha":13.29365, "fx":[-183.32955,-188.28387,-186.0067,-152.47116], "fy":[15.50791,36.94249,-51.40521,-113.08135]},
+    {"t":0.40635, "x":6.0526, "y":4.28579, "heading":-1.78281, "vx":0.65883, "vy":2.49391, "omega":3.33079, "ax":-11.24635, "ay":-0.94353, "alpha":9.55158, "fx":[-189.99601,-192.05367,-190.79656,-177.03846], "fy":[22.68595,29.88248,-39.1751,-76.30586]},
+    {"t":0.43344, "x":6.06633, "y":4.353, "heading":-1.69258, "vx":0.35417, "vy":2.46835, "omega":3.58954, "ax":-11.68619, "ay":-0.1224, "alpha":3.33254, "fx":[-194.6774,-195.19842,-195.26434,-194.07233], "fy":[12.80514,14.49478,-14.48494,-20.9761]},
+    {"t":0.46053, "x":6.07163, "y":4.41982, "heading":-1.59534, "vx":0.03759, "vy":2.46503, "omega":3.67982, "ax":-11.8108, "ay":-0.32027, "alpha":-2.72346, "fx":[-196.80111,-196.25968,-197.09144,-197.36954], "fy":[-16.88724,-19.90256,7.90248,7.53241]},
+    {"t":0.48762, "x":6.06832, "y":4.48648, "heading":-1.49565, "vx":-0.28236, "vy":2.45636, "omega":3.60604, "ax":-11.72365, "ay":-0.66556, "alpha":-5.03667, "fx":[-194.75778,-193.38745,-196.31638,-197.249], "fy":[-32.63073,-37.3356,16.05827,9.52967]},
+    {"t":0.50936, "x":6.05941, "y":4.53973, "heading":-1.41726, "vx":-0.53723, "vy":2.44189, "omega":3.49654, "ax":-11.55908, "ay":-0.2719, "alpha":-8.38077, "fx":[-192.95982,-190.78264,-190.99487,-195.99987], "fy":[-40.75833,-45.656,45.74834,22.53613]},
+    {"t":0.5311, "x":6.045, "y":4.59275, "heading":-1.34125, "vx":-0.78852, "vy":2.43598, "omega":3.31435, "ax":-11.33029, "ay":0.14144, "alpha":-11.3489, "fx":[-191.06007,-188.78271,-181.09504,-194.54419], "fy":[-47.51787,-49.06135,74.58102,31.42942]},
+    {"t":0.55284, "x":6.02518, "y":4.64574, "heading":-1.26919, "vx":-1.03484, "vy":2.43905, "omega":3.06762, "ax":-11.06754, "ay":0.4795, "alpha":-13.7812, "fx":[-188.81665,-187.0906,-168.75556,-193.29958], "fy":[-54.1993,-49.0598,98.35998,36.8712]},
+    {"t":0.57458, "x":6.00006, "y":4.69888, "heading":-1.2025, "vx":-1.27545, "vy":2.44948, "omega":2.76802, "ax":-10.79554, "ay":0.69942, "alpha":-15.68707, "fx":[-185.93223,-184.98213,-156.49965,-192.41161], "fy":[-61.58434,-47.1657,115.93163,39.45408]},
+    {"t":0.59632, "x":5.96979, "y":4.75229, "heading":-1.14233, "vx":-1.51014, "vy":2.46468, "omega":2.42699, "ax":-10.52538, "ay":0.80727, "alpha":-17.15473, "fx":[-182.2572,-181.63025,-146.08693,-191.83789], "fy":[-69.58104,-44.01915,127.72051,39.70688]},
+    {"t":0.61806, "x":5.93447, "y":4.80607, "heading":-1.08957, "vx":-1.73896, "vy":2.48223, "omega":2.05405, "ax":-10.25639, "ay":0.83528, "alpha":-18.26998, "fx":[-177.87957,-176.07982,-138.52239,-191.39454], "fy":[-77.37828,-39.43815,134.40584,38.10536]},
+    {"t":0.6398, "x":5.89424, "y":4.86023, "heading":-1.04491, "vx":-1.96193, "vy":2.50039, "omega":1.65687, "ax":-9.98293, "ay":0.81135, "alpha":-19.06192, "fx":[-173.0884,-167.4096,-134.3818,-190.76294], "fy":[-83.73222,-33.33566,136.19537,34.97181]},
+    {"t":0.66153, "x":5.84923, "y":4.91478, "heading":-1.00889, "vx":-2.17895, "vy":2.51803, "omega":1.24247, "ax":-9.70329, "ay":0.69468, "alpha":-19.4098, "fx":[-167.9723,-155.55233,-134.10855,-189.36322], "fy":[-87.34007,-28.48529,132.04366,30.10161]},
+    {"t":0.68327, "x":5.79957, "y":4.96968, "heading":-0.98188, "vx":-2.3899, "vy":2.53313, "omega":0.8205, "ax":-9.38299, "ay":0.26126, "alpha":-18.78086, "fx":[-160.92006,-140.74857,-138.42709,-185.54408], "fy":[-87.28317,-32.9363,116.47472,21.16477]},
+    {"t":0.70501, "x":5.74539, "y":5.02481, "heading":-0.96404, "vx":-2.59388, "vy":2.53881, "omega":0.41221, "ax":-8.5023, "ay":-1.42673, "alpha":-14.43191, "fx":[-139.93249,-114.50625,-144.01096,-168.46708], "fy":[-84.93529,-57.52626,54.83634,-7.50631]},
+    {"t":0.72675, "x":5.68699, "y":5.07967, "heading":-0.95508, "vx":-2.77872, "vy":2.50779, "omega":0.09847, "ax":-4.24135, "ay":-3.70395, "alpha":-2.11135, "fx":[-70.40703,-63.38916,-71.17587,-77.83283], "fy":[-68.73628,-64.7013,-54.5113,-59.02366]},
+    {"t":0.74849, "x":5.62558, "y":5.13331, "heading":-0.95294, "vx":-2.87093, "vy":2.42727, "omega":0.05257, "ax":-1.57003, "ay":-1.80933, "alpha":-0.1438, "fx":[-26.23539,-25.67717,-26.10808,-26.66587], "fy":[-30.64276,-30.26257,-29.67826,-30.05937]},
+    {"t":0.77023, "x":5.5628, "y":5.18565, "heading":-0.9518, "vx":-2.90506, "vy":2.38794, "omega":0.04944, "ax":-0.54799, "ay":-0.65726, "alpha":-0.02649, "fx":[-9.14941,-9.04459,-9.12005,-9.22486], "fy":[-11.04608,-10.97187,-10.86647,-10.94069]},
+    {"t":0.79197, "x":5.49951, "y":5.23741, "heading":-0.95072, "vx":-2.91697, "vy":2.37365, "omega":0.04886, "ax":-0.20276, "ay":-0.24583, "alpha":-0.00745, "fx":[-3.38416,-3.35462,-3.37573,-3.40527], "fy":[-4.12317,-4.10211,-4.07254,-4.0936]},
+    {"t":0.81371, "x":5.43605, "y":5.28896, "heading":-0.94966, "vx":-2.92138, "vy":2.3683, "omega":0.0487, "ax":-0.11758, "ay":-0.14331, "alpha":-0.00325, "fx":[-1.96178,-1.94889,-1.95812,-1.971], "fy":[-2.4,-2.39078,-2.37789,-2.38711]},
+    {"t":0.83545, "x":5.37251, "y":5.34041, "heading":-0.9486, "vx":-2.92394, "vy":2.36519, "omega":0.04863, "ax":-0.18328, "ay":-0.22375, "alpha":-0.00622, "fx":[-3.05865,-3.03405,-3.05171,-3.07631], "fy":[-3.75096,-3.73333,-3.70872,-3.72635]},
+    {"t":0.85719, "x":5.30891, "y":5.39177, "heading":-0.94754, "vx":-2.92792, "vy":2.36032, "omega":0.0485, "ax":-0.48038, "ay":-0.58958, "alpha":-0.01914, "fx":[-8.01802,-7.94246,-7.99723,-8.07279], "fy":[-9.89303,-9.83895,-9.76308,-9.81716]},
+    {"t":0.87893, "x":5.24514, "y":5.44295, "heading":-0.94649, "vx":-2.93837, "vy":2.34751, "omega":0.04808, "ax":-1.33636, "ay":-1.67099, "alpha":-0.05142, "fx":[-22.29969,-22.0996,-22.2534,-22.45344], "fy":[-28.02734,-27.88836,-27.68196,-27.82104]},
+    {"t":0.90067, "x":5.18094, "y":5.49359, "heading":-0.94544, "vx":-2.96742, "vy":2.31118, "omega":0.04696, "ax":-3.14526, "ay":-4.12827, "alpha":-0.09124, "fx":[-52.42638,-52.09505,-52.43384,-52.76464], "fy":[-69.10178,-68.9204,-68.53056,-68.71264]},
+    {"t":0.92241, "x":5.11569, "y":5.54286, "heading":-0.94442, "vx":-3.03579, "vy":2.22143, "omega":0.04498, "ax":-4.80508, "ay":-7.02145, "alpha":0.13817, "fx":[-80.23162,-80.68898,-79.96671,-79.50604], "fy":[-116.69559,-116.76157,-117.39203,-117.32787]},
+    {"t":0.94415, "x":5.04856, "y":5.58949, "heading":-0.94345, "vx":-3.14026, "vy":2.06879, "omega":0.04798, "ax":-4.71975, "ay":-7.17713, "alpha":-0.63359, "fx":[-78.08639,-75.90067,-79.30057,-81.41627], "fy":[-121.18246,-120.94164,-118.07836,-118.35456]},
+    {"t":0.96937, "x":4.96785, "y":5.63939, "heading":-0.94224, "vx":-3.2593, "vy":1.88775, "omega":0.032, "ax":-2.17374, "ay":-3.84089, "alpha":-0.06535, "fx":[-36.24693,-35.99318,-36.22358,-36.47717], "fy":[-64.22842,-64.08459,-63.82318,-63.96731]},
+    {"t":0.9946, "x":4.88494, "y":5.68578, "heading":-0.94143, "vx":-3.31413, "vy":1.79087, "omega":0.03035, "ax":-0.64258, "ay":-1.19343, "alpha":-0.01356, "fx":[-10.71827,-10.66493,-10.70465,-10.75799], "fy":[-19.93956,-19.90154,-19.84811,-19.88614]},
+    {"t":1.01982, "x":4.80115, "y":5.73058, "heading":-0.94066, "vx":-3.33034, "vy":1.76077, "omega":0.03001, "ax":-0.17158, "ay":-0.32355, "alpha":-0.00294, "fx":[-2.86179,-2.85021,-2.85867,-2.87024], "fy":[-5.40346,-5.39504,-5.38346,-5.39189]},
+    {"t":1.04505, "x":4.71709, "y":5.77489, "heading":-0.93991, "vx":-3.33467, "vy":1.75261, "omega":0.02994, "ax":-0.0456, "ay":-0.08643, "alpha":-0.00043, "fx":[-0.76028,-0.7586,-0.75983,-0.76151], "fy":[-1.44223,-1.44099,-1.43931,-1.44054]},
+    {"t":1.07027, "x":4.63296, "y":5.81907, "heading":-0.93915, "vx":-3.33582, "vy":1.75043, "omega":0.02993, "ax":-0.01206, "ay":-0.02299, "alpha":0.00022, "fx":[-0.20087,-0.20172,-0.2011,-0.20024], "fy":[-0.38242,-0.38305,-0.3839,-0.38328]},
+    {"t":1.09549, "x":4.54882, "y":5.86321, "heading":-0.9384, "vx":-3.33612, "vy":1.74985, "omega":0.02993, "ax":-0.00311, "ay":-0.00604, "alpha":0.00039, "fx":[-0.05168,-0.05321,-0.05209,-0.05056], "fy":[-0.09941,-0.10053,-0.10206,-0.10094]},
+    {"t":1.12072, "x":4.46467, "y":5.90735, "heading":-0.93764, "vx":-3.3362, "vy":1.7497, "omega":0.02994, "ax":-0.00073, "ay":-0.00152, "alpha":0.00044, "fx":[-0.01186,-0.01358,-0.01232,-0.01061], "fy":[-0.02385,-0.02511,-0.02683,-0.02557]},
+    {"t":1.14594, "x":4.38052, "y":5.95148, "heading":-0.93689, "vx":-3.33622, "vy":1.74966, "omega":0.02995, "ax":-0.00009, "ay":-0.00031, "alpha":0.00045, "fx":[-0.00123,-0.003,-0.0017,0.00006], "fy":[-0.00368,-0.00498,-0.00675,-0.00545]},
+    {"t":1.17116, "x":4.29636, "y":5.99561, "heading":-0.93613, "vx":-3.33622, "vy":1.74965, "omega":0.02996, "ax":0.00008, "ay":0.00001, "alpha":0.00046, "fx":[0.00161,-0.00018,0.00114,0.00292], "fy":[0.00171,0.00039,-0.00139,-0.00008]},
+    {"t":1.19639, "x":4.21221, "y":6.03974, "heading":-0.93537, "vx":-3.33622, "vy":1.74965, "omega":0.02997, "ax":0.00013, "ay":0.0001, "alpha":0.00046, "fx":[0.00237,0.00057,0.0019,0.00369], "fy":[0.00315,0.00183,0.00003,0.00135]},
+    {"t":1.22161, "x":4.12806, "y":6.08388, "heading":-0.93462, "vx":-3.33622, "vy":1.74965, "omega":0.02999, "ax":0.00014, "ay":0.00012, "alpha":0.00046, "fx":[0.00257,0.00077,0.0021,0.0039], "fy":[0.00354,0.00221,0.00041,0.00174]},
+    {"t":1.24683, "x":4.04391, "y":6.12801, "heading":-0.93386, "vx":-3.33621, "vy":1.74966, "omega":0.03, "ax":0.00014, "ay":0.00012, "alpha":0.00046, "fx":[0.00263,0.00083,0.00216,0.00397], "fy":[0.00364,0.00231,0.0005,0.00184]},
+    {"t":1.27206, "x":3.95976, "y":6.17214, "heading":-0.93311, "vx":-3.33621, "vy":1.74966, "omega":0.03001, "ax":0.00014, "ay":0.00013, "alpha":0.00046, "fx":[0.00265,0.00084,0.00218,0.00399], "fy":[0.00367,0.00233,0.00052,0.00186]},
+    {"t":1.29728, "x":3.87561, "y":6.21627, "heading":-0.93235, "vx":-3.33621, "vy":1.74966, "omega":0.03002, "ax":0.00015, "ay":0.00013, "alpha":0.00046, "fx":[0.00265,0.00084,0.00218,0.004], "fy":[0.00368,0.00233,0.00051,0.00186]},
+    {"t":1.3225, "x":3.79146, "y":6.26041, "heading":-0.93159, "vx":-3.3362, "vy":1.74967, "omega":0.03003, "ax":0.00014, "ay":0.00012, "alpha":0.00047, "fx":[0.00264,0.00082,0.00218,0.004], "fy":[0.00365,0.0023,0.00048,0.00183]},
+    {"t":1.34773, "x":3.70731, "y":6.30454, "heading":-0.93083, "vx":-3.3362, "vy":1.74967, "omega":0.03004, "ax":0.00014, "ay":0.00012, "alpha":0.00047, "fx":[0.00259,0.00077,0.00213,0.00395], "fy":[0.00356,0.0022,0.00037,0.00173]},
+    {"t":1.37295, "x":3.62316, "y":6.34867, "heading":-0.93008, "vx":-3.3362, "vy":1.74967, "omega":0.03006, "ax":0.00013, "ay":0.00009, "alpha":0.00047, "fx":[0.00239,0.00056,0.00193,0.00376], "fy":[0.00317,0.00181,-0.00002,0.00134]},
+    {"t":1.39818, "x":3.539, "y":6.39281, "heading":-0.92932, "vx":-3.33619, "vy":1.74968, "omega":0.03007, "ax":0.00008, "ay":0.00001, "alpha":0.00047, "fx":[0.00163,-0.0002,0.00117,0.003], "fy":[0.00173,0.00036,-0.00147,-0.0001]},
+    {"t":1.4234, "x":3.45485, "y":6.43694, "heading":-0.92856, "vx":-3.33619, "vy":1.74968, "omega":0.03008, "ax":-0.00009, "ay":-0.00032, "alpha":0.00047, "fx":[-0.00122,-0.00305,-0.00168,0.00014], "fy":[-0.0037,-0.00506,-0.00688,-0.00552]},
+    {"t":1.44862, "x":3.3707, "y":6.48107, "heading":-0.9278, "vx":-3.33619, "vy":1.74967, "omega":0.03009, "ax":-0.00073, "ay":-0.00153, "alpha":0.00046, "fx":[-0.01194,-0.01372,-0.01238,-0.0106], "fy":[-0.02403,-0.02536,-0.02714,-0.02581]},
+    {"t":1.47385, "x":3.28655, "y":6.5252, "heading":-0.92704, "vx":-3.33621, "vy":1.74963, "omega":0.0301, "ax":-0.00314, "ay":-0.0061, "alpha":0.00041, "fx":[-0.05207,-0.05367,-0.05247,-0.05088], "fy":[-0.10021,-0.10141,-0.103,-0.1018]},
+    {"t":1.49907, "x":3.2024, "y":6.56933, "heading":-0.92628, "vx":-3.33629, "vy":1.74948, "omega":0.03011, "ax":-0.01215, "ay":-0.02318, "alpha":0.00023, "fx":[-0.2024,-0.2033,-0.20263,-0.20172], "fy":[-0.3856,-0.38628,-0.38718,-0.38651]},
+    {"t":1.52429, "x":3.11824, "y":6.61345, "heading":-0.92552, "vx":-3.3366, "vy":1.74889, "omega":0.03012, "ax":-0.04587, "ay":-0.08719, "alpha":-0.00044, "fx":[-0.76489,-0.7632,-0.76447,-0.76617], "fy":[-1.45493,-1.45365,-1.45196,-1.45323]},
+    {"t":1.54952, "x":3.03407, "y":6.65754, "heading":-0.92476, "vx":-3.33775, "vy":1.74669, "omega":0.03011, "ax":-0.17156, "ay":-0.32695, "alpha":-0.00289, "fx":[-2.86123,-2.84998,-2.85848,-2.86973], "fy":[-5.4599,-5.45143,-5.44018,-5.44865]},
+    {"t":1.57474, "x":2.94982, "y":6.70149, "heading":-0.924, "vx":-3.34208, "vy":1.73844, "omega":0.03003, "ax":-0.63035, "ay":-1.21208, "alpha":-0.01857, "fx":[-10.51583,-10.44369,-10.49938,-10.57153], "fy":[-20.26749,-20.21416,-20.14193,-20.19526]},
+    {"t":1.59996, "x":2.86532, "y":6.74496, "heading":-0.92325, "vx":-3.35798, "vy":1.70787, "omega":0.02957, "ax":-2.28282, "ay":-3.81255, "alpha":-1.17181, "fx":[-38.17231,-33.7139,-37.96411,-42.36402], "fy":[-67.19922,-64.58208,-59.85957,-62.57262]},
+    {"t":1.62519, "x":2.7799, "y":6.78682, "heading":-0.9225, "vx":-3.41556, "vy":1.61171, "omega":0.00001, "ax":-1.48681, "ay":-3.24268, "alpha":-0.00026, "fx":[-24.78453,-24.78353,-24.78439,-24.78539], "fy":[-54.05477,-54.05413,-54.05315,-54.05379]},
+    {"t":1.66119, "x":2.65595, "y":6.84275, "heading":-0.9225, "vx":-3.4691, "vy":1.49495, "omega":0.0, "ax":-0.19635, "ay":-0.45859, "alpha":0.0, "fx":[-3.27306,-3.27306,-3.27306,-3.27306], "fy":[-7.64447,-7.64447,-7.64447,-7.64447]},
+    {"t":1.6972, "x":2.53092, "y":6.89628, "heading":-0.9225, "vx":-3.47617, "vy":1.47844, "omega":0.0, "ax":-0.02492, "ay":-0.05863, "alpha":0.0, "fx":[-0.41535,-0.41535,-0.41535,-0.41535], "fy":[-0.97741,-0.97741,-0.97741,-0.97741]},
+    {"t":1.7332, "x":2.40574, "y":6.94948, "heading":-0.9225, "vx":-3.47706, "vy":1.47633, "omega":0.0, "ax":-0.00317, "ay":-0.00748, "alpha":0.0, "fx":[-0.05291,-0.05291,-0.05291,-0.05291], "fy":[-0.12467,-0.12467,-0.12467,-0.12467]},
+    {"t":1.76921, "x":2.28055, "y":7.00263, "heading":-0.9225, "vx":-3.47718, "vy":1.47606, "omega":0.0, "ax":0.00414, "ay":-0.0029, "alpha":0.0, "fx":[0.0691,0.0691,0.0691,0.06909], "fy":[-0.04842,-0.04842,-0.04842,-0.04842]},
+    {"t":1.80521, "x":2.15535, "y":7.05577, "heading":-0.9225, "vx":-3.47703, "vy":1.47595, "omega":0.0, "ax":8.77032, "ay":-3.723, "alpha":-0.00005, "fx":[146.19687,146.19706,146.19707,146.19688], "fy":[-62.0609,-62.06057,-62.06035,-62.06067]},
+    {"t":1.84122, "x":2.03585, "y":7.1065, "heading":-0.9225, "vx":-3.16125, "vy":1.34191, "omega":0.0, "ax":10.90842, "ay":-4.63048, "alpha":0.0, "fx":[181.83821,181.83819,181.83818,181.8382], "fy":[-77.18783,-77.18787,-77.18789,-77.18785]},
+    {"t":1.87723, "x":1.92909, "y":7.15181, "heading":-0.9225, "vx":-2.76849, "vy":1.17518, "omega":0.0, "ax":10.9582, "ay":-4.6516, "alpha":0.00001, "fx":[182.66787,182.66785,182.66784,182.66786], "fy":[-77.53995,-77.54,-77.54003,-77.53998]},
+    {"t":1.91323, "x":1.83652, "y":7.19111, "heading":-0.9225, "vx":-2.37393, "vy":1.0077, "omega":0.0, "ax":10.97525, "ay":-4.65884, "alpha":0.00001, "fx":[182.95208,182.95206,182.95205,182.95207], "fy":[-77.66058,-77.66063,-77.66066,-77.66061]},
+    {"t":1.94924, "x":1.75816, "y":7.22437, "heading":-0.9225, "vx":-1.97876, "vy":0.83996, "omega":0.0, "ax":10.98385, "ay":-4.66249, "alpha":0.00001, "fx":[183.09556,183.09554,183.09553,183.09555], "fy":[-77.72147,-77.72152,-77.72155,-77.7215]},
+    {"t":1.98524, "x":1.69403, "y":7.2516, "heading":-0.9225, "vx":-1.58328, "vy":0.67208, "omega":0.0, "ax":10.98904, "ay":-4.6647, "alpha":0.00001, "fx":[183.18207,183.18205,183.18203,183.18206], "fy":[-77.75818,-77.75824,-77.75827,-77.75822]},
+    {"t":2.02125, "x":1.64415, "y":7.27277, "heading":-0.9225, "vx":-1.18762, "vy":0.50413, "omega":0.0, "ax":10.99251, "ay":-4.66617, "alpha":0.00001, "fx":[183.23991,183.23988,183.23987,183.23989], "fy":[-77.78273,-77.78279,-77.78282,-77.78276]},
+    {"t":2.05725, "x":1.60851, "y":7.2879, "heading":-0.9225, "vx":-0.79183, "vy":0.33612, "omega":0.0, "ax":10.99499, "ay":-4.66722, "alpha":0.00001, "fx":[183.2813,183.28128,183.28126,183.28129], "fy":[-77.8003,-77.80036,-77.80039,-77.80033]},
+    {"t":2.09326, "x":1.58713, "y":7.29697, "heading":-0.9225, "vx":-0.39595, "vy":0.16807, "omega":0.0, "ax":10.99686, "ay":-4.66801, "alpha":0.00001, "fx":[183.31239,183.31236,183.31235,183.31237], "fy":[-77.81349,-77.81355,-77.81358,-77.81352]},
+    {"t":2.12926, "x":1.58, "y":7.3, "heading":-0.9225, "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/HToBarge.traj b/src/main/deploy/choreo/HToBarge.traj
new file mode 100644
index 00000000..d9ff2e9e
--- /dev/null
+++ b/src/main/deploy/choreo/HToBarge.traj
@@ -0,0 +1,53 @@
+{
+ "name":"HToBarge",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":5.713146, "y":4.0509, "heading":3.141592653589793, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":7.7597, "y":4.3597, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"H.x", "val":5.713146}, "y":{"exp":"H.y", "val":4.0509}, "heading":{"exp":"H.heading", "val":3.141592653589793}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"barge.x", "val":7.7597}, "y":{"exp":"barge.y", "val":4.3597}, "heading":{"exp":"barge.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,0.86594],
+  "samples":[
+    {"t":0.0, "x":5.71315, "y":4.0509, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":11.81633, "ay":1.78294, "alpha":0.0, "fx":[196.97252,196.97252,196.97252,196.97252], "fy":[29.72075,29.72075,29.72075,29.72075]},
+    {"t":0.04811, "x":5.72682, "y":4.05296, "heading":3.14159, "vx":0.56846, "vy":0.08577, "omega":0.0, "ax":11.8142, "ay":1.78262, "alpha":0.0, "fx":[196.93707,196.93707,196.93707,196.93707], "fy":[29.7154,29.7154,29.7154,29.7154]},
+    {"t":0.09622, "x":5.76784, "y":4.05915, "heading":3.14159, "vx":1.13681, "vy":0.17153, "omega":0.0, "ax":11.81101, "ay":1.78214, "alpha":0.0, "fx":[196.88392,196.88392,196.88392,196.88392], "fy":[29.70738,29.70738,29.70738,29.70738]},
+    {"t":0.14432, "x":5.8362, "y":4.06947, "heading":3.14159, "vx":1.70502, "vy":0.25727, "omega":0.0, "ax":11.8057, "ay":1.78134, "alpha":0.0, "fx":[196.79538,196.79538,196.79538,196.79538], "fy":[29.69402,29.69402,29.69402,29.69402]},
+    {"t":0.19243, "x":5.93188, "y":4.0839, "heading":3.14159, "vx":2.27296, "vy":0.34296, "omega":0.0, "ax":11.79509, "ay":1.77974, "alpha":0.0, "fx":[196.61854,196.61854,196.61854,196.61854], "fy":[29.66734,29.66734,29.66734,29.66734]},
+    {"t":0.24054, "x":6.05488, "y":4.10246, "heading":3.14159, "vx":2.8404, "vy":0.42858, "omega":0.0, "ax":11.76343, "ay":1.77496, "alpha":0.0, "fx":[196.09071,196.09071,196.09071,196.09071], "fy":[29.58769,29.58769,29.58769,29.58769]},
+    {"t":0.28865, "x":6.20514, "y":4.12514, "heading":3.14159, "vx":3.40631, "vy":0.51397, "omega":0.0, "ax":6.85699, "ay":1.03464, "alpha":0.0, "fx":[114.30265,114.30265,114.30265,114.30265], "fy":[17.24687,17.24687,17.24687,17.24687]},
+    {"t":0.33675, "x":6.37694, "y":4.15106, "heading":3.14159, "vx":3.73619, "vy":0.56375, "omega":0.0, "ax":0.00061, "ay":0.00009, "alpha":0.0, "fx":[0.01009,0.01009,0.01009,0.01009], "fy":[0.00152,0.00152,0.00152,0.00152]},
+    {"t":0.38486, "x":6.55668, "y":4.17818, "heading":3.14159, "vx":3.73622, "vy":0.56375, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.43297, "x":6.73642, "y":4.2053, "heading":3.14159, "vx":3.73622, "vy":0.56375, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.48108, "x":6.91616, "y":4.23242, "heading":3.14159, "vx":3.73622, "vy":0.56375, "omega":0.0, "ax":-0.00061, "ay":-0.00009, "alpha":0.0, "fx":[-0.01009,-0.01009,-0.01009,-0.01009], "fy":[-0.00152,-0.00152,-0.00152,-0.00152]},
+    {"t":0.52919, "x":7.09591, "y":4.25954, "heading":3.14159, "vx":3.73619, "vy":0.56375, "omega":0.0, "ax":-6.85699, "ay":-1.03464, "alpha":0.0, "fx":[-114.30265,-114.30265,-114.30265,-114.30265], "fy":[-17.24687,-17.24687,-17.24687,-17.24687]},
+    {"t":0.57729, "x":7.26771, "y":4.28546, "heading":3.14159, "vx":3.40631, "vy":0.51397, "omega":0.0, "ax":-11.76343, "ay":-1.77496, "alpha":0.0, "fx":[-196.09071,-196.09071,-196.09071,-196.09071], "fy":[-29.58769,-29.58769,-29.58769,-29.58769]},
+    {"t":0.6254, "x":7.41797, "y":4.30814, "heading":3.14159, "vx":2.8404, "vy":0.42858, "omega":0.0, "ax":-11.79509, "ay":-1.77974, "alpha":0.0, "fx":[-196.61854,-196.61854,-196.61854,-196.61854], "fy":[-29.66734,-29.66734,-29.66734,-29.66734]},
+    {"t":0.67351, "x":7.54096, "y":4.3267, "heading":3.14159, "vx":2.27296, "vy":0.34296, "omega":0.0, "ax":-11.8057, "ay":-1.78134, "alpha":0.0, "fx":[-196.79538,-196.79538,-196.79538,-196.79538], "fy":[-29.69402,-29.69402,-29.69402,-29.69402]},
+    {"t":0.72162, "x":7.63665, "y":4.34113, "heading":3.14159, "vx":1.70502, "vy":0.25727, "omega":0.0, "ax":-11.81101, "ay":-1.78214, "alpha":0.0, "fx":[-196.88392,-196.88392,-196.88392,-196.88392], "fy":[-29.70738,-29.70738,-29.70738,-29.70738]},
+    {"t":0.76973, "x":7.70501, "y":4.35145, "heading":3.14159, "vx":1.13681, "vy":0.17153, "omega":0.0, "ax":-11.8142, "ay":-1.78262, "alpha":0.0, "fx":[-196.93707,-196.93707,-196.93707,-196.93707], "fy":[-29.7154,-29.7154,-29.7154,-29.7154]},
+    {"t":0.81783, "x":7.74603, "y":4.35764, "heading":3.14159, "vx":0.56846, "vy":0.08577, "omega":0.0, "ax":-11.81633, "ay":-1.78294, "alpha":0.0, "fx":[-196.97252,-196.97252,-196.97252,-196.97252], "fy":[-29.72075,-29.72075,-29.72075,-29.72075]},
+    {"t":0.86594, "x":7.7597, "y":4.3597, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+  "splits":[0]
+ },
+ "events":[]
+}
diff --git a/src/main/deploy/choreo/HTofetch.traj b/src/main/deploy/choreo/HTofetch.traj
new file mode 100644
index 00000000..1259d4ad
--- /dev/null
+++ b/src/main/deploy/choreo/HTofetch.traj
@@ -0,0 +1,118 @@
+{
+ "name":"HTofetch",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":5.713146, "y":4.0509, "heading":3.141592653589793, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":5.946532726287842, "y":4.711103439331055, "heading":0.0, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":5.048557281494141, "y":5.589489936828613, "heading":-1.773711057838377, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":2.7798967361450195, "y":6.786823749542236, "heading":-0.9225007574586108, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"H.x", "val":5.713146}, "y":{"exp":"H.y", "val":4.0509}, "heading":{"exp":"H.heading", "val":3.141592653589793}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"5.946532726287842 m", "val":5.946532726287842}, "y":{"exp":"4.711103439331055 m", "val":4.711103439331055}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":24, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":{"exp":"5.048557281494141 m", "val":5.048557281494141}, "y":{"exp":"5.589489936828613 m", "val":5.589489936828613}, "heading":{"exp":"-1.773711057838377 rad", "val":-1.773711057838377}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"2.7798967361450195 m", "val":2.7798967361450195}, "y":{"exp":"6.786823749542236 m", "val":6.786823749542236}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,0.40545,0.78804,1.56044,2.06453],
+  "samples":[
+    {"t":0.0, "x":5.71315, "y":4.0509, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":3.48725, "ay":7.66008, "alpha":31.62453, "fx":[199.01044,137.64181,-3.35502,-100.77411], "fy":[5.64204,143.97036,199.07484,162.07218]},
+    {"t":0.02534, "x":5.71427, "y":4.05336, "heading":3.14159, "vx":0.08837, "vy":0.19411, "omega":0.80139, "ax":4.42969, "ay":8.10886, "alpha":26.83778, "fx":[198.84886,137.07195,-1.74327,-38.81406], "fy":[8.90218,144.47371,199.05716,188.25044]},
+    {"t":0.05068, "x":5.71793, "y":4.06088, "heading":-3.12129, "vx":0.20062, "vy":0.39959, "omega":1.48147, "ax":5.66188, "ay":8.22824, "alpha":22.01529, "fx":[198.3863,134.87446,-1.12531,45.38771], "fy":[15.55031,146.47515,199.00686,187.61064]},
+    {"t":0.07602, "x":5.72483, "y":4.07365, "heading":-3.08374, "vx":0.3441, "vy":0.6081, "omega":2.03935, "ax":6.56355, "ay":8.01438, "alpha":19.56386, "fx":[197.28103,131.20131,-0.76773,109.93016], "fy":[25.51133,149.70255,198.92892,160.24066]},
+    {"t":0.10136, "x":5.73566, "y":4.09163, "heading":-3.03207, "vx":0.51042, "vy":0.81119, "omega":2.53511, "ax":6.89129, "ay":7.95842, "alpha":18.5762, "fx":[194.89881,126.14819,0.69033,137.76096], "fy":[39.29289,153.88011,198.80813,138.67069]},
+    {"t":0.1267, "x":5.7508, "y":4.11474, "heading":-2.96782, "vx":0.68505, "vy":1.01286, "omega":3.00585, "ax":6.86441, "ay":8.26602, "alpha":17.21531, "fx":[189.7361,119.60068,5.91157,142.45715], "fy":[58.84513,158.8538,198.52285,134.94048]},
+    {"t":0.15204, "x":5.77037, "y":4.14306, "heading":-2.89165, "vx":0.859, "vy":1.22233, "omega":3.44209, "ax":6.60864, "ay":8.96938, "alpha":14.04677, "fx":[176.81565,111.07357,21.54326,131.21885], "fy":[89.83647,164.60495,197.07299,146.54685]},
+    {"t":0.17738, "x":5.79426, "y":4.17692, "heading":-2.80443, "vx":1.02647, "vy":1.44962, "omega":3.79805, "ax":5.97748, "ay":10.11147, "alpha":5.30917, "fx":[129.43177,98.33678,67.24058,103.55756], "fy":[149.24577,171.60903,185.69632,167.66215]},
+    {"t":0.20272, "x":5.82219, "y":4.2169, "heading":-2.70818, "vx":1.17794, "vy":1.70585, "omega":3.93258, "ax":2.25544, "ay":10.10698, "alpha":-18.83728, "fx":[-67.68189,0.23825,159.09183,58.74016], "fy":[184.62909,185.71672,115.17228,188.3962]},
+    {"t":0.22807, "x":5.85276, "y":4.26337, "heading":-2.60853, "vx":1.23509, "vy":1.96197, "omega":3.45524, "ax":-2.31199, "ay":10.37478, "alpha":-17.92516, "fx":[-108.15036,-117.40681,77.40927,-6.01084], "fy":[163.97076,152.08144,179.02501,196.69331]},
+    {"t":0.25341, "x":5.88332, "y":4.31642, "heading":-2.52097, "vx":1.17651, "vy":2.22487, "omega":3.001, "ax":-7.39319, "ay":8.54949, "alpha":-11.4404, "fx":[-153.35952,-170.71297,-76.24152,-92.64987], "fy":[123.11558,94.39106,179.17666,173.3806]},
+    {"t":0.27875, "x":5.91076, "y":4.37555, "heading":-2.44493, "vx":0.98916, "vy":2.44152, "omega":2.71109, "ax":-10.33468, "ay":5.50276, "alpha":-5.19513, "fx":[-179.25883,-187.01473,-165.02236,-157.80095], "fy":[82.05107,60.71499,106.26341,117.88394]},
+    {"t":0.30409, "x":5.9325, "y":4.43918, "heading":-2.37622, "vx":0.72727, "vy":2.58096, "omega":2.57945, "ax":-11.38081, "ay":3.22059, "alpha":-2.34262, "fx":[-191.14796,-193.67939,-188.63234,-185.39101], "fy":[50.18628,38.43336,57.93781,68.18524]},
+    {"t":0.32943, "x":5.94728, "y":4.50562, "heading":-2.31086, "vx":0.43887, "vy":2.66258, "omega":2.52008, "ax":-11.75006, "ay":1.66141, "alpha":-1.54123, "fx":[-196.3286,-197.18611,-195.63192,-194.32463], "fy":[25.88875,17.32059,29.73733,37.83277]},
+    {"t":0.35477, "x":5.95463, "y":4.57362, "heading":-2.247, "vx":0.14112, "vy":2.70468, "omega":2.48103, "ax":-11.87025, "ay":0.5349, "alpha":-1.81364, "fx":[-198.20471,-198.22904,-197.89167,-197.16013], "fy":[7.12905,-3.46881,10.95546,21.0501]},
+    {"t":0.38011, "x":5.95439, "y":4.64233, "heading":-2.18413, "vx":-0.15968, "vy":2.71823, "omega":2.43507, "ax":-11.87568, "ay":-0.34907, "alpha":-2.70086, "fx":[-198.3866,-196.97326,-198.36901,-198.11849], "fy":[-8.25377,-24.36053,-2.92155,12.2607]},
+    {"t":0.40545, "x":5.94653, "y":4.7111, "heading":-2.12242, "vx":-0.46062, "vy":2.70939, "omega":2.36663, "ax":-11.80686, "ay":-0.92047, "alpha":-3.70295, "fx":[-197.45433,-193.98996,-197.76424,-198.04997], "fy":[-18.93099,-40.71407,-10.87954,9.1497]},
+    {"t":0.42458, "x":5.93556, "y":4.76276, "heading":-2.07715, "vx":-0.68648, "vy":2.69178, "omega":2.29579, "ax":-11.73116, "ay":-1.08329, "alpha":-5.30372, "fx":[-196.81027,-190.40646,-197.5275,-197.467], "fy":[-24.01269,-54.46979,-9.94748,16.19804]},
+    {"t":0.44371, "x":5.92028, "y":4.81406, "heading":-2.03323, "vx":-0.91089, "vy":2.67105, "omega":2.19433, "ax":-11.63624, "ay":-1.21816, "alpha":-6.87746, "fx":[-196.01031,-185.9341,-197.27099,-196.66676], "fy":[-29.07524,-67.56648,-7.30918,22.72619]},
+    {"t":0.46284, "x":5.90073, "y":4.86493, "heading":-1.99125, "vx":-1.13349, "vy":2.64775, "omega":2.06277, "ax":-11.52615, "ay":-1.31705, "alpha":-8.39331, "fx":[-195.07841,-180.83432,-196.90239,-195.72651], "fy":[-33.9247,-79.55093,-2.81888,28.47624]},
+    {"t":0.48197, "x":5.87694, "y":4.91534, "heading":-1.95179, "vx":-1.35398, "vy":2.62256, "omega":1.90221, "ax":-11.4044, "ay":-1.37883, "alpha":-9.83529, "fx":[-194.03818,-175.39619,-196.26185,-194.72732], "fy":[-38.44421,-90.15133,3.38221,33.27552]},
+    {"t":0.5011, "x":5.84895, "y":4.96526, "heading":-1.91541, "vx":-1.57214, "vy":2.59618, "omega":1.71406, "ax":-11.27391, "ay":-1.41092, "alpha":-11.17954, "fx":[-192.90204,-169.90916,-195.16771,-193.74379], "fy":[-42.57631,-99.19427,10.72479,36.96823]},
+    {"t":0.52023, "x":5.81681, "y":5.01467, "heading":-1.88262, "vx":-1.78781, "vy":2.56919, "omega":1.5002, "ax":-11.13617, "ay":-1.4348, "alpha":-12.38515, "fx":[-191.64128,-164.54597,-193.51527,-192.83594], "fy":[-46.38669,-106.70297,18.05437,39.36545]},
+    {"t":0.53936, "x":5.78057, "y":5.06355, "heading":-1.85392, "vx":-2.00084, "vy":2.54174, "omega":1.26327, "ax":-10.98944, "ay":-1.49289, "alpha":-13.38507, "fx":[-190.13191,-159.19582,-191.37858,-192.0486], "fy":[-50.16329,-112.99868,23.50156,40.1174]},
+    {"t":0.55849, "x":5.74029, "y":5.1119, "heading":-1.82975, "vx":-2.21107, "vy":2.51318, "omega":1.00722, "ax":-10.82402, "ay":-1.66313, "alpha":-14.0615, "fx":[-188.04431,-153.28109,-188.98442,-191.41475], "fy":[-54.58244,-118.74754,24.06372,38.37212]},
+    {"t":0.57762, "x":5.69601, "y":5.15967, "heading":-1.81048, "vx":-2.41813, "vy":2.48137, "omega":0.73823, "ax":-10.60508, "ay":-2.10596, "alpha":-14.15054, "fx":[-184.53028,-145.52976,-186.17478,-190.8915], "fy":[-61.10425,-124.90291,13.82952,31.75652]},
+    {"t":0.59675, "x":5.64781, "y":5.20675, "heading":-1.79636, "vx":-2.621, "vy":2.44108, "omega":0.46753, "ax":-10.18945, "ay":-3.20276, "alpha":-12.89774, "fx":[-177.05615,-134.12031,-178.73257,-189.50391], "fy":[-73.0056,-131.78333,-21.11884,12.35358]},
+    {"t":0.61588, "x":5.59581, "y":5.25287, "heading":-1.78742, "vx":-2.81592, "vy":2.37981, "omega":0.2208, "ax":-8.90534, "ay":-5.61028, "alpha":-8.20716, "fx":[-155.37561,-119.33152,-145.71959,-173.364], "fy":[-98.21719,-135.07479,-91.26144,-49.52942]},
+    {"t":0.63501, "x":5.54031, "y":5.29736, "heading":-1.78319, "vx":-2.98628, "vy":2.27249, "omega":0.0638, "ax":-6.18204, "ay":-7.57561, "alpha":-1.37823, "fx":[-105.8448,-97.57218,-100.16263,-108.62662], "fy":[-125.98019,-131.33692,-126.75225,-121.05799]},
+    {"t":0.65414, "x":5.48205, "y":5.33945, "heading":-1.78197, "vx":-3.10454, "vy":2.12757, "omega":0.03744, "ax":-4.40397, "ay":-6.6642, "alpha":-0.03656, "fx":[-73.50819,-73.28844,-73.31593,-73.53586], "fy":[-111.10205,-111.2042,-111.07594,-110.97365]},
+    {"t":0.67327, "x":5.42186, "y":5.37893, "heading":-1.78126, "vx":-3.18878, "vy":2.00009, "omega":0.03674, "ax":-2.67836, "ay":-4.41696, "alpha":0.04954, "fx":[-44.50671,-44.77146,-44.78706,-44.52256], "fy":[-73.56761,-73.4825,-73.68979,-73.77484]},
+    {"t":0.6924, "x":5.36037, "y":5.41638, "heading":-1.78055, "vx":-3.24002, "vy":1.91559, "omega":0.03769, "ax":-1.33496, "ay":-2.30994, "alpha":0.03418, "fx":[-22.15469,-22.32306,-22.35153,-22.18322], "fy":[-38.44741,-38.40659,-38.56386,-38.60468]},
+    {"t":0.71153, "x":5.29814, "y":5.45261, "heading":-1.77983, "vx":-3.26556, "vy":1.8714, "omega":0.03834, "ax":-0.77077, "ay":-1.37315, "alpha":0.02741, "fx":[-12.76929,-12.90148,-12.92748,-12.79532], "fy":[-22.8399,-22.81051,-22.93951,-22.9689]},
+    {"t":0.73065, "x":5.23553, "y":5.48815, "heading":-1.7791, "vx":-3.2803, "vy":1.84514, "omega":0.03886, "ax":-0.76551, "ay":-1.56377, "alpha":0.24985, "fx":[-12.03758,-13.24893,-13.48285,-12.27366], "fy":[-25.61595,-25.34765,-26.51854,-26.78686]},
+    {"t":0.74978, "x":5.17264, "y":5.52316, "heading":-1.77836, "vx":-3.29495, "vy":1.81522, "omega":0.04364, "ax":1.3381, "ay":-4.02663, "alpha":8.13286, "fx":[50.15138,6.11883,-3.56993,36.52181], "fy":[-52.19017,-46.57461,-83.093,-86.62988]},
+    {"t":0.76891, "x":5.10985, "y":5.55715, "heading":-1.77752, "vx":-3.26935, "vy":1.73819, "omega":0.19922, "ax":6.80613, "ay":-4.9949, "alpha":21.83373, "fx":[174.18911,128.44925,27.03638,124.14487], "fy":[-28.87736,-6.22251,-167.07937,-130.8714]},
+    {"t":0.78804, "x":5.04856, "y":5.58949, "heading":-1.77371, "vx":-3.13915, "vy":1.64264, "omega":0.6169, "ax":2.15608, "ay":-5.27224, "alpha":11.23849, "fx":[78.43834,13.6894,-2.38958,54.02534], "fy":[-65.81397,-63.19064,-111.34924,-111.18928]},
+    {"t":0.81665, "x":4.95964, "y":5.63432, "heading":-1.75606, "vx":-3.07747, "vy":1.49182, "omega":0.9384, "ax":2.97361, "ay":-3.985, "alpha":11.06488, "fx":[86.51009,31.18575,14.17853,66.40052], "fy":[-42.56544,-38.13961,-93.34114,-91.66599]},
+    {"t":0.84526, "x":4.87282, "y":5.67537, "heading":-1.72922, "vx":-2.9924, "vy":1.37782, "omega":1.25494, "ax":3.0465, "ay":-2.94052, "alpha":9.50546, "fx":[79.73918,34.55576,22.51883,66.32103], "fy":[-27.90791,-23.20295,-72.84204,-72.11556]},
+    {"t":0.87387, "x":4.78846, "y":5.71358, "heading":-1.69332, "vx":-2.90525, "vy":1.2937, "omega":1.52686, "ax":2.67961, "ay":-1.7139, "alpha":6.8584, "fx":[63.74908,31.62275,25.83205,57.4671], "fy":[-13.1034,-9.61233,-45.37778,-46.18582]},
+    {"t":0.90247, "x":4.70644, "y":5.74989, "heading":-1.64964, "vx":-2.8286, "vy":1.24467, "omega":1.72306, "ax":1.89521, "ay":-0.3029, "alpha":3.32709, "fx":[40.15147,24.40712,23.0297,38.7804], "fy":[2.53398,3.92862,-12.84871,-13.81095]},
+    {"t":0.93108, "x":4.6263, "y":5.78537, "heading":-1.60035, "vx":-2.77438, "vy":1.236, "omega":1.81824, "ax":0.88478, "ay":0.9571, "alpha":-0.22048, "fx":[14.20179,15.277,15.29558,14.22132], "fy":[15.44047,15.39645,16.46805,16.51284]},
+    {"t":0.95969, "x":4.54729, "y":5.82112, "heading":-1.54833, "vx":-2.74907, "vy":1.26338, "omega":1.81193, "ax":-0.01322, "ay":1.74103, "alpha":-2.77606, "fx":[-7.03796,6.9028,6.46088,-7.20751], "fy":[22.25373,22.56017,35.78578,35.48885]},
+    {"t":0.98829, "x":4.46865, "y":5.85798, "heading":-1.4965, "vx":-2.74945, "vy":1.31319, "omega":1.73252, "ax":-0.6337, "ay":2.04811, "alpha":-4.14176, "fx":[-20.49911,0.54864,-0.99496,-21.30826], "fy":[23.47371,25.25514,44.84623,42.98911]},
+    {"t":1.0169, "x":4.38973, "y":5.89638, "heading":-1.44693, "vx":-2.76757, "vy":1.37178, "omega":1.61403, "ax":-0.97856, "ay":2.03158, "alpha":-4.59843, "fx":[-26.83402,-3.57099,-6.25424,-28.58956], "fy":[21.41301,24.6669,46.38031,43.00167]},
+    {"t":1.04551, "x":4.31016, "y":5.93646, "heading":-1.40076, "vx":-2.79557, "vy":1.4299, "omega":1.48248, "ax":-1.1, "ay":1.83449, "alpha":-4.45438, "fx":[-27.88126,-5.66065,-9.20033,-30.60376], "fy":[18.0257,22.18208,43.19058,38.92193]},
+    {"t":1.07412, "x":4.22974, "y":5.97811, "heading":-1.35835, "vx":-2.82704, "vy":1.48238, "omega":1.35506, "ax":-1.06149, "ay":1.5605, "alpha":-3.95723, "fx":[-25.55751,-6.16266,-10.11725,-28.94098], "fy":[14.51497,18.93904,37.54581,33.05136]},
+    {"t":1.10272, "x":4.14843, "y":6.02116, "heading":-1.31959, "vx":-2.8574, "vy":1.52702, "omega":1.24185, "ax":-0.92807, "ay":1.27875, "alpha":-3.30292, "fx":[-21.53973,-5.6377,-9.56962,-25.13499], "fy":[11.49159,15.71049,31.15606,26.90659]},
+    {"t":1.13133, "x":4.0663, "y":6.06537, "heading":-1.28406, "vx":-2.88395, "vy":1.5636, "omega":1.14736, "ax":-0.75682, "ay":1.02934, "alpha":-2.63615, "fx":[-17.10319,-4.61847,-8.21613,-20.52555], "fy":[9.16722,12.91645,25.15321,21.39729]},
+    {"t":1.15994, "x":3.98349, "y":6.11052, "heading":-1.25124, "vx":-2.9056, "vy":1.59305, "omega":1.07195, "ax":-0.58823, "ay":0.8288, "alpha":-2.045, "fx":[-13.04055,-3.49839,-6.61268,-16.07048], "fy":[7.51599,10.70333,20.11387,16.9295]},
+    {"t":1.18855, "x":3.90013, "y":6.15643, "heading":-1.22057, "vx":-2.92243, "vy":1.61676, "omega":1.01345, "ax":-0.44414, "ay":0.67772, "alpha":-1.56645, "fx":[-9.71249,-2.50239,-5.1144,-12.28523], "fy":[6.40337,9.04894,16.18854,13.548]},
+    {"t":1.21715, "x":3.81635, "y":6.20296, "heading":-1.19158, "vx":-2.93514, "vy":1.63614, "omega":0.96864, "ax":-0.33192, "ay":0.56858, "alpha":-1.20275, "fx":[-7.18663,-1.72171,-3.88841,-9.33486], "fy":[5.67307,7.85499,13.2808,11.10328]},
+    {"t":1.24576, "x":3.73224, "y":6.25, "heading":-1.16387, "vx":-2.94463, "vy":1.65241, "omega":0.93423, "ax":-0.25089, "ay":0.49169, "alpha":-0.93975, "fx":[-5.38704,-1.17076,-2.9819,-7.18902], "fy":[5.18968,7.00785,11.20115,9.38625]},
+    {"t":1.27437, "x":3.6479, "y":6.29747, "heading":-1.13714, "vx":-2.95181, "vy":1.66648, "omega":0.90734, "ax":-0.19784, "ay":0.43827, "alpha":-0.76049, "fx":[-4.20547,-0.83637,-2.39282,-5.75691], "fy":[4.84798,6.40794,9.76238,8.2048]},
+    {"t":1.30298, "x":3.56338, "y":6.34532, "heading":-1.11119, "vx":-2.95747, "vy":1.67901, "omega":0.88559, "ax":-0.17094, "ay":0.40167, "alpha":-0.65393, "fx":[-3.57381,-0.71375,-2.12677,-4.98359], "fy":[4.5628,5.978,8.82754,7.41418]},
+    {"t":1.33158, "x":3.4787, "y":6.39352, "heading":-1.08585, "vx":-2.96236, "vy":1.6905, "omega":0.86688, "ax":-0.17295, "ay":0.37736, "alpha":-0.62146, "fx":[-3.51941,-0.83679,-2.24781,-4.9277], "fy":[4.24667,5.65982,8.33346,6.92196]},
+    {"t":1.36019, "x":3.39389, "y":6.44203, "heading":-1.06105, "vx":-2.96731, "vy":1.7013, "omega":0.8491, "ax":-0.21543, "ay":0.36246, "alpha":-0.68537, "fx":[-4.23724,-1.31826,-2.94662,-5.86211], "fy":[3.77102,5.40299,8.31229,6.68216]},
+    {"t":1.3888, "x":3.30891, "y":6.49085, "heading":-1.03676, "vx":-2.97347, "vy":1.71167, "omega":0.8295, "ax":-0.32669, "ay":0.35443, "alpha":-0.90478, "fx":[-6.22766,-2.42664,-4.66759,-8.46117], "fy":[2.88851,5.13868,8.92646,6.6788]},
+    {"t":1.4174, "x":3.22372, "y":6.53996, "heading":-1.01303, "vx":-2.98281, "vy":1.72181, "omega":0.80361, "ax":-0.56873, "ay":0.34674, "alpha":-1.40828, "fx":[-10.59302,-4.75872,-8.38098,-14.18926], "fy":[1.04467,4.69756,10.51404,6.86371]},
+    {"t":1.44601, "x":3.13815, "y":6.58936, "heading":-0.99005, "vx":-2.99908, "vy":1.73173, "omega":0.76333, "ax":-1.07389, "ay":0.31095, "alpha":-2.45443, "fx":[-19.67732,-9.66092,-16.18987,-26.07662], "fy":[-3.15794,3.48755,13.53281,6.87142]},
+    {"t":1.47462, "x":3.05192, "y":6.63903, "heading":-0.96821, "vx":-3.02981, "vy":1.74062, "omega":0.69311, "ax":-2.11008, "ay":0.12164, "alpha":-4.49102, "fx":[-38.13463,-20.25568,-32.58011,-49.72569], "fy":[-13.59097,-0.88395,17.77593,4.80958]},
+    {"t":1.50323, "x":2.96438, "y":6.68887, "heading":-0.94838, "vx":-3.09017, "vy":1.7441, "omega":0.56464, "ax":-4.04878, "ay":-0.66416, "alpha":-7.85167, "fx":[-71.17881,-42.70998,-65.42349,-90.65235], "fy":[-39.72737,-17.75501,19.04196,-5.84457]},
+    {"t":1.53183, "x":2.87432, "y":6.73849, "heading":-0.93223, "vx":-3.20599, "vy":1.7251, "omega":0.34002, "ax":-6.62688, "ay":-2.49332, "alpha":-11.8812, "fx":[-108.24421,-76.5289,-117.36196,-139.73283], "fy":[-85.67455,-64.45791,12.64585,-28.7631]},
+    {"t":1.56044, "x":2.7799, "y":6.78682, "heading":-0.9225, "vx":-3.39557, "vy":1.65378, "omega":0.00013, "ax":-1.98563, "ay":-4.28048, "alpha":-0.00367, "fx":[-33.09988,-33.08547,-33.09905,-33.11346], "fy":[-71.36465,-71.35664,-71.34253,-71.35055]},
+    {"t":1.59645, "x":2.65635, "y":6.8436, "heading":-0.9225, "vx":-3.46707, "vy":1.49965, "omega":0.0, "ax":-0.2803, "ay":-0.65404, "alpha":0.0, "fx":[-4.67255,-4.67255,-4.67255,-4.67255], "fy":[-10.9026,-10.9026,-10.90259,-10.9026]},
+    {"t":1.63245, "x":2.53133, "y":6.89717, "heading":-0.9225, "vx":-3.47716, "vy":1.4761, "omega":0.0, "ax":-0.03554, "ay":-0.08381, "alpha":0.0, "fx":[-0.59239,-0.59239,-0.59239,-0.59239], "fy":[-1.39711,-1.39711,-1.39711,-1.39711]},
+    {"t":1.66846, "x":2.40611, "y":6.95026, "heading":-0.9225, "vx":-3.47844, "vy":1.47308, "omega":0.0, "ax":-0.00453, "ay":-0.01069, "alpha":0.0, "fx":[-0.07544,-0.07544,-0.07544,-0.07544], "fy":[-0.17821,-0.17821,-0.17821,-0.17821]},
+    {"t":1.70447, "x":2.28086, "y":7.0033, "heading":-0.9225, "vx":-3.4786, "vy":1.4727, "omega":0.0, "ax":0.00397, "ay":-0.00332, "alpha":0.0, "fx":[0.06612,0.06612,0.06612,0.06612], "fy":[-0.05526,-0.05526,-0.05526,-0.05526]},
+    {"t":1.74047, "x":2.15561, "y":7.05632, "heading":-0.9225, "vx":-3.47846, "vy":1.47258, "omega":0.0, "ax":8.772, "ay":-3.71372, "alpha":-0.00077, "fx":[146.2236,146.2263,146.22639,146.2237], "fy":[-61.9099,-61.90529,-61.90202,-61.90664]},
+    {"t":1.77648, "x":2.03605, "y":7.10693, "heading":-0.9225, "vx":-3.16261, "vy":1.33886, "omega":-0.00003, "ax":10.91291, "ay":-4.61989, "alpha":0.00007, "fx":[181.91317,181.91296,181.91284,181.91306], "fy":[-77.01096,-77.01146,-77.01175,-77.01125]},
+    {"t":1.81248, "x":1.92925, "y":7.15215, "heading":-0.9225, "vx":-2.76968, "vy":1.17252, "omega":-0.00003, "ax":10.96271, "ay":-4.64097, "alpha":0.00009, "fx":[182.74327,182.74298,182.74281,182.7431], "fy":[-77.36214,-77.36281,-77.36321,-77.36253]},
+    {"t":1.84849, "x":1.83663, "y":7.19136, "heading":-0.9225, "vx":-2.37495, "vy":1.00542, "omega":-0.00002, "ax":10.97976, "ay":-4.64819, "alpha":0.0001, "fx":[183.02762,183.0273,183.02712,183.02744], "fy":[-77.48243,-77.48317,-77.4836,-77.48286]},
+    {"t":1.8845, "x":1.75824, "y":7.22454, "heading":-0.9225, "vx":-1.97962, "vy":0.83805, "omega":-0.00002, "ax":10.98837, "ay":-4.65183, "alpha":0.0001, "fx":[183.17117,183.17084,183.17065,183.17098], "fy":[-77.54315,-77.54393,-77.54438,-77.5436]},
+    {"t":1.9205, "x":1.69408, "y":7.2517, "heading":-0.9225, "vx":-1.58397, "vy":0.67056, "omega":-0.00002, "ax":10.99357, "ay":-4.65403, "alpha":0.0001, "fx":[183.25772,183.25738,183.25718,183.25752], "fy":[-77.57977,-77.58056,-77.58102,-77.58023]},
+    {"t":1.95651, "x":1.64418, "y":7.27283, "heading":-0.9225, "vx":-1.18813, "vy":0.50298, "omega":-0.00001, "ax":10.99704, "ay":-4.6555, "alpha":0.0001, "fx":[183.31558,183.31524,183.31504,183.31538], "fy":[-77.60425,-77.60506,-77.60553,-77.60471]},
+    {"t":1.99251, "x":1.60852, "y":7.28792, "heading":-0.9225, "vx":-0.79217, "vy":0.33536, "omega":-0.00001, "ax":10.99952, "ay":-4.65655, "alpha":0.00011, "fx":[183.35699,183.35664,183.35644,183.35679], "fy":[-77.62176,-77.62259,-77.62306,-77.62224]},
+    {"t":2.02852, "x":1.58713, "y":7.29698, "heading":-0.9225, "vx":-0.39612, "vy":0.16769, "omega":0.0, "ax":11.00139, "ay":-4.65734, "alpha":0.00011, "fx":[183.38809,183.38774,183.38754,183.38789], "fy":[-77.63492,-77.63575,-77.63623,-77.6354]},
+    {"t":2.06453, "x":1.58, "y":7.3, "heading":-0.9225, "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/ITofetch.traj b/src/main/deploy/choreo/ITofetch.traj
new file mode 100644
index 00000000..259e8aaf
--- /dev/null
+++ b/src/main/deploy/choreo/ITofetch.traj
@@ -0,0 +1,86 @@
+{
+ "name":"ITofetch",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":5.365372248154253, "y":4.933261807735684, "heading":4.1887902047863905, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":5.302966594696045, "y":5.351784706115723, "heading":0.0, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"I.x", "val":5.365372248154253}, "y":{"exp":"I.y", "val":4.933261807735684}, "heading":{"exp":"I.heading", "val":4.1887902047863905}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"5.302966594696045 m", "val":5.302966594696045}, "y":{"exp":"5.351784706115723 m", "val":5.351784706115723}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,0.28199,1.59662],
+  "samples":[
+    {"t":0.0, "x":5.36537, "y":4.93326, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":1.33825, "ay":10.78681, "alpha":17.42556, "fx":[98.32084,-7.89077,-97.24014,96.042], "fy":[173.06566,198.94005,173.53589,173.70229]},
+    {"t":0.02014, "x":5.36564, "y":4.93545, "heading":-2.0944, "vx":0.02696, "vy":0.21727, "omega":0.35099, "ax":0.98965, "ay":10.89848, "alpha":16.68775, "fx":[93.53971,-11.32764,-99.51187,83.28783], "fy":[175.65719,198.74837,172.20667,180.07778]},
+    {"t":0.04028, "x":5.36639, "y":4.94204, "heading":-2.08733, "vx":0.04689, "vy":0.43679, "omega":0.68711, "ax":0.56841, "ay":11.00252, "alpha":15.94211, "fx":[87.29458,-15.94712,-102.3803,68.93357], "fy":[178.79805,198.40055,170.47242,185.95572]},
+    {"t":0.06043, "x":5.36745, "y":4.95307, "heading":-2.07349, "vx":0.05834, "vy":0.6584, "omega":1.00822, "ax":0.05735, "ay":11.09367, "alpha":15.15684, "fx":[79.20425,-21.90127,-105.8927,52.4134], "fy":[182.47325,197.79473,168.25907,191.17755]},
+    {"t":0.08057, "x":5.36863, "y":4.96858, "heading":-2.05318, "vx":0.05949, "vy":0.88185, "omega":1.31351, "ax":-0.56752, "ay":11.16169, "alpha":14.29371, "fx":[68.68638,-29.41207,-110.10198,32.98634], "fy":[186.62348,196.77236,165.46962,195.3745]},
+    {"t":0.10071, "x":5.36972, "y":4.9886, "heading":-2.02672, "vx":0.04806, "vy":1.10667, "omega":1.60141, "ax":-1.33778, "ay":11.18705, "alpha":13.30906, "fx":[54.83728,-38.80515,-115.06966,9.8369], "fy":[191.07429,195.07981,161.97439,197.80277]},
+    {"t":0.12085, "x":5.37041, "y":5.01316, "heading":-1.99447, "vx":0.02112, "vy":1.332, "omega":1.86948, "ax":-2.29213, "ay":11.1345, "alpha":12.15058, "fx":[36.23712,-50.56228,-120.86738,-17.64219], "fy":[195.36008,192.29358,157.59769,197.17567]},
+    {"t":0.14099, "x":5.37037, "y":5.04225, "heading":-1.95681, "vx":-0.02505, "vy":1.55627, "omega":2.11422, "ax":-3.47307, "ay":10.94386, "alpha":10.73731, "fx":[10.68446,-65.39858,-127.57432,-49.28938], "fy":[198.28206,187.6677,152.10009,191.66583]},
+    {"t":0.16114, "x":5.36916, "y":5.07582, "heading":-1.91423, "vx":-0.09501, "vy":1.7767, "omega":2.33049, "ax":-4.91876, "ay":10.51669, "alpha":8.90014, "fx":[-24.89285,-84.34906,-135.26422,-83.46714], "fy":[196.84682,179.81082,145.15827,179.41657]},
+    {"t":0.18128, "x":5.36625, "y":5.11374, "heading":-1.86728, "vx":-0.19408, "vy":1.98853, "omega":2.50976, "ax":-6.63806, "ay":9.69385, "alpha":6.30645, "fx":[-73.02326,-108.73545,-143.96898,-116.88539], "fy":[184.29857,166.00275,136.35276,159.71347]},
+    {"t":0.20142, "x":5.361, "y":5.15576, "heading":-1.81673, "vx":-0.32778, "vy":2.18378, "omega":2.63678, "ax":-8.52116, "ay":8.25039, "alpha":2.62434, "fx":[-129.42087,-139.39385,-153.60736,-145.75251], "fy":[149.97834,140.93511,125.18447,134.0219]},
+    {"t":0.22156, "x":5.35267, "y":5.20142, "heading":-1.76362, "vx":-0.49942, "vy":2.34996, "omega":2.68964, "ax":-10.20684, "ay":6.05701, "alpha":-1.88087, "fx":[-175.83929,-173.16194,-163.89796,-167.67338], "fy":[91.22459,95.96359,111.13003,105.55188]},
+    {"t":0.2417, "x":5.34054, "y":5.24998, "heading":-1.70945, "vx":-0.705, "vy":2.47196, "omega":2.65176, "ax":-11.23627, "ay":3.37185, "alpha":-6.54531, "fx":[-196.38903,-195.98801,-174.35155,-182.48408], "fy":[26.9507,26.72649,93.62575,77.52559]},
+    {"t":0.26185, "x":5.32406, "y":5.30045, "heading":-1.65604, "vx":-0.93133, "vy":2.53988, "omega":2.51992, "ax":-11.49796, "ay":0.85402, "alpha":-10.20558, "fx":[-197.20931,-193.03082,-184.81765,-191.60397], "fy":[-21.70095,-43.41882,70.59482,51.46909]},
+    {"t":0.28199, "x":5.30297, "y":5.35178, "heading":-1.60528, "vx":-1.16292, "vy":2.55708, "omega":2.31436, "ax":-11.34954, "ay":-0.06074, "alpha":-12.99525, "fx":[-194.2824,-183.17731,-185.37572,-193.93017], "fy":[-42.35329,-76.41324,70.81131,43.90487]},
+    {"t":0.31955, "x":5.25128, "y":5.44779, "heading":-1.51835, "vx":-1.58922, "vy":2.5548, "omega":1.82625, "ax":-11.16685, "ay":-0.32771, "alpha":-14.84387, "fx":[-191.33751,-177.34466,-181.28157,-194.62057], "fy":[-53.47901,-88.248,79.93527,39.94049]},
+    {"t":0.35711, "x":5.18371, "y":5.54352, "heading":-1.44975, "vx":-2.00865, "vy":2.54249, "omega":1.2687, "ax":-11.02516, "ay":-0.83264, "alpha":-15.75449, "fx":[-187.42785,-170.28588,-181.57916,-195.84369], "fy":[-64.88313,-99.88494,77.7645,31.4845]},
+    {"t":0.39467, "x":5.10049, "y":5.63843, "heading":-1.4021, "vx":-2.42277, "vy":2.51121, "omega":0.67695, "ax":-10.84569, "ay":-2.24015, "alpha":-14.86867, "fx":[-179.28323,-155.68311,-190.888,-197.31554], "fy":[-82.90396,-119.20802,44.21407,8.5288]},
+    {"t":0.43223, "x":5.00183, "y":5.73117, "heading":-1.37667, "vx":-2.83014, "vy":2.42707, "omega":0.11846, "ax":-7.95837, "ay":-8.34894, "alpha":-2.94045, "fx":[-131.21202,-118.07983,-135.16089,-146.1963], "fy":[-142.0539,-152.29291,-136.45718,-125.88759]},
+    {"t":0.46979, "x":4.88992, "y":5.81645, "heading":-1.37222, "vx":-3.12907, "vy":2.11348, "omega":0.00802, "ax":-5.5511, "ay":-9.22155, "alpha":-0.03265, "fx":[-92.55306,-92.36051,-92.51535,-92.70784], "fy":[-153.73246,-153.82446,-153.70528,-153.61313]},
+    {"t":0.50735, "x":4.76847, "y":5.88932, "heading":-1.37192, "vx":-3.33757, "vy":1.76711, "omega":0.00679, "ax":-2.71609, "ay":-5.531, "alpha":-0.00116, "fx":[-45.27778,-45.2716,-45.27406,-45.28024], "fy":[-92.20138,-92.20161,-92.19675,-92.19652]},
+    {"t":0.54492, "x":4.64119, "y":5.9518, "heading":-1.37167, "vx":-3.43959, "vy":1.55936, "omega":0.00675, "ax":-0.4024, "ay":-0.89921, "alpha":0.00016, "fx":[-6.70753,-6.70829,-6.70813,-6.70737], "fy":[-14.98888,-14.98903,-14.98979,-14.98963]},
+    {"t":0.58248, "x":4.51171, "y":6.00973, "heading":-1.37142, "vx":-3.45471, "vy":1.52558, "omega":0.00675, "ax":-0.04708, "ay":-0.1068, "alpha":0.00003, "fx":[-0.78477,-0.78492,-0.78489,-0.78474], "fy":[-1.78014,-1.78016,-1.78032,-1.78029]},
+    {"t":0.62004, "x":4.38192, "y":6.06696, "heading":-1.37116, "vx":-3.45648, "vy":1.52157, "omega":0.00676, "ax":-0.00553, "ay":-0.01257, "alpha":0.0, "fx":[-0.09223,-0.09222,-0.09222,-0.09222], "fy":[-0.20954,-0.20954,-0.20954,-0.20954]},
+    {"t":0.6576, "x":4.25209, "y":6.1241, "heading":-1.37091, "vx":-3.45668, "vy":1.5211, "omega":0.00676, "ax":-0.00065, "ay":-0.00148, "alpha":-0.00001, "fx":[-0.01086,-0.01083,-0.01084,-0.01086], "fy":[-0.02464,-0.02464,-0.02461,-0.02462]},
+    {"t":0.69516, "x":4.12225, "y":6.18124, "heading":-1.37065, "vx":-3.45671, "vy":1.52104, "omega":0.00676, "ax":-0.00008, "ay":-0.00017, "alpha":-0.00001, "fx":[-0.00128,-0.00125,-0.00126,-0.00129], "fy":[-0.00288,-0.00288,-0.00285,-0.00285]},
+    {"t":0.73272, "x":3.99241, "y":6.23837, "heading":-1.3704, "vx":-3.45671, "vy":1.52104, "omega":0.00676, "ax":-0.00001, "ay":-0.00002, "alpha":-0.00001, "fx":[-0.00016,-0.00013,-0.00013,-0.00016], "fy":[-0.00033,-0.00032,-0.00029,-0.0003]},
+    {"t":0.77028, "x":3.86258, "y":6.2955, "heading":-1.37015, "vx":-3.45671, "vy":1.52104, "omega":0.00676, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00002,0.0,0.0,-0.00003], "fy":[-0.00002,-0.00002,0.00001,0.00001]},
+    {"t":0.80784, "x":3.73274, "y":6.35263, "heading":-1.36989, "vx":-3.45671, "vy":1.52104, "omega":0.00675, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00001,0.00002,0.00001,-0.00001], "fy":[0.00001,0.00002,0.00005,0.00004]},
+    {"t":0.8454, "x":3.6029, "y":6.40976, "heading":-1.36964, "vx":-3.45671, "vy":1.52104, "omega":0.00675, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00001,0.00002,0.00002,-0.00001], "fy":[0.00002,0.00002,0.00005,0.00005]},
+    {"t":0.88296, "x":3.47306, "y":6.46689, "heading":-1.36939, "vx":-3.45671, "vy":1.52104, "omega":0.00675, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00001,0.00002,0.00002,-0.00001], "fy":[0.00002,0.00002,0.00005,0.00005]},
+    {"t":0.92052, "x":3.34323, "y":6.52403, "heading":-1.36913, "vx":-3.45671, "vy":1.52104, "omega":0.00675, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00001,0.00002,0.00002,-0.00001], "fy":[0.00002,0.00002,0.00005,0.00005]},
+    {"t":0.95809, "x":3.21339, "y":6.58116, "heading":-1.36888, "vx":-3.45671, "vy":1.52104, "omega":0.00675, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00001,0.00002,0.00001,-0.00001], "fy":[0.00001,0.00002,0.00005,0.00004]},
+    {"t":0.99565, "x":3.08355, "y":6.63829, "heading":-1.36862, "vx":-3.45671, "vy":1.52104, "omega":0.00675, "ax":0.0, "ay":0.0, "alpha":-0.00001, "fx":[-0.00003,0.0,-0.00001,-0.00004], "fy":[-0.00004,-0.00003,0.0,-0.00001]},
+    {"t":1.03321, "x":2.95371, "y":6.69542, "heading":-1.36837, "vx":-3.45671, "vy":1.52104, "omega":0.00675, "ax":-0.00001, "ay":-0.00002, "alpha":-0.00001, "fx":[-0.0002,-0.00017,-0.00018,-0.00021], "fy":[-0.00043,-0.00042,-0.00039,-0.0004]},
+    {"t":1.07077, "x":2.82388, "y":6.75255, "heading":-1.36812, "vx":-3.45671, "vy":1.52104, "omega":0.00675, "ax":-0.0001, "ay":-0.00022, "alpha":-0.00001, "fx":[-0.00167,-0.00164,-0.00165,-0.00167], "fy":[-0.00376,-0.00375,-0.00372,-0.00373]},
+    {"t":1.10833, "x":2.69404, "y":6.80968, "heading":-1.36786, "vx":-3.45671, "vy":1.52103, "omega":0.00675, "ax":-0.00085, "ay":-0.00192, "alpha":-0.00001, "fx":[-0.01413,-0.0141,-0.0141,-0.01413], "fy":[-0.03208,-0.03207,-0.03205,-0.03205]},
+    {"t":1.14589, "x":2.5642, "y":6.86681, "heading":-1.36761, "vx":-3.45675, "vy":1.52096, "omega":0.00675, "ax":-0.0072, "ay":-0.01636, "alpha":0.0, "fx":[-0.11996,-0.11996,-0.11996,-0.11996], "fy":[-0.27273,-0.27273,-0.27273,-0.27273]},
+    {"t":1.18345, "x":2.43436, "y":6.92393, "heading":-1.36736, "vx":-3.45702, "vy":1.52034, "omega":0.00675, "ax":-0.06091, "ay":-0.13913, "alpha":0.00036, "fx":[-1.01462,-1.01632,-1.01598,-1.01427], "fy":[-2.31828,-2.31863,-2.32033,-2.31998]},
+    {"t":1.22101, "x":2.30447, "y":6.98094, "heading":-1.3671, "vx":-3.4593, "vy":1.51511, "omega":0.00677, "ax":-0.15975, "ay":-1.32101, "alpha":0.88889, "fx":[-0.96359,-5.26094,-4.35308,-0.07405], "fy":[-19.49592,-20.35367,-24.54017,-23.69245]},
+    {"t":1.25857, "x":2.17442, "y":7.03691, "heading":-1.36685, "vx":-3.4653, "vy":1.4655, "omega":0.04015, "ax":9.15451, "ay":-3.26707, "alpha":22.58334, "fx":[192.11094,174.84769,71.63979,171.80666], "fy":[37.35986,21.92793,-181.56922,-95.56031]},
+    {"t":1.29613, "x":2.05072, "y":7.08966, "heading":-1.36534, "vx":-3.12145, "vy":1.34278, "omega":0.88841, "ax":9.46171, "ay":-3.00963, "alpha":22.58626, "fx":[193.74067,186.41718,76.21186,174.51865], "fy":[39.82576,35.60064,-182.23085,-93.87182]},
+    {"t":1.33369, "x":1.94015, "y":7.13797, "heading":-1.33197, "vx":-2.76606, "vy":1.22974, "omega":1.73677, "ax":10.47838, "ay":-4.52087, "alpha":11.36449, "fx":[198.16364,187.49701,134.41004,178.60757], "fy":[-7.62451,-61.48964,-145.66541,-86.66306]},
+    {"t":1.37126, "x":1.84364, "y":7.18097, "heading":-1.26674, "vx":-2.37248, "vy":1.05993, "omega":2.16363, "ax":10.90805, "ay":-4.78128, "alpha":-1.43772, "fx":[177.7531,181.71734,185.66404,182.19325], "fy":[-88.69767,-80.35221,-70.67001,-79.08645]},
+    {"t":1.40882, "x":1.76222, "y":7.21741, "heading":-1.18547, "vx":-1.96277, "vy":0.88034, "omega":2.10963, "ax":10.76029, "ay":-4.72261, "alpha":-7.02503, "fx":[157.38924,182.41201,196.08828,181.58583], "fy":[-121.58306,-79.46072,-33.19546,-80.65507]},
+    {"t":1.44638, "x":1.69609, "y":7.24714, "heading":-1.10623, "vx":-1.5586, "vy":0.70296, "omega":1.84576, "ax":10.59223, "ay":-4.67521, "alpha":-9.88779, "fx":[145.8387,184.20351,198.65991,177.56722], "fy":[-135.41729,-75.52706,-11.49334,-89.29648]},
+    {"t":1.48394, "x":1.64502, "y":7.27025, "heading":-1.0369, "vx":-1.16075, "vy":0.52735, "omega":1.47436, "ax":10.43802, "ay":-4.66674, "alpha":-11.74506, "fx":[139.82738,186.02802,199.03918,171.09232], "fy":[-141.73095,-71.09751,2.92568,-101.26674]},
+    {"t":1.5215, "x":1.60878, "y":7.28677, "heading":-0.98152, "vx":-0.76869, "vy":0.35206, "omega":1.03321, "ax":10.29502, "ay":-4.67891, "alpha":-13.1806, "fx":[136.70538,187.54592,198.67388,163.527], "fy":[-144.81818,-67.11385,13.12644,-113.17532]},
+    {"t":1.55906, "x":1.58717, "y":7.29669, "heading":-0.94271, "vx":-0.382, "vy":0.17632, "omega":0.53813, "ax":10.17001, "ay":-4.69419, "alpha":-14.32693, "fx":[134.91603,188.63887,198.09066,156.47144], "fy":[-146.53658,-64.06617,20.43019,-122.8268]},
+    {"t":1.59662, "x":1.58, "y":7.3, "heading":-0.9225, "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/JTofetch.traj b/src/main/deploy/choreo/JTofetch.traj
new file mode 100644
index 00000000..6fda7d82
--- /dev/null
+++ b/src/main/deploy/choreo/JTofetch.traj
@@ -0,0 +1,84 @@
+{
+ "name":"JTofetch",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":5.079583864905389, "y":5.098261807735684, "heading":4.1887902047863905, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":5.048037528991699, "y":5.385420322418213, "heading":0.0, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"J.x", "val":5.079583864905389}, "y":{"exp":"J.y", "val":5.098261807735684}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"5.048037528991699 m", "val":5.048037528991699}, "y":{"exp":"5.385420322418213 m", "val":5.385420322418213}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,0.23407,1.49933],
+  "samples":[
+    {"t":0.0, "x":5.07958, "y":5.09826, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":2.03058, "ay":10.68207, "alpha":17.55961, "fx":[104.8924,1.44305,-83.35832,112.41828], "fy":[169.12385,199.05367,180.53238,163.55007]},
+    {"t":0.01801, "x":5.07991, "y":5.09999, "heading":-2.0944, "vx":0.03656, "vy":0.19233, "omega":0.31616, "ax":1.58061, "ay":10.80398, "alpha":17.02636, "fx":[99.57186,-3.94683,-89.6233,99.39029], "fy":[172.26777,198.99368,177.47151,171.65552]},
+    {"t":0.03601, "x":5.08083, "y":5.10521, "heading":-2.0887, "vx":0.06502, "vy":0.38686, "omega":0.62273, "ax":1.02804, "ay":10.9224, "alpha":16.42696, "fx":[92.7234,-10.68987,-96.72699,83.2409], "fy":[175.99994,198.71514,173.66191,179.90775]},
+    {"t":0.05402, "x":5.08216, "y":5.11394, "heading":-2.07749, "vx":0.08353, "vy":0.58352, "omega":0.9185, "ax":0.3456, "ay":11.02493, "alpha":15.74144, "fx":[83.90712,-19.00169,-104.74457,62.88305], "fy":[180.31301,198.05752,168.90046,187.84989]},
+    {"t":0.07202, "x":5.08372, "y":5.12624, "heading":-2.06095, "vx":0.08975, "vy":0.78203, "omega":1.20193, "ax":-0.50049, "ay":11.08783, "alpha":14.95828, "fx":[72.47686,-29.15367,-113.71196,37.01713], "fy":[185.13682,196.77727,162.94402,194.4568]},
+    {"t":0.09003, "x":5.08526, "y":5.14211, "heading":-2.03931, "vx":0.08074, "vy":0.98167, "omega":1.47125, "ax":-1.54536, "ay":11.06836, "alpha":14.09315, "fx":[57.47925,-41.47873,-123.60126,4.55886], "fy":[190.2488,194.50231,155.51402,197.75185]},
+    {"t":0.10803, "x":5.08646, "y":5.16158, "heading":-2.01282, "vx":0.05292, "vy":1.18095, "omega":1.725, "ax":-2.81026, "ay":10.89932, "alpha":13.20006, "fx":[37.52933,-56.36243,-134.28857,-34.26138], "fy":[195.07236,190.66146,146.31451,194.69764]},
+    {"t":0.12604, "x":5.08696, "y":5.18461, "heading":-1.98176, "vx":0.00232, "vy":1.3772, "omega":1.96267, "ax":-4.27968, "ay":10.49785, "alpha":12.3189, "fx":[10.75229,-74.19234,-145.52006,-76.40048], "fy":[198.24654,184.38261,135.07135,182.27578]},
+    {"t":0.14404, "x":5.08631, "y":5.21111, "heading":-1.94642, "vx":-0.07474, "vy":1.56621, "omega":2.18448, "ax":-5.89365, "ay":9.79125, "alpha":11.3196, "fx":[-24.8438,-95.20711,-156.88656,-116.04007], "fy":[196.84905,174.37331,121.60036,160.03907]},
+    {"t":0.16205, "x":5.08401, "y":5.2409, "heading":-1.90709, "vx":-0.18086, "vy":1.74251, "omega":2.38829, "ax":-7.56426, "ay":8.72752, "alpha":9.82301, "fx":[-69.62813,-119.14981,-167.82552,-147.76655], "fy":[185.65962,158.87072,105.90826,131.49537]},
+    {"t":0.18005, "x":5.07952, "y":5.27369, "heading":-1.86409, "vx":-0.31705, "vy":1.89965, "omega":2.56516, "ax":-9.16046, "ay":7.27047, "alpha":7.51409, "fx":[-118.65707,-144.67369,-177.68016,-169.79114], "fy":[158.76695,135.91358,88.30423,101.79597]},
+    {"t":0.19806, "x":5.07233, "y":5.30907, "heading":-1.8179, "vx":-0.48199, "vy":2.03056, "omega":2.70045, "ax":-10.48031, "ay":5.46708, "alpha":4.61478, "fx":[-160.60995,-168.79143,-185.83736,-183.56816], "fy":[116.16697,104.33013,69.45384,74.58339]},
+    {"t":0.21606, "x":5.06195, "y":5.34652, "heading":-1.76928, "vx":-0.67069, "vy":2.12899, "omega":2.78354, "ax":-11.35683, "ay":3.5157, "alpha":1.70479, "fx":[-186.45816,-187.32068,-191.9056,-191.56709], "fy":[67.53138,65.35596,50.29546,51.23755]},
+    {"t":0.23407, "x":5.04804, "y":5.38542, "heading":-1.71916, "vx":-0.87517, "vy":2.19229, "omega":2.81423, "ax":-11.65903, "ay":2.18361, "alpha":-4.31049, "fx":[-198.11274,-198.22185,-189.65966,-191.4076], "fy":[17.44065,14.66249,59.4844,54.0111]},
+    {"t":0.27128, "x":5.0074, "y":5.46852, "heading":-1.61444, "vx":-1.30904, "vy":2.27355, "omega":2.65383, "ax":-11.10276, "ay":1.03982, "alpha":-15.17374, "fx":[-196.27784,-186.84168,-168.56949,-188.62166], "fy":[-31.63195,-66.49033,104.59394,62.86142]},
+    {"t":0.30849, "x":4.95099, "y":5.55384, "heading":-1.51568, "vx":-1.72222, "vy":2.31225, "omega":2.08916, "ax":-10.86547, "ay":0.74706, "alpha":-17.51993, "fx":[-193.53002,-180.74322,-160.41273,-189.80283], "fy":[-44.65532,-80.33255,116.1701,58.63035]},
+    {"t":0.34571, "x":4.87938, "y":5.64041, "heading":-1.43793, "vx":-2.12656, "vy":2.34005, "omega":1.43718, "ax":-10.76582, "ay":0.3422, "alpha":-18.25826, "fx":[-190.36473,-175.84915,-159.95296,-191.6775], "fy":[-55.27441,-88.48878,115.75113,50.82904]},
+    {"t":0.38292, "x":4.79279, "y":5.72772, "heading":-1.38445, "vx":-2.52719, "vy":2.35278, "omega":0.75773, "ax":-10.72546, "ay":-0.80219, "alpha":-17.67921, "fx":[-183.91826,-163.7394,-172.77353,-194.72169], "fy":[-71.07992,-105.58123,91.55468,31.61768]},
+    {"t":0.42014, "x":4.69132, "y":5.81472, "heading":-1.35625, "vx":-2.92632, "vy":2.32293, "omega":0.09982, "ax":-7.36563, "ay":-8.55723, "alpha":-2.49511, "fx":[-121.96147,-110.15942,-124.24778,-134.75755], "fy":[-144.92004,-152.95105,-140.55742,-132.15141]},
+    {"t":0.45735, "x":4.57732, "y":5.89524, "heading":-1.35254, "vx":-3.20043, "vy":2.00449, "omega":0.00697, "ax":-4.62079, "ay":-8.18381, "alpha":-0.01561, "fx":[-77.04054,-76.95135,-77.0123,-77.10148], "fy":[-136.43582,-136.46384,-136.4046,-136.37657]},
+    {"t":0.49456, "x":4.45502, "y":5.96417, "heading":-1.35228, "vx":-3.37238, "vy":1.69994, "omega":0.00639, "ax":-1.27545, "ay":-2.62259, "alpha":-0.00024, "fx":[-21.26154,-21.26039,-21.26069,-21.26185], "fy":[-43.71805,-43.71783,-43.71672,-43.71694]},
+    {"t":0.53178, "x":4.32864, "y":6.02562, "heading":-1.35204, "vx":-3.41985, "vy":1.60234, "omega":0.00638, "ax":-0.15789, "ay":-0.33861, "alpha":0.00006, "fx":[-2.63192,-2.63219,-2.63213,-2.63186], "fy":[-5.64425,-5.64431,-5.64458,-5.64452]},
+    {"t":0.56899, "x":4.20127, "y":6.08501, "heading":-1.3518, "vx":-3.42572, "vy":1.58974, "omega":0.00638, "ax":-0.01896, "ay":-0.04089, "alpha":0.00001, "fx":[-0.31611,-0.31614,-0.31613,-0.3161], "fy":[-0.68162,-0.68163,-0.68166,-0.68165]},
+    {"t":0.6062, "x":4.07377, "y":6.14414, "heading":-1.35157, "vx":-3.42643, "vy":1.58822, "omega":0.00638, "ax":-0.00228, "ay":-0.00493, "alpha":0.0, "fx":[-0.03809,-0.03807,-0.03808,-0.03809], "fy":[-0.08216,-0.08216,-0.08215,-0.08215]},
+    {"t":0.64342, "x":3.94626, "y":6.20324, "heading":-1.35133, "vx":-3.42651, "vy":1.58804, "omega":0.00638, "ax":-0.00027, "ay":-0.00059, "alpha":0.0, "fx":[-0.00459,-0.00457,-0.00458,-0.00459], "fy":[-0.00989,-0.00989,-0.00987,-0.00987]},
+    {"t":0.68063, "x":3.81875, "y":6.26234, "heading":-1.35109, "vx":-3.42652, "vy":1.58801, "omega":0.00638, "ax":-0.00003, "ay":-0.00007, "alpha":0.0, "fx":[-0.00055,-0.00053,-0.00054,-0.00056], "fy":[-0.00118,-0.00117,-0.00115,-0.00116]},
+    {"t":0.71784, "x":3.69123, "y":6.32143, "heading":-1.35085, "vx":-3.42652, "vy":1.58801, "omega":0.00638, "ax":0.0, "ay":-0.00001, "alpha":0.0, "fx":[-0.00007,-0.00005,-0.00005,-0.00007], "fy":[-0.00013,-0.00012,-0.00011,-0.00011]},
+    {"t":0.75506, "x":3.56372, "y":6.38053, "heading":-1.35062, "vx":-3.42652, "vy":1.58801, "omega":0.00638, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,0.00001,0.00001,-0.00001], "fy":[0.0,0.0,0.00002,0.00002]},
+    {"t":0.79227, "x":3.43621, "y":6.43962, "heading":-1.35038, "vx":-3.42652, "vy":1.58801, "omega":0.00638, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.00002,0.00001,-0.00001], "fy":[0.00001,0.00002,0.00004,0.00003]},
+    {"t":0.82948, "x":3.30869, "y":6.49872, "heading":-1.35014, "vx":-3.42652, "vy":1.58801, "omega":0.00638, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.00002,0.00001,-0.00001], "fy":[0.00001,0.00002,0.00004,0.00003]},
+    {"t":0.8667, "x":3.18118, "y":6.55781, "heading":-1.3499, "vx":-3.42652, "vy":1.58801, "omega":0.00638, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.00001,0.00001,-0.00001], "fy":[0.00001,0.00001,0.00003,0.00003]},
+    {"t":0.90391, "x":3.05367, "y":6.61691, "heading":-1.34967, "vx":-3.42652, "vy":1.58801, "omega":0.00638, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00003,-0.00001,-0.00001,-0.00003], "fy":[-0.00004,-0.00004,-0.00002,-0.00003]},
+    {"t":0.94112, "x":2.92615, "y":6.67601, "heading":-1.34943, "vx":-3.42652, "vy":1.58801, "omega":0.00638, "ax":-0.00001, "ay":-0.00003, "alpha":0.0, "fx":[-0.00023,-0.00022,-0.00022,-0.00024], "fy":[-0.00049,-0.00048,-0.00046,-0.00047]},
+    {"t":0.97834, "x":2.79864, "y":6.7351, "heading":-1.34919, "vx":-3.42652, "vy":1.58801, "omega":0.00638, "ax":-0.00012, "ay":-0.00025, "alpha":0.0, "fx":[-0.00193,-0.00192,-0.00192,-0.00194], "fy":[-0.00416,-0.00415,-0.00413,-0.00414]},
+    {"t":1.01555, "x":2.67113, "y":6.7942, "heading":-1.34895, "vx":-3.42653, "vy":1.588, "omega":0.00638, "ax":-0.00096, "ay":-0.00208, "alpha":0.0, "fx":[-0.01604,-0.01603,-0.01603,-0.01605], "fy":[-0.0346,-0.0346,-0.03458,-0.03459]},
+    {"t":1.05277, "x":2.54361, "y":6.85329, "heading":-1.34872, "vx":-3.42656, "vy":1.58792, "omega":0.00638, "ax":-0.00798, "ay":-0.01723, "alpha":0.0, "fx":[-0.13304,-0.13304,-0.13304,-0.13304], "fy":[-0.28717,-0.28717,-0.28718,-0.28717]},
+    {"t":1.08998, "x":2.41609, "y":6.91237, "heading":-1.34848, "vx":-3.42686, "vy":1.58728, "omega":0.00638, "ax":-0.06581, "ay":-0.14311, "alpha":0.00072, "fx":[-1.09566,-1.0991,-1.09833,-1.09489], "fy":[-2.38344,-2.3842,-2.38764,-2.38688]},
+    {"t":1.12719, "x":2.28852, "y":6.97134, "heading":-1.34824, "vx":-3.42931, "vy":1.58196, "omega":0.00641, "ax":0.25839, "ay":-1.53056, "alpha":2.02822, "fx":[8.15781,-1.70462,0.51905,10.25689], "fy":[-19.67061,-21.88709,-31.33949,-29.15736]},
+    {"t":1.16441, "x":2.16109, "y":7.02915, "heading":-1.348, "vx":-3.41969, "vy":1.525, "omega":0.08188, "ax":9.13824, "ay":-3.37467, "alpha":22.48842, "fx":[192.14192,175.37824,69.46167,172.3387], "fy":[39.85917,13.78089,-183.16232,-95.49417]},
+    {"t":1.20162, "x":2.04015, "y":7.08356, "heading":-1.34496, "vx":-3.07963, "vy":1.39942, "omega":0.91876, "ax":9.40204, "ay":-3.16216, "alpha":22.42045, "fx":[193.38799,186.73461,72.39016,174.39704], "fy":[42.00163,25.44838,-183.9842,-94.3127]},
+    {"t":1.23883, "x":1.93206, "y":7.13345, "heading":-1.31077, "vx":-2.72975, "vy":1.28174, "omega":1.7531, "ax":10.54989, "ay":-4.88993, "alpha":8.55927, "fx":[196.27323,182.57204,146.3711,178.23027], "fy":[-28.67597,-76.10792,-133.79042,-87.47697]},
+    {"t":1.27605, "x":1.83778, "y":7.17776, "heading":-1.24553, "vx":-2.33715, "vy":1.09977, "omega":2.07162, "ax":10.79824, "ay":-5.0127, "alpha":-2.0184, "fx":[173.94017,180.12251,185.60865,180.33481], "fy":[-96.04208,-83.98091,-70.92011,-83.29434]},
+    {"t":1.31326, "x":1.75829, "y":7.21522, "heading":-1.16844, "vx":-1.93531, "vy":0.91323, "omega":1.99651, "ax":10.66787, "ay":-4.95279, "alpha":-6.83981, "fx":[155.95249,181.32679,195.17512,178.85877], "fy":[-123.45169,-81.93336,-38.25727,-86.60031]},
+    {"t":1.35047, "x":1.69365, "y":7.24577, "heading":-1.09414, "vx":-1.53832, "vy":0.72892, "omega":1.74198, "ax":10.52294, "ay":-4.9076, "alpha":-9.45155, "fx":[145.79876,183.18709,198.11756,174.54601], "fy":[-135.47254,-77.96451,-18.65847,-95.13369]},
+    {"t":1.38769, "x":1.64369, "y":7.2695, "heading":-1.02931, "vx":-1.14672, "vy":0.54629, "omega":1.39025, "ax":10.38887, "ay":-4.89207, "alpha":-11.19972, "fx":[140.37049,184.98248,198.98524,168.37173], "fy":[-141.19729,-73.7716,-5.43421,-105.79042]},
+    {"t":1.4249, "x":1.60821, "y":7.28644, "heading":-0.97758, "vx":-0.76012, "vy":0.36424, "omega":0.97347, "ax":10.26609, "ay":-4.89203, "alpha":-12.54709, "fx":[137.4338,186.46216,199.06394,161.56353], "fy":[-144.12759,-70.0616,4.01297,-116.01498]},
+    {"t":1.46211, "x":1.58703, "y":7.29661, "heading":-0.94135, "vx":-0.37808, "vy":0.18219, "omega":0.50655, "ax":10.1597, "ay":-4.89576, "alpha":-13.61192, "fx":[135.66947,187.53245,198.84132,155.38627], "fy":[-145.83803,-67.22749,10.86831,-124.24262]},
+    {"t":1.49933, "x":1.58, "y":7.3, "heading":-0.9225, "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/KTofetch.traj b/src/main/deploy/choreo/KTofetch.traj
new file mode 100644
index 00000000..3caee708
--- /dev/null
+++ b/src/main/deploy/choreo/KTofetch.traj
@@ -0,0 +1,62 @@
+{
+ "name":"KTofetch",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":4.141549248154255, "y":5.238261807735684, "heading":5.235987755982989, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"K.x", "val":4.141549248154255}, "y":{"exp":"K.y", "val":5.238261807735684}, "heading":{"exp":"K.heading", "val":5.235987755982989}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,1.18786],
+  "samples":[
+    {"t":0.0, "x":4.14155, "y":5.23826, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-9.30248, "ay":7.48763, "alpha":1.47964, "fx":[-149.29707,-157.60885,-160.94359,-152.42208], "fy":[131.86641,121.81793,117.35584,128.22069]},
+    {"t":0.04399, "x":4.13255, "y":5.24551, "heading":-1.0472, "vx":-0.40926, "vy":0.32942, "omega":0.0651, "ax":-9.30148, "ay":7.48696, "alpha":1.40786, "fx":[-149.55806,-157.47153,-160.6394,-152.53601], "fy":[131.52214,121.94427,117.71654,128.03333]},
+    {"t":0.08799, "x":4.10554, "y":5.26725, "heading":-1.04433, "vx":-0.81848, "vy":0.6588, "omega":0.12703, "ax":-9.30003, "ay":7.48598, "alpha":1.30707, "fx":[-149.93165,-157.29136,-160.20587,-152.67915], "fy":[131.02795,122.10443,118.22852,127.79009]},
+    {"t":0.13198, "x":4.06053, "y":5.30347, "heading":-1.03874, "vx":-1.22763, "vy":0.98815, "omega":0.18454, "ax":-9.29772, "ay":7.48442, "alpha":1.15514, "fx":[-150.49452,-157.01859,-159.55041,-152.89086], "fy":[130.27733,122.34548,118.99602,127.42793]},
+    {"t":0.17598, "x":3.99752, "y":5.35419, "heading":-1.03063, "vx":-1.63668, "vy":1.31742, "omega":0.23536, "ax":-9.29355, "ay":7.48156, "alpha":0.9, "fx":[-151.42715,-156.53476,-158.45424,-153.25996], "fy":[129.01491,122.77812,120.26054,126.8023]},
+    {"t":0.21997, "x":3.91652, "y":5.41939, "heading":-1.02027, "vx":-2.04555, "vy":1.64657, "omega":0.27495, "ax":-9.28393, "ay":7.47483, "alpha":0.38299, "fx":[-153.27652,-155.46762,-156.24904,-154.04122], "fy":[126.44043,123.74312,122.73557,125.48835]},
+    {"t":0.26397, "x":3.81755, "y":5.49907, "heading":-1.00817, "vx":-2.45399, "vy":1.97542, "omega":0.2918, "ax":-9.24422, "ay":7.4461, "alpha":-1.21804, "fx":[-158.80664,-151.70824,-149.47734,-156.39479], "fy":[118.14195,127.08227,129.82424,121.44312]},
+    {"t":0.30796, "x":3.70064, "y":5.59318, "heading":-0.99534, "vx":-2.86069, "vy":2.30301, "omega":0.23822, "ax":-1.87667, "ay":1.49597, "alpha":-5.40306, "fx":[-36.16714,-13.06221,-27.04978,-48.85372], "fy":[6.32244,21.85331,43.32978,28.24292]},
+    {"t":0.35196, "x":3.57297, "y":5.69595, "heading":-0.98486, "vx":-2.94325, "vy":2.36883, "omega":0.00051, "ax":-0.00078, "ay":-0.00061, "alpha":-0.0003, "fx":[-0.01329,-0.01207,-0.01288,-0.0141], "fy":[-0.0111,-0.01029,-0.00908,-0.00988]},
+    {"t":0.39595, "x":3.44348, "y":5.80016, "heading":-0.98483, "vx":-2.94329, "vy":2.3688, "omega":0.0005, "ax":-0.00005, "ay":-0.00006, "alpha":0.0, "fx":[-0.00081,-0.00081,-0.00081,-0.00081], "fy":[-0.00101,-0.00101,-0.00101,-0.00101]},
+    {"t":0.43995, "x":3.31399, "y":5.90438, "heading":-0.98481, "vx":-2.94329, "vy":2.3688, "omega":0.0005, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00007,-0.00007,-0.00007,-0.00007], "fy":[-0.00008,-0.00008,-0.00008,-0.00008]},
+    {"t":0.48394, "x":3.1845, "y":6.00859, "heading":-0.98479, "vx":-2.94329, "vy":2.3688, "omega":0.0005, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]},
+    {"t":0.52794, "x":3.05501, "y":6.11281, "heading":-0.98477, "vx":-2.94329, "vy":2.3688, "omega":0.0005, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.57193, "x":2.92552, "y":6.21702, "heading":-0.98475, "vx":-2.94329, "vy":2.3688, "omega":0.0005, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.61593, "x":2.79603, "y":6.32124, "heading":-0.98473, "vx":-2.94329, "vy":2.3688, "omega":0.0005, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.65992, "x":2.66654, "y":6.42545, "heading":-0.9847, "vx":-2.94329, "vy":2.3688, "omega":0.0005, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00001,0.00001,0.00001,0.00001], "fy":[0.00001,0.00001,0.00001,0.00001]},
+    {"t":0.70392, "x":2.53705, "y":6.52967, "heading":-0.98468, "vx":-2.94329, "vy":2.3688, "omega":0.0005, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00006,0.00006,0.00006,0.00006], "fy":[0.00008,0.00008,0.00008,0.00008]},
+    {"t":0.74791, "x":2.40756, "y":6.63388, "heading":-0.98466, "vx":-2.94329, "vy":2.3688, "omega":0.0005, "ax":0.00005, "ay":0.00006, "alpha":0.0, "fx":[0.00081,0.00081,0.00081,0.00081], "fy":[0.001,0.001,0.001,0.001]},
+    {"t":0.79191, "x":2.27808, "y":6.7381, "heading":-0.98464, "vx":-2.94329, "vy":2.3688, "omega":0.0005, "ax":0.00078, "ay":0.0006, "alpha":0.0003, "fx":[0.01322,0.012,0.01281,0.01403], "fy":[0.01102,0.01021,0.00899,0.0098]},
+    {"t":0.8359, "x":2.14859, "y":6.84231, "heading":-0.98462, "vx":-2.94325, "vy":2.36883, "omega":0.00051, "ax":1.87609, "ay":-1.4956, "alpha":5.41639, "fx":[35.97637,12.96027,27.22164,48.93588], "fy":[-6.22983,-22.04581,-43.39519,-28.05305]},
+    {"t":0.87989, "x":2.02092, "y":6.94508, "heading":-0.98459, "vx":-2.86071, "vy":2.30303, "omega":0.2388, "ax":9.24453, "ay":-7.44631, "alpha":1.18094, "fx":[158.61767,151.67634,149.68154,156.43182], "fy":[-118.39608,-127.12547,-129.59049,-121.39333]},
+    {"t":0.92389, "x":1.90401, "y":7.03919, "heading":-0.97409, "vx":-2.454, "vy":1.97543, "omega":0.29076, "ax":9.28393, "ay":-7.47482, "alpha":-0.39111, "fx":[153.28157,155.55074,156.24662,153.95546], "fy":[-126.43533,-123.63891,-122.73819,-125.59389]},
+    {"t":0.96788, "x":1.80503, "y":7.11887, "heading":-0.9613, "vx":-2.04556, "vy":1.64658, "omega":0.27355, "ax":9.29356, "ay":-7.48156, "alpha":-0.89919, "fx":[151.56152,156.76384,158.33838,153.01329], "fy":[-128.85843,-122.48495,-120.41171,-127.10075]},
+    {"t":1.01188, "x":1.72403, "y":7.18407, "heading":-0.94926, "vx":-1.63669, "vy":1.31743, "omega":0.23399, "ax":9.29776, "ay":-7.48444, "alpha":-1.15015, "fx":[150.73576,157.3864,159.34812,152.48673], "fy":[-129.99972,-121.87115,-119.265,-127.91236]},
+    {"t":1.05587, "x":1.66102, "y":7.23479, "heading":-0.93897, "vx":-1.22764, "vy":0.98815, "omega":0.18339, "ax":9.30009, "ay":-7.48602, "alpha":-1.29972, "fx":[150.2601,157.77607,159.93583,152.14006], "fy":[-130.65278,-121.47656,-118.59166,-128.4326]},
+    {"t":1.09987, "x":1.61601, "y":7.27102, "heading":-0.9309, "vx":-0.81848, "vy":0.65881, "omega":0.12621, "ax":9.30156, "ay":-7.48701, "alpha":-1.39902, "fx":[149.95302,158.0441,160.31892,151.89406], "fy":[-131.07321,-121.20039,-118.15071,-128.79542]},
+    {"t":1.14386, "x":1.589, "y":7.29275, "heading":-0.92535, "vx":-0.40926, "vy":0.32942, "omega":0.06466, "ax":9.30257, "ay":-7.48769, "alpha":-1.46973, "fx":[149.73638,158.23601,160.59019,151.71497], "fy":[-131.36881,-121.00131,-117.83714,-129.05769]},
+    {"t":1.18786, "x":1.58, "y":7.3, "heading":-0.9225, "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/LTofetch.traj b/src/main/deploy/choreo/LTofetch.traj
new file mode 100644
index 00000000..b1793876
--- /dev/null
+++ b/src/main/deploy/choreo/LTofetch.traj
@@ -0,0 +1,61 @@
+{
+ "name":"LTofetch",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":3.85576086490539, "y":5.073261807735684, "heading":5.235987755982989, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"L.x", "val":3.85576086490539}, "y":{"exp":"L.y", "val":5.073261807735684}, "heading":{"exp":"L.heading", "val":5.235987755982989}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,1.1599],
+  "samples":[
+    {"t":0.0, "x":3.85576, "y":5.07326, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-8.53361, "ay":8.35003, "alpha":1.69894, "fx":[-135.29516,-146.13702,-149.48934,-138.083], "fy":[146.20273,135.37161,131.63835,143.55109]},
+    {"t":0.04461, "x":3.84727, "y":5.08157, "heading":-1.0472, "vx":-0.3807, "vy":0.37251, "omega":0.07579, "ax":-8.53318, "ay":8.34975, "alpha":1.57634, "fx":[-135.77905,-145.85828,-148.95191,-138.3866], "fy":[145.7101,135.62586,132.19697,143.21252]},
+    {"t":0.08922, "x":3.82179, "y":5.1065, "heading":-1.04382, "vx":-0.76138, "vy":0.74501, "omega":0.14612, "ax":-8.53244, "ay":8.34924, "alpha":1.40351, "fx":[-136.47457,-145.47963,-148.18347,-138.78876], "fy":[144.99713,135.96655,132.98904,142.75845]},
+    {"t":0.13384, "x":3.77934, "y":5.14804, "heading":-1.0373, "vx":-1.14202, "vy":1.11748, "omega":0.20873, "ax":-8.53101, "ay":8.34818, "alpha":1.14149, "fx":[-137.52888,-144.89347,-147.01851,-139.39076], "fy":[143.90244,136.49089,134.17312,142.0739]},
+    {"t":0.17845, "x":3.7199, "y":5.2062, "heading":-1.02799, "vx":-1.52261, "vy":1.48991, "omega":0.25965, "ax":-8.52779, "ay":8.34559, "alpha":0.69727, "fx":[-139.29662,-143.83631,-145.0621,-140.42138], "fy":[142.02639,137.43224,136.11628,140.89315]},
+    {"t":0.22306, "x":3.64349, "y":5.28097, "heading":-1.0164, "vx":-1.90305, "vy":1.86222, "omega":0.29076, "ax":-8.51789, "ay":8.33712, "alpha":-0.22035, "fx":[-142.89513,-141.43602,-141.08833,-142.53702], "fy":[138.04422,139.53622,139.89811,138.42433]},
+    {"t":0.26767, "x":3.55011, "y":5.37235, "heading":-1.00343, "vx":-2.28304, "vy":2.23415, "omega":0.28093, "ax":-8.45458, "ay":8.27976, "alpha":-3.24685, "fx":[-154.46311,-131.99368,-128.55704,-148.72158], "fy":[123.5563,147.22848,150.55531,130.73814]},
+    {"t":0.31228, "x":3.43985, "y":5.48026, "heading":-0.9909, "vx":-2.66022, "vy":2.60352, "omega":0.13608, "ax":-0.90434, "ay":0.86738, "alpha":-3.03619, "fx":[-17.37385,-4.81232,-12.88343,-25.23017], "fy":[4.17382,12.4575,24.70221,16.50197]},
+    {"t":0.35689, "x":3.32027, "y":5.59727, "heading":-0.98483, "vx":-2.70056, "vy":2.64222, "omega":0.00063, "ax":-0.00077, "ay":-0.00063, "alpha":-0.00016, "fx":[-0.01294,-0.01231,-0.01273,-0.01337], "fy":[-0.01098,-0.01056,-0.00992,-0.01035]},
+    {"t":0.40151, "x":3.1998, "y":5.71514, "heading":-0.9848, "vx":-2.7006, "vy":2.64219, "omega":0.00062, "ax":-0.00005, "ay":-0.00005, "alpha":0.0, "fx":[-0.0009,-0.0009,-0.0009,-0.0009], "fy":[-0.00092,-0.00092,-0.00092,-0.00092]},
+    {"t":0.44612, "x":3.07932, "y":5.83301, "heading":-0.98477, "vx":-2.7006, "vy":2.64219, "omega":0.00062, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00007,-0.00007,-0.00007,-0.00007], "fy":[-0.00007,-0.00007,-0.00007,-0.00007]},
+    {"t":0.49073, "x":2.95884, "y":5.95088, "heading":-0.98474, "vx":-2.7006, "vy":2.64219, "omega":0.00062, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]},
+    {"t":0.53534, "x":2.83836, "y":6.06876, "heading":-0.98472, "vx":-2.7006, "vy":2.64219, "omega":0.00062, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.57995, "x":2.71788, "y":6.18663, "heading":-0.98469, "vx":-2.7006, "vy":2.64219, "omega":0.00062, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.62456, "x":2.59741, "y":6.3045, "heading":-0.98466, "vx":-2.7006, "vy":2.64219, "omega":0.00062, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00001,0.00001,0.00001,0.00001], "fy":[0.00001,0.00001,0.00001,0.00001]},
+    {"t":0.66918, "x":2.47693, "y":6.42237, "heading":-0.98463, "vx":-2.7006, "vy":2.64219, "omega":0.00062, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00007,0.00007,0.00007,0.00007], "fy":[0.00007,0.00007,0.00007,0.00007]},
+    {"t":0.71379, "x":2.35645, "y":6.54025, "heading":-0.9846, "vx":-2.7006, "vy":2.64219, "omega":0.00062, "ax":0.00005, "ay":0.00005, "alpha":0.0, "fx":[0.00088,0.00088,0.00088,0.00088], "fy":[0.0009,0.0009,0.0009,0.0009]},
+    {"t":0.7584, "x":2.23597, "y":6.65812, "heading":-0.98458, "vx":-2.7006, "vy":2.64219, "omega":0.00062, "ax":0.00076, "ay":0.00061, "alpha":0.00016, "fx":[0.01272,0.01208,0.01251,0.01314], "fy":[0.01076,0.01033,0.0097,0.01012]},
+    {"t":0.80301, "x":2.11549, "y":6.77599, "heading":-0.98455, "vx":-2.70056, "vy":2.64222, "omega":0.00063, "ax":0.9032, "ay":-0.8666, "alpha":3.03908, "fx":[17.29153,4.76954,12.92714,25.23514], "fy":[-4.1377,-12.50823,-24.71076,-16.42645]},
+    {"t":0.84762, "x":1.99592, "y":6.893, "heading":-0.98452, "vx":-2.66027, "vy":2.60356, "omega":0.13621, "ax":8.45532, "ay":-8.28031, "alpha":3.20948, "fx":[154.18578,131.84967,128.887,148.86202], "fy":[-123.90012,-147.36417,-150.2754,-130.5752]},
+    {"t":0.89223, "x":1.88565, "y":7.00091, "heading":-0.97844, "vx":-2.28306, "vy":2.23416, "omega":0.27939, "ax":8.51793, "ay":-8.33711, "alpha":0.21384, "fx":[142.84828,141.41995,141.13668,142.55417], "fy":[-138.09283,-139.55309,-139.84968,-138.40657]},
+    {"t":0.93685, "x":1.79228, "y":7.09229, "heading":-0.96598, "vx":-1.90306, "vy":1.86223, "omega":0.28893, "ax":8.52782, "ay":-8.34559, "alpha":-0.69454, "fx":[139.42349,144.00052,144.94458,140.24972], "fy":[-141.90268,-137.25972,-136.24069,-141.06472]},
+    {"t":0.98146, "x":1.71586, "y":7.16706, "heading":-0.95309, "vx":-1.52262, "vy":1.48992, "omega":0.25795, "ax":8.53107, "ay":-8.3482, "alpha":-1.13454, "fx":[137.82035,145.24937,146.75901,139.00658], "fy":[-143.62451,-136.11127,-134.45556,-142.45089]},
+    {"t":1.02607, "x":1.65642, "y":7.22522, "heading":-0.94158, "vx":-1.14204, "vy":1.11749, "omega":0.20733, "ax":8.53252, "ay":-8.34929, "alpha":-1.39429, "fx":[136.91132,146.00717,147.80473,138.20862], "fy":[-144.58619,-135.3989,-133.40818,-143.32143]},
+    {"t":1.07068, "x":1.61397, "y":7.26676, "heading":-0.93233, "vx":-0.76139, "vy":0.74501, "omega":0.14513, "ax":8.53328, "ay":-8.34982, "alpha":-1.56576, "fx":[136.33206,146.52052,148.48052,137.64956], "fy":[-145.19422,-134.90912,-132.72442,-143.92239]},
+    {"t":1.11529, "x":1.58849, "y":7.29169, "heading":-0.92586, "vx":-0.3807, "vy":0.37251, "omega":0.07528, "ax":8.53373, "ay":-8.35011, "alpha":-1.68742, "fx":[135.92832,146.88692,148.95566,137.24158], "fy":[-145.61559,-134.55655,-132.24017,-144.35701]},
+    {"t":1.1599, "x":1.58, "y":7.3, "heading":-0.9225, "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/New Path.traj b/src/main/deploy/choreo/New Path.traj
deleted file mode 100644
index 7b2e9ed1..00000000
--- a/src/main/deploy/choreo/New Path.traj	
+++ /dev/null
@@ -1,28 +0,0 @@
-{
- "name":"New Path",
- "version":1,
- "snapshot":{
-  "waypoints":[],
-  "constraints":[],
-  "targetDt":0.05
- },
- "params":{
-  "waypoints":[
-    {"x":{"exp":"7.1008875 m", "val":7.1008875}, "y":{"exp":"5.0756788 m", "val":5.0756788}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
-  "constraints":[
-    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
-    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
-    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":true}],
-  "targetDt":{
-   "exp":"0.05 s",
-   "val":0.05
-  }
- },
- "trajectory":{
-  "sampleType":null,
-  "waypoints":[],
-  "samples":[],
-  "splits":[]
- },
- "events":[]
-}
diff --git a/src/main/deploy/choreo/fetchToA.traj b/src/main/deploy/choreo/fetchToA.traj
new file mode 100644
index 00000000..3946a8ce
--- /dev/null
+++ b/src/main/deploy/choreo/fetchToA.traj
@@ -0,0 +1,77 @@
+{
+ "name":"fetchToA",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":2.904038667678833, "y":4.603036403656006, "heading":0.0, "intervals":8, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":3.2654999999999994, "y":4.3309, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false},
+    {"from":0, "to":2, "data":{"type":"KeepOutCircle", "props":{"x":1.2161137368530035, "y":5.864292200654745, "r":0.3248109411104518}}, "enabled":true}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"2.904038667678833 m", "val":2.904038667678833}, "y":{"exp":"4.603036403656006 m", "val":4.603036403656006}, "heading":{"exp":"A.heading", "val":0.0}, "intervals":8, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"A.x", "val":3.2654999999999994}, "y":{"exp":"A.y", "val":4.3309}, "heading":{"exp":"A.heading", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false},
+    {"from":0, "to":2, "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"1.2161137368530035 m", "val":1.2161137368530035}, "y":{"exp":"5.864292200654745 m", "val":5.864292200654745}, "r":{"exp":"0.3248109411104518 m", "val":0.3248109411104518}}}, "enabled":true}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,0.99526,1.27164],
+  "samples":[
+    {"t":0.0, "x":1.58, "y":7.3, "heading":-0.9225, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.37175, "ay":-7.48175, "alpha":33.10335, "fx":[198.66606,-98.8079,34.44026,157.20141], "fy":[-7.565,-172.76859,-196.20581,-122.32964]},
+    {"t":0.03318, "x":1.58241, "y":7.29588, "heading":-0.9225, "vx":0.14503, "vy":-0.24821, "omega":1.09822, "ax":4.50093, "ay":-7.95038, "alpha":30.38104, "fx":[196.42291,-87.37293,36.39949,154.66383], "fy":[-30.08741,-178.75035,-195.8162,-125.46224]},
+    {"t":0.06635, "x":1.58969, "y":7.28327, "heading":-0.88607, "vx":0.29435, "vy":-0.51197, "omega":2.10612, "ax":4.72333, "ay":-8.58592, "alpha":26.06711, "fx":[188.24685,-68.2282,42.4103,152.51334], "fy":[-63.15336,-186.81466,-194.55022,-127.97448]},
+    {"t":0.09953, "x":1.60206, "y":7.26156, "heading":-0.8162, "vx":0.45105, "vy":-0.79681, "omega":2.9709, "ax":4.89542, "ay":-9.45137, "alpha":19.60797, "fx":[164.71427,-38.80184,52.14006,148.36503], "fy":[-110.55148,-194.95694,-192.08338,-132.60712]},
+    {"t":0.1327, "x":1.61972, "y":7.22993, "heading":-0.71764, "vx":0.61346, "vy":-1.11036, "omega":3.6214, "ax":4.92551, "ay":-10.34068, "alpha":11.44697, "fx":[120.87881,7.3257,65.28107,134.93774], "fy":[-157.19078,-198.48623,-187.84367,-145.97572]},
+    {"t":0.16588, "x":1.64278, "y":7.1874, "heading":-0.59749, "vx":0.77687, "vy":-1.45342, "omega":4.00116, "ax":4.9465, "ay":-10.822, "alpha":-0.3874, "fx":[81.89601,84.79126,83.0369,80.09898], "fy":[-180.67652,-179.3252,-180.1258,-181.46254]},
+    {"t":0.19905, "x":1.67127, "y":7.13323, "heading":-0.46475, "vx":0.94097, "vy":-1.81244, "omega":3.98831, "ax":4.99099, "ay":-8.52292, "alpha":-22.56257, "fx":[71.05427,175.87142,154.61955,-68.75546], "fy":[-185.30378,-91.24956,-106.03434,-185.70451]},
+    {"t":0.23223, "x":1.70524, "y":7.06841, "heading":-0.33244, "vx":1.10655, "vy":-2.09519, "omega":3.23979, "ax":2.98615, "ay":-9.39468, "alpha":-22.09954, "fx":[82.2634,181.52201,-4.39824,-60.27639], "fy":[-180.35752,-78.38001,-179.29189,-188.38994]},
+    {"t":0.2654, "x":1.74359, "y":6.99373, "heading":-0.22496, "vx":1.20561, "vy":-2.40686, "omega":2.50663, "ax":3.11534, "ay":-9.44504, "alpha":-22.06113, "fx":[92.29467,181.28857,-24.60156,-41.25706], "fy":[-174.95695,-77.12505,-184.74689,-192.94803]},
+    {"t":0.29858, "x":1.7853, "y":6.90868, "heading":-0.1418, "vx":1.30896, "vy":-2.72021, "omega":1.77474, "ax":3.40318, "ay":-9.25368, "alpha":-22.40658, "fx":[100.70062,182.00074,-27.69127,-28.09242], "fy":[-169.27649,-71.79149,-181.68722,-194.26215]},
+    {"t":0.33175, "x":1.8306, "y":6.81335, "heading":-0.08292, "vx":1.42187, "vy":-3.0272, "omega":1.0314, "ax":3.74552, "ay":-8.82264, "alpha":-22.5682, "fx":[107.01644,180.67119,-20.47081,-17.47272], "fy":[-162.31676,-63.99655,-169.66042,-192.30278]},
+    {"t":0.36493, "x":1.87983, "y":6.70807, "heading":-0.04871, "vx":1.54612, "vy":-3.31989, "omega":0.28269, "ax":2.39255, "ay":-2.53056, "alpha":-7.9516, "fx":[56.18168,62.2805,22.50174,18.56685], "fy":[-60.25273,-22.87184,-22.82632,-62.78168]},
+    {"t":0.3981, "x":1.93244, "y":6.59653, "heading":-0.03933, "vx":1.6255, "vy":-3.40385, "omega":0.01889, "ax":-1.70452, "ay":-0.81434, "alpha":-0.03763, "fx":[-28.32521,-28.32157,-28.50199,-28.50561], "fy":[-13.67307,-13.48719,-13.47624,-13.66207]},
+    {"t":0.43128, "x":1.98543, "y":6.48316, "heading":-0.0387, "vx":1.56895, "vy":-3.43086, "omega":0.01764, "ax":-2.1554, "ay":-0.96317, "alpha":-0.00731, "fx":[-35.91236,-35.91203,-35.94672,-35.94705], "fy":[-16.07513,-16.03867,-16.0362,-16.07266]},
+    {"t":0.46446, "x":2.03629, "y":6.36881, "heading":-0.03812, "vx":1.49744, "vy":-3.46282, "omega":0.0174, "ax":-0.79999, "ay":-0.3418, "alpha":0.00227, "fx":[-13.34077,-13.34107,-13.33009,-13.32978], "fy":[-5.69195,-5.70301,-5.70341,-5.69234]},
+    {"t":0.49763, "x":2.08553, "y":6.25374, "heading":-0.03754, "vx":1.4709, "vy":-3.47415, "omega":0.01747, "ax":-0.25477, "ay":-0.10734, "alpha":0.00074, "fx":[-4.24864,-4.24878,-4.24518,-4.24505], "fy":[-1.78743,-1.79103,-1.79116,-1.78756]},
+    {"t":0.53081, "x":2.13419, "y":6.13843, "heading":-0.03696, "vx":1.46245, "vy":-3.47772, "omega":0.0175, "ax":-0.12186, "ay":-0.05097, "alpha":0.00066, "fx":[-2.03285,-2.03294,-2.02975,-2.02966], "fy":[-0.84804,-0.85123,-0.85133,-0.84814]},
+    {"t":0.56398, "x":2.18264, "y":6.02303, "heading":-0.03638, "vx":1.45841, "vy":-3.47941, "omega":0.01752, "ax":-0.08524, "ay":-0.0354, "alpha":0.00086, "fx":[-1.42293,-1.4231,-1.4189,-1.41873], "fy":[-0.58798,-0.59218,-0.59235,-0.58815]},
+    {"t":0.59716, "x":2.23098, "y":5.90758, "heading":-0.0358, "vx":1.45558, "vy":-3.48058, "omega":0.01755, "ax":-0.07234, "ay":-0.02984, "alpha":0.00109, "fx":[-1.20841,-1.2086,-1.2033,-1.20311], "fy":[-0.49473,-0.50004,-0.50023,-0.49492]},
+    {"t":0.63033, "x":2.27923, "y":5.79209, "heading":-0.03521, "vx":1.45318, "vy":-3.48157, "omega":0.01759, "ax":-0.06581, "ay":-0.02699, "alpha":0.00127, "fx":[-1.10004,-1.10027,-1.09409,-1.09385], "fy":[-0.44672,-0.45291,-0.45314,-0.44696]},
+    {"t":0.66351, "x":2.3274, "y":5.67657, "heading":-0.03463, "vx":1.451, "vy":-3.48247, "omega":0.01763, "ax":-0.06019, "ay":-0.02455, "alpha":0.00138, "fx":[-1.00652,-1.00675,-1.00003,-0.99979], "fy":[-0.40584,-0.41256,-0.4128,-0.40607]},
+    {"t":0.69668, "x":2.3755, "y":5.56103, "heading":-0.03405, "vx":1.449, "vy":-3.48328, "omega":0.01767, "ax":-0.05115, "ay":-0.02074, "alpha":0.00141, "fx":[-0.85595,-0.85619,-0.84932,-0.84908], "fy":[-0.34222,-0.34908,-0.34933,-0.34246]},
+    {"t":0.72986, "x":2.42355, "y":5.44546, "heading":-0.03346, "vx":1.4473, "vy":-3.48397, "omega":0.01772, "ax":-0.02124, "ay":-0.00833, "alpha":0.00128, "fx":[-0.35699,-0.3572,-0.35099,-0.35078], "fy":[-0.13571,-0.14192,-0.14213,-0.13592]},
+    {"t":0.76303, "x":2.47155, "y":5.32987, "heading":-0.03287, "vx":1.4466, "vy":-3.48425, "omega":0.01776, "ax":0.13962, "ay":0.05832, "alpha":0.00043, "fx":[2.32635,2.32628,2.32836,2.32843], "fy":[0.97332,0.97125,0.97117,0.97325]},
+    {"t":0.79621, "x":2.51962, "y":5.21431, "heading":-0.03228, "vx":1.45123, "vy":-3.48231, "omega":0.01778, "ax":1.12296, "ay":0.47436, "alpha":-0.00357, "fx":[18.72771,18.72813,18.71085,18.71043], "fy":[7.89832,7.91585,7.91654,7.89901]},
+    {"t":0.82938, "x":2.56838, "y":5.09905, "heading":-0.03169, "vx":1.48849, "vy":-3.46657, "omega":0.01766, "ax":5.4701, "ay":2.52125, "alpha":-0.01179, "fx":[91.21294,91.20339,91.15509,91.16464], "fy":[41.98819,42.05469,42.06789,42.00137]},
+    {"t":0.86256, "x":2.62077, "y":4.98543, "heading":-0.03111, "vx":1.66996, "vy":-3.38293, "omega":0.01727, "ax":9.06286, "ay":5.10025, "alpha":0.19172, "fx":[150.4861,151.07491,151.65753,151.07538], "fy":[85.98164,84.69923,84.05738,85.33641]},
+    {"t":0.89573, "x":2.68116, "y":4.876, "heading":-0.03053, "vx":1.97062, "vy":-3.21373, "omega":0.02363, "ax":7.44331, "ay":8.46713, "alpha":6.46326, "fx":[91.98616,120.12888,154.83817,129.35264], "fy":[167.19016,144.48125,110.93303,141.96783]},
+    {"t":0.92891, "x":2.75063, "y":4.77405, "heading":-0.02975, "vx":2.21756, "vy":-2.93283, "omega":0.23805, "ax":3.83408, "ay":10.49287, "alpha":12.68026, "fx":[10.20347,13.96133,138.6035,92.88068], "fy":[196.02037,193.0152,137.32369,173.28522]},
+    {"t":0.96209, "x":2.82631, "y":4.68252, "heading":-0.02185, "vx":2.34475, "vy":-2.58472, "omega":0.65872, "ax":-0.11229, "ay":11.37823, "alpha":-11.85497, "fx":[63.41772,39.99402,-41.0631,-69.83581], "fy":[186.62869,193.93447,193.72518,184.39012]},
+    {"t":0.99526, "x":2.90404, "y":4.60304, "heading":0.0, "vx":2.34103, "vy":-2.20725, "omega":0.26543, "ax":-3.95659, "ay":11.11062, "alpha":-5.39387, "fx":[-48.56906,-37.27287,-79.15539,-98.82061], "fy":[192.07413,194.88149,182.01072,171.86817]},
+    {"t":1.02981, "x":2.98256, "y":4.53341, "heading":0.00917, "vx":2.20434, "vy":-1.8234, "omega":0.07908, "ax":-6.93754, "ay":9.65253, "alpha":-2.81793, "fx":[-112.88706,-100.21561,-118.40107,-131.07813], "fy":[163.32796,171.50654,159.55883,149.21902]},
+    {"t":1.06436, "x":3.05457, "y":4.47618, "heading":0.0119, "vx":1.96466, "vy":-1.48992, "omega":-0.01827, "ax":-8.32735, "ay":8.52863, "alpha":-1.37406, "fx":[-138.70229,-131.99336,-139.0633,-145.49267], "fy":[142.39326,148.67326,142.11672,135.48933]},
+    {"t":1.0989, "x":3.11748, "y":4.42979, "heading":0.01127, "vx":1.67697, "vy":-1.19527, "omega":-0.06574, "ax":-9.06741, "ay":7.75712, "alpha":-0.49271, "fx":[-151.30295,-148.9393,-151.0222,-153.33282], "fy":[129.13794,131.86815,129.4898,126.73429]},
+    {"t":1.13345, "x":3.17, "y":4.39313, "heading":0.009, "vx":1.36371, "vy":-0.92728, "omega":-0.08277, "ax":-9.5112, "ay":7.21536, "alpha":0.08741, "fx":[-158.50231,-158.90699,-158.59283,-158.1862], "fy":[120.33811,119.80161,120.21508,120.75126]},
+    {"t":1.168, "x":3.21144, "y":4.3654, "heading":0.00614, "vx":1.03511, "vy":-0.67801, "omega":-0.07975, "ax":-9.80192, "ay":6.81961, "alpha":0.49384, "fx":[-163.08343,-165.28855,-163.73648,-161.46457], "fy":[114.15818,110.93368,113.20073,116.42613]},
+    {"t":1.20255, "x":3.24135, "y":4.34604, "heading":0.00338, "vx":0.69648, "vy":-0.4424, "omega":-0.06269, "ax":-10.0052, "ay":6.51981, "alpha":0.79289, "fx":[-166.22778,-169.65423,-167.42547,-163.81989], "fy":[109.60871,104.21595,107.74301,113.1607]},
+    {"t":1.2371, "x":3.25944, "y":4.33465, "heading":0.00122, "vx":0.35082, "vy":-0.21716, "omega":-0.03529, "ax":-10.1545, "ay":6.28561, "alpha":1.02155, "fx":[-168.50996,-172.79872,-170.18327,-165.59052], "fy":[106.12894,98.98061,103.39212,110.61094]},
+    {"t":1.27164, "x":3.2655, "y":4.3309, "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/fetchToB.traj b/src/main/deploy/choreo/fetchToB.traj
new file mode 100644
index 00000000..b5a4dcc7
--- /dev/null
+++ b/src/main/deploy/choreo/fetchToB.traj
@@ -0,0 +1,79 @@
+{
+ "name":"fetchToB",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":2.8945255279541016, "y":4.331230163574219, "heading":0.0, "intervals":9, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":3.2654999999999994, "y":4.0009, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false},
+    {"from":0, "to":2, "data":{"type":"KeepOutCircle", "props":{"x":1.2161137368530035, "y":5.864292200654745, "r":0.3248109411104518}}, "enabled":true}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "intervals":31, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"2.8945255279541016 m", "val":2.8945255279541016}, "y":{"exp":"4.331230163574219 m", "val":4.331230163574219}, "heading":{"exp":"B.heading", "val":0.0}, "intervals":9, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"B.x", "val":3.2654999999999994}, "y":{"exp":"B.y", "val":4.0009}, "heading":{"exp":"A.heading", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false},
+    {"from":0, "to":2, "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"1.2161137368530035 m", "val":1.2161137368530035}, "y":{"exp":"5.864292200654745 m", "val":5.864292200654745}, "r":{"exp":"0.3248109411104518 m", "val":0.3248109411104518}}}, "enabled":true}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,1.05745,1.34678],
+  "samples":[
+    {"t":0.0, "x":1.58, "y":7.3, "heading":-0.9225, "vx":0.0, "vy":0.0, "omega":0.0, "ax":4.29255, "ay":-7.27785, "alpha":34.27516, "fx":[198.8038,-103.95681,33.29364,158.07835], "fy":[2.06266,-169.72816,-196.40719,-121.20025]},
+    {"t":0.03411, "x":1.5825, "y":7.29577, "heading":-0.9225, "vx":0.14642, "vy":-0.24826, "omega":1.16917, "ax":4.44566, "ay":-7.80124, "alpha":31.29806, "fx":[197.37594,-91.69967,35.40802,155.34386], "fy":[-22.97789,-176.57258,-195.99958,-124.62168]},
+    {"t":0.06822, "x":1.59008, "y":7.28276, "heading":-0.88262, "vx":0.29807, "vy":-0.51437, "omega":2.23678, "ax":4.70384, "ay":-8.5328, "alpha":26.41912, "fx":[188.98942,-70.3335,42.06953,152.91771], "fy":[-60.81716,-186.02741,-194.62132,-127.48496]},
+    {"t":0.10233, "x":1.60298, "y":7.26025, "heading":-0.80632, "vx":0.45852, "vy":-0.80543, "omega":3.13797, "ax":4.88, "ay":-9.54354, "alpha":18.94636, "fx":[161.08486,-36.29252,52.92827,147.6681], "fy":[-115.71642,-195.42133,-191.85211,-133.35487]},
+    {"t":0.13644, "x":1.62146, "y":7.22722, "heading":-0.69928, "vx":0.62499, "vy":-1.13097, "omega":3.78425, "ax":4.90355, "ay":-10.5044, "alpha":9.51734, "fx":[110.83305,19.69636,67.83816,128.59167], "fy":[-164.43555,-197.59607,-186.8713,-151.51032]},
+    {"t":0.17056, "x":1.64563, "y":7.18253, "heading":-0.5702, "vx":0.79225, "vy":-1.48929, "omega":4.1089, "ax":4.81306, "ay":-10.72322, "alpha":-6.17847, "fx":[74.4954,115.43828,90.95216,40.04], "fy":[-183.90556,-161.22161,-175.77079,-194.10559]},
+    {"t":0.20467, "x":1.67546, "y":7.12549, "heading":-0.43004, "vx":0.95643, "vy":-1.85507, "omega":3.89814, "ax":4.69633, "ay":-8.55189, "alpha":-22.71177, "fx":[72.66532,178.70648,134.08155,-72.31108], "fy":[-184.64281,-85.38842,-115.86063,-184.33188]},
+    {"t":0.23878, "x":1.71082, "y":7.05724, "heading":-0.29707, "vx":1.11663, "vy":-2.14678, "omega":3.12342, "ax":2.88316, "ay":-9.50527, "alpha":-21.98692, "fx":[84.70621,180.55134,-19.24384,-53.77012], "fy":[-179.1351,-80.22631,-184.16164,-190.26999]},
+    {"t":0.27289, "x":1.75058, "y":6.97848, "heading":-0.19052, "vx":1.21498, "vy":-2.47102, "omega":2.37342, "ax":3.17646, "ay":-9.4364, "alpha":-22.11592, "fx":[95.04143,181.06782,-28.3094,-35.99974], "fy":[-173.31599,-76.99275,-185.0526,-193.83992]},
+    {"t":0.307, "x":1.79388, "y":6.8887, "heading":-0.10956, "vx":1.32333, "vy":-2.79291, "omega":1.61902, "ax":3.52107, "ay":-9.16794, "alpha":-22.56047, "fx":[103.96311,182.37645,-28.31932,-23.24227], "fy":[-166.89637,-69.27061,-180.63216,-194.50145]},
+    {"t":0.34111, "x":1.84106, "y":6.7881, "heading":-0.05434, "vx":1.44344, "vy":-3.10564, "omega":0.84945, "ax":3.9917, "ay":-8.54923, "alpha":-22.51992, "fx":[110.77609,180.11458,-13.27203,-11.46004], "fy":[-157.94345,-58.46652,-162.78075,-190.8557]},
+    {"t":0.37522, "x":1.89262, "y":6.67719, "heading":-0.02536, "vx":1.5796, "vy":-3.39726, "omega":0.08127, "ax":0.72017, "ay":-0.48647, "alpha":-1.81193, "fx":[16.26634,16.54727,7.72898,7.47686], "fy":[-12.60815,-3.79174,-3.58607,-12.45069]},
+    {"t":0.40933, "x":1.94693, "y":6.56102, "heading":-0.02259, "vx":1.60416, "vy":-3.41385, "omega":0.01947, "ax":-3.53314, "ay":-1.61802, "alpha":-0.07396, "fx":[-58.71719,-58.73835,-59.07435,-59.05309], "fy":[-27.18267,-26.79817,-26.76087,-27.14502]},
+    {"t":0.44345, "x":1.99959, "y":6.44363, "heading":-0.02192, "vx":1.48365, "vy":-3.46905, "omega":0.01694, "ax":-3.70857, "ay":-1.50513, "alpha":0.01136, "fx":[-61.84721,-61.84401,-61.79303,-61.79623], "fy":[-25.05719,-25.11685,-25.12248,-25.06281]},
+    {"t":0.47756, "x":2.04804, "y":6.32442, "heading":-0.02135, "vx":1.35714, "vy":-3.52039, "omega":0.01733, "ax":-0.85052, "ay":-0.32317, "alpha":0.00318, "fx":[-14.18526,-14.18553,-14.17013,-14.16985], "fy":[-5.37908,-5.39463,-5.39503,-5.37949]},
+    {"t":0.51167, "x":2.09384, "y":6.20414, "heading":-0.02075, "vx":1.32813, "vy":-3.53141, "omega":0.01744, "ax":-0.23233, "ay":-0.08685, "alpha":0.001, "fx":[-3.87522,-3.87532,-3.87046,-3.87036], "fy":[-1.44522,-1.45009,-1.4502,-1.44533]},
+    {"t":0.54578, "x":2.13901, "y":6.08363, "heading":-0.02016, "vx":1.3202, "vy":-3.53438, "omega":0.01747, "ax":-0.11113, "ay":-0.04119, "alpha":0.00089, "fx":[-1.85452,-1.8546,-1.85029,-1.8502], "fy":[-0.68435,-0.68866,-0.68875,-0.68444]},
+    {"t":0.57989, "x":2.18398, "y":5.96305, "heading":-0.01956, "vx":1.31641, "vy":-3.53578, "omega":0.0175, "ax":-0.08232, "ay":-0.03026, "alpha":0.00112, "fx":[-1.37498,-1.37508,-1.36964,-1.36953], "fy":[-0.50158,-0.50702,-0.50713,-0.50169]},
+    {"t":0.614, "x":2.22883, "y":5.84242, "heading":-0.01897, "vx":1.31361, "vy":-3.53681, "omega":0.01754, "ax":-0.07293, "ay":-0.0266, "alpha":0.00136, "fx":[-1.2189,-1.21902,-1.2124,-1.21227], "fy":[-0.44007,-0.4467,-0.44682,-0.4402]},
+    {"t":0.64811, "x":2.2736, "y":5.72176, "heading":-0.01837, "vx":1.31112, "vy":-3.53772, "omega":0.01759, "ax":-0.06775, "ay":-0.02456, "alpha":0.00153, "fx":[-1.13304,-1.13317,-1.12571,-1.12558], "fy":[-0.40561,-0.41307,-0.4132,-0.40574]},
+    {"t":0.68222, "x":2.31828, "y":5.60107, "heading":-0.01777, "vx":1.30881, "vy":-3.53856, "omega":0.01764, "ax":-0.06259, "ay":-0.02257, "alpha":0.00162, "fx":[-1.04722,-1.04735,-1.03948,-1.03935], "fy":[-0.3722,-0.38007,-0.38021,-0.37234]},
+    {"t":0.71633, "x":2.36289, "y":5.48035, "heading":-0.01717, "vx":1.30667, "vy":-3.53933, "omega":0.01769, "ax":-0.05626, "ay":-0.02019, "alpha":0.00161, "fx":[-0.94166,-0.94179,-0.93395,-0.93382], "fy":[-0.33256,-0.3404,-0.34053,-0.33269]},
+    {"t":0.75045, "x":2.40743, "y":5.35961, "heading":-0.01656, "vx":1.30475, "vy":-3.54002, "omega":0.01775, "ax":-0.04806, "ay":-0.01719, "alpha":0.00146, "fx":[-0.80469,-0.80481,-0.7977,-0.79758], "fy":[-0.28294,-0.29005,-0.29016,-0.28305]},
+    {"t":0.78456, "x":2.45191, "y":5.23885, "heading":-0.01596, "vx":1.30311, "vy":-3.5406, "omega":0.0178, "ax":-0.03388, "ay":-0.01201, "alpha":0.00128, "fx":[-0.56789,-0.56798,-0.56174,-0.56164], "fy":[-0.19704,-0.20328,-0.20338,-0.19713]},
+    {"t":0.81867, "x":2.49634, "y":5.11807, "heading":-0.01535, "vx":1.30196, "vy":-3.54101, "omega":0.01784, "ax":0.01907, "ay":0.00735, "alpha":0.00089, "fx":[0.3158,0.31574,0.32008,0.32014], "fy":[0.12479,0.12045,0.12039,0.12473]},
+    {"t":0.85278, "x":2.54076, "y":4.99728, "heading":-0.01474, "vx":1.30261, "vy":-3.54076, "omega":0.01787, "ax":0.34008, "ay":0.12562, "alpha":-0.00092, "fx":[5.67114,5.67121,5.66675,5.66669], "fy":[2.09166,2.09614,2.09623,2.09175]},
+    {"t":0.88689, "x":2.5854, "y":4.87658, "heading":-0.01413, "vx":1.31421, "vy":-3.53648, "omega":0.01784, "ax":2.37174, "ay":0.91055, "alpha":-0.00945, "fx":[39.55841,39.55764,39.51317,39.51394], "fy":[15.15362,15.20116,15.20327,15.15574]},
+    {"t":0.921, "x":2.63161, "y":4.75647, "heading":-0.01352, "vx":1.39511, "vy":-3.50542, "omega":0.01752, "ax":7.84707, "ay":3.48324, "alpha":-0.01095, "fx":[130.83449,130.81458,130.7794,130.79931], "fy":[58.01751,58.089,58.11036,58.03885]},
+    {"t":0.95511, "x":2.68376, "y":4.63892, "heading":-0.01293, "vx":1.66278, "vy":-3.3866, "omega":0.01715, "ax":9.55171, "ay":5.5566, "alpha":0.52183, "fx":[157.52669,159.42461,160.88717,159.05092], "fy":[95.39285,91.79113,89.87135,93.44839]},
+    {"t":0.98922, "x":2.74604, "y":4.52664, "heading":-0.01234, "vx":1.9886, "vy":-3.19706, "omega":0.03495, "ax":6.68499, "ay":9.16873, "alpha":8.55668, "fx":[67.17323,101.51194,155.99546,121.06151], "fy":[182.05964,161.97929,114.57185,152.74249]},
+    {"t":1.02334, "x":2.81776, "y":4.42291, "heading":-0.01115, "vx":2.21664, "vy":-2.8843, "omega":0.32683, "ax":1.98486, "ay":11.51989, "alpha":-5.97505, "fx":[68.193,53.0124,5.09352,6.04763], "fy":[184.48072,190.05156,197.16883,196.42276]},
+    {"t":1.05745, "x":2.89453, "y":4.33123, "heading":0.0, "vx":2.28434, "vy":-2.49134, "omega":0.12301, "ax":-3.09748, "ay":11.44121, "alpha":-2.56216, "fx":[-42.57879,-37.64188,-59.51433,-66.79886], "fy":[193.22505,194.42736,188.92345,186.30198]},
+    {"t":1.08959, "x":2.96636, "y":4.25705, "heading":0.00395, "vx":2.18477, "vy":-2.12354, "omega":0.04064, "ax":-6.2004, "ay":10.15351, "alpha":-1.33017, "fx":[-101.43708,-95.84642,-105.2083,-110.93865], "fy":[170.48271,173.75124,168.2825,164.50026]},
+    {"t":1.12174, "x":3.03339, "y":4.19403, "heading":0.00526, "vx":1.98544, "vy":-1.79712, "omega":-0.00212, "ax":-7.57593, "ay":9.19877, "alpha":-0.67977, "fx":[-125.919,-122.69711,-126.67184,-129.86029], "fy":[153.65528,156.26384,153.07795,150.35938]},
+    {"t":1.15389, "x":3.09331, "y":4.14101, "heading":0.00519, "vx":1.74189, "vy":-1.5014, "omega":-0.02397, "ax":-8.3015, "ay":8.56429, "alpha":-0.29922, "fx":[-138.35081,-136.90335,-138.42007,-139.85415], "fy":[142.79231,144.18882,142.74106,141.32822]},
+    {"t":1.18604, "x":3.14501, "y":4.09717, "heading":0.00442, "vx":1.47501, "vy":-1.22608, "omega":-0.03359, "ax":-8.73879, "ay":8.12686, "alpha":-0.05402, "fx":[-145.67904,-145.41863,-145.66417,-145.92402], "fy":[135.4618,135.74251,135.48026,135.19915]},
+    {"t":1.21818, "x":3.18792, "y":4.06195, "heading":0.00334, "vx":1.19408, "vy":-0.96482, "omega":-0.03533, "ax":-9.02791, "ay":7.81079, "alpha":0.11583, "fx":[-150.45706,-151.01012,-150.5264,-149.9704], "fy":[130.24485,129.60104,130.16003,130.80245]},
+    {"t":1.25033, "x":3.22164, "y":4.03497, "heading":0.00221, "vx":0.90386, "vy":-0.71372, "omega":-0.03161, "ax":-9.23211, "ay":7.57297, "alpha":0.23996, "fx":[-153.7998,-154.9333,-153.99664,-152.84951], "fy":[126.36311,124.96687,126.11452,127.50633]},
+    {"t":1.28248, "x":3.24592, "y":4.01594, "heading":0.00119, "vx":0.60706, "vy":-0.47027, "omega":-0.02389, "ax":-9.38352, "ay":7.38803, "alpha":0.33446, "fx":[-156.26193,-157.82544,-156.58961,-154.99825], "fy":[123.37,121.35865,122.94297,124.94808]},
+    {"t":1.31463, "x":3.26059, "y":4.00464, "heading":0.00042, "vx":0.30541, "vy":-0.23276, "omega":-0.01314, "ax":-9.50006, "ay":7.24032, "alpha":0.40871, "fx":[-158.1477,-160.04002,-158.59654,-156.66118], "fy":[120.99484,118.47536,120.39385,122.90689]},
+    {"t":1.34678, "x":3.2655, "y":4.0009, "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/fetchToF.traj b/src/main/deploy/choreo/fetchToF.traj
new file mode 100644
index 00000000..7cb0af49
--- /dev/null
+++ b/src/main/deploy/choreo/fetchToF.traj
@@ -0,0 +1,144 @@
+{
+ "name":"fetchToF",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "intervals":54, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":2.9735233783721924, "y":3.2965428829193115, "heading":1.0253809355466683, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":4.682854175567627, "y":2.118865728378296, "heading":1.9269363946215805, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":5.068535475516535, "y":2.873601513280177, "heading":2.0943951023931953, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false},
+    {"from":0, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":1.7453292519943295}}, "enabled":true},
+    {"from":0, "to":1, "data":{"type":"KeepOutCircle", "props":{"x":1.242441799491644, "y":5.837604267057031, "r":0.3808915973251697}}, "enabled":true}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "intervals":54, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"2.9735233783721924 m", "val":2.9735233783721924}, "y":{"exp":"3.2965428829193115 m", "val":3.2965428829193115}, "heading":{"exp":"1.0253809355466685 rad", "val":1.0253809355466683}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":{"exp":"4.682854175567627 m", "val":4.682854175567627}, "y":{"exp":"2.118865728378296 m", "val":2.118865728378296}, "heading":{"exp":"1.9269363946215805 rad", "val":1.9269363946215805}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"F.x", "val":5.122885135094611}, "y":{"exp":"F.y", "val":2.978538192264317}, "heading":{"exp":"F.heading", "val":2.0943951023931953}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false},
+    {"from":0, "to":2, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"100 deg / s", "val":1.7453292519943295}}}, "enabled":true},
+    {"from":0, "to":1, "data":{"type":"KeepOutCircle", "props":{"x":{"exp":"1.242441799491644 m", "val":1.242441799491644}, "y":{"exp":"5.837604267057031 m", "val":5.837604267057031}, "r":{"exp":"0.3808915973251697 m", "val":0.3808915973251697}}}, "enabled":true}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,1.41138,2.16121,2.59249],
+  "samples":[
+    {"t":0.0, "x":1.58, "y":7.3, "heading":-0.9225, "vx":0.0, "vy":0.0, "omega":0.0, "ax":5.21054, "ay":-8.12275, "alpha":27.36671, "fx":[195.32878,-56.27088,50.07612,158.29462], "fy":[-37.15471,-190.81694,-192.77315,-120.86464]},
+    {"t":0.03003, "x":1.58235, "y":7.29634, "heading":-0.9225, "vx":0.15647, "vy":-0.24392, "omega":0.8218, "ax":5.64042, "ay":-9.1135, "alpha":19.44736, "fx":[177.98317,-11.2966,58.93904,150.46643], "fy":[-88.53196,-198.55403,-190.19192,-130.39294]},
+    {"t":0.06006, "x":1.58959, "y":7.2849, "heading":-0.89782, "vx":0.32585, "vy":-0.51759, "omega":1.4058, "ax":6.00033, "ay":-9.89778, "alpha":10.20668, "fx":[143.84101,44.73738,74.76783,136.74436], "fy":[-137.19975,-193.73662,-184.44336,-144.58547]},
+    {"t":0.09009, "x":1.60208, "y":7.2649, "heading":-0.85561, "vx":0.50603, "vy":-0.81482, "omega":1.7123, "ax":6.17014, "ay":-10.20396, "alpha":0.98986, "fx":[106.29951,97.9907,99.61343,107.50912], "fy":[-168.0134,-172.99665,-172.09716,-167.27339]},
+    {"t":0.12012, "x":1.62006, "y":7.23583, "heading":-0.80419, "vx":0.69132, "vy":-1.12124, "omega":1.74202, "ax":6.17874, "ay":-10.19352, "alpha":0.00932, "fx":[103.02583,102.95034,102.96753,103.04299], "fy":[-169.9032,-169.94903,-169.93897,-169.89315]},
+    {"t":0.15015, "x":1.64361, "y":7.19756, "heading":-0.75188, "vx":0.87686, "vy":-1.42734, "omega":1.7423, "ax":6.19136, "ay":-10.17058, "alpha":0.00001, "fx":[103.20698,103.20692,103.20694,103.20699], "fy":[-169.53868,-169.53871,-169.53871,-169.53867]},
+    {"t":0.18018, "x":1.67273, "y":7.15011, "heading":-0.69956, "vx":1.06278, "vy":-1.73276, "omega":1.7423, "ax":6.21797, "ay":-10.12467, "alpha":-0.00009, "fx":[103.65037,103.6511,103.65086,103.65012], "fy":[-168.77352,-168.77307,-168.77321,-168.77367]},
+    {"t":0.21021, "x":1.70745, "y":7.09352, "heading":-0.64724, "vx":1.24951, "vy":-2.03679, "omega":1.7423, "ax":6.29492, "ay":-9.99468, "alpha":-0.00039, "fx":[104.93238,104.93536,104.93421,104.93122], "fy":[-166.60706,-166.60516,-166.60584,-166.60773]},
+    {"t":0.24023, "x":1.74781, "y":7.02785, "heading":-0.59492, "vx":1.43854, "vy":-2.33693, "omega":1.74229, "ax":7.30314, "ay":-7.90868, "alpha":-0.62643, "fx":[120.49339,124.2949,123.04531,119.12585], "fy":[-133.46797,-129.75995,-130.16322,-133.94432]},
+    {"t":0.27026, "x":1.7943, "y":6.9541, "heading":-0.5426, "vx":1.65785, "vy":-2.57442, "omega":1.72348, "ax":1.86325, "ay":-0.7829, "alpha":-4.52113, "fx":[34.54543,45.82131,27.82973,16.04175], "fy":[-28.22282,-9.0867,2.45327,-17.3461]},
+    {"t":0.30029, "x":1.84492, "y":6.87644, "heading":-0.49084, "vx":1.7138, "vy":-2.59793, "omega":1.58771, "ax":-0.44052, "ay":-0.76217, "alpha":-1.55778, "fx":[-5.76691,-2.19316,-8.93867,-12.47412], "fy":[-17.82765,-11.18057,-7.57302,-14.23903]},
+    {"t":0.33032, "x":1.89619, "y":6.79808, "heading":-0.44316, "vx":1.70057, "vy":-2.62082, "omega":1.54093, "ax":-0.75694, "ay":-0.9531, "alpha":-1.41231, "fx":[-10.94719,-8.02979,-14.31195,-17.18221], "fy":[-20.47258,-14.30829,-11.29746,-17.47285]},
+    {"t":0.36035, "x":1.94691, "y":6.71895, "heading":-0.39689, "vx":1.67784, "vy":-2.64944, "omega":1.49852, "ax":-0.73288, "ay":-1.45116, "alpha":-2.29904, "fx":[-9.11647,-4.81203,-15.39917,-19.53948], "fy":[-31.46767,-21.34715,-16.88394,-27.06181]},
+    {"t":0.39038, "x":1.99697, "y":6.63874, "heading":-0.35189, "vx":1.65583, "vy":-2.69302, "omega":1.42948, "ax":-1.01606, "ay":-2.63068, "alpha":-4.19928, "fx":[-10.36441,-3.25195,-24.00153,-30.13103], "fy":[-56.57411,-38.60842,-30.95527,-49.27087]},
+    {"t":0.42041, "x":2.04623, "y":6.55668, "heading":-0.30896, "vx":1.62532, "vy":-2.77201, "omega":1.30338, "ax":-3.25183, "ay":-4.77672, "alpha":-6.00608, "fx":[-40.96587,-35.22233,-69.46196,-71.17592], "fy":[-98.03567,-75.39401,-60.38425,-84.68861]},
+    {"t":0.45044, "x":2.09357, "y":6.47129, "heading":-0.26982, "vx":1.52767, "vy":-2.91546, "omega":1.12302, "ax":-7.1791, "ay":-6.10299, "alpha":-5.33053, "fx":[-101.97244,-110.58485,-137.68528,-128.44584], "fy":[-123.94607,-102.63243,-77.6712,-102.68619]},
+    {"t":0.48047, "x":2.13621, "y":6.38099, "heading":-0.2361, "vx":1.31209, "vy":-3.09873, "omega":0.96295, "ax":-7.53058, "ay":-6.13608, "alpha":-6.64325, "fx":[-102.52901,-116.33543,-148.28116,-134.97907], "fy":[-130.85765,-102.97103,-71.0172,-104.29632]},
+    {"t":0.5105, "x":2.17222, "y":6.28517, "heading":-0.20718, "vx":1.08595, "vy":-3.28299, "omega":0.76345, "ax":-5.9696, "ay":-3.77389, "alpha":-4.0164, "fx":[-89.50092,-91.96436,-109.62855,-106.94766], "fy":[-78.64359,-57.78966,-47.29533,-67.90689]},
+    {"t":0.54053, "x":2.20214, "y":6.18488, "heading":-0.18426, "vx":0.90669, "vy":-3.39631, "omega":0.64285, "ax":-2.05046, "ay":-1.05956, "alpha":-0.97527, "fx":[-32.24448,-31.52041,-36.13014,-36.82581], "fy":[-20.55642,-15.799,-14.78186,-19.51206]},
+    {"t":0.57056, "x":2.22844, "y":6.08241, "heading":-0.16495, "vx":0.84511, "vy":-3.42813, "omega":0.61356, "ax":-0.69251, "ay":-0.17804, "alpha":0.16872, "fx":[-11.88154,-12.01452,-11.20633,-11.07313], "fy":[-2.49311,-3.30585,-3.44275,-2.62967]},
+    {"t":0.60059, "x":2.25351, "y":5.97939, "heading":-0.14653, "vx":0.82432, "vy":-3.43348, "omega":0.61862, "ax":-0.42064, "ay":-0.00258, "alpha":0.42141, "fx":[-7.8764,-8.17543,-6.14766,-5.84819], "fy":[1.1232,-0.90909,-1.20985,0.82386]},
+    {"t":0.63062, "x":2.27807, "y":5.87628, "heading":-0.12795, "vx":0.81168, "vy":-3.43356, "omega":0.63128, "ax":-0.38481, "ay":0.05032, "alpha":0.5433, "fx":[-7.55651,-7.89436,-5.27276,-4.93462], "fy":[2.32047,-0.30567,-0.64405,1.98434]},
+    {"t":0.66065, "x":2.30227, "y":5.7732, "heading":-0.109, "vx":0.80013, "vy":-3.43205, "omega":0.64759, "ax":-0.40439, "ay":0.08339, "alpha":0.65881, "fx":[-8.15902,-8.50913,-5.32309,-4.97292], "fy":[3.1599,-0.0317,-0.38138,2.81379]},
+    {"t":0.69068, "x":2.32612, "y":5.67017, "heading":-0.08955, "vx":0.78798, "vy":-3.42954, "omega":0.66738, "ax":-0.43762, "ay":0.10855, "alpha":0.76986, "fx":[-8.99098,-9.3285,-5.59864,-5.26145], "fy":[3.84447,0.1076,-0.2283,3.51389]},
+    {"t":0.7207, "x":2.34958, "y":5.56723, "heading":-0.06951, "vx":0.77484, "vy":-3.42628, "omega":0.6905, "ax":-0.46968, "ay":0.123, "alpha":0.86027, "fx":[-9.76901,-10.06335,-5.88931,-5.59579], "fy":[4.28561,0.10305,-0.18852,4.00115]},
+    {"t":0.75073, "x":2.37264, "y":5.4644, "heading":-0.04877, "vx":0.76074, "vy":-3.42259, "omega":0.71633, "ax":-0.49136, "ay":0.12244, "alpha":0.91276, "fx":[-10.29691,-10.51764,-6.08392,-5.8644], "fy":[4.36932,-0.07406,-0.29156,4.16013]},
+    {"t":0.78076, "x":2.39526, "y":5.36168, "heading":-0.02726, "vx":0.74598, "vy":-3.41891, "omega":0.74374, "ax":-0.49449, "ay":0.10165, "alpha":0.90699, "fx":[-10.38531,-10.50923,-6.10007,-5.97736], "fy":[3.9627,-0.4564,-0.57799,3.84929]},
+    {"t":0.81079, "x":2.41744, "y":5.25906, "heading":-0.00493, "vx":0.73113, "vy":-3.41586, "omega":0.77098, "ax":-0.47956, "ay":0.06754, "alpha":0.85787, "fx":[-10.06845,-10.09135,-5.91925,-5.89726], "fy":[3.22581,-0.9555,-0.97776,3.2106]},
+    {"t":0.84082, "x":2.43918, "y":5.15651, "heading":0.01822, "vx":0.71673, "vy":-3.41383, "omega":0.79674, "ax":-0.44797, "ay":0.02607, "alpha":0.77893, "fx":[-9.39545,-9.32728,-5.53914,-5.60782], "fy":[2.29749,-1.49829,-1.43113,2.37004]},
+    {"t":0.87085, "x":2.4605, "y":5.05401, "heading":0.04215, "vx":0.70328, "vy":-3.41305, "omega":0.82013, "ax":-0.39694, "ay":-0.01965, "alpha":0.67369, "fx":[-8.32336,-8.18495,-4.91022,-5.04875], "fy":[1.24352,-2.03659,-1.90033,1.38337]},
+    {"t":0.90088, "x":2.48144, "y":4.95151, "heading":0.06678, "vx":0.69136, "vy":-3.41364, "omega":0.84036, "ax":-0.31436, "ay":-0.06951, "alpha":0.53048, "fx":[-6.61495,-6.44181,-3.86567,-4.03866], "fy":[0.04487,-2.53394,-2.36312,0.21749]},
+    {"t":0.93091, "x":2.50206, "y":4.84897, "heading":0.09201, "vx":0.68192, "vy":-3.41573, "omega":0.85629, "ax":-0.17141, "ay":-0.12563, "alpha":0.3172, "fx":[-3.69774,-3.55524,-2.01714,-2.15946], "fy":[-1.39584,-2.93422,-2.79289,-1.25413]},
+    {"t":0.96094, "x":2.52246, "y":4.74634, "heading":0.11773, "vx":0.67677, "vy":-3.4195, "omega":0.86581, "ax":0.08771, "ay":-0.18702, "alpha":-0.01523, "fx":[1.50336,1.49463,1.42097,1.4297], "fy":[-3.14997,-3.07634,-3.0851,-3.15872]},
+    {"t":0.99097, "x":2.54282, "y":4.64357, "heading":0.14373, "vx":0.67941, "vy":-3.42511, "omega":0.86536, "ax":0.54743, "ay":-0.23185, "alpha":-0.489, "fx":[10.47003,10.13456,7.77999,8.11724], "fy":[-4.87157,-2.51209,-2.85689,-5.21855]},
+    {"t":1.021, "x":2.56347, "y":4.54061, "heading":0.16971, "vx":0.69585, "vy":-3.43208, "omega":0.85067, "ax":1.24559, "ay":-0.18619, "alpha":-0.9536, "fx":[23.4074,22.6511,18.11421,18.88072], "fy":[-5.00664,-0.393,-1.19054,-5.82445]},
+    {"t":1.05103, "x":2.58493, "y":4.43746, "heading":0.19526, "vx":0.73325, "vy":-3.43767, "omega":0.82204, "ax":1.92394, "ay":0.04962, "alpha":-0.93896, "fx":[34.6881,33.82331,29.44859,30.32459], "fy":[-1.01438,3.57295,2.6849,-1.93481]},
+    {"t":1.08106, "x":2.60781, "y":4.33425, "heading":0.21994, "vx":0.79103, "vy":-3.43618, "omega":0.79384, "ax":2.19343, "ay":0.49121, "alpha":-0.04549, "fx":[36.69281,36.6427,36.4341,36.48424], "fy":[8.09963,8.32344,8.27676,8.05286]},
+    {"t":1.11109, "x":2.63256, "y":4.23129, "heading":0.24378, "vx":0.85689, "vy":-3.42143, "omega":0.79247, "ax":2.1812, "ay":1.10579, "alpha":1.57626, "fx":[31.72627,33.78956,40.98796,38.93467], "fy":[21.48805,13.73001,15.43891,23.07496]},
+    {"t":1.14112, "x":2.65927, "y":4.12904, "heading":0.26758, "vx":0.92239, "vy":-3.38822, "omega":0.83981, "ax":2.13092, "ay":1.87147, "alpha":3.6342, "fx":[24.39753,29.96413,46.67001,41.05409], "fy":[38.21326,20.40925,24.52439,41.63924]},
+    {"t":1.17114, "x":2.68793, "y":4.02814, "heading":0.2928, "vx":0.98638, "vy":-3.33202, "omega":0.94894, "ax":2.11135, "ay":2.67722, "alpha":5.66612, "fx":[17.06457,27.05466,53.49784,43.16356], "fy":[55.45941,28.01826,34.66691,60.36754]},
+    {"t":1.20117, "x":2.7185, "y":3.92929, "heading":0.32129, "vx":1.04979, "vy":-3.25163, "omega":1.11909, "ax":1.95473, "ay":3.14479, "alpha":6.63869, "fx":[10.62603,23.47846,54.88604,41.34681], "fy":[64.53263,33.07362,41.48008,70.60191]},
+    {"t":1.2312, "x":2.75091, "y":3.83306, "heading":0.3549, "vx":1.10849, "vy":-3.15719, "omega":1.31845, "ax":1.27425, "ay":2.73124, "alpha":5.3967, "fx":[3.45512,13.83273,39.31967,28.35694], "fy":[54.10585,29.35324,37.60459,61.05007]},
+    {"t":1.26123, "x":2.78477, "y":3.73949, "heading":0.39449, "vx":1.14675, "vy":-3.07517, "omega":1.48051, "ax":0.30036, "ay":1.64606, "alpha":2.69952, "fx":[-3.73564,1.42411,13.81772,8.52112], "fy":[31.02235,18.97751,23.9493,35.80703]},
+    {"t":1.29126, "x":2.81934, "y":3.64788, "heading":0.43895, "vx":1.15577, "vy":-3.02574, "omega":1.56157, "ax":-0.07797, "ay":1.07425, "alpha":1.30428, "fx":[-5.55043,-2.84847,2.96387,0.23639], "fy":[19.42175,13.70791,16.40573,22.09385]},
+    {"t":1.32129, "x":2.85402, "y":3.55751, "heading":0.48584, "vx":1.15343, "vy":-2.99348, "omega":1.60074, "ax":1.19447, "ay":1.52426, "alpha":1.55917, "fx":[14.70561,18.4499,25.11136,21.37781], "fy":[27.13646,20.32774,23.72594,30.44444]},
+    {"t":1.35132, "x":2.88919, "y":3.4683, "heading":0.53391, "vx":1.1893, "vy":-2.94771, "omega":1.64756, "ax":6.20144, "ay":2.98282, "alpha":1.09095, "fx":[100.22181,103.59846,106.47971,103.19995], "fy":[51.83922,45.52546,47.70022,53.82353]},
+    {"t":1.38135, "x":2.9277, "y":3.38113, "heading":0.58339, "vx":1.37552, "vy":-2.85814, "omega":1.68032, "ax":10.0174, "ay":2.75603, "alpha":-4.87592, "fx":[174.06531,162.01505,159.83706,172.02383], "fy":[35.63244,69.87487,59.81776,18.44192]},
+    {"t":1.41138, "x":2.97352, "y":3.29654, "heading":0.63385, "vx":1.67634, "vy":-2.77538, "omega":1.5339, "ax":6.86802, "ay":7.43489, "alpha":8.53224, "fx":[76.21343,135.45799,143.60309,102.67189], "fy":[151.257,96.09427,103.12892,145.2639]},
+    {"t":1.43013, "x":3.00615, "y":3.24582, "heading":0.6626, "vx":1.80508, "vy":-2.63601, "omega":1.69384, "ax":7.18229, "ay":5.279, "alpha":1.43913, "fx":[115.21659,121.90843,124.06772,117.70863], "fy":[91.42871,82.47039,84.79917,93.29519]},
+    {"t":1.44887, "x":3.04125, "y":3.19734, "heading":0.69435, "vx":1.93972, "vy":-2.53705, "omega":1.72082, "ax":5.01168, "ay":3.9007, "alpha":0.1425, "fx":[83.07475,83.63492,84.00895,83.45084], "fy":[65.20473,64.51182,64.84218,65.53254]},
+    {"t":1.46762, "x":3.0785, "y":3.15046, "heading":0.72661, "vx":2.03367, "vy":-2.46393, "omega":1.72349, "ax":2.41404, "ay":2.42693, "alpha":0.05321, "fx":[40.05698,40.24515,40.42477,40.23674], "fy":[40.48159,40.2738,40.4301,40.63773]},
+    {"t":1.48636, "x":3.11704, "y":3.1047, "heading":0.75892, "vx":2.07892, "vy":-2.41843, "omega":1.72449, "ax":0.66638, "ay":1.31277, "alpha":0.02481, "fx":[11.02208,11.10702,11.19446,11.10952], "fy":[21.8866,21.79869,21.88001,21.9679]},
+    {"t":1.50511, "x":3.15613, "y":3.0596, "heading":0.79124, "vx":2.09141, "vy":-2.39382, "omega":1.72495, "ax":-0.26969, "ay":0.66299, "alpha":0.01033, "fx":[-4.53122,-4.49543,-4.45987,-4.49566], "fy":[11.05146,11.01626,11.05204,11.08723]},
+    {"t":1.52385, "x":3.19529, "y":3.01484, "heading":0.82358, "vx":2.08636, "vy":-2.3814, "omega":1.72515, "ax":-0.74038, "ay":0.30577, "alpha":0.00346, "fx":[-12.35361,-12.34134,-12.32989,-12.34216], "fy":[5.09648,5.08504,5.09746,5.1089]},
+    {"t":1.5426, "x":3.23427, "y":2.97025, "heading":0.85592, "vx":2.07248, "vy":-2.37566, "omega":1.72521, "ax":-0.97616, "ay":0.10224, "alpha":0.00022, "fx":[-16.2728,-16.27198,-16.27128,-16.27209], "fy":[1.70417,1.70346,1.70429,1.705]},
+    {"t":1.56134, "x":3.27295, "y":2.92574, "heading":0.88826, "vx":2.05418, "vy":-2.37375, "omega":1.72521, "ax":-1.09536, "ay":-0.02377, "alpha":-0.00132, "fx":[-18.25461,-18.25955,-18.26356,-18.25863], "fy":[-0.39576,-0.39167,-0.39668,-0.40077]},
+    {"t":1.58009, "x":3.31126, "y":2.88124, "heading":0.9206, "vx":2.03365, "vy":-2.37419, "omega":1.72519, "ax":-1.15568, "ay":-0.11053, "alpha":-0.00206, "fx":[-19.25762,-19.26553,-19.27153,-19.26361], "fy":[-1.84148,-1.83535,-1.84339,-1.84952]},
+    {"t":1.59884, "x":3.34918, "y":2.83671, "heading":0.95294, "vx":2.01198, "vy":-2.37626, "omega":1.72515, "ax":-1.18505, "ay":-0.17684, "alpha":-0.00241, "fx":[-19.74606,-19.75557,-19.76228,-19.75276], "fy":[-2.94649,-2.9396,-2.94926,-2.95615]},
+    {"t":1.61758, "x":3.38669, "y":2.79214, "heading":0.98528, "vx":1.98977, "vy":-2.37958, "omega":1.72511, "ax":-1.19681, "ay":-0.23174, "alpha":-0.00257, "fx":[-19.94158,-19.95196,-19.95876,-19.94838], "fy":[-3.86119,-3.85417,-3.8647,-3.87172]},
+    {"t":1.63633, "x":3.42378, "y":2.74749, "heading":1.01762, "vx":1.96733, "vy":-2.38392, "omega":1.72506, "ax":-1.1966, "ay":-0.27902, "alpha":-0.00262, "fx":[-19.93815,-19.94894,-19.95551,-19.94471], "fy":[-4.64901,-4.64221,-4.65315,-4.65995]},
+    {"t":1.65507, "x":3.46044, "y":2.70275, "heading":1.04995, "vx":1.9449, "vy":-2.38915, "omega":1.72501, "ax":-1.18492, "ay":-0.31894, "alpha":-0.00257, "fx":[-19.74362,-19.75443,-19.76053,-19.74971], "fy":[-5.31419,-5.30787,-5.31882,-5.32514]},
+    {"t":1.67382, "x":3.49669, "y":2.65791, "heading":1.08229, "vx":1.92269, "vy":-2.39513, "omega":1.72496, "ax":-1.15635, "ay":-0.34768, "alpha":-0.00241, "fx":[-19.26797,-19.27828,-19.28365,-19.27334], "fy":[-5.79322,-5.78764,-5.79807,-5.80365]},
+    {"t":1.69256, "x":3.53253, "y":2.61295, "heading":1.11463, "vx":1.90101, "vy":-2.40165, "omega":1.72491, "ax":-1.09477, "ay":-0.35397, "alpha":-0.00203, "fx":[-18.24266,-18.25151,-18.25576,-18.24691], "fy":[-5.8983,-5.89389,-5.90283,-5.90724]},
+    {"t":1.71131, "x":3.56798, "y":2.56787, "heading":1.14696, "vx":1.88049, "vy":-2.40829, "omega":1.72488, "ax":-0.9602, "ay":-0.30982, "alpha":-0.00123, "fx":[-16.00222,-16.00766,-16.01005,-16.00461], "fy":[-5.16298,-5.16051,-5.166,-5.16846]},
+    {"t":1.73006, "x":3.60306, "y":2.52267, "heading":1.17929, "vx":1.86249, "vy":-2.41409, "omega":1.72485, "ax":-0.65639, "ay":-0.14691, "alpha":0.00055, "fx":[-10.94347,-10.941,-10.93995,-10.94241], "fy":[-2.44963,-2.4507,-2.44822,-2.44715]},
+    {"t":1.7488, "x":3.63786, "y":2.47739, "heading":1.21163, "vx":1.85019, "vy":-2.41685, "omega":1.72486, "ax":0.04577, "ay":0.30245, "alpha":0.0049, "fx":[0.74752,0.76988,0.77831,0.75595], "fy":[5.03476,5.02633,5.04866,5.05709]},
+    {"t":1.76755, "x":3.67255, "y":2.43214, "heading":1.24396, "vx":1.85105, "vy":-2.41118, "omega":1.72496, "ax":1.61202, "ay":1.40883, "alpha":0.01642, "fx":[26.82186,26.89811,26.9213,26.84507], "fy":[23.46103,23.4328,23.50805,23.53628]},
+    {"t":1.78629, "x":3.70753, "y":2.38718, "heading":1.2763, "vx":1.88126, "vy":-2.38477, "omega":1.72526, "ax":4.2133, "ay":3.45415, "alpha":0.0407, "fx":[70.12849,70.32222,70.33878,70.14515], "fy":[57.53541,57.43692,57.62263,57.72109]},
+    {"t":1.80504, "x":3.74354, "y":2.34309, "heading":1.30864, "vx":1.96024, "vy":-2.32002, "omega":1.72603, "ax":6.36473, "ay":5.55101, "alpha":0.0702, "fx":[105.96132,106.31304,106.23243,105.88093], "fy":[92.50723,92.24929,92.55835,92.81585]},
+    {"t":1.82378, "x":3.7814, "y":2.30057, "heading":1.34099, "vx":2.07956, "vy":-2.21596, "omega":1.72734, "ax":7.23466, "ay":6.99639, "alpha":0.09093, "fx":[120.45532,120.94783,120.74106,120.24878], "fy":[116.64599,116.23687,116.60764,117.01535]},
+    {"t":1.84253, "x":3.82165, "y":2.26026, "heading":1.37337, "vx":2.21517, "vy":-2.08481, "omega":1.72905, "ax":7.33204, "ay":7.99155, "alpha":0.09859, "fx":[122.0732,122.65468,122.36962,121.78852], "fy":[133.27222,132.79787,133.15942,133.63158]},
+    {"t":1.86128, "x":3.86447, "y":2.22258, "heading":1.40579, "vx":2.35262, "vy":-1.935, "omega":1.7309, "ax":7.06269, "ay":8.75142, "alpha":0.09592, "fx":[117.57847,118.19198,117.88452,117.27165], "fy":[145.95862,145.4968,145.8065,146.26594]},
+    {"t":1.88002, "x":3.90981, "y":2.18785, "heading":1.43823, "vx":2.48501, "vy":-1.77095, "omega":1.73269, "ax":6.60654, "ay":9.37585, "alpha":0.08742, "fx":[109.9728,110.57454,110.28235,109.68145], "fy":[156.37231,155.96693,156.21072,156.61398]},
+    {"t":1.89877, "x":3.95755, "y":2.1563, "heading":1.47071, "vx":2.60886, "vy":-1.59519, "omega":1.73433, "ax":6.0411, "ay":9.90841, "alpha":0.07807, "fx":[100.54641,101.12013,100.85744,100.28466], "fy":[165.24618,164.9071,165.0914,165.42874]},
+    {"t":1.91751, "x":4.00752, "y":2.12814, "heading":1.50322, "vx":2.7221, "vy":-1.40945, "omega":1.7358, "ax":5.39469, "ay":10.37274, "alpha":0.08306, "fx":[89.74009,90.3872,90.11307,89.46735], "fy":[172.99279,172.66374,172.82552,173.1526]},
+    {"t":1.93626, "x":4.0595, "y":2.10354, "heading":1.53576, "vx":2.82323, "vy":-1.21501, "omega":1.73735, "ax":4.57443, "ay":10.82377, "alpha":0.14412, "fx":[75.87926,77.07235,76.62523,75.43749], "fy":[180.56889,180.07244,180.28788,180.77872]},
+    {"t":1.955, "x":4.11322, "y":2.08266, "heading":1.56833, "vx":2.90898, "vy":-1.01211, "omega":1.74005, "ax":2.69124, "ay":11.48362, "alpha":0.06772, "fx":[44.62301,45.24139,45.09953,44.48291], "fy":[191.4768,191.33376,191.37634,191.5185]},
+    {"t":1.97375, "x":4.16823, "y":2.06571, "heading":1.60095, "vx":2.95943, "vy":-0.79684, "omega":1.74132, "ax":0.41478, "ay":11.82263, "alpha":0.00723, "fx":[6.88179,6.95123,6.94671,6.87729], "fy":[197.07832,197.07593,197.07686,197.07924]},
+    {"t":1.99249, "x":4.22378, "y":2.05285, "heading":1.63359, "vx":2.9672, "vy":-0.57522, "omega":1.74146, "ax":-1.15592, "ay":11.79892, "alpha":0.00175, "fx":[-19.27734,-19.26053,-19.25996,-19.27677], "fy":[196.68145,196.68309,196.68329,196.68165]},
+    {"t":2.01124, "x":4.27919, "y":2.04414, "heading":1.66624, "vx":2.94553, "vy":-0.35404, "omega":1.74149, "ax":-2.21772, "ay":11.66462, "alpha":0.00071, "fx":[-36.97197,-36.96525,-36.96464,-36.97136], "fy":[194.44286,194.44414,194.4443,194.44303]},
+    {"t":2.02999, "x":4.33402, "y":2.03955, "heading":1.69888, "vx":2.90396, "vy":-0.13538, "omega":1.74151, "ax":-2.96231, "ay":11.51176, "alpha":0.00037, "fx":[-49.38212,-49.37871,-49.3783,-49.3817], "fy":[191.89494,191.89581,191.89594,191.89507]},
+    {"t":2.04873, "x":4.38794, "y":2.03904, "heading":1.73153, "vx":2.84843, "vy":0.08041, "omega":1.74151, "ax":-3.50613, "ay":11.36833, "alpha":0.00021, "fx":[-58.44662,-58.44469,-58.44443,-58.44636], "fy":[189.50428,189.50488,189.50497,189.50438]},
+    {"t":2.06748, "x":4.44072, "y":2.04254, "heading":1.76417, "vx":2.78271, "vy":0.29352, "omega":1.74152, "ax":-3.91777, "ay":11.24125, "alpha":0.0001, "fx":[-65.30789,-65.30694,-65.3068,-65.30775], "fy":[187.38604,187.38637,187.38643,187.38609]},
+    {"t":2.08622, "x":4.49219, "y":2.05002, "heading":1.79682, "vx":2.70926, "vy":0.50424, "omega":1.74152, "ax":-4.23878, "ay":11.13069, "alpha":-0.00093, "fx":[-70.65362,-70.66194,-70.66308,-70.65476], "fy":[185.54515,185.54199,185.54152,185.54468]},
+    {"t":2.10497, "x":4.54223, "y":2.06143, "heading":1.82946, "vx":2.62981, "vy":0.7129, "omega":1.7415, "ax":-4.49533, "ay":11.0349, "alpha":-0.04017, "fx":[-74.73339,-75.09115,-75.13706,-74.77865], "fy":[184.02926,183.88377,183.86343,184.00928]},
+    {"t":2.12371, "x":4.59074, "y":2.07673, "heading":1.86211, "vx":2.54554, "vy":0.91975, "omega":1.74075, "ax":-4.70242, "ay":10.9467, "alpha":-1.24247, "fx":[-72.42074,-83.1132,-84.66283,-73.35171], "fy":[185.04335,180.49856,179.7316,184.63125]},
+    {"t":2.14246, "x":4.63763, "y":2.09589, "heading":1.89474, "vx":2.45739, "vy":1.12495, "omega":1.71746, "ax":-4.80332, "ay":10.72226, "alpha":-7.04828, "fx":[-52.07846,-103.42444,-117.26956,-47.50397], "fy":[191.89645,169.83751,160.34837,192.85739]},
+    {"t":2.16121, "x":4.68285, "y":2.11887, "heading":1.92694, "vx":2.36735, "vy":1.32595, "omega":1.58533, "ax":-5.16557, "ay":10.59998, "alpha":-6.42761, "fx":[-60.4215,-107.46154,-119.4277,-57.11941], "fy":[189.63817,167.53638,159.0841,190.52743]},
+    {"t":2.19201, "x":4.75333, "y":2.16474, "heading":1.97577, "vx":2.20822, "vy":1.65249, "omega":1.38733, "ax":-5.84304, "ay":10.21034, "alpha":-6.91559, "fx":[-70.70537,-118.97599,-132.32085,-67.60071], "fy":[185.97353,159.48231,148.40081,186.94944]},
+    {"t":2.22282, "x":4.81858, "y":2.22049, "heading":2.01851, "vx":2.02822, "vy":1.96703, "omega":1.17429, "ax":-6.81571, "ay":9.53823, "alpha":-7.57835, "fx":[-85.19095,-134.21947,-150.35261,-84.69522], "fy":[179.68394,146.75776,129.88719,179.6618]},
+    {"t":2.25362, "x":4.87783, "y":2.28561, "heading":2.05469, "vx":1.81826, "vy":2.26086, "omega":0.94083, "ax":-8.2565, "ay":8.23996, "alpha":-8.39341, "fx":[-107.27194,-154.73348,-173.99106,-114.53098], "fy":[167.24278,124.72616,95.53898,161.9168]},
+    {"t":2.28443, "x":4.92993, "y":2.35917, "heading":2.08367, "vx":1.56391, "vy":2.5147, "omega":0.68226, "ax":-10.26859, "ay":5.41262, "alpha":-8.92368, "fx":[-143.08329,-180.27959,-195.6565,-165.67059], "fy":[137.44686,83.20491,32.14755,108.10372]},
+    {"t":2.31523, "x":4.97323, "y":2.43921, "heading":2.10469, "vx":1.24758, "vy":2.68144, "omega":0.40736, "ax":-11.59248, "ay":-0.39787, "alpha":-8.85616, "fx":[-190.14274,-198.23491,-189.4262,-195.16021], "fy":[55.20189,6.8993,-58.30836,-30.32234]},
+    {"t":2.34604, "x":5.00616, "y":2.52162, "heading":2.11724, "vx":0.89046, "vy":2.66918, "omega":0.13454, "ax":-9.63579, "ay":-6.73211, "alpha":-6.17894, "fx":[-178.33248,-176.46442,-148.08128,-139.61803], "fy":[-85.97855,-90.53374,-131.96105,-140.41069]},
+    {"t":2.37685, "x":5.02902, "y":2.60065, "heading":2.12138, "vx":0.59362, "vy":2.46179, "omega":-0.0558, "ax":-6.54828, "ay":-9.90781, "alpha":-2.93413, "fx":[-115.63984,-124.17069,-104.18059,-92.63565], "fy":[-161.20119,-154.88882,-169.03564,-175.50834]},
+    {"t":2.40765, "x":5.0442, "y":2.67179, "heading":2.11966, "vx":0.3919, "vy":2.15658, "omega":-0.14619, "ax":-4.38276, "ay":-11.08133, "alpha":-1.04148, "fx":[-74.03509,-79.58735,-72.19287,-66.41866], "fy":[-184.37406,-182.0775,-185.15466,-187.27584]},
+    {"t":2.43846, "x":5.0542, "y":2.73297, "heading":2.11516, "vx":0.25689, "vy":1.81521, "omega":-0.17828, "ax":-3.01454, "ay":-11.54197, "alpha":0.08928, "fx":[-50.24298,-49.65747,-50.2593,-50.84423], "fy":[-192.4033,-192.55355,-192.39568,-192.24364]},
+    {"t":2.46926, "x":5.06068, "y":2.78341, "heading":2.10967, "vx":0.16402, "vy":1.45965, "omega":-0.17553, "ax":-2.11469, "ay":-11.74378, "alpha":0.81143, "fx":[-35.62474,-29.77883,-34.86966,-40.73017], "fy":[-195.74713,-196.71049,-195.85858,-194.73622]},
+    {"t":2.50007, "x":5.06473, "y":2.8228, "heading":2.10426, "vx":0.09888, "vy":1.09787, "omega":-0.15053, "ax":-1.48925, "ay":-11.83973, "alpha":1.30374, "fx":[-25.91704,-16.04243,-23.65699,-33.68397], "fy":[-197.3399,-198.37378,-197.5909,-196.14604]},
+    {"t":2.53087, "x":5.06707, "y":2.85101, "heading":2.09962, "vx":0.053, "vy":0.73314, "omega":-0.11037, "ax":-1.03313, "ay":-11.88748, "alpha":1.65814, "fx":[-19.05716,-6.1243,-15.19597,-28.5095], "fy":[-198.17161,-198.97836,-198.46954,-197.01449]},
+    {"t":2.56168, "x":5.06821, "y":2.86795, "heading":2.09622, "vx":0.02117, "vy":0.36694, "omega":-0.05929, "ax":-0.68727, "ay":-11.91136, "alpha":1.92451, "fx":[-13.96663,1.31972,-8.6234,-24.55541], "fy":[-198.6337,-199.10713,-198.90141,-197.58422]},
+    {"t":2.59249, "x":5.06854, "y":2.8736, "heading":2.0944, "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/fetchToI.traj b/src/main/deploy/choreo/fetchToI.traj
new file mode 100644
index 00000000..18875e85
--- /dev/null
+++ b/src/main/deploy/choreo/fetchToI.traj
@@ -0,0 +1,85 @@
+{
+ "name":"fetchToI",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":5.334167003631592, "y":5.231322765350342, "heading":0.0, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":5.482468790097718, "y":4.939213976119822, "heading":-2.0943951023931953, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"5.334167003631592 m", "val":5.334167003631592}, "y":{"exp":"5.231322765350342 m", "val":5.231322765350342}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":{"exp":"I.x", "val":5.365372248154253}, "y":{"exp":"I.y", "val":4.933261807735684}, "heading":{"exp":"I.heading", "val":4.1887902047863905}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,1.33398,1.57861],
+  "samples":[
+    {"t":0.0, "x":1.58, "y":7.3, "heading":-0.9225, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.19863, "ay":-5.26921, "alpha":-19.61353, "fx":[117.80278,188.05418,195.24232,112.24773], "fy":[-160.63517,-65.7931,39.1921,-164.10455]},
+    {"t":0.03811, "x":1.58668, "y":7.29617, "heading":-0.9225, "vx":0.35059, "vy":-0.20083, "omega":-0.74754, "ax":9.45706, "ay":-5.23702, "alpha":-17.93093, "fx":[121.80822,187.64797,196.77194,124.35047], "fy":[-157.57539,-66.85427,30.31145,-155.07613]},
+    {"t":0.07623, "x":1.60691, "y":7.28471, "heading":-0.95099, "vx":0.71104, "vy":-0.40043, "omega":-1.43096, "ax":9.78535, "ay":-5.21127, "alpha":-15.43487, "fx":[126.52396,186.32811,198.36205,141.25428], "fy":[-153.74464,-70.32395,16.36021,-139.76923]},
+    {"t":0.11434, "x":1.64112, "y":7.26567, "heading":-1.00553, "vx":1.08399, "vy":-0.59905, "omega":-2.01924, "ax":10.14178, "ay":-5.19873, "alpha":-12.18245, "fx":[134.43686,184.0885,198.89653,158.81231], "fy":[-146.75447,-75.80137,-4.73501,-119.35079]},
+    {"t":0.15245, "x":1.6898, "y":7.23906, "heading":-1.08249, "vx":1.47054, "vy":-0.7972, "omega":-2.48356, "ax":10.48233, "ay":-5.24624, "alpha":-7.53816, "fx":[150.80327,180.97875,195.07312,172.08666], "fy":[-129.63013,-82.60103,-38.39653,-99.1817]},
+    {"t":0.19057, "x":1.75346, "y":7.20486, "heading":-1.17715, "vx":1.87006, "vy":-0.99715, "omega":-2.77087, "ax":10.6616, "ay":-5.28719, "alpha":1.72818, "fx":[182.83825,177.44998,172.34172,178.26477], "fy":[-77.41756,-88.96516,-98.61191,-87.5453]},
+    {"t":0.22868, "x":1.83248, "y":7.16302, "heading":-1.28276, "vx":2.27641, "vy":-1.19866, "omega":-2.705, "ax":9.38427, "ay":-3.7072, "alpha":20.96226, "fx":[191.88257,185.44618,71.74461,176.65142], "fy":[49.8874,-21.61519,-184.77225,-90.689]},
+    {"t":0.2668, "x":1.92606, "y":7.11464, "heading":-1.38586, "vx":2.63408, "vy":-1.33996, "omega":-1.90605, "ax":9.2953, "ay":-3.46207, "alpha":22.41269, "fx":[194.73318,186.94103,67.18608,170.93271], "fy":[34.30435,20.77709,-185.72918,-100.19617]},
+    {"t":0.30491, "x":2.03321, "y":7.06106, "heading":-1.4585, "vx":2.98836, "vy":-1.47191, "omega":-1.05182, "ax":9.02425, "ay":-3.72768, "alpha":22.77515, "fx":[194.76897,179.83295,62.07957,165.03797], "fy":[22.2318,21.68532,-185.28448,-107.18719]},
+    {"t":0.34302, "x":2.15366, "y":7.00225, "heading":-1.49859, "vx":3.33231, "vy":-1.61399, "omega":-0.18377, "ax":1.04482, "ay":-2.22522, "alpha":4.57909, "fx":[28.64059,5.25172,6.68006,29.09442], "fy":[-25.21224,-27.38722,-49.07854,-46.69513]},
+    {"t":0.38114, "x":2.28142, "y":6.93912, "heading":-1.5056, "vx":3.37213, "vy":-1.6988, "omega":-0.00925, "ax":-0.08119, "ay":-0.16298, "alpha":0.00242, "fx":[-1.34791,-1.35969,-1.35891,-1.34713], "fy":[-2.71058,-2.71136,-2.72313,-2.72235]},
+    {"t":0.41925, "x":2.40989, "y":6.87425, "heading":-1.50595, "vx":3.36903, "vy":-1.70501, "omega":-0.00915, "ax":-0.00932, "ay":-0.01843, "alpha":0.0, "fx":[-0.15544,-0.15544,-0.15544,-0.15544], "fy":[-0.30714,-0.30715,-0.30714,-0.30714]},
+    {"t":0.45736, "x":2.53829, "y":6.80925, "heading":-1.5063, "vx":3.36868, "vy":-1.70571, "omega":-0.00915, "ax":-0.00106, "ay":-0.00208, "alpha":-0.00002, "fx":[-0.01767,-0.01757,-0.01758,-0.01768], "fy":[-0.03479,-0.03478,-0.03468,-0.03469]},
+    {"t":0.49548, "x":2.66668, "y":6.74424, "heading":-1.50665, "vx":3.36864, "vy":-1.70579, "omega":-0.00915, "ax":-0.00012, "ay":-0.00023, "alpha":-0.00002, "fx":[-0.00204,-0.00193,-0.00194,-0.00205], "fy":[-0.0039,-0.00389,-0.00379,-0.00379]},
+    {"t":0.53359, "x":2.79507, "y":6.67923, "heading":-1.50699, "vx":3.36863, "vy":-1.7058, "omega":-0.00915, "ax":-0.00001, "ay":-0.00002, "alpha":-0.00002, "fx":[-0.00027,-0.00016,-0.00017,-0.00027], "fy":[-0.0004,-0.00039,-0.00028,-0.00029]},
+    {"t":0.57171, "x":2.92346, "y":6.61421, "heading":-1.50734, "vx":3.36863, "vy":-1.7058, "omega":-0.00916, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[-0.00007,0.00004,0.00004,-0.00007], "fy":[0.0,0.00001,0.00012,0.00011]},
+    {"t":0.60982, "x":3.05185, "y":6.5492, "heading":-1.50769, "vx":3.36863, "vy":-1.7058, "omega":-0.00916, "ax":0.0, "ay":0.00001, "alpha":-0.00002, "fx":[-0.00004,0.00006,0.00006,-0.00005], "fy":[0.00005,0.00005,0.00016,0.00015]},
+    {"t":0.64793, "x":3.18025, "y":6.48418, "heading":-1.50804, "vx":3.36863, "vy":-1.7058, "omega":-0.00916, "ax":0.0, "ay":0.00001, "alpha":-0.00002, "fx":[-0.00004,0.00007,0.00006,-0.00005], "fy":[0.00005,0.00006,0.00017,0.00016]},
+    {"t":0.68605, "x":3.30864, "y":6.41917, "heading":-1.50839, "vx":3.36863, "vy":-1.7058, "omega":-0.00916, "ax":0.0, "ay":0.00001, "alpha":-0.00002, "fx":[-0.00004,0.00007,0.00006,-0.00005], "fy":[0.00005,0.00006,0.00017,0.00016]},
+    {"t":0.72416, "x":3.43703, "y":6.35415, "heading":-1.50874, "vx":3.36863, "vy":-1.7058, "omega":-0.00916, "ax":0.0, "ay":0.00001, "alpha":-0.00002, "fx":[-0.00004,0.00007,0.00006,-0.00005], "fy":[0.00005,0.00006,0.00017,0.00016]},
+    {"t":0.76227, "x":3.56542, "y":6.28914, "heading":-1.50909, "vx":3.36863, "vy":-1.7058, "omega":-0.00916, "ax":0.0, "ay":0.00001, "alpha":-0.00002, "fx":[-0.00004,0.00007,0.00006,-0.00005], "fy":[0.00005,0.00006,0.00017,0.00016]},
+    {"t":0.80039, "x":3.69381, "y":6.22413, "heading":-1.50944, "vx":3.36863, "vy":-1.7058, "omega":-0.00916, "ax":0.0, "ay":0.00001, "alpha":-0.00002, "fx":[-0.00004,0.00007,0.00006,-0.00005], "fy":[0.00005,0.00006,0.00017,0.00016]},
+    {"t":0.8385, "x":3.8222, "y":6.15911, "heading":-1.50979, "vx":3.36863, "vy":-1.7058, "omega":-0.00916, "ax":0.0, "ay":0.00001, "alpha":-0.00002, "fx":[-0.00004,0.00006,0.00006,-0.00005], "fy":[0.00005,0.00005,0.00016,0.00015]},
+    {"t":0.87662, "x":3.95059, "y":6.0941, "heading":-1.51014, "vx":3.36863, "vy":-1.7058, "omega":-0.00916, "ax":0.0, "ay":0.0, "alpha":-0.00002, "fx":[-0.00007,0.00003,0.00003,-0.00008], "fy":[-0.00001,-0.00001,0.0001,0.0001]},
+    {"t":0.91473, "x":4.07898, "y":6.02908, "heading":-1.51048, "vx":3.36863, "vy":-1.7058, "omega":-0.00916, "ax":-0.00002, "ay":-0.00003, "alpha":-0.00002, "fx":[-0.00033,-0.00022,-0.00023,-0.00033], "fy":[-0.00052,-0.00051,-0.0004,-0.00041]},
+    {"t":0.95284, "x":4.20737, "y":5.96407, "heading":-1.51083, "vx":3.36863, "vy":-1.7058, "omega":-0.00916, "ax":-0.00015, "ay":-0.00029, "alpha":-0.00002, "fx":[-0.00257,-0.00247,-0.00247,-0.00258], "fy":[-0.00495,-0.00495,-0.00484,-0.00485]},
+    {"t":0.99096, "x":4.33577, "y":5.89905, "heading":-1.51118, "vx":3.36863, "vy":-1.70581, "omega":-0.00916, "ax":-0.00134, "ay":-0.00264, "alpha":-0.00002, "fx":[-0.02238,-0.02229,-0.02229,-0.02239], "fy":[-0.04409,-0.04408,-0.04399,-0.04399]},
+    {"t":1.02907, "x":4.46416, "y":5.83404, "heading":-1.51153, "vx":3.36857, "vy":-1.70591, "omega":-0.00917, "ax":-0.01182, "ay":-0.02334, "alpha":0.0, "fx":[-0.19706,-0.19708,-0.19708,-0.19706], "fy":[-0.38909,-0.38909,-0.38911,-0.38911]},
+    {"t":1.06718, "x":4.59254, "y":5.769, "heading":-1.51188, "vx":3.36812, "vy":-1.7068, "omega":-0.00917, "ax":-0.10447, "ay":-0.20564, "alpha":0.00019, "fx":[-1.74109,-1.74203,-1.74197,-1.74103], "fy":[-3.42749,-3.42754,-3.42848,-3.42843]},
+    {"t":1.1053, "x":4.72083, "y":5.7038, "heading":-1.51223, "vx":3.36414, "vy":-1.71464, "omega":-0.00916, "ax":-0.91597, "ay":-1.75358, "alpha":0.00103, "fx":[-15.2664,-15.27148,-15.27108,-15.266], "fy":[-29.22872,-29.2289,-29.23385,-29.23367]},
+    {"t":1.14341, "x":4.84839, "y":5.63717, "heading":-1.51258, "vx":3.32923, "vy":-1.78148, "omega":-0.00912, "ax":-4.39536, "ay":-7.34895, "alpha":-0.08639, "fx":[-73.41372,-72.90314,-73.12299,-73.63406], "fy":[-122.58269,-122.75354,-122.42458,-122.25317]},
+    {"t":1.18153, "x":4.97208, "y":5.56394, "heading":-1.51293, "vx":3.16171, "vy":-2.06157, "omega":-0.01241, "ax":-9.85912, "ay":-4.35398, "alpha":-12.51517, "fx":[-163.77061,-123.05542,-180.93167,-189.62918], "fy":[-99.68515,-141.89888,-28.16663,-20.56457]},
+    {"t":1.21964, "x":5.08543, "y":5.4822, "heading":-1.5134, "vx":2.78594, "vy":-2.22752, "omega":-0.48941, "ax":-10.61238, "ay":0.36801, "alpha":-18.99875, "fx":[-190.44131,-170.13722,-157.64764,-189.38696], "fy":[-51.65603,-95.34089,115.86393,55.67145]},
+    {"t":1.25775, "x":5.1839, "y":5.39757, "heading":-1.53205, "vx":2.38146, "vy":-2.21349, "omega":-1.21352, "ax":-10.7035, "ay":0.81794, "alpha":-18.64371, "fx":[-193.17425,-177.40774,-154.80739,-188.29952], "fy":[-44.50356,-85.50256,122.40591,62.13875]},
+    {"t":1.29587, "x":5.26689, "y":5.3138, "heading":-1.57831, "vx":1.97351, "vy":-2.18232, "omega":-1.92411, "ax":-10.93816, "ay":0.96515, "alpha":-16.69589, "fx":[-195.10353,-183.26506,-162.58411,-188.38272], "fy":[-37.08059,-74.50844,112.99958,62.94352]},
+    {"t":1.33398, "x":5.33417, "y":5.23132, "heading":-1.65164, "vx":1.55662, "vy":-2.14553, "omega":-2.56045, "ax":-11.23346, "ay":3.30457, "alpha":-6.55829, "fx":[-196.41272,-195.44921,-173.92682,-183.23649], "fy":[24.33156,27.40758,93.62974,74.97355]},
+    {"t":1.3528, "x":5.36147, "y":5.19153, "heading":-1.69982, "vx":1.34523, "vy":-2.08335, "omega":-2.68386, "ax":-10.40308, "ay":5.71511, "alpha":-0.33086, "fx":[-174.40999,-173.84494,-172.40214,-173.00036], "fy":[93.4614,94.4603,97.08716,96.06351]},
+    {"t":1.37162, "x":5.38494, "y":5.15334, "heading":-1.75033, "vx":1.14947, "vy":-1.9758, "omega":-2.69008, "ax":-9.20918, "ay":7.38335, "alpha":4.63954, "fx":[-132.72709,-149.58936,-170.96519,-160.76851], "fy":[146.92029,130.0555,100.09915,115.23238]},
+    {"t":1.39043, "x":5.40494, "y":5.11747, "heading":-1.80095, "vx":0.97618, "vy":-1.83687, "omega":-2.60278, "ax":-8.03789, "ay":8.42497, "alpha":8.25392, "fx":[-88.9296,-129.98095,-169.36434,-147.67611], "fy":[177.11258,149.98043,103.16156,131.50613]},
+    {"t":1.40925, "x":5.42189, "y":5.0844, "heading":-1.84992, "vx":0.82492, "vy":-1.67833, "omega":-2.44746, "ax":-7.04313, "ay":9.06394, "alpha":10.70514, "fx":[-52.57208,-114.51692,-167.60189,-134.93164], "fy":[191.27108,162.30088,106.28717,144.50673]},
+    {"t":1.42807, "x":5.43616, "y":5.05442, "heading":-1.89598, "vx":0.69239, "vy":-1.50777, "omega":-2.24602, "ax":-6.23461, "ay":9.46886, "alpha":12.37044, "fx":[-24.48171,-102.05924,-165.73501,-123.43557], "fy":[196.99489,170.54865,109.39552,154.42616]},
+    {"t":1.44689, "x":5.44809, "y":5.02772, "heading":-1.93824, "vx":0.57507, "vy":-1.32959, "omega":-2.01324, "ax":-5.58167, "ay":9.73586, "alpha":13.54061, "fx":[-2.91782,-91.79753,-163.83174,-113.62775], "fy":[198.60329,176.38324,112.39727,161.78451]},
+    {"t":1.4657, "x":5.45792, "y":5.00443, "heading":-1.97613, "vx":0.47004, "vy":-1.14638, "omega":-1.75844, "ax":-5.05164, "ay":9.91805, "alpha":14.39419, "fx":[13.86179,-83.21602,-161.95701,-105.5225], "fy":[198.23066,180.66435,115.21746,167.20403]},
+    {"t":1.48452, "x":5.46587, "y":4.98461, "heading":-2.00922, "vx":0.37498, "vy":-0.95975, "omega":-1.48757, "ax":-4.61697, "ay":10.04649, "alpha":15.0357, "fx":[27.13236,-75.98715,-160.1683,-98.82746], "fy":[196.92635,183.87888,117.79936,171.2762]},
+    {"t":1.50334, "x":5.47211, "y":4.96833, "heading":-2.03721, "vx":0.2881, "vy":-0.7707, "omega":-1.20464, "ax":-4.25501, "ay":10.14068, "alpha":15.52641, "fx":[37.76504,-69.89789,-158.51496,-93.06811], "fy":[195.22629,186.32405,120.1022,174.50869]},
+    {"t":1.52216, "x":5.47678, "y":4.95562, "heading":-2.05988, "vx":0.20803, "vy":-0.57988, "omega":-0.91247, "ax":-3.94696, "ay":10.21359, "alpha":15.90278, "fx":[46.35173,-64.80538,-157.03876,-87.68348], "fy":[193.41786,188.19265,122.09733,177.31469]},
+    {"t":1.54097, "x":5.48, "y":4.94652, "heading":-2.07705, "vx":0.13376, "vy":-0.38768, "omega":-0.61322, "ax":-3.67693, "ay":10.2741, "alpha":16.18789, "fx":[53.30134,-60.61059,-155.77477,-82.08638], "fy":[191.66227,189.61572,123.76468,180.01477]},
+    {"t":1.55979, "x":5.48186, "y":4.94104, "heading":-2.08859, "vx":0.06457, "vy":-0.19435, "omega":-0.3086, "ax":-3.43119, "ay":10.32817, "alpha":16.39967, "fx":[58.90352,-57.24242,-154.75241,-75.69378], "fy":[190.05286,190.6852,125.08954,182.83519]},
+    {"t":1.57861, "x":5.48247, "y":4.93921, "heading":-2.0944, "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/fetchToJ.traj b/src/main/deploy/choreo/fetchToJ.traj
new file mode 100644
index 00000000..be6170a3
--- /dev/null
+++ b/src/main/deploy/choreo/fetchToJ.traj
@@ -0,0 +1,84 @@
+{
+ "name":"fetchToJ",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":5.066040992736816, "y":5.357499599456787, "heading":0.0, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":5.197827024483464, "y":5.103551976119823, "heading":-2.0943951023931953, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"5.066040992736816 m", "val":5.066040992736816}, "y":{"exp":"5.357499599456787 m", "val":5.357499599456787}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":{"exp":"J.x", "val":5.079583864905389}, "y":{"exp":"J.y", "val":5.098261807735684}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,1.26054,1.48928],
+  "samples":[
+    {"t":0.0, "x":1.58, "y":7.3, "heading":-0.9225, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.08607, "ay":-5.28672, "alpha":-20.28608, "fx":[116.26898,188.16113,194.57562,106.83609], "fy":[-161.74078,-65.47054,42.33172,-167.62871]},
+    {"t":0.03707, "x":1.58624, "y":7.29637, "heading":-0.9225, "vx":0.33686, "vy":-0.196, "omega":-0.7521, "ax":9.35394, "ay":-5.25091, "alpha":-18.60079, "fx":[120.12667,187.78515,196.1771,119.61367], "fy":[-158.8518,-66.45028,33.88221,-158.70065]},
+    {"t":0.07415, "x":1.60516, "y":7.28549, "heading":-0.95038, "vx":0.68366, "vy":-0.39068, "omega":-1.44171, "ax":9.69813, "ay":-5.21835, "alpha":-16.08914, "fx":[124.59001,186.51668,197.96649,137.57948], "fy":[-155.30509,-69.80357,20.47415,-143.31509]},
+    {"t":0.11122, "x":1.63717, "y":7.26742, "heading":-1.00384, "vx":1.04321, "vy":-0.58415, "omega":-2.03821, "ax":10.07579, "ay":-5.19108, "alpha":-12.84879, "fx":[132.03077,184.35351,198.9373,156.51261], "fy":[-148.90942,-75.13597,0.16986,-122.25583]},
+    {"t":0.1483, "x":1.68277, "y":7.2422, "heading":-1.0794, "vx":1.41677, "vy":-0.7766, "omega":-2.51458, "ax":10.44052, "ay":-5.21908, "alpha":-8.36671, "fx":[147.54214,181.35341,196.18373,171.07448], "fy":[-133.31135,-81.76187,-32.12787,-100.79716]},
+    {"t":0.18537, "x":1.74248, "y":7.20982, "heading":-1.17263, "vx":1.80384, "vy":-0.9701, "omega":-2.82477, "ax":10.68225, "ay":-5.26509, "alpha":0.37808, "fx":[179.20442,177.98341,176.91878,178.16528], "fy":[-85.44018,-87.93173,-90.08116,-87.61278]},
+    {"t":0.22245, "x":1.81669, "y":7.17023, "heading":-1.27736, "vx":2.19988, "vy":-1.1653, "omega":-2.81075, "ax":9.53343, "ay":-4.07363, "alpha":19.31854, "fx":[193.19837,185.36173,79.88613,177.22428], "fy":[44.23824,-45.07551,-181.32797,-89.45685]},
+    {"t":0.25952, "x":1.90481, "y":7.12423, "heading":-1.38156, "vx":2.55333, "vy":-1.31633, "omega":-2.09452, "ax":9.33447, "ay":-3.40646, "alpha":22.30618, "fx":[194.54259,186.90674,69.33351,171.62179], "fy":[35.04793,21.59237,-184.85881,-98.9178]},
+    {"t":0.2966, "x":2.00589, "y":7.07309, "heading":-1.45922, "vx":2.8994, "vy":-1.44262, "omega":-1.26753, "ax":9.07912, "ay":-3.66143, "alpha":22.68317, "fx":[194.71942,180.39849,64.56132,165.69911], "fy":[22.53694,23.80829,-184.36845,-106.11365]},
+    {"t":0.33367, "x":2.11962, "y":7.01709, "heading":-1.50621, "vx":3.23601, "vy":-1.57837, "omega":-0.42656, "ax":3.5871, "ay":-3.39727, "alpha":11.23752, "fx":[91.16888,33.47687,31.33337,83.20212], "fy":[-24.31183,-34.49804,-90.95894,-76.75449]},
+    {"t":0.37075, "x":2.24206, "y":6.95623, "heading":-1.52202, "vx":3.369, "vy":-1.70432, "omega":-0.00994, "ax":-0.09909, "ay":-0.20308, "alpha":0.00852, "fx":[-1.63211,-1.67354,-1.67149,-1.63006], "fy":[-3.36357,-3.3656,-3.40702,-3.40499]},
+    {"t":0.40782, "x":2.36689, "y":6.89291, "heading":-1.52239, "vx":3.36532, "vy":-1.71185, "omega":-0.00962, "ax":-0.01238, "ay":-0.02433, "alpha":0.00001, "fx":[-0.20628,-0.20633,-0.20632,-0.20628], "fy":[-0.4056,-0.4056,-0.40565,-0.40564]},
+    {"t":0.4449, "x":2.49165, "y":6.82942, "heading":-1.52275, "vx":3.36487, "vy":-1.71275, "omega":-0.00962, "ax":-0.0015, "ay":-0.00293, "alpha":-0.00002, "fx":[-0.02498,-0.02487,-0.02487,-0.02498], "fy":[-0.04894,-0.04894,-0.04883,-0.04884]},
+    {"t":0.48197, "x":2.6164, "y":6.76592, "heading":-1.52311, "vx":3.36481, "vy":-1.71286, "omega":-0.00962, "ax":-0.00018, "ay":-0.00035, "alpha":-0.00003, "fx":[-0.00306,-0.00294,-0.00295,-0.00307], "fy":[-0.00586,-0.00585,-0.00573,-0.00574]},
+    {"t":0.51904, "x":2.74115, "y":6.70242, "heading":-1.52346, "vx":3.3648, "vy":-1.71287, "omega":-0.00962, "ax":-0.00002, "ay":-0.00004, "alpha":-0.00003, "fx":[-0.00042,-0.00029,-0.00029,-0.00042], "fy":[-0.00066,-0.00065,-0.00052,-0.00053]},
+    {"t":0.55612, "x":2.8659, "y":6.63891, "heading":-1.52382, "vx":3.3648, "vy":-1.71287, "omega":-0.00962, "ax":0.0, "ay":0.0, "alpha":-0.00003, "fx":[-0.00009,0.00003,0.00002,-0.0001], "fy":[-0.00003,-0.00002,0.00011,0.0001]},
+    {"t":0.59319, "x":2.99065, "y":6.57541, "heading":-1.52418, "vx":3.3648, "vy":-1.71287, "omega":-0.00962, "ax":0.0, "ay":0.00001, "alpha":-0.00003, "fx":[-0.00006,0.00007,0.00006,-0.00006], "fy":[0.00005,0.00005,0.00018,0.00018]},
+    {"t":0.63027, "x":3.1154, "y":6.51191, "heading":-1.52453, "vx":3.3648, "vy":-1.71287, "omega":-0.00963, "ax":0.0, "ay":0.00001, "alpha":-0.00003, "fx":[-0.00005,0.00008,0.00007,-0.00006], "fy":[0.00006,0.00006,0.00019,0.00018]},
+    {"t":0.66734, "x":3.24015, "y":6.4484, "heading":-1.52489, "vx":3.3648, "vy":-1.71287, "omega":-0.00963, "ax":0.0, "ay":0.00001, "alpha":-0.00003, "fx":[-0.00005,0.00007,0.00007,-0.00006], "fy":[0.00006,0.00006,0.00019,0.00019]},
+    {"t":0.70442, "x":3.3649, "y":6.3849, "heading":-1.52525, "vx":3.3648, "vy":-1.71287, "omega":-0.00963, "ax":0.0, "ay":0.00001, "alpha":-0.00003, "fx":[-0.00005,0.00008,0.00007,-0.00006], "fy":[0.00006,0.00006,0.00019,0.00019]},
+    {"t":0.74149, "x":3.48964, "y":6.32139, "heading":-1.5256, "vx":3.3648, "vy":-1.71287, "omega":-0.00963, "ax":0.0, "ay":0.00001, "alpha":-0.00003, "fx":[-0.00005,0.00007,0.00007,-0.00006], "fy":[0.00006,0.00006,0.00019,0.00018]},
+    {"t":0.77857, "x":3.61439, "y":6.25789, "heading":-1.52596, "vx":3.3648, "vy":-1.71287, "omega":-0.00963, "ax":0.0, "ay":0.00001, "alpha":-0.00003, "fx":[-0.00007,0.00006,0.00006,-0.00007], "fy":[0.00003,0.00004,0.00017,0.00016]},
+    {"t":0.81564, "x":3.73914, "y":6.19439, "heading":-1.52632, "vx":3.3648, "vy":-1.71287, "omega":-0.00963, "ax":-0.00001, "ay":-0.00001, "alpha":-0.00003, "fx":[-0.00017,-0.00004,-0.00004,-0.00017], "fy":[-0.00016,-0.00016,-0.00003,-0.00004]},
+    {"t":0.85272, "x":3.86389, "y":6.13088, "heading":-1.52667, "vx":3.3648, "vy":-1.71287, "omega":-0.00963, "ax":-0.00006, "ay":-0.0001, "alpha":-0.00003, "fx":[-0.00099,-0.00086,-0.00087,-0.00099], "fy":[-0.00178,-0.00178,-0.00165,-0.00165]},
+    {"t":0.88979, "x":3.98864, "y":6.06738, "heading":-1.52703, "vx":3.3648, "vy":-1.71288, "omega":-0.00963, "ax":-0.00046, "ay":-0.00091, "alpha":-0.00003, "fx":[-0.0078,-0.00767,-0.00768,-0.0078], "fy":[-0.01516,-0.01516,-0.01503,-0.01504]},
+    {"t":0.92686, "x":4.11339, "y":6.00387, "heading":-1.52739, "vx":3.36478, "vy":-1.71291, "omega":-0.00963, "ax":-0.00385, "ay":-0.00755, "alpha":-0.00002, "fx":[-0.06415,-0.06406,-0.06407,-0.06415], "fy":[-0.12592,-0.12592,-0.12583,-0.12584]},
+    {"t":0.96394, "x":4.23813, "y":5.94036, "heading":-1.52775, "vx":3.36464, "vy":-1.71319, "omega":-0.00963, "ax":-0.03186, "ay":-0.06254, "alpha":0.00005, "fx":[-0.53097,-0.53123,-0.53122,-0.53096], "fy":[-1.04246,-1.04247,-1.04273,-1.04272]},
+    {"t":1.00101, "x":4.36285, "y":5.8768, "heading":-1.5281, "vx":3.36346, "vy":-1.71551, "omega":-0.00963, "ax":-0.26479, "ay":-0.51569, "alpha":0.00053, "fx":[-4.41273,-4.41529,-4.41518,-4.41262], "fy":[-8.59497,-8.59508,-8.59763,-8.59753]},
+    {"t":1.03809, "x":4.48737, "y":5.81285, "heading":-1.52846, "vx":3.35364, "vy":-1.73463, "omega":-0.00961, "ax":-2.01103, "ay":-3.69607, "alpha":-0.00051, "fx":[-33.52399,-33.52134,-33.52171,-33.52437], "fy":[-61.61285,-61.61301,-61.61067,-61.61051]},
+    {"t":1.07516, "x":4.61032, "y":5.746, "heading":-1.52882, "vx":3.27908, "vy":-1.87166, "omega":-0.00963, "ax":-5.85744, "ay":-8.27493, "alpha":-1.1148, "fx":[-98.94265,-92.38136,-96.33328,-102.9055], "fy":[-138.3716,-141.78979,-137.59299,-134.00212]},
+    {"t":1.11224, "x":4.72787, "y":5.67092, "heading":-1.52917, "vx":3.06192, "vy":-2.17845, "omega":-0.05096, "ax":-10.42693, "ay":0.36677, "alpha":-19.6169, "fx":[-189.11827,-165.09406,-153.4343,-187.60115], "fy":[-51.48763,-98.4443,117.24991,57.13761]},
+    {"t":1.14931, "x":4.83422, "y":5.59041, "heading":-1.53106, "vx":2.67535, "vy":-2.16485, "omega":-0.77825, "ax":-10.48588, "ay":1.14178, "alpha":-19.88874, "fx":[-193.29718,-176.60528,-143.57636,-185.6998], "fy":[-42.08819,-84.67181,134.4169,68.47478]},
+    {"t":1.18639, "x":4.9262, "y":5.51093, "heading":-1.55992, "vx":2.28659, "vy":-2.12252, "omega":-1.51562, "ax":-10.61282, "ay":1.2559, "alpha":-19.15651, "fx":[-194.70988,-180.1442,-147.1425,-185.64579], "fy":[-38.05375,-80.27894,131.95741,70.11624]},
+    {"t":1.22346, "x":5.00368, "y":5.4331, "heading":-1.61611, "vx":1.89312, "vy":-2.07596, "omega":-2.22584, "ax":-11.39194, "ay":1.98463, "alpha":-9.94836, "fx":[-198.48838,-197.46546,-175.68642,-187.95218], "fy":[-7.17876,-16.69596,91.90705,64.29875]},
+    {"t":1.26054, "x":5.06604, "y":5.3575, "heading":-1.69863, "vx":1.47077, "vy":-2.00238, "omega":-2.59467, "ax":-11.19154, "ay":3.89078, "alpha":-2.37829, "fx":[-190.79274,-189.67193,-181.68297,-184.08287], "fy":[52.61493,55.89106,78.15099,72.77256]},
+    {"t":1.27813, "x":5.09019, "y":5.32287, "heading":-1.74428, "vx":1.27385, "vy":-1.93392, "omega":-2.63652, "ax":-10.29682, "ay":5.8743, "alpha":2.43215, "fx":[-163.68164,-168.67869,-178.70207,-175.50978], "fy":[111.25837,103.80925,85.29608,91.32307]},
+    {"t":1.29573, "x":5.11101, "y":5.28975, "heading":-1.79068, "vx":1.09267, "vy":-1.83055, "omega":-2.59372, "ax":-9.19016, "ay":7.30164, "alpha":6.39724, "fx":[-124.07758,-147.5387,-175.84171,-165.3243], "fy":[154.34043,132.52638,91.46063,108.53174]},
+    {"t":1.31332, "x":5.12881, "y":5.25867, "heading":-1.83631, "vx":0.93096, "vy":-1.70208, "omega":-2.48116, "ax":-8.11825, "ay":8.25317, "alpha":9.34806, "fx":[-84.40426,-129.60757,-173.05574,-154.24171], "fy":[179.32351,150.35755,96.94479,123.67956]},
+    {"t":1.33092, "x":5.14394, "y":5.23, "heading":-1.87997, "vx":0.78812, "vy":-1.55686, "omega":-2.31667, "ax":-7.18582, "ay":8.88008, "alpha":11.40978, "fx":[-50.92795,-114.83168,-170.34044,-143.03671], "fy":[191.70246,162.0923,101.88539,136.42658]},
+    {"t":1.34851, "x":5.15669, "y":5.20398, "heading":-1.92073, "vx":0.66168, "vy":-1.40061, "omega":-2.11591, "ax":-6.40407, "ay":9.30373, "alpha":12.84074, "fx":[-24.3627,-102.59797,-167.71362,-132.33696], "fy":[196.98452,170.22025,106.3464,146.80368]},
+    {"t":1.36611, "x":5.16734, "y":5.18077, "heading":-1.95797, "vx":0.54899, "vy":-1.2369, "omega":-1.88997, "ax":-5.753, "ay":9.59978, "alpha":13.85563, "fx":[-3.52439,-92.36264,-165.20207,-122.50995], "fy":[198.56263,176.0739,110.36048,155.09759]},
+    {"t":1.38371, "x":5.17611, "y":5.1605, "heading":-1.99122, "vx":0.44776, "vy":-1.06799, "omega":-1.64617, "ax":-5.20829, "ay":9.81335, "alpha":14.59532, "fx":[12.93293,-83.7272,-162.83473,-113.64954], "fy":[198.2604,180.40937,113.94738,161.71802]},
+    {"t":1.4013, "x":5.18318, "y":5.14322, "heading":-2.02019, "vx":0.35612, "vy":-0.89531, "omega":-1.38935, "ax":-4.74815, "ay":9.97233, "alpha":15.14607, "fx":[26.07817,-76.41104,-160.63952,-105.62513], "fy":[197.0353,183.68232,117.12164,167.09658]},
+    {"t":1.4189, "x":5.18871, "y":5.12901, "heading":-2.04463, "vx":0.27257, "vy":-0.71984, "omega":-1.12285, "ax":-4.35411, "ay":10.09484, "alpha":15.56049, "fx":[36.686,-70.21628,-158.64187,-98.15167], "fy":[195.39904,186.1826,119.89551,171.62744]},
+    {"t":1.43649, "x":5.19284, "y":5.11791, "heading":-2.06439, "vx":0.19596, "vy":-0.54222, "omega":-0.84905, "ax":-4.0105, "ay":10.19309, "alpha":15.87201, "fx":[45.30645,-65.00126,-156.86423,-90.85337], "fy":[193.63345,188.10323,122.27992,175.63879]},
+    {"t":1.45409, "x":5.19566, "y":5.10995, "heading":-2.07933, "vx":0.12539, "vy":-0.36286, "omega":-0.56977, "ax":-3.70387, "ay":10.27535, "alpha":16.10418, "fx":[52.33227,-60.66233,-155.32612,-83.31048], "fy":[191.89837,189.57754,124.28454,179.38041]},
+    {"t":1.47168, "x":5.1973, "y":5.10515, "heading":-2.08936, "vx":0.06022, "vy":-0.18206, "omega":-0.28641, "ax":-3.42252, "ay":10.34702, "alpha":16.27726, "fx":[58.04852,-57.12184,-154.04432,-75.08916], "fy":[190.28618,190.70016,125.91743,183.01591]},
+    {"t":1.48928, "x":5.19783, "y":5.10355, "heading":-2.0944, "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/fetchToK.traj b/src/main/deploy/choreo/fetchToK.traj
new file mode 100644
index 00000000..1c9ae16e
--- /dev/null
+++ b/src/main/deploy/choreo/fetchToK.traj
@@ -0,0 +1,62 @@
+{
+ "name":"fetchToK",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":4.1950062900977185, "y":5.342536486719823, "heading":-1.0471975511965976, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"K.x", "val":4.141549248154255}, "y":{"exp":"K.y", "val":5.238261807735684}, "heading":{"exp":"K.heading", "val":5.235987755982989}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,1.18221],
+  "samples":[
+    {"t":0.0, "x":1.58, "y":7.3, "heading":-0.9225, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.56034, "ay":-7.15659, "alpha":-1.40829, "fx":[154.37156,162.12086,164.50001,156.47267], "fy":[-125.88713,-115.74236,-112.3134,-123.2447]},
+    {"t":0.04379, "x":1.58916, "y":7.29314, "heading":-0.9225, "vx":0.41861, "vy":-0.31336, "omega":-0.06166, "ax":9.55917, "ay":-7.15582, "alpha":-1.35316, "fx":[154.54533,161.99642,164.27658,156.56876], "fy":[-125.62332,-115.86251,-112.58174,-123.06857]},
+    {"t":0.08757, "x":1.61666, "y":7.27256, "heading":-0.9252, "vx":0.83716, "vy":-0.62668, "omega":-0.12091, "ax":9.5575, "ay":-7.15471, "alpha":-1.27582, "fx":[154.78137,161.8092,163.96912,156.71578], "fy":[-125.26116,-116.04793,-112.9478,-122.80554]},
+    {"t":0.13136, "x":1.66247, "y":7.23826, "heading":-0.93049, "vx":1.25564, "vy":-0.93995, "omega":-0.17677, "ax":9.55491, "ay":-7.153, "alpha":-1.15948, "fx":[155.13486,161.52318,163.50748,156.93731], "fy":[-124.71494,-116.331,-113.49365,-122.40849]},
+    {"t":0.17514, "x":1.72661, "y":7.19025, "heading":-0.93824, "vx":1.67401, "vy":-1.25315, "omega":-0.22754, "ax":9.55038, "ay":-7.14999, "alpha":-0.96481, "fx":[155.734,161.05181,162.72724,157.288], "fy":[-123.78218,-116.78927,-114.40851,-121.7674]},
+    {"t":0.21893, "x":1.80906, "y":7.12852, "heading":-0.9482, "vx":2.09218, "vy":-1.56622, "omega":-0.26979, "ax":9.54053, "ay":-7.14339, "alpha":-0.57288, "fx":[156.96151,160.12407,161.13108,157.92776], "fy":[-121.84056,-117.66276,-116.24903,-120.55496]},
+    {"t":0.26271, "x":1.90982, "y":7.0531, "heading":-0.96001, "vx":2.50992, "vy":-1.87899, "omega":-0.29487, "ax":9.50451, "ay":-7.11882, "alpha":0.61971, "fx":[160.7023,157.25566,156.19134,159.5931], "fy":[-115.59533,-120.21933,-121.66558,-117.18925]},
+    {"t":0.3065, "x":2.02883, "y":6.964, "heading":-0.97292, "vx":2.92608, "vy":-2.1907, "omega":-0.26774, "ax":2.25073, "ay":-1.67377, "alpha":6.10391, "fx":[42.93959,16.90104,33.04672,57.18731], "fy":[-6.55303,-25.1991,-48.88199,-30.96965]},
+    {"t":0.35028, "x":2.1591, "y":6.86648, "heading":-0.98465, "vx":3.02463, "vy":-2.26398, "omega":-0.00047, "ax":0.00069, "ay":0.00046, "alpha":0.00035, "fx":[0.01178,0.01035,0.0113,0.01272], "fy":[0.0088,0.00785,0.00643,0.00737]},
+    {"t":0.39407, "x":2.29154, "y":6.76735, "heading":-0.98467, "vx":3.02466, "vy":-2.26396, "omega":-0.00046, "ax":0.00004, "ay":0.00005, "alpha":0.0, "fx":[0.00063,0.00063,0.00063,0.00063], "fy":[0.00085,0.00085,0.00085,0.00085]},
+    {"t":0.43786, "x":2.42398, "y":6.66822, "heading":-0.98469, "vx":3.02466, "vy":-2.26396, "omega":-0.00046, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00005,0.00005,0.00005,0.00005], "fy":[0.00007,0.00007,0.00007,0.00007]},
+    {"t":0.48164, "x":2.55641, "y":6.56909, "heading":-0.98471, "vx":3.02466, "vy":-2.26396, "omega":-0.00046, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.00001,0.00001,0.00001,0.00001]},
+    {"t":0.52543, "x":2.68885, "y":6.46996, "heading":-0.98473, "vx":3.02466, "vy":-2.26396, "omega":-0.00046, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.56921, "x":2.82129, "y":6.37083, "heading":-0.98475, "vx":3.02466, "vy":-2.26396, "omega":-0.00046, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.613, "x":2.95372, "y":6.2717, "heading":-0.98477, "vx":3.02466, "vy":-2.26396, "omega":-0.00046, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.65678, "x":3.08616, "y":6.17257, "heading":-0.98479, "vx":3.02466, "vy":-2.26396, "omega":-0.00046, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]},
+    {"t":0.70057, "x":3.2186, "y":6.07345, "heading":-0.98481, "vx":3.02466, "vy":-2.26396, "omega":-0.00046, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00005,-0.00005,-0.00005,-0.00005], "fy":[-0.00007,-0.00007,-0.00007,-0.00007]},
+    {"t":0.74436, "x":3.35103, "y":5.97432, "heading":-0.98483, "vx":3.02466, "vy":-2.26396, "omega":-0.00046, "ax":-0.00004, "ay":-0.00005, "alpha":0.0, "fx":[-0.00064,-0.00064,-0.00064,-0.00064], "fy":[-0.00085,-0.00085,-0.00085,-0.00085]},
+    {"t":0.78814, "x":3.48347, "y":5.87519, "heading":-0.98485, "vx":3.02466, "vy":-2.26396, "omega":-0.00046, "ax":-0.00069, "ay":-0.00046, "alpha":-0.00035, "fx":[-0.01182,-0.01039,-0.01134,-0.01276], "fy":[-0.00885,-0.0079,-0.00648,-0.00742]},
+    {"t":0.83193, "x":3.6159, "y":5.77606, "heading":-0.98487, "vx":3.02463, "vy":-2.26398, "omega":-0.00047, "ax":-2.25107, "ay":1.67397, "alpha":-6.08946, "fx":[-43.17201,-17.01484,-32.82442,-57.08584], "fy":[6.65494,24.94442,48.81109,31.2064]},
+    {"t":0.87571, "x":3.74618, "y":5.67853, "heading":-0.98489, "vx":2.92606, "vy":-2.19069, "omega":-0.2671, "ax":-9.50436, "ay":7.11873, "alpha":-0.64968, "fx":[-160.83794,-157.25633,-156.04955,-159.58852], "fy":[115.40551,120.21451,121.8463,117.19707]},
+    {"t":0.9195, "x":3.86519, "y":5.58944, "heading":-0.99658, "vx":2.50991, "vy":-1.87899, "omega":-0.29555, "ax":-9.54054, "ay":7.1434, "alpha":0.5658, "fx":[-156.93459,-160.01254,-161.15289,-158.04512], "fy":[121.87394,117.81433,116.21968,120.40055]},
+    {"t":0.96328, "x":3.96594, "y":5.51401, "heading":-1.00952, "vx":2.09217, "vy":-1.56621, "omega":-0.27078, "ax":-9.55037, "ay":7.14999, "alpha":0.96485, "fx":[-155.59756,-160.80845,-162.84205,-157.55252], "fy":[123.95205,117.12475,114.24668,121.42398]},
+    {"t":1.00707, "x":4.04839, "y":5.45229, "heading":-1.02138, "vx":1.674, "vy":-1.25314, "omega":-0.22853, "ax":-9.55488, "ay":7.15298, "alpha":1.16287, "fx":[-154.91056,-161.15807,-163.69169,-157.34067], "fy":[124.99175,116.8371,113.22975,121.88847]},
+    {"t":1.05085, "x":4.11253, "y":5.40427, "heading":-1.03139, "vx":1.25563, "vy":-0.93995, "omega":-0.17761, "ax":-9.55745, "ay":7.15468, "alpha":1.28111, "fx":[-154.48785,-161.34282,-164.20574,-157.23616], "fy":[125.62129,116.69634,112.60556,122.13742]},
+    {"t":1.09464, "x":4.15835, "y":5.36998, "heading":-1.03916, "vx":0.83715, "vy":-0.62667, "omega":-0.12152, "ax":-9.55911, "ay":7.15578, "alpha":1.35967, "fx":[-154.20122,-161.45441,-164.55019,-157.17756], "fy":[126.04385,116.61746,112.18346,122.28898]},
+    {"t":1.13843, "x":4.18584, "y":5.3494, "heading":-1.04448, "vx":0.4186, "vy":-0.31335, "omega":-0.06199, "ax":-9.56028, "ay":7.15655, "alpha":1.41565, "fx":[-153.99588,-161.53194,-164.79564,-157.13732], "fy":[126.34491,116.56365,111.88107,122.39511]},
+    {"t":1.18221, "x":4.19501, "y":5.34254, "heading":-1.0472, "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/fetchToL.traj b/src/main/deploy/choreo/fetchToL.traj
new file mode 100644
index 00000000..c2277900
--- /dev/null
+++ b/src/main/deploy/choreo/fetchToL.traj
@@ -0,0 +1,61 @@
+{
+ "name":"fetchToL",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":1.58, "y":7.3, "heading":-0.9225007574586108, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":3.85576086490539, "y":5.073261807735684, "heading":5.235987755982989, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"fetch.x", "val":1.58}, "y":{"exp":"fetch.y", "val":7.3}, "heading":{"exp":"fetch.heading", "val":-0.9225007574586108}, "intervals":26, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"L.x", "val":3.85576086490539}, "y":{"exp":"L.y", "val":5.073261807735684}, "heading":{"exp":"L.heading", "val":5.235987755982989}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,1.1599],
+  "samples":[
+    {"t":0.0, "x":1.58, "y":7.3, "heading":-0.9225, "vx":0.0, "vy":0.0, "omega":0.0, "ax":8.53374, "ay":-8.35012, "alpha":-1.68607, "fx":[135.95079,146.9041,148.93542,137.22294], "fy":[-145.59464,-134.53775,-132.26291,-144.37478]},
+    {"t":0.04461, "x":1.58849, "y":7.29169, "heading":-0.9225, "vx":0.3807, "vy":-0.37251, "omega":-0.07522, "ax":8.53329, "ay":-8.34983, "alpha":-1.56474, "fx":[136.3828,146.57425,148.43588,137.59029], "fy":[-145.14667,-134.85064,-132.77418,-143.97916]},
+    {"t":0.08922, "x":1.61397, "y":7.26676, "heading":-0.92586, "vx":0.76139, "vy":-0.74501, "omega":-0.14502, "ax":8.53252, "ay":-8.34929, "alpha":-1.39371, "fx":[136.97942,146.08674,147.74458,138.12139], "fy":[-144.52186,-135.31288,-133.47452,-143.40568]},
+    {"t":0.13384, "x":1.65642, "y":7.22522, "heading":-0.93233, "vx":1.14204, "vy":-1.11749, "omega":-0.2072, "ax":8.53107, "ay":-8.3482, "alpha":-1.13457, "fx":[137.88949,145.33793,146.69701,138.91083], "fy":[-143.5584,-136.01647,-134.52283,-142.54448]},
+    {"t":0.17845, "x":1.71586, "y":7.16706, "heading":-0.94157, "vx":1.52262, "vy":-1.48992, "omega":-0.25781, "ax":8.52781, "ay":-8.34558, "alpha":-0.69561, "fx":[139.46752,144.06852,144.90378,140.17814], "fy":[-141.85967,-137.18813,-136.2837,-141.13602]},
+    {"t":0.22306, "x":1.79228, "y":7.09229, "heading":-0.95307, "vx":1.90306, "vy":-1.86223, "omega":-0.28885, "ax":8.51793, "ay":-8.33711, "alpha":0.21062, "fx":[142.82103,141.40719,141.16407,142.56676], "fy":[-138.1208,-139.5661,-139.82202,-138.3933]},
+    {"t":0.26767, "x":1.88565, "y":7.00091, "heading":-0.96596, "vx":2.28306, "vy":-2.23416, "omega":-0.27945, "ax":8.45547, "ay":-8.28041, "alpha":3.20082, "fx":[154.01859,131.62117,129.10416,149.05055], "fy":[-124.10285,-147.57134,-150.09079,-130.3569]},
+    {"t":0.31228, "x":1.99592, "y":6.893, "heading":-0.97842, "vx":2.66028, "vy":-2.60356, "omega":-0.13666, "ax":0.90305, "ay":-0.8665, "alpha":3.04911, "fx":[17.23345,4.71998,12.98054,25.2796], "fy":[-4.08942,-12.56409,-24.75408,-16.36879]},
+    {"t":0.35689, "x":2.11549, "y":6.77599, "heading":-0.98452, "vx":2.70056, "vy":-2.64222, "omega":-0.00063, "ax":0.00075, "ay":0.00061, "alpha":0.00016, "fx":[0.01269,0.01205,0.01248,0.01312], "fy":[0.01073,0.01031,0.00967,0.01009]},
+    {"t":0.40151, "x":2.23597, "y":6.65812, "heading":-0.98455, "vx":2.7006, "vy":-2.64219, "omega":-0.00062, "ax":0.00005, "ay":0.00005, "alpha":0.0, "fx":[0.00088,0.00088,0.00088,0.00088], "fy":[0.0009,0.0009,0.0009,0.0009]},
+    {"t":0.44612, "x":2.35645, "y":6.54025, "heading":-0.98458, "vx":2.7006, "vy":-2.64219, "omega":-0.00062, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00007,0.00007,0.00007,0.00007], "fy":[0.00007,0.00007,0.00007,0.00007]},
+    {"t":0.49073, "x":2.47693, "y":6.42237, "heading":-0.9846, "vx":2.7006, "vy":-2.64219, "omega":-0.00062, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00001,0.00001,0.00001,0.00001], "fy":[0.00001,0.00001,0.00001,0.00001]},
+    {"t":0.53534, "x":2.59741, "y":6.3045, "heading":-0.98463, "vx":2.7006, "vy":-2.64219, "omega":-0.00062, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.57995, "x":2.71788, "y":6.18663, "heading":-0.98466, "vx":2.7006, "vy":-2.64219, "omega":-0.00062, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.62456, "x":2.83836, "y":6.06876, "heading":-0.98469, "vx":2.7006, "vy":-2.64219, "omega":-0.00062, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[-0.00001,-0.00001,-0.00001,-0.00001]},
+    {"t":0.66918, "x":2.95884, "y":5.95088, "heading":-0.98472, "vx":2.7006, "vy":-2.64219, "omega":-0.00062, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00007,-0.00007,-0.00007,-0.00007], "fy":[-0.00007,-0.00007,-0.00007,-0.00007]},
+    {"t":0.71379, "x":3.07932, "y":5.83301, "heading":-0.98474, "vx":2.7006, "vy":-2.64219, "omega":-0.00062, "ax":-0.00005, "ay":-0.00005, "alpha":0.0, "fx":[-0.0009,-0.0009,-0.0009,-0.0009], "fy":[-0.00092,-0.00092,-0.00092,-0.00092]},
+    {"t":0.7584, "x":3.1998, "y":5.71514, "heading":-0.98477, "vx":2.7006, "vy":-2.64219, "omega":-0.00062, "ax":-0.00077, "ay":-0.00063, "alpha":-0.00016, "fx":[-0.01293,-0.01229,-0.01271,-0.01335], "fy":[-0.01097,-0.01055,-0.00991,-0.01033]},
+    {"t":0.80301, "x":3.32027, "y":5.59727, "heading":-0.9848, "vx":2.70056, "vy":-2.64222, "omega":-0.00063, "ax":-0.90421, "ay":0.86727, "alpha":-3.04617, "fx":[-17.31655,-4.76284,-12.93644,-25.27491], "fy":[4.12528,12.51263,24.74551,16.44474]},
+    {"t":0.84762, "x":3.43985, "y":5.48026, "heading":-0.98483, "vx":2.66022, "vy":-2.60353, "omega":-0.13653, "ax":-8.45473, "ay":8.27986, "alpha":-3.23837, "fx":[-154.29948,-131.75602,-128.7738,-148.91578], "fy":[123.75539,147.44423,150.37196,130.51388]},
+    {"t":0.89223, "x":3.55011, "y":5.37235, "heading":-0.99092, "vx":2.28304, "vy":-2.23415, "omega":-0.28099, "ax":-8.51789, "ay":8.33712, "alpha":-0.21713, "fx":[-142.8679,-141.42141,-141.1157,-142.55135], "fy":[138.07219,139.55111,139.87049,138.40927]},
+    {"t":0.93685, "x":3.64349, "y":5.28097, "heading":-1.00345, "vx":1.90305, "vy":-1.86222, "omega":-0.29068, "ax":-8.52778, "ay":8.34559, "alpha":0.69834, "fx":[-139.33681,-143.90723,-145.02547,-140.34646], "fy":[141.98724,137.35776,136.15491,140.96793]},
+    {"t":0.98146, "x":3.7199, "y":5.2062, "heading":-1.01642, "vx":1.52261, "vy":-1.48991, "omega":-0.25953, "ax":-8.53101, "ay":8.34818, "alpha":1.14154, "fx":[-137.59133,-144.98783,-146.96436,-139.28799], "fy":[143.84301,136.39046,134.23204,142.17486]},
+    {"t":1.02607, "x":3.77934, "y":5.14804, "heading":-1.028, "vx":1.14202, "vy":-1.11748, "omega":-0.2086, "ax":-8.53244, "ay":8.34924, "alpha":1.40296, "fx":[-136.53538,-145.56573,-148.13228,-138.69326], "fy":[144.94008,135.87421,133.04577,142.8514]},
+    {"t":1.07068, "x":3.82179, "y":5.1065, "heading":-1.03731, "vx":0.76138, "vy":-0.74501, "omega":-0.14601, "ax":-8.53318, "ay":8.34976, "alpha":1.57536, "fx":[-135.82428,-145.9173,-148.9143,-138.32045], "fy":[145.66806,135.56226,132.23916,143.27651]},
+    {"t":1.11529, "x":3.84727, "y":5.08157, "heading":-1.04382, "vx":0.3807, "vy":-0.37251, "omega":-0.07573, "ax":-8.53362, "ay":8.35004, "alpha":1.69761, "fx":[-135.31584,-146.15667,-149.47163,-138.06111], "fy":[146.18362,135.35036,131.6584,143.57218]},
+    {"t":1.1599, "x":3.85576, "y":5.07326, "heading":-1.0472, "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/startDeepToA.traj b/src/main/deploy/choreo/startDeepToA.traj
new file mode 100644
index 00000000..3d631628
--- /dev/null
+++ b/src/main/deploy/choreo/startDeepToA.traj
@@ -0,0 +1,140 @@
+{
+ "name":"startDeepToA",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":7.1008875, "y":7.2570308, "heading":3.141592653589793, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":5.715839862823486, "y":6.955289840698242, "heading":-2.505084190145257, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":4.5624847412109375, "y":6.526900768280029, "heading":-1.5707963267948966, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":3.4915122985839844, "y":6.0326056480407715, "heading":-1.1801894971536726, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":2.7500696182250977, "y":5.027539253234863, "heading":-0.4964232389825454, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":3.2654999999999994, "y":4.3309, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"startDeep.x", "val":7.1008875}, "y":{"exp":"startDeep.y", "val":7.2570308}, "heading":{"exp":"startDeep.heading", "val":3.141592653589793}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"5.715839862823486 m", "val":5.715839862823486}, "y":{"exp":"6.955289840698242 m", "val":6.955289840698242}, "heading":{"exp":"-2.505084190145257 rad", "val":-2.505084190145257}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":{"exp":"4.5624847412109375 m", "val":4.5624847412109375}, "y":{"exp":"6.526900768280029 m", "val":6.526900768280029}, "heading":{"exp":"-1.5707963267948966 rad", "val":-1.5707963267948966}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":{"exp":"3.4915122985839844 m", "val":3.4915122985839844}, "y":{"exp":"6.0326056480407715 m", "val":6.0326056480407715}, "heading":{"exp":"-1.1801894971536726 rad", "val":-1.1801894971536726}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":{"exp":"2.7500696182250977 m", "val":2.7500696182250977}, "y":{"exp":"5.027539253234863 m", "val":5.027539253234863}, "heading":{"exp":"-0.4964232389825454 rad", "val":-0.4964232389825454}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"A.x", "val":3.2654999999999994}, "y":{"exp":"A.y", "val":4.3309}, "heading":{"exp":"A.heading", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,0.64964,0.98523,1.30887,1.72044,2.14431],
+  "samples":[
+    {"t":0.0, "x":7.10089, "y":7.25703, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.98703, "ay":-0.68351, "alpha":51.68758, "fx":[30.03545,51.61357,-184.83133,-162.66523], "fy":[-196.75842,191.97313,74.15801,-114.94761]},
+    {"t":0.03094, "x":7.09898, "y":7.2567, "heading":3.14159, "vx":-0.12334, "vy":-0.02114, "omega":1.59896, "ax":-4.14924, "ay":-0.72301, "alpha":51.16733, "fx":[25.4868,46.23445,-185.94531,-162.43919], "fy":[-197.30364,193.12829,71.13776,-115.1716]},
+    {"t":0.06187, "x":7.09318, "y":7.2557, "heading":-3.09213, "vx":-0.2517, "vy":-0.04351, "omega":3.18183, "ax":-4.54265, "ay":-0.90461, "alpha":49.88583, "fx":[25.79996,19.53815,-189.75435,-158.47904], "fy":[-197.07578,197.27507,59.88388,-120.40078]},
+    {"t":0.09281, "x":7.08322, "y":7.25392, "heading":-2.9937, "vx":-0.39222, "vy":-0.0715, "omega":4.72505, "ax":-5.46426, "ay":-1.45173, "alpha":46.38521, "fx":[22.58789,-41.00113,-195.29986,-150.63319], "fy":[-197.02144,193.14038,36.83151,-129.74911]},
+    {"t":0.12374, "x":7.06847, "y":7.25102, "heading":-2.84753, "vx":-0.56126, "vy":-0.1164, "omega":6.15998, "ax":-8.49805, "ay":-4.57609, "alpha":26.18352, "fx":[-41.18479,-188.30673,-196.44537,-140.69668], "fy":[-192.34473,49.45038,-22.99055,-139.24001]},
+    {"t":0.15468, "x":7.04704, "y":7.24523, "heading":-2.65697, "vx":-0.82415, "vy":-0.25797, "omega":6.96997, "ax":-8.01607, "ay":-5.6046, "alpha":-23.97468, "fx":[-188.46436,-122.53281,-35.21495,-188.284], "fy":[-50.92258,-152.29371,-188.47126,17.98355]},
+    {"t":0.18561, "x":7.01771, "y":7.23457, "heading":-2.44135, "vx":-1.07213, "vy":-0.43135, "omega":6.22831, "ax":-8.89897, "ay":-2.14035, "alpha":-25.35479, "fx":[-194.85724,-144.09136,-104.33255,-150.08536], "fy":[-3.85878,-129.34847,-129.93915,120.43181]},
+    {"t":0.21655, "x":6.98029, "y":7.2202, "heading":-2.24868, "vx":-1.34742, "vy":-0.49756, "omega":5.44396, "ax":-9.56214, "ay":1.94527, "alpha":-21.60102, "fx":[-194.34089,-158.6342,-134.47111,-150.13897], "fy":[5.02446,-107.45085,110.63316,121.50002]},
+    {"t":0.24748, "x":6.93403, "y":7.20574, "heading":-2.08027, "vx":-1.64322, "vy":-0.43738, "omega":4.77573, "ax":-9.93867, "ay":2.00997, "alpha":-19.31499, "fx":[-193.38579,-168.14759,-139.8891,-161.26922], "fy":[-0.28474,-86.88238,116.00404,105.1842]},
+    {"t":0.27842, "x":6.87844, "y":7.19317, "heading":-1.93253, "vx":-1.95068, "vy":-0.3752, "omega":4.17822, "ax":-10.048, "ay":1.43064, "alpha":-18.40624, "fx":[-191.16835,-164.80396,-144.60109,-169.40813], "fy":[-14.31128,-86.26643,107.78896,88.18094]},
+    {"t":0.30935, "x":6.81329, "y":7.18225, "heading":-1.80328, "vx":-2.26151, "vy":-0.33094, "omega":3.60882, "ax":-9.86456, "ay":0.36132, "alpha":-18.64914, "fx":[-185.46531,-149.825,-147.35381,-175.10556], "fy":[-35.35086,-101.13769,92.34129,68.23972]},
+    {"t":0.34029, "x":6.73861, "y":7.17218, "heading":-1.69164, "vx":-2.56667, "vy":-0.31977, "omega":3.03191, "ax":-9.22173, "ay":-1.12562, "alpha":-19.31829, "fx":[-173.0428,-120.6793,-144.47865,-176.68647], "fy":[-61.28595,-122.79926,65.62821,43.4027]},
+    {"t":0.37122, "x":6.6548, "y":7.16175, "heading":-1.59785, "vx":-2.85195, "vy":-0.35459, "omega":2.43429, "ax":-7.88069, "ay":-2.80364, "alpha":-19.07933, "fx":[-150.76612,-80.94905,-124.53738,-169.21702], "fy":[-86.28072,-136.14825,22.22253,13.26504]},
+    {"t":0.40216, "x":6.5628, "y":7.14944, "heading":-1.52254, "vx":-3.09574, "vy":-0.44132, "omega":1.84407, "ax":-5.8603, "ay":-3.94977, "alpha":-16.57223, "fx":[-118.91902,-47.20771,-80.5114,-144.11565], "fy":[-98.27601,-126.74938,-21.39976,-16.93772]},
+    {"t":0.43309, "x":6.46423, "y":7.1339, "heading":-1.4655, "vx":-3.27703, "vy":-0.56351, "omega":1.33141, "ax":-3.62381, "ay":-3.66164, "alpha":-11.66535, "fx":[-79.49112,-25.54642,-39.93755,-96.65345], "fy":[-86.20225,-93.51933,-32.13374,-32.29578]},
+    {"t":0.46403, "x":6.36112, "y":7.11471, "heading":-1.42431, "vx":-3.38913, "vy":-0.67678, "omega":0.97054, "ax":-1.72977, "ay":-2.35792, "alpha":-6.08777, "fx":[-40.35326,-11.28869,-16.77472,-46.92104], "fy":[-54.79295,-52.87843,-23.10352,-26.4464]},
+    {"t":0.49496, "x":6.25545, "y":7.09265, "heading":-1.39428, "vx":-3.44264, "vy":-0.74972, "omega":0.78221, "ax":-0.69808, "ay":-1.46027, "alpha":-2.73023, "fx":[-16.9264,-3.7999,-6.26512,-19.55499], "fy":[-31.8504,-29.80749,-16.75348,-18.9567]},
+    {"t":0.5259, "x":6.14862, "y":7.06876, "heading":-1.37009, "vx":-3.46424, "vy":-0.79489, "omega":0.69775, "ax":-0.28741, "ay":-1.35153, "alpha":-1.59467, "fx":[-7.8201,-0.15345,-1.73319,-9.4573], "fy":[-27.02736,-25.55795,-18.01236,-19.51976]},
+    {"t":0.55683, "x":6.04131, "y":7.04352, "heading":-1.3485, "vx":-3.47313, "vy":-0.8367, "omega":0.64842, "ax":-0.25537, "ay":-1.67823, "alpha":-1.9022, "fx":[-7.76272,1.38455,-0.69991,-9.94938], "fy":[-33.37175,-31.4539,-22.54703,-24.52828]},
+    {"t":0.58777, "x":5.93375, "y":7.01683, "heading":-1.32844, "vx":-3.48103, "vy":-0.88862, "omega":0.58958, "ax":-1.00296, "ay":-2.56386, "alpha":-5.05428, "fx":[-25.32371,-1.14393,-7.68136,-32.72659], "fy":[-56.53547,-52.12469,-28.50965,-33.78365]},
+    {"t":0.6187, "x":5.82558, "y":6.98812, "heading":-1.3102, "vx":-3.51205, "vy":-0.96793, "omega":0.43322, "ax":-2.2915, "ay":-6.02917, "alpha":-12.59065, "fx":[-55.84516,8.34302,-15.59264,-89.69812], "fy":[-125.53502,-125.12478,-74.28058,-77.07319]},
+    {"t":0.64964, "x":5.71584, "y":6.95529, "heading":-1.2968, "vx":-3.58294, "vy":-1.15445, "omega":0.04373, "ax":4.89392, "ay":-2.85174, "alpha":11.45515, "fx":[106.63836,52.71082,59.48851,107.47973], "fy":[-6.78814,-31.83826,-89.67447,-61.84778]},
+    {"t":0.66642, "x":5.65641, "y":6.93552, "heading":-1.29607, "vx":-3.50082, "vy":-1.2023, "omega":0.23594, "ax":2.16237, "ay":-1.76856, "alpha":4.32288, "fx":[44.15163,23.35708,28.37695,48.29685], "fy":[-16.0309,-22.75867,-43.04648,-36.088]},
+    {"t":0.6832, "x":5.59797, "y":6.91509, "heading":-1.29211, "vx":-3.46454, "vy":-1.23197, "omega":0.30848, "ax":0.7843, "ay":-0.88331, "alpha":1.32411, "fx":[15.33822,9.09346,10.82973,17.0343], "fy":[-10.72196,-12.55098,-18.72992,-16.89485]},
+    {"t":0.69998, "x":5.53995, "y":6.8943, "heading":-1.28693, "vx":-3.45138, "vy":-1.24679, "omega":0.3307, "ax":0.29947, "ay":-0.43981, "alpha":0.44514, "fx":[5.73257,3.64761,4.25247,6.33533], "fy":[-5.98681,-6.59737,-8.67606,-8.06544]},
+    {"t":0.71676, "x":5.48208, "y":6.87332, "heading":-1.28138, "vx":-3.44635, "vy":-1.25417, "omega":0.33817, "ax":0.14591, "ay":-0.23433, "alpha":0.24196, "fx":[2.82931,1.69928,2.0353,3.165], "fy":[-3.17322,-3.51,-4.63901,-4.30224]},
+    {"t":0.73353, "x":5.42427, "y":6.85224, "heading":-1.27571, "vx":-3.44391, "vy":-1.25811, "omega":0.34223, "ax":0.10316, "ay":-0.14526, "alpha":0.22204, "fx":[2.07986,1.04494,1.35934,2.39408], "fy":[-1.74671,-2.06141,-3.09596,-2.78126]},
+    {"t":0.75031, "x":5.3665, "y":6.83111, "heading":-1.26997, "vx":-3.44218, "vy":-1.26054, "omega":0.34595, "ax":0.09723, "ay":-0.11577, "alpha":0.23992, "fx":[2.00584,0.88962,1.23578,2.35183], "fy":[-1.19864,-1.54504,-2.66102,-2.3146]},
+    {"t":0.76709, "x":5.30875, "y":6.80994, "heading":-1.26416, "vx":-3.44054, "vy":-1.26249, "omega":0.34998, "ax":0.10405, "ay":-0.11939, "alpha":0.25683, "fx":[2.14207,0.94931,1.3268,2.51936], "fy":[-1.20499,-1.58277,-2.77525,-2.39745]},
+    {"t":0.78387, "x":5.25104, "y":6.78874, "heading":-1.25829, "vx":-3.4388, "vy":-1.26449, "omega":0.35429, "ax":0.11592, "ay":-0.1435, "alpha":0.26463, "fx":[2.34776,1.12101,1.51714,2.74362], "fy":[-1.58058,-1.9771,-3.20342,-2.80689]},
+    {"t":0.80065, "x":5.19335, "y":6.7675, "heading":-1.25235, "vx":-3.43685, "vy":-1.2669, "omega":0.35873, "ax":0.13046, "ay":-0.1818, "alpha":0.26296, "fx":[2.58272,1.36596,1.76675,2.9832], "fy":[-2.22182,-2.62317,-3.83924,-3.43789]},
+    {"t":0.81743, "x":5.1357, "y":6.74622, "heading":-1.24633, "vx":-3.43466, "vy":-1.26995, "omega":0.36314, "ax":0.14683, "ay":-0.23063, "alpha":0.253, "fx":[2.8357,1.66713,2.05968,3.2279], "fy":[-3.06414,-3.45747,-4.62496,-4.23166]},
+    {"t":0.83421, "x":5.07809, "y":6.72488, "heading":-1.24023, "vx":-3.4322, "vy":-1.27382, "omega":0.36738, "ax":0.16456, "ay":-0.28706, "alpha":0.23609, "fx":[3.10105,2.01259,2.38542,3.47351], "fy":[-4.05474,-4.42856,-5.51548,-5.14171]},
+    {"t":0.85099, "x":5.02052, "y":6.70346, "heading":-1.23407, "vx":-3.42944, "vy":-1.27863, "omega":0.37135, "ax":0.18309, "ay":-0.34797, "alpha":0.21357, "fx":[3.37187,2.38901,2.7322,3.71471], "fy":[-5.13785,-5.48223,-6.46307,-6.11876]},
+    {"t":0.86777, "x":4.963, "y":6.68196, "heading":-1.22784, "vx":-3.42637, "vy":-1.28447, "omega":0.37493, "ax":0.20181, "ay":-0.41014, "alpha":0.18702, "fx":[3.64079,2.78168,3.08743,3.94622], "fy":[-6.25493,-6.562,-7.41867,-7.11168]},
+    {"t":0.88455, "x":4.90554, "y":6.66035, "heading":-1.22155, "vx":-3.42298, "vy":-1.29136, "omega":0.37807, "ax":0.22079, "ay":-0.47167, "alpha":0.15876, "fx":[3.91252,3.18455,3.44854,4.17627], "fy":[-7.36725,-7.63262,-8.3579,-8.09259]},
+    {"t":0.90133, "x":4.84813, "y":6.63861, "heading":-1.2152, "vx":-3.41928, "vy":-1.29927, "omega":0.38073, "ax":0.24292, "ay":-0.53638, "alpha":0.13347, "fx":[4.24199,3.63112,3.85679,4.46746], "fy":[-8.52365,-8.75074,-9.35871,-9.13168]},
+    {"t":0.91811, "x":4.79079, "y":6.61673, "heading":-1.20881, "vx":-3.4152, "vy":-1.30827, "omega":0.38297, "ax":0.27925, "ay":-0.62471, "alpha":0.12161, "fx":[4.82843,4.27269,4.48165,5.0372], "fy":[-10.0321,-10.24277,-10.79495,-10.58433]},
+    {"t":0.93489, "x":4.73353, "y":6.59469, "heading":-1.20239, "vx":-3.41051, "vy":-1.31875, "omega":0.38501, "ax":0.36078, "ay":-0.80039, "alpha":0.14328, "fx":[6.21652,5.56197,5.81174,6.46595], "fy":[-12.89179,-13.1449,-13.79254,-13.53954]},
+    {"t":0.95167, "x":4.67635, "y":6.57245, "heading":-1.19593, "vx":-3.40446, "vy":-1.33218, "omega":0.38742, "ax":0.56267, "ay":-1.23279, "alpha":0.2213, "fx":[9.69334,8.67745,9.06633,10.08095], "fy":[-19.85402,-20.25529,-21.2458,-20.84496]},
+    {"t":0.96845, "x":4.6193, "y":6.54993, "heading":-1.18943, "vx":-3.39502, "vy":-1.35287, "omega":0.39113, "ax":1.05199, "ay":-2.31143, "alpha":0.34386, "fx":[18.05394,16.42936,17.02124,18.63997], "fy":[-37.45743,-38.11622,-39.60237,-38.94572]},
+    {"t":0.98523, "x":4.56248, "y":6.5269, "heading":-1.18286, "vx":-3.37737, "vy":-1.39165, "omega":0.3969, "ax":0.66561, "ay":-2.503, "alpha":-1.13833, "fx":[9.4472,14.80648,12.77431,7.35344], "fy":[-45.21896,-43.10316,-38.21287,-40.36015]},
+    {"t":1.00226, "x":4.50505, "y":6.50283, "heading":-1.1761, "vx":-3.36603, "vy":-1.43429, "omega":0.37751, "ax":0.3547, "ay":-1.31252, "alpha":-0.60344, "fx":[5.10207,7.84849,6.72772,3.97263], "fy":[-23.78321,-22.65045,-19.97254,-21.11018]},
+    {"t":1.0193, "x":4.44777, "y":6.47821, "heading":-1.16967, "vx":-3.35999, "vy":-1.45665, "omega":0.36723, "ax":0.26277, "ay":-0.80059, "alpha":-0.23395, "fx":[4.07497,5.1291,4.6859,3.63096], "fy":[-14.09025,-13.64424,-12.6005,-13.04693]},
+    {"t":1.03633, "x":4.39057, "y":6.45328, "heading":-1.16342, "vx":-3.35551, "vy":-1.47028, "omega":0.36324, "ax":0.24041, "ay":-0.59627, "alpha":-0.05124, "fx":[3.94189,4.1717,4.07309,3.84325], "fy":[-10.10331,-10.00418,-9.7757,-9.87484]},
+    {"t":1.05336, "x":4.33345, "y":6.42815, "heading":-1.15723, "vx":-3.35142, "vy":-1.48044, "omega":0.36237, "ax":0.23434, "ay":-0.51763, "alpha":0.01625, "fx":[3.92679,3.85416,3.88589,3.95851], "fy":[-8.57649,-8.60837,-8.68067,-8.64879]},
+    {"t":1.0704, "x":4.2764, "y":6.40286, "heading":-1.15106, "vx":-3.34742, "vy":-1.48926, "omega":0.36265, "ax":0.22728, "ay":-0.48238, "alpha":0.02421, "fx":[3.81869,3.71079,3.75874,3.86664], "fy":[-7.96317,-8.01133,-8.1188,-8.07064]},
+    {"t":1.08743, "x":4.21941, "y":6.37742, "heading":-1.14488, "vx":-3.34355, "vy":-1.49747, "omega":0.36306, "ax":0.21446, "ay":-0.45569, "alpha":0.00475, "fx":[3.5808,3.55968,3.56923,3.59034], "fy":[-7.58087,-7.59045,-7.61149,-7.60191]},
+    {"t":1.10447, "x":4.16249, "y":6.35185, "heading":-1.1387, "vx":-3.3399, "vy":-1.50524, "omega":0.36314, "ax":0.19626, "ay":-0.42548, "alpha":-0.02437, "fx":[3.24236,3.35032,3.30069,3.19272], "fy":[-7.17133,-7.12154,-7.01391,-7.0637]},
+    {"t":1.1215, "x":4.10562, "y":6.32614, "heading":-1.13251, "vx":-3.33656, "vy":-1.51248, "omega":0.36273, "ax":0.17622, "ay":-0.39078, "alpha":-0.05086, "fx":[2.87764,3.1022,2.99722,2.77263], "fy":[-6.67876,-6.57352,-6.34952,-6.45477]},
+    {"t":1.13853, "x":4.04881, "y":6.30032, "heading":-1.12633, "vx":-3.33356, "vy":-1.51914, "omega":0.36186, "ax":0.16099, "ay":-0.35644, "alpha":-0.06141, "fx":[2.61269,2.88299,2.75453,2.4842], "fy":[-6.14085,-6.01214,-5.7424,-5.87113]},
+    {"t":1.15557, "x":3.99205, "y":6.2744, "heading":-1.12017, "vx":-3.33081, "vy":-1.52521, "omega":0.36081, "ax":0.16155, "ay":-0.33117, "alpha":-0.03594, "fx":[2.65211,2.80982,2.73366,2.57595], "fy":[-5.63738,-5.56107,-5.40366,-5.47997]},
+    {"t":1.1726, "x":3.93534, "y":6.24837, "heading":-1.11402, "vx":-3.32806, "vy":-1.53085, "omega":0.3602, "ax":0.1963, "ay":-0.3278, "alpha":0.06082, "fx":[3.34003,3.07391,3.2044,3.47049], "fy":[-5.26612,-5.39696,-5.66256,-5.53174]},
+    {"t":1.18963, "x":3.87868, "y":6.22224, "heading":-1.10789, "vx":-3.32472, "vy":-1.53644, "omega":0.36124, "ax":0.29768, "ay":-0.36513, "alpha":0.29495, "fx":[5.28595,3.99881,4.63895,5.9253], "fy":[-5.12292,-5.76627,-7.05016,-6.40701]},
+    {"t":1.20667, "x":3.82209, "y":6.19602, "heading":-1.10173, "vx":-3.31965, "vy":-1.54266, "omega":0.36626, "ax":0.5239, "ay":-0.4711, "alpha":0.79108, "fx":[9.59154,6.14561,7.87921,11.31644], "fy":[-5.25969,-7.0144,-10.44601,-8.69224]},
+    {"t":1.2237, "x":3.76562, "y":6.16967, "heading":-1.09549, "vx":-3.31072, "vy":-1.55068, "omega":0.37974, "ax":0.97311, "ay":-0.68616, "alpha":1.76482, "fx":[18.14189,10.45139,14.33679,21.95473], "fy":[-5.61321,-9.63136,-17.26231,-13.24507]},
+    {"t":1.24074, "x":3.70937, "y":6.14316, "heading":-1.08903, "vx":-3.29415, "vy":-1.56237, "omega":0.4098, "ax":1.77167, "ay":-1.06558, "alpha":3.48087, "fx":[33.48515,18.22775,25.8144,40.60416], "fy":[-6.07319,-14.42815,-29.4557,-21.09373]},
+    {"t":1.25777, "x":3.65351, "y":6.11639, "heading":-1.08205, "vx":-3.26397, "vy":-1.58052, "omega":0.46909, "ax":2.95597, "ay":-1.68597, "alpha":5.90756, "fx":[56.81647,30.49895,42.72076,67.06207], "fy":[-7.436,-23.30019,-48.70617,-32.97498]},
+    {"t":1.2748, "x":3.59834, "y":6.08923, "heading":-1.07405, "vx":-3.21362, "vy":-1.60924, "omega":0.56972, "ax":4.29106, "ay":-2.6871, "alpha":8.15179, "fx":[84.34428,46.62864,61.0818,94.0652], "fy":[-14.19056,-41.02272,-74.48072,-49.47694]},
+    {"t":1.29184, "x":3.54422, "y":6.06142, "heading":-1.06435, "vx":-3.14052, "vy":-1.65501, "omega":0.70858, "ax":5.39735, "ay":-4.32245, "alpha":8.69562, "fx":[108.55689,63.8346,74.96916,112.52403], "fy":[-36.95646,-74.59132,-103.72753,-72.93748]},
+    {"t":1.30887, "x":3.49151, "y":6.03261, "heading":-1.05228, "vx":-3.04859, "vy":-1.72864, "omega":0.8567, "ax":0.7547, "ay":-6.59931, "alpha":-8.47555, "fx":[2.32607,46.32817,26.97964,-25.31187], "fy":[-129.74049,-114.14414,-88.98736,-107.15762]},
+    {"t":1.32945, "x":3.42894, "y":5.99564, "heading":-1.03465, "vx":-3.03306, "vy":-1.86444, "omega":0.68228, "ax":0.06626, "ay":-4.17338, "alpha":-6.75405, "fx":[-4.94336,25.76493,8.20802,-24.61119], "fy":[-88.90082,-75.05757,-49.12661,-65.18767]},
+    {"t":1.35003, "x":3.36653, "y":5.95638, "heading":-1.02061, "vx":-3.03169, "vy":-1.95033, "omega":0.54329, "ax":0.22867, "ay":-2.4829, "alpha":-3.598, "fx":[0.79359,16.25847,7.01247,-8.81743], "fy":[-52.83079,-44.17594,-29.70612,-38.84195]},
+    {"t":1.37061, "x":3.30419, "y":5.91572, "heading":-1.00943, "vx":-3.02699, "vy":-2.00142, "omega":0.46925, "ax":0.89774, "ay":-1.89041, "alpha":-0.97714, "fx":[14.13306,18.28375,15.81301,11.62996], "fy":[-34.74144,-32.17366,-28.27056,-30.86307]},
+    {"t":1.39119, "x":3.24209, "y":5.87414, "heading":-0.99977, "vx":-3.00851, "vy":-2.04032, "omega":0.44914, "ax":2.92392, "ay":-2.53103, "alpha":2.69936, "fx":[51.75392,39.86835,45.97827,57.36097], "fy":[-32.70636,-41.18846,-51.54951,-43.3202]},
+    {"t":1.41177, "x":3.1808, "y":5.83161, "heading":-0.99053, "vx":-2.94834, "vy":-2.09241, "omega":0.50469, "ax":6.46845, "ay":-3.73809, "alpha":8.18179, "fx":[122.67795,86.10849,95.252,127.26525], "fy":[-25.87781,-66.82936,-95.30367,-61.23768]},
+    {"t":1.43234, "x":3.1215, "y":5.78776, "heading":-0.98014, "vx":-2.81523, "vy":-2.16933, "omega":0.67306, "ax":8.69231, "ay":-4.17068, "alpha":10.73637, "fx":[166.60173,126.37939,123.29413,163.31127], "fy":[-9.81711,-84.75469,-119.10853,-64.4128]},
+    {"t":1.45292, "x":3.06541, "y":5.74224, "heading":-0.96629, "vx":-2.63635, "vy":-2.25516, "omega":0.894, "ax":9.70564, "ay":-4.14178, "alpha":10.87422, "fx":[182.87978,150.03569,137.49745,176.74039], "fy":[-1.7903,-88.29964,-123.83955,-62.2367]},
+    {"t":1.4735, "x":3.01321, "y":5.69495, "heading":-0.9479, "vx":-2.43662, "vy":-2.34039, "omega":1.11778, "ax":10.26627, "ay":-3.95823, "alpha":10.20067, "fx":[189.64092,164.06772,147.83391,182.99236], "fy":[0.26389,-84.77796,-120.61653,-58.79669]},
+    {"t":1.49408, "x":2.96524, "y":5.64595, "heading":-0.92489, "vx":-2.22536, "vy":-2.42185, "omega":1.32769, "ax":10.64183, "ay":-3.71505, "alpha":9.23034, "fx":[192.97911,173.11957,156.79933,186.67907], "fy":[-0.55719,-78.64563,-113.66311,-54.84617]},
+    {"t":1.51466, "x":2.9217, "y":5.59533, "heading":-0.89757, "vx":-2.00636, "vy":-2.4983, "omega":1.51764, "ax":10.92524, "ay":-3.4422, "alpha":8.11618, "fx":[194.83033,179.44096,164.96057,189.24199], "fy":[-2.79634,-71.61732,-104.51417,-50.59133]},
+    {"t":1.53524, "x":2.88272, "y":5.54319, "heading":-0.86634, "vx":-1.78154, "vy":-2.56913, "omega":1.68466, "ax":11.15337, "ay":-3.14954, "alpha":6.9107, "fx":[195.91464,184.15012,172.41359,191.2067], "fy":[-5.79339,-64.2561,-93.81381,-46.14208]},
+    {"t":1.55582, "x":2.84842, "y":5.48965, "heading":-0.83167, "vx":-1.55202, "vy":-2.63395, "omega":1.82687, "ax":11.34222, "ay":-2.84062, "alpha":5.6368, "fx":[196.54256,187.82851,179.1171,192.78904], "fy":[-9.21714,-56.71174,-81.88506,-41.59342]},
+    {"t":1.57639, "x":2.81889, "y":5.43485, "heading":-0.79408, "vx":-1.31861, "vy":-2.6924, "omega":1.94287, "ax":11.49882, "ay":-2.51734, "alpha":4.3092, "fx":[196.86643,190.7852,184.98173,194.0861], "fy":[-12.86827,-49.00551,-68.9335,-37.04429]},
+    {"t":1.59697, "x":2.79419, "y":5.37891, "heading":-0.7541, "vx":-1.08198, "vy":-2.74421, "omega":2.03155, "ax":11.62608, "ay":-2.18161, "alpha":2.94148, "fx":[196.97225,193.18358,189.90206,195.14646], "fy":[-16.59776,-41.13518,-55.13059,-32.60196]},
+    {"t":1.61755, "x":2.77438, "y":5.32197, "heading":-0.71229, "vx":-0.84273, "vy":-2.7891, "omega":2.09208, "ax":11.72499, "ay":-1.83597, "alpha":1.5481, "fx":[196.9179,195.10744,193.77481,195.99954], "fy":[-20.27524,-33.11174,-40.65059,-28.38144]},
+    {"t":1.63813, "x":2.75952, "y":5.26419, "heading":-0.66924, "vx":-0.60145, "vy":-2.82688, "omega":2.12394, "ax":11.79594, "ay":-1.4838, "alpha":0.14429, "fx":[196.75007,196.5984,196.51345,196.6688], "fy":[-23.78102,-24.96901,-25.68494,-24.50199]},
+    {"t":1.65871, "x":2.74964, "y":5.2057, "heading":-0.62553, "vx":-0.3587, "vy":-2.85742, "omega":2.12691, "ax":11.8394, "ay":-1.12883, "alpha":-1.25724, "fx":[196.5099,197.67938,198.06172,197.17758], "fy":[-27.01614,-16.75227,-10.42189,-21.07805]},
+    {"t":1.67929, "x":2.74477, "y":5.14666, "heading":-0.58176, "vx":-0.11506, "vy":-2.88065, "omega":2.10104, "ax":11.85594, "ay":-0.76885, "alpha":-2.67882, "fx":[196.21946,198.37293,198.38595,197.55297], "fy":[-30.0042,-8.37301,5.28197,-18.17031]},
+    {"t":1.69987, "x":2.74491, "y":5.08722, "heading":-0.53853, "vx":0.12892, "vy":-2.89647, "omega":2.04591, "ax":11.83737, "ay":-0.34679, "alpha":-4.43183, "fx":[195.75247,198.6768,197.03001,197.83376], "fy":[-33.61831,1.45584,24.54701,-15.50772]},
+    {"t":1.72044, "x":2.75007, "y":5.02754, "heading":-0.49642, "vx":0.37251, "vy":-2.90361, "omega":1.95471, "ax":11.84661, "ay":0.31591, "alpha":-4.40013, "fx":[197.44245,198.4163,195.50333,198.54737], "fy":[-22.62101,12.1724,35.39767,-3.88482]},
+    {"t":1.74538, "x":2.76304, "y":4.95524, "heading":-0.44769, "vx":0.66789, "vy":-2.89573, "omega":1.845, "ax":11.77177, "ay":1.14815, "alpha":-4.861, "fx":[198.26538,196.95797,191.52347,198.17186], "fy":[-11.7263,26.14389,52.35042,9.78871]},
+    {"t":1.77031, "x":2.78335, "y":4.8834, "heading":-0.40168, "vx":0.9614, "vy":-2.8671, "omega":1.7238, "ax":11.60375, "ay":2.23969, "alpha":-4.82773, "fx":[198.33165,193.81107,185.66767,195.90535], "fy":[6.39123,43.03758,69.85354,30.05555]},
+    {"t":1.79524, "x":2.81093, "y":4.81261, "heading":-0.3587, "vx":1.25072, "vy":-2.81126, "omega":1.60343, "ax":11.23073, "ay":3.73217, "alpha":-3.80091, "fx":[194.53181,187.37719,178.1047,188.82972], "fy":[37.78508,64.85697,86.80837,59.40344]},
+    {"t":1.82018, "x":2.8456, "y":4.74367, "heading":-0.31873, "vx":1.53074, "vy":-2.7182, "omega":1.50866, "ax":10.30287, "ay":5.87586, "alpha":-1.14884, "fx":[175.5665,172.05761,167.8057,171.54581], "fy":[91.08469,97.67762,104.70829,98.32067]},
+    {"t":1.84511, "x":2.88697, "y":4.67773, "heading":-0.28111, "vx":1.78762, "vy":-2.5717, "omega":1.48001, "ax":7.53294, "ay":9.13402, "alpha":0.77318, "fx":[121.82315,124.02017,129.35975,127.07888], "fy":[155.34466,153.50633,149.07573,151.11226]},
+    {"t":1.87004, "x":2.93389, "y":4.61644, "heading":-0.24421, "vx":1.97544, "vy":-2.34396, "omega":1.49929, "ax":1.66573, "ay":11.70821, "alpha":-2.1321, "fx":[37.3307,38.40291,19.2366,16.09718], "fy":[193.72525,193.72553,196.5742,196.656]},
+    {"t":1.89498, "x":2.98366, "y":4.56164, "heading":-0.20683, "vx":2.01697, "vy":-2.05203, "omega":1.44613, "ax":-3.31006, "ay":11.3269, "alpha":-4.32346, "fx":[-45.77857,-29.17881,-62.74133,-83.00948], "fy":[192.19058,195.73981,187.81736,179.50805]},
+    {"t":1.91991, "x":3.03292, "y":4.514, "heading":-0.17077, "vx":1.93444, "vy":-1.76962, "omega":1.33833, "ax":-5.98801, "ay":10.15756, "alpha":-5.24877, "fx":[-97.38233,-68.71217,-102.93272,-130.24169], "fy":[172.33041,185.95199,169.5946,149.4098]},
+    {"t":1.94484, "x":3.07929, "y":4.47303, "heading":-0.1374, "vx":1.78514, "vy":-1.51636, "omega":1.20746, "ax":-7.39578, "ay":9.18202, "alpha":-5.66547, "fx":[-125.49204,-91.66087,-123.80653,-152.17697], "fy":[153.45745,176.04279,155.30689,127.43253]},
+    {"t":1.96978, "x":3.1215, "y":4.43808, "heading":-0.10729, "vx":1.60074, "vy":-1.28742, "omega":1.06621, "ax":-8.2068, "ay":8.46678, "alpha":-5.88447, "fx":[-141.44662,-105.90688,-136.05341,-163.80646], "fy":[139.1873,168.0513,144.90257,112.4075]},
+    {"t":1.99471, "x":3.15886, "y":4.40861, "heading":-0.08071, "vx":1.39612, "vy":-1.07631, "omega":0.91949, "ax":-8.7183, "ay":7.94187, "alpha":-6.01194, "fx":[-151.21947,-115.3805,-143.9725,-170.74717], "fy":[128.73348,161.83711,137.18548,101.79259]},
+    {"t":2.01964, "x":3.19096, "y":4.38424, "heading":-0.05778, "vx":1.17874, "vy":-0.8783, "omega":0.76959, "ax":-9.06496, "ay":7.54674, "alpha":-6.09182, "fx":[-157.65071,-122.05009,-149.46368,-175.26948], "fy":[120.95366,156.97817,131.29359,93.97649]},
+    {"t":2.04458, "x":3.21753, "y":4.36469, "heading":-0.0386, "vx":0.95272, "vy":-0.69013, "omega":0.6177, "ax":-9.31324, "ay":7.24092, "alpha":-6.14512, "fx":[-162.15152,-126.96338,-153.46379,-178.4105], "fy":[114.9953,153.11784,126.68265,88.01488]},
+    {"t":2.06951, "x":3.23839, "y":4.34974, "heading":-0.02319, "vx":0.72051, "vy":-0.50959, "omega":0.46448, "ax":-9.49886, "ay":6.9982, "alpha":-6.18292, "fx":[-165.47331,-130.71552,-156.47948,-180.69715], "fy":[110.28108,149.99663,123.00864,83.34035]},
+    {"t":2.09444, "x":3.2534, "y":4.3392, "heading":-0.01161, "vx":0.48368, "vy":-0.3351, "omega":0.31032, "ax":-9.64238, "ay":6.80133, "alpha":-6.21148, "fx":[-168.04363,-133.66489,-158.80579,-182.42093], "fy":[106.42283,147.43106,120.04803,79.59756]},
+    {"t":2.11938, "x":3.26247, "y":4.33296, "heading":-0.00388, "vx":0.24326, "vy":-0.16552, "omega":0.15545, "ax":-9.7564, "ay":6.63864, "alpha":-6.23459, "fx":[-170.12003,-136.03853,-160.62509,-183.75442], "fy":[103.15532,145.29071,117.65079,76.55484]},
+    {"t":2.14431, "x":3.2655, "y":4.3309, "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/startDeepToB.traj b/src/main/deploy/choreo/startDeepToB.traj
new file mode 100644
index 00000000..d3a5b4ef
--- /dev/null
+++ b/src/main/deploy/choreo/startDeepToB.traj
@@ -0,0 +1,140 @@
+{
+ "name":"startDeepToB",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":7.1008875, "y":7.2570308, "heading":3.141592653589793, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":5.715839862823486, "y":6.955289840698242, "heading":-2.505084190145257, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":4.595438003540039, "y":6.493947982788086, "heading":-1.5707963267948966, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":3.4256062507629395, "y":5.90079402923584, "heading":-1.1801894971536726, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":2.7665462493896484, "y":4.763915061950684, "heading":-0.4964232389825454, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":3.2654999999999994, "y":4.0009, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"startDeep.x", "val":7.1008875}, "y":{"exp":"startDeep.y", "val":7.2570308}, "heading":{"exp":"startDeep.heading", "val":3.141592653589793}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"5.715839862823486 m", "val":5.715839862823486}, "y":{"exp":"6.955289840698242 m", "val":6.955289840698242}, "heading":{"exp":"-2.505084190145257 rad", "val":-2.505084190145257}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":{"exp":"4.595438003540039 m", "val":4.595438003540039}, "y":{"exp":"6.493947982788086 m", "val":6.493947982788086}, "heading":{"exp":"-1.5707963267948966 rad", "val":-1.5707963267948966}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":{"exp":"3.4256062507629395 m", "val":3.4256062507629395}, "y":{"exp":"5.90079402923584 m", "val":5.90079402923584}, "heading":{"exp":"-1.1801894971536726 rad", "val":-1.1801894971536726}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
+    {"x":{"exp":"2.7665462493896484 m", "val":2.7665462493896484}, "y":{"exp":"4.763915061950684 m", "val":4.763915061950684}, "heading":{"exp":"-0.4964232389825454 rad", "val":-0.4964232389825454}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"B.x", "val":3.2654999999999994}, "y":{"exp":"B.y", "val":4.0009}, "heading":{"exp":"B.heading", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,0.65215,0.98345,1.34899,1.75906,2.18507],
+  "samples":[
+    {"t":0.0, "x":7.10089, "y":7.25703, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-3.95492, "ay":-0.67683, "alpha":51.78215, "fx":[30.93604,52.64672,-184.60707,-162.6819], "fy":[-196.61836,191.69748,74.7142,-114.92336]},
+    {"t":0.03105, "x":7.09898, "y":7.2567, "heading":3.14159, "vx":-0.12282, "vy":-0.02102, "omega":1.60808, "ax":-4.10988, "ay":-0.71741, "alpha":51.28451, "fx":[26.50708,47.6008,-185.72063,-162.42584], "fy":[-197.16732,192.80096,71.71914,-115.18822]},
+    {"t":0.06211, "x":7.09318, "y":7.25571, "heading":-3.09165, "vx":-0.25045, "vy":-0.0433, "omega":3.2007, "ax":-4.49168, "ay":-0.89963, "alpha":50.04431, "fx":[27.09084,21.36427,-189.59334,-158.35831], "fy":[-196.89658,197.08566,60.37868,-120.55331]},
+    {"t":0.09316, "x":7.08324, "y":7.25393, "heading":-2.99226, "vx":-0.38994, "vy":-0.07124, "omega":4.75482, "ax":-5.4052, "ay":-1.44843, "alpha":46.58791, "fx":[24.1256,-39.01018,-195.27286,-150.25117], "fy":[-196.81635,193.51502,36.89364,-130.17104]},
+    {"t":0.12422, "x":7.06853, "y":7.25102, "heading":-2.8446, "vx":-0.55779, "vy":-0.11622, "omega":6.20159, "ax":-8.55668, "ay":-4.86313, "alpha":24.78681, "fx":[-43.93903,-191.22024,-195.74378,-139.64014], "fy":[-191.58159,34.9083,-27.44185,-140.14902]},
+    {"t":0.15527, "x":7.04708, "y":7.24506, "heading":-2.65201, "vx":-0.82352, "vy":-0.26724, "omega":6.97134, "ax":-8.05823, "ay":-5.44736, "alpha":-24.23166, "fx":[-188.81219,-123.74288,-37.50132,-187.25065], "fy":[-48.67402,-150.97072,-187.35503,23.78009]},
+    {"t":0.18633, "x":7.01762, "y":7.23414, "heading":-2.43552, "vx":-1.07377, "vy":-0.43641, "omega":6.21883, "ax":-8.91458, "ay":-1.7378, "alpha":-25.17613, "fx":[-194.68842,-145.29838,-108.79507,-145.62487], "fy":[0.18736,-127.3866,-114.24267,125.56852]},
+    {"t":0.21738, "x":6.97997, "y":7.21975, "heading":-2.24239, "vx":-1.35061, "vy":-0.49037, "omega":5.43699, "ax":-9.56524, "ay":2.20078, "alpha":-21.29643, "fx":[-193.94642,-162.58175,-131.86972,-149.39414], "fy":[8.60586,-100.32684,116.39276,122.07219]},
+    {"t":0.24844, "x":6.93342, "y":7.20558, "heading":-2.07355, "vx":-1.64765, "vy":-0.42203, "omega":4.77564, "ax":-9.92196, "ay":2.22204, "alpha":-18.98559, "fx":[-193.02903,-170.81043,-137.48311,-160.25443], "fy":[2.91705,-79.95157,118.98573,106.21017]},
+    {"t":0.27949, "x":6.87747, "y":7.19354, "heading":-1.92524, "vx":-1.95577, "vy":-0.35302, "omega":4.18605, "ax":-10.02576, "ay":1.59694, "alpha":-18.11939, "fx":[-190.8562,-166.61741,-142.55501,-168.46993], "fy":[-11.83088,-80.58663,109.85594,89.04237]},
+    {"t":0.31055, "x":6.8119, "y":7.18335, "heading":-1.79525, "vx":-2.26712, "vy":-0.30343, "omega":3.62335, "ax":-9.83004, "ay":0.46222, "alpha":-18.41763, "fx":[-184.95654,-150.73405,-145.53392,-174.22378], "fy":[-33.85082,-97.1029,93.30797,68.4657]},
+    {"t":0.3416, "x":6.73675, "y":7.17415, "heading":-1.68272, "vx":-2.57239, "vy":-0.28908, "omega":3.0514, "ax":-9.14167, "ay":-1.11128, "alpha":-19.08277, "fx":[-171.73147,-120.01236,-142.32664,-175.47874], "fy":[-60.90634,-120.14363,64.54674,42.40526]},
+    {"t":0.37266, "x":6.65246, "y":7.16464, "heading":-1.58796, "vx":-2.85628, "vy":-0.32359, "omega":2.45879, "ax":-7.70158, "ay":-2.86354, "alpha":-18.66998, "fx":[-147.81342,-78.69138,-120.40704,-166.61452], "fy":[-86.49898,-133.37913,18.3296,10.61346]},
+    {"t":0.40371, "x":6.56004, "y":7.15321, "heading":-1.51161, "vx":-3.09545, "vy":-0.41251, "omega":1.879, "ax":-5.59057, "ay":-3.98975, "alpha":-15.88503, "fx":[-114.0989,-44.62143,-75.19449,-138.85376], "fy":[-97.81827,-122.86995,-24.87909,-20.4615]},
+    {"t":0.43477, "x":6.46122, "y":7.13847, "heading":-1.45325, "vx":-3.26907, "vy":-0.53641, "omega":1.38569, "ax":-3.36874, "ay":-3.70641, "alpha":-10.97211, "fx":[-74.26447,-23.00691,-36.56698,-90.78306], "fy":[-85.74985,-91.20788,-34.73035,-35.44819]},
+    {"t":0.46582, "x":6.35808, "y":7.12003, "heading":-1.41022, "vx":-3.37368, "vy":-0.65152, "omega":1.04496, "ax":-1.59663, "ay":-2.70255, "alpha":-5.93932, "fx":[-37.68861,-9.09574,-14.91369,-44.76223], "fy":[-60.07401,-57.9175,-29.38334,-32.82618]},
+    {"t":0.49688, "x":6.25254, "y":7.09849, "heading":-1.37777, "vx":-3.42326, "vy":-0.73544, "omega":0.86051, "ax":-0.5726, "ay":-2.18721, "alpha":-2.93692, "fx":[-15.14523,-0.87255,-3.78911,-18.3728], "fy":[-44.44224,-42.12656,-28.37745,-30.8927]},
+    {"t":0.52793, "x":6.14595, "y":7.0746, "heading":-1.35105, "vx":-3.44105, "vy":-0.80337, "omega":0.76931, "ax":-0.01665, "ay":-2.13277, "alpha":-1.49397, "fx":[-3.09884,4.18365,2.58782,-4.78273], "fy":[-39.74736,-38.23351,-31.33861,-32.88952]},
+    {"t":0.55898, "x":6.03908, "y":7.04862, "heading":-1.32716, "vx":-3.44156, "vy":-0.8696, "omega":0.72291, "ax":0.10607, "ay":-2.16166, "alpha":-1.34612, "fx":[-0.69484,5.84371,4.26768,-2.34401], "fy":[-39.87656,-38.34115,-32.17576,-33.74155]},
+    {"t":0.59004, "x":5.93226, "y":7.02058, "heading":-1.30471, "vx":-3.43827, "vy":-0.93673, "omega":0.68111, "ax":-1.09495, "ay":-2.7119, "alpha":-5.5505, "fx":[-27.22308,-0.82357,-8.77734,-36.18519], "fy":[-60.43019,-55.24429,-29.41231,-35.73753]},
+    {"t":0.62109, "x":5.82496, "y":6.99018, "heading":-1.28356, "vx":-3.47227, "vy":-1.02094, "omega":0.50874, "ax":-2.66575, "ay":-6.60205, "alpha":-14.56468, "fx":[-62.44385,12.70679,-21.05851,-106.95121], "fy":[-137.41208,-138.77461,-82.10064,-81.92477]},
+    {"t":0.65215, "x":5.71584, "y":6.95529, "heading":-1.26776, "vx":-3.55506, "vy":-1.22597, "omega":0.05644, "ax":5.44094, "ay":-3.93804, "alpha":11.57852, "fx":[119.29546,61.75819,65.75096,115.98709], "fy":[-21.02874,-56.77799,-108.71541,-76.05859]},
+    {"t":0.66959, "x":5.65468, "y":6.93331, "heading":-1.26677, "vx":-3.46018, "vy":-1.29464, "omega":0.25833, "ax":2.69162, "ay":-2.60889, "alpha":4.6072, "fx":[53.90145,31.20869,36.51001,57.85191], "fy":[-28.67645,-37.50133,-58.2964,-49.48159]},
+    {"t":0.68702, "x":5.59475, "y":6.91034, "heading":-1.26227, "vx":-3.41325, "vy":-1.34013, "omega":0.33867, "ax":0.98237, "ay":-1.32915, "alpha":1.23395, "fx":[18.41847,12.60201,14.35759,20.12441], "fy":[-18.36616,-20.28579,-25.94607,-24.0273]},
+    {"t":0.70446, "x":5.53538, "y":6.88677, "heading":-1.25636, "vx":-3.39612, "vy":-1.3633, "omega":0.36019, "ax":0.35027, "ay":-0.64872, "alpha":0.29045, "fx":[6.29672,4.94664,5.3814,6.73025], "fy":[-9.92276,-10.36372,-11.70471,-11.26392]},
+    {"t":0.7219, "x":5.47622, "y":6.8629, "heading":-1.25008, "vx":-3.39001, "vy":-1.37462, "omega":0.36525, "ax":0.15382, "ay":-0.32686, "alpha":0.13547, "fx":[2.77361,2.14686,2.35462,2.98124], "fy":[-5.03157,-5.23992,-5.86558,-5.65726]},
+    {"t":0.73933, "x":5.41713, "y":6.83888, "heading":-1.24371, "vx":-3.38733, "vy":-1.38032, "omega":0.36761, "ax":0.10139, "ay":-0.18145, "alpha":0.16742, "fx":[1.94539,1.17293,1.43481,2.20716], "fy":[-2.50767,-2.76982,-3.54183,-3.2797]},
+    {"t":0.75677, "x":5.35808, "y":6.81479, "heading":-1.2373, "vx":-3.38556, "vy":-1.38348, "omega":0.37053, "ax":0.09516, "ay":-0.12538, "alpha":0.22007, "fx":[1.91747,0.90447,1.25527,2.26812], "fy":[-1.40811,-1.75914,-2.77186,-2.42083]},
+    {"t":0.77421, "x":5.29906, "y":6.79064, "heading":-1.23084, "vx":-3.3839, "vy":-1.38567, "omega":0.37437, "ax":0.1044, "ay":-0.11809, "alpha":0.2567, "fx":[2.12134,0.94241,1.35921,2.53794], "fy":[-1.17064,-1.58773,-2.76637,-2.34928]},
+    {"t":0.79164, "x":5.24007, "y":6.76647, "heading":-1.22431, "vx":-3.38208, "vy":-1.38772, "omega":0.37885, "ax":0.11987, "ay":-0.1405, "alpha":0.27412, "fx":[2.3997,1.14364,1.5969,2.85268], "fy":[-1.48738,-1.94106,-3.19667,-2.74299]},
+    {"t":0.80908, "x":5.18112, "y":6.74225, "heading":-1.21771, "vx":-3.37999, "vy":-1.39017, "omega":0.38363, "ax":0.13911, "ay":-0.1833, "alpha":0.27586, "fx":[2.71732,1.45617,1.92054,3.18135], "fy":[-2.19286,-2.65787,-3.91825,-3.45326]},
+    {"t":0.82652, "x":5.1222, "y":6.71798, "heading":-1.21102, "vx":-3.37757, "vy":-1.39337, "omega":0.38844, "ax":0.16157, "ay":-0.24131, "alpha":0.2659, "fx":[3.07196,1.85906,2.31467,3.52716], "fy":[-3.18846,-3.64501,-4.85664,-4.40014]},
+    {"t":0.84396, "x":5.06333, "y":6.69364, "heading":-1.20425, "vx":-3.37475, "vy":-1.39758, "omega":0.39307, "ax":0.18715, "ay":-0.3104, "alpha":0.24818, "fx":[3.46815,2.33862,2.77133,3.90041], "fy":[-4.39342,-4.82741,-5.955,-5.5211]},
+    {"t":0.86139, "x":5.00452, "y":6.66923, "heading":-1.19739, "vx":-3.37149, "vy":-1.40299, "omega":0.3974, "ax":0.21577, "ay":-0.38593, "alpha":0.22742, "fx":[3.91153,2.87879,3.28216,4.31446], "fy":[-5.71565,-6.12068,-7.15073,-6.7458]},
+    {"t":0.87883, "x":4.94576, "y":6.64471, "heading":-1.19046, "vx":-3.36772, "vy":-1.40972, "omega":0.40137, "ax":0.24779, "ay":-0.46242, "alpha":0.21096, "fx":[4.41841,3.4625,3.84304,4.7985], "fy":[-7.04076,-7.42338,-8.37575,-7.99326]},
+    {"t":0.89627, "x":4.88707, "y":6.62005, "heading":-1.18346, "vx":-3.3634, "vy":-1.41778, "omega":0.40504, "ax":0.28602, "ay":-0.53514, "alpha":0.21275, "fx":[5.05388,4.09198,4.48215,5.44352], "fy":[-8.24541,-8.63838,-9.59552,-9.2027]},
+    {"t":0.9137, "x":4.82847, "y":6.59525, "heading":-1.1764, "vx":-3.35842, "vy":-1.42711, "omega":0.40875, "ax":0.34141, "ay":-0.60514, "alpha":0.26373, "fx":[6.04058,4.85074,5.34221,6.53112], "fy":[-9.2481,-9.74436,-10.92661,-10.43062]},
+    {"t":0.93114, "x":4.76996, "y":6.57027, "heading":-1.16927, "vx":-3.35246, "vy":-1.43767, "omega":0.41335, "ax":0.44713, "ay":-0.69195, "alpha":0.43655, "fx":[8.02432,6.05829,6.88394,8.84693], "fy":[-10.14057,-10.97875,-12.92808,-12.09063]},
+    {"t":0.94858, "x":4.71157, "y":6.5451, "heading":-1.16207, "vx":-3.34467, "vy":-1.44973, "omega":0.42096, "ax":0.68943, "ay":-0.86446, "alpha":0.89424, "fx":[12.65568,8.6283,10.33803,14.34804], "fy":[-11.54161,-13.30405,-17.27729,-15.51764]},
+    {"t":0.96601, "x":4.65336, "y":6.51969, "heading":-1.15473, "vx":-3.33264, "vy":-1.46481, "omega":0.43656, "ax":1.26349, "ay":-1.31765, "alpha":1.97009, "fx":[23.69517,14.75018,18.49732,27.30415], "fy":[-15.58274,-19.66892,-28.33854,-24.26812]},
+    {"t":0.98345, "x":4.59544, "y":6.49395, "heading":-1.14711, "vx":-3.31061, "vy":-1.48778, "omega":0.47091, "ax":0.02486, "ay":-1.88231, "alpha":-2.36135, "fx":[-2.50424,8.17048,3.40502,-7.41365], "fy":[-38.76154,-34.21428,-23.92092,-28.61177]},
+    {"t":1.00173, "x":4.53493, "y":6.46644, "heading":-1.13851, "vx":-3.31016, "vy":-1.52218, "omega":0.42775, "ax":0.18569, "ay":-1.05605, "alpha":-0.87103, "fx":[2.0456,5.92536,4.15149,0.25911], "fy":[-20.39764,-18.62823,-14.80504,-16.58459]},
+    {"t":1.02001, "x":4.47446, "y":6.43844, "heading":-1.13069, "vx":-3.30676, "vy":-1.54149, "omega":0.41183, "ax":0.25541, "ay":-0.73474, "alpha":-0.27545, "fx":[3.93334,5.1521,4.58233,3.36256], "fy":[-13.13809,-12.56554,-11.35703,-11.93019]},
+    {"t":1.03828, "x":4.41407, "y":6.41015, "heading":-1.12316, "vx":-3.3021, "vy":-1.55492, "omega":0.4068, "ax":0.26691, "ay":-0.59144, "alpha":-0.07708, "fx":[4.36063,4.70003,4.53801,4.19854], "fy":[-10.1092,-9.9463,-9.60889,-9.77182]},
+    {"t":1.05656, "x":4.35376, "y":6.38163, "heading":-1.11573, "vx":-3.29722, "vy":-1.56572, "omega":0.40539, "ax":0.24598, "ay":-0.50148, "alpha":-0.03843, "fx":[4.05713,4.22562,4.14354,3.97503], "fy":[-8.48451,-8.40207,-8.2343,-8.31675]},
+    {"t":1.07484, "x":4.29354, "y":6.35293, "heading":-1.10832, "vx":-3.29272, "vy":-1.57489, "omega":0.40469, "ax":0.20875, "ay":-0.42517, "alpha":-0.06, "fx":[3.41394,3.67589,3.54572,3.28374], "fy":[-7.28325,-7.15268,-6.89154,-7.02213]},
+    {"t":1.09311, "x":4.23339, "y":6.32407, "heading":-1.10092, "vx":-3.28891, "vy":-1.58266, "omega":0.40359, "ax":0.16716, "ay":-0.35472, "alpha":-0.09712, "fx":[2.68229,3.10448,2.89054,2.46829], "fy":[-6.2309,-6.01654,-5.59524,-5.80963]},
+    {"t":1.11139, "x":4.17331, "y":6.29509, "heading":-1.09354, "vx":-3.28585, "vy":-1.58914, "omega":0.40181, "ax":0.13188, "ay":-0.29303, "alpha":-0.12359, "fx":[2.06901,2.60408,2.32777,1.79263], "fy":[-5.2901,-5.01346,-4.47915,-4.75583]},
+    {"t":1.12967, "x":4.11327, "y":6.26599, "heading":-1.0862, "vx":-3.28344, "vy":-1.5945, "omega":0.39956, "ax":0.11443, "ay":-0.24619, "alpha":-0.1161, "fx":[1.78878,2.2894,2.02611,1.52543], "fy":[-4.48561,-4.22209,-3.72196,-3.98552]},
+    {"t":1.14795, "x":4.05328, "y":6.23681, "heading":-1.0789, "vx":-3.28135, "vy":-1.599, "omega":0.39743, "ax":0.13004, "ay":-0.2219, "alpha":-0.04386, "fx":[2.124,2.31237,2.21154,2.02316], "fy":[-3.84357,-3.74263,-3.55441,-3.65536]},
+    {"t":1.16622, "x":3.99333, "y":6.20755, "heading":-1.07163, "vx":-3.27897, "vy":-1.60306, "omega":0.39663, "ax":0.20328, "ay":-0.23033, "alpha":0.14533, "fx":[3.53026,2.90845,3.24697,3.86866], "fy":[-3.35921,-3.69844,-4.31963,-3.98043]},
+    {"t":1.1845, "x":3.93343, "y":6.17821, "heading":-1.06438, "vx":-3.27526, "vy":-1.60727, "omega":0.39929, "ax":0.37763, "ay":-0.28526, "alpha":0.54898, "fx":[6.81874,4.47815,5.7724,8.11017], "fy":[-2.93588,-4.23701,-6.57415,-5.27329]},
+    {"t":1.20278, "x":3.87363, "y":6.14879, "heading":-1.05709, "vx":-3.26836, "vy":-1.61248, "omega":0.40932, "ax":0.72893, "ay":-0.40306, "alpha":1.3454, "fx":[13.41698,7.69658,10.89983,16.59056], "fy":[-2.2388,-5.49018,-11.19912,-7.94683]},
+    {"t":1.22105, "x":3.81402, "y":6.11925, "heading":-1.04961, "vx":-3.25503, "vy":-1.61985, "omega":0.43391, "ax":1.36871, "ay":-0.59532, "alpha":2.807, "fx":[25.47551,13.55503,20.26776,31.96494], "fy":[-0.47061,-7.47283,-19.38797,-12.36324]},
+    {"t":1.23933, "x":3.75476, "y":6.08954, "heading":-1.04168, "vx":-3.23002, "vy":-1.63073, "omega":0.48522, "ax":2.38263, "ay":-0.85579, "alpha":5.14327, "fx":[44.82648,22.99652,35.18744,55.85898], "fy":[3.53416,-10.06709,-32.1431,-18.38622]},
+    {"t":1.25761, "x":3.69612, "y":6.05959, "heading":-1.03281, "vx":-3.18647, "vy":-1.64637, "omega":0.57922, "ax":3.66771, "ay":-1.17022, "alpha":8.04829, "fx":[69.82707,35.91784,54.23782,84.57335], "fy":[9.80739,-13.62864,-49.03975,-25.16706]},
+    {"t":1.27589, "x":3.63849, "y":6.02931, "heading":-1.02222, "vx":-3.11943, "vy":-1.66776, "omega":0.72632, "ax":4.92435, "ay":-1.59239, "alpha":10.6019, "fx":[94.69868,50.88543,72.63852,110.12385], "fy":[14.88398,-20.58028,-67.90203,-32.57898]},
+    {"t":1.29416, "x":3.5823, "y":5.99856, "heading":-1.00895, "vx":-3.02943, "vy":-1.69686, "omega":0.92009, "ax":5.935, "ay":-2.31954, "alpha":11.94137, "fx":[115.4052,66.27067,86.20658,127.85211], "fy":[11.99933,-36.74335,-87.10152,-42.81663]},
+    {"t":1.31244, "x":3.52792, "y":5.96716, "heading":-0.99213, "vx":-2.92096, "vy":-1.73925, "omega":1.13835, "ax":6.6182, "ay":-3.65447, "alpha":11.42174, "fx":[131.11213,79.84156,93.67199,136.66318], "fy":[-8.72619,-68.33473,-106.5571,-60.05489]},
+    {"t":1.33072, "x":3.47564, "y":5.93476, "heading":-0.97132, "vx":-2.79999, "vy":-1.80605, "omega":1.3471, "ax":6.83366, "ay":-5.72231, "alpha":8.30488, "fx":[135.78563,88.85622,96.518,134.49538], "fy":[-58.50403,-108.71701,-125.66355,-88.6683]},
+    {"t":1.34899, "x":3.42561, "y":5.90079, "heading":-0.9467, "vx":-2.6751, "vy":-1.91064, "omega":1.49889, "ax":3.59337, "ay":-7.27554, "alpha":-4.84872, "fx":[51.5643,78.79716,70.58669,38.65067], "fy":[-133.60339,-117.78215,-107.85344,-125.88002]},
+    {"t":1.3695, "x":3.37151, "y":5.86009, "heading":-0.91597, "vx":-2.60142, "vy":-2.05981, "omega":1.39948, "ax":1.32861, "ay":-5.75444, "alpha":-7.76522, "fx":[16.17287,52.64146,30.70048,-10.92533], "fy":[-116.32815,-96.65772,-73.89005,-96.81915]},
+    {"t":1.39, "x":3.31846, "y":5.81665, "heading":-0.88728, "vx":-2.57418, "vy":-2.17779, "omega":1.24026, "ax":0.58865, "ay":-4.44236, "alpha":-7.09012, "fx":[6.53816,36.71499,14.06263,-18.06601], "fy":[-94.54709,-75.99879,-52.17165,-73.4906]},
+    {"t":1.4105, "x":3.2658, "y":5.77106, "heading":-0.86185, "vx":-2.56211, "vy":-2.26887, "omega":1.09489, "ax":0.63059, "ay":-3.53792, "alpha":-5.47565, "fx":[8.5656,30.61271,12.89129,-10.0232], "fy":[-75.86768,-60.09839,-41.29332,-58.64206]},
+    {"t":1.43101, "x":3.2134, "y":5.7238, "heading":-0.8394, "vx":-2.54918, "vy":-2.34141, "omega":0.98263, "ax":0.88553, "ay":-3.02939, "alpha":-4.14653, "fx":[13.5317,29.69236,16.24317,-0.42192], "fy":[-63.74794,-50.90007,-36.82357,-50.52208]},
+    {"t":1.45151, "x":3.16132, "y":5.67516, "heading":-0.81925, "vx":-2.53102, "vy":-2.40353, "omega":0.89761, "ax":1.31497, "ay":-3.046, "alpha":-3.53415, "fx":[20.91818,34.58085,23.15533,9.02544], "fy":[-62.16479,-50.64486,-39.06211,-51.22936]},
+    {"t":1.47201, "x":3.1097, "y":5.62524, "heading":-0.80085, "vx":-2.50406, "vy":-2.46598, "omega":0.82515, "ax":3.04565, "ay":-4.22942, "alpha":-3.00937, "fx":[48.92563,61.52675,53.00829,39.61754], "fy":[-80.09058,-68.96896,-60.54804,-72.40211]},
+    {"t":1.49252, "x":3.059, "y":5.57379, "heading":-0.78393, "vx":-2.44162, "vy":-2.5527, "omega":0.76344, "ax":6.51989, "ay":-5.62195, "alpha":-0.34215, "fx":[108.08596,109.76801,109.29039,107.58911], "fy":[-94.97673,-93.11536,-92.4422,-94.32675]},
+    {"t":1.51302, "x":3.01031, "y":5.52027, "heading":-0.76828, "vx":-2.30794, "vy":-2.66796, "omega":0.75643, "ax":8.78424, "ay":-5.43055, "alpha":2.56013, "fx":[152.39034,140.23218,140.88524,152.20854], "fy":[-78.3855,-97.11612,-101.67075,-84.92635]},
+    {"t":1.53352, "x":2.96484, "y":5.46442, "heading":-0.75277, "vx":-2.12783, "vy":-2.77931, "omega":0.80892, "ax":9.95079, "ay":-4.73168, "alpha":4.06087, "fx":[174.95652,158.82341,157.23569,172.48424], "fy":[-56.36412,-90.28821,-98.59956,-70.2472]},
+    {"t":1.55403, "x":2.9233, "y":5.40644, "heading":-0.73618, "vx":-1.92381, "vy":-2.87632, "omega":0.89218, "ax":10.60561, "ay":-4.02376, "alpha":4.51458, "fx":[185.57975,171.27395,167.90176,182.40617], "fy":[-39.87259,-79.56826,-90.81857,-58.03725]},
+    {"t":1.57453, "x":2.88609, "y":5.34662, "heading":-0.71789, "vx":-1.70636, "vy":-2.95882, "omega":0.98475, "ax":11.01583, "ay":-3.39297, "alpha":4.35766, "fx":[190.79178,179.89509,175.91152,187.91597], "fy":[-29.08995,-67.95884,-80.91213,-48.27553]},
+    {"t":1.59503, "x":2.85342, "y":5.28525, "heading":-0.6977, "vx":-1.4805, "vy":-3.02839, "omega":1.07409, "ax":11.29502, "ay":-2.8338, "alpha":3.84744, "fx":[193.55058,185.96003,182.31062,191.3091], "fy":[-22.37822,-56.59997,-69.78131,-40.19314]},
+    {"t":1.61554, "x":2.82543, "y":5.22256, "heading":-0.67568, "vx":-1.24892, "vy":-3.08649, "omega":1.15298, "ax":11.49457, "ay":-2.32896, "alpha":3.12238, "fx":[195.12621,190.26186,187.49133,193.55644], "fy":[-18.38336,-45.85843,-57.81312,-33.23537]},
+    {"t":1.63604, "x":2.80224, "y":5.15879, "heading":-0.65204, "vx":-1.01324, "vy":-3.13424, "omega":1.21699, "ax":11.63968, "ay":-1.86405, "alpha":2.25958, "fx":[196.08506,193.31152,191.6008,195.114], "fy":[-16.19509,-35.79649,-45.23011,-27.06955]},
+    {"t":1.65654, "x":2.78392, "y":5.09413, "heading":-0.62708, "vx":-0.77459, "vy":-3.17246, "omega":1.26332, "ax":11.74369, "ay":-1.42911, "alpha":1.30557, "fx":[196.69552,195.44327,194.68971,196.21804], "fy":[-15.21678,-26.36755,-32.19944,-21.50661]},
+    {"t":1.67705, "x":2.7705, "y":5.02879, "heading":-0.60118, "vx":-0.5338, "vy":-3.20176, "omega":1.29009, "ax":11.81422, "ay":-1.01773, "alpha":0.29127, "fx":[197.09351,196.87901,196.77366,197.00307], "fy":[-15.04934,-17.49805,-18.86992,-16.44298]},
+    {"t":1.69755, "x":2.76204, "y":4.96292, "heading":-0.57473, "vx":-0.29157, "vy":-3.22263, "omega":1.29606, "ax":11.85603, "ay":-0.62606, "alpha":-0.76109, "fx":[197.35287,197.76941,197.86145,197.55325], "fy":[-15.41842,-9.11927,-5.38243,-11.82412]},
+    {"t":1.71805, "x":2.75855, "y":4.89672, "heading":-0.54816, "vx":-0.04849, "vy":-3.23547, "omega":1.28046, "ax":11.87238, "ay":-0.25074, "alpha":-1.84108, "fx":[197.51528,198.22061,197.96549,197.92637], "fy":[-16.15183,-1.14838,8.19335,-7.61189]},
+    {"t":1.73856, "x":2.76006, "y":4.83033, "heading":-0.5219, "vx":0.19493, "vy":-3.24061, "omega":1.24271, "ax":11.86198, "ay":0.1395, "alpha":-3.09079, "fx":[197.56691,198.28801,196.91689,198.1619], "fy":[-17.59537,7.12963,23.32155,-3.55426]},
+    {"t":1.75906, "x":2.76655, "y":4.76392, "heading":-0.49642, "vx":0.43814, "vy":-3.23775, "omega":1.17934, "ax":11.83707, "ay":0.97228, "alpha":-2.91618, "fx":[198.49042,197.56022,195.17471,198.04756], "fy":[-2.89727,19.99649,36.04449,11.68576]},
+    {"t":1.78412, "x":2.78124, "y":4.68308, "heading":-0.46687, "vx":0.73477, "vy":-3.21338, "omega":1.10626, "ax":11.68988, "ay":2.06257, "alpha":-2.68007, "fx":[197.61903,194.94004,191.23693,195.66281], "fy":[16.6735,36.85516,52.42613,31.57373]},
+    {"t":1.80918, "x":2.80333, "y":4.60321, "heading":-0.43915, "vx":1.02771, "vy":-3.1617, "omega":1.0391, "ax":11.33561, "ay":3.5309, "alpha":-1.51213, "fx":[191.904,189.02195,185.74833,189.16263], "fy":[48.9752,59.33466,68.70844,58.4151]},
+    {"t":1.83424, "x":2.83264, "y":4.52509, "heading":-0.41311, "vx":1.31177, "vy":-3.07321, "omega":1.00121, "ax":10.48305, "ay":5.53731, "alpha":1.2126, "fx":[170.85783,174.30091,178.52756,175.30342], "fy":[99.51184,93.17745,84.95111,91.57674]},
+    {"t":1.8593, "x":2.8688, "y":4.44981, "heading":-0.38802, "vx":1.57447, "vy":-2.93445, "omega":1.0316, "ax":8.506, "ay":8.03332, "alpha":6.21684, "fx":[116.37562,129.89281,168.89442,152.00098], "fy":[159.57465,147.88008,101.81866,126.37304]},
+    {"t":1.88435, "x":2.91093, "y":4.3788, "heading":-0.36217, "vx":1.78762, "vy":-2.73314, "omega":1.18739, "ax":4.70199, "ay":10.22147, "alpha":13.02472, "fx":[38.37325,13.05353,144.96266,117.13016], "fy":[193.84929,195.84957,132.64244,159.20672]},
+    {"t":1.90941, "x":2.9572, "y":4.31352, "heading":-0.33241, "vx":1.90545, "vy":-2.477, "omega":1.51378, "ax":-0.27987, "ay":11.56826, "alpha":8.71211, "fx":[-24.17547,-62.76981,27.30532,40.97864], "fy":[196.31647,186.93041,194.76278,193.33941]},
+    {"t":1.93447, "x":3.00486, "y":4.25508, "heading":-0.29448, "vx":1.89844, "vy":-2.18711, "omega":1.73209, "ax":-4.09965, "ay":11.13589, "alpha":1.22189, "fx":[-69.42679,-76.11554,-67.21911,-60.59517], "fy":[185.38423,182.66751,186.06445,188.40384]},
+    {"t":1.95953, "x":3.05115, "y":4.20377, "heading":-0.25107, "vx":1.7957, "vy":-1.90805, "omega":1.76271, "ax":-6.01442, "ay":10.22391, "alpha":-2.99114, "fx":[-100.41174,-82.34112,-100.62277,-117.65446], "fy":[170.80633,180.36333,170.95372,159.58755]},
+    {"t":1.98459, "x":3.09426, "y":4.15916, "heading":-0.2069, "vx":1.64499, "vy":-1.65185, "omega":1.68776, "ax":-7.02939, "ay":9.48109, "alpha":-5.40589, "fx":[-120.28739,-86.06452,-116.77202,-145.5826], "fy":[157.6418,178.8768,160.69147,134.9711]},
+    {"t":2.00965, "x":3.13327, "y":4.12075, "heading":-0.16461, "vx":1.46884, "vy":-1.41426, "omega":1.55229, "ax":-7.62902, "ay":8.93006, "alpha":-6.90031, "fx":[-132.96588,-88.6368,-126.50402,-160.58186], "fy":[147.28181,177.79315,153.35423,117.01036]},
+    {"t":2.03471, "x":3.16769, "y":4.08811, "heading":-0.12571, "vx":1.27766, "vy":-1.19048, "omega":1.37937, "ax":-8.01662, "ay":8.51872, "alpha":-7.89637, "fx":[-141.22164,-90.59048,-133.15941,-169.5615], "fy":[139.53629,176.9329,147.75248,103.79007]},
+    {"t":2.05977, "x":3.19719, "y":4.06095, "heading":-0.09114, "vx":1.07677, "vy":-0.97701, "omega":1.1815, "ax":-8.2846, "ay":8.20446, "alpha":-8.5986, "fx":[-146.77603,-92.16172,-138.05304,-175.41049], "fy":[133.80948,176.21475,143.29228,93.74103]},
+    {"t":2.08483, "x":3.22157, "y":4.03905, "heading":-0.06154, "vx":0.86916, "vy":-0.77141, "omega":0.96602, "ax":-8.47954, "ay":7.95839, "alpha":-9.11648, "fx":[-150.68128,-93.46007,-141.79802,-179.4599], "fy":[129.50504,175.60295,139.66439,85.87766]},
+    {"t":2.10989, "x":3.24069, "y":4.02221, "heading":-0.03733, "vx":0.65667, "vy":-0.57198, "omega":0.73757, "ax":-8.6271, "ay":7.76116, "alpha":-9.51368, "fx":[-153.59771,-94.53625,-144.7168,-182.38741], "fy":[126.1259,175.08378,136.69813,79.59176]},
+    {"t":2.13495, "x":3.25443, "y":4.01032, "heading":-0.01884, "vx":0.44048, "vy":-0.37749, "omega":0.49917, "ax":-8.74248, "ay":7.59959, "alpha":-9.82957, "fx":[-155.95564,-95.41283,-146.99472,-184.56842], "fy":[123.27947,174.65409,134.29425,74.498]},
+    {"t":2.16001, "x":3.26273, "y":4.00324, "heading":-0.00634, "vx":0.2214, "vy":-0.18705, "omega":0.25284, "ax":-8.83522, "ay":7.4644, "alpha":-10.08985, "fx":[-158.04558,-96.09911,-148.74546,-186.22533], "fy":[120.65961,174.31526,132.39246,70.34432]},
+    {"t":2.18507, "x":3.2655, "y":4.0009, "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/startDeepToK.traj b/src/main/deploy/choreo/startDeepToK.traj
new file mode 100644
index 00000000..09605127
--- /dev/null
+++ b/src/main/deploy/choreo/startDeepToK.traj
@@ -0,0 +1,102 @@
+{
+ "name":"startDeepToK",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":7.1008875, "y":7.2570308, "heading":3.141592653589793, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":5.25449800491333, "y":6.856431007385254, "heading":-1.348714396235503, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":4.315337657928467, "y":6.098512172698975, "heading":-1.231504335673354, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":4.141549248154255, "y":5.238261807735684, "heading":5.235987755982989, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false},
+    {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":4.0}}, "enabled":true}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"startDeep.x", "val":7.1008875}, "y":{"exp":"startDeep.y", "val":7.2570308}, "heading":{"exp":"startDeep.heading", "val":3.141592653589793}, "intervals":30, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"5.25449800491333 m", "val":5.25449800491333}, "y":{"exp":"6.856431007385254 m", "val":6.856431007385254}, "heading":{"exp":"-1.348714396235503 rad", "val":-1.348714396235503}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"4.315337657928467 m", "val":4.315337657928467}, "y":{"exp":"6.098512172698975 m", "val":6.098512172698975}, "heading":{"exp":"-1.231504335673354 rad", "val":-1.231504335673354}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"K.x", "val":4.141549248154255}, "y":{"exp":"K.y", "val":5.238261807735684}, "heading":{"exp":"K.heading", "val":5.235987755982989}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false},
+    {"from":0, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"4 rad / s", "val":4.0}}}, "enabled":true}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,0.78448,1.1217,1.51614],
+  "samples":[
+    {"t":0.0, "x":7.10089, "y":7.25703, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.47932, "ay":-0.72111, "alpha":43.21051, "fx":[-24.92178,-47.01019,-189.92906,-170.16731], "fy":[-197.39403,192.9434,59.84905,-103.4806]},
+    {"t":0.02615, "x":7.09867, "y":7.25678, "heading":3.14159, "vx":-0.16943, "vy":-0.01886, "omega":1.12993, "ax":-7.31643, "ay":-0.91857, "alpha":39.43408, "fx":[-42.27699,-82.45346,-191.65576,-171.45904], "fy":[-194.32503,180.43763,53.87546,-101.23622]},
+    {"t":0.0523, "x":7.09174, "y":7.25598, "heading":-3.11205, "vx":-0.36075, "vy":-0.04288, "omega":2.1611, "ax":-8.44952, "ay":-1.46376, "alpha":33.398, "fx":[-65.32239,-131.64786,-194.48595,-171.94145], "fy":[-187.68238,148.16424,42.19271,-100.27552]},
+    {"t":0.07845, "x":7.07942, "y":7.25436, "heading":-3.05553, "vx":-0.5817, "vy":-0.08115, "omega":3.03444, "ax":-9.7636, "ay":-2.34496, "alpha":24.40774, "fx":[-103.12893,-177.32925,-197.54514,-173.01506], "fy":[-169.60763,88.32896,23.09025,-98.16878]},
+    {"t":0.1046, "x":7.06087, "y":7.25143, "heading":-2.97619, "vx":-0.83701, "vy":-0.14247, "omega":3.67269, "ax":-10.98662, "ay":-3.11225, "alpha":11.9618, "fx":[-157.99936,-197.94616,-198.47,-178.15092], "fy":[-119.76242,8.82234,-8.6179,-87.96056]},
+    {"t":0.13075, "x":7.03523, "y":7.24664, "heading":-2.88015, "vx":-1.1243, "vy":-0.22386, "omega":3.98548, "ax":-11.39941, "ay":-3.36904, "alpha":0.41508, "fx":[-189.4208,-190.57111,-190.61225,-189.48685], "fy":[-58.15202,-54.26128,-54.20704,-58.02083]},
+    {"t":0.1569, "x":7.00193, "y":7.23964, "heading":-2.77593, "vx":-1.42239, "vy":-0.31195, "omega":3.99633, "ax":-11.30636, "ay":-3.55302, "alpha":0.00279, "fx":[-188.46771,-188.4758,-188.47554,-188.46746], "fy":[-59.23913,-59.21347,-59.21512,-59.24078]},
+    {"t":0.18305, "x":6.96087, "y":7.23026, "heading":-2.67143, "vx":-1.71805, "vy":-0.40486, "omega":3.99641, "ax":-11.0173, "ay":-4.07605, "alpha":-0.0038, "fx":[-183.65898,-183.64639,-183.64723,-183.65983], "fy":[-67.93104,-67.96485,-67.96069,-67.92687]},
+    {"t":0.20919, "x":6.91218, "y":7.21828, "heading":-2.56692, "vx":-2.00614, "vy":-0.51145, "omega":3.99631, "ax":-1.86701, "ay":-9.89062, "alpha":-4.60079, "fx":[-57.70924,-32.65345,-4.49592,-29.62975], "fy":[-159.14418,-169.83115,-169.4336,-161.07832]},
+    {"t":0.23534, "x":6.85908, "y":7.20153, "heading":-2.46242, "vx":-2.05496, "vy":-0.77008, "omega":3.876, "ax":-2.55379, "ay":-3.46312, "alpha":-11.1718, "fx":[-80.99858,-43.47627,-0.97331,-44.83367], "fy":[-51.30583,-92.01239,-68.63034,-18.96588]},
+    {"t":0.26149, "x":6.80447, "y":7.18021, "heading":-2.36107, "vx":-2.12174, "vy":-0.86064, "omega":3.58386, "ax":-3.98621, "ay":2.41128, "alpha":-10.1995, "fx":[-97.41588,-73.17948,-31.80225,-63.39548], "fy":[37.18877,1.02825,46.64003,75.92227]},
+    {"t":0.28764, "x":6.74762, "y":7.15853, "heading":-2.26735, "vx":-2.22598, "vy":-0.79759, "omega":3.31715, "ax":-4.53785, "ay":3.11553, "alpha":-9.6461, "fx":[-104.99351,-81.37577,-42.55674,-73.64927], "fy":[44.39343,14.64569,64.15134,84.54725]},
+    {"t":0.31379, "x":6.68786, "y":7.13874, "heading":-2.18061, "vx":-2.34464, "vy":-0.71612, "omega":3.06491, "ax":-4.77216, "ay":2.68112, "alpha":-9.41926, "fx":[-107.50102,-81.83317,-48.45683,-80.40748], "fy":[34.89165,7.99588,59.20841,76.67583]},
+    {"t":0.33994, "x":6.62492, "y":7.12093, "heading":-2.10047, "vx":-2.46943, "vy":-0.64601, "omega":2.81861, "ax":-4.69508, "ay":2.01532, "alpha":-8.99082, "fx":[-104.3714,-76.58827,-49.8378,-82.26145], "fy":[22.70906,-1.0653,48.55255,64.18115]},
+    {"t":0.36609, "x":6.55874, "y":7.10472, "heading":-2.02676, "vx":-2.5922, "vy":-0.59331, "omega":2.5835, "ax":-4.36367, "ay":1.3886, "alpha":-8.25311, "fx":[-96.44967,-67.74067,-47.48329,-79.2878], "fy":[12.00199,-7.66764,37.34666,50.90849]},
+    {"t":0.39224, "x":6.48947, "y":7.08968, "heading":-1.9592, "vx":-2.70631, "vy":-0.557, "omega":2.36769, "ax":-3.8659, "ay":0.90118, "alpha":-7.25285, "fx":[-85.21814,-57.36007,-42.72978,-72.4629], "fy":[4.24547,-10.80678,27.82009,38.82985]},
+    {"t":0.41839, "x":6.41738, "y":7.07543, "heading":-1.89729, "vx":-2.8074, "vy":-0.53343, "omega":2.17803, "ax":-3.29988, "ay":0.57461, "alpha":-6.11328, "fx":[-72.48733,-47.14072,-37.01831,-63.38343], "fy":[-0.35556,-11.03899,20.72019,28.98823]},
+    {"t":0.44454, "x":6.34284, "y":7.06167, "heading":-1.84034, "vx":-2.89369, "vy":-0.51841, "omega":2.01817, "ax":-2.75265, "ay":0.38465, "alpha":-4.98226, "fx":[-60.03981,-38.21109,-31.48593,-53.80489], "fy":[-2.38546,-9.4974,15.88019,21.65071]},
+    {"t":0.47069, "x":6.26623, "y":7.04825, "heading":-1.78756, "vx":-2.96567, "vy":-0.50835, "omega":1.88789, "ax":-2.28255, "ay":0.28567, "alpha":-3.98259, "fx":[-49.207,-31.08824,-26.78622,-45.11479], "fy":[-2.81676,-7.32249,12.70026,16.48681]},
+    {"t":0.49684, "x":6.1879, "y":7.03505, "heading":-1.7382, "vx":-3.02536, "vy":-0.50088, "omega":1.78375, "ax":-1.91502, "ay":0.22984, "alpha":-3.18655, "fx":[-40.65946,-25.80067,-23.14457,-38.085], "fy":[-2.64607,-5.39811,10.50422,12.86534]},
+    {"t":0.52299, "x":6.10813, "y":7.02203, "heading":-1.69155, "vx":-3.07543, "vy":-0.49487, "omega":1.70042, "ax":-1.65181, "ay":0.17591, "alpha":-2.62386, "fx":[-34.52841,-22.10325,-20.52578,-32.98175], "fy":[-2.71807,-4.34983,8.69753,10.09947]},
+    {"t":0.54914, "x":6.02715, "y":7.00915, "heading":-1.64709, "vx":-3.11863, "vy":-0.49027, "omega":1.63181, "ax":-1.48444, "ay":0.08735, "alpha":-2.30794, "fx":[-30.69039,-19.66338,-18.79209,-29.83376], "fy":[-3.77038,-4.69957,6.76215,7.53202]},
+    {"t":0.57529, "x":5.94509, "y":6.99636, "heading":-1.60442, "vx":-3.15745, "vy":-0.48798, "omega":1.57146, "ax":-1.40452, "ay":-0.07582, "alpha":-2.26349, "fx":[-29.01672,-18.15782,-17.80052,-28.67563], "fy":[-6.61651,-7.08055,4.16086,4.4807]},
+    {"t":0.60144, "x":5.86204, "y":6.98358, "heading":-1.56332, "vx":-3.19417, "vy":-0.48997, "omega":1.51227, "ax":-1.40889, "ay":-0.36822, "alpha":-2.54837, "fx":[-29.51,-17.28011,-17.44099,-29.71131], "fy":[-12.38069,-12.46829,0.19672,0.10023]},
+    {"t":0.62758, "x":5.77803, "y":6.97064, "heading":-1.52378, "vx":-3.23101, "vy":-0.4996, "omega":1.44563, "ax":-1.49961, "ay":-0.8695, "alpha":-3.26577, "fx":[-32.30455,-16.66402,-17.6233,-33.39935], "fy":[-22.68037,-22.37635,-6.1435,-6.77614]},
+    {"t":0.65373, "x":5.69303, "y":6.95728, "heading":-1.48598, "vx":-3.27023, "vy":-0.52233, "omega":1.36023, "ax":-1.67477, "ay":-1.68539, "alpha":-4.54621, "fx":[-37.42622,-15.67643,-18.1667,-40.4014], "fy":[-39.4907,-38.83213,-16.33766,-17.71838]},
+    {"t":0.67988, "x":5.60695, "y":6.94304, "heading":-1.45041, "vx":-3.31402, "vy":-0.5664, "omega":1.24135, "ax":-1.87791, "ay":-2.95566, "alpha":-6.41104, "fx":[-43.7017,-12.69941,-18.07495,-50.7397], "fy":[-64.6249,-64.07993,-33.17537,-35.19764]},
+    {"t":0.70603, "x":5.51964, "y":6.92722, "heading":-1.41795, "vx":-3.36313, "vy":-0.64369, "omega":1.07371, "ax":-1.67661, "ay":-5.0597, "alpha":-8.02383, "fx":[-43.10506,-1.67455,-10.10368,-56.90975], "fy":[-100.94563,-100.90574,-67.22218,-68.29758]},
+    {"t":0.73218, "x":5.43113, "y":6.90866, "heading":-1.38987, "vx":-3.40697, "vy":-0.776, "omega":0.86389, "ax":0.32191, "ay":-8.12585, "alpha":-5.88579, "fx":[-10.22378,27.2832,24.83437,-20.42927], "fy":[-144.82377,-141.13642,-125.99863,-129.85714]},
+    {"t":0.75833, "x":5.34215, "y":6.88559, "heading":-1.36728, "vx":-3.39855, "vy":-0.98849, "omega":0.70998, "ax":3.56899, "ay":-9.6786, "alpha":-0.2099, "fx":[58.58307,60.24371,60.41045,58.73623], "fy":[-161.79001,-161.22014,-160.88179,-161.45837]},
+    {"t":0.78448, "x":5.2545, "y":6.85643, "heading":-1.34871, "vx":-3.30523, "vy":-1.24158, "omega":0.70449, "ax":5.04266, "ay":-8.79125, "alpha":2.59578, "fx":[95.58621,76.33264,73.26132,91.05485], "fy":[-138.433,-148.53298,-153.99703,-145.2207]},
+    {"t":0.80556, "x":5.18596, "y":6.82831, "heading":-1.33387, "vx":-3.19895, "vy":-1.42686, "omega":0.7592, "ax":3.05379, "ay":-7.96292, "alpha":-2.52705, "fx":[42.49975,59.23988,60.03702,41.84418], "fy":[-138.74776,-133.18589,-126.42305,-132.5957]},
+    {"t":0.82663, "x":5.11921, "y":6.79647, "heading":-1.31787, "vx":-3.13458, "vy":-1.59469, "omega":0.70594, "ax":0.87828, "ay":-6.00446, "alpha":-5.72055, "fx":[2.05603,33.86279,29.58885,-6.94564], "fy":[-113.36938,-106.60851,-86.32344,-94.06451]},
+    {"t":0.84771, "x":5.05334, "y":6.76153, "heading":-1.30299, "vx":-3.11607, "vy":-1.72124, "omega":0.58537, "ax":-0.11427, "ay":-3.82525, "alpha":-5.31952, "fx":[-11.32732,15.18401,8.51181,-19.98774], "fy":[-77.81252,-72.18779,-49.31498,-55.74503]},
+    {"t":0.86879, "x":4.98764, "y":6.7244, "heading":-1.29065, "vx":-3.11848, "vy":-1.80186, "omega":0.47326, "ax":-0.1711, "ay":-2.46707, "alpha":-3.70135, "fx":[-9.0806,8.66751,3.65772,-14.65333], "fy":[-51.68783,-47.18208,-30.391,-35.23852]},
+    {"t":0.88986, "x":4.92188, "y":6.68587, "heading":-1.28067, "vx":-3.12209, "vy":-1.85386, "omega":0.39525, "ax":0.15554, "ay":-2.28984, "alpha":-2.80045, "fx":[-2.12047,11.2835,7.46865,-6.26036], "fy":[-46.33004,-42.58388,-29.93101,-33.83703]},
+    {"t":0.91094, "x":4.85611, "y":6.64629, "heading":-1.27234, "vx":-3.11881, "vy":-1.90212, "omega":0.33623, "ax":0.9854, "ay":-3.4817, "alpha":-2.75782, "fx":[11.37347,25.11936,21.76335,7.44831], "fy":[-65.94537,-61.77711,-50.04094,-54.38938]},
+    {"t":0.93201, "x":4.7906, "y":6.60543, "heading":-1.26526, "vx":-3.09804, "vy":-1.9755, "omega":0.2781, "ax":2.75004, "ay":-5.79675, "alpha":-2.58109, "fx":[39.48025,54.0082,52.69199,37.18691], "fy":[-103.89273,-98.32447,-89.16759,-95.13134]},
+    {"t":0.95309, "x":4.72591, "y":6.56251, "heading":-1.2594, "vx":-3.04008, "vy":-2.09767, "omega":0.2237, "ax":4.86595, "ay":-7.58987, "alpha":-1.60407, "fx":[75.58282,85.77222,86.88963,76.2072], "fy":[-131.47217,-126.0017,-121.36925,-127.23504]},
+    {"t":0.97417, "x":4.66292, "y":6.51661, "heading":-1.25468, "vx":-2.93753, "vy":-2.25764, "omega":0.18989, "ax":6.44864, "ay":-8.17552, "alpha":-0.65669, "fx":[104.8241,109.09753,110.20337,105.85811], "fy":[-138.67138,-135.66274,-133.84145,-136.95238]},
+    {"t":0.99524, "x":4.60244, "y":6.46721, "heading":-1.25068, "vx":-2.80161, "vy":-2.42995, "omega":0.17605, "ax":7.51627, "ay":-8.12696, "alpha":-0.17389, "fx":[124.54986,125.63335,126.03719,124.95037], "fy":[-136.19925,-135.26824,-134.74142,-135.68083]},
+    {"t":1.01632, "x":4.54506, "y":6.41419, "heading":-1.24697, "vx":-2.6432, "vy":-2.60123, "omega":0.17239, "ax":8.25904, "ay":-7.85524, "alpha":-0.13818, "fx":[137.08571,137.88844,138.26318,137.45947], "fy":[-131.57941,-130.7796,-130.30405,-131.10935]},
+    {"t":1.03739, "x":4.49119, "y":6.35762, "heading":-1.24334, "vx":-2.46913, "vy":-2.76679, "omega":0.16948, "ax":8.79279, "ay":-7.53116, "alpha":-0.46532, "fx":[144.63444,147.14622,148.50872,145.99683], "fy":[-127.826,-125.03561,-123.22528,-126.07662]},
+    {"t":1.05847, "x":4.4411, "y":6.29764, "heading":-1.23976, "vx":-2.28381, "vy":-2.92552, "omega":0.15967, "ax":9.18165, "ay":-7.22051, "alpha":-1.08363, "fx":[148.66517,154.14703,157.41918,151.98358], "fy":[-125.91051,-119.33621,-114.65656,-121.54661]},
+    {"t":1.07955, "x":4.39501, "y":6.23438, "heading":-1.2364, "vx":-2.0903, "vy":-3.0777, "omega":0.13683, "ax":9.45696, "ay":-6.95503, "alpha":-1.96532, "fx":[149.87349,159.34042,165.27936,156.07863], "fy":[-126.25388,-114.35876,-105.12714,-118.00844]},
+    {"t":1.10062, "x":4.35305, "y":6.16797, "heading":-1.23352, "vx":-1.89098, "vy":-3.22429, "omega":0.09541, "ax":9.63018, "ay":-6.74328, "alpha":-3.43739, "fx":[147.15085,163.27378,173.38379,158.31326], "fy":[-130.59712,-110.16057,-92.77398,-116.09714]},
+    {"t":1.1217, "x":4.31534, "y":6.09851, "heading":-1.2315, "vx":-1.68801, "vy":-3.36641, "omega":0.02296, "ax":10.68037, "ay":-5.08003, "alpha":-0.53148, "fx":[176.46698,178.08446,179.57725,178.01803], "fy":[-87.94418,-84.71274,-81.40548,-84.66402]},
+    {"t":1.148, "x":4.27464, "y":6.00823, "heading":-1.2309, "vx":-1.40716, "vy":-3.49999, "omega":0.00899, "ax":11.26585, "ay":-3.45229, "alpha":1.25676, "fx":[190.19148,188.14523,185.2078,187.64068], "fy":[-49.51276,-56.27863,-65.63574,-58.76514]},
+    {"t":1.17429, "x":4.24154, "y":5.915, "heading":-1.23066, "vx":-1.11092, "vy":-3.59077, "omega":0.04203, "ax":11.23878, "ay":1.69977, "alpha":10.63061, "fx":[182.35329,178.53139,191.73739,196.75791], "fy":[73.47891,77.89809,-37.70721,-0.33265]},
+    {"t":1.20059, "x":4.21621, "y":5.82117, "heading":-1.22956, "vx":-0.81539, "vy":-3.54608, "omega":0.32157, "ax":8.3953, "ay":7.31317, "alpha":13.72072, "fx":[129.76816,70.33227,176.94067,182.74157], "fy":[148.80248,183.35101,81.47469,73.99985]},
+    {"t":1.22688, "x":4.19767, "y":5.73045, "heading":-1.2211, "vx":-0.59463, "vy":-3.35377, "omega":0.68237, "ax":5.45283, "ay":10.4099, "alpha":5.3392, "fx":[90.51112,57.59007,93.36575,122.11731], "fy":[175.96951,189.04095,173.69232,155.40947]},
+    {"t":1.25318, "x":4.18392, "y":5.64586, "heading":-1.20316, "vx":-0.45124, "vy":-3.08003, "omega":0.82277, "ax":3.775, "ay":11.26295, "alpha":1.42997, "fx":[63.81866,53.6971,62.02463,72.16924], "fy":[187.63712,190.72726,188.10862,184.51908]},
+    {"t":1.27947, "x":4.17336, "y":5.56876, "heading":-1.18152, "vx":-0.35197, "vy":-2.78387, "omega":0.86037, "ax":2.81552, "ay":11.56413, "alpha":-0.74256, "fx":[46.13982,51.8292,47.70219,42.06203], "fy":[192.96845,191.54062,192.63764,193.92751]},
+    {"t":1.30577, "x":4.16508, "y":5.49956, "heading":-1.1589, "vx":-0.27794, "vy":-2.47978, "omega":0.84085, "ax":2.21377, "ay":11.69197, "alpha":-2.07433, "fx":[34.14506,50.77093,39.38272,23.31091], "fy":[195.60288,192.0112,194.72607,197.25764]},
+    {"t":1.33207, "x":4.15853, "y":5.43839, "heading":-1.13679, "vx":-0.21972, "vy":-2.17233, "omega":0.7863, "ax":1.80567, "ay":11.75333, "alpha":-2.96385, "fx":[25.73401,50.10811,33.81099,10.7454], "fy":[196.9998,192.31399,195.90651,198.46949]},
+    {"t":1.35836, "x":4.15338, "y":5.38533, "heading":-1.11611, "vx":-0.17224, "vy":-1.86326, "omega":0.70836, "ax":1.51209, "ay":11.78519, "alpha":-3.59676, "fx":[19.64282,49.65961,29.74005,1.78075], "fy":[197.78657,192.52407,196.65878,198.84443]},
+    {"t":1.38466, "x":4.14937, "y":5.34041, "heading":-1.09749, "vx":-0.13248, "vy":-1.55336, "omega":0.61378, "ax":1.29128, "ay":11.80249, "alpha":-4.06862, "fx":[15.0931,49.33555,26.60104,-4.9293], "fy":[198.25354,192.67858,197.17551,198.85991]},
+    {"t":1.41095, "x":4.14634, "y":5.30364, "heading":-1.08135, "vx":-0.09852, "vy":-1.24301, "omega":0.5068, "ax":1.11941, "ay":11.81205, "alpha":-4.43327, "fx":[11.58632,49.08814,24.10151,-10.13579], "fy":[198.54395,192.79757,197.54788,198.71559]},
+    {"t":1.43725, "x":4.14413, "y":5.27504, "heading":-1.06802, "vx":-0.06909, "vy":-0.9324, "omega":0.39022, "ax":0.98194, "ay":11.81725, "alpha":-4.72334, "fx":[8.79028,48.89104,22.0777,-14.28538], "fy":[198.73224,192.89244,197.82447,198.50263]},
+    {"t":1.46354, "x":4.14265, "y":5.25461, "heading":-1.05776, "vx":-0.04327, "vy":-0.62166, "omega":0.26601, "ax":0.86952, "ay":11.81989, "alpha":-4.95973, "fx":[6.4754,48.72939,20.43186,-17.65846], "fy":[198.85865,192.97001,198.03357,198.26546]},
+    {"t":1.48984, "x":4.14182, "y":5.24235, "heading":-1.05076, "vx":-0.0204, "vy":-0.31084, "omega":0.13559, "ax":0.77591, "ay":11.82097, "alpha":-5.1565, "fx":[4.478,48.59456,19.10267,-20.43904], "fy":[198.94552,193.03448,198.19277,198.0268]},
+    {"t":1.51614, "x":4.14155, "y":5.23826, "heading":-1.0472, "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/startDeepToL.traj b/src/main/deploy/choreo/startDeepToL.traj
new file mode 100644
index 00000000..cdef07d3
--- /dev/null
+++ b/src/main/deploy/choreo/startDeepToL.traj
@@ -0,0 +1,99 @@
+{
+ "name":"startDeepToL",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":7.1008875, "y":7.2570308, "heading":3.141592653589793, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":5.336880683898926, "y":6.839954376220703, "heading":-1.865030672844723, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":4.101142883300781, "y":5.999652862548828, "heading":-1.1659041662799314, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":3.85576086490539, "y":5.073261807735684, "heading":5.235987755982989, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"startDeep.x", "val":7.1008875}, "y":{"exp":"startDeep.y", "val":7.2570308}, "heading":{"exp":"startDeep.heading", "val":3.141592653589793}, "intervals":25, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"5.336880683898926 m", "val":5.336880683898926}, "y":{"exp":"6.839954376220703 m", "val":6.839954376220703}, "heading":{"exp":"-1.865030672844723 rad", "val":-1.865030672844723}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"4.101142883300781 m", "val":4.101142883300781}, "y":{"exp":"5.999652862548828 m", "val":5.999652862548828}, "heading":{"exp":"-1.1659041662799314 rad", "val":-1.1659041662799314}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"L.x", "val":3.85576086490539}, "y":{"exp":"L.y", "val":5.073261807735684}, "heading":{"exp":"L.heading", "val":5.235987755982989}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,0.70317,1.18034,1.59507],
+  "samples":[
+    {"t":0.0, "x":7.10089, "y":7.25703, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-6.1157, "ay":-0.62485, "alpha":44.73771, "fx":[-17.60513,-31.32055,-188.65648,-170.20095], "fy":[-198.20588,196.19354,63.80111,-103.45237]},
+    {"t":0.02813, "x":7.09847, "y":7.25678, "heading":3.14159, "vx":-0.17202, "vy":-0.01757, "omega":1.25833, "ax":-6.54037, "ay":-0.70208, "alpha":42.97268, "fx":[-26.6454,-49.00594,-189.71499,-170.73279], "fy":[-197.11386,192.36322,60.43095,-102.49351]},
+    {"t":0.05625, "x":7.09104, "y":7.25601, "heading":-3.1062, "vx":-0.35598, "vy":-0.03732, "omega":2.46702, "ax":-7.16306, "ay":-0.99789, "alpha":40.14913, "fx":[-35.55912,-80.34168,-192.13587,-169.58237], "fy":[-195.56522,181.31689,51.98,-104.26883]},
+    {"t":0.08438, "x":7.0782, "y":7.25457, "heading":-3.03681, "vx":-0.55745, "vy":-0.06539, "omega":3.59629, "ax":-8.0428, "ay":-1.59248, "alpha":35.52511, "fx":[-50.93333,-122.63705,-195.33555,-167.37252], "fy":[-191.8851,155.58511,37.70776,-107.59133]},
+    {"t":0.11251, "x":7.05934, "y":7.2521, "heading":-2.93566, "vx":-0.78367, "vy":-0.11018, "omega":4.5955, "ax":-9.32965, "ay":-2.5334, "alpha":27.0282, "fx":[-88.57947,-169.37678,-198.1552,-165.97166], "fy":[-177.17232,102.25272,15.37307,-109.37602]},
+    {"t":0.14063, "x":7.0336, "y":7.248, "heading":-2.8064, "vx":-1.04608, "vy":-0.18144, "omega":5.35572, "ax":-10.92183, "ay":-3.54645, "alpha":10.69184, "fx":[-161.05411,-197.58186,-196.66268,-172.94806], "fy":[-114.40487,-0.21714,-24.96902,-96.87976]},
+    {"t":0.16876, "x":6.99986, "y":7.24149, "heading":-2.65576, "vx":-1.35328, "vy":-0.28119, "omega":5.65644, "ax":-9.37215, "ay":-3.9267, "alpha":-22.85924, "fx":[-196.42171,-148.1804,-94.51563,-185.79891], "fy":[-22.6905,-130.80915,-171.00542,62.6803]},
+    {"t":0.19689, "x":6.95809, "y":7.23203, "heading":-2.49666, "vx":-1.61689, "vy":-0.39163, "omega":5.01349, "ax":-9.12241, "ay":-3.26395, "alpha":-25.16118, "fx":[-196.86511,-143.93228,-97.98769,-169.4799], "fy":[-16.61758,-134.87576,-165.07479,98.93397]},
+    {"t":0.22501, "x":6.909, "y":7.21972, "heading":-2.35565, "vx":-1.87347, "vy":-0.48344, "omega":4.30578, "ax":-9.22183, "ay":-2.80837, "alpha":-23.88119, "fx":[-196.49932,-137.34766,-119.02262,-162.0244], "fy":[-18.12974,-140.83779,-139.01246,110.72344]},
+    {"t":0.25314, "x":6.85266, "y":7.20501, "heading":-2.23454, "vx":-2.13285, "vy":-0.56243, "omega":3.63408, "ax":-9.4544, "ay":-1.45363, "alpha":-22.52847, "fx":[-195.41558,-126.66444,-148.18373,-160.13741], "fy":[-24.88242,-149.61475,-35.4415,113.01324]},
+    {"t":0.28127, "x":6.78893, "y":7.18862, "heading":-2.13233, "vx":-2.39878, "vy":-0.60332, "omega":3.00042, "ax":-9.73696, "ay":-0.53039, "alpha":-21.83634, "fx":[-193.32601,-124.99058,-162.25397,-168.67146], "fy":[-34.52587,-149.65435,50.28121,98.5334]},
+    {"t":0.3094, "x":6.71761, "y":7.17144, "heading":-2.04793, "vx":-2.67265, "vy":-0.61823, "omega":2.38623, "ax":-9.71163, "ay":-0.79368, "alpha":-21.70631, "fx":[-190.32886,-121.13105,-161.53137,-174.56124], "fy":[-44.21092,-150.75227,57.02134,85.02097]},
+    {"t":0.33752, "x":6.63859, "y":7.15374, "heading":-1.98082, "vx":-2.94581, "vy":-0.64056, "omega":1.7757, "ax":-9.49721, "ay":-1.25037, "alpha":-21.7262, "fx":[-186.0021,-113.81927,-156.02013,-177.41451], "fy":[-53.55358,-152.82043,49.95491,73.04686]},
+    {"t":0.36565, "x":6.55198, "y":7.13523, "heading":-1.93087, "vx":-3.21293, "vy":-0.67573, "omega":1.16461, "ax":-8.98954, "ay":-1.87229, "alpha":-21.34461, "fx":[-178.67419,-104.08565,-140.50577,-176.13938], "fy":[-61.56757,-151.47154,28.45134,59.74716]},
+    {"t":0.39378, "x":6.45806, "y":7.11548, "heading":-1.89811, "vx":-3.46578, "vy":-0.72839, "omega":0.56426, "ax":-6.99377, "ay":-2.63156, "alpha":-17.47355, "fx":[-149.00611,-84.28652,-83.92893,-149.10942], "fy":[-63.40593,-121.69293,-17.27627,26.90754]},
+    {"t":0.4219, "x":6.35781, "y":7.09395, "heading":-1.88224, "vx":-3.66249, "vy":-0.80241, "omega":0.07278, "ax":-0.22051, "ay":-0.68203, "alpha":-0.86342, "fx":[-6.32395,-2.31169,-1.02262,-5.04505], "fy":[-12.71763,-14.00932,-10.02235,-8.7274]},
+    {"t":0.45003, "x":6.25471, "y":7.07111, "heading":-1.8802, "vx":-3.66869, "vy":-0.82159, "omega":0.0485, "ax":0.02439, "ay":-0.13426, "alpha":-0.0132, "fx":[0.36612,0.42735,0.44693,0.3857], "fy":[-2.25882,-2.27839,-2.21717,-2.19761]},
+    {"t":0.47816, "x":6.15153, "y":7.04795, "heading":-1.87883, "vx":-3.66801, "vy":-0.82537, "omega":0.04812, "ax":0.00676, "ay":-0.0306, "alpha":0.00019, "fx":[0.11335,0.11245,0.11216,0.11306], "fy":[-0.50982,-0.50953,-0.51044,-0.51072]},
+    {"t":0.50628, "x":6.04836, "y":7.02472, "heading":-1.87748, "vx":-3.66782, "vy":-0.82623, "omega":0.04813, "ax":0.00345, "ay":-0.01511, "alpha":0.00066, "fx":[0.05957,0.05652,0.05556,0.0586], "fy":[-0.25082,-0.24986,-0.2529,-0.25387]},
+    {"t":0.53441, "x":5.9452, "y":7.00148, "heading":-1.87613, "vx":-3.66772, "vy":-0.82665, "omega":0.04815, "ax":0.00971, "ay":-0.04371, "alpha":0.0001, "fx":[0.16222,0.16174,0.16159,0.16207], "fy":[-0.72854,-0.72839,-0.72886,-0.72901]},
+    {"t":0.56254, "x":5.84204, "y":6.97821, "heading":-1.87477, "vx":-3.66745, "vy":-0.82788, "omega":0.04815, "ax":0.04323, "ay":-0.1959, "alpha":-0.00277, "fx":[0.7121,0.72498,0.72902,0.71614], "fy":[-3.27006,-3.27409,-3.26122,-3.25719]},
+    {"t":0.59066, "x":5.7389, "y":6.95485, "heading":-1.87342, "vx":-3.66623, "vy":-0.83339, "omega":0.04807, "ax":0.20104, "ay":-0.88906, "alpha":-0.01239, "fx":[3.31312,3.37098,3.38921,3.33134], "fy":[-14.84,-14.85775,-14.8004,-14.78265]},
+    {"t":0.61879, "x":5.63586, "y":6.93105, "heading":-1.87206, "vx":-3.66058, "vy":-0.8584, "omega":0.04772, "ax":0.94582, "ay":-3.64006, "alpha":0.07557, "fx":[16.02245,15.64573,15.5103,15.88675], "fy":[-60.55634,-60.47337,-60.79986,-60.88262]},
+    {"t":0.64692, "x":5.53328, "y":6.90547, "heading":-1.87072, "vx":-3.63398, "vy":-0.96078, "omega":0.04985, "ax":3.94253, "ay":-7.68502, "alpha":3.64959, "fx":[82.65895,62.80932,48.70948,68.70272], "fy":[-119.20456,-122.96251,-136.77856,-133.47656]},
+    {"t":0.67504, "x":5.43262, "y":6.87541, "heading":-1.86932, "vx":-3.52308, "vy":-1.17694, "omega":0.1525, "ax":8.47278, "ay":-5.93608, "alpha":13.52403, "fx":[178.01654,166.3716,84.84064,135.71993], "fy":[-54.86856,-51.36273,-160.0326,-129.54278]},
+    {"t":0.70317, "x":5.33688, "y":6.83995, "heading":-1.86503, "vx":-3.28477, "vy":-1.3439, "omega":0.53289, "ax":8.93072, "ay":-5.08812, "alpha":13.99414, "fx":[181.44178,172.10326,98.28835,143.65013], "fy":[-41.5296,-27.44376,-150.37629,-119.91652]},
+    {"t":0.72589, "x":5.26455, "y":6.8081, "heading":-1.85292, "vx":-3.08184, "vy":-1.45951, "omega":0.85087, "ax":9.13687, "ay":-2.72951, "alpha":15.91799, "fx":[179.83887,160.38464,115.03288,153.97278], "fy":[-5.71711,34.34237,-117.41609,-93.20768]},
+    {"t":0.74862, "x":5.19688, "y":6.77424, "heading":-1.83359, "vx":-2.87423, "vy":-1.52154, "omega":1.21257, "ax":8.57898, "ay":-0.07937, "alpha":17.18463, "fx":[168.70384,126.40422,118.86867,158.05284], "fy":[32.65686,88.14627,-65.84781,-60.24733]},
+    {"t":0.77134, "x":5.13378, "y":6.73964, "heading":-1.80604, "vx":-2.67929, "vy":-1.52334, "omega":1.60305, "ax":7.22157, "ay":2.26137, "alpha":17.03529, "fx":[148.37913,86.62482,95.19271,151.32395], "fy":[63.66266,115.8189,-2.92447,-25.77334]},
+    {"t":0.79406, "x":5.07477, "y":6.70561, "heading":-1.76961, "vx":-2.5152, "vy":-1.47196, "omega":1.99013, "ax":5.40425, "ay":3.80426, "alpha":15.43728, "fx":[120.64324,54.11673,55.17402,130.41073], "fy":[83.03104,120.24775,42.10775,8.27423]},
+    {"t":0.81678, "x":5.01901, "y":6.67315, "heading":-1.72439, "vx":-2.3924, "vy":-1.38551, "omega":2.3409, "ax":3.26022, "ay":4.35551, "alpha":12.29552, "fx":[83.16351,24.75907,19.17895,90.28351], "fy":[89.70321,108.97929,56.18027,35.55414]},
+    {"t":0.83951, "x":4.96549, "y":6.64279, "heading":-1.6712, "vx":-2.31832, "vy":-1.28655, "omega":2.62029, "ax":0.64541, "ay":3.82085, "alpha":7.09063, "fx":[29.98602,-5.82806,-10.58391,29.46096], "fy":[77.02498,81.1501,50.52942,46.06254]},
+    {"t":0.86223, "x":4.91298, "y":6.61454, "heading":-1.61166, "vx":-2.30366, "vy":-1.19973, "omega":2.78141, "ax":-2.10832, "ay":2.07939, "alpha":0.09089, "fx":[-34.90179,-35.34249,-35.38779,-34.94684], "fy":[34.88897,34.8798,34.43618,34.44503]},
+    {"t":0.88495, "x":4.86009, "y":6.58782, "heading":-1.54846, "vx":-2.35157, "vy":-1.15248, "omega":2.78347, "ax":-3.96006, "ay":-0.16256, "alpha":-6.06555, "fx":[-78.73525,-52.4374,-53.31755,-79.55927], "fy":[-18.54305,-19.67698,14.72746,12.65308]},
+    {"t":0.90767, "x":4.80564, "y":6.56159, "heading":-1.48521, "vx":-2.44155, "vy":-1.15617, "omega":2.64565, "ax":-4.71125, "ay":-2.18804, "alpha":-10.20306, "fx":[-94.73409,-51.62488,-62.77326,-105.00478], "fy":[-61.16996,-68.09378,-6.62758,-10.00286]},
+    {"t":0.9304, "x":4.74894, "y":6.53475, "heading":-1.4251, "vx":-2.5486, "vy":-1.20589, "omega":2.41381, "ax":-4.72431, "ay":-3.96384, "alpha":-12.95196, "fx":[-95.38078,-37.54852,-63.40477,-118.67389], "fy":[-94.38589,-105.83992,-31.18423,-32.89119]},
+    {"t":0.95312, "x":4.68981, "y":6.50633, "heading":-1.37025, "vx":-2.65595, "vy":-1.29596, "omega":2.11951, "ax":-4.18142, "ay":-5.59395, "alpha":-14.69294, "fx":[-86.94297,-15.31717,-52.51582,-124.03326], "fy":[-121.70266,-133.29172,-60.9187,-57.08065]},
+    {"t":0.97584, "x":4.62838, "y":6.47544, "heading":-1.32209, "vx":-2.75096, "vy":-1.42307, "omega":1.78565, "ax":-3.0722, "ay":-7.16215, "alpha":-15.37198, "fx":[-71.03713,10.91863,-24.62151,-120.10807], "fy":[-144.74257,-151.33403,-96.38344,-85.09828]},
+    {"t":0.99856, "x":4.56508, "y":6.44125, "heading":-1.28151, "vx":-2.82077, "vy":-1.58581, "omega":1.43636, "ax":-1.35281, "ay":-8.60779, "alpha":-14.72203, "fx":[-46.7454,37.57243,19.15583,-100.18544], "fy":[-163.78884,-161.20459,-128.848,-120.10929]},
+    {"t":1.02129, "x":4.50064, "y":6.403, "heading":-1.24887, "vx":-2.85151, "vy":-1.7814, "omega":1.10184, "ax":0.86495, "ay":-9.63649, "alpha":-12.60259, "fx":[-13.27429,63.63627,63.29937,-55.98795], "fy":[-177.21342,-164.12508,-144.95773,-156.24668]},
+    {"t":1.04401, "x":4.43607, "y":6.36003, "heading":-1.22384, "vx":-2.83185, "vy":-2.00036, "omega":0.81547, "ax":3.21544, "ay":-9.98734, "alpha":-9.53622, "fx":[25.3993,87.79833,96.61742,4.58416], "fy":[-182.0148,-161.27315,-146.59919,-176.04955]},
+    {"t":1.06673, "x":4.37255, "y":6.312, "heading":-1.20531, "vx":-2.75879, "vy":-2.2273, "omega":0.59879, "ax":5.16803, "ay":-9.76082, "alpha":-6.79926, "fx":[61.17725,107.52136,118.41127,57.48458], "fy":[-178.23708,-155.12384,-142.29765,-175.17406]},
+    {"t":1.08945, "x":4.3112, "y":6.25887, "heading":-1.1917, "vx":-2.64136, "vy":-2.44909, "omega":0.44429, "ax":6.55275, "ay":-9.28605, "alpha":-5.0935, "fx":[88.48605,122.79967,133.32178,92.31705], "fy":[-170.09908,-147.68553,-135.76764,-165.62398]},
+    {"t":1.11218, "x":4.25287, "y":6.20082, "heading":-1.18161, "vx":-2.49246, "vy":-2.66009, "omega":0.32855, "ax":7.4959, "ay":-8.77904, "alpha":-4.33547, "fx":[106.76814,134.6558,144.93175,113.45669], "fy":[-161.91402,-139.97334,-127.74204,-155.74]},
+    {"t":1.1349, "x":4.19817, "y":6.13811, "heading":-1.17414, "vx":-2.32214, "vy":-2.85957, "omega":0.23004, "ax":8.133, "ay":-8.32925, "alpha":-4.29293, "fx":[117.66709,143.75445,154.70642,126.16503], "fy":[-156.01741,-132.7251,-118.59864,-148.03703]},
+    {"t":1.15762, "x":4.14751, "y":6.07099, "heading":-1.16891, "vx":-2.13734, "vy":-3.04884, "omega":0.13249, "ax":8.5349, "ay":-7.96622, "alpha":-5.16145, "fx":[121.2479,150.92375,164.50691,132.41248], "fy":[-154.53654,-126.07393,-106.61883,-143.94294]},
+    {"t":1.18034, "x":4.10114, "y":5.99965, "heading":-1.1659, "vx":-1.9434, "vy":-3.22985, "omega":0.01521, "ax":10.29218, "ay":-5.78236, "alpha":-0.42263, "fx":[170.16882,171.78838,172.95089,171.35474], "fy":[-98.86575,-96.08848,-93.89351,-96.70861]},
+    {"t":1.20997, "x":4.04809, "y":5.90144, "heading":-1.16545, "vx":-1.63851, "vy":-3.40114, "omega":0.00269, "ax":10.79392, "ay":-4.61571, "alpha":-0.01245, "fx":[179.89643,179.93173,179.96252,179.92725], "fy":[-77.0191,-76.94049,-76.86435,-76.94298]},
+    {"t":1.23959, "x":4.00429, "y":5.79866, "heading":-1.16537, "vx":-1.31876, "vy":-3.53787, "omega":0.00233, "ax":11.17062, "ay":-3.25467, "alpha":0.7252, "fx":[187.54558,186.24194,184.81322,186.23502], "fy":[-49.61491,-53.76337,-58.89901,-54.73762]},
+    {"t":1.26921, "x":3.97012, "y":5.69243, "heading":-1.1653, "vx":-0.98785, "vy":-3.63429, "omega":0.02381, "ax":8.67867, "ay":6.68088, "alpha":14.65671, "fx":[132.05344,78.84021,183.91266,183.87101], "fy":[144.99921,177.317,55.83424,67.3179]},
+    {"t":1.29884, "x":3.94467, "y":5.5877, "heading":-1.1646, "vx":-0.73076, "vy":-3.43638, "omega":0.45799, "ax":4.70137, "ay":10.82216, "alpha":3.4575, "fx":[78.14076,56.39978,79.30123,99.63661], "fy":[181.50719,189.18004,180.47844,170.43525]},
+    {"t":1.32846, "x":3.92508, "y":5.49065, "heading":-1.15103, "vx":-0.5915, "vy":-3.11579, "omega":0.56041, "ax":3.35382, "ay":11.40537, "alpha":0.3986, "fx":[56.11924,53.2973,55.69327,58.5163], "fy":[190.08609,190.88119,190.17709,189.34369]},
+    {"t":1.35808, "x":3.90903, "y":5.40336, "heading":-1.13443, "vx":-0.49214, "vy":-2.77793, "omega":0.57222, "ax":2.74469, "ay":11.58551, "alpha":-0.95691, "fx":[44.991,52.10656,46.4898,39.42327], "fy":[193.32998,191.56193,193.03285,194.57511]},
+    {"t":1.38771, "x":3.89566, "y":5.32615, "heading":-1.11748, "vx":-0.41084, "vy":-2.43473, "omega":0.54387, "ax":2.40252, "ay":11.66572, "alpha":-1.70898, "fx":[38.50271,51.49288,41.48892,28.71064], "fy":[194.90903,191.92531,194.3751,196.63806]},
+    {"t":1.41733, "x":3.88454, "y":5.25914, "heading":-1.10137, "vx":-0.33967, "vy":-2.08915, "omega":0.49324, "ax":2.18425, "ay":11.70925, "alpha":-2.18489, "fx":[34.35692,51.1239,38.27362,21.88719], "fy":[195.80355,192.15018,195.15905,197.63739]},
+    {"t":1.44695, "x":3.87544, "y":5.20239, "heading":-1.08676, "vx":-0.27496, "vy":-1.74229, "omega":0.42852, "ax":2.03316, "ay":11.73597, "alpha":-2.51246, "fx":[31.53059,50.87832,35.99592,17.16227], "fy":[196.36454,192.30294,195.67621,198.18814]},
+    {"t":1.47658, "x":3.86818, "y":5.15593, "heading":-1.07406, "vx":-0.21473, "vy":-1.39463, "omega":0.35409, "ax":1.92246, "ay":11.75379, "alpha":-2.75142, "fx":[29.50137,50.70233,34.28551,13.6965], "fy":[196.74386,192.41367,196.04359,198.51927]},
+    {"t":1.5062, "x":3.86267, "y":5.11977, "heading":-1.06357, "vx":-0.15779, "vy":-1.04644, "omega":0.27259, "ax":1.8379, "ay":11.76642, "alpha":-2.93334, "fx":[27.97471,50.56909,32.9565,11.04719], "fy":[197.01649,192.49783,196.31686,198.73106]},
+    {"t":1.53582, "x":3.8588, "y":5.09394, "heading":-1.0555, "vx":-0.10334, "vy":-0.69788, "omega":0.18569, "ax":1.77122, "ay":11.77577, "alpha":-3.07646, "fx":[26.77162,50.46419,31.90601,8.95953], "fy":[197.22325,192.56406,196.52577,198.87288]},
+    {"t":1.56545, "x":3.85651, "y":5.07843, "heading":-1.05, "vx":-0.05087, "vy":-0.34905, "omega":0.09456, "ax":1.71729, "ay":11.78295, "alpha":-3.19203, "fx":[25.77683,50.37938,31.07292,7.27676], "fy":[197.38812,192.61751,196.68761,198.97126]},
+    {"t":1.59507, "x":3.85576, "y":5.07326, "heading":-1.0472, "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/startGToG.traj b/src/main/deploy/choreo/startGToG.traj
new file mode 100644
index 00000000..e995b6b6
--- /dev/null
+++ b/src/main/deploy/choreo/startGToG.traj
@@ -0,0 +1,50 @@
+{
+ "name":"startGToG",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":7.1008875, "y":3.6225774894, "heading":3.141592653589793, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":5.7769125, "y":3.6225774894, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"startG.x", "val":7.1008875}, "y":{"exp":"startG.y", "val":3.6225774894}, "heading":{"exp":"startG.heading", "val":3.141592653589793}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"G.x", "val":5.713146}, "y":{"exp":"G.y", "val":3.7209}, "heading":{"exp":"G.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,0.71864],
+  "samples":[
+    {"t":0.0, "x":7.10089, "y":3.62258, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-10.30717, "ay":0.0, "alpha":0.0, "fx":[-175.32205,-175.32205,-175.32205,-175.32205], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.04791, "x":7.08906, "y":3.62258, "heading":3.14159, "vx":-0.49381, "vy":0.0, "omega":0.0, "ax":-10.30564, "ay":0.0, "alpha":0.0, "fx":[-175.29602,-175.29602,-175.29602,-175.29602], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.09582, "x":7.05357, "y":3.62258, "heading":3.14159, "vx":-0.98755, "vy":0.0, "omega":0.0, "ax":-10.3035, "ay":0.0, "alpha":0.0, "fx":[-175.2596,-175.2596,-175.2596,-175.2596], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.14373, "x":6.99444, "y":3.62258, "heading":3.14159, "vx":-1.48118, "vy":0.0, "omega":0.0, "ax":-10.30029, "ay":0.0, "alpha":0.0, "fx":[-175.20495,-175.20495,-175.20495,-175.20495], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.19164, "x":6.91165, "y":3.62258, "heading":3.14159, "vx":-1.97466, "vy":0.0, "omega":0.0, "ax":-10.29493, "ay":0.0, "alpha":0.0, "fx":[-175.11389,-175.11389,-175.11389,-175.11389], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.23955, "x":6.80523, "y":3.62258, "heading":3.14159, "vx":-2.46789, "vy":0.0, "omega":0.0, "ax":-10.28423, "ay":0.0, "alpha":0.0, "fx":[-174.93185,-174.93185,-174.93185,-174.93185], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.28746, "x":6.67519, "y":3.62258, "heading":3.14159, "vx":-2.9606, "vy":0.0, "omega":0.0, "ax":-10.25218, "ay":0.0, "alpha":0.0, "fx":[-174.3866,-174.3866,-174.3866,-174.3866], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.33537, "x":6.52159, "y":3.62258, "heading":3.14159, "vx":-3.45178, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.38328, "x":6.35621, "y":3.62258, "heading":3.14159, "vx":-3.45178, "vy":0.0, "omega":0.0, "ax":10.25218, "ay":0.0, "alpha":0.0, "fx":[174.3866,174.3866,174.3866,174.3866], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.43118, "x":6.20261, "y":3.62258, "heading":3.14159, "vx":-2.9606, "vy":0.0, "omega":0.0, "ax":10.28423, "ay":0.0, "alpha":0.0, "fx":[174.93185,174.93185,174.93185,174.93185], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.47909, "x":6.07257, "y":3.62258, "heading":3.14159, "vx":-2.46789, "vy":0.0, "omega":0.0, "ax":10.29493, "ay":0.0, "alpha":0.0, "fx":[175.11389,175.11389,175.11389,175.11389], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.527, "x":5.96615, "y":3.62258, "heading":3.14159, "vx":-1.97466, "vy":0.0, "omega":0.0, "ax":10.30029, "ay":0.0, "alpha":0.0, "fx":[175.20495,175.20495,175.20495,175.20495], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.57491, "x":5.88336, "y":3.62258, "heading":3.14159, "vx":-1.48118, "vy":0.0, "omega":0.0, "ax":10.3035, "ay":0.0, "alpha":0.0, "fx":[175.2596,175.2596,175.2596,175.2596], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.62282, "x":5.82423, "y":3.62258, "heading":3.14159, "vx":-0.98755, "vy":0.0, "omega":0.0, "ax":10.30564, "ay":0.0, "alpha":0.0, "fx":[175.29602,175.29602,175.29602,175.29602], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.67073, "x":5.78874, "y":3.62258, "heading":3.14159, "vx":-0.49381, "vy":0.0, "omega":0.0, "ax":10.30717, "ay":0.0, "alpha":0.0, "fx":[175.32205,175.32205,175.32205,175.32205], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.71864, "x":5.77691, "y":3.62258, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+  "splits":[0]
+ },
+ "events":[]
+}
diff --git a/src/main/deploy/choreo/startHToH.traj b/src/main/deploy/choreo/startHToH.traj
new file mode 100644
index 00000000..7e9dd17a
--- /dev/null
+++ b/src/main/deploy/choreo/startHToH.traj
@@ -0,0 +1,49 @@
+{
+ "name":"startHToH",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":7.1008875, "y":3.9512534894, "heading":3.141592653589793, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":5.713146, "y":4.0509, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"startH.x", "val":7.1008875}, "y":{"exp":"startH.y", "val":3.9512534894}, "heading":{"exp":"startH.heading", "val":3.141592653589793}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"H.x", "val":5.713146}, "y":{"exp":"H.y", "val":4.0509}, "heading":{"exp":"H.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,0.68648],
+  "samples":[
+    {"t":0.0, "x":7.10089, "y":3.95125, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-11.91965, "ay":0.85589, "alpha":0.0, "fx":[-198.69483,-198.69483,-198.69483,-198.69483], "fy":[14.26724,14.26724,14.26724,14.26724]},
+    {"t":0.04903, "x":7.08656, "y":3.95228, "heading":3.14159, "vx":-0.58447, "vy":0.04197, "omega":0.0, "ax":-11.91755, "ay":0.85574, "alpha":0.0, "fx":[-198.65988,-198.65988,-198.65988,-198.65988], "fy":[14.26473,14.26473,14.26473,14.26473]},
+    {"t":0.09807, "x":7.04357, "y":3.95537, "heading":3.14159, "vx":-1.16885, "vy":0.08393, "omega":0.0, "ax":-11.91441, "ay":0.85551, "alpha":0.0, "fx":[-198.60745,-198.60745,-198.60745,-198.60745], "fy":[14.26097,14.26097,14.26097,14.26097]},
+    {"t":0.1471, "x":6.97193, "y":3.96051, "heading":3.14159, "vx":-1.75306, "vy":0.12588, "omega":0.0, "ax":-11.90916, "ay":0.85514, "alpha":0.0, "fx":[-198.52004,-198.52004,-198.52004,-198.52004], "fy":[14.25469,14.25469,14.25469,14.25469]},
+    {"t":0.19614, "x":6.87166, "y":3.96771, "heading":3.14159, "vx":-2.33702, "vy":0.16781, "omega":0.0, "ax":-11.89868, "ay":0.85438, "alpha":0.0, "fx":[-198.34521,-198.34521,-198.34521,-198.34521], "fy":[14.24214,14.24214,14.24214,14.24214]},
+    {"t":0.24517, "x":6.74276, "y":3.97697, "heading":3.14159, "vx":-2.92047, "vy":0.2097, "omega":0.0, "ax":-11.86726, "ay":0.85213, "alpha":0.0, "fx":[-197.82156,-197.82156,-197.82156,-197.82156], "fy":[14.20454,14.20454,14.20454,14.20454]},
+    {"t":0.29421, "x":6.58529, "y":3.98828, "heading":3.14159, "vx":-3.50238, "vy":0.25149, "omega":0.0, "ax":-5.43311, "ay":0.39012, "alpha":0.0, "fx":[-90.56729,-90.56729,-90.56729,-90.56729], "fy":[6.50317,6.50317,6.50317,6.50317]},
+    {"t":0.34324, "x":6.40702, "y":4.00108, "heading":3.14159, "vx":-3.76879, "vy":0.27062, "omega":0.0, "ax":5.43311, "ay":-0.39012, "alpha":0.0, "fx":[90.56729,90.56729,90.56729,90.56729], "fy":[-6.50317,-6.50317,-6.50317,-6.50317]},
+    {"t":0.39228, "x":6.22875, "y":4.01388, "heading":3.14159, "vx":-3.50238, "vy":0.25149, "omega":0.0, "ax":11.86726, "ay":-0.85213, "alpha":0.0, "fx":[197.82156,197.82156,197.82156,197.82156], "fy":[-14.20454,-14.20454,-14.20454,-14.20454]},
+    {"t":0.44131, "x":6.07128, "y":4.02518, "heading":3.14159, "vx":-2.92047, "vy":0.2097, "omega":0.0, "ax":11.89868, "ay":-0.85438, "alpha":0.0, "fx":[198.34521,198.34521,198.34521,198.34521], "fy":[-14.24214,-14.24214,-14.24214,-14.24214]},
+    {"t":0.49035, "x":5.94238, "y":4.03444, "heading":3.14159, "vx":-2.33702, "vy":0.16781, "omega":0.0, "ax":11.90916, "ay":-0.85514, "alpha":0.0, "fx":[198.52004,198.52004,198.52004,198.52004], "fy":[-14.25469,-14.25469,-14.25469,-14.25469]},
+    {"t":0.53938, "x":5.8421, "y":4.04164, "heading":3.14159, "vx":-1.75306, "vy":0.12588, "omega":0.0, "ax":11.91441, "ay":-0.85551, "alpha":0.0, "fx":[198.60745,198.60745,198.60745,198.60745], "fy":[-14.26097,-14.26097,-14.26097,-14.26097]},
+    {"t":0.58841, "x":5.77046, "y":4.04678, "heading":3.14159, "vx":-1.16885, "vy":0.08393, "omega":0.0, "ax":11.91755, "ay":-0.85574, "alpha":0.0, "fx":[198.65988,198.65988,198.65988,198.65988], "fy":[-14.26473,-14.26473,-14.26473,-14.26473]},
+    {"t":0.63745, "x":5.72748, "y":4.04987, "heading":3.14159, "vx":-0.58447, "vy":0.04197, "omega":0.0, "ax":11.91965, "ay":-0.85589, "alpha":0.0, "fx":[198.69483,198.69483,198.69483,198.69483], "fy":[-14.26724,-14.26724,-14.26724,-14.26724]},
+    {"t":0.68648, "x":5.71315, "y":4.0509, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+  "splits":[0]
+ },
+ "events":[]
+}
diff --git a/src/main/deploy/choreo/startToG.traj b/src/main/deploy/choreo/startToG.traj
new file mode 100644
index 00000000..5fa6c15b
--- /dev/null
+++ b/src/main/deploy/choreo/startToG.traj
@@ -0,0 +1,52 @@
+{
+ "name":"startToG",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":5.713146, "y":3.7209, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":17, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"G.x", "val":5.713146}, "y":{"exp":"G.y", "val":3.7209}, "heading":{"exp":"G.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,0.83154],
+  "samples":[
+    {"t":0.0, "x":7.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-8.55109, "ay":-8.34798, "alpha":0.0, "fx":[-142.54254,-142.54254,-142.54254,-142.54254], "fy":[-139.15676,-139.15676,-139.15676,-139.15676]},
+    {"t":0.04891, "x":7.09066, "y":5.06569, "heading":3.14159, "vx":-0.41827, "vy":-0.40833, "omega":0.0, "ax":-8.54958, "ay":-8.3465, "alpha":0.0, "fx":[-142.5174,-142.5174,-142.5174,-142.5174], "fy":[-139.13222,-139.13222,-139.13222,-139.13222]},
+    {"t":0.09783, "x":7.05997, "y":5.03573, "heading":3.14159, "vx":-0.83646, "vy":-0.81659, "omega":0.0, "ax":-8.54732, "ay":-8.3443, "alpha":0.0, "fx":[-142.47968,-142.47968,-142.47968,-142.47968], "fy":[-139.0954,-139.0954,-139.0954,-139.0954]},
+    {"t":0.14674, "x":7.00883, "y":4.98581, "heading":3.14159, "vx":-1.25454, "vy":-1.22474, "omega":0.0, "ax":-8.54355, "ay":-8.34061, "alpha":0.0, "fx":[-142.41681,-142.41681,-142.41681,-142.41681], "fy":[-139.03401,-139.03401,-139.03401,-139.03401]},
+    {"t":0.19566, "x":6.93725, "y":4.91592, "heading":3.14159, "vx":-1.67244, "vy":-1.63272, "omega":0.0, "ax":-8.536, "ay":-8.33325, "alpha":0.0, "fx":[-142.29108,-142.29108,-142.29108,-142.29108], "fy":[-138.91127,-138.91127,-138.91127,-138.91127]},
+    {"t":0.24457, "x":6.84523, "y":4.82609, "heading":3.14159, "vx":-2.08997, "vy":-2.04033, "omega":0.0, "ax":-8.51342, "ay":-8.3112, "alpha":0.0, "fx":[-141.91464,-141.91464,-141.91464,-141.91464], "fy":[-138.54377,-138.54377,-138.54377,-138.54377]},
+    {"t":0.29348, "x":6.73282, "y":4.71635, "heading":3.14159, "vx":-2.5064, "vy":-2.44686, "omega":0.0, "ax":-4.03419, "ay":-3.93837, "alpha":0.0, "fx":[-67.248,-67.248,-67.248,-67.248], "fy":[-65.65067,-65.65067,-65.65067,-65.65067]},
+    {"t":0.3424, "x":6.60539, "y":4.59195, "heading":3.14159, "vx":-2.70372, "vy":-2.6395, "omega":0.0, "ax":-0.00029, "ay":-0.00028, "alpha":0.0, "fx":[-0.00483,-0.00483,-0.00483,-0.00483], "fy":[-0.00472,-0.00472,-0.00472,-0.00472]},
+    {"t":0.39131, "x":6.47314, "y":4.46284, "heading":3.14159, "vx":-2.70374, "vy":-2.63952, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.44023, "x":6.34089, "y":4.33373, "heading":3.14159, "vx":-2.70374, "vy":-2.63952, "omega":0.0, "ax":0.00029, "ay":0.00028, "alpha":0.0, "fx":[0.00483,0.00483,0.00483,0.00483], "fy":[0.00472,0.00472,0.00472,0.00472]},
+    {"t":0.48914, "x":6.20864, "y":4.20463, "heading":3.14159, "vx":-2.70372, "vy":-2.6395, "omega":0.0, "ax":4.03419, "ay":3.93837, "alpha":0.0, "fx":[67.248,67.248,67.248,67.248], "fy":[65.65067,65.65067,65.65067,65.65067]},
+    {"t":0.53805, "x":6.08122, "y":4.08023, "heading":3.14159, "vx":-2.5064, "vy":-2.44686, "omega":0.0, "ax":8.51342, "ay":8.3112, "alpha":0.0, "fx":[141.91464,141.91464,141.91464,141.91464], "fy":[138.54377,138.54377,138.54377,138.54377]},
+    {"t":0.58697, "x":5.9688, "y":3.97049, "heading":3.14159, "vx":-2.08997, "vy":-2.04033, "omega":0.0, "ax":8.536, "ay":8.33325, "alpha":0.0, "fx":[142.29108,142.29108,142.29108,142.29108], "fy":[138.91127,138.91127,138.91127,138.91127]},
+    {"t":0.63588, "x":5.87679, "y":3.88065, "heading":3.14159, "vx":-1.67244, "vy":-1.63272, "omega":0.0, "ax":8.54355, "ay":8.34061, "alpha":0.0, "fx":[142.41681,142.41681,142.41681,142.41681], "fy":[139.03401,139.03401,139.03401,139.03401]},
+    {"t":0.68479, "x":5.8052, "y":3.81077, "heading":3.14159, "vx":-1.25454, "vy":-1.22474, "omega":0.0, "ax":8.54732, "ay":8.3443, "alpha":0.0, "fx":[142.47968,142.47968,142.47968,142.47968], "fy":[139.0954,139.0954,139.0954,139.0954]},
+    {"t":0.73371, "x":5.75406, "y":3.76084, "heading":3.14159, "vx":-0.83646, "vy":-0.81659, "omega":0.0, "ax":8.54958, "ay":8.3465, "alpha":0.0, "fx":[142.5174,142.5174,142.5174,142.5174], "fy":[139.13222,139.13222,139.13222,139.13222]},
+    {"t":0.78262, "x":5.72338, "y":3.73089, "heading":3.14159, "vx":-0.41827, "vy":-0.40833, "omega":0.0, "ax":8.55109, "ay":8.34798, "alpha":0.0, "fx":[142.54254,142.54254,142.54254,142.54254], "fy":[139.15676,139.15676,139.15676,139.15676]},
+    {"t":0.83154, "x":5.71315, "y":3.7209, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+  "splits":[0]
+ },
+ "events":[]
+}
diff --git a/src/main/deploy/choreo/startToH.traj b/src/main/deploy/choreo/startToH.traj
new file mode 100644
index 00000000..ee42fda4
--- /dev/null
+++ b/src/main/deploy/choreo/startToH.traj
@@ -0,0 +1,51 @@
+{
+ "name":"startToH",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":5.713146, "y":4.0509, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"H.x", "val":5.713146}, "y":{"exp":"H.y", "val":4.0509}, "heading":{"exp":"H.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,0.77479],
+  "samples":[
+    {"t":0.0, "x":7.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-9.61317, "ay":-7.09886, "alpha":0.0, "fx":[-160.247,-160.247,-160.247,-160.247], "fy":[-118.33452,-118.33452,-118.33452,-118.33452]},
+    {"t":0.04842, "x":7.08962, "y":5.06736, "heading":3.14159, "vx":-0.46551, "vy":-0.34376, "omega":0.0, "ax":-9.61146, "ay":-7.09759, "alpha":0.0, "fx":[-160.21842,-160.21842,-160.21842,-160.21842], "fy":[-118.31342,-118.31342,-118.31342,-118.31342]},
+    {"t":0.09685, "x":7.0558, "y":5.04239, "heading":3.14159, "vx":-0.93094, "vy":-0.68746, "omega":0.0, "ax":-9.60889, "ay":-7.09569, "alpha":0.0, "fx":[-160.17555,-160.17555,-160.17555,-160.17555], "fy":[-118.28176,-118.28176,-118.28176,-118.28176]},
+    {"t":0.14527, "x":6.99946, "y":5.00078, "heading":3.14159, "vx":-1.39625, "vy":-1.03106, "omega":0.0, "ax":-9.6046, "ay":-7.09253, "alpha":0.0, "fx":[-160.10413,-160.10413,-160.10413,-160.10413], "fy":[-118.22902,-118.22902,-118.22902,-118.22902]},
+    {"t":0.1937, "x":6.92058, "y":4.94253, "heading":3.14159, "vx":-1.86135, "vy":-1.37451, "omega":0.0, "ax":-9.59604, "ay":-7.0862, "alpha":0.0, "fx":[-159.9614,-159.9614,-159.9614,-159.9614], "fy":[-118.12362,-118.12362,-118.12362,-118.12362]},
+    {"t":0.24212, "x":6.8192, "y":4.86767, "heading":3.14159, "vx":-2.32603, "vy":-1.71766, "omega":0.0, "ax":-9.57045, "ay":-7.06731, "alpha":0.0, "fx":[-159.53485,-159.53485,-159.53485,-159.53485], "fy":[-117.80863,-117.80863,-117.80863,-117.80863]},
+    {"t":0.29055, "x":6.69534, "y":4.7762, "heading":3.14159, "vx":-2.78948, "vy":-2.05989, "omega":0.0, "ax":-5.16445, "ay":-3.81369, "alpha":0.0, "fx":[-86.08886,-86.08886,-86.08886,-86.08886], "fy":[-63.57238,-63.57238,-63.57238,-63.57238]},
+    {"t":0.33897, "x":6.55421, "y":4.67198, "heading":3.14159, "vx":-3.03956, "vy":-2.24457, "omega":0.0, "ax":-0.00042, "ay":-0.00031, "alpha":0.0, "fx":[-0.00694,-0.00694,-0.00694,-0.00694], "fy":[-0.00512,-0.00512,-0.00512,-0.00512]},
+    {"t":0.3874, "x":6.40702, "y":4.56329, "heading":3.14159, "vx":-3.03958, "vy":-2.24458, "omega":0.0, "ax":0.00042, "ay":0.00031, "alpha":0.0, "fx":[0.00694,0.00694,0.00694,0.00694], "fy":[0.00512,0.00512,0.00512,0.00512]},
+    {"t":0.43582, "x":6.25983, "y":4.4546, "heading":3.14159, "vx":-3.03956, "vy":-2.24457, "omega":0.0, "ax":5.16445, "ay":3.81369, "alpha":0.0, "fx":[86.08886,86.08886,86.08886,86.08886], "fy":[63.57238,63.57238,63.57238,63.57238]},
+    {"t":0.48425, "x":6.11869, "y":4.35038, "heading":3.14159, "vx":-2.78948, "vy":-2.05989, "omega":0.0, "ax":9.57045, "ay":7.06731, "alpha":0.0, "fx":[159.53485,159.53485,159.53485,159.53485], "fy":[117.80863,117.80863,117.80863,117.80863]},
+    {"t":0.53267, "x":5.99483, "y":4.25891, "heading":3.14159, "vx":-2.32603, "vy":-1.71766, "omega":0.0, "ax":9.59604, "ay":7.0862, "alpha":0.0, "fx":[159.9614,159.9614,159.9614,159.9614], "fy":[118.12362,118.12362,118.12362,118.12362]},
+    {"t":0.58109, "x":5.89345, "y":4.18404, "heading":3.14159, "vx":-1.86135, "vy":-1.37451, "omega":0.0, "ax":9.6046, "ay":7.09253, "alpha":0.0, "fx":[160.10413,160.10413,160.10413,160.10413], "fy":[118.22902,118.22902,118.22902,118.22902]},
+    {"t":0.62952, "x":5.81458, "y":4.1258, "heading":3.14159, "vx":-1.39625, "vy":-1.03106, "omega":0.0, "ax":9.60889, "ay":7.09569, "alpha":0.0, "fx":[160.17555,160.17555,160.17555,160.17555], "fy":[118.28176,118.28176,118.28176,118.28176]},
+    {"t":0.67794, "x":5.75823, "y":4.08419, "heading":3.14159, "vx":-0.93094, "vy":-0.68746, "omega":0.0, "ax":9.61146, "ay":7.09759, "alpha":0.0, "fx":[160.21842,160.21842,160.21842,160.21842], "fy":[118.31342,118.31342,118.31342,118.31342]},
+    {"t":0.72637, "x":5.72442, "y":4.05922, "heading":3.14159, "vx":-0.46551, "vy":-0.34376, "omega":0.0, "ax":9.61317, "ay":7.09886, "alpha":0.0, "fx":[160.247,160.247,160.247,160.247], "fy":[118.33452,118.33452,118.33452,118.33452]},
+    {"t":0.77479, "x":5.71315, "y":4.0509, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+  "splits":[0]
+ },
+ "events":[]
+}
diff --git a/src/main/deploy/choreo/startToI.traj b/src/main/deploy/choreo/startToI.traj
new file mode 100644
index 00000000..06487118
--- /dev/null
+++ b/src/main/deploy/choreo/startToI.traj
@@ -0,0 +1,67 @@
+{
+ "name":"startToI",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":6.0711771011352536, "y":5.008563137054443, "heading":4.1887902047863905, "intervals":10, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":5.365372248154253, "y":4.933261807735684, "heading":4.1887902047863905, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"6.0711771011352536 m", "val":6.0711771011352536}, "y":{"exp":"5.008563137054443 m", "val":5.008563137054443}, "heading":{"exp":"I.heading", "val":4.1887902047863905}, "intervals":10, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"I.x", "val":5.365372248154253}, "y":{"exp":"I.y", "val":4.933261807735684}, "heading":{"exp":"I.heading", "val":4.1887902047863905}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,0.47516,0.82156],
+  "samples":[
+    {"t":0.0, "x":7.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.5334, "ay":-0.23846, "alpha":38.98149, "fx":[-62.46701,-73.25328,-185.8047,-180.78761], "fy":[-188.75032,184.72701,71.60764,-83.48433]},
+    {"t":0.02376, "x":7.09876, "y":5.07561, "heading":3.14159, "vx":-0.17898, "vy":-0.00567, "omega":0.92613, "ax":-7.88082, "ay":-0.27029, "alpha":37.29377, "fx":[-72.37092,-85.05554,-186.57552,-181.47584], "fy":[-185.09051,179.48978,69.4669,-81.88889]},
+    {"t":0.04752, "x":7.09229, "y":5.0754, "heading":-3.11959, "vx":-0.36621, "vy":-0.01209, "omega":1.81216, "ax":-8.32246, "ay":-0.39854, "alpha":35.01714, "fx":[-83.64137,-101.51012,-188.25455,-181.51979], "fy":[-180.13949,170.60558,64.62911,-81.66901]},
+    {"t":0.07127, "x":7.08124, "y":5.075, "heading":-3.07654, "vx":-0.56394, "vy":-0.02156, "omega":2.6441, "ax":-8.89692, "ay":-0.60412, "alpha":31.78712, "fx":[-99.42841,-121.9092,-190.64063,-181.25153], "fy":[-171.71104,156.5409,56.9755,-82.08671]},
+    {"t":0.09503, "x":7.06533, "y":5.07432, "heading":-3.01372, "vx":-0.77531, "vy":-0.03591, "omega":3.3993, "ax":-9.66225, "ay":-0.82759, "alpha":26.91595, "fx":[-124.10144,-145.42941,-193.42929,-181.30024], "fy":[-154.46688,134.78001,46.21018,-81.70573]},
+    {"t":0.11879, "x":7.04418, "y":5.07323, "heading":-2.93296, "vx":-1.00487, "vy":-0.05557, "omega":4.03877, "ax":-10.62855, "ay":-0.94402, "alpha":19.50463, "fx":[-158.83904,-170.63889,-196.18095,-183.03267], "fy":[-117.90678,100.65138,31.56396,-77.25423]},
+    {"t":0.14255, "x":7.01731, "y":5.07165, "heading":-2.837, "vx":-1.25738, "vy":-0.078, "omega":4.50216, "ax":-11.52988, "ay":-0.9297, "alpha":9.51428, "fx":[-188.76185,-192.64941,-198.08329,-189.29555], "fy":[-58.59084,45.22986,10.40609,-59.03541]},
+    {"t":0.16631, "x":6.98418, "y":5.06953, "heading":-2.73004, "vx":-1.53131, "vy":-0.10009, "omega":4.7282, "ax":-11.73549, "ay":-1.01011, "alpha":-4.47617, "fx":[-197.69514,-193.16332,-194.58547,-197.05602], "fy":[-5.11766,-41.95634,-32.31004,12.03209]},
+    {"t":0.19006, "x":6.94449, "y":5.06687, "heading":-2.61771, "vx":-1.81012, "vy":-0.12408, "omega":4.62186, "ax":-9.62842, "ay":-1.662, "alpha":-24.50404, "fx":[-197.32777,-163.16014,-131.02215,-150.49426], "fy":[14.41945,-111.36131,-140.74016,126.86305]},
+    {"t":0.21382, "x":6.89876, "y":5.06345, "heading":-2.5079, "vx":-2.03888, "vy":-0.16357, "omega":4.03969, "ax":-9.59433, "ay":-1.49098, "alpha":-23.78566, "fx":[-197.38964,-157.14643,-139.74997,-145.44557], "fy":[10.54069,-119.08733,-123.35908,132.49014]},
+    {"t":0.23758, "x":6.84762, "y":5.05914, "heading":-2.41193, "vx":-2.26682, "vy":-0.19899, "omega":3.47459, "ax":-9.58912, "ay":-1.02806, "alpha":-22.87308, "fx":[-197.36323,-149.19034,-149.69022,-143.14011], "fy":[4.76397,-128.18825,-79.79664,134.67157]},
+    {"t":0.26134, "x":6.79105, "y":5.05412, "heading":-2.32938, "vx":-2.49464, "vy":-0.22342, "omega":2.93117, "ax":-9.65532, "ay":0.46747, "alpha":-22.14552, "fx":[-197.0163,-143.96034,-155.56061,-147.2608], "fy":[-2.56185,-133.05238,37.26402,129.51989]},
+    {"t":0.2851, "x":6.72906, "y":5.04895, "heading":-2.25974, "vx":-2.72403, "vy":-0.21231, "omega":2.40503, "ax":-9.71433, "ay":0.70571, "alpha":-21.90802, "fx":[-196.16542,-142.66212,-154.18881,-154.71674], "fy":[-9.84688,-132.97849,70.57087,119.31015]},
+    {"t":0.30886, "x":6.6616, "y":5.0441, "heading":-2.2026, "vx":-2.95483, "vy":-0.19555, "omega":1.88454, "ax":-9.67806, "ay":0.52448, "alpha":-21.8629, "fx":[-194.68838,-139.95685,-150.97713,-159.69179], "fy":[-16.56328,-133.52323,74.70833,110.34961]},
+    {"t":0.33261, "x":6.58867, "y":5.03961, "heading":-2.15782, "vx":-3.18476, "vy":-0.18308, "omega":1.36512, "ax":-9.52318, "ay":0.19888, "alpha":-21.77589, "fx":[-192.07563,-135.70289,-144.90832,-162.30044], "fy":[-22.57623,-133.5622,67.6228,101.77644]},
+    {"t":0.35637, "x":6.51032, "y":5.03531, "heading":-2.12539, "vx":-3.41101, "vy":-0.17836, "omega":0.84776, "ax":-9.08857, "ay":-0.31495, "alpha":-21.11162, "fx":[-185.81186,-129.20731,-129.98971,-160.99958], "fy":[-27.43742,-128.50162,45.00174,89.93698]},
+    {"t":0.38013, "x":6.42671, "y":5.03099, "heading":-2.10525, "vx":-3.62694, "vy":-0.18584, "omega":0.34619, "ax":-5.43121, "ay":-1.13341, "alpha":-12.84248, "fx":[-122.79783,-80.19154,-54.30482,-104.8487], "fy":[-27.74734,-71.22489,-8.18593,31.58436]},
+    {"t":0.40389, "x":6.33901, "y":5.02625, "heading":-2.09703, "vx":-3.75597, "vy":-0.21277, "omega":0.04108, "ax":-0.07135, "ay":-0.60925, "alpha":-0.25185, "fx":[-2.02961,-0.96608,-0.34881,-1.4128], "fy":[-10.37729,-10.99215,-9.93488,-9.3194]},
+    {"t":0.42765, "x":6.24976, "y":5.02102, "heading":-2.09605, "vx":-3.75767, "vy":-0.22725, "omega":0.03509, "ax":0.0543, "ay":-0.99371, "alpha":-0.02258, "fx":[0.82952,0.9253,0.98091,0.88513], "fy":[-16.58463,-16.63932,-16.54477,-16.49006]},
+    {"t":0.4514, "x":6.1605, "y":5.01534, "heading":-2.09522, "vx":-3.75638, "vy":-0.25085, "omega":0.03456, "ax":-0.26789, "ay":-2.91092, "alpha":-1.45404, "fx":[-9.55406,-3.06378,0.66154,-5.90576], "fy":[-49.70823,-53.07084,-47.38157,-43.93406]},
+    {"t":0.47516, "x":6.07118, "y":5.00856, "heading":-2.0944, "vx":-3.76274, "vy":-0.32001, "omega":0.00001, "ax":1.97945, "ay":-2.31211, "alpha":-0.00039, "fx":[32.99508,32.99669,32.99776,32.99615], "fy":[-38.54225,-38.54309,-38.54139,-38.54055]},
+    {"t":0.5098, "x":5.94202, "y":4.99609, "heading":-2.09439, "vx":-3.69418, "vy":-0.4001, "omega":0.0, "ax":11.75911, "ay":1.23153, "alpha":-0.00001, "fx":[196.01869,196.0187,196.01869,196.01868], "fy":[20.52901,20.52893,20.52904,20.52911]},
+    {"t":0.54444, "x":5.82111, "y":4.98297, "heading":-2.09439, "vx":-3.28684, "vy":-0.35744, "omega":0.0, "ax":11.82592, "ay":1.26986, "alpha":0.0, "fx":[197.13237,197.13237,197.13237,197.13237], "fy":[21.16789,21.16789,21.16789,21.16789]},
+    {"t":0.57908, "x":5.71435, "y":4.97135, "heading":-2.09439, "vx":-2.87719, "vy":-0.31346, "omega":0.0, "ax":11.84747, "ay":1.28225, "alpha":0.0, "fx":[197.49167,197.49167,197.49167,197.49167], "fy":[21.37442,21.37444,21.37441,21.37439]},
+    {"t":0.61372, "x":5.62179, "y":4.96126, "heading":-2.09439, "vx":-2.46679, "vy":-0.26904, "omega":0.0, "ax":11.8581, "ay":1.28836, "alpha":0.00001, "fx":[197.6688,197.66879,197.6688,197.6688], "fy":[21.47628,21.47632,21.47627,21.47623]},
+    {"t":0.64836, "x":5.54346, "y":4.95271, "heading":-2.09439, "vx":-2.05603, "vy":-0.22441, "omega":0.0, "ax":11.86442, "ay":1.29199, "alpha":0.00001, "fx":[197.77421,197.77421,197.77422,197.77422], "fy":[21.53691,21.53696,21.5369,21.53685]},
+    {"t":0.683, "x":5.47935, "y":4.94572, "heading":-2.09439, "vx":-1.64504, "vy":-0.17965, "omega":0.0, "ax":11.86862, "ay":1.29441, "alpha":0.00001, "fx":[197.84412,197.84411,197.84412,197.84413], "fy":[21.57712,21.57717,21.5771,21.57706]},
+    {"t":0.71764, "x":5.42949, "y":4.94027, "heading":-2.0944, "vx":-1.23391, "vy":-0.13482, "omega":0.0, "ax":11.8716, "ay":1.29612, "alpha":0.00001, "fx":[197.89387,197.89386,197.89387,197.89387], "fy":[21.60574,21.60579,21.60572,21.60566]},
+    {"t":0.75228, "x":5.39387, "y":4.93638, "heading":-2.0944, "vx":-0.82268, "vy":-0.08992, "omega":0.0, "ax":11.87383, "ay":1.29741, "alpha":0.00001, "fx":[197.93107,197.93106,197.93107,197.93108], "fy":[21.62713,21.62719,21.62711,21.62706]},
+    {"t":0.78692, "x":5.3725, "y":4.93404, "heading":-2.0944, "vx":-0.41137, "vy":-0.04498, "omega":0.0, "ax":11.87556, "ay":1.2984, "alpha":0.00001, "fx":[197.95994,197.95994,197.95994,197.95995], "fy":[21.64374,21.6438,21.64372,21.64366]},
+    {"t":0.82156, "x":5.36537, "y":4.93326, "heading":-2.0944, "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/startToJ.traj b/src/main/deploy/choreo/startToJ.traj
new file mode 100644
index 00000000..3ea7f607
--- /dev/null
+++ b/src/main/deploy/choreo/startToJ.traj
@@ -0,0 +1,69 @@
+{
+ "name":"startToJ",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":6.123365116119384, "y":5.0878586769104, "heading":4.1887902047863905, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":5.079583864905389, "y":5.098261807735684, "heading":4.1887902047863905, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"6.123365116119384 m", "val":6.123365116119384}, "y":{"exp":"5.0878586769104 m", "val":5.0878586769104}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"J.x", "val":5.079583864905389}, "y":{"exp":"J.y", "val":5.098261807735684}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,0.46084,0.89582],
+  "samples":[
+    {"t":0.0, "x":7.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.51509, "ay":0.05345, "alpha":39.09871, "fx":[-68.41964,-66.01077,-182.7646,-183.89644], "fy":[-186.59936,187.48875,79.04036,-76.36584]},
+    {"t":0.02304, "x":7.09889, "y":5.07569, "heading":3.14159, "vx":-0.17316, "vy":0.00123, "omega":0.90091, "ax":-7.84186, "ay":0.06014, "alpha":37.5235, "fx":[-78.82117,-76.00759,-183.45077,-184.60077], "fy":[-182.3517,183.57027,77.3416,-74.55017]},
+    {"t":0.04608, "x":7.09282, "y":5.07574, "heading":-3.12083, "vx":-0.35385, "vy":0.00262, "omega":1.76552, "ax":-8.26524, "ay":-0.00627, "alpha":35.36671, "fx":[-90.85741,-90.38966,-185.08143,-184.78212], "fy":[-176.50893,176.82888,73.22762,-73.96589]},
+    {"t":0.06913, "x":7.08247, "y":5.0758, "heading":-3.08015, "vx":-0.5443, "vy":0.00247, "omega":2.58044, "ax":-8.82806, "ay":-0.1171, "alpha":32.25499, "fx":[-107.61098,-108.80473,-187.48477,-184.73749], "fy":[-166.57936,166.01105,66.64664,-73.88645]},
+    {"t":0.09217, "x":7.06759, "y":5.07582, "heading":-3.0207, "vx":-0.74771, "vy":-0.00023, "omega":3.32365, "ax":-9.58658, "ay":-0.19757, "alpha":27.53522, "fx":[-132.93094,-130.87756,-190.40383,-185.00268], "fy":[-146.77285,149.06395,57.46264,-72.92702]},
+    {"t":0.11521, "x":7.04781, "y":5.07576, "heading":-2.94411, "vx":-0.96861, "vy":-0.00478, "omega":3.95811, "ax":-10.5352, "ay":-0.1119, "alpha":20.50071, "fx":[-166.2782,-156.03799,-193.49588,-186.65458], "fy":[-106.93508,122.2119,45.33399,-68.07203]},
+    {"t":0.13825, "x":7.0227, "y":5.07562, "heading":-2.85291, "vx":-1.21136, "vy":-0.00736, "omega":4.43049, "ax":-11.42662, "ay":0.13268, "alpha":11.46708, "fx":[-191.91514,-182.06091,-196.29682,-191.63242], "fy":[-46.84749,77.75214,29.12395,-51.18202]},
+    {"t":0.16129, "x":6.99175, "y":5.07549, "heading":-2.75082, "vx":-1.47465, "vy":-0.0043, "omega":4.69471, "ax":-11.85585, "ay":0.22487, "alpha":-0.28637, "fx":[-197.63697,-197.66849,-197.63281,-197.58702], "fy":[4.45972,1.9362,3.02427,5.57371]},
+    {"t":0.18434, "x":6.95463, "y":5.07545, "heading":-2.64265, "vx":-1.74783, "vy":0.00088, "omega":4.68811, "ax":-10.00815, "ay":-0.66064, "alpha":-22.3902, "fx":[-196.01254,-171.07308,-157.89512,-142.34361], "fy":[27.20374,-98.75319,-108.68827,136.18756]},
+    {"t":0.20738, "x":6.9117, "y":5.0753, "heading":-2.53462, "vx":-1.97844, "vy":-0.01434, "omega":4.1722, "ax":-9.67314, "ay":-0.67124, "alpha":-23.41645, "fx":[-196.44506,-162.34469,-152.52245,-133.67403], "fy":[22.63104,-111.95319,-100.05047,144.61549]},
+    {"t":0.23042, "x":6.86354, "y":5.07479, "heading":-2.43849, "vx":-2.20132, "vy":-0.02981, "omega":3.63264, "ax":-9.61316, "ay":0.5258, "alpha":-22.46424, "fx":[-196.91663,-154.33552,-156.33704,-133.39809], "fy":[15.47716,-122.06326,-2.97907,144.62452]},
+    {"t":0.25346, "x":6.81027, "y":5.07424, "heading":-2.35479, "vx":-2.42283, "vy":-0.01769, "omega":3.11502, "ax":-9.67218, "ay":1.55333, "alpha":-21.95795, "fx":[-197.02815,-153.21416,-151.57449,-143.10556], "fy":[7.37599,-122.4732,84.19985,134.47061]},
+    {"t":0.2765, "x":6.75187, "y":5.07424, "heading":-2.28301, "vx":-2.6457, "vy":0.0181, "omega":2.60907, "ax":-9.73071, "ay":1.461, "alpha":-21.84654, "fx":[-196.65043,-151.77656,-149.41594,-150.98202], "fy":[-0.32961,-122.89238,96.00645,124.63251]},
+    {"t":0.29954, "x":6.68833, "y":5.07505, "heading":-2.22289, "vx":-2.86991, "vy":0.05176, "omega":2.10569, "ax":-9.71264, "ay":1.20976, "alpha":-21.9311, "fx":[-195.71112,-148.55619,-146.99996,-156.35312], "fy":[-7.70109,-124.8015,96.93073,116.23649]},
+    {"t":0.32259, "x":6.61962, "y":5.07656, "heading":-2.17437, "vx":-3.09371, "vy":0.07964, "omega":1.60035, "ax":-9.60869, "ay":0.86987, "alpha":-22.04914, "fx":[-193.94053,-143.61663,-143.35508,-159.77651], "fy":[-14.72775,-127.26189,91.50163,108.48937]},
+    {"t":0.34563, "x":6.54579, "y":5.07863, "heading":-2.1375, "vx":-3.31511, "vy":0.09968, "omega":1.0923, "ax":-9.36436, "ay":0.39332, "alpha":-21.90345, "fx":[-190.38478,-136.77,-136.23031,-161.01272], "fy":[-21.28764,-128.19179,75.94731,99.75793]},
+    {"t":0.36867, "x":6.46691, "y":5.08103, "heading":-2.11233, "vx":-3.53088, "vy":0.10875, "omega":0.5876, "ax":-8.60632, "ay":-0.45222, "alpha":-20.41751, "fx":[-179.14628,-124.59096,-114.25799,-155.85777], "fy":[-27.46092,-119.56869,34.74466,82.13152]},
+    {"t":0.39171, "x":6.38327, "y":5.08342, "heading":-2.09879, "vx":-3.72919, "vy":0.09833, "omega":0.11714, "ax":-1.47131, "ay":-1.37813, "alpha":-3.45319, "fx":[-35.80766,-21.29191,-13.01604,-27.98837], "fy":[-25.71332,-34.54489,-20.3002,-11.3328]},
+    {"t":0.41475, "x":6.29695, "y":5.08532, "heading":-2.09609, "vx":-3.76309, "vy":0.06657, "omega":0.03758, "ax":-0.03625, "ay":-0.52795, "alpha":-0.06757, "fx":[-0.82953,-0.54423,-0.37894,-0.66427], "fy":[-8.86035,-9.02511,-8.74101,-8.57621]},
+    {"t":0.4378, "x":6.21024, "y":5.08671, "heading":-2.09523, "vx":-3.76392, "vy":0.05441, "omega":0.03602, "ax":-0.53687, "ay":-0.39517, "alpha":-1.56275, "fx":[-14.13232,-7.55458,-3.75042,-10.3602], "fy":[-7.96283,-11.79512,-5.21378,-1.37766]},
+    {"t":0.46084, "x":6.12337, "y":5.08786, "heading":-2.0944, "vx":-3.77629, "vy":0.0453, "omega":0.00001, "ax":-0.02871, "ay":-0.20163, "alpha":-0.0003, "fx":[-0.47958,-0.47831,-0.47758,-0.47885], "fy":[-3.36133,-3.36206,-3.36079,-3.36006]},
+    {"t":0.49709, "x":5.98646, "y":5.08937, "heading":-2.09439, "vx":-3.77734, "vy":0.03799, "omega":0.0, "ax":-0.00026, "ay":-0.02536, "alpha":0.0, "fx":[-0.00427,-0.00427,-0.00427,-0.00427], "fy":[-0.42269,-0.42269,-0.42269,-0.42269]},
+    {"t":0.53333, "x":5.84954, "y":5.09073, "heading":-2.09439, "vx":-3.77734, "vy":0.03707, "omega":0.0, "ax":0.00351, "ay":-0.00326, "alpha":0.0, "fx":[0.05848,0.05848,0.05848,0.05848], "fy":[-0.05433,-0.05433,-0.05433,-0.05433]},
+    {"t":0.56958, "x":5.71262, "y":5.09207, "heading":-2.09439, "vx":-3.77722, "vy":0.03695, "omega":0.0, "ax":8.82385, "ay":-0.08666, "alpha":-0.00006, "fx":[147.08923,147.08935,147.08942,147.0893], "fy":[-1.44472,-1.44496,-1.44454,-1.44431]},
+    {"t":0.60583, "x":5.58149, "y":5.09335, "heading":-2.09439, "vx":-3.45737, "vy":0.03381, "omega":0.0, "ax":11.85014, "ay":-0.11591, "alpha":0.00001, "fx":[197.53615,197.53615,197.53614,197.53614], "fy":[-1.93208,-1.93205,-1.9321,-1.93213]},
+    {"t":0.64208, "x":5.46396, "y":5.0945, "heading":-2.09439, "vx":-3.02781, "vy":0.02961, "omega":0.0, "ax":11.90444, "ay":-0.11643, "alpha":0.00001, "fx":[198.44122,198.44122,198.44122,198.44122], "fy":[-1.94081,-1.94077,-1.94083,-1.94087]},
+    {"t":0.67833, "x":5.36202, "y":5.0955, "heading":-2.09439, "vx":-2.5963, "vy":0.02539, "omega":0.0, "ax":11.9229, "ay":-0.11661, "alpha":0.00001, "fx":[198.74895,198.74895,198.74895,198.74895], "fy":[-1.94377,-1.94374,-1.9438,-1.94384]},
+    {"t":0.71458, "x":5.27574, "y":5.09634, "heading":-2.09439, "vx":-2.16411, "vy":0.02116, "omega":0.0, "ax":11.93219, "ay":-0.1167, "alpha":0.00001, "fx":[198.90384,198.90384,198.90384,198.90384], "fy":[-1.94527,-1.94523,-1.9453,-1.94534]},
+    {"t":0.75083, "x":5.20514, "y":5.09703, "heading":-2.09439, "vx":-1.73158, "vy":0.01693, "omega":0.0, "ax":11.93778, "ay":-0.11675, "alpha":0.00001, "fx":[198.99708,198.99708,198.99708,198.99708], "fy":[-1.94617,-1.94613,-1.9462,-1.94624]},
+    {"t":0.78707, "x":5.15021, "y":5.09757, "heading":-2.0944, "vx":-1.29886, "vy":0.0127, "omega":0.0, "ax":11.94152, "ay":-0.11679, "alpha":0.00001, "fx":[199.05936,199.05936,199.05936,199.05936], "fy":[-1.94677,-1.94673,-1.9468,-1.94684]},
+    {"t":0.82332, "x":5.11098, "y":5.09795, "heading":-2.0944, "vx":-0.86599, "vy":0.00847, "omega":0.0, "ax":11.94419, "ay":-0.11681, "alpha":0.00001, "fx":[199.1039,199.1039,199.1039,199.1039], "fy":[-1.9472,-1.94715,-1.94723,-1.94727]},
+    {"t":0.85957, "x":5.08743, "y":5.09819, "heading":-2.0944, "vx":-0.43303, "vy":0.00423, "omega":0.0, "ax":11.9462, "ay":-0.11683, "alpha":0.00001, "fx":[199.13733,199.13733,199.13733,199.13733], "fy":[-1.94752,-1.94748,-1.94755,-1.94759]},
+    {"t":0.89582, "x":5.07958, "y":5.09826, "heading":-2.0944, "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/startToK.traj b/src/main/deploy/choreo/startToK.traj
new file mode 100644
index 00000000..fd43a2c8
--- /dev/null
+++ b/src/main/deploy/choreo/startToK.traj
@@ -0,0 +1,84 @@
+{
+ "name":"startToK",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":4.455554008483887, "y":5.959114074707031, "heading":-1.325818250323842, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":4.141549248154255, "y":5.238261807735684, "heading":5.235987755982989, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"4.455554008483887 m", "val":4.455554008483887}, "y":{"exp":"5.959114074707031 m", "val":5.959114074707031}, "heading":{"exp":"-1.325818250323842 rad", "val":-1.325818250323842}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"K.x", "val":4.141549248154255}, "y":{"exp":"K.y", "val":5.238261807735684}, "heading":{"exp":"K.heading", "val":5.235987755982989}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,1.0504,1.44926],
+  "samples":[
+    {"t":0.0, "x":7.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.86629, "ay":1.39146, "alpha":44.45758, "fx":[-31.87881,-7.19172,-155.9394,-196.1433], "fy":[-195.58974,198.97892,123.96842,-34.57787]},
+    {"t":0.03282, "x":7.09773, "y":5.07643, "heading":3.14159, "vx":-0.19256, "vy":0.04567, "omega":1.45931, "ax":-6.53541, "ay":1.62439, "alpha":41.41147, "fx":[-69.21768,-13.79279,-155.78571,-196.97244], "fy":[-185.20002,198.5714,124.10152,-29.16179]},
+    {"t":0.06565, "x":7.08789, "y":5.0788, "heading":-3.09369, "vx":-0.40708, "vy":0.09899, "omega":2.81864, "ax":-7.93554, "ay":2.37656, "alpha":33.32614, "fx":[-143.06971,-29.98897,-158.51402,-197.55375], "fy":[-134.49679,196.69599,120.49904,-24.23346]},
+    {"t":0.09847, "x":7.07025, "y":5.08333, "heading":-3.00117, "vx":-0.66757, "vy":0.17701, "omega":3.91257, "ax":-9.01388, "ay":5.41878, "alpha":18.7973, "fx":[-183.56978,-56.20484,-162.8063,-198.44755], "fy":[69.07975,190.73029,114.445,-12.94108]},
+    {"t":0.1313, "x":7.04348, "y":5.09206, "heading":-2.87274, "vx":-0.96345, "vy":0.35488, "omega":4.52959, "ax":-9.08258, "ay":6.68279, "alpha":12.96599, "fx":[-147.09074,-97.19322,-165.1023,-196.22258], "fy":[131.87489,173.15285,110.63795,29.92978]},
+    {"t":0.16412, "x":7.00696, "y":5.10731, "heading":-2.72406, "vx":-1.26158, "vy":0.57424, "omega":4.95519, "ax":-8.79865, "ay":7.71345, "alpha":-6.81199, "fx":[-157.36724,-175.07231,-134.34651,-119.89113], "fy":[120.32069,91.88087,144.59749,157.51921]},
+    {"t":0.19695, "x":6.96081, "y":5.13032, "heading":-2.5614, "vx":-1.5504, "vy":0.82743, "omega":4.73159, "ax":-7.83505, "ay":7.24662, "alpha":-18.65473, "fx":[-170.35289,-195.60644,-65.11047,-91.35602], "fy":[100.81855,22.47234,184.46455,175.43507]},
+    {"t":0.22977, "x":6.9057, "y":5.16138, "heading":-2.40609, "vx":-1.80758, "vy":1.0653, "omega":4.11925, "ax":-8.50982, "ay":6.32065, "alpha":-19.04918, "fx":[-180.97427,-196.04597,-78.93588,-111.46252], "fy":[79.13072,1.44587,178.00423,162.86809]},
+    {"t":0.2626, "x":6.84178, "y":5.19975, "heading":-2.27088, "vx":-2.08691, "vy":1.27277, "omega":3.49396, "ax":-9.21507, "ay":4.63545, "alpha":-20.70408, "fx":[-191.46788,-189.56842,-99.94318,-133.46337], "fy":[45.67302,-45.09189,164.19781,144.30404]},
+    {"t":0.29542, "x":6.76831, "y":5.24403, "heading":-2.15619, "vx":-2.3894, "vy":1.42493, "omega":2.81435, "ax":-9.72284, "ay":1.36486, "alpha":-22.30126, "fx":[-195.52187,-150.56762,-141.92653,-160.28409], "fy":[-10.78497,-121.89816,112.71937,110.96974]},
+    {"t":0.32825, "x":6.68464, "y":5.29154, "heading":-2.06381, "vx":-2.70855, "vy":1.46973, "omega":2.08232, "ax":-8.83467, "ay":2.04797, "alpha":-26.25103, "fx":[-192.695,-147.55374,-94.47901,-154.35125], "fy":[-8.62906,-116.35321,147.48351,114.05317]},
+    {"t":0.36107, "x":6.59097, "y":5.34089, "heading":-1.99545, "vx":-2.99855, "vy":1.53696, "omega":1.22063, "ax":-7.67277, "ay":3.64483, "alpha":-25.26723, "fx":[-182.26923,-151.46338,-46.29581,-131.57713], "fy":[19.30644,-63.05664,158.97239,127.80788]},
+    {"t":0.3939, "x":6.48841, "y":5.3933, "heading":-1.95539, "vx":-3.2504, "vy":1.6566, "omega":0.39124, "ax":-2.2739, "ay":2.76327, "alpha":-9.78345, "fx":[-70.36097,-27.34281,-5.27046,-48.64501], "fy":[31.95606,15.05909,63.12142,74.11267]},
+    {"t":0.42672, "x":6.3805, "y":5.44917, "heading":-1.94254, "vx":-3.32504, "vy":1.7473, "omega":0.0701, "ax":0.07387, "ay":0.28408, "alpha":-0.20464, "fx":[0.58567,1.515,1.87687,0.94775], "fy":[4.45262,4.09042,5.01851,5.3806]},
+    {"t":0.45955, "x":6.27139, "y":5.50667, "heading":-1.94024, "vx":-3.32262, "vy":1.75663, "omega":0.06338, "ax":0.01823, "ay":0.04273, "alpha":-0.01471, "fx":[0.25753,0.32436,0.35023,0.28341], "fy":[0.69181,0.66594,0.73276,0.75864]},
+    {"t":0.49237, "x":6.16234, "y":5.56436, "heading":-1.93816, "vx":-3.32202, "vy":1.75803, "omega":0.0629, "ax":0.00181, "ay":0.00767, "alpha":-0.00867, "fx":[0.00297,0.04236,0.05752,0.01813], "fy":[0.11578,0.10062,0.14,0.15516]},
+    {"t":0.5252, "x":6.05329, "y":5.62207, "heading":-1.9361, "vx":-3.32196, "vy":1.75828, "omega":0.06261, "ax":-0.00101, "ay":0.00185, "alpha":-0.00789, "fx":[-0.04166,-0.00578,0.00794,-0.02794], "fy":[0.01975,0.00603,0.04191,0.05563]},
+    {"t":0.55802, "x":5.94425, "y":5.67979, "heading":-1.93404, "vx":-3.32199, "vy":1.75834, "omega":0.06235, "ax":-0.00229, "ay":-0.00081, "alpha":-0.00748, "fx":[-0.06163,-0.02757,-0.01462,-0.04868], "fy":[-0.02399,-0.03694,-0.00288,0.01007]},
+    {"t":0.59085, "x":5.8352, "y":5.7375, "heading":-1.932, "vx":-3.32207, "vy":1.75832, "omega":0.06211, "ax":-0.00783, "ay":-0.01208, "alpha":-0.00622, "fx":[-0.15011,-0.12178,-0.11107,-0.13941], "fy":[-0.21018,-0.22088,-0.19255,-0.18184]},
+    {"t":0.62367, "x":5.72615, "y":5.79521, "heading":-1.92996, "vx":-3.32233, "vy":1.75792, "omega":0.06191, "ax":-0.04215, "ay":-0.08182, "alpha":0.00129, "fx":[-0.69853,-0.7044,-0.7066,-0.70073], "fy":[-1.36215,-1.35995,-1.36581,-1.36801]},
+    {"t":0.6565, "x":5.61708, "y":5.85287, "heading":-1.92793, "vx":-3.32371, "vy":1.75523, "omega":0.06195, "ax":-0.24545, "ay":-0.52161, "alpha":0.07742, "fx":[-3.84877,-4.20283,-4.33417,-3.98018], "fy":[-8.58486,-8.4526,-8.80511,-8.93736]},
+    {"t":0.68932, "x":5.50784, "y":5.91021, "heading":-1.92589, "vx":-3.33177, "vy":1.73811, "omega":0.06449, "ax":-0.78666, "ay":-3.28516, "alpha":2.33627, "fx":[-5.31255,-17.06323,-20.73466,-9.34231], "fy":[-51.94817,-47.80924,-57.65343,-61.63723]},
+    {"t":0.72215, "x":5.39805, "y":5.96549, "heading":-1.92378, "vx":-3.35759, "vy":1.63028, "omega":0.14118, "ax":2.75987, "ay":-8.00364, "alpha":17.68871, "fx":[132.69168,20.48512,-33.4244,64.27072], "fy":[-97.23321,-110.70199,-163.61092,-162.12137]},
+    {"t":0.75497, "x":5.28933, "y":6.01469, "heading":-1.91914, "vx":-3.267, "vy":1.36756, "omega":0.72181, "ax":3.5832, "ay":-8.61755, "alpha":19.83604, "fx":[161.0045,42.47074,-41.40998,76.85561], "fy":[-90.31281,-131.44015,-180.33266,-172.51591]},
+    {"t":0.7878, "x":5.18402, "y":6.05494, "heading":-1.89545, "vx":-3.14938, "vy":1.08469, "omega":1.37292, "ax":3.13453, "ay":-9.98717, "alpha":15.46977, "fx":[146.26212,24.03809,-31.49297,70.19708], "fy":[-121.63444,-175.82861,-188.64089,-179.82118]},
+    {"t":0.82062, "x":5.08233, "y":6.08516, "heading":-1.85038, "vx":-3.04649, "vy":0.75686, "omega":1.88071, "ax":1.8874, "ay":-11.31566, "alpha":8.54037, "fx":[87.85631,6.3539,-15.7869,47.42458], "fy":[-174.47526,-194.06509,-195.3757,-190.59017]},
+    {"t":0.85345, "x":4.98335, "y":6.10391, "heading":-1.78865, "vx":-2.98453, "vy":0.38543, "omega":2.16105, "ax":2.14018, "ay":-11.53476, "alpha":5.39405, "fx":[70.7087,19.66163,4.77809,47.55499], "fy":[-184.05514,-195.88011,-197.39049,-191.78957]},
+    {"t":0.88627, "x":4.88653, "y":6.11035, "heading":-1.71771, "vx":-2.91428, "vy":0.0068, "omega":2.33811, "ax":2.72288, "ay":-11.53075, "alpha":2.98084, "fx":[64.16033,36.80184,27.92301,52.67117], "fy":[-187.29128,-194.4352,-196.10673,-191.01516]},
+    {"t":0.9191, "x":4.79234, "y":6.10436, "heading":-1.64096, "vx":-2.8249, "vy":-0.3717, "omega":2.43596, "ax":3.38256, "ay":-11.41022, "alpha":0.74089, "fx":[60.8537,54.32095,51.99899,58.36871], "fy":[-188.84457,-190.81295,-191.49317,-189.66082]},
+    {"t":0.95192, "x":4.70143, "y":6.08601, "heading":-1.561, "vx":-2.71387, "vy":-0.74623, "omega":2.46028, "ax":4.03737, "ay":-11.20434, "alpha":-1.445, "fx":[59.07138,71.21501,75.84151,63.07612], "fy":[-189.68161,-185.48064,-183.58672,-188.33475]},
+    {"t":0.98475, "x":4.61453, "y":6.05548, "heading":-1.48025, "vx":-2.58135, "vy":-1.11402, "omega":2.41284, "ax":4.66342, "ay":-10.92385, "alpha":-3.68928, "fx":[58.22862,87.45538,99.25116,66.01253], "fy":[-190.11946,-178.59838,-172.21805,-187.44551]},
+    {"t":1.01757, "x":4.53231, "y":6.01303, "heading":-1.40104, "vx":-2.42827, "vy":-1.47259, "omega":2.29174, "ax":5.48423, "ay":-10.35021, "alpha":-7.87215, "fx":[54.88429,109.91174,135.92241,64.95927], "fy":[-191.24381,-165.88881,-145.15229,-187.8471]},
+    {"t":1.0504, "x":4.45555, "y":5.95911, "heading":-1.32582, "vx":-2.24825, "vy":-1.81233, "omega":2.03334, "ax":6.36759, "ay":-9.87174, "alpha":-7.09366, "fx":[73.88508,122.11045,144.14765,84.43569], "fy":[-184.62076,-156.99966,-136.8145,-179.79375]},
+    {"t":1.07699, "x":4.39802, "y":5.90743, "heading":-1.27175, "vx":-2.07893, "vy":-2.07483, "omega":1.84471, "ax":7.22974, "ay":-9.20487, "alpha":-7.75367, "fx":[86.8659,135.99852,159.40302,99.79771], "fy":[-178.7597,-145.01015,-118.4688,-171.52468]},
+    {"t":1.10358, "x":4.3453, "y":5.84901, "heading":-1.2227, "vx":-1.88668, "vy":-2.3196, "omega":1.63853, "ax":8.41854, "ay":-8.04224, "alpha":-8.60241, "fx":[105.14054,153.66043,178.01045,124.52065], "fy":[-168.47194,-125.95178,-87.68901,-154.1286]},
+    {"t":1.13017, "x":4.2981, "y":5.78448, "heading":-1.17913, "vx":-1.66283, "vy":-2.53345, "omega":1.40979, "ax":9.97042, "ay":-5.85868, "alpha":-9.48117, "fx":[132.02858,174.73517,194.77441,163.27051], "fy":[-148.0148,-94.24029,-36.97828,-111.41195]},
+    {"t":1.15676, "x":4.25741, "y":5.71504, "heading":-1.14164, "vx":-1.3977, "vy":-2.68924, "omega":1.15767, "ax":11.33457, "ay":-1.78472, "alpha":-10.51358, "fx":[169.72208,193.83062,195.23461,196.98022], "fy":[-101.92854,-42.14438,33.84761,-8.77663]},
+    {"t":1.18335, "x":4.22425, "y":5.6429, "heading":-1.11085, "vx":-1.0963, "vy":-2.7367, "omega":0.87811, "ax":10.88301, "ay":3.60988, "alpha":-10.89736, "fx":[197.59417,195.87734,169.44636,162.74024], "fy":[-4.57685,30.64044,102.8347,111.80187]},
+    {"t":1.20994, "x":4.19895, "y":5.57141, "heading":-1.08751, "vx":-0.80691, "vy":-2.64071, "omega":0.58833, "ax":8.66549, "ay":7.81879, "alpha":-7.82967, "fx":[167.11713,170.25003,131.86014,108.57074], "fy":[105.83856,101.70261,148.21581,165.58505]},
+    {"t":1.23653, "x":4.18056, "y":5.50395, "heading":-1.07186, "vx":-0.57649, "vy":-2.4328, "omega":0.38014, "ax":6.29656, "ay":9.98797, "alpha":-5.17122, "fx":[115.36193,131.67165,98.75315,74.05552], "fy":[161.18279,148.50176,172.25589,184.0381]},
+    {"t":1.26312, "x":4.16745, "y":5.44279, "heading":-1.06175, "vx":-0.40906, "vy":-2.16721, "omega":0.24263, "ax":4.55067, "ay":10.96435, "alpha":-3.48718, "fx":[79.02991,97.23224,73.83764,53.33041], "fy":[182.09836,173.20333,184.4666,191.3137]},
+    {"t":1.28972, "x":4.15818, "y":5.38904, "heading":-1.0553, "vx":-0.28805, "vy":-1.87565, "omega":0.1499, "ax":3.3418, "ay":11.42462, "alpha":-2.35877, "fx":[56.03252,71.13899,55.60093,40.0526], "fy":[190.63475,185.59029,190.86835,194.67848]},
+    {"t":1.31631, "x":4.15171, "y":5.3432, "heading":-1.05131, "vx":-0.19919, "vy":-1.57186, "omega":0.08718, "ax":2.48782, "ay":11.65803, "alpha":-1.56738, "fx":[40.87462,51.98544,42.06351,30.95929], "fy":[194.57881,191.93954,194.38359,196.43332]},
+    {"t":1.3429, "x":4.14729, "y":5.30553, "heading":-1.049, "vx":-0.13303, "vy":-1.26186, "omega":0.0455, "ax":1.86309, "ay":11.7839, "alpha":-0.99058, "fx":[30.32095,37.7574,31.76159,24.38723], "fy":[196.58991,195.31281,196.39341,197.43172]},
+    {"t":1.36949, "x":4.14441, "y":5.27614, "heading":-1.04779, "vx":-0.08349, "vy":-0.94852, "omega":0.01916, "ax":1.39034, "ay":11.85483, "alpha":-0.55545, "fx":[22.61641,26.93672,23.71946,19.43275], "fy":[197.69,197.15444,197.57575,198.03712]},
+    {"t":1.39608, "x":4.14268, "y":5.25511, "heading":-1.04728, "vx":-0.04652, "vy":-0.63328, "omega":0.00439, "ax":1.02193, "ay":11.89587, "alpha":-0.21729, "fx":[16.77226,18.50228,17.29455,15.57133], "fy":[198.32104,198.16928,198.28136,198.42202]},
+    {"t":1.42267, "x":4.14181, "y":5.24248, "heading":-1.04716, "vx":-0.01935, "vy":-0.31696, "omega":-0.00139, "ax":0.72761, "ay":11.9198, "alpha":0.05224, "fx":[12.20042,11.77773,12.05732,12.48048], "fy":[198.69363,198.71869,198.70125,198.67556]},
+    {"t":1.44926, "x":4.14155, "y":5.23826, "heading":-1.0472, "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/startToL.traj b/src/main/deploy/choreo/startToL.traj
new file mode 100644
index 00000000..8dcdc7be
--- /dev/null
+++ b/src/main/deploy/choreo/startToL.traj
@@ -0,0 +1,84 @@
+{
+ "name":"startToL",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":7.1008875, "y":5.0756788, "heading":3.141592653589793, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":4.180931568145752, "y":5.788817405700684, "heading":-1.2827410322432835, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":3.85576086490539, "y":5.073261807735684, "heading":5.235987755982989, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"4.180931568145752 m", "val":4.180931568145752}, "y":{"exp":"5.788817405700684 m", "val":5.788817405700684}, "heading":{"exp":"-1.2827410322432835 rad", "val":-1.2827410322432835}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"L.x", "val":3.85576086490539}, "y":{"exp":"L.y", "val":5.073261807735684}, "heading":{"exp":"L.heading", "val":5.235987755982989}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,1.10537,1.49745],
+  "samples":[
+    {"t":0.0, "x":7.10089, "y":5.07568, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-5.72765, "ay":1.05979, "alpha":45.60884, "fx":[-20.40347,-7.01096,-161.16336,-193.3312], "fy":[-197.49548,198.97143,117.09875,-47.91016]},
+    {"t":0.0335, "x":7.09767, "y":5.07627, "heading":3.14159, "vx":-0.19185, "vy":0.0355, "omega":1.52772, "ax":-6.24476, "ay":1.1967, "alpha":43.42892, "fx":[-46.27794,-14.49748,-161.17053,-194.44284], "fy":[-192.77535,198.49962,117.02164,-42.95195]},
+    {"t":0.06699, "x":7.08774, "y":5.07813, "heading":-3.09042, "vx":-0.40103, "vy":0.07558, "omega":2.98242, "ax":-7.27619, "ay":1.42797, "alpha":38.37283, "fx":[-92.69361,-33.06614,-164.25745,-195.14522], "fy":[-174.29214,196.16872,112.53435,-39.19674]},
+    {"t":0.10049, "x":7.07023, "y":5.08147, "heading":-2.99052, "vx":-0.64475, "vy":0.12341, "omega":4.26775, "ax":-9.37106, "ay":3.62815, "alpha":22.3557, "fx":[-194.75012,-64.19966,-169.12948,-196.76486], "fy":[-22.24437,188.12902,104.83592,-28.80256]},
+    {"t":0.13398, "x":7.04338, "y":5.08764, "heading":-2.84757, "vx":-0.95865, "vy":0.24494, "omega":5.01658, "ax":-9.74415, "ay":5.85512, "alpha":11.91269, "fx":[-165.48149,-115.02862,-171.9935,-197.21753], "fy":[107.73817,161.6872,99.39489,21.58771]},
+    {"t":0.16748, "x":7.0058, "y":5.09913, "heading":-2.67953, "vx":-1.28504, "vy":0.44107, "omega":5.41561, "ax":-8.27501, "ay":6.99416, "alpha":-16.93947, "fx":[-169.38127,-196.65523,-94.59338,-91.13202], "fy":[102.77219,16.78938,171.13576,175.65968]},
+    {"t":0.20098, "x":6.95811, "y":5.11782, "heading":-2.49813, "vx":-1.56222, "vy":0.67534, "omega":4.84821, "ax":-8.42921, "ay":6.49415, "alpha":-18.75189, "fx":[-178.14532,-196.70348,-82.29165,-104.9034], "fy":[86.03637,2.54253,176.93069,167.50813]},
+    {"t":0.23447, "x":6.90106, "y":5.14409, "heading":-2.33573, "vx":-1.84456, "vy":0.89287, "omega":4.22009, "ax":-9.04295, "ay":5.51776, "alpha":-18.93632, "fx":[-186.90023,-195.08369,-96.22513,-124.75748], "fy":[63.39659,-16.85266,168.69285,152.67656]},
+    {"t":0.26797, "x":6.8342, "y":5.17709, "heading":-2.19438, "vx":-2.14747, "vy":1.0777, "omega":3.5858, "ax":-9.61987, "ay":3.72774, "alpha":-20.45976, "fx":[-194.56235,-184.13311,-116.94239,-145.79684], "fy":[28.30778,-62.69257,151.52971,131.41331]},
+    {"t":0.30146, "x":6.75687, "y":5.21528, "heading":-2.07427, "vx":-2.46969, "vy":1.20256, "omega":2.90048, "ax":-9.84603, "ay":0.17041, "alpha":-21.65097, "fx":[-193.06931,-136.42574,-156.59344,-170.42593], "fy":[-30.64096,-136.96374,85.32109,93.64625]},
+    {"t":0.33496, "x":6.66862, "y":5.25566, "heading":-1.97711, "vx":-2.7995, "vy":1.20827, "omega":2.17525, "ax":-9.08969, "ay":-0.19465, "alpha":-24.62674, "fx":[-187.06988,-117.38366,-130.19827,-171.43136], "fy":[-44.98784,-147.19546,95.63626,83.56804]},
+    {"t":0.36846, "x":6.56975, "y":5.29602, "heading":-1.90425, "vx":-3.10397, "vy":1.20175, "omega":1.35036, "ax":-8.03166, "ay":2.22523, "alpha":-24.82627, "fx":[-180.99335,-136.28269,-71.36922,-146.89036], "fy":[-7.91647,-88.59347,139.7737,105.1103]},
+    {"t":0.40195, "x":6.46127, "y":5.33752, "heading":-1.85902, "vx":-3.373, "vy":1.27628, "omega":0.51877, "ax":-3.34735, "ay":2.90194, "alpha":-12.47764, "fx":[-94.81057,-41.28195,-15.96901,-71.13315], "fy":[25.60712,9.08388,77.17693,81.62782]},
+    {"t":0.43545, "x":6.34641, "y":5.3819, "heading":-1.84164, "vx":-3.48512, "vy":1.37349, "omega":0.10082, "ax":-0.01345, "ay":0.40002, "alpha":-0.48449, "fx":[-1.67879,0.59815,1.2296,-1.04578], "fy":[5.84801,5.21701,7.4887,8.11868]},
+    {"t":0.46895, "x":6.22967, "y":5.42813, "heading":-1.83827, "vx":-3.48557, "vy":1.38689, "omega":0.08459, "ax":0.01305, "ay":0.05915, "alpha":-0.0349, "fx":[0.11302,0.27697,0.3219,0.15795], "fy":[0.92653,0.8816,1.04555,1.09048]},
+    {"t":0.50244, "x":6.11292, "y":5.47462, "heading":-1.83543, "vx":-3.48513, "vy":1.38887, "omega":0.08342, "ax":0.00006, "ay":0.01144, "alpha":-0.01751, "fx":[-0.05134,0.03095,0.05325,-0.02904], "fy":[0.16073,0.13843,0.22072,0.24302]},
+    {"t":0.53594, "x":5.99618, "y":5.52115, "heading":-1.83264, "vx":-3.48513, "vy":1.38925, "omega":0.08284, "ax":-0.00223, "ay":0.00414, "alpha":-0.01557, "fx":[-0.08359,-0.01034,0.00929,-0.06396], "fy":[0.04222,0.02258,0.09583,0.11546]},
+    {"t":0.56943, "x":5.87944, "y":5.56768, "heading":-1.82986, "vx":-3.48521, "vy":1.38939, "omega":0.08232, "ax":-0.00252, "ay":0.00295, "alpha":-0.01494, "fx":[-0.08646,-0.01612,0.00252,-0.06782], "fy":[0.02325,0.00461,0.07494,0.09358]},
+    {"t":0.60293, "x":5.7627, "y":5.61423, "heading":-1.82711, "vx":-3.48529, "vy":1.38949, "omega":0.08181, "ax":-0.00263, "ay":0.0023, "alpha":-0.01443, "fx":[-0.08674,-0.01873,-0.00091,-0.06892], "fy":[0.01324,-0.00458,0.06342,0.08125]},
+    {"t":0.63643, "x":5.64596, "y":5.66077, "heading":-1.82437, "vx":-3.48538, "vy":1.38957, "omega":0.08133, "ax":-0.00358, "ay":-0.00079, "alpha":-0.01354, "fx":[-0.09995,-0.03611,-0.01957,-0.08341], "fy":[-0.03688,-0.05343,0.01042,0.02696]},
+    {"t":0.66992, "x":5.52921, "y":5.70731, "heading":-1.82164, "vx":-3.4855, "vy":1.38954, "omega":0.08088, "ax":-0.01023, "ay":-0.02056, "alpha":-0.0098, "fx":[-0.19956,-0.15331,-0.14146,-0.18771], "fy":[-0.35996,-0.37181,-0.32556,-0.3137]},
+    {"t":0.70342, "x":5.41245, "y":5.75385, "heading":-1.81893, "vx":-3.48584, "vy":1.38885, "omega":0.08055, "ax":-0.05164, "ay":-0.14837, "alpha":0.0176, "fx":[-0.80872,-0.89182,-0.91287,-0.82977], "fy":[-2.44219,-2.42113,-2.5042,-2.52526]},
+    {"t":0.73691, "x":5.29566, "y":5.80028, "heading":-1.81623, "vx":-3.48757, "vy":1.38388, "omega":0.08114, "ax":-0.22542, "ay":-0.99283, "alpha":0.45311, "fx":[-2.41196,-4.57041,-5.10142,-2.94702], "fy":[-15.7575,-15.21753,-17.34315,-17.88202]},
+    {"t":0.77041, "x":5.17871, "y":5.84608, "heading":-1.81352, "vx":-3.49512, "vy":1.35063, "omega":0.09632, "ax":1.3661, "ay":-5.38128, "alpha":9.30156, "fx":[58.59854,3.80507,-9.50058,38.18587], "fy":[-74.0312,-68.56769,-106.3117,-109.90279]},
+    {"t":0.80391, "x":5.06241, "y":5.8883, "heading":-1.81029, "vx":-3.44936, "vy":1.17037, "omega":0.40788, "ax":4.17116, "ay":-7.6073, "alpha":18.86725, "fx":[154.9357,47.76738,-14.31404,89.73565], "fy":[-74.26433,-105.47016,-172.09914,-155.4067]},
+    {"t":0.8374, "x":4.94921, "y":5.92324, "heading":-1.79663, "vx":-3.30964, "vy":0.91556, "omega":1.03986, "ax":4.06322, "ay":-8.67308, "alpha":18.34432, "fx":[162.71767,37.40347,-19.60963,90.41645], "fy":[-84.78346,-144.54474,-183.78346,-165.19274]},
+    {"t":0.8709, "x":4.84063, "y":5.94904, "heading":-1.7618, "vx":-3.17354, "vy":0.62504, "omega":1.65432, "ax":2.92028, "ay":-10.63646, "alpha":10.49739, "fx":[114.02322,18.40862,-5.20323,67.49005], "fy":[-151.9385,-185.16992,-191.61055,-180.49987]},
+    {"t":0.90439, "x":4.73596, "y":5.96401, "heading":-1.70638, "vx":-3.07572, "vy":0.26876, "omega":2.00594, "ax":2.15455, "ay":-11.49864, "alpha":4.24309, "fx":[62.33027,21.56139,12.49098,47.2788], "fy":[-185.70379,-194.40885,-195.90561,-190.68882]},
+    {"t":0.93789, "x":4.63415, "y":5.96656, "heading":-1.63919, "vx":-3.00356, "vy":-0.11639, "omega":2.14807, "ax":2.69844, "ay":-11.529, "alpha":1.69419, "fx":[55.1467,39.4352,35.31317,50.03185], "fy":[-189.64341,-193.47076,-194.41733,-191.20018]},
+    {"t":0.97139, "x":4.53506, "y":5.9562, "heading":-1.56724, "vx":-2.91317, "vy":-0.50257, "omega":2.20482, "ax":3.41999, "ay":-11.38563, "alpha":-0.33778, "fx":[55.07472,58.0598,58.96398,55.93998], "fy":[-190.37757,-189.49428,-189.19513,-190.10519]},
+    {"t":1.00488, "x":4.43939, "y":5.93297, "heading":-1.49339, "vx":-2.79861, "vy":-0.88394, "omega":2.19351, "ax":4.13336, "ay":-11.14891, "alpha":-2.266, "fx":[56.65914,75.59884,81.99308,61.35345], "fy":[-190.29065,-183.61505,-180.75384,-188.7283]},
+    {"t":1.03838, "x":4.34797, "y":5.89711, "heading":-1.41991, "vx":-2.66016, "vy":-1.25739, "omega":2.1176, "ax":4.78867, "ay":-10.84897, "alpha":-4.18126, "fx":[58.80075,91.62955,103.63319,65.23565], "fy":[-189.8678,-176.41369,-169.50091,-187.60597]},
+    {"t":1.07187, "x":4.26155, "y":5.84891, "heading":-1.34898, "vx":-2.49976, "vy":-1.62079, "omega":1.97755, "ax":5.54694, "ay":-10.33967, "alpha":-7.44376, "fx":[59.06032,111.38961,133.68383,65.72587], "fy":[-189.94416,-164.8376,-147.12626,-187.52153]},
+    {"t":1.10537, "x":4.18093, "y":5.78882, "heading":-1.28274, "vx":-2.31396, "vy":-1.96713, "omega":1.72821, "ax":6.68578, "ay":-9.68924, "alpha":-6.51318, "fx":[82.30925,126.28405,145.40446,91.79725], "fy":[-180.96708,-153.59762,-135.39862,-176.09629]},
+    {"t":1.13338, "x":4.11875, "y":5.72993, "heading":-1.23434, "vx":-2.12672, "vy":-2.23848, "omega":1.5458, "ax":7.8407, "ay":-8.72933, "alpha":-7.10148, "fx":[100.33269,143.68449,164.46914,114.31666], "fy":[-171.46833,-137.29176,-111.1903,-162.10466]},
+    {"t":1.16138, "x":4.06226, "y":5.66381, "heading":-1.19105, "vx":-1.90713, "vy":-2.48296, "omega":1.34692, "ax":9.4219, "ay":-6.89615, "alpha":-7.88261, "fx":[126.47944,165.91196,186.33044,149.51216], "fy":[-152.92761,-109.12396,-67.97058,-129.8002]},
+    {"t":1.18939, "x":4.01255, "y":5.59157, "heading":-1.15333, "vx":-1.64326, "vy":-2.67609, "omega":1.12616, "ax":11.12025, "ay":-3.33476, "alpha":-8.68475, "fx":[163.3375,188.93819,198.19235,191.00889], "fy":[-112.16482,-60.57923,0.75461,-50.3657]},
+    {"t":1.21739, "x":3.97089, "y":5.51532, "heading":-1.12179, "vx":-1.33182, "vy":-2.76948, "omega":0.88293, "ax":11.39095, "ay":2.0782, "alpha":-9.24489, "fx":[196.22077,197.95181,182.05152,183.30247], "fy":[-25.18757,11.73571,78.39399,73.62831]},
+    {"t":1.2454, "x":3.93805, "y":5.43857, "heading":-1.09706, "vx":-1.01281, "vy":-2.71128, "omega":0.62402, "ax":9.50304, "ay":6.83162, "alpha":-7.25986, "fx":[178.43174,178.14965,146.02395,131.03926], "fy":[85.60982,87.18711,134.27855,148.44408]},
+    {"t":1.27341, "x":3.91342, "y":5.36531, "heading":-1.07958, "vx":-0.74667, "vy":-2.51996, "omega":0.4207, "ax":7.10866, "ay":9.44, "alpha":-4.92478, "fx":[129.92301,141.68037,111.26614,91.12235], "fy":[149.74614,139.01726,164.45817,176.21943]},
+    {"t":1.30141, "x":3.89529, "y":5.29844, "heading":-1.0678, "vx":-0.54758, "vy":-2.25558, "omega":0.28278, "ax":5.26892, "ay":10.64176, "alpha":-3.39156, "fx":[92.23712,107.65497,84.83427,66.59488], "fy":[175.80652,166.95145,179.69016,187.12401]},
+    {"t":1.32942, "x":3.88202, "y":5.23945, "heading":-1.05988, "vx":-0.40002, "vy":-1.95754, "omega":0.18779, "ax":3.98991, "ay":11.21567, "alpha":-2.37474, "fx":[67.73132,81.64588,65.67842,50.9842], "fy":[186.82063,181.23773,187.65844,192.1227]},
+    {"t":1.35742, "x":3.87239, "y":5.18902, "heading":-1.05462, "vx":-0.28828, "vy":-1.64344, "omega":0.12128, "ax":3.09067, "ay":11.51286, "alpha":-1.66904, "fx":[51.47309,62.55634,51.64311,40.40739], "fy":[192.06608,188.78069,192.08114,194.72762]},
+    {"t":1.38543, "x":3.86552, "y":5.14751, "heading":-1.05123, "vx":-0.20172, "vy":-1.32101, "omega":0.07454, "ax":2.43672, "ay":11.67858, "alpha":-1.15883, "fx":[40.14317,48.40375,41.08955,32.83953], "fy":[194.83825,192.9699,194.67625,196.22104]},
+    {"t":1.41344, "x":3.86083, "y":5.11509, "heading":-1.04914, "vx":-0.13348, "vy":-0.99394, "omega":0.04209, "ax":1.94452, "ay":11.77662, "alpha":-0.77635, "fx":[31.87747,37.66194,32.93435,27.18325], "fy":[196.42437,195.40738,196.27032,197.1404]},
+    {"t":1.44144, "x":3.85785, "y":5.09188, "heading":-1.04796, "vx":-0.07902, "vy":-0.66412, "omega":0.02034, "ax":1.56269, "ay":11.83731, "alpha":-0.4806, "fx":[25.61419,29.3027,26.4739,22.8067], "fy":[197.3875,196.87867,197.2851,197.73793]},
+    {"t":1.46945, "x":3.85625, "y":5.07792, "heading":-1.04739, "vx":-0.03525, "vy":-0.3326, "omega":0.00689, "ax":1.25882, "ay":11.87618, "alpha":-0.24584, "fx":[20.71986,22.64655,21.24421,19.32487], "fy":[197.99909,197.79013,197.94855,198.14304]},
+    {"t":1.49745, "x":3.85576, "y":5.07326, "heading":-1.0472, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+  "splits":[0]
+ },
+ "events":[]
+}
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 4f60eb8e..27e36de3 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -57,7 +57,9 @@ public void disabledInit() {
   }
 
   @Override
-  public void disabledPeriodic() {}
+  public void disabledPeriodic() {
+    if (!m_robotContainer.hasSwerveZeroed()) m_robotContainer.zeroSwerve();
+  }
 
   @Override
   public void disabledExit() {}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 16769b67..3129e8e9 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -6,13 +6,16 @@
 
 import static edu.wpi.first.units.Units.Rotations;
 
+import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.ConditionalCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.commands.algae.IntakeAlgaeCommand;
@@ -23,6 +26,7 @@
 import frc.robot.commands.biscuit.JogBiscuitCommand;
 import frc.robot.commands.coral.EnableEjectBeamCommand;
 import frc.robot.commands.coral.OpenLoopCoralCommand;
+import frc.robot.commands.drive.DriveAutonCommand;
 import frc.robot.commands.drive.DriveTeleopCommand;
 import frc.robot.commands.drive.ResetGyroCommand;
 import frc.robot.commands.elevator.HoldElevatorCommand;
@@ -35,13 +39,17 @@
 import frc.robot.commands.robotState.InterruptAutoCommand;
 import frc.robot.commands.robotState.ReefCycleCommand;
 import frc.robot.commands.robotState.ScoreAlgaeCommand;
+import frc.robot.commands.robotState.SetScoreSideCommand;
 import frc.robot.commands.robotState.SetScoreSideRightCommand;
 import frc.robot.commands.robotState.SetScoringLevelCommand;
 import frc.robot.commands.robotState.StowCommand;
 import frc.robot.commands.robotState.ToggleAlgaeHeightCommand;
-import frc.robot.commands.robotState.ToggleAutoCommand;
+import frc.robot.commands.robotState.ToggleAutoPlacingCommand;
 import frc.robot.commands.robotState.ToggleGetAlgaeCommand;
 import frc.robot.commands.robotState.setScoreSideLeftCommand;
+import frc.robot.commands.tagAlign.DriveTuningCommand;
+import frc.robot.commands.tagAlign.YawTuningCommand;
+import frc.robot.commands.vision.SetVisionUpdatesCommand;
 import frc.robot.constants.BiscuitConstants;
 import frc.robot.constants.ElevatorConstants;
 import frc.robot.constants.RobotConstants;
@@ -52,12 +60,14 @@
 import frc.robot.subsystems.battMon.BattMonSubsystem;
 import frc.robot.subsystems.biscuit.BiscuitIOFX;
 import frc.robot.subsystems.biscuit.BiscuitSubsystem;
+import frc.robot.subsystems.climb.ClimbIO;
+import frc.robot.subsystems.climb.ClimbIOServoFX;
 import frc.robot.subsystems.climb.ClimbSubsystem;
 import frc.robot.subsystems.coral.CoralIO;
 import frc.robot.subsystems.coral.CoralIOFX;
 import frc.robot.subsystems.coral.CoralSubsystem;
 import frc.robot.subsystems.drive.DriveSubsystem;
-import frc.robot.subsystems.drive.Swerve;
+import frc.robot.subsystems.drive.SwerveFXS;
 import frc.robot.subsystems.elevator.ElevatorIO;
 import frc.robot.subsystems.elevator.ElevatorIOFX;
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
@@ -65,7 +75,9 @@
 import frc.robot.subsystems.funnel.FunnelSubsystem;
 import frc.robot.subsystems.led.LEDIO;
 import frc.robot.subsystems.led.LEDSubsystem;
+import frc.robot.subsystems.pathHandler.PathHandler;
 import frc.robot.subsystems.robotState.RobotStateSubsystem;
+import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide;
 import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel;
 import frc.robot.subsystems.tagAlign.TagAlignSubsystem;
 import frc.robot.subsystems.vision.VisionSubsystem;
@@ -83,12 +95,13 @@ public class RobotContainer {
   private final BiscuitIOFX biscuitIO;
   private final BiscuitSubsystem biscuitSubsystem;
 
+  private final ClimbIO climbIO;
   private final ClimbSubsystem climbSubsystem;
 
   private final CoralIO coralIO;
   private final CoralSubsystem coralSubsystem;
 
-  private final Swerve swerve;
+  private final SwerveFXS swerve;
   private final DriveSubsystem driveSubsystem;
 
   private final ElevatorIO elevatorIO;
@@ -104,6 +117,8 @@ public class RobotContainer {
 
   private final VisionSubsystem visionSubsystem;
 
+  private final PathHandler pathHandler;
+
   private final XboxController xboxController = new XboxController(1);
   private final Joystick driveJoystick = new Joystick(0);
   private final FlyskyJoystick flysky = new FlyskyJoystick(driveJoystick);
@@ -120,12 +135,13 @@ public RobotContainer() {
     biscuitIO = new BiscuitIOFX();
     biscuitSubsystem = new BiscuitSubsystem(biscuitIO);
 
-    climbSubsystem = new ClimbSubsystem();
+    climbIO = new ClimbIOServoFX();
+    climbSubsystem = new ClimbSubsystem(climbIO);
 
     coralIO = new CoralIOFX();
     coralSubsystem = new CoralSubsystem(coralIO);
 
-    swerve = new Swerve();
+    swerve = new SwerveFXS();
     driveSubsystem = new DriveSubsystem(swerve);
 
     elevatorIO = new ElevatorIOFX();
@@ -135,7 +151,7 @@ public RobotContainer() {
     funnelSubsystem = new FunnelSubsystem(funnelIO);
 
     ledIO = new LEDIO();
-    ledSubsystem = new LEDSubsystem();
+    ledSubsystem = new LEDSubsystem(ledIO);
 
     visionSubsystem = new VisionSubsystem(driveSubsystem);
 
@@ -157,6 +173,8 @@ public RobotContainer() {
 
     driveSubsystem.setRobotStateSubsystem(robotStateSubsystem);
 
+    pathHandler = new PathHandler(driveSubsystem, tagAlignSubsystem, robotStateSubsystem);
+
     configureTelemetry();
     configureDriverBindings();
     configureOperatorBindings();
@@ -171,9 +189,19 @@ private void configureTelemetry() {
     elevatorSubsystem.registerWith(telemetryService);
     funnelSubsystem.registerWith(telemetryService);
     biscuitSubsystem.registerWith(telemetryService);
+    ledSubsystem.registerWith(telemetryService);
+    climbSubsystem.registerWith(telemetryService);
     telemetryService.start();
   }
 
+  public boolean hasSwerveZeroed() {
+    return driveSubsystem.hasZeroed();
+  }
+
+  public void zeroSwerve() {
+    driveSubsystem.zeroModules();
+  }
+
   private void configureDriverBindings() {
     driveSubsystem.setDefaultCommand(
         new DriveTeleopCommand(
@@ -196,9 +224,15 @@ private void configureDriverBindings() {
                 coralSubsystem,
                 biscuitSubsystem,
                 algaeSubsystem));
-    new JoystickButton(driveJoystick, Button.SWA.id)
+
+    // Interupt
+    new JoystickButton(driveJoystick, Button.SWG_UP.id)
         .onTrue(new InterruptAutoCommand(robotStateSubsystem))
         .onFalse(new InterruptAutoCommand(robotStateSubsystem));
+    new JoystickButton(driveJoystick, Button.SWG_DWN.id)
+        .onTrue(new InterruptAutoCommand(robotStateSubsystem))
+        .onFalse(new InterruptAutoCommand(robotStateSubsystem));
+
     new JoystickButton(driveJoystick, Button.SWB_UP.id)
         .onTrue(new ZeroElevatorCommand(elevatorSubsystem))
         .onFalse(new ZeroElevatorCommand(elevatorSubsystem));
@@ -206,8 +240,6 @@ private void configureDriverBindings() {
         .onTrue(new ZeroElevatorCommand(elevatorSubsystem))
         .onFalse(new ZeroElevatorCommand(elevatorSubsystem));
 
-    // other stuff
-
     new JoystickButton(driveJoystick, Button.M_SWH.id)
         .onTrue(
             new ConditionalCommand(
@@ -245,6 +277,10 @@ private void configureDriverBindings() {
         .onFalse(
             new FloorAlgaeCommand(
                 robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem));
+
+    // climb
+    new JoystickButton(driveJoystick, Button.SWA.id)
+        .onTrue(new InstantCommand(() -> robotStateSubsystem.toClimb()));
   }
 
   private void configureOperatorBindings() {
@@ -259,34 +295,10 @@ private void configureOperatorBindings() {
         .onTrue(new SetScoringLevelCommand(robotStateSubsystem, ScoringLevel.L4));
 
     // Set scoring side
-    // new JoystickButton(xboxController, XboxController.Button.kLeftStick.value)
-    //     .onTrue(new SetScoreSideCommand(robotStateSubsystem, ScoreSide.LEFT));
-    // new JoystickButton(xboxController, XboxController.Button.kRightStick.value)
-    //     .onTrue(new SetScoreSideCommand(robotStateSubsystem, ScoreSide.RIGHT));
-
-    // Move biscuit
-    new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband))
-        .onTrue(
-            new JogBiscuitCommand(
-                biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountUp, Rotations)))
-        .onFalse(new HoldBiscuitCommand(biscuitSubsystem));
-    new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband))
-        .onTrue(
-            new JogBiscuitCommand(
-                biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations)))
-        .onFalse(new HoldBiscuitCommand(biscuitSubsystem));
-
-    // Move elevator
-    new Trigger((() -> xboxController.getLeftY() < -RobotConstants.kTestingDeadband))
-        .onTrue(
-            new JogElevatorCommand(
-                elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations)))
-        .onFalse(new HoldElevatorCommand(elevatorSubsystem));
-    new Trigger((() -> xboxController.getLeftY() > RobotConstants.kTestingDeadband))
-        .onTrue(
-            new JogElevatorCommand(
-                elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, Rotations)))
-        .onFalse(new HoldElevatorCommand(elevatorSubsystem));
+    new JoystickButton(xboxController, XboxController.Button.kLeftStick.value)
+        .onTrue(new SetScoreSideCommand(robotStateSubsystem, ScoreSide.LEFT));
+    new JoystickButton(xboxController, XboxController.Button.kRightStick.value)
+        .onTrue(new SetScoreSideCommand(robotStateSubsystem, ScoreSide.RIGHT));
 
     // Stow
     new JoystickButton(xboxController, XboxController.Button.kBack.value)
@@ -310,12 +322,16 @@ private void configureOperatorBindings() {
 
     // Scoring
     new JoystickButton(xboxController, XboxController.Button.kA.value)
-        .onTrue(new ToggleAutoCommand(robotStateSubsystem));
+        .onTrue(new ToggleAutoPlacingCommand(robotStateSubsystem));
 
     new JoystickButton(xboxController, XboxController.Button.kRightStick.value)
         .onTrue(new SetScoreSideRightCommand(robotStateSubsystem));
     new JoystickButton(xboxController, XboxController.Button.kLeftStick.value)
         .onTrue(new setScoreSideLeftCommand(robotStateSubsystem));
+
+    // Prep Climb
+    new JoystickButton(xboxController, XboxController.Button.kStart.value)
+        .onTrue(new InstantCommand(() -> robotStateSubsystem.toPrepClimb()));
   }
 
   private void configureTestOperatorBindings() {
@@ -427,6 +443,85 @@ public void configurePitDashboard() {
         .add("Zero Elevator", new ZeroElevatorCommand(elevatorSubsystem))
         .withPosition(2, 1)
         .withSize(1, 1);
+
+    Shuffleboard.getTab("Debug")
+        .add(
+            "Toggle LED autoplacing",
+            new InstantCommand(() -> ledSubsystem.setAutoPlacing(!ledSubsystem.getAutoPlacing())))
+        .withPosition(1, 1)
+        .withSize(1, 1);
+    Shuffleboard.getTab("Debug")
+        .add(
+            "Toggle LED Current Limiting",
+            new InstantCommand(
+                () -> ledSubsystem.setCurrentLimiting(!ledSubsystem.getCurrentLimiting())))
+        .withPosition(2, 1)
+        .withSize(1, 1);
+
+    GenericEntry yawP =
+        Shuffleboard.getTab("Debug")
+            .add("Yaw kP", 0)
+            .withPosition(4, 2)
+            .withSize(1, 1)
+            .withWidget(BuiltInWidgets.kTextView)
+            .getEntry();
+    Shuffleboard.getTab("Debug")
+        .add("Yaw Tuning", new YawTuningCommand(driveSubsystem, () -> yawP.getDouble(0)))
+        .withPosition(3, 2)
+        .withSize(1, 1);
+
+    Shuffleboard.getTab("Debug")
+        .add(
+            "Drive Tuning",
+            new DriveTuningCommand(driveSubsystem, () -> yawP.getDouble(0), tagAlignSubsystem))
+        .withPosition(4, 2)
+        .withSize(1, 1);
+
+    Shuffleboard.getTab("Pit")
+        .add(
+            "Five Meter Path",
+            new DriveAutonCommand(driveSubsystem, "FiveMeterTestPath", true, true, false))
+        .withPosition(2, 0)
+        .withSize(1, 1);
+
+    Shuffleboard.getTab("Pit")
+        .add("Turn Off Vision Updates", new SetVisionUpdatesCommand(visionSubsystem, false))
+        .withPosition(0, 0)
+        .withSize(1, 1);
+
+    Shuffleboard.getTab("Pit")
+        .add("Turn On Vision Updates", new SetVisionUpdatesCommand(visionSubsystem, true))
+        .withPosition(1, 0)
+        .withSize(1, 1);
+
+    Shuffleboard.getTab("Pit")
+        .add("Prep Climb", new InstantCommand(() -> robotStateSubsystem.toPrepClimb()))
+        .withPosition(3, 0)
+        .withSize(1, 1);
+    Shuffleboard.getTab("Pit")
+        .add("Climb", new InstantCommand(() -> robotStateSubsystem.toClimb()))
+        .withPosition(4, 0)
+        .withSize(1, 1);
+
+    // Shuffleboard.getTab("Pit")
+    //     .add(
+    //         "Start Auton",
+    //         new NonProcessorShallowAutonCommand(
+    //             driveSubsystem,
+    //             pathHandler,
+    //             robotStateSubsystem,
+    //             algaeSubsystem,
+    //             biscuitSubsystem,
+    //             coralSubsystem,
+    //             elevatorSubsystem,
+    //             tagAlignSubsystem,
+    //             "startToJ",
+    //             PathHandlerConstants.kpathNames,
+    //             List.of('K', 'L', 'M'),
+    //             List.of(4, 4, 4),
+    //             'J'))
+    //     .withPosition(3, 0)
+    //     .withSize(1, 1);
   }
 
   public Command getAutonomousCommand() {
diff --git a/src/main/java/frc/robot/commands/auton/NonProcessorShallowAutonCommand.java b/src/main/java/frc/robot/commands/auton/NonProcessorShallowAutonCommand.java
new file mode 100644
index 00000000..66355e26
--- /dev/null
+++ b/src/main/java/frc/robot/commands/auton/NonProcessorShallowAutonCommand.java
@@ -0,0 +1,62 @@
+package frc.robot.commands.auton;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.commands.drive.DriveAutonCommand;
+import frc.robot.commands.drive.ResetGyroCommand;
+import frc.robot.commands.drive.SetGyroOffsetCommand;
+import frc.robot.commands.elevator.ZeroElevatorCommand;
+import frc.robot.subsystems.algae.AlgaeSubsystem;
+import frc.robot.subsystems.biscuit.BiscuitSubsystem;
+import frc.robot.subsystems.coral.CoralSubsystem;
+import frc.robot.subsystems.drive.DriveSubsystem;
+import frc.robot.subsystems.elevator.ElevatorSubsystem;
+import frc.robot.subsystems.pathHandler.PathHandler;
+import frc.robot.subsystems.robotState.RobotStateSubsystem;
+import frc.robot.subsystems.tagAlign.TagAlignSubsystem;
+import java.util.List;
+
+public class NonProcessorShallowAutonCommand extends SequentialCommandGroup {
+
+  private PathHandler pathHandler;
+  private DriveSubsystem driveSubsystem;
+  private DriveAutonCommand startPath;
+
+  public NonProcessorShallowAutonCommand(
+      DriveSubsystem driveSubsystem,
+      PathHandler pathHandler,
+      RobotStateSubsystem robotStateSubsystem,
+      AlgaeSubsystem algaeSubsystem,
+      BiscuitSubsystem biscuitSubsystem,
+      CoralSubsystem coralSubsystem,
+      ElevatorSubsystem elevatorSubsystem,
+      TagAlignSubsystem tagAlignSubsystem,
+      String startPathName,
+      String[][] pathNames,
+      List<Character> NodeNames,
+      List<Integer> NodeLevels,
+      char startNode) {
+    addRequirements(
+        driveSubsystem, algaeSubsystem, biscuitSubsystem, coralSubsystem, elevatorSubsystem);
+    this.pathHandler = pathHandler;
+    this.driveSubsystem = driveSubsystem;
+
+    startPath = new DriveAutonCommand(driveSubsystem, startPathName, false, true, false);
+
+    addCommands(
+        new SequentialCommandGroup(
+            new ParallelCommandGroup(
+                new ZeroElevatorCommand(elevatorSubsystem),
+                new SequentialCommandGroup(
+                    new ResetGyroCommand(driveSubsystem),
+                    new SetGyroOffsetCommand(driveSubsystem, new Rotation2d(180)))),
+            startPath,
+            new frc.robot.commands.pathhHandler.StartPathHandlerCommand(
+                pathHandler, pathNames, NodeNames, NodeLevels, startNode, false)));
+  }
+
+  public void reassignAlliance() {
+    startPath.reassignAlliance();
+  }
+}
diff --git a/src/main/java/frc/robot/commands/climb/Climb.java b/src/main/java/frc/robot/commands/climb/Climb.java
new file mode 100644
index 00000000..08c2a4c4
--- /dev/null
+++ b/src/main/java/frc/robot/commands/climb/Climb.java
@@ -0,0 +1,20 @@
+package frc.robot.commands.climb;
+
+import frc.robot.subsystems.climb.ClimbSubsystem;
+
+public class Climb {
+
+  ClimbSubsystem climbSubsystem;
+  /*
+  @Override
+  public void initialize() {
+
+  }
+
+  @Override
+  public void isFinished() {
+
+  }
+  */
+
+}
diff --git a/src/main/java/frc/robot/commands/climb/PrepClimb.java b/src/main/java/frc/robot/commands/climb/PrepClimb.java
new file mode 100644
index 00000000..2f9fba93
--- /dev/null
+++ b/src/main/java/frc/robot/commands/climb/PrepClimb.java
@@ -0,0 +1,20 @@
+package frc.robot.commands.climb;
+
+import frc.robot.subsystems.climb.ClimbSubsystem;
+
+public class PrepClimb {
+
+  ClimbSubsystem climbSubsystem;
+  /*
+  @Override
+  public void initialize() {
+
+  }
+
+  @Override
+  public void isFinished() {
+
+  }
+  */
+
+}
diff --git a/src/main/java/frc/robot/commands/drive/DriveAutonCommand.java b/src/main/java/frc/robot/commands/drive/DriveAutonCommand.java
index 114e18d8..ca8834cf 100644
--- a/src/main/java/frc/robot/commands/drive/DriveAutonCommand.java
+++ b/src/main/java/frc/robot/commands/drive/DriveAutonCommand.java
@@ -4,9 +4,11 @@
 import choreo.trajectory.SwerveSample;
 import choreo.trajectory.Trajectory;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.commands.auton.AutoCommandInterface;
+import frc.robot.constants.DriveConstants;
 import frc.robot.subsystems.drive.DriveSubsystem;
 import java.util.Optional;
 import org.slf4j.Logger;
@@ -20,6 +22,7 @@ public class DriveAutonCommand extends Command implements AutoCommandInterface {
   private boolean isTherePath = false;
   private String trajectoryName;
   private boolean mirrorTrajectory = false;
+  private boolean mirrorToProcessor = false;
 
   private boolean resetOdometry;
   private boolean lastPath;
@@ -28,13 +31,15 @@ public DriveAutonCommand(
       DriveSubsystem driveSubsystem,
       String trajectoryName,
       boolean lastPath,
-      boolean resetOdometry) {
+      boolean resetOdometry,
+      boolean mirrorToProcessor) {
 
     addRequirements(driveSubsystem);
     this.driveSubsystem = driveSubsystem;
     this.resetOdometry = resetOdometry;
     this.lastPath = lastPath;
     this.trajectoryName = trajectoryName;
+    this.mirrorToProcessor = mirrorToProcessor;
     Optional<Trajectory<SwerveSample>> tempTrajectory = Choreo.loadTrajectory(trajectoryName);
     if (tempTrajectory.isPresent()) {
       trajectory = tempTrajectory.get();
@@ -51,12 +56,46 @@ public void reassignAlliance() {
     mirrorTrajectory = driveSubsystem.shouldFlip();
   }
 
+  private SwerveSample mirrorToProcessor(SwerveSample sample) {
+    if (mirrorToProcessor) {
+      sample =
+          new SwerveSample(
+              sample.t,
+              sample.x,
+              DriveConstants.kFieldMaxY - sample.y,
+              sample.heading * -1,
+              sample.vx,
+              DriveConstants.kFieldMaxY - sample.vy,
+              sample.omega * -1,
+              sample.ax,
+              DriveConstants.kFieldMaxY - sample.ay,
+              sample.alpha * -1,
+              sample.moduleForcesX(),
+              new double[] {
+                sample.moduleForcesY()[0] * -1,
+                sample.moduleForcesY()[1] * -1,
+                sample.moduleForcesY()[2] * -1,
+                sample.moduleForcesY()[3] * -1
+              });
+    }
+    return sample;
+  }
+
+  private Pose2d mirrorToProcessor(Pose2d pose) {
+    pose =
+        new Pose2d(
+            pose.getX(),
+            DriveConstants.kFieldMaxY - pose.getY(),
+            Rotation2d.fromDegrees(pose.getRotation().getDegrees() * -1));
+    return pose;
+  }
+
   @Override
   public void initialize() {
     if (isTherePath) {
       driveSubsystem.setAutoDebugMsg("Initialize " + trajectoryName);
       Pose2d initialPose = new Pose2d();
-      initialPose = trajectory.getInitialPose(mirrorTrajectory).get();
+      initialPose = mirrorToProcessor(trajectory.getInitialPose(mirrorTrajectory).get());
       if (resetOdometry) {
         driveSubsystem.resetOdometry(initialPose);
       }
@@ -66,7 +105,8 @@ public void initialize() {
       driveSubsystem.grapherTrajectoryActive(true);
       timer.reset();
       logger.info("Begin Trajectory: {}", trajectoryName);
-      SwerveSample desiredState = trajectory.sampleAt(timer.get(), mirrorTrajectory).get();
+      SwerveSample desiredState =
+          mirrorToProcessor(trajectory.sampleAt(timer.get(), mirrorTrajectory).get());
       driveSubsystem.calculateController(desiredState);
     }
   }
@@ -74,7 +114,8 @@ public void initialize() {
   @Override
   public void execute() {
     if (isTherePath) {
-      SwerveSample desiredState = trajectory.sampleAt(timer.get(), mirrorTrajectory).get();
+      SwerveSample desiredState =
+          mirrorToProcessor(trajectory.sampleAt(timer.get(), mirrorTrajectory).get());
       driveSubsystem.calculateController(desiredState);
     }
   }
@@ -94,7 +135,8 @@ public void end(boolean interrupted) {
 
     if (!interrupted && !lastPath) {
       driveSubsystem.calculateController(
-          trajectory.sampleAt(trajectory.getTotalTime(), mirrorTrajectory).get());
+          mirrorToProcessor(
+              trajectory.sampleAt(trajectory.getTotalTime(), mirrorTrajectory).get()));
     } else {
       driveSubsystem.drive(0, 0, 0);
     }
diff --git a/src/main/java/frc/robot/commands/drive/SetGyroOffsetCommand.java b/src/main/java/frc/robot/commands/drive/SetGyroOffsetCommand.java
new file mode 100644
index 00000000..9725cac4
--- /dev/null
+++ b/src/main/java/frc/robot/commands/drive/SetGyroOffsetCommand.java
@@ -0,0 +1,21 @@
+package frc.robot.commands.drive;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.drive.DriveSubsystem;
+
+public class SetGyroOffsetCommand extends InstantCommand {
+  private DriveSubsystem driveSubsystem;
+  private Rotation2d offset;
+
+  public SetGyroOffsetCommand(DriveSubsystem driveSubsystem, Rotation2d offset) {
+    this.driveSubsystem = driveSubsystem;
+    this.offset = offset;
+    addRequirements(driveSubsystem);
+  }
+
+  @Override
+  public void initialize() {
+    driveSubsystem.setGyroOffset(offset);
+  }
+}
diff --git a/src/main/java/frc/robot/commands/pathhHandler/KillPathHandlerCommand.java b/src/main/java/frc/robot/commands/pathhHandler/KillPathHandlerCommand.java
new file mode 100644
index 00000000..254f2a36
--- /dev/null
+++ b/src/main/java/frc/robot/commands/pathhHandler/KillPathHandlerCommand.java
@@ -0,0 +1,18 @@
+package frc.robot.commands.pathhHandler;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.pathHandler.PathHandler;
+
+public class KillPathHandlerCommand extends InstantCommand {
+
+  PathHandler pathHandler;
+
+  public KillPathHandlerCommand(PathHandler pathHandler) {
+    this.pathHandler = pathHandler;
+  }
+
+  @Override
+  public void initialize() {
+    pathHandler.killPathHandler();
+  }
+}
diff --git a/src/main/java/frc/robot/commands/pathhHandler/SetPathHandlerCommand.java b/src/main/java/frc/robot/commands/pathhHandler/SetPathHandlerCommand.java
new file mode 100644
index 00000000..a60903c9
--- /dev/null
+++ b/src/main/java/frc/robot/commands/pathhHandler/SetPathHandlerCommand.java
@@ -0,0 +1,38 @@
+package frc.robot.commands.pathhHandler;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.pathHandler.PathHandler;
+import java.util.List;
+
+public class SetPathHandlerCommand extends InstantCommand {
+  private PathHandler pathHandler;
+  private String[][] pathNames;
+  private List<Character> NodeNames;
+  private List<Integer> NodeLevels;
+  private Character startNode;
+  private boolean mirrorToProcessor;
+
+  public SetPathHandlerCommand(
+      PathHandler pathHandler,
+      String[][] pathNames,
+      List<Character> NodeNames,
+      List<Integer> NodeLevels,
+      Character startNode,
+      boolean mirrorToProcessor) {
+    this.pathHandler = pathHandler;
+    this.pathNames = pathNames;
+    this.NodeNames = NodeNames;
+    this.NodeLevels = NodeLevels;
+    this.startNode = startNode;
+    this.mirrorToProcessor = mirrorToProcessor;
+  }
+
+  @Override
+  public void initialize() {
+    pathHandler.setPathNames(pathNames);
+    pathHandler.setNodeNames(NodeNames);
+    pathHandler.setNodeLevels(NodeLevels);
+    pathHandler.setStartNode(startNode);
+    pathHandler.setMirrorToProcessor(mirrorToProcessor);
+  }
+}
diff --git a/src/main/java/frc/robot/commands/pathhHandler/StartPathHandlerCommand.java b/src/main/java/frc/robot/commands/pathhHandler/StartPathHandlerCommand.java
new file mode 100644
index 00000000..3465ca72
--- /dev/null
+++ b/src/main/java/frc/robot/commands/pathhHandler/StartPathHandlerCommand.java
@@ -0,0 +1,50 @@
+package frc.robot.commands.pathhHandler;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.pathHandler.PathHandler;
+import java.util.List;
+
+public class StartPathHandlerCommand extends Command {
+  private PathHandler pathHandler;
+  private String[][] pathNames;
+  private List<Character> NodeNames;
+  private List<Integer> NodeLevels;
+  private Character startNode;
+  private boolean mirrorToProcessor;
+
+  public StartPathHandlerCommand(
+      PathHandler pathHandler,
+      String[][] pathNames,
+      List<Character> NodeNames,
+      List<Integer> NodeLevels,
+      Character startNode,
+      boolean mirrorToProcessor) {
+    this.pathHandler = pathHandler;
+    this.pathNames = pathNames;
+    this.NodeNames = NodeNames;
+    this.NodeLevels = NodeLevels;
+    this.startNode = startNode;
+    this.mirrorToProcessor = mirrorToProcessor;
+  }
+
+  public StartPathHandlerCommand(PathHandler pathHandler) {
+    this.pathHandler = pathHandler;
+  }
+
+  @Override
+  public void initialize() {
+    if (pathNames != null) {
+      pathHandler.setPathNames(pathNames);
+      pathHandler.setNodeNames(NodeNames);
+      pathHandler.setNodeLevels(NodeLevels);
+      pathHandler.setStartNode(startNode);
+      pathHandler.setMirrorToProcessor(mirrorToProcessor);
+    }
+    pathHandler.startPathHandler();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return pathHandler.isFinished();
+  }
+}
diff --git a/src/main/java/frc/robot/commands/robotState/AutoReefCycleCommand.java b/src/main/java/frc/robot/commands/robotState/AutoReefCycleCommand.java
index 3cc2fe40..f49e46d3 100644
--- a/src/main/java/frc/robot/commands/robotState/AutoReefCycleCommand.java
+++ b/src/main/java/frc/robot/commands/robotState/AutoReefCycleCommand.java
@@ -7,6 +7,7 @@
 import frc.robot.subsystems.drive.DriveSubsystem;
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
 import frc.robot.subsystems.robotState.RobotStateSubsystem;
+import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide;
 import frc.robot.subsystems.tagAlign.TagAlignSubsystem;
 
 public class AutoReefCycleCommand extends Command {
@@ -34,13 +35,15 @@ public AutoReefCycleCommand(
   public void initialize() {
     driveSubsystem.setIgnoreSticks(true);
     robotStateSubsystem.toReefAlign();
-    scoringCoral = robotStateSubsystem.hasCoral();
+    scoringCoral =
+        robotStateSubsystem.hasCoral()
+            && !(robotStateSubsystem.getGetAlgaeOnCycle()
+                && robotStateSubsystem.getScoreSide() == ScoreSide.RIGHT);
   }
 
   @Override
   public void end(boolean interrupted) {
     tagAlignSubsystem.terminate();
-    driveSubsystem.setIgnoreSticks(false);
   }
 
   @Override
diff --git a/src/main/java/frc/robot/commands/robotState/InterruptAutoCommand.java b/src/main/java/frc/robot/commands/robotState/InterruptAutoCommand.java
index d7790eb2..a75f09ba 100644
--- a/src/main/java/frc/robot/commands/robotState/InterruptAutoCommand.java
+++ b/src/main/java/frc/robot/commands/robotState/InterruptAutoCommand.java
@@ -3,9 +3,8 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.subsystems.robotState.RobotStateSubsystem;
 
-public class InterruptAutoCommand
-    extends InstantCommand { // TODO fix this command, it dose not work
-  RobotStateSubsystem robotState;
+public class InterruptAutoCommand extends InstantCommand {
+  private RobotStateSubsystem robotState;
 
   public InterruptAutoCommand(RobotStateSubsystem robotState) {
     this.robotState = robotState;
diff --git a/src/main/java/frc/robot/commands/robotState/ToggleAutoCommand.java b/src/main/java/frc/robot/commands/robotState/ToggleAutoPlacingCommand.java
similarity index 74%
rename from src/main/java/frc/robot/commands/robotState/ToggleAutoCommand.java
rename to src/main/java/frc/robot/commands/robotState/ToggleAutoPlacingCommand.java
index 2d0a90e1..9f8cae1f 100644
--- a/src/main/java/frc/robot/commands/robotState/ToggleAutoCommand.java
+++ b/src/main/java/frc/robot/commands/robotState/ToggleAutoPlacingCommand.java
@@ -3,10 +3,10 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.subsystems.robotState.RobotStateSubsystem;
 
-public class ToggleAutoCommand extends InstantCommand {
+public class ToggleAutoPlacingCommand extends InstantCommand {
   private RobotStateSubsystem robotStateSubsystem;
 
-  public ToggleAutoCommand(RobotStateSubsystem robotState) {
+  public ToggleAutoPlacingCommand(RobotStateSubsystem robotState) {
     this.robotStateSubsystem = robotState;
   }
 
diff --git a/src/main/java/frc/robot/commands/tagAlign/DriveTuningCommand.java b/src/main/java/frc/robot/commands/tagAlign/DriveTuningCommand.java
new file mode 100644
index 00000000..954dc5ba
--- /dev/null
+++ b/src/main/java/frc/robot/commands/tagAlign/DriveTuningCommand.java
@@ -0,0 +1,56 @@
+package frc.robot.commands.tagAlign;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.drive.DriveSubsystem;
+import frc.robot.subsystems.tagAlign.TagAlignSubsystem;
+import frc.robot.subsystems.tagAlign.TagAlignSubsystem.TagAlignStates;
+import java.util.function.DoubleSupplier;
+
+public class DriveTuningCommand extends Command {
+  private DriveSubsystem driveSubsystem;
+  private TagAlignSubsystem tagAlignSubsystem;
+  private Pose2d start;
+  private boolean driving = false;
+  private DoubleSupplier pSupplier;
+
+  public DriveTuningCommand(
+      DriveSubsystem driveSubsystem,
+      DoubleSupplier pSupplier,
+      TagAlignSubsystem tagAlignSubsystem) {
+    this.driveSubsystem = driveSubsystem;
+    this.tagAlignSubsystem = tagAlignSubsystem;
+    this.pSupplier = pSupplier;
+
+    addRequirements(driveSubsystem);
+  }
+
+  @Override
+  public void initialize() {
+    start = driveSubsystem.getPoseMeters();
+    driving = true;
+    // driveSubsystem.move(0, 1.5, 0, false);
+    tagAlignSubsystem.start(Alliance.Blue, true, false);
+  }
+
+  @Override
+  public void execute() {
+    // if (!driving
+    //     && driveSubsystem.getPoseMeters().getTranslation().getDistance(start.getTranslation())
+    //         > 1) {
+    //   driving = true;
+    //   tagAlignSubsystem.start(Alliance.Blue, true, false);
+    // }
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    driveSubsystem.stopDriving();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return driving && tagAlignSubsystem.getState() == TagAlignStates.DONE;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/tagAlign/TagAlignCommand.java b/src/main/java/frc/robot/commands/tagAlign/TagAlignCommand.java
index 354426c1..8acae4b1 100644
--- a/src/main/java/frc/robot/commands/tagAlign/TagAlignCommand.java
+++ b/src/main/java/frc/robot/commands/tagAlign/TagAlignCommand.java
@@ -16,7 +16,7 @@ public TagAlignCommand(TagAlignSubsystem tagAlignSubsystem, DriveSubsystem drive
 
   @Override
   public void initialize() {
-    tagAlignSubsystem.start(Alliance.Blue, true);
+    tagAlignSubsystem.start(Alliance.Blue, true, false);
   }
 
   @Override
diff --git a/src/main/java/frc/robot/commands/tagAlign/YawTuningCommand.java b/src/main/java/frc/robot/commands/tagAlign/YawTuningCommand.java
new file mode 100644
index 00000000..fe2ae787
--- /dev/null
+++ b/src/main/java/frc/robot/commands/tagAlign/YawTuningCommand.java
@@ -0,0 +1,64 @@
+package frc.robot.commands.tagAlign;
+
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.drive.DriveSubsystem;
+import java.util.function.DoubleSupplier;
+import org.littletonrobotics.junction.Logger;
+
+public class YawTuningCommand extends Command {
+  private DriveSubsystem driveSubsystem;
+  private Pose2d start;
+  private ProfiledPIDController driveOmega;
+  private boolean adjustYaw = false;
+  private DoubleSupplier pSupplier;
+
+  public YawTuningCommand(DriveSubsystem driveSubsystem, DoubleSupplier pSupplier) {
+    this.driveSubsystem = driveSubsystem;
+    this.pSupplier = pSupplier;
+
+    this.driveOmega = new ProfiledPIDController(8.0, 0, 0, new Constraints(1000, 1000));
+    this.driveOmega.enableContinuousInput(Math.toRadians(-180), Math.toRadians(180));
+
+    addRequirements(driveSubsystem);
+  }
+
+  @Override
+  public void initialize() {
+    this.driveOmega.setP(pSupplier.getAsDouble());
+    Logger.recordOutput("TagAlignSubsystem/OmegaKp", driveOmega.getP());
+    start = driveSubsystem.getPoseMeters();
+    adjustYaw = false;
+    driveSubsystem.move(0.5, 0, 0, true);
+  }
+
+  @Override
+  public void execute() {
+    if (!adjustYaw
+        && driveSubsystem.getPoseMeters().getTranslation().getDistance(start.getTranslation())
+            > 1) {
+      adjustYaw = true;
+      driveOmega.reset(driveSubsystem.getGyroRotation2d().getRadians());
+    }
+    if (adjustYaw) {
+      double vOmega =
+          driveOmega.calculate(
+              driveSubsystem.getGyroRotation2d().getRadians(), Math.toRadians(100));
+      Logger.recordOutput("TagAlignSubsystem/DriveOmegaError", driveOmega.getPositionError());
+      Logger.recordOutput("TagAlignSubsystem/OmegaSetpoint", driveOmega.getSetpoint().position);
+      driveSubsystem.move(0.5, 0, vOmega, true);
+    }
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    driveSubsystem.stopDriving();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return driveSubsystem.getPoseMeters().getTranslation().getDistance(start.getTranslation()) > 4;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/vision/SetVisionUpdatesCommand.java b/src/main/java/frc/robot/commands/vision/SetVisionUpdatesCommand.java
new file mode 100644
index 00000000..efd85558
--- /dev/null
+++ b/src/main/java/frc/robot/commands/vision/SetVisionUpdatesCommand.java
@@ -0,0 +1,19 @@
+package frc.robot.commands.vision;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.vision.VisionSubsystem;
+
+public class SetVisionUpdatesCommand extends InstantCommand {
+  VisionSubsystem visionSubsystem;
+  private boolean enable;
+
+  public SetVisionUpdatesCommand(VisionSubsystem visionSubsystem, boolean enable) {
+    this.visionSubsystem = visionSubsystem;
+    this.enable = enable;
+  }
+
+  @Override
+  public void initialize() {
+    visionSubsystem.setVisionUpdating(enable);
+  }
+}
diff --git a/src/main/java/frc/robot/constants/AlgaeConstants.java b/src/main/java/frc/robot/constants/AlgaeConstants.java
index 45b3c0f5..05c76dfe 100644
--- a/src/main/java/frc/robot/constants/AlgaeConstants.java
+++ b/src/main/java/frc/robot/constants/AlgaeConstants.java
@@ -32,7 +32,7 @@ public class AlgaeConstants {
   public static final double kIntakingSpeed = -1;
 
   public static final double kHasAlgaeVelThreshold = 10;
-  public static final double kHasAlgaeCounts = 1;
+  public static final double kHasAlgaeCounts = 2;
 
   // Example Talon FX Config
   public static TalonFXSConfiguration getFXConfig() {
diff --git a/src/main/java/frc/robot/constants/BiscuitConstants.java b/src/main/java/frc/robot/constants/BiscuitConstants.java
index 33f6c0d1..2b84e5bb 100644
--- a/src/main/java/frc/robot/constants/BiscuitConstants.java
+++ b/src/main/java/frc/robot/constants/BiscuitConstants.java
@@ -24,7 +24,7 @@
 
 public class BiscuitConstants {
 
-  public static final double kZero = .36;
+  public static final double kZero = .37;
   public static final double kTicksPerRot = 160;
   public static final int talonID = 25;
   public static final double kCloseEnough = 0.05;
@@ -36,6 +36,7 @@ public class BiscuitConstants {
   public static final Angle kStowSetpoint = Rotations.of(1.862);
   public static final Angle kFunnelSetpoint = kStowSetpoint;
   public static final Angle kPrestageSetpoint = kStowSetpoint;
+  public static final Angle kPrestageAlgaeSetpoint = Rotations.of(9.089);
 
   // Algae removal
   public static final Angle kL2AlgaeSetpoint = Rotations.of(24.104);
@@ -44,6 +45,8 @@ public class BiscuitConstants {
   public static final Angle kL2AlgaeRemovalSetpoint = kStowSetpoint;
   public static final Angle kL3AlgaeRemovalSetpoint = kStowSetpoint;
 
+  public static final double kTagAlignThreshold = 20.0;
+
   // Coral score
   public static final Angle kL1CoralSetpoint = kStowSetpoint;
   public static final Angle kL2CoralSetpoint = kStowSetpoint;
@@ -58,7 +61,7 @@ public class BiscuitConstants {
   // Algae scoring
   public static final Angle kProcessorSetpoint = Rotations.of(41.193);
   public static final Angle kBargeSetpoint = Rotations.of(12.3489);
-  public static final Angle kBargeBackwardSetpoint = Rotations.of(-12.3489);
+  public static final Angle kBargeBackwardSetpoint = Rotations.of(-12.3489); // 9.089
 
   // jogging
   public static final double kJogAmountUp = 10;
diff --git a/src/main/java/frc/robot/constants/ClimbConstants.java b/src/main/java/frc/robot/constants/ClimbConstants.java
index 07b22b1c..8255b126 100644
--- a/src/main/java/frc/robot/constants/ClimbConstants.java
+++ b/src/main/java/frc/robot/constants/ClimbConstants.java
@@ -1,3 +1,128 @@
 package frc.robot.constants;
 
-public class ClimbConstants {}
+import static edu.wpi.first.units.Units.Degrees;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.FeedbackConfigs;
+import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs;
+import com.ctre.phoenix6.configs.MotionMagicConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
+import com.ctre.phoenix6.signals.ForwardLimitSourceValue;
+import com.ctre.phoenix6.signals.ForwardLimitTypeValue;
+import com.ctre.phoenix6.signals.GravityTypeValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.ctre.phoenix6.signals.ReverseLimitSourceValue;
+import com.ctre.phoenix6.signals.ReverseLimitTypeValue;
+import edu.wpi.first.units.measure.Angle;
+// import edu.wpi.first.units.measure.AngularVelocity;
+
+public class ClimbConstants {
+  public static int kPivotArmFrontFxId = 45;
+  // public static int kPivotArmFollowFxId = 46;
+  public static int kCANcoderId = 46;
+  public static int kDeployServoId = 1;
+  public static int kRatchetServoId = 2;
+  public static int kCageAlignedDIOId = 11;
+
+  public static final double kPivotArmCloseEnough = 0.01; // fixme
+  public static final double kArmMaxFwd = 0;
+  public static final double kArmMaxRev = -0.29;
+  public static final Angle kArmZeroTicks = Degrees.of(1530);
+
+  // Deploy Servo
+  public static final double kPinDeployedPosition = 0.75;
+  public static final double kPinRetractedPosition = 0.18;
+
+  // Ratchet Servo
+  public static final double kRatchetEngagedPos = 0.0;
+  public static final double kRatchetDisengagedPos = 1.0;
+
+  // Climb positions
+  // public static final Angle kClimbCagePos = Rotations.of(); // will test
+  public static final Double kClimbRatchedEngage = -0.12;
+  public static final double kFullyClimbed = kArmMaxFwd;
+  public static final double kClimbOpenLoopSpeed = 4.0;
+
+  public static TalonFXConfiguration getPivotArmFxConfig() {
+    TalonFXConfiguration armFxConfig = new TalonFXConfiguration();
+
+    CurrentLimitsConfigs current =
+        new CurrentLimitsConfigs()
+            .withStatorCurrentLimit(10)
+            .withStatorCurrentLimitEnable(false)
+            .withSupplyCurrentLimit(30)
+            .withSupplyCurrentLowerLimit(30)
+            .withSupplyCurrentLowerTime(1)
+            .withSupplyCurrentLimitEnable(true);
+    armFxConfig.CurrentLimits = current;
+
+    HardwareLimitSwitchConfigs hwLimit =
+        new HardwareLimitSwitchConfigs()
+            .withForwardLimitAutosetPositionEnable(false)
+            .withForwardLimitEnable(false)
+            .withForwardLimitType(ForwardLimitTypeValue.NormallyOpen)
+            .withForwardLimitSource(ForwardLimitSourceValue.LimitSwitchPin)
+            .withReverseLimitAutosetPositionEnable(false)
+            .withReverseLimitEnable(false)
+            .withReverseLimitType(ReverseLimitTypeValue.NormallyOpen)
+            .withReverseLimitSource(ReverseLimitSourceValue.LimitSwitchPin);
+    armFxConfig.HardwareLimitSwitch = hwLimit;
+
+    SoftwareLimitSwitchConfigs swLimit =
+        new SoftwareLimitSwitchConfigs()
+            .withForwardSoftLimitEnable(true)
+            .withForwardSoftLimitThreshold(kArmMaxFwd)
+            .withReverseSoftLimitEnable(true)
+            .withReverseSoftLimitThreshold(kArmMaxRev);
+    armFxConfig.SoftwareLimitSwitch = swLimit;
+
+    Slot0Configs slot0 =
+        new Slot0Configs()
+            .withKP(0)
+            .withKI(0)
+            .withKD(0)
+            .withGravityType(GravityTypeValue.Elevator_Static)
+            .withKG(0)
+            .withKS(0)
+            .withKV(0)
+            .withKA(0);
+    armFxConfig.Slot0 = slot0;
+
+    MotionMagicConfigs motionMagic =
+        new MotionMagicConfigs()
+            .withMotionMagicAcceleration(0)
+            .withMotionMagicCruiseVelocity(0)
+            .withMotionMagicExpo_kA(0)
+            .withMotionMagicExpo_kV(0)
+            .withMotionMagicJerk(0);
+    armFxConfig.MotionMagic = motionMagic;
+
+    MotorOutputConfigs motorOut =
+        new MotorOutputConfigs()
+            .withDutyCycleNeutralDeadband(0.01)
+            .withNeutralMode(NeutralModeValue.Brake);
+    armFxConfig.MotorOutput = motorOut;
+
+    FeedbackConfigs feedbackConfigs =
+        new FeedbackConfigs()
+            .withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder)
+            .withFeedbackRemoteSensorID(kCANcoderId);
+    armFxConfig.Feedback = feedbackConfigs;
+
+    return armFxConfig;
+  }
+
+  public static CurrentLimitsConfigs getZeroCurrentLimit() {
+    CurrentLimitsConfigs config = new CurrentLimitsConfigs();
+    return config;
+  }
+
+  public static CurrentLimitsConfigs getRunCurrentLimit() {
+    CurrentLimitsConfigs config = new CurrentLimitsConfigs();
+    return config;
+  }
+}
diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java
index 9c54826b..049e4a8c 100644
--- a/src/main/java/frc/robot/constants/DriveConstants.java
+++ b/src/main/java/frc/robot/constants/DriveConstants.java
@@ -4,12 +4,21 @@
 import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
 import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration;
 import com.ctre.phoenix.sensors.SensorVelocityMeasPeriod;
+import com.ctre.phoenix6.configs.CommutationConfigs;
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.ExternalFeedbackConfigs;
+import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs;
+import com.ctre.phoenix6.configs.MotionMagicConfigs;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.Pigeon2Configuration;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.configs.TalonFXSConfiguration;
+import com.ctre.phoenix6.signals.ExternalFeedbackSensorSourceValue;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.MotorArrangementValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.ctre.phoenix6.signals.SensorPhaseValue;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -31,15 +40,15 @@ public class DriveConstants {
   public static final double kDriveGearRatio =
       (kDriveMotorOutputGear / kDriveInputGear) * (kBevelInputGear / kBevelOutputGear);
 
-  public static final double kWheelDiameterInches = 4.0;
-  public static final double kMaxSpeedMetersPerSecond = 3.384;
+  public static final double kWheelDiameterInches = 3.375;
+  public static final double kMaxSpeedMetersPerSecond = 3.782;
   public static final double kSpeedStillThreshold = 0.1; // meters per second
   public static final double kGyroRateStillThreshold = 10.0; // 25  5 degrees per second
   public static final double kGyroDifferentThreshold = 5.0; // 5 degrees
   public static final int kGyroDifferentCount = 3;
 
-  public static final double kRobotLength = 0.6223;
-  public static final double kRobotWidth = 0.6223;
+  public static final double kRobotLength = 0.61595;
+  public static final double kRobotWidth = 0.61595;
   public static final double kFieldMaxX = 17.526;
   public static final double kCenterLineX = 8.763;
 
@@ -53,6 +62,7 @@ public class DriveConstants {
   public static final double kPHolonomic = 3.0; // was 3
   public static final double kIHolonomic = 0.0000;
   public static final double kDHolonomic = 0.00; // kPHolonomic/100
+  public static double kFieldMaxY;
 
   public static Translation2d[] getWheelLocationMeters() {
     final double x = kRobotLength / 2.0; // front-back, was ROBOT_LENGTH
@@ -74,62 +84,64 @@ public static Translation2d[] getWheelLocationMeters() {
   public static final Pose2d kResetOdomPose =
       new Pose2d(new Translation2d(0.5, 3.62), Rotation2d.fromDegrees(67));
 
-  // public static TalonFXSConfiguration
-  //     getAzimuthTalonConfig() { // will be changed to a TalonFXConfiguration
-  //   // constructor sets encoder to Quad/CTRE_MagEncoder_Relative
-  //   TalonFXSConfiguration azimuthConfig = new TalonFXSConfiguration();
+  public static TalonFXSConfiguration getAzimuthFXSConfig() {
+    // constructor sets encoder to Quad/CTRE_MagEncoder_Relative
+    TalonFXSConfiguration azimuthConfig = new TalonFXSConfiguration();
 
-  //   HardwareLimitSwitchConfigs hardwareLimitSwitchConfigs = new HardwareLimitSwitchConfigs();
-  //   hardwareLimitSwitchConfigs.ForwardLimitEnable = false;
-  //   hardwareLimitSwitchConfigs.ReverseLimitEnable = false;
-  //   azimuthConfig.HardwareLimitSwitch = hardwareLimitSwitchConfigs;
+    HardwareLimitSwitchConfigs hardwareLimitSwitchConfigs = new HardwareLimitSwitchConfigs();
+    hardwareLimitSwitchConfigs.ForwardLimitEnable = false;
+    hardwareLimitSwitchConfigs.ReverseLimitEnable = false;
+    azimuthConfig.HardwareLimitSwitch = hardwareLimitSwitchConfigs;
 
-  //   CurrentLimitsConfigs currentConfig = new CurrentLimitsConfigs();
-  //   currentConfig.SupplyCurrentLowerTime = 0;
-  //   currentConfig.SupplyCurrentLowerLimit = 0;
+    CurrentLimitsConfigs currentConfig = new CurrentLimitsConfigs();
+    currentConfig.SupplyCurrentLimit = 20;
+    currentConfig.SupplyCurrentLowerTime = 20;
+    currentConfig.SupplyCurrentLowerLimit = 1;
 
-  //   currentConfig.SupplyCurrentLimit = 10;
-  //   currentConfig.SupplyCurrentLimitEnable = true;
+    currentConfig.SupplyCurrentLimitEnable = true;
+    currentConfig.StatorCurrentLimitEnable = false;
 
-  //   azimuthConfig.CurrentLimits = currentConfig;
+    azimuthConfig.CurrentLimits = currentConfig;
 
-  //   Slot0Configs slot0Config = new Slot0Configs();
-  //   slot0Config.kP = 360.35;
-  //   slot0Config.kI = 0.0;
-  //   slot0Config.kD = 3.604;
+    Slot0Configs slot0Config = new Slot0Configs();
+    slot0Config.kP = 300;
+    slot0Config.kI = 0.0;
+    slot0Config.kD = 3;
+    slot0Config.kV = 4.3;
 
-  //   azimuthConfig.Slot0 = slot0Config;
+    azimuthConfig.Slot0 = slot0Config;
 
-  //   ExternalFeedbackConfigs externalFeedbackConfigs = new ExternalFeedbackConfigs();
-  //   externalFeedbackConfigs.VelocityFilterTimeConstant = 0.1;
-  //   externalFeedbackConfigs.ExternalFeedbackSensorSource =
-  //       ExternalFeedbackSensorSourceValue.PulseWidth;
-  //   azimuthConfig.ExternalFeedback = externalFeedbackConfigs;
+    ExternalFeedbackConfigs externalFeedbackConfigs = new ExternalFeedbackConfigs();
+    externalFeedbackConfigs.VelocityFilterTimeConstant = 0.02; // ?
+    externalFeedbackConfigs.ExternalFeedbackSensorSource =
+        ExternalFeedbackSensorSourceValue.Quadrature;
+    externalFeedbackConfigs.SensorPhase = SensorPhaseValue.Opposed;
+    azimuthConfig.ExternalFeedback = externalFeedbackConfigs;
 
-  //   VoltageConfigs voltageConfig = new VoltageConfigs();
-  //   voltageConfig.SupplyVoltageTimeConstant = 3.2; // FIXME, seems very long
-  //   azimuthConfig.Voltage = voltageConfig;
+    // VoltageConfigs voltageConfig = new VoltageConfigs();
+    // voltageConfig.SupplyVoltageTimeConstant = 3.2; // FIXME, seems very long
+    // azimuthConfig.Voltage = voltageConfig;
 
-  //   MotionMagicConfigs motionConfig = new MotionMagicConfigs();
-  //   motionConfig.MotionMagicCruiseVelocity = 800;
-  //   motionConfig.MotionMagicAcceleration = 10_000;
-  //   azimuthConfig.MotionMagic = motionConfig;
+    MotionMagicConfigs motionConfig = new MotionMagicConfigs();
+    motionConfig.MotionMagicCruiseVelocity = 2;
+    motionConfig.MotionMagicAcceleration = 20;
+    azimuthConfig.MotionMagic = motionConfig;
 
-  //   MotorOutputConfigs motorConfigs = new MotorOutputConfigs();
-  //   motorConfigs.DutyCycleNeutralDeadband = 0.04;
-  //   motorConfigs.NeutralMode = NeutralModeValue.Coast;
-  //   azimuthConfig.MotorOutput = motorConfigs;
+    MotorOutputConfigs motorConfigs = new MotorOutputConfigs();
+    // motorConfigs.DutyCycleNeutralDeadband = 0.04;
+    motorConfigs.NeutralMode = NeutralModeValue.Coast;
+    motorConfigs.Inverted = InvertedValue.Clockwise_Positive;
+    azimuthConfig.MotorOutput = motorConfigs;
 
-  //   CommutationConfigs commutationConfigs = new CommutationConfigs();
-  //   commutationConfigs.MotorArrangement = MotorArrangementValue.Minion_JST;
+    CommutationConfigs commutationConfigs = new CommutationConfigs();
+    commutationConfigs.MotorArrangement = MotorArrangementValue.Minion_JST;
 
-  //   azimuthConfig.Commutation = commutationConfigs;
+    azimuthConfig.Commutation = commutationConfigs;
 
-  //   return azimuthConfig;
-  // }
+    return azimuthConfig;
+  }
 
-  public static TalonSRXConfiguration
-      getAzimuthTalonConfig() { // will be changed to a TalonFXConfiguration
+  public static TalonSRXConfiguration getAzimuthTalonConfig() {
     // constructor sets encoder to Quad/CTRE_MagEncoder_Relative
     TalonSRXConfiguration azimuthConfig = new TalonSRXConfiguration();
 
@@ -166,7 +178,8 @@ public static TalonFXConfiguration getDriveTalonConfig() {
 
     CurrentLimitsConfigs currentConfig = new CurrentLimitsConfigs();
     currentConfig.SupplyCurrentLimit = 60;
-
+    currentConfig.SupplyCurrentLowerLimit = 60;
+    currentConfig.SupplyCurrentLowerTime = 1.0;
     currentConfig.StatorCurrentLimit = 140;
 
     currentConfig.SupplyCurrentLimitEnable = true;
@@ -175,10 +188,13 @@ public static TalonFXConfiguration getDriveTalonConfig() {
     driveConfig.CurrentLimits = currentConfig;
 
     Slot0Configs slot0Config = new Slot0Configs();
-    slot0Config.kP = 0.5; // 0.16 using phoenix 6 migrate
-    slot0Config.kI = 0.5; // 0.0002 using phoenix 6 migrate
+    slot0Config.kP = 0.2;
+    slot0Config.kI = 0.0;
     slot0Config.kD = 0.0;
-    slot0Config.kV = 0.12; // 0.047 using phoenix 6 migrate
+    slot0Config.kV = 0.117;
+    slot0Config.kS = 0.0;
+    slot0Config.kA = 0.0;
+    slot0Config.kG = 0.0;
     driveConfig.Slot0 = slot0Config;
 
     MotorOutputConfigs motorConfigs = new MotorOutputConfigs();
@@ -198,9 +214,9 @@ public static Pigeon2Configuration getPigeon2Configuration() {
     config.MountPose.MountPoseRoll = 0.0;
     config.MountPose.MountPosePitch = 0.0;
 
-    config.GyroTrim.GyroScalarX = 0.0;
-    config.GyroTrim.GyroScalarY = 0.0;
-    config.GyroTrim.GyroScalarZ = -4.55;
+    config.GyroTrim.GyroScalarX = -1.2;
+    config.GyroTrim.GyroScalarY = 4.8;
+    config.GyroTrim.GyroScalarZ = -2.9;
 
     return config;
   }
diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java
index aef896e5..6053181b 100644
--- a/src/main/java/frc/robot/constants/ElevatorConstants.java
+++ b/src/main/java/frc/robot/constants/ElevatorConstants.java
@@ -38,7 +38,7 @@ public class ElevatorConstants {
 
   // Setpoints
   // Idle
-  public static final Angle kFunnelSetpoint = Rotations.of(2.03125); // was 2.40430
+  public static final Angle kFunnelSetpoint = Rotations.of(0.3676757); // was 2.03125
   public static final Angle kStowSetpoint = kFunnelSetpoint;
 
   // Algae removal
diff --git a/src/main/java/frc/robot/constants/LEDConstants.java b/src/main/java/frc/robot/constants/LEDConstants.java
index 9a082f2c..3a07509a 100644
--- a/src/main/java/frc/robot/constants/LEDConstants.java
+++ b/src/main/java/frc/robot/constants/LEDConstants.java
@@ -1,3 +1,63 @@
 package frc.robot.constants;
 
-public class LEDConstants {}
+import edu.wpi.first.wpilibj.util.Color;
+
+public class LEDConstants {
+  public static final Color invertRedGreen(Color color) {
+    return new Color(color.green, color.red, color.blue);
+  }
+
+  public static final int kLEDPort = 0;
+  // Auto/Operator = 27
+  // other = 21.5
+  public static final int kTotalStripLength = 51;
+  public static final int kBottomStripLength = 34;
+  public static final int kTopStripLength = 17;
+  public static final int kTopFirstIndex = kBottomStripLength;
+  public static final int kAlgeaEnd = kBottomStripLength / 3 + 1;
+
+  public static final int kLevelStart = kTopFirstIndex;
+  public static final int kPlaceStart = kTopFirstIndex + kTopStripLength / 3;
+  public static final int kGetAlgeaStart = kTopFirstIndex + kTopStripLength / 3 * 2;
+  public static final int kAutoPlacingStart = kBottomStripLength / 3 * 2 + 1;
+
+  public static final Color kAlmostBlack = new Color(0, 0, 1);
+
+  // Normal LED Colors
+  public static final Color kHasAlgea = invertRedGreen(Color.kTeal);
+  public static final Color kNotHasAlgea = kAlmostBlack;
+
+  public static final Color kCoralNotInRobot = invertRedGreen(Color.kOrange);
+  public static final Color kCoralInFunnel = invertRedGreen(Color.kPurple);
+  public static final Color kCoralInRobot = invertRedGreen(Color.kAntiqueWhite);
+
+  public static final Color kL1 = invertRedGreen(Color.kRed);
+  public static final Color kL2 = invertRedGreen(Color.kLightYellow);
+  public static final Color kL3 = invertRedGreen(Color.kGreen);
+  public static final Color kL4 = invertRedGreen(Color.kBlue);
+
+  public static final Color kManual = kAlmostBlack;
+  public static final Color kRight = invertRedGreen(Color.kOrangeRed);
+  public static final Color kLeft = invertRedGreen(Color.kPurple);
+
+  public static final Color kGetAlgea = invertRedGreen(Color.kTeal);
+  public static final Color kNotGetAlgea = kAlmostBlack;
+
+  public static final Color kCurrentLimiting = invertRedGreen(Color.kRed);
+
+  public static final Color kAutoPlacing = invertRedGreen(Color.kBlue);
+
+  // Climb LED Colors
+  public static final Color kWaitingForCage = invertRedGreen(Color.kRed);
+  public static final Color kHasCage = invertRedGreen(Color.kGreen);
+  public static final Color kClimbed = invertRedGreen(Color.kGoldenrod);
+  public static final Color[] kGameColors = {
+    invertRedGreen(Color.kBlack), // a dummy color
+    invertRedGreen(Color.kDarkRed),
+    invertRedGreen(Color.kDarkGreen),
+    invertRedGreen(Color.kDarkBlue),
+    invertRedGreen(Color.kPink),
+    invertRedGreen(Color.kLightGreen),
+    invertRedGreen(Color.kLightBlue)
+  };
+}
diff --git a/src/main/java/frc/robot/constants/PathHandlerConstants.java b/src/main/java/frc/robot/constants/PathHandlerConstants.java
new file mode 100644
index 00000000..dcf04ee5
--- /dev/null
+++ b/src/main/java/frc/robot/constants/PathHandlerConstants.java
@@ -0,0 +1,34 @@
+package frc.robot.constants;
+
+public class PathHandlerConstants {
+  public static final String[][] kpathNames = {
+    {
+      "fetchToA",
+      "fetchToB",
+      "FiveMeterTestPath",
+      "FiveMeterTestPath",
+      "FiveMeterTestPath",
+      "fetchToF",
+      "fetchToG",
+      "fetchToH",
+      "fetchToI",
+      "fetchToJ",
+      "fetchToK",
+      "fetchToL"
+    },
+    {
+      "ATofetch",
+      "BTofetch",
+      "FiveMeterTestPath",
+      "FiveMeterTestPath",
+      "FiveMeterTestPath",
+      "FiveMeterTestPath",
+      "GTofetch",
+      "HTofetch",
+      "ITofetch",
+      "JTofetch",
+      "KTofetch",
+      "LTofetch"
+    }
+  };
+}
diff --git a/src/main/java/frc/robot/constants/RobotStateConstants.java b/src/main/java/frc/robot/constants/RobotStateConstants.java
index 1ddc8768..fa3cd957 100644
--- a/src/main/java/frc/robot/constants/RobotStateConstants.java
+++ b/src/main/java/frc/robot/constants/RobotStateConstants.java
@@ -7,5 +7,8 @@ public class RobotStateConstants {
   public static final double kBlueBargeSafeX = 7.6;
   public static final double kRedBargeSafeX = DriveConstants.kFieldMaxX - kBlueBargeSafeX;
 
-  public static final double kCoralEjectTimer = 0.5;
+  public static final double kCoralEjectTimer = 0.75;
+  public static final double kAlgaeEjectTimer = 0.5;
+
+  public static final double kProcessorStowRadius = 0.5;
 }
diff --git a/src/main/java/frc/robot/constants/TagServoingConstants.java b/src/main/java/frc/robot/constants/TagServoingConstants.java
index 2a56807e..359876ff 100644
--- a/src/main/java/frc/robot/constants/TagServoingConstants.java
+++ b/src/main/java/frc/robot/constants/TagServoingConstants.java
@@ -1,6 +1,7 @@
 package frc.robot.constants;
 
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 import edu.wpi.first.math.util.Units;
 
 public class TagServoingConstants {
@@ -10,13 +11,22 @@ public class TagServoingConstants {
 
   // Offsets
   public static final double kLeftCamOffset = VisionConstants.kCam1Pose.getY();
-  public static final double kRightCamOffset = VisionConstants.kCam2Pose.getY();
+  public static final double kRightCamOffset = VisionConstants.kCam3Pose.getY();
 
   // Targets
   public static final double kHorizontalTarget = 800;
   public static final double kLeftCamDiagTarget = 1180;
   public static final double kRightCamDiagTarget = 985;
 
+  // Constraints
+  public static final Constraints driveXConstraints = new Constraints(2, 100000);
+  public static final Constraints driveYConstraints = new Constraints(2, 100000);
+  public static final Constraints driveOmegaConstraints = new Constraints(10000, 20000);
+
+  public static final Constraints alignXConstraints = new Constraints(2, 100000);
+  public static final Constraints alignYConstraints = new Constraints(2, 100000);
+  // public static final Constraints alignOmegaConstraints = new Constraints(10000, 20000);
+
   public static final double[] kAngleTarget = {
     Units.degreesToRadians(0),
     Units.degreesToRadians(60),
@@ -32,20 +42,34 @@ public class TagServoingConstants {
 
   // Tag align
   public static final double kHorizontalCloseEnough = 20;
-  public static final double kAngleCloseEnough = Units.degreesToRadians(3.0);
+  public static final double kAngleCloseEnough = Units.degreesToRadians(1.0);
   public static final double kDiagCloseEnough = 20;
   public static final double kNoUpdateMicrosec = 500_000;
 
   // Drive
-  public static final double kInitialDriveRadius = 1.7; // 1.5
-  public static final double kStopXDriveRadius =
-      kInitialDriveRadius; // Should be closer to reef than target pose
-  public static final double kMinStopXDriveRadius = 1.55; // 1.35
-  public static final double kDriveCloseEnough = 0.1;
-  public static final double kMinVelX = 0.85;
+  public static final double kCoralInitialDriveRadius = 1.7; // 0.36 away from reef wall
+  public static final double kCoralAlignRadius = 1.34; // 1.293823 is perfectly against the reef
+  // public static final double kCoralStopXDriveRadius =
+  //     kCoralInitialDriveRadius; // Should be closer to reef than target pose
+  public static final double kAlgaeInitialDriveRadius = 1.6;
+  public static final double kAlgaeAlignRadius = 1.34;
+  public static final double kAlgaeStopXDriveRadius =
+      kAlgaeInitialDriveRadius; // Should be closer to reef than target pose
+  // public static final double kMinVelX = 0.85;
+
+  // End conditions
+  public static final double kInitialCloseEnough = 0.1;
+  public static final double kCoralDriveXCloseEnough = 0.03;
+  public static final double kCoralDriveYCloseEnough = 0.025;
+  public static final double kAlgaeDriveXCloseEnough = 0.03;
+  public static final double kAlgaeDriveYCloseEnough = 0.025;
+
+  public static final double kEndDriveCurrentThreshold = 25;
+  public static final int kEndCountThreshold = 1;
+  public static final double kEndVelThreshold = 2;
 
   // Reef
-  public static final Translation2d kBlueReefPose = new Translation2d(4.524, 4.033);
+  public static final Translation2d kBlueReefPose = new Translation2d(4.489323, 4.0259);
 
-  public static final Translation2d kRedReefPose = new Translation2d(13.084, 4.033);
+  public static final Translation2d kRedReefPose = new Translation2d(13.058902, 4.0259);
 }
diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java
index 1e6a8065..8369db74 100644
--- a/src/main/java/frc/robot/constants/VisionConstants.java
+++ b/src/main/java/frc/robot/constants/VisionConstants.java
@@ -61,8 +61,9 @@ public final class VisionConstants {
   public static final double FOV75YUYVSingleTagCoeff = 22.0 / 100.0;
   public static final double FOV75YUYVBaseTrust = 3.0;
 
-  // Gyro error scaling
-  public static final double kYawErrorThreshold = Units.degreesToRadians(45);
+  // Gyro error
+  public static final double kYawErrorThreshold = Units.degreesToRadians(30);
+  public static final double kCamErrorZThreshold = 0.3;
 
   // Constants for cameras
   public static final int kNumCams = 5;
@@ -94,22 +95,21 @@ public final class VisionConstants {
   public static final int kCircularBufferSize = 1000;
   // Poses
   public static final Pose3d kCam1Pose =
-      new Pose3d(new Translation3d(0.28, 0.02, 0.30), new Rotation3d());
-
+      new Pose3d(new Translation3d(0.305, 0.025, 0.311), new Rotation3d());
   public static final Pose3d kCam2Pose =
       new Pose3d(
-          new Translation3d(-0.21, -0.31, 0.44),
-          new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(0.0)));
+          new Translation3d(0.236, 0.108, 0.932),
+          new Rotation3d(0, Units.degreesToRadians(10), Units.degreesToRadians(22.5)));
   public static final Pose3d kCam3Pose =
-      new Pose3d(new Translation3d(0.09, -0.31, 0.36), new Rotation3d());
+      new Pose3d(new Translation3d(0.133, -0.305, 0.311), new Rotation3d());
   public static final Pose3d kCam4Pose =
       new Pose3d(
-          new Translation3d(-0.22, -0.335, 0.50),
-          new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(138.0)));
+          new Translation3d(0.236, -.038, 0.932),
+          new Rotation3d(0, Units.degreesToRadians(10), Units.degreesToRadians(-22.5)));
   public static final Pose3d kCam5Pose =
       new Pose3d(
-          new Translation3d(-0.22, -0.335, 0.50),
-          new Rotation3d(0, Units.degreesToRadians(20.0), Units.degreesToRadians(138.0)));
+          new Translation3d(-0.229, 0.073, 0.934),
+          new Rotation3d(0, Units.degreesToRadians(10), Units.degreesToRadians(180)));
   // Increase these numbers to trust sensor readings from encoders and gyros less. This matrix is
   // in the form [theta], with units in radians.
   public static Matrix<N1, N1> kLocalMeasurementStdDevs =
diff --git a/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java b/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java
index c8acc2dd..3fb167ea 100644
--- a/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java
+++ b/src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java
@@ -57,7 +57,7 @@ public BiscuitIOFX() {
     rawQuadrature = talon.getRawQuadraturePosition();
     rawQuadrature.setUpdateFrequency(20);
     rawPulseWidth = talon.getRawPulseWidthPosition();
-    rawPulseWidth.setUpdateFrequency(20);
+    rawPulseWidth.setUpdateFrequency(200);
     zero();
   }
 
@@ -85,7 +85,7 @@ public void zero() {
     double pos = MathUtil.inputModulus(rawPulseWidth.getValueAsDouble(), 0, 1);
     double setPos = BiscuitConstants.kTicksPerRot * (BiscuitConstants.kZero - pos);
     talon.setPosition(setPos);
-    logger.info("set Biscuit position to " + setPos);
+    logger.info("set Biscuit position to " + setPos + " abs pos: " + pos);
     didZero = true;
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbIO.java b/src/main/java/frc/robot/subsystems/climb/ClimbIO.java
new file mode 100644
index 00000000..129e1caf
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/climb/ClimbIO.java
@@ -0,0 +1,39 @@
+package frc.robot.subsystems.climb;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import edu.wpi.first.units.measure.Angle;
+import org.littletonrobotics.junction.AutoLog;
+import org.strykeforce.telemetry.TelemetryService;
+
+public interface ClimbIO {
+
+  @AutoLog
+  static class ClimbIOInputs {
+    public double position = 0.0;
+    public double velocity = 0.0;
+    public double ratchetServoPosition = 0.0;
+    public double pinServoPosition = 0.0;
+  }
+
+  public default void setPosition(Angle position) {}
+
+  public default void setRatchetServoPosition(
+      double position) {} // needed? since it's double, not angle
+
+  public default void setPinServoPosition(
+      double position) {} // may make a method to stick these both into one method
+
+  public default void updateInputs(ClimbIOInputs inputs) {}
+
+  public default void zero() {}
+
+  public default void registerWith(TelemetryService telemetryService) {}
+
+  public default void setSoftLimitsEnabled(boolean enable) {}
+
+  public default void setCurrentLimit(CurrentLimitsConfigs config) {}
+
+  public default void setPercent(double percent) {}
+
+  public default void setCoastMode(boolean coast) {}
+}
diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbIOServoFX.java b/src/main/java/frc/robot/subsystems/climb/ClimbIOServoFX.java
new file mode 100644
index 00000000..571044c1
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/climb/ClimbIOServoFX.java
@@ -0,0 +1,143 @@
+package frc.robot.subsystems.climb;
+
+import static edu.wpi.first.units.Units.Rotations;
+
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.configs.TalonFXConfigurator;
+import com.ctre.phoenix6.controls.MotionMagicDutyCycle;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.wpilibj.Servo;
+import frc.robot.constants.ClimbConstants;
+import frc.robot.subsystems.climb.ClimbIO.ClimbIOInputs;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+import org.strykeforce.telemetry.TelemetryService;
+
+public class ClimbIOServoFX implements ClimbIO {
+  // will prob need healthchecks eventually
+  private Logger logger;
+  private TalonFX talonFxPivotArmFront;
+  private CANcoder canCoder;
+  private Servo ratchetServo;
+  private Servo pinServo;
+
+  private final Angle absPivotArmFrontSensorInitial;
+  private Angle relSetpointOffset;
+  private Angle pivotArmSetpoint;
+
+  TalonFXConfigurator configuratorFront;
+  private MotionMagicDutyCycle positionRequestMain =
+      new MotionMagicDutyCycle(0).withEnableFOC(false).withFeedForward(0).withSlot(0);
+  private VoltageOut voltageRequest = new VoltageOut(0).withEnableFOC(false);
+
+  StatusSignal<Angle> currPosition;
+
+  public ClimbIOServoFX() {
+    logger = LoggerFactory.getLogger(this.getClass());
+    talonFxPivotArmFront = new TalonFX(ClimbConstants.kPivotArmFrontFxId);
+    ratchetServo = new Servo(ClimbConstants.kRatchetServoId);
+    canCoder = new CANcoder(ClimbConstants.kCANcoderId);
+    pinServo = new Servo(ClimbConstants.kDeployServoId);
+
+    absPivotArmFrontSensorInitial =
+        talonFxPivotArmFront.getPosition().getValue(); // only for logging
+    configuratorFront = talonFxPivotArmFront.getConfigurator();
+    configuratorFront.apply(new TalonFXConfiguration());
+    configuratorFront.apply(ClimbConstants.getPivotArmFxConfig());
+
+    currPosition = talonFxPivotArmFront.getPosition();
+  }
+
+  // @Override
+  public String getName() {
+    return "Climb";
+  }
+
+  @Override
+  public void setPosition(Angle position) {
+    pivotArmSetpoint = position.plus(relSetpointOffset);
+
+    logger.info("Setting position to {} rotations", pivotArmSetpoint.in(Rotations));
+
+    talonFxPivotArmFront.setControl(positionRequestMain.withPosition(pivotArmSetpoint));
+  }
+
+  @Override
+  public void setRatchetServoPosition(double position) {
+    // ratchetServo.setPosition(position);
+    ratchetServo.set(position);
+    // could be wrong but I'm assuming set rather than setPosition
+  }
+
+  @Override
+  public void setPinServoPosition(double position) {
+    // pinServo.setPosition(position);
+    pinServo.set(position);
+  }
+
+  @Override
+  public void updateInputs(ClimbIOInputs inputs) {
+    inputs.position = currPosition.refresh().getValueAsDouble();
+    inputs.ratchetServoPosition = ratchetServo.getPosition();
+    inputs.pinServoPosition = pinServo.getPosition();
+  }
+
+  @Override
+  public void zero() {
+    relSetpointOffset = ClimbConstants.kArmZeroTicks;
+    /*
+    logger.info(
+        "Abs: {}, Zero Pos: {}, Offset: {}"
+        absPivotArmFrontSensorInitial,
+        ClimbConstants.kZeroTicks,
+        absPivotArmFrontSensorInitial.minus(ClimbConstants.kZeroTicks));
+        setPosition(0.0);
+
+    );
+    */
+    talonFxPivotArmFront.setPosition(0.0);
+  }
+
+  @Override
+  public void setSoftLimitsEnabled(boolean enable) {
+    configuratorFront.apply(
+        ClimbConstants.getPivotArmFxConfig()
+            .SoftwareLimitSwitch
+            .withForwardSoftLimitEnable(enable)
+            .withReverseSoftLimitEnable(enable)); // fixme
+    // configuratorBack.apply(ClimbConstants.getPivotArmFxConfig().SoftwareLimitSwitch.withForwardSoftLimitEnable(enable)
+    // .withReverseSoftLimitEnable());
+  }
+
+  @Override
+  public void setCurrentLimit(CurrentLimitsConfigs config) {
+    configuratorFront.apply(config);
+  }
+
+  @Override
+  public void setPercent(double percent) {
+    talonFxPivotArmFront.setControl(voltageRequest.withOutput(percent));
+  }
+
+  @Override
+  public void setCoastMode(boolean coast) {
+    configuratorFront.apply(
+        ClimbConstants.getPivotArmFxConfig()
+            .MotorOutput
+            .withNeutralMode(coast ? NeutralModeValue.Coast : NeutralModeValue.Brake));
+  }
+
+  public void goToZero() {}
+
+  @Override
+  public void registerWith(TelemetryService telemetryService) {
+    telemetryService.register(talonFxPivotArmFront, false);
+    telemetryService.register(canCoder);
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java
index 85fbd567..8c6e4ec4 100644
--- a/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java
@@ -1,19 +1,186 @@
+// still have a non-negligible amount of stuff to do on the state machine
+
 package frc.robot.subsystems.climb;
 
-public class ClimbSubsystem {
+import static edu.wpi.first.units.Units.Rotations;
+
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.constants.ClimbConstants;
+import frc.robot.standards.ClosedLoopPosSubsystem;
+import frc.robot.subsystems.climb.ClimbSubsystem.ClimbState;
+import java.util.Set;
+import org.littletonrobotics.junction.Logger;
+import org.strykeforce.telemetry.TelemetryService;
+import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
+import org.strykeforce.telemetry.measurable.Measure;
+
+public class ClimbSubsystem extends MeasurableSubsystem implements ClosedLoopPosSubsystem {
+  private final ClimbIO io;
+
+  private ClimbIOInputsAutoLogged climbInputs = new ClimbIOInputsAutoLogged();
+
+  private boolean isRotatingToPosition = false;
+  private boolean isClimbZeroed = false;
+  private boolean isRatchetOn = false;
+  private boolean inPosition = false;
+  private boolean isBeamBroken = false;
+  private boolean isPinDeployed = false;
+  private boolean proceedToClimb = false;
+  private int climbZeroStableCounts; // idk what this means
+  // more
+
+  private double setpoints = 0.0;
+  private ClimbState curState = ClimbState.INIT;
+  private Timer hangTimer = new Timer();
+
+  public ClimbSubsystem(ClimbIO io) {
+    this.io = io;
+    enableRatchet(false);
+    deployClimb(false);
+    zero();
+  }
+
+  @Override
+  public Angle getPosition() {
+    return Rotations.of(climbInputs.position);
+  }
+
+  @Override
+  public void setPosition(Angle position) {
+    io.setPosition(position);
+    setpoints = position.in(Rotations);
+  }
 
+  @Override
   public boolean isFinished() {
-    // TODO Auto-generated method stub
-    throw new UnsupportedOperationException("Unimplemented method 'isFinished'");
+    return (Math.abs(setpoints - climbInputs.position) < ClimbConstants.kPivotArmCloseEnough);
+  }
+
+  @Override
+  public void zero() {
+    io.zero();
+  }
+
+  private void setClimbDebugMsg(String msg) {
+    Logger.recordOutput("Climb/DebugMsg", msg);
+  }
+
+  public void toggleClimbRatchet() {
+    enableRatchet(!isRatchetOn);
+  }
+
+  public void enableRatchet(boolean enable) {
+    if (enable) {
+      io.setRatchetServoPosition(ClimbConstants.kRatchetEngagedPos); // fixme
+      setClimbDebugMsg("Engaging Ratchet");
+      isRatchetOn = true;
+    } else {
+      io.setRatchetServoPosition(ClimbConstants.kRatchetDisengagedPos);
+      setClimbDebugMsg("Disengaging Ratchet");
+      isRatchetOn = false;
+    }
+  }
+
+  public double getSetpoints() {
+    return setpoints;
+  }
+
+  public ClimbState getState() {
+    return curState;
   }
 
   public void prepClimb() {
-    // TODO Auto-generated method stub
-    throw new UnsupportedOperationException("Unimplemented method 'prepClimb'");
+    enableRatchet(false);
+    deployClimb(true);
+    setState(ClimbState.PREPPED);
   }
 
   public void climb() {
-    // TODO Auto-generated method stub
-    throw new UnsupportedOperationException("Unimplemented method 'climb'");
+    if (curState == ClimbState.PREPPED) {
+      // setPosition(ClimbConstants.kClimbCagePos);
+      io.setPercent(ClimbConstants.kClimbOpenLoopSpeed);
+      setState(ClimbState.CLIMBING);
+    }
+  }
+
+  private void deployClimb(boolean enable) {
+    if (enable) {
+      io.setPinServoPosition(ClimbConstants.kPinDeployedPosition);
+      setClimbDebugMsg("Deploying Pin");
+      isPinDeployed = true;
+    } else {
+      io.setPinServoPosition(ClimbConstants.kPinRetractedPosition);
+      setClimbDebugMsg("Retracting Pin");
+      isPinDeployed = true;
+    }
+  }
+
+  public void toggleDeployState() {
+    deployClimb(!isPinDeployed);
+  }
+
+  public boolean isClimbFinished() {
+    return curState == ClimbState.CLIMBED;
+  }
+
+  public void stow() {}
+
+  private void setState(ClimbState state) {
+    setClimbDebugMsg(curState + "->" + state);
+    setpoints = ClimbConstants.kFullyClimbed;
+    curState = state;
+  }
+
+  @Override
+  public void periodic() {
+    // Read Inputs
+    io.updateInputs(climbInputs);
+    Logger.processInputs(getName(), climbInputs);
+    // climbInputs.ratchetPosition = ratchetServo.getPosition();
+
+    // State Machine
+    switch (curState) {
+      case INIT:
+        break;
+      case PREPPED:
+        break;
+      case CLIMBING:
+        if (climbInputs.position >= ClimbConstants.kClimbRatchedEngage && !isRatchetOn) {
+          enableRatchet(true);
+        }
+        if (isFinished()) {
+          io.setCoastMode(true);
+          setState(ClimbState.CLIMBED);
+          io.setPercent(0.0);
+        }
+        break;
+      default:
+        break;
+    }
+
+    // Log Outputs
+    Logger.recordOutput("Climb/curState", curState);
+    Logger.recordOutput("Climb/setpoint", setpoints);
+  }
+
+  @Override
+  public void registerWith(TelemetryService telemetryService) {
+    super.registerWith(telemetryService);
+    io.registerWith(telemetryService);
+  }
+
+  @Override
+  public Set<Measure> getMeasures() {
+    return Set.of();
+  }
+
+  public enum ClimbState {
+    INIT,
+    STOWED,
+    PREP_CLIMBING,
+    PREPPED,
+    CLIMBING,
+    CLIMBED
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
index f5bdf66e..cd7e1a00 100644
--- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
@@ -83,6 +83,14 @@ public DriveSubsystem(SwerveIO io) {
     holonomicController.setEnabled(true);
   }
 
+  public boolean hasZeroed() {
+    return inputs.didZero;
+  }
+
+  public void zeroModules() {
+    io.zeroModules();
+  }
+
   // Open-Loop Swerve Movements
   public void drive(double vXmps, double vYmps, double vOmegaRadps) {
     if (!ignoreSticks) {
@@ -90,6 +98,11 @@ public void drive(double vXmps, double vYmps, double vOmegaRadps) {
     }
   }
 
+  public void stopDriving() {
+    this.move(0, 0, 0, false);
+    io.drive(0, 0, 0, false);
+  }
+
   public void setAzimuthVel(double vel) {
     io.setAzimuthVel(vel);
   }
@@ -108,7 +121,7 @@ public void calculateController(SwerveSample desiredState) {
     holoContInput = desiredState;
     double xFF = desiredState.vx;
     double yFF = desiredState.vy;
-    double rotationFF = desiredState.heading;
+    double rotationFF = desiredState.omega;
 
     Pose2d pose = inputs.poseMeters;
     double xFeedback = xController.calculate(pose.getX(), desiredState.x);
@@ -167,6 +180,7 @@ public void setRobotStateSubsystem(RobotStateSubsystem robotStateSubsystem) {
   }
 
   public void setIgnoreSticks(boolean ignore) {
+    org.littletonrobotics.junction.Logger.recordOutput("DriveSubsystem/Ignoring Sticks", ignore);
     this.ignoreSticks = ignore;
   }
 
@@ -234,6 +248,10 @@ public ChassisSpeeds getFieldRelSpeed() {
     return inputs.fieldRelSpeed;
   }
 
+  public ChassisSpeeds getRobotRelSpeed() {
+    return inputs.robotRelSpeed;
+  }
+
   public void setDriveState(DriveStates state) {
     currDriveState = state;
   }
@@ -269,6 +287,15 @@ public void setEnableHolo(boolean enabled) {
     logger.info("Holonomic Controller Enabled: {}", enabled);
   }
 
+  public void prepClimb() {
+    io.setDriveCoast(true);
+    io.setSwerveModuleAngles(
+        Rotation2d.fromDegrees(90),
+        Rotation2d.fromDegrees(90),
+        Rotation2d.fromDegrees(90),
+        Rotation2d.fromDegrees(90));
+  }
+
   public void teleResetGyro() {
     logger.info("Driver Joystick: Reset Gyro");
     double gyroResetDegs = robotStateSubsystem.getAllianceColor() == Alliance.Blue ? 0.0 : 180.0;
@@ -356,6 +383,14 @@ public PIDController getomegaControllerNonProfiled() {
         omegaController.getP(), omegaController.getI(), omegaController.getD());
   }
 
+  public double getAvgDriveCurrent() {
+    return inputs.avgDriveCurrent;
+  }
+
+  public double getAvgRearDriveVel() {
+    return inputs.avgRearDriveVel;
+  }
+
   public void setDriveMultiplier(double multiplier) {
     driveMultiplier = multiplier;
   }
diff --git a/src/main/java/frc/robot/subsystems/drive/Swerve.java b/src/main/java/frc/robot/subsystems/drive/Swerve.java
index afb82abb..eded81d2 100644
--- a/src/main/java/frc/robot/subsystems/drive/Swerve.java
+++ b/src/main/java/frc/robot/subsystems/drive/Swerve.java
@@ -7,6 +7,7 @@
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.configs.TalonFXConfigurator;
 import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -52,6 +53,7 @@ public class Swerve implements SwerveIO, Checkable {
   private SwerveDriveKinematics kinematics;
   private double fieldY = 0.0;
   private double fieldX = 0.0;
+  private boolean didZero = false;
 
   public Swerve() {
 
@@ -93,6 +95,7 @@ public Swerve() {
               .build();
       swerveModules[i].loadAndSetAzimuthZeroReference();
     }
+    didZero = true;
 
     pigeon = new SF_PIGEON2(DriveConstants.kPigeonCanID, "rio");
     pigeon.applyConfig(DriveConstants.getPigeon2Configuration());
@@ -161,15 +164,7 @@ private ChassisSpeeds getRobotRelSpeed() {
     return kinematics.toChassisSpeeds(swerveModuleStates);
   }
 
-  private ChassisSpeeds getFieldRelSpeed() {
-    // SwerveDriveKinematics kinematics = swerveDrive.getKinematics();
-    // SwerveModule[] swerveModules = swerveDrive.getSwerveModules();
-    SwerveModuleState[] swerveModuleStates = new SwerveModuleState[4];
-    for (int i = 0; i < 4; ++i) {
-      swerveModuleStates[i] = swerveModules[i].getState();
-    }
-    ChassisSpeeds roboRelSpeed = kinematics.toChassisSpeeds(swerveModuleStates);
-
+  private ChassisSpeeds getFieldRelSpeed(ChassisSpeeds roboRelSpeed) {
     Rotation2d heading = swerveDrive.getHeading().unaryMinus();
     fieldX =
         roboRelSpeed.vxMetersPerSecond * heading.getCos()
@@ -181,6 +176,26 @@ private ChassisSpeeds getFieldRelSpeed() {
     return new ChassisSpeeds(fieldX, fieldY, roboRelSpeed.omegaRadiansPerSecond);
   }
 
+  public double getAvgDriveCurrent() {
+    double sum = 0;
+
+    for (int i = 0; i < 4; i++) {
+      sum += drives[i].getStatorCurrent().getValueAsDouble();
+    }
+
+    return sum / 4.0;
+  }
+
+  public double getRearDriveAvgVel() {
+    double sum = 0;
+
+    for (int i = 2; i < 4; i++) {
+      sum += FastMath.abs(drives[i].getVelocity().getValueAsDouble());
+    }
+
+    return sum / 2.0;
+  }
+
   @Override
   public void setOdometry(OdometryStrategy odom) {
     swerveDrive.setOdometry(odom);
@@ -197,6 +212,18 @@ public void setBothGyroOffset(Rotation2d rotation) {
     navxOffset = rotation;
   }
 
+  @Override
+  public void setDriveCoast(boolean coast) {
+    for (int i = 0; i < 4; i++) {
+      drives[i]
+          .getConfigurator()
+          .apply(
+              DriveConstants.getDriveTalonConfig()
+                  .MotorOutput
+                  .withNeutralMode(coast ? NeutralModeValue.Coast : NeutralModeValue.Brake));
+    }
+  }
+
   @Override
   public void resetGyro() {
     swerveDrive.resetGyro();
@@ -245,6 +272,13 @@ public void configDriveCurrents(CurrentLimitsConfigs config) {
     }
   }
 
+  @Override
+  public void zeroModules() {
+    for (int i = 0; i < 4; i++) {
+      swerveModules[i].loadAndSetAzimuthZeroReference();
+    }
+  }
+
   @Override
   public void updateInputs(SwerveIOInputs inputs) {
     swerveDrive.updateInputs(); // Call before swerveDrive.periodic()
@@ -268,9 +302,13 @@ public void updateInputs(SwerveIOInputs inputs) {
       inputs.azimuthVels[i] = azimuths[i].getSelectedSensorVelocity();
       inputs.azimuthCurrent[i] = azimuths[i].getSupplyCurrent();
     }
-    inputs.fieldRelSpeed = getFieldRelSpeed();
+    inputs.avgDriveCurrent = getAvgDriveCurrent();
+    inputs.avgRearDriveVel = getRearDriveAvgVel();
+    inputs.robotRelSpeed = getRobotRelSpeed();
+    inputs.fieldRelSpeed = getFieldRelSpeed(inputs.robotRelSpeed);
     inputs.fieldY = fieldY;
     inputs.fieldX = fieldX;
+    inputs.didZero = didZero;
   }
 
   @Override
diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java b/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java
new file mode 100644
index 00000000..0582d2c6
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/drive/SwerveFXS.java
@@ -0,0 +1,306 @@
+package frc.robot.subsystems.drive;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.configs.TalonFXConfigurator;
+import com.ctre.phoenix6.configs.TalonFXSConfiguration;
+import com.ctre.phoenix6.configs.TalonFXSConfigurator;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.hardware.TalonFXS;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import frc.robot.constants.DriveConstants;
+import frc.robot.constants.VisionConstants;
+import net.jafama.FastMath;
+import org.littletonrobotics.junction.Logger;
+import org.strykeforce.gyro.SF_AHRS;
+import org.strykeforce.gyro.SF_PIGEON2;
+import org.strykeforce.healthcheck.Checkable;
+import org.strykeforce.swerve.FXSwerveModule;
+import org.strykeforce.swerve.OdometryStrategy;
+import org.strykeforce.swerve.PoseEstimatorOdometryStrategy;
+import org.strykeforce.swerve.SwerveDrive;
+import org.strykeforce.swerve.SwerveModule;
+import org.strykeforce.swerve.V6TalonSwerveModule;
+import org.strykeforce.swerve.V6TalonSwerveModule.ClosedLoopUnits;
+import org.strykeforce.telemetry.TelemetryService;
+
+public class SwerveFXS implements SwerveIO, Checkable {
+  private final SwerveDrive swerveDrive;
+
+  // Grapher stuff
+  private PoseEstimatorOdometryStrategy odometryStrategy;
+
+  private SF_PIGEON2 pigeon;
+  private SF_AHRS navx;
+  private Rotation2d navxOffset = new Rotation2d();
+
+  private TalonFXSConfigurator configuratorFXS;
+  private TalonFXConfigurator configurator;
+
+  private TalonFXS[] azimuths = new TalonFXS[4];
+  private TalonFX[] drives = new TalonFX[4];
+
+  private FXSwerveModule[] swerveModules;
+  private SwerveDriveKinematics kinematics;
+  private double fieldY = 0.0;
+  private double fieldX = 0.0;
+  private boolean didZero = false;
+
+  public SwerveFXS() {
+
+    var moduleBuilder =
+        new FXSwerveModule.FXBuilder()
+            .driveGearRatio(DriveConstants.kDriveGearRatio)
+            .wheelDiameterInches(DriveConstants.kWheelDiameterInches)
+            .driveMaximumMetersPerSecond(DriveConstants.kMaxSpeedMetersPerSecond)
+            .latencyCompensation(true)
+            .encoderOpposed(false);
+    swerveModules = new FXSwerveModule[4];
+    Translation2d[] wheelLocations = DriveConstants.getWheelLocationMeters();
+    didZero = true;
+    for (int i = 0; i < 4; i++) {
+      var azimuthFXS = new TalonFXS(i, "*");
+      configuratorFXS = azimuthFXS.getConfigurator();
+      configuratorFXS.apply(new TalonFXSConfiguration()); // factory default
+      configuratorFXS.apply(DriveConstants.getAzimuthFXSConfig());
+
+      azimuthFXS.getRawPulseWidthPosition().setUpdateFrequency(200);
+
+      azimuths[i] = azimuthFXS;
+
+      var driveTalon = new TalonFX(i + 10, "*");
+      configurator = driveTalon.getConfigurator();
+      configurator.apply(new TalonFXConfiguration()); // factory default
+      configurator.apply(DriveConstants.getDriveTalonConfig());
+      drives[i] = driveTalon;
+
+      swerveModules[i] =
+          moduleBuilder
+              .azimuthTalon(azimuthFXS)
+              .driveTalon(driveTalon)
+              .wheelLocationMeters(wheelLocations[i])
+              .closedLoopUnits(ClosedLoopUnits.VOLTAGE)
+              .build();
+      boolean zeroCheck = swerveModules[i].zeroAndCheck();
+      didZero = zeroCheck && didZero;
+    }
+
+    pigeon = new SF_PIGEON2(DriveConstants.kPigeonCanID, "*");
+    pigeon.applyConfig(DriveConstants.getPigeon2Configuration());
+    navx = new SF_AHRS();
+    swerveDrive = new SwerveDrive(false, 0.02, pigeon, swerveModules);
+    swerveDrive.resetGyro();
+    swerveDrive.setGyroOffset(Rotation2d.fromDegrees(0));
+
+    kinematics = swerveDrive.getKinematics();
+
+    odometryStrategy =
+        new PoseEstimatorOdometryStrategy(
+            swerveDrive.getHeading(),
+            new Pose2d(),
+            swerveDrive.getKinematics(),
+            VisionConstants.kStateStdDevs,
+            VisionConstants.kLocalMeasurementStdDevs,
+            VisionConstants.kVisionMeasurementStdDevs,
+            getSwerveModulePositions());
+
+    swerveDrive.setOdometry(odometryStrategy);
+  }
+
+  // Getters/Setter
+  @Override
+  public String getName() {
+    return "Swerve";
+  }
+
+  @Override
+  public void zeroModules() {
+    didZero = true;
+    for (int i = 0; i < 4; i++) {
+      boolean zeroCheck = swerveModules[i].zeroAndCheck();
+      didZero = zeroCheck && didZero;
+    }
+  }
+
+  private SwerveModule[] getSwerveModules() {
+    return swerveDrive.getSwerveModules();
+  }
+
+  public void setSwerveModuleAngles(Rotation2d FL, Rotation2d FR, Rotation2d BL, Rotation2d BR) {
+    swerveModules[0].setAzimuthRotation2d(FL);
+    swerveModules[1].setAzimuthRotation2d(FR);
+    swerveModules[2].setAzimuthRotation2d(BL);
+    swerveModules[3].setAzimuthRotation2d(BR);
+  }
+
+  private SwerveModulePosition[] getSwerveModulePositions() {
+    SwerveModule[] swerveModules = getSwerveModules();
+    SwerveModulePosition[] temp = {null, null, null, null};
+    for (int i = 0; i < 4; ++i) {
+      temp[i] = swerveModules[i].getPosition();
+    }
+    return temp;
+  }
+
+  public SwerveModuleState[] getSwerveModuleStates() {
+    V6TalonSwerveModule[] swerveModules = (V6TalonSwerveModule[]) swerveDrive.getSwerveModules();
+    SwerveModuleState[] swerveModuleStates = new SwerveModuleState[4];
+    for (int i = 0; i < 4; i++) {
+      swerveModuleStates[i] = swerveModules[i].getState();
+    }
+    return swerveModuleStates;
+  }
+
+  private ChassisSpeeds getRobotRelSpeed() {
+    SwerveDriveKinematics kinematics = swerveDrive.getKinematics();
+    SwerveModule[] swerveModules = swerveDrive.getSwerveModules();
+    SwerveModuleState[] swerveModuleStates = new SwerveModuleState[4];
+    for (int i = 0; i < 4; ++i) {
+      swerveModuleStates[i] = swerveModules[i].getState();
+    }
+    return kinematics.toChassisSpeeds(swerveModuleStates);
+  }
+
+  private ChassisSpeeds getFieldRelSpeed(ChassisSpeeds roboRelSpeed) {
+    Rotation2d heading = swerveDrive.getHeading().unaryMinus();
+    fieldX =
+        roboRelSpeed.vxMetersPerSecond * heading.getCos()
+            + roboRelSpeed.vyMetersPerSecond * heading.getSin();
+    fieldY =
+        -roboRelSpeed.vxMetersPerSecond * heading.getSin()
+            + roboRelSpeed.vyMetersPerSecond * heading.getCos();
+
+    return new ChassisSpeeds(fieldX, fieldY, roboRelSpeed.omegaRadiansPerSecond);
+  }
+
+  public double getAvgDriveCurrent() {
+    double sum = 0;
+
+    for (int i = 0; i < 4; i++) {
+      sum += drives[i].getStatorCurrent().getValueAsDouble();
+    }
+
+    return sum / 4.0;
+  }
+
+  public double getRearDriveAvgVel() {
+    double sum = 0;
+
+    for (int i = 2; i < 4; i++) {
+      sum += FastMath.abs(drives[i].getVelocity().getValueAsDouble());
+    }
+
+    return sum / 2.0;
+  }
+
+  @Override
+  public void setOdometry(OdometryStrategy odom) {
+    swerveDrive.setOdometry(odom);
+  }
+
+  @Override
+  public void setPigeonGyroOffset(Rotation2d rotation) {
+    swerveDrive.setGyroOffset(rotation);
+  }
+
+  @Override
+  public void setBothGyroOffset(Rotation2d rotation) {
+    swerveDrive.setGyroOffset(rotation);
+    navxOffset = rotation;
+  }
+
+  @Override
+  public void resetGyro() {
+    swerveDrive.resetGyro();
+    navx.reset();
+  }
+
+  @Override
+  public void resetOdometry(Pose2d pose) {
+    swerveDrive.resetOdometry(pose);
+    navx.reset();
+  }
+
+  @Override
+  public void addVisionMeasurement(Pose2d pose, double timestamp) {
+    Logger.recordOutput("Drive/VisionSampledPose", odometryStrategy.getSample(timestamp));
+    odometryStrategy.addVisionMeasurement(pose, timestamp);
+  }
+
+  @Override
+  public void addVisionMeasurement(Pose2d pose2d, double timestamp, Matrix<N3, N1> stdDevs) {
+    Logger.recordOutput("Drive/VisionSampledPose", odometryStrategy.getSample(timestamp));
+    odometryStrategy.addVisionMeasurement(pose2d, timestamp, stdDevs);
+  }
+
+  @Override
+  public void drive(double vXmps, double vYmps, double vOmegaRadps, boolean isFieldOriented) {
+    swerveDrive.drive(vXmps, vYmps, vOmegaRadps, isFieldOriented);
+  }
+
+  @Override
+  public void move(double vXmps, double vYmps, double vOmegaRadps, boolean isFieldOriented) {
+    swerveDrive.move(vXmps, vYmps, vOmegaRadps, isFieldOriented);
+  }
+
+  @Override
+  public void setAzimuthVel(double vel) {
+    for (int i = 0; i < 4; i++) {
+      azimuths[i].set(vel);
+    }
+  }
+
+  @Override
+  public void configDriveCurrents(CurrentLimitsConfigs config) {
+    for (int i = 0; i < 4; i++) {
+      drives[i].getConfigurator().apply(config);
+    }
+  }
+
+  @Override
+  public void updateInputs(SwerveIOInputs inputs) {
+    swerveDrive.updateInputs(); // Call before swerveDrive.periodic()
+    swerveDrive.periodic();
+
+    inputs.odometryX = swerveDrive.getPoseMeters().getX();
+    inputs.odometryY = swerveDrive.getPoseMeters().getY();
+    inputs.swervePose = odometryStrategy.getPoseMeters();
+    inputs.odometryRotation2D = swerveDrive.getPoseMeters().getRotation().getDegrees();
+    inputs.gyroRotation2d = swerveDrive.getHeading();
+    inputs.navxRotation2d = navx.getRotation2d().rotateBy(navxOffset);
+    inputs.normalizedGyroRotation =
+        FastMath.normalizeMinusPiPi(swerveDrive.getHeading().getRadians());
+    inputs.gyroPitch = pigeon.getPitch();
+    inputs.gyroRoll = pigeon.getRoll();
+    inputs.gyroRate = swerveDrive.getGyroRate();
+    inputs.isConnected = pigeon.getPigeon2().getUpTime().hasUpdated();
+    inputs.poseMeters = swerveDrive.getPoseMeters();
+    inputs.pigeonTemp = pigeon.getPigeon2().getTemperature().getValueAsDouble();
+    for (int i = 0; i < 4; ++i) {
+      inputs.azimuthVels[i] = azimuths[i].getVelocity().getValueAsDouble();
+      inputs.azimuthCurrent[i] = azimuths[i].getSupplyCurrent().getValueAsDouble();
+    }
+    inputs.avgDriveCurrent = getAvgDriveCurrent();
+    inputs.avgRearDriveVel = getRearDriveAvgVel();
+    inputs.robotRelSpeed = getRobotRelSpeed();
+    inputs.fieldRelSpeed = getFieldRelSpeed(inputs.robotRelSpeed);
+    inputs.fieldY = fieldY;
+    inputs.fieldX = fieldX;
+    inputs.didZero = didZero;
+  }
+
+  @Override
+  public void registerWith(TelemetryService telemetryService) {
+    swerveDrive.registerWith(telemetryService);
+    pigeon.registerWith(telemetryService);
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveIO.java b/src/main/java/frc/robot/subsystems/drive/SwerveIO.java
index f8cd0b5c..0bc6bfce 100644
--- a/src/main/java/frc/robot/subsystems/drive/SwerveIO.java
+++ b/src/main/java/frc/robot/subsystems/drive/SwerveIO.java
@@ -35,9 +35,13 @@ public static class SwerveIOInputs {
     public double pigeonTemp = 0;
     public double fieldX = 0;
     public double fieldY = 0;
+    public ChassisSpeeds robotRelSpeed = new ChassisSpeeds();
     public ChassisSpeeds fieldRelSpeed = new ChassisSpeeds();
     public double[] azimuthVels = {0, 0, 0, 0};
     public double[] azimuthCurrent = {0, 0, 0, 0};
+    public double avgDriveCurrent = 0;
+    public double avgRearDriveVel = 0;
+    public boolean didZero = false;
   }
 
   private SwerveModule[] getSwerveModules() {
@@ -63,6 +67,8 @@ public default SwerveDriveKinematics getKinematics() {
     return null;
   }
 
+  public default void setDriveCoast(boolean coast) {}
+
   public default void setOdometry(OdometryStrategy Odom) {}
 
   public default void setPigeonGyroOffset(Rotation2d rotation) {}
@@ -96,4 +102,6 @@ public default void configDriveCurrents(CurrentLimitsConfigs config) {}
   public default BooleanSupplier getAzimuth1FwdLimitSwitch() {
     return () -> false;
   }
+
+  public default void zeroModules() {}
 }
diff --git a/src/main/java/frc/robot/subsystems/example/ExampleIOFX.java b/src/main/java/frc/robot/subsystems/example/ExampleIOFX.java
index bee51796..c7d2d4de 100644
--- a/src/main/java/frc/robot/subsystems/example/ExampleIOFX.java
+++ b/src/main/java/frc/robot/subsystems/example/ExampleIOFX.java
@@ -27,8 +27,8 @@ public class ExampleIOFX implements ExampleIO {
   TalonFXConfigurator configurator;
   private MotionMagicDutyCycle positionRequest =
       new MotionMagicDutyCycle(0).withEnableFOC(false).withFeedForward(0).withSlot(0);
-  StatusSignal<Angle> currPosition;
-  StatusSignal<AngularVelocity> currVelocity;
+  StatusSignal<Angle> curPosition;
+  StatusSignal<AngularVelocity> curVelocity;
 
   public ExampleIOFX() {
     logger = LoggerFactory.getLogger(this.getClass());
@@ -42,8 +42,8 @@ public ExampleIOFX() {
     configurator.apply(ExampleConstants.getFXConfig());
 
     // Attach status signals
-    currPosition = talonFx.getPosition();
-    currVelocity = talonFx.getVelocity();
+    curPosition = talonFx.getPosition();
+    curVelocity = talonFx.getVelocity();
   }
 
   @Override
@@ -64,9 +64,9 @@ public void setPosition(Angle position) {
 
   @Override
   public void updateInputs(ExampleIOInputs inputs) {
-    BaseStatusSignal.refreshAll(currVelocity, currPosition);
-    inputs.velocity = currVelocity.getValue();
-    inputs.position = currPosition.getValue().minus(relSetpointOffset);
+    BaseStatusSignal.refreshAll(curVelocity, curPosition);
+    inputs.velocity = curVelocity.getValue();
+    inputs.position = curPosition.getValue().minus(relSetpointOffset);
   }
 
   @Override
diff --git a/src/main/java/frc/robot/subsystems/led/LEDIO.java b/src/main/java/frc/robot/subsystems/led/LEDIO.java
index cdad1a71..bffd4833 100644
--- a/src/main/java/frc/robot/subsystems/led/LEDIO.java
+++ b/src/main/java/frc/robot/subsystems/led/LEDIO.java
@@ -1,3 +1,84 @@
 package frc.robot.subsystems.led;
 
-public class LEDIO {}
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.AddressableLEDBufferView;
+import edu.wpi.first.wpilibj.LEDPattern;
+import edu.wpi.first.wpilibj.util.Color;
+import frc.robot.constants.LEDConstants;
+
+public class LEDIO {
+  private AddressableLED ledBase;
+  private AddressableLEDBuffer led;
+  private AddressableLEDBufferView ledTop;
+
+  public LEDIO() {
+    ledBase = new AddressableLED(LEDConstants.kLEDPort);
+    ledBase.setLength(LEDConstants.kTotalStripLength);
+    ledBase.start();
+    led = new AddressableLEDBuffer(LEDConstants.kTotalStripLength);
+    ledTop = led.createView(LEDConstants.kTopFirstIndex, LEDConstants.kTotalStripLength - 1);
+    ledBase.setData(led);
+  }
+
+  public void setPort(int port) {
+    ledBase.close();
+    ledBase = new AddressableLED(port);
+    ledBase.setLength(led.getLength());
+    ledBase.start();
+    ledBase.setData(led);
+  }
+
+  public void setLength(int length) {
+    ledBase.setLength(length);
+    led = new AddressableLEDBuffer(length);
+    ledTop = led.createView(LEDConstants.kTopFirstIndex, LEDConstants.kTotalStripLength - 1);
+    ledBase.setData(led);
+  }
+
+  public void updateLEDs() {
+    ledBase.setData(led);
+  }
+
+  public void setLED(int index, Color color) {
+    led.setLED(index, color);
+  }
+
+  public void setLED(int index, int r, int g, int b) {
+    led.setRGB(index, r, g, b);
+  }
+
+  public Color getLED(int index) {
+    return led.getLED(index);
+  }
+
+  public void setLEDTop(int index, Color color) {
+    ledTop.setLED(index, color);
+  }
+
+  public void setLEDTop(int index, int r, int g, int b) {
+    ledTop.setRGB(index, r, g, b);
+  }
+
+  public Color getLEDTop(int index) {
+    return ledTop.getLED(index);
+  }
+
+  public void setOff() {
+    for (int i = 0; i < led.getLength(); i++) {
+      setLED(i, Color.kBlack);
+    }
+    ledBase.setData(led);
+  }
+
+  public void setStrip(LEDPattern pattern) {
+    pattern.applyTo(led);
+  }
+
+  public void setStrip(Color color) {
+    for (int i = 0; i < led.getLength(); i++) {
+      setLED(i, color);
+    }
+    ledBase.setData(led);
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java
index b9e30fd5..26fbfb3e 100644
--- a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java
@@ -1,9 +1,340 @@
 package frc.robot.subsystems.led;
 
-public class LEDSubsystem {
+import static edu.wpi.first.units.Units.Seconds;
+
+import edu.wpi.first.wpilibj.LEDPattern;
+import edu.wpi.first.wpilibj.util.Color;
+import frc.robot.constants.LEDConstants;
+import frc.robot.subsystems.robotState.RobotStateSubsystem.CoralLoc;
+import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel;
+import java.util.Map;
+import java.util.Set;
+import org.littletonrobotics.junction.Logger;
+import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
+import org.strykeforce.telemetry.measurable.Measure;
+
+public class LEDSubsystem extends MeasurableSubsystem {
+  private LEDIO io;
+  private LEDPattern base = LEDPattern.solid(Color.kBlack);
+  public LEDStates currState = LEDStates.OFF;
+
+  // section patterns
+  private LEDPattern algae =
+      LEDPattern.steps(Map.of(0, LEDConstants.kHasAlgea, LEDConstants.kAlgeaEnd, Color.kBlack));
+  private LEDPattern coral = LEDPattern.solid(LEDConstants.kCoralInRobot);
+  private LEDPattern level =
+      LEDPattern.steps(
+          Map.of(
+              LEDConstants.kLevelStart / (double) LEDConstants.kTotalStripLength,
+              LEDConstants.kL1));
+  private LEDPattern place =
+      LEDPattern.steps(
+          Map.of(
+              LEDConstants.kPlaceStart / (double) LEDConstants.kTotalStripLength,
+              LEDConstants.kManual));
+  private LEDPattern getAlgea =
+      LEDPattern.steps(
+          Map.of(
+              LEDConstants.kGetAlgeaStart / (double) LEDConstants.kTotalStripLength,
+              LEDConstants.kNotGetAlgea));
+  private LEDPattern autoplace =
+      LEDPattern.steps(
+              Map.of(
+                  LEDConstants.kAutoPlacingStart / (double) LEDConstants.kTotalStripLength,
+                  LEDConstants.kAutoPlacing))
+          .blink(Seconds.of(0.25));
+  private LEDPattern currentLimiting =
+      LEDPattern.solid(LEDConstants.kCurrentLimiting).blink(Seconds.of(0.5), Seconds.of(1));
+
+  // section booleans and states
+  private boolean hasAlgae = false;
+  private CoralLoc coralState = CoralLoc.NONE;
+  private ScoringLevel levelState = ScoringLevel.L4;
+  private PlaceStates placeState = PlaceStates.MANUAL;
+  private boolean shouldGetAlgae = false;
+  private boolean autoPlacing = false;
+  private boolean isLimiting = false;
+
+  // game stuff
+  private int[] bottomUnits = new int[LEDConstants.kTopStripLength];
+  private int[] topUnits = new int[LEDConstants.kTopStripLength];
+
+  public LEDSubsystem(LEDIO io) {
+    this.io = io;
+  }
+
+  public void setState(LEDStates state) {
+    currState = state;
+    switch (currState) {
+      case OFF -> io.setOff();
+      case NORMAL -> buildBase();
+      case CLIMB_EMPTY -> io.setStrip(LEDConstants.kWaitingForCage);
+      case CLIMB_FULL -> io.setStrip(LEDConstants.kHasCage);
+      case CLIMB_UP -> io.setStrip(LEDConstants.kClimbed);
+      default -> {}
+    }
+  }
+
+  public LEDStates getState() {
+    return currState;
+  }
+
+  // setters
+  public void setAlgeaLights(boolean on) {
+    hasAlgae = on;
+    buildBase();
+  }
+
+  public void setCoralLights(CoralLoc state) {
+    coralState = state;
+    buildBase();
+  }
+
+  public void setLevelLights(ScoringLevel state) {
+    levelState = state;
+    buildBase();
+  }
+
+  public void setPlaceLights(PlaceStates state) {
+    placeState = state;
+    buildBase();
+  }
+
+  public void setGetAlgeaLights(boolean on) {
+    shouldGetAlgae = on;
+    buildBase();
+  }
 
   public void setAutoPlacing(boolean isAutoPlacing) {
-    // TODO Auto-generated method stub
-    throw new UnsupportedOperationException("Unimplemented method 'setAutoPlacing'");
+    autoPlacing = isAutoPlacing;
+    buildBase();
+  }
+
+  public void setCurrentLimiting(boolean isCurrentLimiting) {
+    this.isLimiting = isCurrentLimiting;
+    buildBase();
+  }
+
+  // getters
+  public boolean getAlgeaLights() {
+    return hasAlgae;
+  }
+
+  public CoralLoc getCoralLights() {
+    return coralState;
+  }
+
+  public ScoringLevel getLevelLights() {
+    return levelState;
+  }
+
+  public PlaceStates getPlaceLights() {
+    return placeState;
+  }
+
+  public boolean getGetAlgeaLights() {
+    return shouldGetAlgae;
+  }
+
+  public boolean getAutoPlacing() {
+    return autoPlacing;
+  }
+
+  public boolean getCurrentLimiting() {
+    return isLimiting;
+  }
+
+  private void buildBase() {
+    algae =
+        LEDPattern.steps(
+            Map.of(
+                0,
+                hasAlgae ? LEDConstants.kHasAlgea : LEDConstants.kNotHasAlgea,
+                LEDConstants.kAlgeaEnd / (double) LEDConstants.kTotalStripLength,
+                Color.kBlack));
+    switch (coralState) {
+      case FUNNEL, TRANSFER -> coral = LEDPattern.solid(LEDConstants.kCoralInFunnel);
+      case CORAL, SCORING -> coral = LEDPattern.solid(LEDConstants.kCoralInRobot);
+      case NONE -> coral = LEDPattern.solid(LEDConstants.kCoralNotInRobot);
+    }
+    switch (levelState) {
+      case L1 -> level =
+          LEDPattern.steps(
+              Map.of(
+                  LEDConstants.kLevelStart / (double) LEDConstants.kTotalStripLength,
+                  LEDConstants.kL1));
+      case L2 -> level =
+          LEDPattern.steps(
+              Map.of(
+                  LEDConstants.kLevelStart / (double) LEDConstants.kTotalStripLength,
+                  LEDConstants.kL2));
+      case L3 -> level =
+          LEDPattern.steps(
+              Map.of(
+                  LEDConstants.kLevelStart / (double) LEDConstants.kTotalStripLength,
+                  LEDConstants.kL3));
+      case L4 -> level =
+          LEDPattern.steps(
+              Map.of(
+                  LEDConstants.kLevelStart / (double) LEDConstants.kTotalStripLength,
+                  LEDConstants.kL4));
+    }
+    switch (placeState) {
+      case MANUAL -> place =
+          LEDPattern.steps(
+              Map.of(
+                  LEDConstants.kPlaceStart / (double) LEDConstants.kTotalStripLength,
+                  LEDConstants.kManual));
+      case LEFT -> place =
+          LEDPattern.steps(
+              Map.of(
+                  LEDConstants.kPlaceStart / (double) LEDConstants.kTotalStripLength,
+                  LEDConstants.kLeft));
+      case RIGHT -> place =
+          LEDPattern.steps(
+              Map.of(
+                  LEDConstants.kPlaceStart / (double) LEDConstants.kTotalStripLength,
+                  LEDConstants.kRight));
+    }
+    getAlgea =
+        LEDPattern.steps(
+            Map.of(
+                LEDConstants.kGetAlgeaStart / (double) LEDConstants.kTotalStripLength,
+                shouldGetAlgae ? LEDConstants.kGetAlgea : LEDConstants.kNotGetAlgea));
+    base = algae.overlayOn(coral);
+
+    if (isLimiting) {
+      base = currentLimiting.overlayOn(base);
+    }
+
+    if (autoPlacing) {
+      base = autoplace.overlayOn(base);
+    }
+
+    base = getAlgea.overlayOn(place.overlayOn(level.overlayOn(base)));
+  }
+
+  // game stuff
+  private void advanceUnits() {
+    for (int i = 0; i <= LEDConstants.kTopStripLength + 1; i++) {
+      bottomUnits[i + 1] = bottomUnits[i];
+      topUnits[i] = topUnits[i + 1];
+      if (i == 0) bottomUnits[i] = 0;
+      if (i == LEDConstants.kTopStripLength) topUnits[i] = 0;
+      if (topUnits[i] != 0 && bottomUnits[i] != 0) {
+        if (bottomUnits[i] == topUnits[i]) {
+          bottomUnits[i] = 0;
+          topUnits[i] = 0;
+        } else if (bottomUnits[i] > topUnits[i] || (bottomUnits[i] == 1 && topUnits[i] == 3)) {
+          topUnits[i] = 0;
+        } else {
+          bottomUnits[i] = 0;
+        }
+      }
+      if (topUnits[i + 1] != 0 && bottomUnits[i] != 0) {
+        if (bottomUnits[i] == topUnits[i + 1]) {
+          bottomUnits[i] = 0;
+          topUnits[i + 1] = 0;
+        } else if (bottomUnits[i] > topUnits[i + 1]
+            || (bottomUnits[i] == 1 && topUnits[i + 1] == 3)) {
+          topUnits[i + 1] = 0;
+        } else {
+          bottomUnits[i + 1] = 0;
+        }
+      }
+    }
+  }
+
+  private void displayUnits() {
+    for (int i = 0; i <= LEDConstants.kTopStripLength + 1; i++) {
+      io.setLEDTop(i, LEDConstants.kGameColors[bottomUnits[i]]);
+      if (topUnits[i] != 0) {
+        io.setLEDTop(i, LEDConstants.kGameColors[topUnits[i] + 3]);
+      }
+    }
+  }
+
+  public void addGameUnit(boolean isBottom, int unitNum) {
+    // the unitNum starts at 1 and ends at 3. if you use anything else, it will be ignored
+    if (unitNum >= 1 && unitNum <= 3) {
+      if (isBottom) {
+        bottomUnits[0] = unitNum;
+      } else {
+        topUnits[LEDConstants.kTopStripLength - 1] = unitNum;
+      }
+    }
+  }
+
+  @Override
+  public void periodic() {
+    Logger.recordOutput("LedSubsystem/state", currState);
+    Logger.recordOutput("LedSubsystem/hasAlgae", hasAlgae);
+    Logger.recordOutput("LedSubsystem/coralState", coralState);
+    Logger.recordOutput("LedSubsystem/levelState", levelState);
+    Logger.recordOutput("LedSubsystem/placeState", placeState);
+    Logger.recordOutput("LedSubsystem/shouldGetAlgae", shouldGetAlgae);
+    Logger.recordOutput("LedSubsystem/autoPlacing", autoPlacing);
+    Logger.recordOutput("LedSubsystem/currentLimiting", isLimiting);
+
+    switch (currState) {
+      case OFF -> {}
+      case NORMAL -> {
+        io.setStrip(base);
+      }
+      case CLIMB_EMPTY -> {}
+      case CLIMB_FULL -> {}
+      case CLIMB_UP -> {
+        advanceUnits();
+        displayUnits();
+      }
+    }
+    io.updateLEDs();
+  }
+
+  @Override
+  public Set<Measure> getMeasures() {
+    return Set.of(
+        new Measure(
+            "State", "the current Overall state of the LEDSubsystem", () -> currState.ordinal()),
+        new Measure(
+            "HasAlgea", "the current algea state of the LEDSubsystem", () -> hasAlgae ? 0 : 1),
+        new Measure(
+            "CoralState",
+            "the current Coral state of the LEDSubsystem",
+            () -> coralState.ordinal()),
+        new Measure(
+            "Level",
+            "the current assumed Level of the LEDSubsystem",
+            () -> (levelState.ordinal() + 1)),
+        new Measure(
+            "PlaceState",
+            "the current Placement state of the LEDSubsystem",
+            () -> placeState.ordinal()),
+        new Measure(
+            "GetAlgea",
+            "the current get algea state of the LEDSubsystem",
+            () -> shouldGetAlgae ? 0 : 1),
+        new Measure(
+            "autoPlacing",
+            "the current autoPlacing state of the LEDSubsystem",
+            () -> autoPlacing ? 0 : 1),
+        new Measure(
+            "currentLimiting",
+            "the current currentLimiting state of the LEDSubsystem",
+            () -> isLimiting ? 0 : 1));
+  }
+
+  public enum LEDStates {
+    OFF,
+    NORMAL,
+    CLIMB_EMPTY,
+    CLIMB_FULL,
+    CLIMB_UP
+  }
+
+  public enum PlaceStates {
+    MANUAL,
+    LEFT,
+    RIGHT
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java
index 338954fa..1fab2fa4 100644
--- a/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java
+++ b/src/main/java/frc/robot/subsystems/pathHandler/PathHandler.java
@@ -4,6 +4,7 @@
 import choreo.trajectory.SwerveSample;
 import choreo.trajectory.Trajectory;
 import edu.wpi.first.wpilibj.Timer;
+import frc.robot.constants.DriveConstants;
 import frc.robot.constants.RobotStateConstants;
 import frc.robot.constants.TagServoingConstants;
 import frc.robot.subsystems.drive.DriveSubsystem;
@@ -13,6 +14,7 @@
 import java.util.Optional;
 import java.util.Set;
 import net.jafama.FastMath;
+import org.slf4j.LoggerFactory;
 import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
 import org.strykeforce.telemetry.measurable.Measure;
 
@@ -20,6 +22,7 @@ public class PathHandler extends MeasurableSubsystem {
   private DriveSubsystem driveSubsystem;
   private RobotStateSubsystem robotStateSubsystem;
   private TagAlignSubsystem tagAlignSubsystem;
+  private static final org.slf4j.Logger logger = LoggerFactory.getLogger(PathHandler.class);
 
   private PathStates curState = PathStates.DONE;
   private boolean isHandling = false;
@@ -34,6 +37,7 @@ public class PathHandler extends MeasurableSubsystem {
   private boolean runningPath = false;
   private boolean isServoing = false;
   private boolean mirrorTrajectory = false;
+  private boolean mirrorToProcessor = false;
   private Character startNode = 'a';
 
   public PathHandler(
@@ -43,7 +47,7 @@ public PathHandler(
     this.driveSubsystem = driveSubsystem;
     this.tagAlignSubsystem = tagAlignSubsystem;
     this.robotStateSubsystem = robotStateSubsystem;
-    reassignAlliance();
+    // reassignAlliance();
   }
 
   public PathHandler(
@@ -51,9 +55,10 @@ public PathHandler(
       TagAlignSubsystem tagAlignSubsystem,
       RobotStateSubsystem robotStateSubsystem,
       String[][] pathNames,
-      List<Character> nodeNames,
-      List<Integer> nodeLevels,
-      Character startNode) {
+      List<Character> NodeNames,
+      List<Integer> NodeLevels,
+      Character startNode,
+      boolean mirrorToProcessor) {
     this.driveSubsystem = driveSubsystem;
     this.tagAlignSubsystem = tagAlignSubsystem;
     this.robotStateSubsystem = robotStateSubsystem;
@@ -61,6 +66,7 @@ public PathHandler(
     this.nodeNames = nodeNames;
     this.nodeLevels = nodeLevels;
     this.startNode = startNode;
+    this.mirrorToProcessor = mirrorToProcessor;
   }
 
   public void setPathNames(String[][] pathNames) {
@@ -87,6 +93,12 @@ public void setStartNode(Character startNode) {
     }
   }
 
+  public void setMirrorToProcessor(boolean mirrorToProcessor) {
+    if (!isHandling) {
+      this.mirrorToProcessor = mirrorToProcessor;
+    }
+  }
+
   public void startPathHandler() {
     nodeNames.add(0, startNode);
     isHandling = true;
@@ -96,8 +108,10 @@ public void startPathHandler() {
 
   public void reassignAlliance() {
     mirrorTrajectory = driveSubsystem.shouldFlip();
+    Optional<Trajectory<SwerveSample>> temp;
     for (int i = 0; i < 12; i++) {
-      Optional<Trajectory<SwerveSample>> temp = Choreo.loadTrajectory(pathNames[i][0]);
+      logger.info(i + "");
+      temp = Choreo.loadTrajectory(pathNames[i][0]);
       fetchPaths.add(temp.get());
       temp = Choreo.loadTrajectory(pathNames[i][1]);
       placePaths.add(temp.get());
@@ -112,7 +126,8 @@ private void startPath(Trajectory<SwerveSample> path) {
       timer.stop();
       timer.reset();
       timer.start();
-      driveSubsystem.calculateController(currPath.getInitialSample(mirrorTrajectory).get());
+      driveSubsystem.calculateController(
+          mirrorToProcessor(currPath.getInitialSample(mirrorTrajectory).get()));
       if (curState == PathStates.PLACE) {
         curState = PathStates.DRIVE_FETCH;
       } else if (curState == PathStates.FETCH) {
@@ -123,13 +138,15 @@ private void startPath(Trajectory<SwerveSample> path) {
 
   private void drivePath() {
     if (isHandling && runningPath && currPath != null) {
-      driveSubsystem.calculateController(currPath.sampleAt(timer.get(), mirrorTrajectory).get());
+      driveSubsystem.calculateController(
+          mirrorToProcessor(currPath.sampleAt(timer.get(), mirrorTrajectory).get()));
       if (timer.hasElapsed(currPath.getTotalTime())) {
         driveSubsystem.setAutoDebugMsg("End " + currPathString);
         runningPath = false;
         timer.stop();
         timer.reset();
-        driveSubsystem.calculateController(currPath.getFinalSample(mirrorTrajectory).get());
+        driveSubsystem.calculateController(
+            mirrorToProcessor(currPath.getFinalSample(mirrorTrajectory).get()));
         if (curState == PathStates.DRIVE_FETCH) {
           curState = PathStates.FETCH;
         } else if (curState == PathStates.DRIVE_PLACE) {
@@ -145,15 +162,15 @@ private void drivePath() {
   private void drivePathServo() {
     if (isHandling && runningPath && currPath != null && isServoing) {
       driveSubsystem.calculateControllerServo(
-          currPath.sampleAt(timer.get(), mirrorTrajectory).get(),
-          tagAlignSubsystem.calculateAlignY());
+          mirrorToProcessor(currPath.sampleAt(timer.get(), mirrorTrajectory).get()),
+          0.0); // TODO: use tag servoing here when ready
       if (timer.hasElapsed(currPath.getTotalTime())) {
         driveSubsystem.setAutoDebugMsg("End " + currPathString);
         runningPath = false;
         timer.stop();
         timer.reset();
         driveSubsystem.calculateControllerServo(
-            currPath.getFinalSample(mirrorTrajectory).get(), 0.0);
+            mirrorToProcessor(currPath.getFinalSample(mirrorTrajectory).get()), 0.0);
         if (curState == PathStates.DRIVE_FETCH) {
           curState = PathStates.FETCH;
         } else if (curState == PathStates.DRIVE_PLACE_SERVO) {
@@ -219,9 +236,39 @@ private boolean shouldTransitionToServoing() {
     }
     return curState == PathStates.DRIVE_PLACE
         && timer.hasElapsed(currPath.getTotalTime() - 1.0)
+        // && TagAlignSubsystem.canSeeTag(desiredTag)
         && isCloseEnough;
   }
 
+  private SwerveSample mirrorToProcessor(SwerveSample sample) {
+    if (mirrorToProcessor) {
+      sample =
+          new SwerveSample(
+              sample.t,
+              sample.x,
+              DriveConstants.kFieldMaxY - sample.y,
+              sample.heading * -1,
+              sample.vx,
+              DriveConstants.kFieldMaxY - sample.vy,
+              sample.omega * -1,
+              sample.ax,
+              DriveConstants.kFieldMaxY - sample.ay,
+              sample.alpha * -1,
+              sample.moduleForcesX(),
+              new double[] {
+                sample.moduleForcesY()[0] * -1,
+                sample.moduleForcesY()[1] * -1,
+                sample.moduleForcesY()[2] * -1,
+                sample.moduleForcesY()[3] * -1
+              });
+    }
+    return sample;
+  }
+
+  public boolean isFinished() {
+    return !isHandling;
+  }
+
   @Override
   public void periodic() {
     switch (curState) {
@@ -247,7 +294,8 @@ public void periodic() {
         if (runningPath && isServoing) {
           tagAlignSubsystem.setup(
               robotStateSubsystem.getAllianceColor(),
-              robotStateSubsystem.getScoreSide() == RobotStateSubsystem.ScoreSide.LEFT);
+              robotStateSubsystem.getScoreSide() == RobotStateSubsystem.ScoreSide.LEFT,
+              false);
           drivePathServo();
         }
       }
diff --git a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java
index d0313e81..74e98eaf 100644
--- a/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java
@@ -1,5 +1,7 @@
 package frc.robot.subsystems.robotState;
 
+import static edu.wpi.first.units.Units.Rotations;
+
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
@@ -8,6 +10,7 @@
 import frc.robot.constants.DriveConstants;
 import frc.robot.constants.ElevatorConstants;
 import frc.robot.constants.RobotStateConstants;
+import frc.robot.constants.TagServoingConstants;
 import frc.robot.subsystems.algae.AlgaeSubsystem;
 import frc.robot.subsystems.battMon.BattMonSubsystem;
 import frc.robot.subsystems.biscuit.BiscuitSubsystem;
@@ -17,9 +20,13 @@
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
 import frc.robot.subsystems.funnel.FunnelSubsystem;
 import frc.robot.subsystems.led.LEDSubsystem;
+import frc.robot.subsystems.led.LEDSubsystem.LEDStates;
+import frc.robot.subsystems.led.LEDSubsystem.PlaceStates;
 import frc.robot.subsystems.tagAlign.TagAlignSubsystem;
+import frc.robot.subsystems.tagAlign.TagAlignSubsystem.TagAlignStates;
 import frc.robot.subsystems.vision.VisionSubsystem;
 import java.util.Set;
+import net.jafama.FastMath;
 import org.littletonrobotics.junction.Logger;
 import org.slf4j.LoggerFactory;
 import org.strykeforce.telemetry.TelemetryService;
@@ -52,8 +59,8 @@ public class RobotStateSubsystem extends MeasurableSubsystem {
   private ScoreSide scoreSide = ScoreSide.LEFT;
   private AlgaeHeight algaeHeight = AlgaeHeight.LOW;
   private AlgaeHeight currentAlgaeHeight = AlgaeHeight.LOW;
-  private CoralLoc coralLoc = CoralLoc.CORAL;
-  private Pose2d algaeRemovalPose;
+  private CoralLoc coralLoc = CoralLoc.NONE;
+  private Pose2d processorReleasePose;
 
   private boolean isAutoPlacing = false;
   private boolean getAlgaeOnCycle = false;
@@ -61,6 +68,7 @@ public class RobotStateSubsystem extends MeasurableSubsystem {
   private boolean isAuto = false;
   private boolean isEjectingAlgae = false;
   private boolean isBargeSafe = true;
+  private boolean prestagingForAlgae = false;
 
   private Timer scoringTimer = new Timer();
 
@@ -87,6 +95,8 @@ public RobotStateSubsystem(
     this.ledSubsystem = ledSubsystem;
     this.tagAlignSubsystem = tagAlignSubsystem;
     this.visionSubsystem = visionSubsystem;
+
+    ledSubsystem.setState(LEDStates.NORMAL);
   }
 
   public RobotStates getState() {
@@ -163,10 +173,14 @@ public void setAllianceColor(Alliance alliance) {
 
   public void setScoringLevel(ScoringLevel scoringLevel) {
     this.scoringLevel = scoringLevel;
+    ledSubsystem.setLevelLights(scoringLevel);
   }
 
   public void setScoreSide(ScoreSide scoreSide) {
     this.scoreSide = scoreSide;
+    if (!isAutoPlacing) ledSubsystem.setPlaceLights(PlaceStates.MANUAL);
+    else if (scoreSide == ScoreSide.LEFT) ledSubsystem.setPlaceLights(PlaceStates.LEFT);
+    else ledSubsystem.setPlaceLights(PlaceStates.RIGHT);
   }
 
   public void setAlgaeHeight(AlgaeHeight algaeHeight) {
@@ -179,18 +193,24 @@ public void toggleAlgaeHeight() {
 
   public void setIsAutoPlacing(boolean isAutoPlacing) {
     this.isAutoPlacing = isAutoPlacing;
+    if (!isAutoPlacing) ledSubsystem.setPlaceLights(PlaceStates.MANUAL);
+    else if (scoreSide == ScoreSide.LEFT) ledSubsystem.setPlaceLights(PlaceStates.LEFT);
+    else ledSubsystem.setPlaceLights(PlaceStates.RIGHT);
   }
 
   public void setGetAlgaeOnCycle(boolean getAlgaeOnCycle) {
     this.getAlgaeOnCycle = getAlgaeOnCycle;
+    ledSubsystem.setGetAlgeaLights(getAlgaeOnCycle);
   }
 
   public void toggleGetAlgaeOnCycle() {
     getAlgaeOnCycle = !getAlgaeOnCycle;
+    ledSubsystem.setGetAlgeaLights(getAlgaeOnCycle);
   }
 
   public void setCurrentLimiting(boolean isCurrentLimiting) {
     this.isCurrentLimiting = isCurrentLimiting;
+    ledSubsystem.setCurrentLimiting(isCurrentLimiting);
   }
 
   public void setIsAuto(boolean isAuto) {
@@ -281,6 +301,22 @@ private void toFunnelLoad() {
     setState(RobotStates.FUNNEL_LOAD, true);
   }
 
+  private void toPrestage() {
+    biscuitSubsystem.setPosition(BiscuitConstants.kPrestageSetpoint);
+
+    prestagingForAlgae = getAlgaeOnCycle;
+
+    if (getAlgaeOnCycle) {
+      elevatorSubsystem.setPosition(ElevatorConstants.kL2AlgaeSetpoint);
+    } else {
+      elevatorSubsystem.setPosition(ElevatorConstants.kPrestageSetpoint);
+    }
+
+    funnelSubsystem.stopMotor();
+
+    setState(RobotStates.PRESTAGE, true);
+  }
+
   public void toPrepCoral() {
     if (curState == RobotStates.REEF_ALIGN_CORAL) {
       if (elevatorSubsystem.isFinished()) {
@@ -296,12 +332,42 @@ public void toReefAlign() {
   }
 
   private void toReefAlign(boolean getAlgae, boolean drive) {
+    boolean wantAlgae = getAlgae || !coralSubsystem.hasCoral();
     if (drive) {
-      tagAlignSubsystem.start(allianceColor, scoreSide == ScoreSide.LEFT);
+      tagAlignSubsystem.start(
+          allianceColor,
+          scoreSide == ScoreSide.LEFT
+              || !coralSubsystem.hasCoral()
+              || (wantAlgae && getAlgaeOnCycle),
+          wantAlgae);
+      setAutoPlacingLed(true);
       setState(RobotStates.REEF_ALIGN);
     }
-    if ((getAlgae || !coralSubsystem.hasCoral())
-        && (scoreSide == ScoreSide.LEFT || !isAutoPlacing)
+
+    boolean algaeSafe =
+        tagAlignSubsystem.getCurRadius(allianceColor) > TagServoingConstants.kAlgaeStopXDriveRadius
+            || tagAlignSubsystem.getState() != TagAlignStates.DRIVE;
+
+    boolean ignoreCoralScoring = isAutoPlacing && getAlgaeOnCycle && scoreSide == ScoreSide.RIGHT;
+
+    if (wantAlgae && !algaeSafe) {
+      algaeSubsystem.intake();
+      biscuitSubsystem.setPosition(BiscuitConstants.kPrestageAlgaeSetpoint);
+
+      switch (getAlgaeLevel()) {
+        case L2 -> {
+          elevatorSubsystem.setPosition(ElevatorConstants.kL2AlgaeSetpoint);
+        }
+        case L3 -> {
+          elevatorSubsystem.setPosition(ElevatorConstants.kL3AlgaeSetpoint);
+        }
+        default -> logger.error("Invalid algae level: {}", getAlgaeLevel());
+      }
+    }
+
+    if (wantAlgae
+        && algaeSafe
+        && (scoreSide == ScoreSide.LEFT || !isAutoPlacing || ignoreCoralScoring)
         && !algaeSubsystem.hasAlgae()) {
       if (needSafeAlgaeTransfer(RobotStates.REEF_ALIGN_ALGAE)) {
         return;
@@ -323,7 +389,7 @@ private void toReefAlign(boolean getAlgae, boolean drive) {
         default -> logger.error("Invalid algae level: {}", getAlgaeLevel());
       }
     } else if (!drive) {
-      if (!coralSubsystem.hasCoral()) {
+      if (!coralSubsystem.hasCoral() || ignoreCoralScoring) {
         toStow();
         return;
       } else {
@@ -432,6 +498,10 @@ public void releaseAlgae() {
     scoringTimer.reset();
     scoringTimer.start();
     isEjectingAlgae = true;
+
+    processorReleasePose = driveSubsystem.getPoseMeters();
+    Logger.recordOutput("RobotState/Processor Release Pose", processorReleasePose);
+
     switch (algaeHeight) {
       case LOW -> {
         algaeSubsystem.scoreProcessor();
@@ -473,6 +543,7 @@ private void toProcessor() {
   public void toInterrupted() {
     if (tagAlignSubsystem.getState() != TagAlignSubsystem.TagAlignStates.DONE) {
       tagAlignSubsystem.terminate();
+      setAutoPlacingLed(false);
       driveSubsystem.setIgnoreSticks(false);
     }
 
@@ -488,25 +559,23 @@ public void toPrepClimb() {
     }
 
     climbSubsystem.prepClimb();
+    driveSubsystem.prepClimb();
 
-    setState(RobotStates.PREP_CLIMB, true);
+    setState(RobotStates.PREP_CLIMB, false);
   }
 
   public void toClimb() {
     if (curState == RobotStates.PREP_CLIMB) {
 
       climbSubsystem.climb();
+      driveSubsystem.prepClimb();
 
-      setState(RobotStates.CLIMB, true);
+      setState(RobotStates.CLIMB, false);
     }
   }
 
   @Override
   public void periodic() {
-    if (funnelSubsystem.hasCoral()) {
-      coralLoc = CoralLoc.FUNNEL;
-    }
-
     Logger.recordOutput("RobotState/state", curState);
     Logger.recordOutput("RobotState/hasCoral", hasCoral());
     Logger.recordOutput("RobotState/hasAlgae", hasAlgae());
@@ -519,6 +588,7 @@ public void periodic() {
     Logger.recordOutput("RobotState/isAuto", isAuto);
     Logger.recordOutput("RobotState/currentLimiting", isCurrentLimiting);
     Logger.recordOutput("RobotState/isEjectingAlgae", isEjectingAlgae);
+    Logger.recordOutput("RobotState/coralLoc", coralLoc);
 
     switch (curState) {
       case TRANSFER -> {
@@ -569,29 +639,28 @@ public void periodic() {
       }
       case REEF_ALIGN -> {
         if (!isAutoPlacing
-            || tagAlignSubsystem.getState() == TagAlignSubsystem.TagAlignStates.DONE
-            || tagAlignSubsystem.getState() == TagAlignSubsystem.TagAlignStates.TAG_ALIGN) {
+            || tagAlignSubsystem.getState() != TagAlignSubsystem.TagAlignStates.DRIVE) {
           toReefAlign(getAlgaeOnCycle, false);
         }
       }
       case REEF_ALIGN_ALGAE -> {
-        if (!isAutoPlacing
-            || tagAlignSubsystem.getState() == TagAlignSubsystem.TagAlignStates.DONE) {
-          if (algaeSubsystem.hasAlgae()) {
-            switch (getAlgaeLevel()) {
-              case L2 -> {
-                biscuitSubsystem.setPosition(BiscuitConstants.kL2AlgaeRemovalSetpoint);
-                elevatorSubsystem.setPosition(ElevatorConstants.kL2AlgaeRemovalSetpoint);
-              }
-              case L3 -> {
-                biscuitSubsystem.setPosition(BiscuitConstants.kL3AlgaeRemovalSetpoint);
-                elevatorSubsystem.setPosition(ElevatorConstants.kL3AlgaeRemovalSetpoint);
-              }
-              default -> logger.error("Invalid algae level: {}", getAlgaeLevel());
+        if (biscuitSubsystem.getPosition().in(Rotations) > BiscuitConstants.kTagAlignThreshold) {
+          tagAlignSubsystem.setProceedToAlign(true);
+        }
+        if (algaeSubsystem.hasAlgae()) {
+          switch (getAlgaeLevel()) {
+            case L2 -> {
+              biscuitSubsystem.setPosition(BiscuitConstants.kL2AlgaeRemovalSetpoint);
+              elevatorSubsystem.setPosition(ElevatorConstants.kL2AlgaeRemovalSetpoint);
             }
-
-            setState(RobotStates.REMOVE_ALGAE);
+            case L3 -> {
+              biscuitSubsystem.setPosition(BiscuitConstants.kL3AlgaeRemovalSetpoint);
+              elevatorSubsystem.setPosition(ElevatorConstants.kL3AlgaeRemovalSetpoint);
+            }
+            default -> logger.error("Invalid algae level: {}", getAlgaeLevel());
           }
+
+          setState(RobotStates.REMOVE_ALGAE);
         }
       }
       case REEF_ALIGN_CORAL -> {
@@ -606,9 +675,11 @@ public void periodic() {
       }
       case REMOVE_ALGAE -> {
         if (biscuitSubsystem.isFinished() && elevatorSubsystem.isFinished()) {
-          if (coralSubsystem.hasCoral()) {
+          if (coralSubsystem.hasCoral()
+              && !(isAutoPlacing && getAlgaeOnCycle && scoreSide == ScoreSide.RIGHT)) {
             toReefAlign(false, false);
           } else {
+            setAutoPlacingLed(false);
             toStowSequential();
           }
         }
@@ -618,6 +689,7 @@ public void periodic() {
         if (!coralSubsystem.hasCoral()
             && scoringTimer.hasElapsed(RobotStateConstants.kCoralEjectTimer)) {
           coralLoc = CoralLoc.NONE;
+          setAutoPlacingLed(false);
           toFunnelLoad();
         } else {
           coralLoc = CoralLoc.SCORING;
@@ -625,6 +697,9 @@ public void periodic() {
       }
 
       case FUNNEL_LOAD -> {
+        if (funnelSubsystem.hasCoral()) {
+          coralLoc = CoralLoc.FUNNEL;
+        }
         if (elevatorSubsystem.isFinished()) {
           funnelSubsystem.startMotor();
         }
@@ -636,25 +711,34 @@ public void periodic() {
       case LOADING_CORAL -> {
         if (coralSubsystem.hasCoral()) {
           coralLoc = CoralLoc.CORAL;
-          biscuitSubsystem.setPosition(BiscuitConstants.kPrestageSetpoint);
-          elevatorSubsystem.setPosition(ElevatorConstants.kPrestageSetpoint);
-          funnelSubsystem.stopMotor();
-
-          setState(RobotStates.PRESTAGE, true);
+          toPrestage();
+        }
+      }
+      case PRESTAGE -> {
+        if (getAlgaeOnCycle != prestagingForAlgae) {
+          toPrestage();
         }
       }
-      case PRESTAGE -> {}
       case HP_ALGAE -> {
         if (algaeSubsystem.hasAlgae()) {
           toProcessor();
         }
       }
       case PROCESSOR_ALGAE -> {
-        if (!algaeSubsystem.hasAlgae()
-            && scoringTimer.hasElapsed(RobotStateConstants.kCoralEjectTimer)
-            && isEjectingAlgae) {
-          isEjectingAlgae = false;
-          toStowSafe();
+        if (isEjectingAlgae
+            && !algaeSubsystem.hasAlgae()
+            && scoringTimer.hasElapsed(RobotStateConstants.kAlgaeEjectTimer)) {
+          Pose2d currentPose = driveSubsystem.getPoseMeters();
+          double distanceFromRelease =
+              FastMath.hypot(
+                  currentPose.getX() - processorReleasePose.getX(),
+                  currentPose.getY() - processorReleasePose.getY());
+          Logger.recordOutput("RobotState/Processor Release Distance", distanceFromRelease);
+
+          if (distanceFromRelease > RobotStateConstants.kProcessorStowRadius) {
+            isEjectingAlgae = false;
+            toStowSafe();
+          }
         } else if (algaeHeight != currentAlgaeHeight) {
           toScoreAlgae();
         }
@@ -680,7 +764,7 @@ public void periodic() {
       }
       case BARGE_ALGAE -> {
         if (!algaeSubsystem.hasAlgae()
-            && scoringTimer.hasElapsed(RobotStateConstants.kCoralEjectTimer)
+            && scoringTimer.hasElapsed(RobotStateConstants.kAlgaeEjectTimer)
             && isEjectingAlgae) {
           isEjectingAlgae = false;
           toStowSafe();
@@ -708,6 +792,8 @@ public void periodic() {
       case IDLE -> {}
       default -> logger.error("Unhandled state: {}", curState);
     }
+    ledSubsystem.setCoralLights(coralLoc);
+    ledSubsystem.setAlgeaLights(hasAlgae());
   }
 
   @Override
diff --git a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java
index fd66a6de..d06e29cf 100644
--- a/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java
@@ -1,68 +1,99 @@
 package frc.robot.subsystems.tagAlign;
 
-import WallEye.Point;
-import WallEye.WallEyeTagResult;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import edu.wpi.first.wpilibj.RobotController;
 import frc.robot.constants.TagServoingConstants;
 import frc.robot.subsystems.drive.DriveSubsystem;
 import frc.robot.subsystems.vision.VisionSubsystem;
 import java.util.Set;
 import net.jafama.FastMath;
 import org.littletonrobotics.junction.Logger;
+import org.slf4j.LoggerFactory;
 import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
 import org.strykeforce.telemetry.measurable.Measure;
 
 public class TagAlignSubsystem extends MeasurableSubsystem {
+  private static final org.slf4j.Logger logger = LoggerFactory.getLogger(DriveSubsystem.class);
   private DriveSubsystem driveSubsystem;
   private VisionSubsystem visionSubsystem;
 
-  private ProfiledPIDController driveX;
-  private ProfiledPIDController driveY;
+  private PIDController driveX;
+  private PIDController driveY;
   private ProfiledPIDController driveOmega;
 
-  private ProfiledPIDController alignX;
-  private ProfiledPIDController alignY;
-  private ProfiledPIDController alignOmega;
+  private PIDController alignX;
+  private PIDController alignY;
+  // private ProfiledPIDController alignOmega;
+
+  // private ProfiledPIDController servoX;
+  // private ProfiledPIDController servoY;
 
   private TagAlignStates curState = TagAlignStates.DONE;
 
   // Set by start()
   private Pose2d targetPose;
-  private int targetCamId;
-  private int targetTagId;
+  // private int targetCamId;
+  // private int targetTagId;
   private int fieldRelHexant;
   private Alliance alliance = Alliance.Blue;
-  private double goalTargetDiag;
-
-  private long startServoTime;
+  // private double goalTargetDiag;
+  // private double stopXRadius = TagServoingConstants.kCoralStopXDriveRadius;
+  private double driveRadius = TagServoingConstants.kCoralInitialDriveRadius;
+  private boolean algae = false;
+  private double driveXCloseEnough = TagServoingConstants.kCoralDriveXCloseEnough;
+  private double driveYCloseEnough = TagServoingConstants.kCoralDriveYCloseEnough;
+  private boolean proceedToAlign = false;
+  private boolean scoreLeft = true;
+  private int currentThresCount = 0;
+  private boolean finalDrive = false;
+
+  // private long startServoTime;
 
   public TagAlignSubsystem(DriveSubsystem driveSubsystem, VisionSubsystem visionSubsystem) {
     this.driveSubsystem = driveSubsystem;
     this.visionSubsystem = visionSubsystem;
 
-    this.driveX = new ProfiledPIDController(5, 0, 0, new Constraints(2, 2.0));
-    this.driveY = new ProfiledPIDController(5.5, 0, 0, new Constraints(2, 3));
-    this.driveOmega = new ProfiledPIDController(5.0, 0, 0, new Constraints(1.0, 1.0));
+    this.driveX = new PIDController(4, 0, 0);
+    this.driveY = new PIDController(4, 0, 0);
+    this.driveOmega =
+        new ProfiledPIDController(6.0, 0, 0, TagServoingConstants.driveOmegaConstraints);
     this.driveOmega.enableContinuousInput(Math.toRadians(-180), Math.toRadians(180));
 
-    this.alignX = new ProfiledPIDController(0.0017, 0, 0, new Constraints(1.0, 1.0)); // 0.0015
-    this.alignY = new ProfiledPIDController(0.00195, 0, 0, new Constraints(1.0, 1.5));
-    this.alignOmega = new ProfiledPIDController(6, 0, 0, new Constraints(1.0, 1.0));
-    this.alignOmega.enableContinuousInput(Math.toRadians(-180), Math.toRadians(180));
+    this.alignX = new PIDController(4, 0, 0); // 0.0015
+    this.alignY = new PIDController(4, 0, 0);
+    // this.alignOmega =
+    //     new ProfiledPIDController(6.0, 0, 0, TagServoingConstants.alignOmegaConstraints);
+    // this.alignOmega.enableContinuousInput(Math.toRadians(-180), Math.toRadians(180));
+
+    // If we revisit tag servoing later...
+    // this.servoX =
+    //     new ProfiledPIDController(4, 0, 0, TagServoingConstants.alignXConstraints);
+    // this.servoY = new ProfiledPIDController(4, 0, 0, TagServoingConstants.alignYConstraints);
 
-    Logger.recordOutput("TagAlignSubsystem/TargetDiag", -1);
-    Logger.recordOutput("TagAlignSubsystem/TargetTag", -1);
-    Logger.recordOutput("TagAlignSubsystem/TargetCenterX", -1);
     Logger.recordOutput("TagAlignSubsystem/Hexant", -1);
-    Logger.recordOutput("TagAlignSubsystem/GoalTargetDiag", -1);
+
+    driveRadius = 1.223823;
+    for (int i = 0; i < 6; i++) {
+      logger.info("Hexant {}, left and right", i);
+
+      logger.info(
+          "{}, {}",
+          getTargetDrivePose(Alliance.Blue, true, i).getX(),
+          getTargetDrivePose(Alliance.Blue, true, i).getY());
+      logger.info(
+          "{}, {}",
+          getTargetDrivePose(Alliance.Blue, false, i).getX(),
+          getTargetDrivePose(Alliance.Blue, false, i).getY());
+    }
+  }
+
+  public void setProceedToAlign(boolean proceed) {
+    this.proceedToAlign = proceed;
   }
 
   public int computeHexant(Alliance color) {
@@ -104,8 +135,7 @@ private Pose2d getTargetDrivePose(Alliance color, boolean scoreLeft) {
 
     Translation2d offset =
         new Translation2d(
-            TagServoingConstants.kInitialDriveRadius,
-            Rotation2d.fromDegrees(computeFieldRelHexant(color) * 60 + 180));
+            driveRadius, Rotation2d.fromDegrees(computeFieldRelHexant(color) * 60 + 180));
 
     Translation2d sideOffset =
         new Translation2d(
@@ -117,13 +147,31 @@ private Pose2d getTargetDrivePose(Alliance color, boolean scoreLeft) {
         Rotation2d.fromDegrees(computeFieldRelHexant(color) * 60));
   }
 
-  private double getCurRadius(Alliance color) {
+  private Pose2d getTargetDrivePose(Alliance color, boolean scoreLeft, int hexant) {
+    Translation2d reefT =
+        color == Alliance.Blue
+            ? TagServoingConstants.kBlueReefPose
+            : TagServoingConstants.kRedReefPose;
+
+    Translation2d offset =
+        new Translation2d(driveRadius, Rotation2d.fromDegrees(hexant * 60 + 180));
+
+    Translation2d sideOffset =
+        new Translation2d(
+            scoreLeft ? TagServoingConstants.kRightCamOffset : TagServoingConstants.kLeftCamOffset,
+            Rotation2d.fromDegrees(hexant * 60 + 180 + 90));
+
+    return new Pose2d(reefT.plus(offset).plus(sideOffset), Rotation2d.fromDegrees(hexant * 60));
+  }
+
+  public double getCurRadius(Alliance color) {
     Translation2d reefT =
         color == Alliance.Blue
             ? TagServoingConstants.kBlueReefPose
             : TagServoingConstants.kRedReefPose;
 
     Translation2d reefRelative = driveSubsystem.getPoseMeters().getTranslation().minus(reefT);
+
     return FastMath.hypot(reefRelative.getX(), reefRelative.getY());
   }
 
@@ -131,219 +179,253 @@ public TagAlignStates getState() {
     return curState;
   }
 
-  public void setup(Alliance alliance, boolean scoreLeft) {
-    targetPose = getTargetDrivePose(alliance, scoreLeft);
+  public void setup(Alliance alliance, boolean scoreLeft, boolean algae) {
     this.alliance = alliance;
-    this.goalTargetDiag =
-        scoreLeft
-            ? TagServoingConstants.kRightCamDiagTarget
-            : TagServoingConstants.kLeftCamDiagTarget;
+    this.scoreLeft = scoreLeft;
+    this.finalDrive = false;
+    // this.goalTargetDiag =
+    //     scoreLeft
+    //         ? TagServoingConstants.kRightCamDiagTarget
+    //         : TagServoingConstants.kLeftCamDiagTarget;
+
+    // this.stopXRadius =
+    //     algae
+    //         ? TagServoingConstants.kAlgaeStopXDriveRadius
+    //         : TagServoingConstants.kCoralStopXDriveRadius;
+    this.driveRadius =
+        algae
+            ? TagServoingConstants.kAlgaeInitialDriveRadius
+            : TagServoingConstants.kCoralInitialDriveRadius;
+    this.algae = algae;
+    this.driveXCloseEnough = TagServoingConstants.kInitialCloseEnough;
+    this.driveYCloseEnough = TagServoingConstants.kInitialCloseEnough;
+    this.currentThresCount = 0;
 
+    targetPose = getTargetDrivePose(alliance, scoreLeft);
     // Inverted, scoring left coral means aligning right camera
-    targetCamId =
-        !scoreLeft ? TagServoingConstants.kLeftServoCam : TagServoingConstants.kRightServoCam;
-    targetTagId =
-        alliance == Alliance.Blue
-            ? TagServoingConstants.kBlueTargetTag[computeHexant(alliance)]
-            : TagServoingConstants.kRedTargetTag[computeHexant(alliance)];
+    // targetCamId =
+    //     !scoreLeft ? TagServoingConstants.kLeftServoCam : TagServoingConstants.kRightServoCam;
+    // targetTagId =
+    //     alliance == Alliance.Blue
+    //         ? TagServoingConstants.kBlueTargetTag[computeHexant(alliance)]
+    //         : TagServoingConstants.kRedTargetTag[computeHexant(alliance)];
 
     fieldRelHexant = computeFieldRelHexant(alliance);
+    proceedToAlign = false;
 
-    Logger.recordOutput("TagAlignSubsystem/TargetTag", targetTagId);
-    Logger.recordOutput("TagAlignSubsystem/GoalTargetDiag", goalTargetDiag);
+    // Logger.recordOutput("TagAlignSubsystem/TargetTag", targetTagId);
+    // Logger.recordOutput("TagAlignSubsystem/GoalTargetDiag", goalTargetDiag);
     Logger.recordOutput("TagAlignSubsystem/TargetPose", targetPose);
+    Logger.recordOutput("TagAlignSubsystem/GettingAlgae", algae);
 
-    Pose2d current = driveSubsystem.getPoseMeters();
-
-    driveX.reset(current.getX());
-    driveY.reset(current.getY());
+    driveX.reset();
+    driveY.reset();
     driveOmega.reset(driveSubsystem.getGyroRotation2d().getRadians());
 
-    alignX.reset(0);
-    alignY.reset(0);
-    alignOmega.reset(driveSubsystem.getGyroRotation2d().getRadians());
+    alignX.reset();
+    alignY.reset();
+    // alignOmega.reset(driveSubsystem.getGyroRotation2d().getRadians());
   }
 
   // CALL setup() first!! Do not use in internal state machine
   public double calculateAlignY() {
-    WallEyeTagResult result = visionSubsystem.getLastResult(targetCamId);
+    return 2767;
 
-    if (result == null) {
-      return 2767;
-    }
+    // WallEyeTagResult result = visionSubsystem.getLastResult(targetCamId);
 
-    int tagIndex = -1;
-    int[] tags = result.getTagIDs();
+    // if (result == null) {
+    //   return 2767;
+    // }
 
-    for (int i = 0; i < tags.length; i++) {
-      if (tags[i] == targetTagId) {
-        tagIndex = i;
-        break;
-      }
-    }
+    // int tagIndex = -1;
+    // int[] tags = result.getTagIDs();
 
-    if (tagIndex == -1) {
-      // Target tag not found
-      return 2767;
-    }
+    // for (int i = 0; i < tags.length; i++) {
+    //   if (tags[i] == targetTagId) {
+    //     tagIndex = i;
+    //     break;
+    //   }
+    // }
+
+    // if (tagIndex == -1) {
+    //   // Target tag not found
+    //   return 2767;
+    // }
 
-    Point center = result.getTagCenters().get(tagIndex);
-    double diag = result.getTagDiags()[tagIndex];
+    // Point center = result.getTagCenters().get(tagIndex);
+    // double diag = result.getTagDiags()[tagIndex];
 
-    Logger.recordOutput("TagAlignSubsystem/TargetDiag", diag);
-    Logger.recordOutput("TagAlignSubsystem/TargetCenterX", center.x());
+    // Logger.recordOutput("TagAlignSubsystem/TargetDiag", diag);
+    // Logger.recordOutput("TagAlignSubsystem/TargetCenterX", center.x());
 
-    double vY = -alignY.calculate(TagServoingConstants.kHorizontalTarget - center.x(), 0);
+    // double vY = -alignY.calculate(TagServoingConstants.kHorizontalTarget - center.x(), 0);
 
-    return vY;
+    // return vY;
   }
 
-  public void start(Alliance alliance, boolean scoreLeft) {
-    setup(alliance, scoreLeft);
+  public void start(Alliance alliance, boolean scoreLeft, boolean algae) {
+    setup(alliance, scoreLeft, algae);
 
     curState = TagAlignStates.DRIVE;
   }
 
+  private void tagAlign() {
+    alignX.reset();
+    alignY.reset();
+
+    curState = TagAlignStates.TAG_ALIGN;
+
+    this.driveXCloseEnough =
+        algae
+            ? TagServoingConstants.kAlgaeDriveXCloseEnough
+            : TagServoingConstants.kCoralDriveXCloseEnough;
+
+    this.driveYCloseEnough =
+        algae
+            ? TagServoingConstants.kAlgaeDriveYCloseEnough
+            : TagServoingConstants.kCoralDriveYCloseEnough;
+
+    // this.goalTargetDiag =
+    //     scoreLeft
+    //         ? TagServoingConstants.kRightCamDiagTarget
+    //         : TagServoingConstants.kLeftCamDiagTarget;
+
+    // this.stopXRadius =
+    //     algae
+    //         ? TagServoingConstants.kAlgaeStopXDriveRadius
+    //         : TagServoingConstants.kCoralStopXDriveRadius;
+    this.driveRadius =
+        algae ? TagServoingConstants.kAlgaeAlignRadius : TagServoingConstants.kCoralAlignRadius;
+    targetPose = getTargetDrivePose(alliance, scoreLeft);
+    Logger.recordOutput("TagAlignSubsystem/TargetPose", targetPose);
+  }
+
   public void terminate() {
-    driveSubsystem.move(0, 0, 0, false);
-    driveSubsystem.drive(0, 0, 0);
+    driveSubsystem.setIgnoreSticks(false);
+    driveSubsystem.stopDriving();
     curState = TagAlignStates.DONE;
   }
 
   @Override
   public void periodic() {
     Logger.recordOutput("TagAlignSubsystem/State", curState.toString());
+    Logger.recordOutput("TagAlignSubsystem/FinalDrive", finalDrive);
     // Logger.recordOutput("TagAlignSubsystem/Hexant", computeHexant(alliance));
 
     switch (curState) {
-      case DRIVE -> {
+      case DRIVE, TAG_ALIGN -> {
         Pose2d current = driveSubsystem.getPoseMeters();
 
-        double vX = driveX.calculate(current.getX(), targetPose.getX());
-        double vY = driveY.calculate(current.getY(), targetPose.getY());
+        double vX = 0;
+        double vY = 0;
         double vOmega =
             driveOmega.calculate(
                 current.getRotation().getRadians(), targetPose.getRotation().getRadians());
 
-        Logger.recordOutput("TagAlignSubsystem/DriveXError", driveX.getPositionError());
-        Logger.recordOutput("TagAlignSubsystem/DriveYError", driveY.getPositionError());
-        Logger.recordOutput("TagAlignSubsystem/DriveOmegaError", driveOmega.getPositionError());
-
-        Translation2d tagRelVel =
-            new Translation2d(vX, vY)
+        Translation2d rotated =
+            current
+                .getTranslation()
                 .rotateBy(
                     Rotation2d.fromRadians(-TagServoingConstants.kAngleTarget[fieldRelHexant]));
-
-        double tagRelX = tagRelVel.getX();
-        double tagRelY = tagRelVel.getY();
-
-        double radius = getCurRadius(alliance);
-        boolean ignoreX = radius < TagServoingConstants.kStopXDriveRadius;
-
-        tagRelX = tagRelX < TagServoingConstants.kMinVelX ? TagServoingConstants.kMinVelX : tagRelX;
-
-        if (radius < TagServoingConstants.kStopXDriveRadius) {
-          tagRelX = 0;
-        }
-
-        Translation2d tagRelError =
+        Translation2d rotatedTarget =
             targetPose
-                .minus(current)
                 .getTranslation()
                 .rotateBy(
                     Rotation2d.fromRadians(-TagServoingConstants.kAngleTarget[fieldRelHexant]));
 
-        Logger.recordOutput("TagAlignSubsystem/Tag Rel Y Error", tagRelError.getY());
-
-        Transform2d poseError = targetPose.minus(current);
-
-        if (FastMath.abs(driveOmega.getPositionError()) < TagServoingConstants.kAngleCloseEnough
-            && (FastMath.hypot(poseError.getX(), poseError.getY())
-                    < TagServoingConstants.kDriveCloseEnough
-                || ignoreX
-                    && FastMath.abs(tagRelError.getY()) < TagServoingConstants.kDriveCloseEnough)) {
-          alignX.reset(0);
-          alignY.reset(0);
-          alignOmega.reset(driveSubsystem.getGyroRotation2d().getRadians());
-
-          curState = TagAlignStates.TAG_ALIGN;
-          startServoTime = RobotController.getFPGATime();
-          break;
+        switch (curState) {
+          case DRIVE -> {
+            vX = driveX.calculate(rotated.getX(), rotatedTarget.getX());
+            vY = driveY.calculate(rotated.getY(), rotatedTarget.getY());
+            Logger.recordOutput("TagAlignSubsystem/DriveXError", driveX.getError());
+            Logger.recordOutput("TagAlignSubsystem/DriveYError", driveY.getError());
+          }
+          case TAG_ALIGN -> {
+            vX = alignX.calculate(rotated.getX(), rotatedTarget.getX());
+            vY = alignY.calculate(rotated.getY(), rotatedTarget.getY());
+            Logger.recordOutput("TagAlignSubsystem/DriveXError", alignX.getError());
+            Logger.recordOutput("TagAlignSubsystem/DriveYError", alignY.getError());
+          }
+          default -> {}
         }
 
-        Logger.recordOutput("TagAlignSubsystem/Tag Rel vX", tagRelX);
-        Logger.recordOutput("TagAlignSubsystem/Tag Rel vY", tagRelY);
-
-        Translation2d adjusted =
-            new Translation2d(tagRelX, tagRelY)
-                .rotateBy(
-                    Rotation2d.fromRadians(TagServoingConstants.kAngleTarget[fieldRelHexant]));
-
-        driveSubsystem.move(adjusted.getX(), adjusted.getY(), vOmega, true);
-      }
+        if (vX > 2) vX = 2;
+        if (vY > 2) vY = 2;
 
-      case TAG_ALIGN -> {
-        WallEyeTagResult result = visionSubsystem.getLastResult(targetCamId);
-
-        if (result == null) {
-          return;
-        }
-
-        int tagIndex = -1;
-        int[] tags = result.getTagIDs();
+        Logger.recordOutput("TagAlignSubsystem/DriveOmegaError", driveOmega.getPositionError());
 
-        for (int i = 0; i < tags.length; i++) {
-          if (tags[i] == targetTagId) {
-            tagIndex = i;
-            break;
+        // double radius = getCurRadius(alliance);
+        boolean ignoreX = false; // radius < stopXRadius && !algae;
+
+        // if (!algae || tagRelX > 0) {
+        //   tagRelX =
+        //       tagRelX < TagServoingConstants.kMinVelX ? TagServoingConstants.kMinVelX : tagRelX;
+        // }
+
+        // if (ignoreX) {
+        //   tagRelX = 0;
+        // }
+
+        // Translation2d poseError = targetPose.getTranslation().minus(current.getTranslation());
+
+        if (FastMath.abs(driveOmega.getPositionError()) < TagServoingConstants.kAngleCloseEnough) {
+          switch (curState) {
+            case DRIVE -> {
+              if (FastMath.abs(driveX.getError()) < driveXCloseEnough
+                      && FastMath.abs(driveY.getError()) < driveYCloseEnough
+                  // || ignoreX && FastMath.abs(tagRelError.getY()) < driveCloseEnough
+                  || FastMath.abs(driveY.getError()) < driveYCloseEnough) {
+                if (proceedToAlign || !algae) {
+                  tagAlign();
+                  break;
+                } else {
+                  driveSubsystem.stopDriving();
+                  curState = TagAlignStates.WAITING;
+                  break;
+                }
+              }
+            }
+            case TAG_ALIGN -> {
+              if (FastMath.abs(alignX.getError()) < driveXCloseEnough
+                  && FastMath.abs(alignY.getError()) < driveYCloseEnough) {
+                finalDrive = true;
+              }
+              if (finalDrive
+                  && driveSubsystem.getAvgDriveCurrent()
+                      > TagServoingConstants.kEndDriveCurrentThreshold
+                  && driveSubsystem.getAvgRearDriveVel() < TagServoingConstants.kEndVelThreshold) {
+
+                currentThresCount++;
+                if (currentThresCount >= TagServoingConstants.kEndCountThreshold) {
+                  terminate();
+                  break;
+                }
+              } else {
+                currentThresCount = 0;
+              }
+            }
           }
         }
 
-        if (tagIndex == -1) {
-          return;
+        if (finalDrive) {
+          vX = 0.25;
         }
 
-        Point center = result.getTagCenters().get(tagIndex);
-        double diag = result.getTagDiags()[tagIndex];
-
-        Logger.recordOutput("TagAlignSubsystem/TargetDiag", diag);
-        Logger.recordOutput("TagAlignSubsystem/TargetCenterX", center.x());
-
-        double vX = -alignX.calculate(goalTargetDiag - diag, 0);
-        double vY = -alignY.calculate(TagServoingConstants.kHorizontalTarget - center.x(), 0);
-
-        Logger.recordOutput("TagAlignSubsystem/X Error", alignX.getPositionError());
-        Logger.recordOutput("TagAlignSubsystem/Y Error", alignY.getPositionError());
-        Logger.recordOutput(
-            "TagAlignSubsystem/Last result delay",
-            (RobotController.getFPGATime() - result.getTimeStamp()) / 1000);
-
-        if (RobotController.getFPGATime() - result.getTimeStamp()
-                > TagServoingConstants.kNoUpdateMicrosec
-            && RobotController.getFPGATime() - startServoTime
-                > TagServoingConstants.kNoUpdateMicrosec) {
-          terminate();
-          break;
-        }
+        Logger.recordOutput("TagAlignSubsystem/Tag Rel Drive vX", vX);
+        Logger.recordOutput("TagAlignSubsystem/Tag Rel Drive vY", vY);
 
-        if (FastMath.abs(goalTargetDiag - diag) < TagServoingConstants.kDiagCloseEnough
-            || diag > goalTargetDiag) {
-          vX = 0;
+        Translation2d fieldRelV =
+            new Translation2d(vX, vY)
+                .rotateBy(
+                    Rotation2d.fromRadians(TagServoingConstants.kAngleTarget[fieldRelHexant]));
 
-          if (FastMath.abs(TagServoingConstants.kHorizontalTarget - center.x())
-              < TagServoingConstants.kHorizontalCloseEnough) {
+        driveSubsystem.move(fieldRelV.getX(), fieldRelV.getY(), vOmega, true);
+      }
 
-            terminate();
-            break;
-          }
+      case WAITING -> {
+        if (proceedToAlign || !algae) {
+          tagAlign();
         }
-
-        double vOmega =
-            alignOmega.calculate(
-                driveSubsystem.getGyroRotation2d().getRadians(),
-                targetPose.getRotation().getRadians());
-
-        driveSubsystem.move(vX, vY, vOmega, false);
       }
 
       case DONE -> {}
@@ -357,6 +439,7 @@ public Set<Measure> getMeasures() {
 
   public enum TagAlignStates {
     DRIVE,
+    WAITING,
     TAG_ALIGN,
     DONE
   }
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
index bc423cd9..bdaeefae 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
@@ -43,6 +43,14 @@ public class VisionSubsystem extends MeasurableSubsystem {
     VisionConstants.kCam5Pose.getTranslation()
   };
 
+  private double[] camHeights = {
+    camPositions[0].getZ(),
+    camPositions[1].getZ(),
+    camPositions[2].getZ(),
+    camPositions[3].getZ(),
+    camPositions[4].getZ()
+  };
+
   // Array of camera rotations
   private Rotation3d[] camRotations = {
     VisionConstants.kCam1Pose.getRotation(),
@@ -292,24 +300,39 @@ private double getStdDevFactor(double distance, int numTags, String camName) {
   private Pose3d getCloserPose(Pose3d pose1, Pose3d pose2, double rotation) {
     /*Which pose rotation is closer to our gyro. We subtract the absolute value of the gyro
     from the rotation of the pose and compare the two*/
-    double pose1Error =
-        FastMath.abs(
-            Rotation2d.fromRadians(rotation)
-                .minus(pose1.getRotation().toRotation2d())
-                .getRadians());
-    double pose2Error =
-        FastMath.abs(
-            Rotation2d.fromRadians(rotation)
-                .minus(pose2.getRotation().toRotation2d())
-                .getRadians());
 
+    double pose1Error = 2767;
+    double pose2Error = 2767;
+    if (pose1 != null) {
+      pose1Error =
+          FastMath.abs(
+              Rotation2d.fromRadians(rotation)
+                  .minus(pose1.getRotation().toRotation2d())
+                  .getRadians());
+    }
+    if (pose2 != null) {
+      pose2Error =
+          FastMath.abs(
+              Rotation2d.fromRadians(rotation)
+                  .minus(pose2.getRotation().toRotation2d())
+                  .getRadians());
+    }
     Logger.recordOutput("Vision/Pose 1 Yaw Error", pose1Error);
     Logger.recordOutput("Vision/Pose 2 Yaw Error", pose2Error);
     Logger.recordOutput("Vision/Historical yaw", rotation);
 
-    if (pose1Error < pose2Error) {
-      if (pose1Error > VisionConstants.kYawErrorThreshold) {}
+    if (pose1Error > VisionConstants.kYawErrorThreshold) {
+      pose1 = null;
+    }
+    if (pose2Error > VisionConstants.kYawErrorThreshold) {
+      pose2 = null;
+    }
+
+    if (pose1 == null && pose2 == null) {
+      return null;
+    }
 
+    if (pose1Error < pose2Error) {
       return pose1;
     } else {
       return pose2;
@@ -317,17 +340,25 @@ private Pose3d getCloserPose(Pose3d pose1, Pose3d pose2, double rotation) {
   }
 
   private Pose3d getCorrectPose(Pose3d pose1, Pose3d pose2, double time, int camIndex) {
-    // double dist1 = Math.abs(camHeights[camIndex] - pose1.getZ());
-    // double dist2 = Math.abs(camHeights[camIndex] - pose2.getZ());
+    double dist1 = Math.abs(camHeights[camIndex] - pose1.getZ());
+    double dist2 = Math.abs(camHeights[camIndex] - pose2.getZ());
+
     // // This filters out results by seeing if they are close to the right height
-    // if (dist1 < dist2 && dist1 < 0.5) {
-    //   return pose1;
-    // }
-    // if (dist2 < dist1 && dist2 < 0.5) {
-    //   return pose2;
-    // }
+    if (dist1 > VisionConstants.kCamErrorZThreshold) {
+      pose1 = null;
+    }
+    if (dist2 > VisionConstants.kCamErrorZThreshold) {
+      pose2 = null;
+    }
+
     // If we don't have enough data in the gyro buffer we default to returning a pose
-    if (gyroBuffer.size() < VisionConstants.kCircularBufferSize) return pose1;
+    if (gyroBuffer.size() < VisionConstants.kCircularBufferSize) {
+      if (pose1 != null) {
+        return pose1;
+      } else {
+        return pose2;
+      }
+    }
 
     // See what pose is closer the the gyro at the time of the photo's capture.
     double rotation =
@@ -437,12 +468,17 @@ public void periodic() {
           Logger.recordOutput("Vision/Raw Camera 2 " + camNames[idx], cam2Pose);
 
           cameraPose = getCorrectPose(cam1Pose, cam2Pose, result.getTimeStamp(), idx);
+
+          if (cameraPose == null) {
+            continue;
+          }
+
+          cameraRotation = cameraPose.getRotation();
           robotTranslation =
               cameraPose
                   .getTranslation()
-                  .minus(camPositions[idx].rotateBy(cameraPose.getRotation()))
+                  .minus(camPositions[idx].rotateBy(cameraRotation))
                   .rotateBy(camRotations[idx]);
-          cameraRotation = cameraPose.getRotation();
         }
         Pose2d robotPose =
             new Pose2d(robotTranslation.toTranslation2d(), cameraRotation.toRotation2d());
diff --git a/vendordeps/thirdcoast.json b/vendordeps/thirdcoast.json
index b1be2433..9f049a00 100644
--- a/vendordeps/thirdcoast.json
+++ b/vendordeps/thirdcoast.json
@@ -1,7 +1,7 @@
 {
     "fileName": "thirdcoast.json",
     "name": "StrykeForce-ThirdCoast",
-    "version": "25.0.4",
+    "version": "25.0.7",
     "uuid": "13c4f4b5-a1c0-4670-8f8d-b7b03628c0d3",
     "frcYear": "2025",
     "mavenUrls": [
@@ -12,7 +12,7 @@
         {
             "groupId": "org.strykeforce",
             "artifactId": "thirdcoast",
-            "version": "25.0.4"
+            "version": "25.0.7"
         }
     ],
     "jniDependencies": [],