From b174f26a9d902351c39e637b2663c84412217023 Mon Sep 17 00:00:00 2001
From: NandaGuntupalli <nanda.guntupalli@gmail.com>
Date: Wed, 21 Jan 2026 20:32:21 -0500
Subject: [PATCH 1/2] Created Drive over Bump Command and Auto Trajectories

---
 .../deploy/choreo/AllianceLSideToClimb.traj   |  85 ++++++++++
 .../deploy/choreo/AllianceRSideToClimb.traj   |  86 +++++++++++
 .../choreo/BallPoseR2_To_MidRBumper.traj      |  30 ++++
 .../deploy/choreo/BallPoseR_ToMidRBumper.traj |  30 ++++
 src/main/deploy/choreo/BumperLToClimb.traj    |  30 ++++
 src/main/deploy/choreo/BumperRToClimb.traj    |  80 ++++++++++
 .../choreo/MidRBumper_To_BallPoseR.traj       |  30 ++++
 .../choreo/MidRBumper_To_BallPoseR2.traj      |  30 ++++
 src/main/deploy/choreo/MidToAllianceL.traj    |  73 +++++----
 src/main/deploy/choreo/MidToAllianceR.traj    | 124 ++++++++++-----
 src/main/deploy/choreo/autos.chor             | 146 +++++++++++++++---
 src/main/deploy/pathplanner/navgrid.json      |   2 +-
 src/main/java/frc/robot/RobotContainer.java   |   4 +
 13 files changed, 649 insertions(+), 101 deletions(-)
 create mode 100644 src/main/deploy/choreo/AllianceLSideToClimb.traj
 create mode 100644 src/main/deploy/choreo/AllianceRSideToClimb.traj
 create mode 100644 src/main/deploy/choreo/BallPoseR2_To_MidRBumper.traj
 create mode 100644 src/main/deploy/choreo/BallPoseR_ToMidRBumper.traj
 create mode 100644 src/main/deploy/choreo/BumperLToClimb.traj
 create mode 100644 src/main/deploy/choreo/BumperRToClimb.traj
 create mode 100644 src/main/deploy/choreo/MidRBumper_To_BallPoseR.traj
 create mode 100644 src/main/deploy/choreo/MidRBumper_To_BallPoseR2.traj

diff --git a/src/main/deploy/choreo/AllianceLSideToClimb.traj b/src/main/deploy/choreo/AllianceLSideToClimb.traj
new file mode 100644
index 0000000..a83d142
--- /dev/null
+++ b/src/main/deploy/choreo/AllianceLSideToClimb.traj
@@ -0,0 +1,85 @@
+{
+ "name":"AllianceLSideToClimb",
+ "version":3,
+ "snapshot":{
+  "waypoints":[
+    {"x":3.606555223464966, "y":7.434945106506348, "heading":0.0, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":1.504361629486084, "y":4.103729248046875, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"AllianceLSide.x", "val":3.606555223464966}, "y":{"exp":"AllianceLSide.y", "val":7.434945106506348}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"ClimbPose.x", "val":1.504361629486084}, "y":{"exp":"ClimbPose.y", "val":4.103729248046875}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "config":{
+   "frontLeft":{
+    "x":0.2794,
+    "y":0.2794
+   },
+   "backLeft":{
+    "x":-0.2794,
+    "y":0.2794
+   },
+   "mass":68.0388555,
+   "inertia":6.0,
+   "gearing":6.5,
+   "radius":0.0508,
+   "vmax":628.3185307179587,
+   "tmax":1.2,
+   "cof":1.5,
+   "bumper":{
+    "front":0.4064,
+    "side":0.4064,
+    "back":0.4064
+   },
+   "differentialTrackWidth":0.5588
+  },
+  "sampleType":"Swerve",
+  "waypoints":[0.0,1.34758],
+  "samples":[
+    {"t":0.0, "x":3.60656, "y":7.43495, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.81334, "ay":-7.6274, "alpha":0.0, "fx":[-81.87356,-81.87356,-81.87356,-81.87356], "fy":[-129.73996,-129.73996,-129.73996,-129.73996]},
+    {"t":0.04991, "x":3.60056, "y":7.42545, "heading":0.0, "vx":-0.24024, "vy":-0.38069, "omega":0.0, "ax":-4.81289, "ay":-7.62669, "alpha":0.0, "fx":[-81.86594,-81.86594,-81.86594,-81.86594], "fy":[-129.72788,-129.72788,-129.72788,-129.72788]},
+    {"t":0.09982, "x":3.58258, "y":7.39695, "heading":0.0, "vx":-0.48045, "vy":-0.76134, "omega":0.0, "ax":-4.81233, "ay":-7.62581, "alpha":0.0, "fx":[-81.85643,-81.85643,-81.85643,-81.85643], "fy":[-129.71281,-129.71281,-129.71281,-129.71281]},
+    {"t":0.14973, "x":3.5526, "y":7.34945, "heading":0.0, "vx":-0.72063, "vy":-1.14194, "omega":0.0, "ax":-4.81162, "ay":-7.62467, "alpha":0.0, "fx":[-81.84424,-81.84424,-81.84424,-81.84424], "fy":[-129.69349,-129.69349,-129.69349,-129.69349]},
+    {"t":0.19964, "x":3.51064, "y":7.28296, "heading":0.0, "vx":-0.96078, "vy":-1.52249, "omega":0.0, "ax":-4.81067, "ay":-7.62316, "alpha":0.0, "fx":[-81.82804,-81.82804,-81.82804,-81.82804], "fy":[-129.66783,-129.66783,-129.66783,-129.66783]},
+    {"t":0.24955, "x":3.4567, "y":7.19748, "heading":0.0, "vx":-1.20088, "vy":-1.90297, "omega":0.0, "ax":-4.80934, "ay":-7.62106, "alpha":0.0, "fx":[-81.80548,-81.80548,-81.80548,-81.80548], "fy":[-129.63207,-129.63207,-129.63207,-129.63207]},
+    {"t":0.29946, "x":3.39077, "y":7.09301, "heading":0.0, "vx":-1.44092, "vy":-2.28333, "omega":0.0, "ax":-4.80736, "ay":-7.61793, "alpha":0.0, "fx":[-81.77188,-81.77188,-81.77188,-81.77188], "fy":[-129.57884,-129.57884,-129.57884,-129.57884]},
+    {"t":0.34937, "x":3.31287, "y":6.96956, "heading":0.0, "vx":-1.68086, "vy":-2.66355, "omega":0.0, "ax":-4.80411, "ay":-7.61278, "alpha":0.0, "fx":[-81.71656,-81.71656,-81.71656,-81.71656], "fy":[-129.49116,-129.49116,-129.49116,-129.49116]},
+    {"t":0.39928, "x":3.22299, "y":6.82714, "heading":0.0, "vx":-1.92063, "vy":-3.0435, "omega":0.0, "ax":-4.79775, "ay":-7.6027, "alpha":0.0, "fx":[-81.60837,-81.60837,-81.60837,-81.60837], "fy":[-129.31973,-129.31973,-129.31973,-129.31973]},
+    {"t":0.44919, "x":3.12116, "y":6.66576, "heading":0.0, "vx":-2.16009, "vy":-3.42296, "omega":0.0, "ax":-4.77979, "ay":-7.57423, "alpha":0.0, "fx":[-81.3028,-81.3028,-81.3028,-81.3028], "fy":[-128.83551,-128.83551,-128.83551,-128.83551]},
+    {"t":0.4991, "x":3.00739, "y":6.48549, "heading":0.0, "vx":-2.39865, "vy":-3.80099, "omega":0.0, "ax":-4.40289, "ay":-6.97699, "alpha":0.0, "fx":[-74.89194,-74.89194,-74.89194,-74.89194], "fy":[-118.67661,-118.67661,-118.67661,-118.67661]},
+    {"t":0.54901, "x":2.88219, "y":6.28709, "heading":0.0, "vx":-2.6184, "vy":-4.14921, "omega":0.0, "ax":-0.00427, "ay":-0.00677, "alpha":0.0, "fx":[-0.0727,-0.0727,-0.0727,-0.0727], "fy":[-0.1152,-0.1152,-0.1152,-0.1152]},
+    {"t":0.59892, "x":2.7515, "y":6.07999, "heading":0.0, "vx":-2.61861, "vy":-4.14955, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[-0.00001,-0.00001,-0.00001,-0.00001], "fy":[-0.00002,-0.00002,-0.00002,-0.00002]},
+    {"t":0.64883, "x":2.62081, "y":5.87289, "heading":0.0, "vx":-2.61861, "vy":-4.14955, "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.69874, "x":2.49011, "y":5.66578, "heading":0.0, "vx":-2.61861, "vy":-4.14955, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00001,0.00001,0.00001,0.00001], "fy":[0.00002,0.00002,0.00002,0.00002]},
+    {"t":0.74865, "x":2.35942, "y":5.45868, "heading":0.0, "vx":-2.61861, "vy":-4.14955, "omega":0.0, "ax":0.00427, "ay":0.00677, "alpha":0.0, "fx":[0.0727,0.0727,0.0727,0.0727], "fy":[0.1152,0.1152,0.1152,0.1152]},
+    {"t":0.79856, "x":2.22873, "y":5.25158, "heading":0.0, "vx":-2.6184, "vy":-4.14921, "omega":0.0, "ax":4.40289, "ay":6.97699, "alpha":0.0, "fx":[74.89194,74.89194,74.89194,74.89194], "fy":[118.67661,118.67661,118.67661,118.67661]},
+    {"t":0.84847, "x":2.10352, "y":5.05318, "heading":0.0, "vx":-2.39865, "vy":-3.80099, "omega":0.0, "ax":4.77979, "ay":7.57423, "alpha":0.0, "fx":[81.3028,81.3028,81.3028,81.3028], "fy":[128.83551,128.83551,128.83551,128.83551]},
+    {"t":0.89838, "x":1.98976, "y":4.87291, "heading":0.0, "vx":-2.16009, "vy":-3.42296, "omega":0.0, "ax":4.79775, "ay":7.6027, "alpha":0.0, "fx":[81.60837,81.60837,81.60837,81.60837], "fy":[129.31973,129.31973,129.31973,129.31973]},
+    {"t":0.94829, "x":1.88793, "y":4.71154, "heading":0.0, "vx":-1.92063, "vy":-3.0435, "omega":0.0, "ax":4.80411, "ay":7.61278, "alpha":0.0, "fx":[81.71656,81.71656,81.71656,81.71656], "fy":[129.49116,129.49116,129.49116,129.49116]},
+    {"t":0.99821, "x":1.79805, "y":4.56912, "heading":0.0, "vx":-1.68086, "vy":-2.66355, "omega":0.0, "ax":4.80736, "ay":7.61793, "alpha":0.0, "fx":[81.77188,81.77188,81.77188,81.77188], "fy":[129.57884,129.57884,129.57884,129.57884]},
+    {"t":1.04812, "x":1.72015, "y":4.44567, "heading":0.0, "vx":-1.44092, "vy":-2.28333, "omega":0.0, "ax":4.80934, "ay":7.62106, "alpha":0.0, "fx":[81.80548,81.80548,81.80548,81.80548], "fy":[129.63207,129.63207,129.63207,129.63207]},
+    {"t":1.09803, "x":1.65422, "y":4.3412, "heading":0.0, "vx":-1.20088, "vy":-1.90297, "omega":0.0, "ax":4.81067, "ay":7.62316, "alpha":0.0, "fx":[81.82804,81.82804,81.82804,81.82804], "fy":[129.66783,129.66783,129.66783,129.66783]},
+    {"t":1.14794, "x":1.60027, "y":4.25572, "heading":0.0, "vx":-0.96078, "vy":-1.52249, "omega":0.0, "ax":4.81162, "ay":7.62467, "alpha":0.0, "fx":[81.84424,81.84424,81.84424,81.84424], "fy":[129.69349,129.69349,129.69349,129.69349]},
+    {"t":1.19785, "x":1.55831, "y":4.18923, "heading":0.0, "vx":-0.72063, "vy":-1.14194, "omega":0.0, "ax":4.81233, "ay":7.62581, "alpha":0.0, "fx":[81.85643,81.85643,81.85643,81.85643], "fy":[129.71281,129.71281,129.71281,129.71281]},
+    {"t":1.24776, "x":1.52834, "y":4.14173, "heading":0.0, "vx":-0.48045, "vy":-0.76134, "omega":0.0, "ax":4.81289, "ay":7.62669, "alpha":0.0, "fx":[81.86594,81.86594,81.86594,81.86594], "fy":[129.72788,129.72788,129.72788,129.72788]},
+    {"t":1.29767, "x":1.51036, "y":4.11323, "heading":0.0, "vx":-0.24024, "vy":-0.38069, "omega":0.0, "ax":4.81334, "ay":7.6274, "alpha":0.0, "fx":[81.87356,81.87356,81.87356,81.87356], "fy":[129.73996,129.73996,129.73996,129.73996]},
+    {"t":1.34758, "x":1.50436, "y":4.10373, "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/AllianceRSideToClimb.traj b/src/main/deploy/choreo/AllianceRSideToClimb.traj
new file mode 100644
index 0000000..4ba5aec
--- /dev/null
+++ b/src/main/deploy/choreo/AllianceRSideToClimb.traj
@@ -0,0 +1,86 @@
+{
+ "name":"AllianceRSideToClimb",
+ "version":3,
+ "snapshot":{
+  "waypoints":[
+    {"x":3.606555223464966, "y":0.6462193727493286, "heading":0.0, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":1.504361629486084, "y":4.103729248046875, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"AllianceRSide.x", "val":3.606555223464966}, "y":{"exp":"AllianceRSide.y", "val":0.6462193727493286}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":28, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"ClimbPose.x", "val":1.504361629486084}, "y":{"exp":"ClimbPose.y", "val":4.103729248046875}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "config":{
+   "frontLeft":{
+    "x":0.2794,
+    "y":0.2794
+   },
+   "backLeft":{
+    "x":-0.2794,
+    "y":0.2794
+   },
+   "mass":68.0388555,
+   "inertia":6.0,
+   "gearing":6.5,
+   "radius":0.0508,
+   "vmax":628.3185307179587,
+   "tmax":1.2,
+   "cof":1.5,
+   "bumper":{
+    "front":0.4064,
+    "side":0.4064,
+    "back":0.4064
+   },
+   "differentialTrackWidth":0.5588
+  },
+  "sampleType":"Swerve",
+  "waypoints":[0.0,1.36961],
+  "samples":[
+    {"t":0.0, "x":3.60656, "y":0.64622, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-4.68561, "ay":7.70649, "alpha":0.0, "fx":[-79.70082,-79.70082,-79.70082,-79.70082], "fy":[131.08515,131.08515,131.08515,131.08515]},
+    {"t":0.04891, "x":3.60095, "y":0.65544, "heading":0.0, "vx":-0.22919, "vy":0.37696, "omega":0.0, "ax":-4.6852, "ay":7.70583, "alpha":0.0, "fx":[-79.694,-79.694,-79.694,-79.694], "fy":[131.07393,131.07393,131.07393,131.07393]},
+    {"t":0.09783, "x":3.58413, "y":0.6831, "heading":0.0, "vx":-0.45837, "vy":0.75389, "omega":0.0, "ax":-4.68471, "ay":7.70502, "alpha":0.0, "fx":[-79.68565,-79.68565,-79.68565,-79.68565], "fy":[131.0602,131.0602,131.0602,131.0602]},
+    {"t":0.14674, "x":3.55611, "y":0.72919, "heading":0.0, "vx":-0.68752, "vy":1.13078, "omega":0.0, "ax":-4.6841, "ay":7.70401, "alpha":0.0, "fx":[-79.6752,-79.6752,-79.6752,-79.6752], "fy":[131.04301,131.04301,131.04301,131.04301]},
+    {"t":0.19566, "x":3.51687, "y":0.79372, "heading":0.0, "vx":-0.91664, "vy":1.50762, "omega":0.0, "ax":-4.68331, "ay":7.70271, "alpha":0.0, "fx":[-79.66173,-79.66173,-79.66173,-79.66173], "fy":[131.02087,131.02087,131.02087,131.02087]},
+    {"t":0.24457, "x":3.46643, "y":0.87668, "heading":0.0, "vx":-1.14573, "vy":1.88439, "omega":0.0, "ax":-4.68225, "ay":7.70097, "alpha":0.0, "fx":[-79.64374,-79.64374,-79.64374,-79.64374], "fy":[130.99127,130.99127,130.99127,130.99127]},
+    {"t":0.29349, "x":3.40479, "y":0.97807, "heading":0.0, "vx":-1.37476, "vy":2.26108, "omega":0.0, "ax":-4.68076, "ay":7.69852, "alpha":0.0, "fx":[-79.61846,-79.61846,-79.61846,-79.61846], "fy":[130.94969,130.94969,130.94969,130.94969]},
+    {"t":0.3424, "x":3.33194, "y":1.09788, "heading":0.0, "vx":-1.60371, "vy":2.63765, "omega":0.0, "ax":-4.67852, "ay":7.69484, "alpha":0.0, "fx":[-79.58036,-79.58036,-79.58036,-79.58036], "fy":[130.88703,130.88703,130.88703,130.88703]},
+    {"t":0.39132, "x":3.2479, "y":1.2361, "heading":0.0, "vx":-1.83256, "vy":3.01404, "omega":0.0, "ax":-4.67476, "ay":7.68865, "alpha":0.0, "fx":[-79.51639,-79.51639,-79.51639,-79.51639], "fy":[130.78181,130.78181,130.78181,130.78181]},
+    {"t":0.44023, "x":3.15267, "y":1.39273, "heading":0.0, "vx":-2.06123, "vy":3.39013, "omega":0.0, "ax":-4.66715, "ay":7.67612, "alpha":0.0, "fx":[-79.3868,-79.3868,-79.3868,-79.3868], "fy":[130.56869,130.56869,130.56869,130.56869]},
+    {"t":0.48915, "x":3.04626, "y":1.56774, "heading":0.0, "vx":-2.28952, "vy":3.76561, "omega":0.0, "ax":-4.64361, "ay":7.63741, "alpha":0.0, "fx":[-78.9864,-78.9864,-78.9864,-78.9864], "fy":[129.91013,129.91013,129.91013,129.91013]},
+    {"t":0.53806, "x":2.92872, "y":1.76107, "heading":0.0, "vx":-2.51666, "vy":4.13919, "omega":0.0, "ax":-0.66091, "ay":1.08701, "alpha":0.0, "fx":[-11.2419,-11.2419,-11.2419,-11.2419], "fy":[18.48973,18.48973,18.48973,18.48973]},
+    {"t":0.58698, "x":2.80482, "y":1.96484, "heading":0.0, "vx":-2.54899, "vy":4.19236, "omega":0.0, "ax":-0.00012, "ay":0.00019, "alpha":0.0, "fx":[-0.00198,-0.00198,-0.00198,-0.00198], "fy":[0.00325,0.00325,0.00325,0.00325]},
+    {"t":0.63589, "x":2.68014, "y":2.16991, "heading":0.0, "vx":-2.54899, "vy":4.19237, "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.68481, "x":2.55546, "y":2.37497, "heading":0.0, "vx":-2.54899, "vy":4.19237, "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.73372, "x":2.43078, "y":2.58004, "heading":0.0, "vx":-2.54899, "vy":4.19237, "omega":0.0, "ax":0.00012, "ay":-0.00019, "alpha":0.0, "fx":[0.00198,0.00198,0.00198,0.00198], "fy":[-0.00325,-0.00325,-0.00325,-0.00325]},
+    {"t":0.78263, "x":2.30609, "y":2.78511, "heading":0.0, "vx":-2.54899, "vy":4.19236, "omega":0.0, "ax":0.66091, "ay":-1.08701, "alpha":0.0, "fx":[11.2419,11.2419,11.2419,11.2419], "fy":[-18.48973,-18.48973,-18.48973,-18.48973]},
+    {"t":0.83155, "x":2.1822, "y":2.98888, "heading":0.0, "vx":-2.51666, "vy":4.13919, "omega":0.0, "ax":4.64361, "ay":-7.63741, "alpha":0.0, "fx":[78.9864,78.9864,78.9864,78.9864], "fy":[-129.91013,-129.91013,-129.91013,-129.91013]},
+    {"t":0.88046, "x":2.06465, "y":3.18221, "heading":0.0, "vx":-2.28952, "vy":3.76561, "omega":0.0, "ax":4.66715, "ay":-7.67612, "alpha":0.0, "fx":[79.3868,79.3868,79.3868,79.3868], "fy":[-130.56869,-130.56869,-130.56869,-130.56869]},
+    {"t":0.92938, "x":1.95825, "y":3.35722, "heading":0.0, "vx":-2.06123, "vy":3.39013, "omega":0.0, "ax":4.67476, "ay":-7.68865, "alpha":0.0, "fx":[79.51639,79.51639,79.51639,79.51639], "fy":[-130.78181,-130.78181,-130.78181,-130.78181]},
+    {"t":0.97829, "x":1.86301, "y":3.51385, "heading":0.0, "vx":-1.83256, "vy":3.01404, "omega":0.0, "ax":4.67852, "ay":-7.69484, "alpha":0.0, "fx":[79.58036,79.58036,79.58036,79.58036], "fy":[-130.88703,-130.88703,-130.88703,-130.88703]},
+    {"t":1.02721, "x":1.77897, "y":3.65207, "heading":0.0, "vx":-1.60371, "vy":2.63765, "omega":0.0, "ax":4.68076, "ay":-7.69852, "alpha":0.0, "fx":[79.61846,79.61846,79.61846,79.61846], "fy":[-130.94969,-130.94969,-130.94969,-130.94969]},
+    {"t":1.07612, "x":1.70613, "y":3.77188, "heading":0.0, "vx":-1.37476, "vy":2.26108, "omega":0.0, "ax":4.68225, "ay":-7.70097, "alpha":0.0, "fx":[79.64374,79.64374,79.64374,79.64374], "fy":[-130.99127,-130.99127,-130.99127,-130.99127]},
+    {"t":1.12504, "x":1.64448, "y":3.87327, "heading":0.0, "vx":-1.14573, "vy":1.88439, "omega":0.0, "ax":4.68331, "ay":-7.70271, "alpha":0.0, "fx":[79.66173,79.66173,79.66173,79.66173], "fy":[-131.02087,-131.02087,-131.02087,-131.02087]},
+    {"t":1.17395, "x":1.59404, "y":3.95623, "heading":0.0, "vx":-0.91664, "vy":1.50762, "omega":0.0, "ax":4.6841, "ay":-7.70401, "alpha":0.0, "fx":[79.6752,79.6752,79.6752,79.6752], "fy":[-131.04301,-131.04301,-131.04301,-131.04301]},
+    {"t":1.22287, "x":1.55481, "y":4.02076, "heading":0.0, "vx":-0.68752, "vy":1.13078, "omega":0.0, "ax":4.68471, "ay":-7.70502, "alpha":0.0, "fx":[79.68565,79.68565,79.68565,79.68565], "fy":[-131.0602,-131.0602,-131.0602,-131.0602]},
+    {"t":1.27178, "x":1.52678, "y":4.06685, "heading":0.0, "vx":-0.45837, "vy":0.75389, "omega":0.0, "ax":4.6852, "ay":-7.70583, "alpha":0.0, "fx":[79.694,79.694,79.694,79.694], "fy":[-131.07393,-131.07393,-131.07393,-131.07393]},
+    {"t":1.3207, "x":1.50997, "y":4.09451, "heading":0.0, "vx":-0.22919, "vy":0.37696, "omega":0.0, "ax":4.68561, "ay":-7.70649, "alpha":0.0, "fx":[79.70082,79.70082,79.70082,79.70082], "fy":[-131.08515,-131.08515,-131.08515,-131.08515]},
+    {"t":1.36961, "x":1.50436, "y":4.10373, "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/BallPoseR2_To_MidRBumper.traj b/src/main/deploy/choreo/BallPoseR2_To_MidRBumper.traj
new file mode 100644
index 0000000..1fcaa9b
--- /dev/null
+++ b/src/main/deploy/choreo/BallPoseR2_To_MidRBumper.traj
@@ -0,0 +1,30 @@
+{
+ "name":"BallPoseR2_To_MidRBumper",
+ "version":3,
+ "snapshot":{
+  "waypoints":[],
+  "constraints":[],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"MidBallPoseR2.x", "val":7.490900039672852}, "y":{"exp":"MidBallPoseR2.y", "val":3.0211198329925537}, "heading":{"exp":"MidBallPoseR2.heading", "val":0.0}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"MidRBumper.x", "val":5.600369930267334}, "y":{"exp":"MidRBumper.y", "val":2.43641996383667}, "heading":{"exp":"MidRBumper.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":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "config":null,
+  "sampleType":null,
+  "waypoints":[],
+  "samples":[],
+  "splits":[]
+ },
+ "events":[]
+}
diff --git a/src/main/deploy/choreo/BallPoseR_ToMidRBumper.traj b/src/main/deploy/choreo/BallPoseR_ToMidRBumper.traj
new file mode 100644
index 0000000..f38f38f
--- /dev/null
+++ b/src/main/deploy/choreo/BallPoseR_ToMidRBumper.traj
@@ -0,0 +1,30 @@
+{
+ "name":"BallPoseR_ToMidRBumper",
+ "version":3,
+ "snapshot":{
+  "waypoints":[],
+  "constraints":[],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"MidBallPoseR.x", "val":7.529880046844482}, "y":{"exp":"MidBallPoseR.y", "val":2.1440699100494385}, "heading":{"exp":"MidBallPoseR.heading", "val":0.0}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"MidRBumper.x", "val":5.600369930267334}, "y":{"exp":"MidRBumper.y", "val":2.43641996383667}, "heading":{"exp":"MidRBumper.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":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "config":null,
+  "sampleType":null,
+  "waypoints":[],
+  "samples":[],
+  "splits":[]
+ },
+ "events":[]
+}
diff --git a/src/main/deploy/choreo/BumperLToClimb.traj b/src/main/deploy/choreo/BumperLToClimb.traj
new file mode 100644
index 0000000..1834cbe
--- /dev/null
+++ b/src/main/deploy/choreo/BumperLToClimb.traj
@@ -0,0 +1,30 @@
+{
+ "name":"BumperLToClimb",
+ "version":3,
+ "snapshot":{
+  "waypoints":[],
+  "constraints":[],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"AllianceLBumper.x", "val":3.5734095573425293}, "y":{"exp":"AllianceLBumper.y", "val":5.5177741050720215}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"ClimbPose.x", "val":1.504361629486084}, "y":{"exp":"ClimbPose.y", "val":4.103729248046875}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "config":null,
+  "sampleType":null,
+  "waypoints":[],
+  "samples":[],
+  "splits":[]
+ },
+ "events":[]
+}
diff --git a/src/main/deploy/choreo/BumperRToClimb.traj b/src/main/deploy/choreo/BumperRToClimb.traj
new file mode 100644
index 0000000..df85328
--- /dev/null
+++ b/src/main/deploy/choreo/BumperRToClimb.traj
@@ -0,0 +1,80 @@
+{
+ "name":"BumperRToClimb",
+ "version":3,
+ "snapshot":{
+  "waypoints":[
+    {"x":3.5734095573425293, "y":2.43641996383667, "heading":0.0, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":1.504361629486084, "y":4.103729248046875, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"AllianceRBumper.x", "val":3.5734095573425293}, "y":{"exp":"AllianceRBumper.y", "val":2.43641996383667}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":22, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"ClimbPose.x", "val":1.504361629486084}, "y":{"exp":"ClimbPose.y", "val":4.103729248046875}, "heading":{"exp":"ClimbPose.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":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "config":{
+   "frontLeft":{
+    "x":0.2794,
+    "y":0.2794
+   },
+   "backLeft":{
+    "x":-0.2794,
+    "y":0.2794
+   },
+   "mass":68.0388555,
+   "inertia":6.0,
+   "gearing":6.5,
+   "radius":0.0508,
+   "vmax":628.3185307179587,
+   "tmax":1.2,
+   "cof":1.5,
+   "bumper":{
+    "front":0.4064,
+    "side":0.4064,
+    "back":0.4064
+   },
+   "differentialTrackWidth":0.5588
+  },
+  "sampleType":"Swerve",
+  "waypoints":[0.0,1.08602],
+  "samples":[
+    {"t":0.0, "x":3.57341, "y":2.43642, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.02274, "ay":5.65916, "alpha":0.0, "fx":[-119.4548,-119.4548,-119.4548,-119.4548], "fy":[96.26074,96.26074,96.26074,96.26074]},
+    {"t":0.04936, "x":3.56485, "y":2.44332, "heading":0.0, "vx":-0.34667, "vy":0.27936, "omega":0.0, "ax":-7.02211, "ay":5.65865, "alpha":0.0, "fx":[-119.44403,-119.44403,-119.44403,-119.44403], "fy":[96.25207,96.25207,96.25207,96.25207]},
+    {"t":0.09873, "x":3.53918, "y":2.464, "heading":0.0, "vx":-0.69332, "vy":0.5587, "omega":0.0, "ax":-7.02132, "ay":5.65802, "alpha":0.0, "fx":[-119.4307,-119.4307,-119.4307,-119.4307], "fy":[96.24133,96.24133,96.24133,96.24133]},
+    {"t":0.14809, "x":3.4964, "y":2.49847, "heading":0.0, "vx":-1.03992, "vy":0.838, "omega":0.0, "ax":-7.02033, "ay":5.65722, "alpha":0.0, "fx":[-119.41379,-119.41379,-119.41379,-119.41379], "fy":[96.2277,96.2277,96.2277,96.2277]},
+    {"t":0.19746, "x":3.43651, "y":2.54673, "heading":0.0, "vx":-1.38648, "vy":1.11727, "omega":0.0, "ax":-7.01902, "ay":5.65617, "alpha":0.0, "fx":[-119.3916,-119.3916,-119.3916,-119.3916], "fy":[96.20982,96.20982,96.20982,96.20982]},
+    {"t":0.24682, "x":3.35952, "y":2.60878, "heading":0.0, "vx":-1.73297, "vy":1.39648, "omega":0.0, "ax":-7.01724, "ay":5.65473, "alpha":0.0, "fx":[-119.36124,-119.36124,-119.36124,-119.36124], "fy":[96.18535,96.18535,96.18535,96.18535]},
+    {"t":0.29619, "x":3.26542, "y":2.68461, "heading":0.0, "vx":-2.07937, "vy":1.67563, "omega":0.0, "ax":-7.01465, "ay":5.65264, "alpha":0.0, "fx":[-119.31716,-119.31716,-119.31716,-119.31716], "fy":[96.14983,96.14983,96.14983,96.14983]},
+    {"t":0.34555, "x":3.15423, "y":2.77421, "heading":0.0, "vx":-2.42565, "vy":1.95467, "omega":0.0, "ax":-7.01055, "ay":5.64934, "alpha":0.0, "fx":[-119.24737,-119.24737,-119.24737,-119.24737], "fy":[96.09359,96.09359,96.09359,96.09359]},
+    {"t":0.39492, "x":3.02595, "y":2.87759, "heading":0.0, "vx":-2.77172, "vy":2.23354, "omega":0.0, "ax":-7.00307, "ay":5.64331, "alpha":0.0, "fx":[-119.12014,-119.12014,-119.12014,-119.12014], "fy":[95.99107,95.99107,95.99107,95.99107]},
+    {"t":0.44428, "x":2.88059, "y":2.99472, "heading":0.0, "vx":-3.11742, "vy":2.51212, "omega":0.0, "ax":-6.98515, "ay":5.62887, "alpha":0.0, "fx":[-118.81538,-118.81538,-118.81538,-118.81538], "fy":[95.74548,95.74548,95.74548,95.74548]},
+    {"t":0.49365, "x":2.71819, "y":3.12559, "heading":0.0, "vx":-3.46224, "vy":2.78999, "omega":0.0, "ax":-6.88544, "ay":5.54852, "alpha":0.0, "fx":[-117.11934,-117.11934,-117.11934,-117.11934], "fy":[94.37876,94.37876,94.37876,94.37876]},
+    {"t":0.54301, "x":2.53889, "y":3.27007, "heading":0.0, "vx":-3.80214, "vy":3.06389, "omega":0.0, "ax":6.88544, "ay":-5.54852, "alpha":0.0, "fx":[117.11934,117.11934,117.11934,117.11934], "fy":[-94.37876,-94.37876,-94.37876,-94.37876]},
+    {"t":0.59237, "x":2.35958, "y":3.41456, "heading":0.0, "vx":-3.46224, "vy":2.78999, "omega":0.0, "ax":6.98515, "ay":-5.62887, "alpha":0.0, "fx":[118.81538,118.81538,118.81538,118.81538], "fy":[-95.74548,-95.74548,-95.74548,-95.74548]},
+    {"t":0.64174, "x":2.19718, "y":3.54543, "heading":0.0, "vx":-3.11742, "vy":2.51212, "omega":0.0, "ax":7.00307, "ay":-5.64331, "alpha":0.0, "fx":[119.12014,119.12014,119.12014,119.12014], "fy":[-95.99107,-95.99107,-95.99107,-95.99107]},
+    {"t":0.6911, "x":2.05183, "y":3.66256, "heading":0.0, "vx":-2.77172, "vy":2.23354, "omega":0.0, "ax":7.01055, "ay":-5.64934, "alpha":0.0, "fx":[119.24737,119.24737,119.24737,119.24737], "fy":[-96.09359,-96.09359,-96.09359,-96.09359]},
+    {"t":0.74047, "x":1.92354, "y":3.76594, "heading":0.0, "vx":-2.42565, "vy":1.95467, "omega":0.0, "ax":7.01465, "ay":-5.65264, "alpha":0.0, "fx":[119.31716,119.31716,119.31716,119.31716], "fy":[-96.14983,-96.14983,-96.14983,-96.14983]},
+    {"t":0.78983, "x":1.81235, "y":3.85554, "heading":0.0, "vx":-2.07937, "vy":1.67563, "omega":0.0, "ax":7.01724, "ay":-5.65473, "alpha":0.0, "fx":[119.36124,119.36124,119.36124,119.36124], "fy":[-96.18535,-96.18535,-96.18535,-96.18535]},
+    {"t":0.8392, "x":1.71825, "y":3.93137, "heading":0.0, "vx":-1.73297, "vy":1.39648, "omega":0.0, "ax":7.01902, "ay":-5.65617, "alpha":0.0, "fx":[119.3916,119.3916,119.3916,119.3916], "fy":[-96.20982,-96.20982,-96.20982,-96.20982]},
+    {"t":0.88856, "x":1.64126, "y":3.99341, "heading":0.0, "vx":-1.38648, "vy":1.11727, "omega":0.0, "ax":7.02033, "ay":-5.65722, "alpha":0.0, "fx":[119.41379,119.41379,119.41379,119.41379], "fy":[-96.2277,-96.2277,-96.2277,-96.2277]},
+    {"t":0.93793, "x":1.58137, "y":4.04167, "heading":0.0, "vx":-1.03992, "vy":0.838, "omega":0.0, "ax":7.02132, "ay":-5.65802, "alpha":0.0, "fx":[119.4307,119.4307,119.4307,119.4307], "fy":[-96.24133,-96.24133,-96.24133,-96.24133]},
+    {"t":0.98729, "x":1.53859, "y":4.07615, "heading":0.0, "vx":-0.69332, "vy":0.5587, "omega":0.0, "ax":7.02211, "ay":-5.65865, "alpha":0.0, "fx":[119.44403,119.44403,119.44403,119.44403], "fy":[-96.25207,-96.25207,-96.25207,-96.25207]},
+    {"t":1.03666, "x":1.51292, "y":4.09683, "heading":0.0, "vx":-0.34667, "vy":0.27936, "omega":0.0, "ax":7.02274, "ay":-5.65916, "alpha":0.0, "fx":[119.4548,119.4548,119.4548,119.4548], "fy":[-96.26074,-96.26074,-96.26074,-96.26074]},
+    {"t":1.08602, "x":1.50436, "y":4.10373, "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/MidRBumper_To_BallPoseR.traj b/src/main/deploy/choreo/MidRBumper_To_BallPoseR.traj
new file mode 100644
index 0000000..0e05053
--- /dev/null
+++ b/src/main/deploy/choreo/MidRBumper_To_BallPoseR.traj
@@ -0,0 +1,30 @@
+{
+ "name":"MidRBumper_To_BallPoseR",
+ "version":3,
+ "snapshot":{
+  "waypoints":[],
+  "constraints":[],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"MidRBumper.x", "val":5.600369930267334}, "y":{"exp":"MidRBumper.y", "val":2.43641996383667}, "heading":{"exp":"MidRBumper.heading", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"MidBallPoseR.x", "val":7.529880046844482}, "y":{"exp":"MidBallPoseR.y", "val":2.1440699100494385}, "heading":{"exp":"MidBallPoseR.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":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "config":null,
+  "sampleType":null,
+  "waypoints":[],
+  "samples":[],
+  "splits":[]
+ },
+ "events":[]
+}
diff --git a/src/main/deploy/choreo/MidRBumper_To_BallPoseR2.traj b/src/main/deploy/choreo/MidRBumper_To_BallPoseR2.traj
new file mode 100644
index 0000000..ed39b97
--- /dev/null
+++ b/src/main/deploy/choreo/MidRBumper_To_BallPoseR2.traj
@@ -0,0 +1,30 @@
+{
+ "name":"MidRBumper_To_BallPoseR2",
+ "version":3,
+ "snapshot":{
+  "waypoints":[],
+  "constraints":[],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"MidRBumper.x", "val":5.600369930267334}, "y":{"exp":"MidRBumper.y", "val":2.43641996383667}, "heading":{"exp":"MidRBumper.heading", "val":0.0}, "intervals":18, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"MidBallPoseR2.x", "val":7.490900039672852}, "y":{"exp":"MidBallPoseR2.y", "val":3.0211198329925537}, "heading":{"exp":"MidBallPoseR2.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":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "config":null,
+  "sampleType":null,
+  "waypoints":[],
+  "samples":[],
+  "splits":[]
+ },
+ "events":[]
+}
diff --git a/src/main/deploy/choreo/MidToAllianceL.traj b/src/main/deploy/choreo/MidToAllianceL.traj
index 843658d..2ea3f3a 100644
--- a/src/main/deploy/choreo/MidToAllianceL.traj
+++ b/src/main/deploy/choreo/MidToAllianceL.traj
@@ -3,7 +3,7 @@
  "version":3,
  "snapshot":{
   "waypoints":[
-    {"x":6.0, "y":7.434945106506348, "heading":0.0, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":6.0, "y":7.434945106506348, "heading":0.0, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":3.606555223464966, "y":7.434945106506348, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
@@ -13,7 +13,7 @@
  },
  "params":{
   "waypoints":[
-    {"x":{"exp":"AllianceLMid.x", "val":6.0}, "y":{"exp":"AllianceLMid.y", "val":7.434945106506348}, "heading":{"exp":"AllianceLMid.heading", "val":0.0}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"AllianceLMid.x", "val":6.0}, "y":{"exp":"AllianceLMid.y", "val":7.434945106506348}, "heading":{"exp":"AllianceLMid.heading", "val":0.0}, "intervals":20, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"AllianceLSide.x", "val":3.606555223464966}, "y":{"exp":"AllianceLSide.y", "val":7.434945106506348}, "heading":{"exp":"AllianceLSide.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},
@@ -27,52 +27,51 @@
  "trajectory":{
   "config":{
    "frontLeft":{
-    "x":0.2794,
-    "y":0.2794
+    "x":0.3175,
+    "y":0.3175
    },
    "backLeft":{
-    "x":-0.2794,
-    "y":0.2794
+    "x":-0.3175,
+    "y":0.3175
    },
-   "mass":68.0388555,
-   "inertia":6.0,
-   "gearing":6.5,
-   "radius":0.0508,
+   "mass":56.69904625,
+   "inertia":7.15,
+   "gearing":5.67,
+   "radius":0.0466538362578,
    "vmax":628.3185307179587,
-   "tmax":1.2,
+   "tmax":1.164,
    "cof":1.5,
    "bumper":{
-    "front":0.4064,
-    "side":0.4064,
-    "back":0.4064
+    "front":0.4572,
+    "side":0.4572,
+    "back":0.4572
    },
    "differentialTrackWidth":0.5588
   },
   "sampleType":"Swerve",
-  "waypoints":[0.0,1.03182],
+  "waypoints":[0.0,0.98023],
   "samples":[
-    {"t":0.0, "x":6.0, "y":7.43495, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-9.01911, "ay":0.0, "alpha":0.0, "fx":[-153.41253,-153.41253,-153.41253,-153.41253], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.04913, "x":5.98911, "y":7.43495, "heading":0.0, "vx":-0.44315, "vy":0.0, "omega":0.0, "ax":-9.01826, "ay":0.0, "alpha":0.0, "fx":[-153.39798,-153.39798,-153.39798,-153.39798], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.09827, "x":5.95645, "y":7.43495, "heading":0.0, "vx":-0.88625, "vy":0.0, "omega":0.0, "ax":-9.01719, "ay":0.0, "alpha":0.0, "fx":[-153.37978,-153.37978,-153.37978,-153.37978], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.1474, "x":5.90202, "y":7.43495, "heading":0.0, "vx":-1.32931, "vy":0.0, "omega":0.0, "ax":-9.01581, "ay":0.0, "alpha":0.0, "fx":[-153.35639,-153.35639,-153.35639,-153.35639], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.19654, "x":5.82583, "y":7.43495, "heading":0.0, "vx":-1.77229, "vy":0.0, "omega":0.0, "ax":-9.01398, "ay":0.0, "alpha":0.0, "fx":[-153.32519,-153.32519,-153.32519,-153.32519], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.24567, "x":5.72786, "y":7.43495, "heading":0.0, "vx":-2.21519, "vy":0.0, "omega":0.0, "ax":-9.01141, "ay":0.0, "alpha":0.0, "fx":[-153.2815,-153.2815,-153.2815,-153.2815], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.29481, "x":5.60814, "y":7.43495, "heading":0.0, "vx":-2.65796, "vy":0.0, "omega":0.0, "ax":-9.00756, "ay":0.0, "alpha":0.0, "fx":[-153.21595,-153.21595,-153.21595,-153.21595], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.34394, "x":5.46667, "y":7.43495, "heading":0.0, "vx":-3.10054, "vy":0.0, "omega":0.0, "ax":-9.00113, "ay":0.0, "alpha":0.0, "fx":[-153.10668,-153.10668,-153.10668,-153.10668], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.39307, "x":5.30347, "y":7.43495, "heading":0.0, "vx":-3.54281, "vy":0.0, "omega":0.0, "ax":-8.98828, "ay":0.0, "alpha":0.0, "fx":[-152.88813,-152.88813,-152.88813,-152.88813], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.44221, "x":5.11854, "y":7.43495, "heading":0.0, "vx":-3.98444, "vy":0.0, "omega":0.0, "ax":-8.9498, "ay":0.0, "alpha":0.0, "fx":[-152.23347,-152.23347,-152.23347,-152.23347], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.49134, "x":4.91197, "y":7.43495, "heading":0.0, "vx":-4.42418, "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.54048, "x":4.69459, "y":7.43495, "heading":0.0, "vx":-4.42418, "vy":0.0, "omega":0.0, "ax":8.9498, "ay":0.0, "alpha":0.0, "fx":[152.23347,152.23347,152.23347,152.23347], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.58961, "x":4.48801, "y":7.43495, "heading":0.0, "vx":-3.98444, "vy":0.0, "omega":0.0, "ax":8.98828, "ay":0.0, "alpha":0.0, "fx":[152.88813,152.88813,152.88813,152.88813], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.63875, "x":4.30309, "y":7.43495, "heading":0.0, "vx":-3.54281, "vy":0.0, "omega":0.0, "ax":9.00113, "ay":0.0, "alpha":0.0, "fx":[153.10668,153.10668,153.10668,153.10668], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.68788, "x":4.13988, "y":7.43495, "heading":0.0, "vx":-3.10054, "vy":0.0, "omega":0.0, "ax":9.00756, "ay":0.0, "alpha":0.0, "fx":[153.21595,153.21595,153.21595,153.21595], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.73702, "x":3.99841, "y":7.43495, "heading":0.0, "vx":-2.65796, "vy":0.0, "omega":0.0, "ax":9.01141, "ay":0.0, "alpha":0.0, "fx":[153.2815,153.2815,153.2815,153.2815], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.78615, "x":3.87869, "y":7.43495, "heading":0.0, "vx":-2.21519, "vy":0.0, "omega":0.0, "ax":9.01398, "ay":0.0, "alpha":0.0, "fx":[153.32519,153.32519,153.32519,153.32519], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.83528, "x":3.78073, "y":7.43495, "heading":0.0, "vx":-1.77229, "vy":0.0, "omega":0.0, "ax":9.01581, "ay":0.0, "alpha":0.0, "fx":[153.35639,153.35639,153.35639,153.35639], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.88442, "x":3.70453, "y":7.43495, "heading":0.0, "vx":-1.32931, "vy":0.0, "omega":0.0, "ax":9.01719, "ay":0.0, "alpha":0.0, "fx":[153.37978,153.37978,153.37978,153.37978], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.93355, "x":3.6501, "y":7.43495, "heading":0.0, "vx":-0.88625, "vy":0.0, "omega":0.0, "ax":9.01826, "ay":0.0, "alpha":0.0, "fx":[153.39798,153.39798,153.39798,153.39798], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.98269, "x":3.61744, "y":7.43495, "heading":0.0, "vx":-0.44315, "vy":0.0, "omega":0.0, "ax":9.01911, "ay":0.0, "alpha":0.0, "fx":[153.41253,153.41253,153.41253,153.41253], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.03182, "x":3.60656, "y":7.43495, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+    {"t":0.0, "x":6.0, "y":7.43495, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-9.97149, "ay":0.0, "alpha":0.0, "fx":[-141.34353,-141.34353,-141.34353,-141.34353], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.04901, "x":5.98802, "y":7.43495, "heading":0.0, "vx":-0.48872, "vy":0.0, "omega":0.0, "ax":-9.97048, "ay":0.0, "alpha":0.0, "fx":[-141.32923,-141.32923,-141.32923,-141.32923], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.09802, "x":5.9521, "y":7.43495, "heading":0.0, "vx":-0.97739, "vy":0.0, "omega":0.0, "ax":-9.96921, "ay":0.0, "alpha":0.0, "fx":[-141.31111,-141.31111,-141.31111,-141.31111], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.14704, "x":5.89222, "y":7.43495, "heading":0.0, "vx":-1.466, "vy":0.0, "omega":0.0, "ax":-9.96753, "ay":0.0, "alpha":0.0, "fx":[-141.28741,-141.28741,-141.28741,-141.28741], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.19605, "x":5.80839, "y":7.43495, "heading":0.0, "vx":-1.95452, "vy":0.0, "omega":0.0, "ax":-9.96525, "ay":0.0, "alpha":0.0, "fx":[-141.25508,-141.25508,-141.25508,-141.25508], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.24506, "x":5.70063, "y":7.43495, "heading":0.0, "vx":-2.44294, "vy":0.0, "omega":0.0, "ax":-9.96196, "ay":0.0, "alpha":0.0, "fx":[-141.20837,-141.20837,-141.20837,-141.20837], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.29407, "x":5.56893, "y":7.43495, "heading":0.0, "vx":-2.93119, "vy":0.0, "omega":0.0, "ax":-9.95678, "ay":0.0, "alpha":0.0, "fx":[-141.13492,-141.13492,-141.13492,-141.13492], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.34308, "x":5.41331, "y":7.43495, "heading":0.0, "vx":-3.41919, "vy":0.0, "omega":0.0, "ax":-9.94744, "ay":0.0, "alpha":0.0, "fx":[-141.00265,-141.00265,-141.00265,-141.00265], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.39209, "x":5.23378, "y":7.43495, "heading":0.0, "vx":-3.90673, "vy":0.0, "omega":0.0, "ax":-9.92566, "ay":0.0, "alpha":0.0, "fx":[-140.69385,-140.69385,-140.69385,-140.69385], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.44111, "x":5.03039, "y":7.43495, "heading":0.0, "vx":-4.39321, "vy":0.0, "omega":0.0, "ax":-9.81674, "ay":0.0, "alpha":0.0, "fx":[-139.14992,-139.14992,-139.14992,-139.14992], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.49012, "x":4.80328, "y":7.43495, "heading":0.0, "vx":-4.87434, "vy":0.0, "omega":0.0, "ax":9.81674, "ay":0.0, "alpha":0.0, "fx":[139.14992,139.14992,139.14992,139.14992], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.53913, "x":4.57617, "y":7.43495, "heading":0.0, "vx":-4.39321, "vy":0.0, "omega":0.0, "ax":9.92566, "ay":0.0, "alpha":0.0, "fx":[140.69385,140.69385,140.69385,140.69385], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.58814, "x":4.37277, "y":7.43495, "heading":0.0, "vx":-3.90673, "vy":0.0, "omega":0.0, "ax":9.94744, "ay":0.0, "alpha":0.0, "fx":[141.00265,141.00265,141.00265,141.00265], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.63715, "x":4.19324, "y":7.43495, "heading":0.0, "vx":-3.41919, "vy":0.0, "omega":0.0, "ax":9.95678, "ay":0.0, "alpha":0.0, "fx":[141.13492,141.13492,141.13492,141.13492], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.68616, "x":4.03762, "y":7.43495, "heading":0.0, "vx":-2.93119, "vy":0.0, "omega":0.0, "ax":9.96196, "ay":0.0, "alpha":0.0, "fx":[141.20837,141.20837,141.20837,141.20837], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.73518, "x":3.90592, "y":7.43495, "heading":0.0, "vx":-2.44294, "vy":0.0, "omega":0.0, "ax":9.96525, "ay":0.0, "alpha":0.0, "fx":[141.25508,141.25508,141.25508,141.25508], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.78419, "x":3.79816, "y":7.43495, "heading":0.0, "vx":-1.95452, "vy":0.0, "omega":0.0, "ax":9.96753, "ay":0.0, "alpha":0.0, "fx":[141.28741,141.28741,141.28741,141.28741], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.8332, "x":3.71434, "y":7.43495, "heading":0.0, "vx":-1.466, "vy":0.0, "omega":0.0, "ax":9.96921, "ay":0.0, "alpha":0.0, "fx":[141.31111,141.31111,141.31111,141.31111], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.88221, "x":3.65446, "y":7.43495, "heading":0.0, "vx":-0.97739, "vy":0.0, "omega":0.0, "ax":9.97048, "ay":0.0, "alpha":0.0, "fx":[141.32923,141.32923,141.32923,141.32923], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.93122, "x":3.61853, "y":7.43495, "heading":0.0, "vx":-0.48872, "vy":0.0, "omega":0.0, "ax":9.97149, "ay":0.0, "alpha":0.0, "fx":[141.34353,141.34353,141.34353,141.34353], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.98023, "x":3.60656, "y":7.43495, "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/MidToAllianceR.traj b/src/main/deploy/choreo/MidToAllianceR.traj
index 5130803..d4aa28b 100644
--- a/src/main/deploy/choreo/MidToAllianceR.traj
+++ b/src/main/deploy/choreo/MidToAllianceR.traj
@@ -3,8 +3,9 @@
  "version":3,
  "snapshot":{
   "waypoints":[
-    {"x":6.0, "y":0.6462193727493286, "heading":0.0, "intervals":21, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":3.606555223464966, "y":0.6462193727493286, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+    {"x":6.0, "y":0.6462193727493286, "heading":0.0, "intervals":20, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":3.606555223464966, "y":0.6462193727493286, "heading":0.0, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":6.3546142578125, "y":9.084023475646973, "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},
@@ -13,8 +14,9 @@
  },
  "params":{
   "waypoints":[
-    {"x":{"exp":"AllianceRMid.x", "val":6.0}, "y":{"exp":"AllianceRMid.y", "val":0.6462193727493286}, "heading":{"exp":"AllianceRMid.heading", "val":0.0}, "intervals":21, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"AllianceRSide.x", "val":3.606555223464966}, "y":{"exp":"AllianceRSide.y", "val":0.6462193727493286}, "heading":{"exp":"AllianceRSide.heading", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+    {"x":{"exp":"AllianceRMid.x", "val":6.0}, "y":{"exp":"AllianceRMid.y", "val":0.6462193727493286}, "heading":{"exp":"AllianceRMid.heading", "val":0.0}, "intervals":20, "split":true, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"AllianceRSide.x", "val":3.606555223464966}, "y":{"exp":"AllianceRSide.y", "val":0.6462193727493286}, "heading":{"exp":"AllianceRSide.heading", "val":0.0}, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"6.3546142578125 m", "val":6.3546142578125}, "y":{"exp":"9.084023475646973 m", "val":9.084023475646973}, "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},
@@ -27,52 +29,96 @@
  "trajectory":{
   "config":{
    "frontLeft":{
-    "x":0.2794,
-    "y":0.2794
+    "x":0.3175,
+    "y":0.3175
    },
    "backLeft":{
-    "x":-0.2794,
-    "y":0.2794
+    "x":-0.3175,
+    "y":0.3175
    },
-   "mass":68.0388555,
-   "inertia":6.0,
-   "gearing":6.5,
-   "radius":0.0508,
+   "mass":56.69904625,
+   "inertia":7.15,
+   "gearing":5.67,
+   "radius":0.0466538362578,
    "vmax":628.3185307179587,
-   "tmax":1.2,
+   "tmax":1.164,
    "cof":1.5,
    "bumper":{
-    "front":0.4064,
-    "side":0.4064,
-    "back":0.4064
+    "front":0.4572,
+    "side":0.4572,
+    "back":0.4572
    },
    "differentialTrackWidth":0.5588
   },
   "sampleType":"Swerve",
-  "waypoints":[0.0,1.03182],
+  "waypoints":[0.0,0.82874,2.97642],
   "samples":[
-    {"t":0.0, "x":6.0, "y":0.64622, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-9.01911, "ay":0.0, "alpha":0.0, "fx":[-153.41253,-153.41253,-153.41253,-153.41253], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.04913, "x":5.98911, "y":0.64622, "heading":0.0, "vx":-0.44315, "vy":0.0, "omega":0.0, "ax":-9.01826, "ay":0.0, "alpha":0.0, "fx":[-153.39798,-153.39798,-153.39798,-153.39798], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.09827, "x":5.95645, "y":0.64622, "heading":0.0, "vx":-0.88625, "vy":0.0, "omega":0.0, "ax":-9.01719, "ay":0.0, "alpha":0.0, "fx":[-153.37978,-153.37978,-153.37978,-153.37978], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.1474, "x":5.90202, "y":0.64622, "heading":0.0, "vx":-1.32931, "vy":0.0, "omega":0.0, "ax":-9.01581, "ay":0.0, "alpha":0.0, "fx":[-153.35639,-153.35639,-153.35639,-153.35639], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.19654, "x":5.82583, "y":0.64622, "heading":0.0, "vx":-1.77229, "vy":0.0, "omega":0.0, "ax":-9.01398, "ay":0.0, "alpha":0.0, "fx":[-153.32519,-153.32519,-153.32519,-153.32519], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.24567, "x":5.72786, "y":0.64622, "heading":0.0, "vx":-2.21519, "vy":0.0, "omega":0.0, "ax":-9.01141, "ay":0.0, "alpha":0.0, "fx":[-153.2815,-153.2815,-153.2815,-153.2815], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.29481, "x":5.60814, "y":0.64622, "heading":0.0, "vx":-2.65796, "vy":0.0, "omega":0.0, "ax":-9.00756, "ay":0.0, "alpha":0.0, "fx":[-153.21595,-153.21595,-153.21595,-153.21595], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.34394, "x":5.46667, "y":0.64622, "heading":0.0, "vx":-3.10054, "vy":0.0, "omega":0.0, "ax":-9.00113, "ay":0.0, "alpha":0.0, "fx":[-153.10668,-153.10668,-153.10668,-153.10668], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.39307, "x":5.30347, "y":0.64622, "heading":0.0, "vx":-3.54281, "vy":0.0, "omega":0.0, "ax":-8.98828, "ay":0.0, "alpha":0.0, "fx":[-152.88813,-152.88813,-152.88813,-152.88813], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.44221, "x":5.11854, "y":0.64622, "heading":0.0, "vx":-3.98444, "vy":0.0, "omega":0.0, "ax":-8.9498, "ay":0.0, "alpha":0.0, "fx":[-152.23347,-152.23347,-152.23347,-152.23347], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.49134, "x":4.91197, "y":0.64622, "heading":0.0, "vx":-4.42418, "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.54048, "x":4.69459, "y":0.64622, "heading":0.0, "vx":-4.42418, "vy":0.0, "omega":0.0, "ax":8.9498, "ay":0.0, "alpha":0.0, "fx":[152.23347,152.23347,152.23347,152.23347], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.58961, "x":4.48801, "y":0.64622, "heading":0.0, "vx":-3.98444, "vy":0.0, "omega":0.0, "ax":8.98828, "ay":0.0, "alpha":0.0, "fx":[152.88813,152.88813,152.88813,152.88813], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.63875, "x":4.30309, "y":0.64622, "heading":0.0, "vx":-3.54281, "vy":0.0, "omega":0.0, "ax":9.00113, "ay":0.0, "alpha":0.0, "fx":[153.10668,153.10668,153.10668,153.10668], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.68788, "x":4.13988, "y":0.64622, "heading":0.0, "vx":-3.10054, "vy":0.0, "omega":0.0, "ax":9.00756, "ay":0.0, "alpha":0.0, "fx":[153.21595,153.21595,153.21595,153.21595], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.73702, "x":3.99841, "y":0.64622, "heading":0.0, "vx":-2.65796, "vy":0.0, "omega":0.0, "ax":9.01141, "ay":0.0, "alpha":0.0, "fx":[153.2815,153.2815,153.2815,153.2815], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.78615, "x":3.87869, "y":0.64622, "heading":0.0, "vx":-2.21519, "vy":0.0, "omega":0.0, "ax":9.01398, "ay":0.0, "alpha":0.0, "fx":[153.32519,153.32519,153.32519,153.32519], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.83528, "x":3.78073, "y":0.64622, "heading":0.0, "vx":-1.77229, "vy":0.0, "omega":0.0, "ax":9.01581, "ay":0.0, "alpha":0.0, "fx":[153.35639,153.35639,153.35639,153.35639], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.88442, "x":3.70453, "y":0.64622, "heading":0.0, "vx":-1.32931, "vy":0.0, "omega":0.0, "ax":9.01719, "ay":0.0, "alpha":0.0, "fx":[153.37978,153.37978,153.37978,153.37978], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.93355, "x":3.6501, "y":0.64622, "heading":0.0, "vx":-0.88625, "vy":0.0, "omega":0.0, "ax":9.01826, "ay":0.0, "alpha":0.0, "fx":[153.39798,153.39798,153.39798,153.39798], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":0.98269, "x":3.61744, "y":0.64622, "heading":0.0, "vx":-0.44315, "vy":0.0, "omega":0.0, "ax":9.01911, "ay":0.0, "alpha":0.0, "fx":[153.41253,153.41253,153.41253,153.41253], "fy":[0.0,0.0,0.0,0.0]},
-    {"t":1.03182, "x":3.60656, "y":0.64622, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+    {"t":0.0, "x":6.0, "y":0.64622, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-8.76331, "ay":-4.75448, "alpha":0.0, "fx":[-124.21786,-124.21786,-124.21786,-124.21786], "fy":[-67.39364,-67.39364,-67.39364,-67.39364]},
+    {"t":0.04144, "x":5.99248, "y":0.64214, "heading":0.0, "vx":-0.36313, "vy":-0.19701, "omega":0.0, "ax":-8.85804, "ay":-4.57332, "alpha":0.0, "fx":[-125.56055,-125.56055,-125.56055,-125.56055], "fy":[-64.82579,-64.82579,-64.82579,-64.82579]},
+    {"t":0.08287, "x":5.96982, "y":0.63005, "heading":0.0, "vx":-0.73018, "vy":-0.38652, "omega":0.0, "ax":-8.96929, "ay":-4.34819, "alpha":0.0, "fx":[-127.13754,-127.13754,-127.13754,-127.13754], "fy":[-61.63455,-61.63455,-61.63455,-61.63455]},
+    {"t":0.12431, "x":5.93187, "y":0.6103, "heading":0.0, "vx":-1.10184, "vy":-0.56669, "omega":0.0, "ax":-9.10094, "ay":-4.06157, "alpha":0.0, "fx":[-129.00368,-129.00368,-129.00368,-129.00368], "fy":[-57.57177,-57.57177,-57.57177,-57.57177]},
+    {"t":0.16575, "x":5.8784, "y":0.58333, "heading":0.0, "vx":-1.47896, "vy":-0.73499, "omega":0.0, "ax":-9.25734, "ay":-3.6858, "alpha":0.0, "fx":[-131.22054,-131.22054,-131.22054,-131.22054], "fy":[-52.24528,-52.24528,-52.24528,-52.24528]},
+    {"t":0.20719, "x":5.80917, "y":0.54971, "heading":0.0, "vx":-1.86256, "vy":-0.88772, "omega":0.0, "ax":-9.44191, "ay":-3.17505, "alpha":0.0, "fx":[-133.83689,-133.83689,-133.83689,-133.83689], "fy":[-45.00559,-45.00559,-45.00559,-45.00559]},
+    {"t":0.24862, "x":5.72388, "y":0.5102, "heading":0.0, "vx":-2.2538, "vy":-1.01929, "omega":0.0, "ax":-9.6519, "ay":-2.44954, "alpha":0.0, "fx":[-136.81332,-136.81332,-136.81332,-136.81332], "fy":[-34.72168,-34.72168,-34.72168,-34.72168]},
+    {"t":0.29006, "x":5.6222, "y":0.46586, "heading":0.0, "vx":-2.65375, "vy":-1.12079, "omega":0.0, "ax":-9.85894, "ay":-1.36414, "alpha":0.0, "fx":[-139.74819,-139.74819,-139.74819,-139.74819], "fy":[-19.33643,-19.33643,-19.33643,-19.33643]},
+    {"t":0.3315, "x":5.50378, "y":0.41825, "heading":0.0, "vx":-3.06228, "vy":-1.17732, "omega":0.0, "ax":-9.93986, "ay":0.34399, "alpha":0.0, "fx":[-140.89508,-140.89508,-140.89508,-140.89508], "fy":[4.87592,4.87592,4.87592,4.87592]},
+    {"t":0.37293, "x":5.36835, "y":0.36976, "heading":0.0, "vx":-3.47415, "vy":-1.16306, "omega":0.0, "ax":-9.45746, "ay":3.04849, "alpha":0.0, "fx":[-134.05719,-134.05719,-134.05719,-134.05719], "fy":[43.21156,43.21156,43.21156,43.21156]},
+    {"t":0.41437, "x":5.21627, "y":0.32418, "heading":0.0, "vx":-3.86604, "vy":-1.03674, "omega":0.0, "ax":-7.39794, "ay":6.6227, "alpha":0.0, "fx":[-104.8641,-104.8641,-104.8641,-104.8641], "fy":[93.87525,93.87525,93.87525,93.87525]},
+    {"t":0.45581, "x":5.04972, "y":0.28691, "heading":0.0, "vx":-4.17259, "vy":-0.76232, "omega":0.0, "ax":-3.57633, "ay":9.26515, "alpha":0.0, "fx":[-50.69366,-50.69366,-50.69366,-50.69366], "fy":[131.33124,131.33124,131.33124,131.33124]},
+    {"t":0.49725, "x":4.87375, "y":0.26327, "heading":0.0, "vx":-4.32079, "vy":-0.37839, "omega":0.0, "ax":-0.06032, "ay":9.94024, "alpha":0.0, "fx":[-0.85504,-0.85504,-0.85504,-0.85504], "fy":[140.90051,140.90051,140.90051,140.90051]},
+    {"t":0.53868, "x":4.69466, "y":0.25613, "heading":0.0, "vx":-4.32329, "vy":0.0335, "omega":0.0, "ax":2.19991, "ay":9.70261, "alpha":0.0, "fx":[31.18318,31.18318,31.18318,31.18318], "fy":[137.53211,137.53211,137.53211,137.53211]},
+    {"t":0.58012, "x":4.5174, "y":0.26584, "heading":0.0, "vx":-4.23213, "vy":0.43555, "omega":0.0, "ax":3.56901, "ay":9.29329, "alpha":0.0, "fx":[50.58984,50.58984,50.58984,50.58984], "fy":[131.73011,131.73011,131.73011,131.73011]},
+    {"t":0.62156, "x":4.3451, "y":0.29187, "heading":0.0, "vx":-4.08424, "vy":0.82064, "omega":0.0, "ax":4.43593, "ay":8.91698, "alpha":0.0, "fx":[62.87831,62.87831,62.87831,62.87831], "fy":[126.39604,126.39604,126.39604,126.39604]},
+    {"t":0.66299, "x":4.17967, "y":0.33353, "heading":0.0, "vx":-3.90043, "vy":1.19013, "omega":0.0, "ax":5.0186, "ay":8.60621, "alpha":0.0, "fx":[71.13739,71.13739,71.13739,71.13739], "fy":[121.99091,121.99091,121.99091,121.99091]},
+    {"t":0.70443, "x":4.02235, "y":0.39023, "heading":0.0, "vx":-3.69247, "vy":1.54675, "omega":0.0, "ax":5.43148, "ay":8.35459, "alpha":0.0, "fx":[76.98996,76.98996,76.98996,76.98996], "fy":[118.42437,118.42437,118.42437,118.42437]},
+    {"t":0.74587, "x":3.87401, "y":0.4615, "heading":0.0, "vx":-3.46741, "vy":1.89294, "omega":0.0, "ax":5.73701, "ay":8.15005, "alpha":0.0, "fx":[81.32073,81.32073,81.32073,81.32073], "fy":[115.52499,115.52499,115.52499,115.52499]},
+    {"t":0.78731, "x":3.73526, "y":0.54693, "heading":0.0, "vx":-3.22968, "vy":2.23065, "omega":0.0, "ax":5.97112, "ay":7.98192, "alpha":0.0, "fx":[84.63924,84.63924,84.63924,84.63924], "fy":[113.14178,113.14178,113.14178,113.14178]},
+    {"t":0.82874, "x":3.60656, "y":0.64622, "heading":0.0, "vx":-2.98225, "vy":2.5614, "omega":0.0, "ax":6.13221, "ay":7.86079, "alpha":0.0, "fx":[86.92262,86.92262,86.92262,86.92262], "fy":[111.42482,111.42482,111.42482,111.42482]},
+    {"t":0.87647, "x":3.47121, "y":0.77742, "heading":0.0, "vx":-2.68959, "vy":2.93657, "omega":0.0, "ax":6.27992, "ay":7.74161, "alpha":0.0, "fx":[89.01643,89.01643,89.01643,89.01643], "fy":[109.73542,109.73542,109.73542,109.73542]},
+    {"t":0.9242, "x":3.35, "y":0.92639, "heading":0.0, "vx":-2.38987, "vy":3.30604, "omega":0.0, "ax":6.46689, "ay":7.58391, "alpha":0.0, "fx":[91.6666,91.6666,91.6666,91.6666], "fy":[107.50016,107.50016,107.50016,107.50016]},
+    {"t":0.97192, "x":3.2433, "y":1.09281, "heading":0.0, "vx":-2.08123, "vy":3.66799, "omega":0.0, "ax":6.7103, "ay":7.36639, "alpha":0.0, "fx":[95.11691,95.11691,95.11691,95.11691], "fy":[104.41678,104.41678,104.41678,104.41678]},
+    {"t":1.01965, "x":3.15161, "y":1.27626, "heading":0.0, "vx":-1.76097, "vy":4.01956, "omega":0.0, "ax":7.03825, "ay":7.04932, "alpha":0.0, "fx":[99.76552,99.76552,99.76552,99.76552], "fy":[99.92246,99.92246,99.92246,99.92246]},
+    {"t":1.06737, "x":3.07559, "y":1.47612, "heading":0.0, "vx":-1.42506, "vy":4.356, "omega":0.0, "ax":7.49805, "ay":6.55121, "alpha":0.0, "fx":[106.2831,106.2831,106.2831,106.2831], "fy":[92.86177,92.86177,92.86177,92.86177]},
+    {"t":1.1151, "x":3.01611, "y":1.69148, "heading":0.0, "vx":-1.06721, "vy":4.66867, "omega":0.0, "ax":8.1667, "ay":5.68339, "alpha":0.0, "fx":[115.76102,115.76102,115.76102,115.76102], "fy":[80.56077,80.56077,80.56077,80.56077]},
+    {"t":1.16283, "x":2.97448, "y":1.92077, "heading":0.0, "vx":-0.67745, "vy":4.93991, "omega":0.0, "ax":9.11468, "ay":3.95938, "alpha":0.0, "fx":[129.19837,129.19837,129.19837,129.19837], "fy":[56.12323,56.12323,56.12323,56.12323]},
+    {"t":1.21055, "x":2.95253, "y":2.16104, "heading":0.0, "vx":-0.24244, "vy":5.12888, "omega":0.0, "ax":9.89834, "ay":0.62318, "alpha":0.0, "fx":[140.30662,140.30662,140.30662,140.30662], "fy":[8.83349,8.83349,8.83349,8.83349]},
+    {"t":1.25828, "x":2.95223, "y":2.40653, "heading":0.0, "vx":0.22997, "vy":5.15862, "omega":0.0, "ax":9.85123, "ay":-0.86737, "alpha":0.0, "fx":[139.63888,139.63888,139.63888,139.63888], "fy":[-12.29475,-12.29475,-12.29475,-12.29475]},
+    {"t":1.306, "x":2.97443, "y":2.65175, "heading":0.0, "vx":0.70013, "vy":5.11722, "omega":0.0, "ax":9.67601, "ay":-1.76797, "alpha":0.0, "fx":[137.1551,137.1551,137.1551,137.1551], "fy":[-25.06052,-25.06052,-25.06052,-25.06052]},
+    {"t":1.35373, "x":3.01886, "y":2.89396, "heading":0.0, "vx":1.16193, "vy":5.03285, "omega":0.0, "ax":9.3637, "ay":-2.60515, "alpha":0.0, "fx":[132.7282,132.7282,132.7282,132.7282], "fy":[-36.92739,-36.92739,-36.92739,-36.92739]},
+    {"t":1.40146, "x":3.08498, "y":3.13119, "heading":0.0, "vx":1.60883, "vy":4.90851, "omega":0.0, "ax":8.78706, "ay":-3.30587, "alpha":0.0, "fx":[124.55443,124.55443,124.55443,124.55443], "fy":[-46.85998,-46.85998,-46.85998,-46.85998]},
+    {"t":1.44918, "x":3.17177, "y":3.36169, "heading":0.0, "vx":2.0282, "vy":4.75073, "omega":0.0, "ax":7.22092, "ay":-3.40166, "alpha":0.0, "fx":[102.35475,102.35475,102.35475,102.35475], "fy":[-48.21772,-48.21772,-48.21772,-48.21772]},
+    {"t":1.49691, "x":3.27679, "y":3.58455, "heading":0.0, "vx":2.37283, "vy":4.58839, "omega":0.0, "ax":2.72623, "ay":-1.45936, "alpha":0.0, "fx":[38.64371,38.64371,38.64371,38.64371], "fy":[-20.68608,-20.68608,-20.68608,-20.68608]},
+    {"t":1.54464, "x":3.39314, "y":3.80187, "heading":0.0, "vx":2.50294, "vy":4.51874, "omega":0.0, "ax":0.42887, "ay":-0.23881, "alpha":0.0, "fx":[6.07906,6.07906,6.07906,6.07906], "fy":[-3.38509,-3.38509,-3.38509,-3.38509]},
+    {"t":1.59236, "x":3.51309, "y":4.01726, "heading":0.0, "vx":2.52341, "vy":4.50734, "omega":0.0, "ax":0.06165, "ay":-0.03454, "alpha":0.0, "fx":[0.87385,0.87385,0.87385,0.87385], "fy":[-0.48959,-0.48959,-0.48959,-0.48959]},
+    {"t":1.64009, "x":3.63359, "y":4.23234, "heading":0.0, "vx":2.52635, "vy":4.50569, "omega":0.0, "ax":0.00885, "ay":-0.00496, "alpha":0.0, "fx":[0.12548,0.12548,0.12548,0.12548], "fy":[-0.07037,-0.07037,-0.07037,-0.07037]},
+    {"t":1.68781, "x":3.75417, "y":4.44738, "heading":0.0, "vx":2.52677, "vy":4.50545, "omega":0.0, "ax":0.00127, "ay":-0.00071, "alpha":0.0, "fx":[0.01802,0.01802,0.01802,0.01802], "fy":[-0.01011,-0.01011,-0.01011,-0.01011]},
+    {"t":1.73554, "x":3.87477, "y":4.6624, "heading":0.0, "vx":2.52683, "vy":4.50542, "omega":0.0, "ax":0.00018, "ay":-0.0001, "alpha":0.0, "fx":[0.00259,0.00259,0.00259,0.00259], "fy":[-0.00145,-0.00145,-0.00145,-0.00145]},
+    {"t":1.78327, "x":3.99536, "y":4.87743, "heading":0.0, "vx":2.52684, "vy":4.50542, "omega":0.0, "ax":0.00003, "ay":-0.00001, "alpha":0.0, "fx":[0.00037,0.00037,0.00037,0.00037], "fy":[-0.00021,-0.00021,-0.00021,-0.00021]},
+    {"t":1.83099, "x":4.11596, "y":5.09246, "heading":0.0, "vx":2.52684, "vy":4.50541, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00005,0.00005,0.00005,0.00005], "fy":[-0.00003,-0.00003,-0.00003,-0.00003]},
+    {"t":1.87872, "x":4.23656, "y":5.30748, "heading":0.0, "vx":2.52684, "vy":4.50541, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.00001,0.00001,0.00001,0.00001], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.92645, "x":4.35715, "y":5.52251, "heading":0.0, "vx":2.52684, "vy":4.50541, "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":1.97417, "x":4.47775, "y":5.73754, "heading":0.0, "vx":2.52684, "vy":4.50541, "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":2.0219, "x":4.59835, "y":5.95256, "heading":0.0, "vx":2.52684, "vy":4.50541, "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":2.06962, "x":4.71894, "y":6.16759, "heading":0.0, "vx":2.52684, "vy":4.50541, "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":2.11735, "x":4.83954, "y":6.38261, "heading":0.0, "vx":2.52684, "vy":4.50541, "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":2.16508, "x":4.96014, "y":6.59764, "heading":0.0, "vx":2.52684, "vy":4.50541, "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":2.2128, "x":5.08073, "y":6.81267, "heading":0.0, "vx":2.52684, "vy":4.50541, "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":2.26053, "x":5.20133, "y":7.02769, "heading":0.0, "vx":2.52684, "vy":4.50541, "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":2.30825, "x":5.32193, "y":7.24272, "heading":0.0, "vx":2.52684, "vy":4.50541, "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":2.35598, "x":5.44252, "y":7.45775, "heading":0.0, "vx":2.52684, "vy":4.50541, "omega":0.0, "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":2.40371, "x":5.56312, "y":7.67277, "heading":0.0, "vx":2.52684, "vy":4.50541, "omega":0.0, "ax":-0.00304, "ay":-0.00543, "alpha":0.0, "fx":[-0.04316,-0.04316,-0.04316,-0.04316], "fy":[-0.07696,-0.07696,-0.07696,-0.07696]},
+    {"t":2.45143, "x":5.68371, "y":7.88779, "heading":0.0, "vx":2.5267, "vy":4.50516, "omega":0.0, "ax":-4.24799, "ay":-7.57426, "alpha":0.0, "fx":[-60.21426,-60.21426,-60.21426,-60.21426], "fy":[-107.36331,-107.36331,-107.36331,-107.36331]},
+    {"t":2.49916, "x":5.79946, "y":8.09418, "heading":0.0, "vx":2.32396, "vy":4.14366, "omega":0.0, "ax":-4.84029, "ay":-8.63034, "alpha":0.0, "fx":[-68.60997,-68.60997,-68.60997,-68.60997], "fy":[-122.33303,-122.33303,-122.33303,-122.33303]},
+    {"t":2.54689, "x":5.90487, "y":8.28211, "heading":0.0, "vx":2.09295, "vy":3.73177, "omega":0.0, "ax":-4.86045, "ay":-8.66628, "alpha":0.0, "fx":[-68.89566,-68.89566,-68.89566,-68.89566], "fy":[-122.84243,-122.84243,-122.84243,-122.84243]},
+    {"t":2.59461, "x":5.99922, "y":8.45034, "heading":0.0, "vx":1.86098, "vy":3.31816, "omega":0.0, "ax":-4.86745, "ay":-8.67877, "alpha":0.0, "fx":[-68.99499,-68.99499,-68.99499,-68.99499], "fy":[-123.01953,-123.01953,-123.01953,-123.01953]},
+    {"t":2.64234, "x":6.08249, "y":8.59882, "heading":0.0, "vx":1.62867, "vy":2.90396, "omega":0.0, "ax":-4.87101, "ay":-8.68512, "alpha":0.0, "fx":[-69.04542,-69.04542,-69.04542,-69.04542], "fy":[-123.10946,-123.10946,-123.10946,-123.10946]},
+    {"t":2.69006, "x":6.15467, "y":8.72753, "heading":0.0, "vx":1.3962, "vy":2.48945, "omega":0.0, "ax":-4.87316, "ay":-8.68895, "alpha":0.0, "fx":[-69.07593,-69.07593,-69.07593,-69.07593], "fy":[-123.16385,-123.16385,-123.16385,-123.16385]},
+    {"t":2.73779, "x":6.21576, "y":8.83644, "heading":0.0, "vx":1.16362, "vy":2.07476, "omega":0.0, "ax":-4.87461, "ay":-8.69152, "alpha":0.0, "fx":[-69.09637,-69.09637,-69.09637,-69.09637], "fy":[-123.20029,-123.20029,-123.20029,-123.20029]},
+    {"t":2.78552, "x":6.26574, "y":8.92556, "heading":0.0, "vx":0.93097, "vy":1.65995, "omega":0.0, "ax":-4.87564, "ay":-8.69337, "alpha":0.0, "fx":[-69.11101,-69.11101,-69.11101,-69.11101], "fy":[-123.2264,-123.2264,-123.2264,-123.2264]},
+    {"t":2.83324, "x":6.30462, "y":8.99489, "heading":0.0, "vx":0.69828, "vy":1.24505, "omega":0.0, "ax":-4.87642, "ay":-8.69475, "alpha":0.0, "fx":[-69.12202,-69.12202,-69.12202,-69.12202], "fy":[-123.24603,-123.24603,-123.24603,-123.24603]},
+    {"t":2.88097, "x":6.33239, "y":9.04441, "heading":0.0, "vx":0.46555, "vy":0.83008, "omega":0.0, "ax":-4.87702, "ay":-8.69583, "alpha":0.0, "fx":[-69.1306,-69.1306,-69.1306,-69.1306], "fy":[-123.26133,-123.26133,-123.26133,-123.26133]},
+    {"t":2.92869, "x":6.34906, "y":9.07412, "heading":0.0, "vx":0.23278, "vy":0.41506, "omega":0.0, "ax":-4.87751, "ay":-8.6967, "alpha":0.0, "fx":[-69.13747,-69.13747,-69.13747,-69.13747], "fy":[-123.27358,-123.27358,-123.27358,-123.27358]},
+    {"t":2.97642, "x":6.35461, "y":9.08402, "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/autos.chor b/src/main/deploy/choreo/autos.chor
index 6844f02..32cdf6f 100644
--- a/src/main/deploy/choreo/autos.chor
+++ b/src/main/deploy/choreo/autos.chor
@@ -5,6 +5,20 @@
  "variables":{
   "expressions":{},
   "poses":{
+   "AllianceLBumper":{
+    "x":{
+     "exp":"AllianceRBumper.x",
+     "val":3.5734095573425293
+    },
+    "y":{
+     "exp":"5.5177741050720215 m",
+     "val":5.5177741050720215
+    },
+    "heading":{
+     "exp":"0 deg",
+     "val":0.0
+    }
+   },
    "AllianceLMid":{
     "x":{
      "exp":"6 m",
@@ -33,6 +47,20 @@
      "val":0.0
     }
    },
+   "AllianceRBumper":{
+    "x":{
+     "exp":"3.5734095573425293 m",
+     "val":3.5734095573425293
+    },
+    "y":{
+     "exp":"2.43641996383667 m",
+     "val":2.43641996383667
+    },
+    "heading":{
+     "exp":"0 deg",
+     "val":0.0
+    }
+   },
    "AllianceRMid":{
     "x":{
      "exp":"AllianceLMid.x",
@@ -60,53 +88,123 @@
      "exp":"AllianceLSide.heading",
      "val":0.0
     }
+   },
+   "ClimbPose":{
+    "x":{
+     "exp":"1.504361629486084 m",
+     "val":1.504361629486084
+    },
+    "y":{
+     "exp":"4.103729248046875 m",
+     "val":4.103729248046875
+    },
+    "heading":{
+     "exp":"0 deg",
+     "val":0.0
+    }
+   },
+   "MidBallPoseR":{
+    "x":{
+     "exp":"7.529880046844482 m",
+     "val":7.529880046844482
+    },
+    "y":{
+     "exp":"2.1440699100494385 m",
+     "val":2.1440699100494385
+    },
+    "heading":{
+     "exp":"0 deg",
+     "val":0.0
+    }
+   },
+   "MidBallPoseR2":{
+    "x":{
+     "exp":"7.490900039672852 m",
+     "val":7.490900039672852
+    },
+    "y":{
+     "exp":"3.0211198329925537 m",
+     "val":3.0211198329925537
+    },
+    "heading":{
+     "exp":"MidBallPoseR.heading",
+     "val":0.0
+    }
+   },
+   "MidLBumper":{
+    "x":{
+     "exp":"5.6198601722717285 m",
+     "val":5.6198601722717285
+    },
+    "y":{
+     "exp":"AllianceLBumper.y",
+     "val":5.5177741050720215
+    },
+    "heading":{
+     "exp":"MidRBumper.heading",
+     "val":0.0
+    }
+   },
+   "MidRBumper":{
+    "x":{
+     "exp":"5.600369930267334 m",
+     "val":5.600369930267334
+    },
+    "y":{
+     "exp":"AllianceRBumper.y",
+     "val":2.43641996383667
+    },
+    "heading":{
+     "exp":"0 deg",
+     "val":0.0
+    }
    }
   }
  },
  "config":{
   "frontLeft":{
    "x":{
-    "exp":"11 in",
-    "val":0.2794
+    "exp":"12.5 in",
+    "val":0.3175
    },
    "y":{
-    "exp":"11 in",
-    "val":0.2794
+    "exp":"12.5 in",
+    "val":0.3175
    }
   },
   "backLeft":{
    "x":{
-    "exp":"-11 in",
-    "val":-0.2794
+    "exp":"-12.5 in",
+    "val":-0.3175
    },
    "y":{
-    "exp":"11 in",
-    "val":0.2794
+    "exp":"12.5 in",
+    "val":0.3175
    }
   },
   "mass":{
-   "exp":"150 lbs",
-   "val":68.0388555
+   "exp":"125 lbs",
+   "val":56.69904625
   },
   "inertia":{
-   "exp":"6 kg m ^ 2",
-   "val":6.0
+   "exp":"7.15 kg m ^ 2",
+   "val":7.15
   },
   "gearing":{
-   "exp":"6.5",
-   "val":6.5
+   "exp":"5.67",
+   "val":5.67
   },
   "radius":{
-   "exp":"2 in",
-   "val":0.0508
+   "exp":"1.836765207 in",
+   "val":0.0466538362578
   },
   "vmax":{
    "exp":"6000 RPM",
    "val":628.3185307179587
   },
   "tmax":{
-   "exp":"1.2 N * m",
-   "val":1.2
+   "exp":"1.164 N * m",
+   "val":1.164
   },
   "cof":{
    "exp":"1.5",
@@ -114,16 +212,16 @@
   },
   "bumper":{
    "front":{
-    "exp":"16 in",
-    "val":0.4064
+    "exp":"18 in",
+    "val":0.4572
    },
    "side":{
-    "exp":"16 in",
-    "val":0.4064
+    "exp":"18 in",
+    "val":0.4572
    },
    "back":{
-    "exp":"16 in",
-    "val":0.4064
+    "exp":"18 in",
+    "val":0.4572
    }
   },
   "differentialTrackWidth":{
diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
index d1f84f1..ac5f521 100644
--- a/src/main/deploy/pathplanner/navgrid.json
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -1 +1 @@
-{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
+{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 30ac7ce..c87f2c6 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -5,6 +5,8 @@
 package frc.robot;
 
 import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.NamedCommands;
+
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -48,6 +50,8 @@ public class RobotContainer {
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
+    NamedCommands.registerCommand("Drive Over Bump To Middle", swerve.driveOverBump("To Middle"));
+    NamedCommands.registerCommand("Drive Over Bump To Alliance", swerve.driveOverBump("To Alliance"));
     // Configure the trigger bindings
     configureDriverBindings();
     swerve.configureAutoBuilder();

From fcfbed457b8d820c104900657491098d0600c69b Mon Sep 17 00:00:00 2001
From: NandaGuntupalli <nanda.guntupalli@gmail.com>
Date: Thu, 22 Jan 2026 15:14:12 -0500
Subject: [PATCH 2/2] Added Spotless Apply to my Code

---
 src/main/java/frc/robot/RobotContainer.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index c87f2c6..41a357e 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -6,7 +6,6 @@
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
-
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -51,7 +50,8 @@ public class RobotContainer {
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     NamedCommands.registerCommand("Drive Over Bump To Middle", swerve.driveOverBump("To Middle"));
-    NamedCommands.registerCommand("Drive Over Bump To Alliance", swerve.driveOverBump("To Alliance"));
+    NamedCommands.registerCommand(
+        "Drive Over Bump To Alliance", swerve.driveOverBump("To Alliance"));
     // Configure the trigger bindings
     configureDriverBindings();
     swerve.configureAutoBuilder();