diff --git a/PathWeaver/FiveBallAuton/PathWeaver/Paths/FiveBallGetTerminalBalls.path b/PathWeaver/FiveBallAuton/PathWeaver/Paths/FiveBallGetTerminalBalls.path
index b3d2bad9..a916edac 100644
--- a/PathWeaver/FiveBallAuton/PathWeaver/Paths/FiveBallGetTerminalBalls.path
+++ b/PathWeaver/FiveBallAuton/PathWeaver/Paths/FiveBallGetTerminalBalls.path
@@ -1,3 +1,3 @@
 X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
 5.3,-6.2296,-2.0,0.0,true,false,
-1.16,-7.029599999999999,-2.5,-3.0,true,false,
+0.92,-7.249599999999999,-2.5,-3.0,true,false,
diff --git a/PathWeaver/FiveBallAuton/PathWeaver/Paths/FiveBallGetWallBall.path b/PathWeaver/FiveBallAuton/PathWeaver/Paths/FiveBallGetWallBall.path
index 03612ea8..b3c8a881 100644
--- a/PathWeaver/FiveBallAuton/PathWeaver/Paths/FiveBallGetWallBall.path
+++ b/PathWeaver/FiveBallAuton/PathWeaver/Paths/FiveBallGetWallBall.path
@@ -1,3 +1,3 @@
 X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
 7.6,-6.579599999999999,0.0,-1.2,true,false,
-7.7,-7.7296,0.0,-1.2,true,false,
+7.7,-7.8796,0.0,-1.2,true,false,
diff --git a/PathWeaver/FiveBallAuton/PathWeaver/Paths/FiveBallShootTerminalBalls.path b/PathWeaver/FiveBallAuton/PathWeaver/Paths/FiveBallShootTerminalBalls.path
index c22f8af7..a61d1bb4 100644
--- a/PathWeaver/FiveBallAuton/PathWeaver/Paths/FiveBallShootTerminalBalls.path
+++ b/PathWeaver/FiveBallAuton/PathWeaver/Paths/FiveBallShootTerminalBalls.path
@@ -1,3 +1,3 @@
 X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
-1.16,-7.029599999999999,-2.0,-2.0,true,true,
+0.92,-7.249599999999999,-2.0,-2.0,true,true,
 7.6,-6.579599999999999,-0.5,-3.0,true,true,
diff --git a/src/main/deploy/FiveBallAuton/output/FiveBallGetTerminalBalls.wpilib.json b/src/main/deploy/FiveBallAuton/output/FiveBallGetTerminalBalls.wpilib.json
index 0789ef3a..66a311fa 100644
--- a/src/main/deploy/FiveBallAuton/output/FiveBallGetTerminalBalls.wpilib.json
+++ b/src/main/deploy/FiveBallAuton/output/FiveBallGetTerminalBalls.wpilib.json
@@ -1,6 +1,6 @@
 [
 {
-"acceleration": 3.1,
+"acceleration": 3.1000000000000005,
 "curvature": -0.0,
 "pose": {
 "rotation": {
@@ -15,783 +15,813 @@
 "velocity": 0.0
 },
 {
-"acceleration": 3.0999999999999996,
-"curvature": -0.14939633178085712,
+"acceleration": 3.1000000000000005,
+"curvature": -0.06225208614632311,
 "pose": {
 "rotation": {
-"radians": 3.1364109766938992
+"radians": 3.1393638343275776
 },
 "translation": {
-"x": 5.236934896111488,
-"y": 2.000113612413406
+"x": 5.236865044236183,
+"y": 2.00004958152771
 }
 },
-"time": 0.20171075782620923,
-"velocity": 0.6253033492612486
+"time": 0.20182230324119085,
+"velocity": 0.6256491400476918
 },
 {
-"acceleration": 3.1000000000000005,
-"curvature": -0.205242583662105,
+"acceleration": 3.0999999999999983,
+"curvature": -0.07559264940851151,
 "pose": {
 "rotation": {
-"radians": 3.124139199708356
+"radians": 3.1345154319257844
 },
 "translation": {
-"x": 5.17068925857544,
-"y": 2.0008432388305666
+"x": 5.170156879425049,
+"y": 2.0003552246093754
 }
 },
-"time": 0.28884068795182066,
-"velocity": 0.8954061326506441
+"time": 0.2894308544500954,
+"velocity": 0.8972356487952959
 },
 {
-"acceleration": 3.100000000000002,
-"curvature": -0.1892545077945501,
+"acceleration": 3.100000000000001,
+"curvature": -0.057104656436799094,
 "pose": {
 "rotation": {
-"radians": 3.1096302339894053
+"radians": 3.1295514636548507
 },
 "translation": {
-"x": 5.098642050623893,
-"y": 2.0026310861110685
+"x": 5.096932174563408,
+"y": 2.00106369972229
 }
 },
-"time": 0.36045155271653995,
-"velocity": 1.117399813421274
+"time": 0.3619588039230498,
+"velocity": 1.1220722921614543
 },
 {
-"acceleration": 3.1,
-"curvature": -0.14228053470247518,
+"acceleration": 3.100000000000001,
+"curvature": -0.029041063103085674,
 "pose": {
 "rotation": {
-"radians": 3.096318276718395
+"radians": 3.1260301169395395
 },
 "translation": {
-"x": 5.018745727539063,
-"y": 2.0057434082031254
+"x": 5.014893188476562,
+"y": 2.0022119140625003
 }
 },
-"time": 0.42604041037504325,
-"velocity": 1.3207252721626344
+"time": 0.42889128878040517,
+"velocity": 1.329562995219256
 },
 {
-"acceleration": 3.0999999999999983,
-"curvature": -0.09167440376355492,
+"acceleration": 3.099999999999997,
+"curvature": -0.0034031373043908616,
 "pose": {
 "rotation": {
-"radians": 3.0859376946300334
+"radians": 3.124592532296335
 },
 "translation": {
-"x": 4.929485681653023,
-"y": 2.0102855265140533
+"x": 4.922342064976692,
+"y": 2.003737211227417
 }
 },
-"time": 0.489052214089175,
-"velocity": 1.5160618636764427
+"time": 0.49362561548832706,
+"velocity": 1.530239408013814
 },
 {
-"acceleration": 3.100000000000001,
-"curvature": -0.04855823255644728,
+"acceleration": 3.1000000000000014,
+"curvature": 0.01646570419242234,
 "pose": {
 "rotation": {
-"radians": 3.079056500168169
+"radians": 3.125344206351967
 },
 "translation": {
-"x": 4.829839687347412,
-"y": 2.0162168502807614
+"x": 4.81813512802124,
+"y": 2.0054876708984377
 }
 },
-"time": 0.5509751307007084,
-"velocity": 1.708022905172196
+"time": 0.5575894678801759,
+"velocity": 1.7285273504285452
 },
 {
-"acceleration": 3.099999999999999,
-"curvature": -0.014924765552287906,
+"acceleration": 3.1000000000000045,
+"curvature": 0.031067702824198906,
 "pose": {
 "rotation": {
-"radians": 3.075653219373112
+"radians": 3.128172640038975
 },
 "translation": {
-"x": 4.719237346053124,
-"y": 2.0233658969402315
+"x": 4.7016371768713,
+"y": 2.007232408523559
 }
 },
-"time": 0.612436814402405,
-"velocity": 1.898554124647456
+"time": 0.6213488647965796,
+"velocity": 1.9261814808693967
 },
 {
-"acceleration": 3.099999999999998,
-"curvature": 0.010698948564768147,
+"acceleration": 3.0999999999999965,
+"curvature": 0.03686023111543499,
 "pose": {
 "rotation": {
-"radians": 3.075489354301132
+"radians": 3.1303160322311676
 },
 "translation": {
-"x": 4.59751953125,
-"y": 2.0314453125
+"x": 4.638708359282464,
+"y": 2.0080114230513573
 }
 },
-"time": 0.6736314148916257,
-"velocity": 2.08825738616404
+"time": 0.653204990546217,
+"velocity": 2.0249354706932725
 },
 {
-"acceleration": 3.1000000000000085,
-"curvature": 0.021204005724306862,
+"acceleration": 3.1000000000000028,
+"curvature": 0.041903109430538835,
 "pose": {
 "rotation": {
-"radians": 3.0765374890806423
+"radians": 3.132921567510807
 },
 "translation": {
-"x": 4.532545071113854,
-"y": 2.035716123320162
+"x": 4.57267578125,
+"y": 2.008671875
 }
 },
-"time": 0.7041226795609535,
-"velocity": 2.182780306638956
+"time": 0.6850405489933958,
+"velocity": 2.123625701879527
 },
 {
-"acceleration": 3.100000000000004,
-"curvature": 0.03053030230055489,
+"acceleration": 3.0999999999999948,
+"curvature": 0.046366490191747274,
 "pose": {
 "rotation": {
-"radians": 3.0782989492086648
+"radians": 3.1359750692785835
 },
 "translation": {
-"x": 4.46489783346653,
-"y": 2.040066891908646
+"x": 4.503582698423415,
+"y": 2.00916728168726
 }
 },
-"time": 0.7345218165701232,
-"velocity": 2.2770176313673822
+"time": 0.7168388117759863,
+"velocity": 2.2222003165055577
 },
 {
-"acceleration": 3.0999999999999917,
-"curvature": 0.038916590914491477,
+"acceleration": 3.1000000000000023,
+"curvature": 0.05039882070565486,
 "pose": {
 "rotation": {
-"radians": 3.0807493596641753
+"radians": 3.1394660399111727
 },
 "translation": {
-"x": 4.3946555381082,
-"y": 2.0444357989355924
+"x": 4.431495576500892,
+"y": 2.009448156356812
 }
 },
-"time": 0.7648055184383399,
-"velocity": 2.370897107158854
+"time": 0.7485760186430589,
+"velocity": 2.3205856577934827
 },
 {
-"acceleration": 3.0999999999999956,
-"curvature": 0.04657410340428204,
+"acceleration": 3.10000000000001,
+"curvature": 0.054127774556309634,
 "pose": {
 "rotation": {
-"radians": 3.0838695934439957
+"radians": -3.1397974660854957
 },
 "translation": {
-"x": 4.321914005279541,
-"y": 2.0487565994262695
+"x": 4.356502662952989,
+"y": 2.0094623300433154
 }
 },
-"time": 0.7949466777934432,
-"velocity": 2.464334701159674
+"time": 0.7802234176319289,
+"velocity": 2.41869259465898
 },
 {
-"acceleration": 3.1,
-"curvature": 0.05368812753171694,
+"acceleration": 3.100000000000008,
+"curvature": 0.05766275594132402,
 "pose": {
 "rotation": {
-"radians": 3.0876459849450866
+"radians": -3.1354475880886223
 },
 "translation": {
-"x": 4.246785888317972,
-"y": 2.052959092147648
+"x": 4.278712558746338,
+"y": 2.0091552734375
 }
 },
-"time": 0.8249155991390411,
-"velocity": 2.5572383573310273
+"time": 0.8117488136121205,
+"velocity": 2.516421322197574
 },
 {
-"acceleration": 3.099999999999999,
-"curvature": 0.06042109455638786,
+"acceleration": 3.1000000000000103,
+"curvature": 0.06109796377193628,
 "pose": {
 "rotation": {
-"radians": 3.092070366938977
+"radians": -3.1306685546635244
 },
 "translation": {
-"x": 4.169399406313897,
-"y": 2.0569695889949795
+"x": 4.198252790067345,
+"y": 2.00847041875124
 }
 },
-"time": 0.8546809397771341,
-"velocity": 2.6495109133091157
+"time": 0.8431177527328201,
+"velocity": 2.6136650334717433
 },
 {
-"acceleration": 3.100000000000006,
-"curvature": 0.06691623451178441,
+"acceleration": 3.1000000000000014,
+"curvature": 0.06451548746739127,
 "pose": {
 "rotation": {
-"radians": 3.0971400148463903
+"radians": -3.125455535512537
 },
 "translation": {
-"x": 4.089897076766938,
-"y": 2.060711384378374
+"x": 4.115268380045891,
+"y": 2.007349481582642
 }
 },
-"time": 0.8842104455882581,
-"velocity": 2.7410523813236
+"time": 0.8742944344550655,
+"velocity": 2.7103127468107044
 },
 {
-"acceleration": 3.099999999999996,
-"curvature": 0.07330128667362283,
+"acceleration": 3.0999999999999934,
+"curvature": 0.06798818847391308,
 "pose": {
 "rotation": {
-"radians": 3.102857552569999
+"radians": -3.1198000277086573
 },
 "translation": {
-"x": 4.008434448242188,
-"y": 2.064105224609375
+"x": 4.02992042047903,
+"y": 2.0057327827811235
 }
 },
-"time": 0.9134715305464339,
-"velocity": 2.8317617446939454
+"time": 0.9052424182148793,
+"velocity": 2.8062514964661274
 },
 {
-"acceleration": 3.0999999999999868,
-"curvature": 0.07969201032464455,
+"acceleration": 3.099999999999994,
+"curvature": 0.07158227800005239,
 "pose": {
 "rotation": {
-"radians": 3.1092308539044526
+"radians": -3.1136899154953674
 },
 "translation": {
-"x": 3.9251788330264388,
-"y": 2.067069777287543
+"x": 3.9423846435546874,
+"y": 2.0035595703124995
 }
 },
-"time": 0.9424317367902882,
-"velocity": 2.9215383840498936
+"time": 0.9359251744480981,
+"velocity": 2.9013680407891056
 },
 {
-"acceleration": 3.100000000000002,
-"curvature": 0.08619538889557767,
+"acceleration": 3.1000000000000054,
+"curvature": 0.0753595831493309,
 "pose": {
 "rotation": {
-"radians": 3.116272959735339
+"radians": -3.1071095011783028
 },
 "translation": {
-"x": 3.8403080397844316,
-"y": 2.069522100687027
+"x": 3.8528499935753646,
+"y": 2.0007683411240578
 }
 },
-"time": 0.9710591032873,
-"velocity": 3.01028322019063
+"time": 0.9663065173036353,
+"velocity": 2.9955502036412707
 },
 {
-"acceleration": 3.1000000000000014,
-"curvature": 0.092912500790419,
+"acceleration": 3.099999999999995,
+"curvature": 0.07937953230000636,
 "pose": {
 "rotation": {
-"radians": 3.1240020217808473
+"radians": -3.1000395025624057
 },
 "translation": {
-"x": 3.75400910621509,
-"y": 2.0713781131431457
+"x": 3.7615171986818314,
+"y": 1.9972971630096437
 }
 },
-"time": 0.9993224646375305,
-"velocity": 3.0978996403763444
+"time": 0.9963509473452129,
+"velocity": 3.0886879367701616
 },
 {
-"acceleration": -0.13738451657531175,
-"curvature": 0.09994106927781776,
+"acceleration": 0.6862218589037145,
+"curvature": 0.08370090527364979,
 "pose": {
 "rotation": {
-"radians": 3.1324412769991254
+"radians": -3.0924570156935345
 },
 "translation": {
-"x": 3.666477031707764,
-"y": 2.072553062438965
+"x": 3.668597342576832,
+"y": 1.993083996474743
 }
 },
-"time": 1.0271916967106947,
-"velocity": 3.1842942598031536
+"time": 1.0260239258864718,
+"velocity": 3.1806741702480643
 },
 {
-"acceleration": -0.30195694936286444,
-"curvature": 0.10737771801924006,
+"acceleration": -0.1852815778744542,
+"curvature": 0.08838339553884791,
 "pose": {
 "rotation": {
-"radians": -3.141566255306724
+"radians": -3.084335443913738
 },
 "translation": {
-"x": 3.5779135099984707,
-"y": 2.0729619951918723
+"x": 3.57431043624878,
+"y": 1.988067016601562
 }
 },
-"time": 1.0550213066179368,
-"velocity": 3.1804709022995676
+"time": 1.0556150887645521,
+"velocity": 3.200980273045383
 },
 {
-"acceleration": -0.32006421974210014,
-"curvature": 0.11531995762024967,
+"acceleration": -0.19990077029084574,
+"curvature": 0.09348902644670312,
 "pose": {
 "rotation": {
-"radians": -3.131616515405519
+"radians": -3.0756443959603974
 },
 "translation": {
-"x": 3.4885256618261336,
-"y": 2.072520226240158
+"x": 3.4788839896954595,
+"y": 1.9821849349141116
 }
 },
-"time": 1.0831644725889558,
-"velocity": 3.171972877757546
+"time": 1.0855091653030993,
+"velocity": 3.1954414513752214
 },
 {
-"acceleration": -0.34195940917695306,
-"curvature": 0.12386791717761024,
+"acceleration": -0.21691680240685277,
+"curvature": 0.09908345431329735,
 "pose": {
 "rotation": {
-"radians": -3.1208562002962705
+"radians": -3.0663495574166837
 },
 "translation": {
-"x": 3.3985247675888246,
-"y": 2.071143808029592
+"x": 3.3825515836477287,
+"y": 1.9753773212432861
 }
 },
-"time": 1.111582324810536,
-"velocity": 3.1628773400594996
+"time": 1.1157597934726768,
+"velocity": 3.189394327502341
 },
 {
-"acceleration": -0.3677450541413191,
-"curvature": 0.13312581546484378,
+"acceleration": -0.23651592450311382,
+"curvature": 0.10523717923142925,
 "pose": {
 "rotation": {
-"radians": -3.109241484993691
+"radians": -3.0564125415588923
 },
 "translation": {
-"x": 3.3081250000000004,
-"y": 2.0687499999999996
+"x": 3.28555144129321,
+"y": 1.9675849255919458
 }
 },
-"time": 1.14021816867741,
-"velocity": 3.1530850438095
+"time": 1.1463028383287384,
+"velocity": 3.182769027876395
 },
 {
-"acceleration": -0.3975637730421008,
-"curvature": 0.143203139594679,
+"acceleration": -0.25892754281093017,
+"curvature": 0.11202666981607158,
 "pose": {
 "rotation": {
-"radians": -3.0967232927235315
+"radians": -3.04579072781183
 },
 "translation": {
-"x": 3.2175421567447486,
-"y": 2.0652577379718426
+"x": 3.1881250000000003,
+"y": 1.9587500000000002
 }
 },
-"time": 1.1690161954782456,
-"velocity": 3.1424947118844635
+"time": 1.1770742153956537,
+"velocity": 3.1754911071811796
 },
 {
-"acceleration": -0.4315746390624188,
-"curvature": 0.15421546389306934,
+"acceleration": -0.284417318230058,
+"curvature": 0.11953538985237645,
 "pose": {
 "rotation": {
-"radians": -3.0832472937146846
+"radians": -3.0344370988771865
 },
 "translation": {
-"x": 3.1269923931360246,
-"y": 2.0605881035327913
+"x": 3.090515483040363,
+"y": 1.948816620409488
 }
 },
-"time": 1.197921949925308,
-"velocity": 3.131002831083861
+"time": 1.2080103941909004,
+"velocity": 3.1674808784217667
 },
 {
-"acceleration": -0.46992423210583184,
-"curvature": 0.16628479663972162,
+"acceleration": -0.31327821030605635,
+"curvature": 0.12785469155615498,
 "pose": {
 "rotation": {
-"radians": -3.0687539696653654
+"radians": -3.022300091419547
 },
 "translation": {
-"x": 3.036690954770893,
-"y": 2.0546647934243083
+"x": 2.992966471314431,
+"y": 1.9377310085296626
 }
 },
-"time": 1.2268827945786849,
-"velocity": 3.118504065005637
+"time": 1.2390489038691321,
+"velocity": 3.1586529887372263
 },
 {
-"acceleration": -0.5127096815206689,
-"curvature": 0.17953928512579365,
+"acceleration": -0.34581777687614657,
+"curvature": 0.13708450981908302,
 "pose": {
 "rotation": {
-"radians": -3.053178782754079
+"radians": -3.0093234802924633
 },
 "translation": {
-"x": 2.946850910186768,
-"y": 2.0474145889282225
+"x": 2.8957204750739045,
+"y": 1.9254418537020683
 }
 },
-"time": 1.255848366683732,
-"velocity": 3.1048924407766667
+"time": 1.2701288388736436,
+"velocity": 3.1489163223225844
 },
 {
-"acceleration": -0.5599308341043244,
-"curvature": 0.19411203802303395,
+"acceleration": -0.3823396493357344,
+"curvature": 0.14733375177695363,
 "pose": {
 "rotation": {
-"radians": -3.0364524992271376
+"radians": -2.9954463230179122
 },
 "translation": {
-"x": 2.857681883517653,
-"y": 2.0387678252533084
+"x": 2.799017505645753,
+"y": 1.911900634765625
 }
 },
-"time": 1.2847710214295274,
-"velocity": 3.0900635156732177
+"time": 1.3011913616214081,
+"velocity": 3.1381743497617878
 },
 {
-"acceleration": -0.6114286869038621,
-"curvature": 0.21013873702963223,
+"acceleration": -0.4231166253831016,
+"curvature": 0.1587202231946665,
 "pose": {
 "rotation": {
-"radians": -3.0185017312022526
+"radians": -2.9806029999937795
 },
 "translation": {
-"x": 2.7693887871503837,
-"y": 2.02865886092186
+"x": 2.703093647155911,
+"y": 1.8970619419217112
 }
 },
-"time": 1.3136062537422108,
-"velocity": 3.073917779992785
+"time": 1.33218019810489,
+"velocity": 3.126326088887371
 },
 {
-"acceleration": -0.6668076167946194,
-"curvature": 0.22775360794488914,
+"acceleration": -0.4683523048245795,
+"curvature": 0.1713698640938533,
 "pose": {
 "rotation": {
-"radians": -2.999249776448002
+"radians": -2.964723397117218
 },
 "translation": {
-"x": 2.682170554380865,
-"y": 2.0170265471562745
+"x": 2.6081796282529837,
+"y": 1.8808837985992426
 }
 },
-"time": 1.3423130884072627,
-"velocity": 3.056365597768366
+"time": 1.3630421208018935,
+"velocity": 3.1132678963029807
 },
 {
-"acceleration": -0.7253399984092461,
-"curvature": 0.24708320970888095,
+"acceleration": -0.5181277384843995,
+"curvature": 0.18541497752359032,
 "pose": {
 "rotation": {
-"radians": -2.9786178542361146
+"radians": -2.9477332915780763
 },
 "translation": {
-"x": 2.5962188720703128,
-"y": 2.003814697265625
+"x": 2.5144993938319398,
+"y": 1.863327983319759
 }
 },
-"time": 1.3708544255575157,
-"velocity": 3.037334016763074
+"time": 1.393727411372849,
+"velocity": 3.0988963697398617
 },
 {
-"acceleration": -0.7858538663783982,
-"curvature": 0.26823739082900444,
+"acceleration": -0.572329339283991,
+"curvature": 0.20099102530350013,
 "pose": {
 "rotation": {
-"radians": -2.9565268550059636
+"radians": -2.929555018790172
 },
 "translation": {
-"x": 2.5117169133014987,
-"y": 1.988972556032241
+"x": 2.422268676757813,
+"y": 1.8443603515625
 }
 },
-"time": 1.3991973254169594,
-"velocity": 3.0167757778241118
+"time": 1.4241902932265387,
+"velocity": 3.083112705657292
 },
 {
-"acceleration": -0.8466076799172795,
-"curvature": 0.29129667994144093,
+"acceleration": -0.6305545927282462,
+"curvature": 0.21823143377434928,
 "pose": {
 "rotation": {
-"radians": -2.9328997406462003
+"radians": -2.910108518824826
 },
 "translation": {
-"x": 2.428838070034982,
-"y": 1.9724552690982824
+"x": 2.331693569589407,
+"y": 1.82395115762949
 }
 },
-"time": 1.4273132128009827,
-"velocity": 2.9946807990167175
+"time": 1.4543893211039536,
+"velocity": 3.0658289159851924
 },
 {
-"acceleration": -2.5464827813335797,
-"curvature": 0.3162953618144895,
+"acceleration": -0.6919932908270083,
+"curvature": 0.23725970775499194,
 "pose": {
 "rotation": {
-"radians": -2.9076647473827535
+"radians": -2.8893128838255713
 },
 "translation": {
-"x": 2.347744685765357,
-"y": 1.9542243523523215
+"x": 2.2429690963029874,
+"y": 1.8020753765106203
 }
 },
-"time": 1.4551779785403793,
-"velocity": 2.9710902743426484
+"time": 1.4842877113654107,
+"velocity": 3.046976348690649
 },
 {
-"acceleration": -3.100000000000005,
-"curvature": 0.34319960448866693,
+"acceleration": -2.285223195999495,
+"curvature": 0.25817800909327804,
 "pose": {
 "rotation": {
-"radians": -2.880759549340817
+"radians": -2.867088552385802
 },
 "translation": {
-"x": 2.268586788177492,
-"y": 1.9342481613159181
+"x": 2.156277784015984,
+"y": 1.7787130257487291
 }
 },
-"time": 1.4829873851501598,
-"velocity": 2.9002740992517384
+"time": 1.5138535927542323,
+"velocity": 3.0265169571321975
 },
 {
-"acceleration": -3.0999999999999996,
-"curvature": 0.3718803337390351,
+"acceleration": -3.1000000000000014,
+"curvature": 0.2810512511733834,
 "pose": {
 "rotation": {
-"radians": -2.8521365302631523
+"radians": -2.8433603200642206
 },
 "translation": {
-"x": 2.1915008218027667,
-"y": 1.9125023605301976
+"x": 2.071788234710696,
+"y": 1.753849487304687
 }
 },
-"time": 1.5110236425886217,
-"velocity": 2.8133617011925067
+"time": 1.5432806404599206,
+"velocity": 2.9592695851253747
 },
 {
-"acceleration": -3.1000000000000054,
-"curvature": 0.4020812011508549,
+"acceleration": -3.099999999999997,
+"curvature": 0.3058857505915382,
 "pose": {
 "rotation": {
-"radians": -2.821769273667714
+"radians": -2.8180613523973967
 },
 "translation": {
-"x": 2.1166083806753173,
-"y": 1.8889703929424293
+"x": 1.9896536969579781,
+"y": 1.727475829422474
 }
 },
-"time": 1.5393697511191355,
-"velocity": 2.725488764747914
+"time": 1.5728906419714515,
+"velocity": 2.867478580439629
 },
 {
-"acceleration": -3.0999999999999943,
-"curvature": 0.43338305991591386,
+"acceleration": -3.0999999999999925,
+"curvature": 0.3326016513894477,
 "pose": {
 "rotation": {
-"radians": -2.7896603063045533
+"radians": -2.7911383905693485
 },
 "translation": {
-"x": 2.044014940988273,
-"y": 1.8636439492926007
+"x": 1.910010637640955,
+"y": 1.6995891284942628
 }
 },
-"time": 1.5680469017083256,
-"velocity": 2.6365895979214242
+"time": 1.6028022671552142,
+"velocity": 2.7747525423699644
 },
 {
-"acceleration": -3.1000000000000054,
-"curvature": 0.4651678928077634,
+"acceleration": -3.100000000000006,
+"curvature": 0.36099881841884157,
 "pose": {
 "rotation": {
-"radians": -2.7558500049803323
+"radians": -2.7625583197658488
 },
 "translation": {
-"x": 1.9738085937500012,
-"y": 1.8365234375000004
+"x": 1.8329773136787137,
+"y": 1.670192790925503
 }
 },
-"time": 1.5970881383988795,
-"velocity": 2.5465617641807072
+"time": 1.6330275567895132,
+"velocity": 2.681054144503638
 },
 {
-"acceleration": -3.0999999999999983,
-"curvature": 0.4965870435585693,
+"acceleration": -3.1000000000000028,
+"curvature": 0.39071682873604313,
 "pose": {
 "rotation": {
-"radians": -2.720426396095654
+"radians": -2.7323162125622686
 },
 "translation": {
-"x": 1.9060587774403412,
-"y": 1.8076184520497929
+"x": 1.7586523437500032,
+"y": 1.6392968750000003
 }
 },
-"time": 1.626540708938047,
-"velocity": 2.4552587955092875
+"time": 1.663589601640533,
+"velocity": 2.5863118054654763
 },
 {
-"acceleration": -3.1000000000000063,
-"curvature": 0.5265405720745356,
+"acceleration": -3.100000000000006,
+"curvature": 0.42119120114574893,
 "pose": {
 "rotation": {
-"radians": -2.683535344922066
+"radians": -2.7004448491902995
 },
 "translation": {
-"x": 1.840815010666848,
-"y": 1.7769482433795938
+"x": 1.6871132800169315,
+"y": 1.6069184127449994
 }
 },
-"time": 1.6564688770763984,
-"velocity": 2.3624814742803983
+"time": 1.694524973145882,
+"velocity": 2.490412153798894
 },
 {
-"acceleration": -3.0999999999999934,
-"curvature": 0.5536759679193193,
+"acceleration": -3.1000000000000076,
+"curvature": 0.4516101285726997,
 "pose": {
 "rotation": {
-"radians": -2.645390370627519
+"radians": -2.6670255398280327
 },
 "translation": {
-"x": 1.7781056248210403,
-"y": 1.7445421872660525
+"x": 1.6184151798486761,
+"y": 1.5730817317962646
 }
 },
-"time": 1.6869573579904311,
-"velocity": 2.267967183446897
+"time": 1.725886671923851,
+"velocity": 2.39319088758719
 },
 {
-"acceleration": -3.0999999999999908,
-"curvature": 0.5764144170052974,
+"acceleration": -3.099999999999997,
+"curvature": 0.4808785375882151,
 "pose": {
 "rotation": {
-"radians": -2.606281081089789
+"radians": -2.6321998261791983
 },
 "translation": {
-"x": 1.717936496734624,
-"y": 1.7104402542114272
+"x": 1.5525891775451628,
+"y": 1.5378187772631655
 }
 },
-"time": 1.7181156385767669,
-"velocity": 2.1713765136292564
+"time": 1.7577477638946473,
+"velocity": 2.294421502477722
 },
 {
-"acceleration": -3.1000000000000014,
-"curvature": 0.5930103634921927,
+"acceleration": -3.099999999999999,
+"curvature": 0.5075987666362337,
 "pose": {
 "rotation": {
-"radians": -2.5665790713043313
+"radians": -2.5961813332729102
 },
 "translation": {
-"x": 1.6602897813357442,
-"y": 1.67469347883016
+"x": 1.4896410560607949,
+"y": 1.501169433593751
 }
 },
-"time": 1.7500836171257186,
-"velocity": 2.0722757801275065
+"time": 1.7902059701825763,
+"velocity": 2.193801062985142
 },
 {
-"acceleration": -3.099999999999998,
-"curvature": 0.601644684385469,
+"acceleration": -3.099999999999993,
+"curvature": 0.5300785746755934,
 "pose": {
 "rotation": {
-"radians": -2.526740156408992
+"radians": -2.559266718134954
 },
 "translation": {
-"x": 1.6051226443052338,
-"y": 1.6373644292354594
+"x": 1.4295498187281224,
+"y": 1.4631818464398387
 }
 },
-"time": 1.783039298587691,
-"velocity": 1.9701131675953918
+"time": 1.823389643570088,
+"velocity": 2.090931675483856
 },
 {
-"acceleration": -3.1000000000000005,
-"curvature": 0.600543846044809,
+"acceleration": -3.100000000000002,
+"curvature": 0.5463762946441645,
 "pose": {
 "rotation": {
-"radians": -2.4873020865593176
+"radians": -2.521844393894039
 },
 "translation": {
-"x": 1.5523659947328303,
-"y": 1.598527676425876
+"x": 1.3722662609815641,
+"y": 1.4239127445220952
 }
 },
-"time": 1.8172098219144808,
-"velocity": 1.864184545282344
+"time": 1.8574658579555954,
+"velocity": 1.9852954108887833
 },
 {
-"acceleration": -3.100000000000001,
-"curvature": 0.5881088132446398,
+"acceleration": -3.0999999999999988,
+"curvature": 0.5543885791284731,
 "pose": {
 "rotation": {
-"radians": -2.4488774352380704
+"radians": -2.4843995976006688
 },
 "translation": {
-"x": 1.5019232177734407,
-"y": 1.5582702636718766
+"x": 1.3177115420810894,
+"y": 1.3834277614951151
 }
 },
-"time": 1.8528881019931212,
-"velocity": 1.7535818770385592
+"time": 1.8926518688626586,
+"velocity": 1.8762187770768872
 },
 {
-"acceleration": -3.1000000000000014,
-"curvature": 0.5243746331495378,
+"acceleration": -3.1,
+"curvature": 0.5519780784283186,
+"pose": {
+"rotation": {
+"radians": -2.4475145210618363
+},
+"translation": {
+"x": 1.2657757568359438,
+"y": 1.3418017578125019
+}
+},
+"time": 1.9292322019200245,
+"velocity": 1.762819744599053
+},
+{
+"acceleration": -3.1,
+"curvature": 0.537127968117313,
 "pose": {
 "rotation": {
-"radians": -2.3778206163447884
+"radians": -2.41186269612263
 },
 "translation": {
-"x": 1.4074475985765487,
-"y": 1.4739068090915701
+"x": 1.2163165073283073,
+"y": 1.2991191425919553
 }
 },
-"time": 1.9304326453637572,
-"velocity": 1.513193792589587
+"time": 1.9675856026822394,
+"velocity": 1.6439242022361868
 },
 {
-"acceleration": -3.1000000000000005,
-"curvature": 0.40455607704192775,
+"acceleration": -3.1,
+"curvature": 0.5081008953924083,
+"pose": {
+"rotation": {
+"radians": -2.378197582575478
+},
+"translation": {
+"x": 1.1691574746370383,
+"y": 1.2554741954803488
+}
+},
+"time": 2.008230272409786,
+"velocity": 1.517925726080792
+},
+{
+"acceleration": -3.1,
+"curvature": 0.4027441432998056,
 "pose": {
 "rotation": {
-"radians": -2.319465088822003
+"radians": -2.3201392349776833
 },
 "translation": {
-"x": 1.320324230194097,
-"y": 1.3852376937866229
+"x": 1.0808566093444911,
+"y": 1.1657257080078152
 }
 },
-"time": 2.0209810904492893,
-"velocity": 1.2324936128244377
+"time": 2.0997228588054555,
+"velocity": 1.2342987082542163
 },
 {
 "acceleration": -3.1,
-"curvature": 0.2284810326064992,
+"curvature": 0.23171987963313434,
 "pose": {
 "rotation": {
-"radians": -2.2799992926243
+"radians": -2.2802714865632456
 },
 "translation": {
-"x": 1.2386600631475515,
-"y": 1.2934561431407952
+"x": 0.9987299150228557,
+"y": 1.0735201740264921
 }
 },
-"time": 2.13783088559494,
-"velocity": 0.8702592478729201
+"time": 2.2170469847024172,
+"velocity": 0.8705939179736353
 },
 {
 "acceleration": -3.1,
-"curvature": 4.0566202598742524e-15,
+"curvature": -1.670373048183504e-15,
 "pose": {
 "rotation": {
-"radians": -2.265534602991597
+"radians": -2.2655346029915986
 },
 "translation": {
-"x": 1.1600000000000064,
-"y": 1.2000000000000028
+"x": 0.9200000000000079,
+"y": 0.980000000000004
 }
 },
-"time": 2.418559675231366,
+"time": 2.497883732435848,
 "velocity": 0.0
 }
 ]
\ No newline at end of file
diff --git a/src/main/deploy/FiveBallAuton/output/FiveBallGetWallBall.wpilib.json b/src/main/deploy/FiveBallAuton/output/FiveBallGetWallBall.wpilib.json
index 43ce541d..7bcfc8ef 100644
--- a/src/main/deploy/FiveBallAuton/output/FiveBallGetWallBall.wpilib.json
+++ b/src/main/deploy/FiveBallAuton/output/FiveBallGetWallBall.wpilib.json
@@ -1,6 +1,6 @@
 [
 {
-"acceleration": 3.1000000000000005,
+"acceleration": 3.1,
 "curvature": 0.0,
 "pose": {
 "rotation": {
@@ -15,243 +15,243 @@
 "velocity": 0.0
 },
 {
-"acceleration": 3.1000000000000005,
-"curvature": 0.21637301791119118,
+"acceleration": 3.1,
+"curvature": 0.2081928972408239,
 "pose": {
 "rotation": {
-"radians": -1.562176478150235
+"radians": -1.5622865055170152
 },
 "translation": {
 "x": 7.600221824645995,
-"y": 1.575110912322998
+"y": 1.5747781753540044
 }
 },
-"time": 0.21980844476318354,
-"velocity": 0.6814061787658691
+"time": 0.22029621073435426,
+"velocity": 0.6829182532764982
 },
 {
-"acceleration": 3.1,
-"curvature": 0.35710654946387,
+"acceleration": 3.0999999999999996,
+"curvature": 0.3124817620468699,
 "pose": {
 "rotation": {
-"radians": -1.5404444139264255
+"radians": -1.5417657271209522
 },
 "translation": {
 "x": 7.601605224609375,
-"y": 1.500802612304688
+"y": 1.4983947753906257
 }
 },
-"time": 0.31026585115217037,
-"velocity": 0.9618241385717283
+"time": 0.3127588767381111,
+"velocity": 0.9695525178881443
 },
 {
-"acceleration": 3.1000000000000005,
-"curvature": 0.43105058793230355,
+"acceleration": 3.100000000000001,
+"curvature": 0.33346763067318097,
 "pose": {
 "rotation": {
-"radians": -1.5111122100698562
+"radians": -1.516011539146444
 },
 "translation": {
 "x": 7.604876899719238,
-"y": 1.4274384498596193
+"y": 1.4201231002807617
 }
 },
-"time": 0.3790035057291948,
-"velocity": 1.174910867760504
+"time": 0.38517536835913224,
+"velocity": 1.1940436419133098
 },
 {
-"acceleration": 3.0999999999999965,
-"curvature": 0.44139710738709803,
+"acceleration": 3.100000000000004,
+"curvature": 0.30044582829401556,
 "pose": {
 "rotation": {
-"radians": -1.4791234553195867
+"radians": -1.4901814678938443
 },
 "translation": {
 "x": 7.6103515625,
-"y": 1.3551757812500007
+"y": 1.3396484375000002
 }
 },
-"time": 0.4363465792089174,
-"velocity": 1.3526743955476441
+"time": 0.4476596974443338,
+"velocity": 1.3877450620774348
 },
 {
-"acceleration": 3.1000000000000005,
-"curvature": 0.3923508686676922,
+"acceleration": 3.1000000000000028,
+"curvature": 0.23807932474186239,
 "pose": {
 "rotation": {
-"radians": -1.4489428197522833
+"radians": -1.467706783779369
 },
 "translation": {
 "x": 7.618000602722168,
-"y": 1.2840003013610843
+"y": 1.2569993972778324
 }
 },
-"time": 0.4863973774731056,
-"velocity": 1.5078318701666273
+"time": 0.5039335633574615,
+"velocity": 1.5621940464081308
 },
 {
-"acceleration": 3.100000000000006,
-"curvature": 0.29262360357063255,
+"acceleration": 3.0999999999999974,
+"curvature": 0.1623873713193968,
 "pose": {
 "rotation": {
-"radians": -1.4243973175143918
+"radians": -1.4506310488148493
 },
 "translation": {
 "x": 7.627520751953124,
-"y": 1.2137603759765634
+"y": 1.1724792480468755
 }
 },
-"time": 0.5313311900271879,
-"velocity": 1.6471266890842826
+"time": 0.5557184417500466,
+"velocity": 1.7227271694251445
 },
 {
-"acceleration": 3.0999999999999344,
-"curvature": 0.1559965281054033,
+"acceleration": 3.0999999999994317,
+"curvature": 0.08183643332401251,
 "pose": {
 "rotation": {
-"radians": -1.4084324058618651
+"radians": -1.440050348996164
 },
 "translation": {
 "x": 7.63840274810791,
-"y": 1.1442013740539556
+"y": 1.0865972518920914
 }
 },
-"time": 0.5724818367234625,
-"velocity": 1.7746936938427342
+"time": 0.6038822262014356,
+"velocity": 1.8720349012244502
 },
 {
-"acceleration": -3.1000000000000014,
-"curvature": 1.8344284778684586e-14,
+"acceleration": -3.0999999999999983,
+"curvature": 1.478055728588439e-14,
 "pose": {
 "rotation": {
-"radians": -1.4029004035445192
+"radians": -1.436474884841925
 },
 "translation": {
 "x": 7.65,
-"y": 1.075000000000001
+"y": 1.0000000000000018
 }
 },
-"time": 0.6107406250661576,
-"velocity": 1.8932959377050864
+"time": 0.6488772758298983,
+"velocity": 2.011519555072659
 },
 {
-"acceleration": -3.0999999999999983,
-"curvature": -0.1559965281053661,
+"acceleration": -3.099999999999999,
+"curvature": -0.08183643332398244,
 "pose": {
 "rotation": {
-"radians": -1.4084324058618625
+"radians": -1.4400503489961614
 },
 "translation": {
 "x": 7.66159725189209,
-"y": 1.0057986259460456
+"y": 0.913402748107913
 }
 },
-"time": 0.648999413408853,
-"velocity": 1.7746936938427302
+"time": 0.6938723254583608,
+"velocity": 1.8720349012244253
 },
 {
-"acceleration": -3.0999999999999988,
-"curvature": -0.2926236035705924,
+"acceleration": -3.099999999999999,
+"curvature": -0.16238737131936462,
 "pose": {
 "rotation": {
-"radians": -1.4243973175143867
+"radians": -1.450631048814844
 },
 "translation": {
 "x": 7.6724792480468755,
-"y": 0.9362396240234387
+"y": 0.8275207519531289
 }
 },
-"time": 0.6901500601051272,
-"velocity": 1.6471266890842802
+"time": 0.7420361099097503,
+"velocity": 1.7227271694251176
 },
 {
-"acceleration": -3.100000000000001,
-"curvature": -0.3923508686676474,
+"acceleration": -3.1,
+"curvature": -0.23807932474182708,
 "pose": {
 "rotation": {
-"radians": -1.448942819752275
+"radians": -1.4677067837793603
 },
 "translation": {
 "x": 7.681999397277833,
-"y": 0.8659996986389178
+"y": 0.7430006027221729
 }
 },
-"time": 0.7350838726592097,
-"velocity": 1.5078318701666245
+"time": 0.7938209883023357,
+"velocity": 1.562194046408103
 },
 {
 "acceleration": -3.0999999999999988,
-"curvature": -0.4413971073870464,
+"curvature": -0.3004458282939752,
 "pose": {
 "rotation": {
-"radians": -1.4791234553195747
+"radians": -1.4901814678938323
 },
 "translation": {
 "x": 7.689648437500002,
-"y": 0.7948242187500023
+"y": 0.6603515625000069
 }
 },
-"time": 0.7851346709233974,
-"velocity": 1.3526743955476426
+"time": 0.8500948542154634,
+"velocity": 1.3877450620774072
 },
 {
-"acceleration": -3.100000000000001,
-"curvature": -0.43105058793224205,
+"acceleration": -3.1,
+"curvature": -0.33346763067313295,
 "pose": {
 "rotation": {
-"radians": -1.5111122100698402
+"radians": -1.5160115391464277
 },
 "translation": {
 "x": 7.695123100280765,
-"y": 0.7225615501403837
+"y": 0.5798768997192472
 }
 },
-"time": 0.8424777444031202,
-"velocity": 1.174910867760502
+"time": 0.9125791833006649,
+"velocity": 1.1940436419132825
 },
 {
 "acceleration": -3.099999999999999,
-"curvature": -0.35710654946379533,
+"curvature": -0.3124817620468095,
 "pose": {
 "rotation": {
-"radians": -1.5404444139264044
+"radians": -1.5417657271209309
 },
 "translation": {
 "x": 7.6983947753906286,
-"y": 0.649197387695315
+"y": 0.5016052246093867
 }
 },
-"time": 0.9112153989801448,
-"velocity": 0.9618241385717257
+"time": 0.9849956749216844,
+"velocity": 0.9695525178881221
 },
 {
-"acceleration": -3.0999999999999996,
-"curvature": -0.21637301791109742,
+"acceleration": -3.1,
+"curvature": -0.20819289724074072,
 "pose": {
 "rotation": {
-"radians": -1.562176478150207
+"radians": -1.5622865055169874
 },
 "translation": {
 "x": 7.69977817535401,
-"y": 0.5748890876770059
+"y": 0.42522182464601155
 }
 },
-"time": 1.0016728053691308,
-"velocity": 0.6814061787658688
+"time": 1.0774583409254392,
+"velocity": 0.6829182532764823
 },
 {
-"acceleration": -3.0999999999999996,
-"curvature": 1.1842378929335095e-13,
+"acceleration": -3.1,
+"curvature": 1.184237892933685e-13,
 "pose": {
 "rotation": {
 "radians": -1.570796326794861
 },
 "translation": {
 "x": 7.700000000000008,
-"y": 0.5000000000000036
+"y": 0.3500000000000192
 }
 },
-"time": 1.2214812501323145,
+"time": 1.2977545516597884,
 "velocity": 0.0
 }
 ]
\ No newline at end of file
diff --git a/src/main/deploy/FiveBallAuton/output/FiveBallShootTerminalBalls.wpilib.json b/src/main/deploy/FiveBallAuton/output/FiveBallShootTerminalBalls.wpilib.json
index d903d6f0..01305871 100644
--- a/src/main/deploy/FiveBallAuton/output/FiveBallShootTerminalBalls.wpilib.json
+++ b/src/main/deploy/FiveBallAuton/output/FiveBallShootTerminalBalls.wpilib.json
@@ -7,1436 +7,1466 @@
 "radians": -2.356194490192345
 },
 "translation": {
-"x": 1.16,
-"y": 1.2000000000000002
+"x": 0.92,
+"y": 0.9800000000000004
 }
 },
 "time": 0.0,
 "velocity": -0.0
 },
 {
-"acceleration": -3.100000000000002,
-"curvature": 0.9822359575894634,
+"acceleration": -3.0999999999999983,
+"curvature": 0.9759588008716872,
 "pose": {
 "rotation": {
-"radians": -2.4031349721578223
+"radians": -2.4031242029554725
 },
 "translation": {
-"x": 1.2239654856920241,
-"y": 1.2619333893060682
+"x": 0.9840353375673294,
+"y": 1.0419974201917652
 }
 },
-"time": 0.23967120470011538,
-"velocity": -0.7429807345703577
+"time": 0.2397986620609087,
+"velocity": -0.743375852388817
 },
 {
-"acceleration": -3.1000000000000005,
-"curvature": 1.2865514458725489,
+"acceleration": -3.099999999999999,
+"curvature": 1.2652258567094021,
 "pose": {
 "rotation": {
-"radians": -2.4556512535031616
+"radians": -2.455286655677203
 },
 "translation": {
-"x": 1.2585758504085243,
-"y": 1.2918848245404666
+"x": 1.0188059879653155,
+"y": 1.0720957839675256
 }
 },
-"time": 0.2949098718844232,
-"velocity": -0.9142206028417121
+"time": 0.29525102937986136,
+"velocity": -0.9152781910775702
 },
 {
-"acceleration": -3.1000000000000005,
-"curvature": 1.4429985126886005,
+"acceleration": -3.1000000000000023,
+"curvature": 1.4014084275219811,
 "pose": {
 "rotation": {
-"radians": -2.5208898350994104
+"radians": -2.5195933603352274
 },
 "translation": {
-"x": 1.296157932281494,
-"y": 1.3206891059875492
+"x": 1.0566903114318849,
+"y": 1.1011771202087406
 }
 },
-"time": 0.34281298729135395,
-"velocity": -1.0627202606031976
+"time": 0.3434903063050969,
+"velocity": -1.0648199495458004
 },
 {
-"acceleration": -3.1000000000000005,
-"curvature": 1.4624715335021672,
+"acceleration": -3.0999999999999974,
+"curvature": 1.401480674221298,
 "pose": {
 "rotation": {
-"radians": -2.5934394288477276
+"radians": -2.590490634714561
 },
 "translation": {
-"x": 1.3375010232813656,
-"y": 1.3480427780188622
+"x": 1.0985155129618942,
+"y": 1.1289727268926804
 }
 },
-"time": 0.3866565326772165,
-"velocity": -1.1986352512993714
+"time": 0.3877949393689783,
+"velocity": -1.2021643120438328
 },
 {
-"acceleration": -3.1000000000000036,
-"curvature": 1.3763170330113328,
+"acceleration": -3.099999999999998,
+"curvature": 1.3020494924305914,
 "pose": {
 "rotation": {
-"radians": -2.6683043829788122
+"radians": -2.6630119167930872
 },
 "translation": {
-"x": 1.3832980436086655,
-"y": 1.3736801594495773
+"x": 1.1450079196691514,
+"y": 1.1552475458383569
 }
 },
-"time": 0.42821065239100187,
-"velocity": -1.3274530224121062
+"time": 0.4299286476651745,
+"velocity": -1.332778807762041
 },
 {
-"acceleration": -3.100000000000002,
-"curvature": 1.2255189419037449,
+"acceleration": -3.099999999999991,
+"curvature": 1.1461900251155233,
 "pose": {
 "rotation": {
-"radians": -2.741457309815668
+"radians": -2.7333009738329497
 },
 "translation": {
-"x": 1.434149021860212,
-"y": 1.3973719689063726
+"x": 1.1967966218851507,
+"y": 1.1797989355959002
 }
 },
-"time": 0.4685695419965958,
-"velocity": -1.4525655801894475
+"time": 0.47097251653926975,
+"velocity": -1.4600148012717362
 },
 {
-"acceleration": -3.0999999999999956,
-"curvature": 1.0482167916418275,
+"acceleration": -3.1000000000000036,
+"curvature": 0.9710496255449543,
 "pose": {
 "rotation": {
-"radians": -2.81011273049768
+"radians": -2.798807234190852
 },
 "translation": {
-"x": 1.4905645751953125,
-"y": 1.418923950195313
+"x": 1.2544171142578127,
+"y": 1.2024554443359383
 }
 },
-"time": 0.5084486639041895,
-"velocity": -1.5761908581029882
+"time": 0.5116249828932217,
+"velocity": -1.5860374469689869
 },
 {
 "acceleration": -3.099999999999999,
-"curvature": 0.8720289868657095,
+"curvature": 0.8017382210739181,
 "pose": {
 "rotation": {
-"radians": -2.8726891334233144
+"radians": -2.858172410421378
 },
 "translation": {
-"x": 1.552969389501959,
-"y": 1.438175497669727
+"x": 1.31831493685022,
+"y": 1.223075582738966
 }
 },
-"time": 0.5483187821905217,
-"velocity": -1.6997882247906178
+"time": 0.5523385704854585,
+"velocity": -1.7122495685049213
 },
 {
-"acceleration": -3.0999999999999996,
-"curvature": 0.7126966799462008,
+"acceleration": -3.1000000000000023,
+"curvature": 0.6514678910298322,
 "pose": {
 "rotation": {
-"radians": -2.928565806492925
+"radians": -2.9109448350820837
 },
 "translation": {
-"x": 1.6217056995630263,
-"y": 1.4549982815980913
+"x": 1.388849316239357,
+"y": 1.241546596884728
 }
 },
-"time": 0.5884796775396854,
-"velocity": -1.8242870003730256
+"time": 0.5933956651229623,
+"velocity": -1.8395265618811827
 },
 {
-"acceleration": -3.0999999999999974,
-"curvature": 0.5766298009169327,
+"acceleration": -3.1000000000000005,
+"curvature": 0.5248229062579082,
 "pose": {
 "rotation": {
-"radians": -2.9777761017693454
+"radians": -2.9572634564661366
 },
 "translation": {
-"x": 1.6970367692224682,
-"y": 1.4692948735319078
+"x": 1.466296806614846,
+"y": 1.2577832411415883
 }
 },
-"time": 0.6291077195950785,
-"velocity": -1.950233930744744
+"time": 0.6349573091313736,
+"velocity": -1.968367658307258
 },
 {
-"acceleration": -3.0999999999999948,
-"curvature": 0.4644392654556843,
+"acceleration": -3.1000000000000045,
+"curvature": 0.4213753628187834,
 "pose": {
 "rotation": {
-"radians": -3.020734976291746
+"radians": -2.997595385625014
 },
 "translation": {
-"x": 1.7791503715515136,
-"y": 1.4809973716735838
+"x": 1.5508549308776856,
+"y": 1.2717265510559086
 }
 },
-"time": 0.6702897386363453,
-"velocity": -2.077898189772671
+"time": 0.6770975672273896,
+"velocity": -2.0990024584049074
 },
 {
-"acceleration": -3.100000000000005,
-"curvature": 0.37383063342427,
+"acceleration": -3.0999999999999948,
+"curvature": 0.3383812045353688,
 "pose": {
 "rotation": {
-"radians": -3.0580392412502526
+"radians": -3.0325521604073287
 },
 "translation": {
-"x": 1.8681622690148652,
-"y": 1.490066026244313
+"x": 1.6426458217389883,
+"y": 1.283342616241426
 }
 },
-"time": 0.7120481977346628,
-"velocity": -2.207349412977455
+"time": 0.7198286993993137,
+"velocity": -2.2314689681378725
 },
 {
 "acceleration": -3.099999999999993,
-"curvature": 0.30145549469270644,
+"curvature": 0.27239755213745453,
 "pose": {
 "rotation": {
-"radians": -3.0903396630862514
+"radians": -3.0627764906592834
 },
 "translation": {
-"x": 1.9641196936368943,
-"y": 1.496487864851952
+"x": 1.741719862818718,
+"y": 1.2926213532686237
 }
 },
-"time": 0.7543600822472433,
-"velocity": -2.338516254966455
+"time": 0.763119782205788,
+"velocity": -2.3656713248379426
 },
 {
-"acceleration": -3.100000000000004,
-"curvature": 0.2439016956546648,
+"acceleration": -3.1000000000000023,
+"curvature": 0.22009660820844362,
 "pose": {
 "rotation": {
-"radians": -3.118268207820987
+"radians": -3.0888802974029548
 },
 "translation": {
-"x": 2.067004827167839,
-"y": 1.5002753178589048
+"x": 1.8480593297444283,
+"y": 1.2995752785541121
 }
 },
-"time": 0.7971709979034728,
-"velocity": -2.471230093500766
+"time": 0.8069104314184868,
+"velocity": -2.5014223373973086
 },
 {
-"acceleration": -3.0999999999999943,
-"curvature": 0.19813089560072256,
+"acceleration": -3.0999999999999996,
+"curvature": 0.1785950980895716,
 "pose": {
 "rotation": {
-"radians": 3.140783286691578
+"radians": -3.1114156954483994
 },
 "translation": {
-"x": 2.1767382812499996,
-"y": 1.50146484375
+"x": 1.9615820312499999,
+"y": 1.3042382812500009
 }
 },
-"time": 0.8404055783728421,
-"velocity": -2.605257292955811
+"time": 0.8511208236365375,
+"velocity": -2.638474553273266
 },
 {
-"acceleration": -3.1,
-"curvature": 0.16161339895679486,
+"acceleration": -3.100000000000019,
+"curvature": 0.14553168131470884,
 "pose": {
 "rotation": {
-"radians": 3.1199357513147596
+"radians": -3.130865245855992
 },
 "translation": {
-"x": 2.2931825775839387,
-"y": 1.500115554500371
+"x": 2.082144950274378,
+"y": 1.3066643961332742
 }
 },
-"time": 0.8839750742771468,
-"velocity": -2.7403227302591553
+"time": 0.8956589373358487,
+"velocity": -2.7765427057411305
 },
 {
-"acceleration": -3.1000000000000116,
-"curvature": 0.13231886724422756,
+"acceleration": -3.099999999999997,
+"curvature": 0.13156129238171102,
 "pose": {
 "rotation": {
-"radians": 3.1019363653395144
+"radians": -3.1395646318853805
 },
 "translation": {
-"x": 2.4161456280946734,
-"y": 1.496307840943337
+"x": 2.1450060625228797,
+"y": 1.3070607130980356
 }
 },
-"time": 0.9277828188371353,
-"velocity": -2.8761267383951195
+"time": 0.9180203125873886,
+"velocity": -2.8458629690209047
 },
 {
-"acceleration": -3.0999999999999974,
-"curvature": 0.11986641481292916,
+"acceleration": -3.100000000000007,
+"curvature": 0.11903132830941344,
 "pose": {
 "rotation": {
-"radians": 3.0938860101839385
+"radians": 3.135542788958473
 },
 "translation": {
-"x": 2.479997582729556,
-"y": 1.4935126773343654
+"x": 2.20954788506031,
+"y": 1.3069265764951714
 }
 },
-"time": 0.9497448186330304,
-"velocity": -2.9442089377623946
+"time": 0.9404261107382637,
+"velocity": -2.9153209432886174
 },
 {
-"acceleration": -3.0999999999999934,
-"curvature": 0.10865031121569792,
+"acceleration": -3.1000000000000023,
+"curvature": 0.10777094801659343,
 "pose": {
 "rotation": {
-"radians": 3.086412877156097
+"radians": 3.1280445818432994
 },
 "translation": {
-"x": 2.545384215097874,
-"y": 1.490141998138279
+"x": 2.275737054925994,
+"y": 1.3062738601811006
 }
 },
-"time": 0.971728438683102,
-"velocity": -3.0123581599176164
+"time": 0.9628634567229797,
+"velocity": -2.9848767158412373
 },
 {
 "acceleration": -3.099999999999981,
-"curvature": 0.09852553947498117,
+"curvature": 0.09762985555403635,
 "pose": {
 "rotation": {
-"radians": 3.0794795778453676
+"radians": 3.1210877465203284
 },
 "translation": {
-"x": 2.6122677049523917,
-"y": 1.4862112247472394
+"x": 2.3435370902530845,
+"y": 1.3051154670305554
 }
 },
-"time": 0.9937209132154973,
-"velocity": -3.0805348309680416
+"time": 0.9853194296805585,
+"velocity": -3.054490232009732
 },
 {
-"acceleration": -2.7394182405481553,
-"curvature": 0.08936442761024233,
+"acceleration": -3.1000000000000028,
+"curvature": 0.08847592925346072,
 "pose": {
 "rotation": {
-"radians": 3.0730519265945175
+"radians": 3.114637246486337
 },
 "translation": {
-"x": 2.6806074714660646,
-"y": 1.4817368507385256
+"x": 2.4129085040529024,
+"y": 1.3034652905893749
 }
 },
-"time": 1.0157095173932946,
-"velocity": -3.148699503919213
+"time": 1.007781122082901,
+"velocity": -3.124121478456993
 },
 {
-"acceleration": -0.3727523254119254,
-"curvature": 0.08105457292437536,
+"acceleration": -1.0803238632608552,
+"curvature": 0.08019304617684565,
 "pose": {
 "rotation": {
-"radians": 3.067098707518469
+"radians": 3.108661079177285
 },
 "translation": {
-"x": 2.7503602819872324,
-"y": 1.4767363989172742
+"x": 2.4838089179992675,
+"y": 1.3013381767272953
 }
 },
-"time": 1.0377087349495762,
-"velocity": -3.208964561770678
+"time": 1.0302356885184738,
+"velocity": -3.1937306344072685
 },
 {
-"acceleration": -0.3360996946631954,
-"curvature": 0.07349696893990415,
+"acceleration": -0.32815094289054286,
+"curvature": 0.07267911506804164,
 "pose": {
 "rotation": {
-"radians": 3.061591452304133
+"radians": 3.103130048556626
 },
 "translation": {
-"x": 2.821480360794812,
-"y": 1.4712283783592284
+"x": 2.5561931762128367,
+"y": 1.2987498852907455
 }
 },
-"time": 1.0599094132464988,
-"velocity": -3.217239916231578
+"time": 1.0528283282475184,
+"velocity": -3.2181380022406105
 },
 {
-"acceleration": -0.3040663232199487,
-"curvature": 0.0666043314440014,
+"acceleration": -0.295741174741994,
+"curvature": 0.06584431682443669,
 "pose": {
 "rotation": {
-"radians": 3.0565042310593022
+"radians": 3.0980175500730964
 },
 "translation": {
-"x": 2.893919497853494,
-"y": 1.4652322414534868
+"x": 2.63001345904544,
+"y": 1.2957170517556378
 }
 },
-"time": 1.0824757441142048,
-"velocity": -3.2248244531458825
+"time": 1.075759687427622,
+"velocity": -3.2256629493773232
 },
 {
-"acceleration": -0.27606952624920245,
-"curvature": 0.0602996130054176,
+"acceleration": -0.2674560997214896,
+"curvature": 0.05960954292343158,
 "pose": {
 "rotation": {
-"radians": 3.0518134574520612
+"radians": 3.093299369592887
 },
 "translation": {
-"x": 2.9676271575689315,
-"y": 1.4587683409452437
+"x": 2.705219396864413,
+"y": 1.2922571488801626
 }
 },
-"time": 1.105395033654426,
-"velocity": -3.231793437247191
+"time": 1.099074307196163,
+"velocity": -3.2325580424163345
 },
 {
-"acceleration": -0.25160421553339746,
-"curvature": 0.05451469145702816,
+"acceleration": -0.24276694994373435,
+"curvature": 0.05390501744978275,
 "pose": {
 "rotation": {
-"radians": 3.0474977086550137
+"radians": 3.0889534970638945
 },
 "translation": {
-"x": 3.0425505875429373,
-"y": 1.4518578869785417
+"x": 2.781758183836937,
+"y": 1.2883884483575825
 }
 },
-"time": 1.1286535658566386,
-"velocity": -3.2382144092135077
+"time": 1.1227587951835294,
+"velocity": -3.238892603197336
 },
 {
-"acceleration": -0.23023370542140448,
-"curvature": 0.04918921610188866,
+"acceleration": -0.22121729924922148,
+"curvature": 0.048669086095780086,
 "pose": {
 "rotation": {
-"radians": 3.0435375601051624
+"radians": 3.084959955078967
 },
 "translation": {
-"x": 3.1186349273286758,
-"y": 1.4445229041390126
+"x": 2.8595746917143696,
+"y": 1.2841299824690218
 }
 },
-"time": 1.1522366625826568,
-"velocity": -3.2441480157651057
+"time": 1.1467987377261428,
+"velocity": -3.244728706725229
 },
 {
-"acceleration": -0.21158131750390663,
-"curvature": 0.044269595034492085,
+"acceleration": -0.20241397193559751,
+"curvature": 0.04384715485261197,
 "pose": {
 "rotation": {
-"radians": 3.0399154347526736
+"radians": 3.081300642104477
 },
 "translation": {
-"x": 3.1958233171858597,
-"y": 1.436786188496626
+"x": 2.9386115836165843,
+"y": 1.2795015057362624
 }
 },
-"time": 1.1761287414534431,
-"velocity": -3.2496487776137473
+"time": 1.1711787592694056,
+"velocity": -3.2501219892466673
 },
 {
-"acceleration": -0.19532284767183072,
-"curvature": 0.03970810744570281,
+"acceleration": -0.18601889620938963,
+"curvature": 0.03939076146202627,
 "pose": {
 "rotation": {
-"radians": 3.036615466255967
+"radians": 3.077959189879776
 },
 "translation": {
-"x": 3.274057006835937,
-"y": 1.4286712646484379
+"x": 3.0188094278163042,
+"y": 1.274523456574535
 }
 },
-"time": 1.2003133717517747,
-"velocity": -3.254765793555613
+"time": 1.1958825797597386,
+"velocity": -3.2551223876740996
 },
 {
-"acceleration": -0.18117989786080882,
-"curvature": 0.0354621257420497,
+"acceleration": -0.17174190110411275,
+"curvature": 0.035256763634379185,
 "pose": {
 "rotation": {
-"radians": 3.033623375453521
+"radians": 3.0749208343342147
 },
 "translation": {
-"x": 3.353275464217295,
-"y": 1.420202342761331
+"x": 3.100106811523437,
+"y": 1.2692169189453129
 }
 },
-"time": 1.2247733284641855,
-"velocity": -3.259543381954611
+"time": 1.2208930701465177,
+"velocity": -3.2597748114895038
 },
 {
-"acceleration": -0.16891403761794696,
-"curvature": 0.031493433519228696,
+"acceleration": -0.15933441457306102,
+"curvature": 0.03140662928347536,
 "pose": {
 "rotation": {
-"radians": 3.030926359379607
+"radians": 3.0721722992829257
 },
 "translation": {
-"x": 3.4334164842404427,
-"y": 1.4114042756147684
+"x": 3.182440454669413,
+"y": 1.2636035840091067
 }
 },
-"time": 1.2494906445886274,
-"velocity": -3.2640216627654306
+"time": 1.2461923061202687,
+"velocity": -3.2641197503721173
 },
 {
-"acceleration": -0.15832174007264693,
-"curvature": 0.02776762673029274,
+"acceleration": -0.14858399802244615,
+"curvature": 0.027805815398323105,
 "pose": {
 "rotation": {
-"radians": 3.0285129920734395
+"radians": 3.0697016921293896
 },
 "translation": {
-"x": 3.514416297543212,
-"y": 1.402302515643533
+"x": 3.265745323691517,
+"y": 1.2577057117782537
 }
 },
-"time": 1.2744466618326908,
-"velocity": -3.2682370844009885
+"time": 1.2717616202135795,
+"velocity": -3.2681938220642097
 },
 {
-"acceleration": -0.14923002750065878,
-"curvature": 0.024253586682684302,
+"acceleration": -0.13930964247661937,
+"curvature": 0.02442322355174713,
 "pose": {
 "rotation": {
-"radians": 3.0263731364453075
+"radians": 3.0674984108062517
 },
 "translation": {
-"x": 3.5962096792459493,
-"y": 1.3929230719804764
+"x": 3.349954745317227,
+"y": 1.251546092769714
 }
 },
-"time": 1.2996220798223268,
-"velocity": -3.272222900384164
+"time": 1.2975816523875003,
+"velocity": -3.272030265673679
 },
 {
-"acceleration": -0.1414927595858332,
-"curvature": 0.02092301472535015,
+"acceleration": -0.13135774949714116,
+"curvature": 0.02123072136566733,
 "pose": {
 "rotation": {
-"radians": 3.024497866499209
+"radians": 3.065553061214505
 },
 "translation": {
-"x": 3.6787300577067077,
-"y": 1.3832924674992686
+"x": 3.4350005203485483,
+"y": 1.2451480096578607
 }
 },
-"time": 1.3249970039342127,
-"velocity": -3.2760096010072077
+"time": 1.3236323992201502,
+"velocity": -3.2756593859011844
 },
 {
-"acceleration": -0.13498750004562746,
-"curvature": 0.017750019613899163,
+"acceleration": -0.12459872288949742,
+"curvature": 0.01820272047291198,
 "pose": {
 "rotation": {
-"radians": 3.022879399262116
+"radians": 3.063857384466695
 },
 "translation": {
-"x": 3.761909623276442,
-"y": 1.3734376958571382
+"x": 3.5208130374463504,
+"y": 1.2385351989272753
 }
 },
-"time": 1.350550991856701,
-"velocity": -3.2796253052767836
+"time": 1.349893261806532,
+"velocity": -3.2791089537103852
 },
 {
-"acceleration": -0.12961290268400236,
-"curvature": 0.014710749551259814,
+"acceleration": -0.11892410309659775,
+"curvature": 0.015315802615618561,
 "pose": {
 "rotation": {
-"radians": 3.0215110358301707
+"radians": 3.062404193296307
 },
 "translation": {
-"x": 3.845679437054205,
-"y": 1.3633861785376213
+"x": 3.6073213869146996,
+"y": 1.231731812525541
 }
 },
-"time": 1.3762630989754405,
-"velocity": -3.2830961183376477
+"time": 1.3763430924700972,
+"velocity": -3.282404568831709
 },
 {
-"acceleration": -0.12528656498327115,
-"curvature": 0.011783061787390045,
+"acceleration": -0.11424418365455255,
+"curvature": 0.012548386493070503,
 "pose": {
 "rotation": {
-"radians": 3.0203871110082585
+"radians": 3.061187317058764
 },
 "translation": {
-"x": 3.929969539642334,
-"y": 1.353165721893311
+"x": 3.694453474485199,
+"y": 1.224762379516033
 }
 },
-"time": 1.4021119226706218,
-"velocity": -3.286446459407747
+"time": 1.4029602403775863,
+"velocity": -3.2855699892735966
 },
 {
-"acceleration": -0.12194330529974043,
-"curvature": 0.008946223424543565,
+"acceleration": -0.11048605733335295,
+"curvature": 0.009880428817381067,
 "pose": {
 "rotation": {
-"radians": 3.019502951089116
+"radians": 3.0602015548166714
 },
 "translation": {
-"x": 4.0147090599016515,
-"y": 1.342804474188597
+"x": 3.7821361351013176,
+"y": 1.2176517677307137
 }
 },
-"time": 1.4280756456034778,
-"velocity": -3.289699365068182
+"time": 1.4297225961396265,
+"velocity": -3.2886274327603036
 },
 {
-"acceleration": -0.11953382815835448,
-"curvature": 0.006180637718211761,
+"acceleration": -0.10759204809869179,
+"curvature": 0.007293153758249844,
 "pose": {
 "rotation": {
-"radians": 3.0188548393899226
+"radians": 3.0594426360711755
 },
 "translation": {
-"x": 4.099826323706656,
-"y": 1.3323308826424185
+"x": 3.870295246702735,
+"y": 1.2104251454229242
 }
 },
-"time": 1.4541320780605604,
-"velocity": -3.292876772566318
+"time": 1.4566076354705129,
+"velocity": -3.2915978547572253
 },
 {
-"acceleration": -0.11802375084031927,
-"curvature": 0.003467590693522533,
+"acceleration": -0.10551849355604949,
+"curvature": 0.004768805564671555,
 "pose": {
 "rotation": {
-"radians": 3.0184399892372413
+"radians": 3.058907188771421
 },
 "translation": {
-"x": 4.185248962700716,
-"y": 1.3217736504710054
+"x": 3.9588558440096673,
+"y": 1.2031079429201785
 }
 },
-"time": 1.4802586994152847,
-"velocity": -3.295999787633692
+"time": 1.4835924619717995,
+"velocity": -3.2945012075080866
 },
 {
-"acceleration": 0.04770251464301885,
-"curvature": 0.0007890133182472645,
+"acceleration": -0.09073567863406042,
+"curvature": 0.0022904196491998503,
 "pose": {
 "rotation": {
-"radians": 3.01825652416491
+"radians": 3.058592714304523
 },
 "translation": {
-"x": 4.270904023051262,
-"y": 1.311161693930626
+"x": 4.047742232307209,
+"y": 1.1957258142769582
 }
 },
-"time": 1.5064326987575296,
-"velocity": -3.299088941210556
+"time": 1.5106538490957397,
+"velocity": -3.297356684310942
 },
 {
-"acceleration": 0.11684655617480712,
-"curvature": -0.0018727452082959479,
+"acceleration": 0.10342381844097344,
+"curvature": -0.00015839218395605906,
 "pose": {
 "rotation": {
-"radians": 3.018303465163963
+"radians": 3.0584975692389254
 },
 "translation": {
-"x": 4.3567180742049825,
-"y": 1.300524099360337
+"x": 4.136878101229668,
+"y": 1.1883045989274983
 }
 },
-"time": 1.5326481947087496,
-"velocity": -3.297838396131069
+"time": 1.5377697851301964,
+"velocity": -3.299817067168826
 },
 {
-"acceleration": 0.11730261288895825,
-"curvature": -0.004535137272412562,
+"acceleration": 0.1031483620309911,
+"curvature": -0.0025936463861710004,
 "pose": {
 "rotation": {
-"radians": 3.0185807248991785
+"radians": 3.0586209536647355
 },
 "translation": {
-"x": 4.442617317643016,
-"y": 1.2898900802247226
+"x": 4.226186638544895,
+"y": 1.1808702833385913
 }
 },
-"time": 1.5589063794416522,
-"velocity": -3.2947702176736273
+"time": 1.564939656279647,
+"velocity": -3.2970070553480006
 },
 {
-"acceleration": 0.11863055671602131,
-"curvature": -0.007215633869697045,
+"acceleration": 0.10363331297308594,
+"curvature": -0.00503118599820125,
 "pose": {
 "rotation": {
-"radians": 3.019089108883937
+"radians": 3.058962906045821
 },
 "translation": {
-"x": 4.528527695636148,
-"y": 1.2792889341566482
+"x": 4.31559064393863,
+"y": 1.1734489626623699
 }
 },
-"time": 1.585191219235796,
-"velocity": -3.2916869372864066
+"time": 1.5921612271833712,
+"velocity": -3.294199194897371
 },
 {
-"acceleration": 0.12084728187889314,
-"curvature": -0.00993194291102344,
+"acceleration": 0.1048820590943684,
+"curvature": -0.007486876362587087,
 "pose": {
 "rotation": {
-"radians": 3.019830323684361
+"radians": 3.059524304570757
 },
 "translation": {
-"x": 4.614375,
-"y": 1.2687500000000007
+"x": 4.4050126427988285,
+"y": 1.1660668023891052
 }
 },
-"time": 1.6114794955643872,
-"velocity": -3.288568344430441
+"time": 1.619410541940743,
+"velocity": -3.291375258132818
 },
 {
-"acceleration": 0.12398396106047756,
-"curvature": -0.012702232961099133,
+"acceleration": 0.10691013542680701,
+"curvature": -0.009976801875388564,
 "pose": {
 "rotation": {
-"radians": 3.020806992306119
+"radians": 3.060306875063784
 },
 "translation": {
-"x": 4.7000849808502245,
-"y": 1.258302614852437
+"x": 4.494375,
+"y": 1.1587500000000013
 }
 },
-"time": 1.6377480862082352,
-"velocity": -3.285393856652343
+"time": 1.6466636935193064,
+"velocity": -3.2885168914784475
 },
 {
-"acceleration": 0.12808669719252339,
-"curvature": -0.015545366737788498,
+"acceleration": 0.10974550596632374,
+"curvature": -0.012517467495737199,
 "pose": {
 "rotation": {
-"radians": 3.0220226770037564
+"radians": 3.0613132055935752
 },
 "translation": {
-"x": 4.785583455357701,
-"y": 1.2479760711081331
+"x": 4.583600033687544,
+"y": 1.1515247466199803
 }
 },
-"time": 1.6639740063429775,
-"velocity": -3.2821422631915818
+"time": 1.6738968646315546,
+"velocity": -3.2856053894667356
 },
 {
-"acceleration": 0.13321753715872708,
-"curvature": -0.018481148989378533,
+"acceleration": 0.11342914575250637,
+"curvature": -0.015126008882058662,
 "pose": {
 "rotation": {
-"radians": 3.0234819098441106
+"radians": 3.062546767997805
 },
 "translation": {
-"x": 4.870796416503726,
-"y": 1.2377995735005243
+"x": 4.672610129062085,
+"y": 1.144417188670487
 }
 },
-"time": 1.6901344505660565,
-"velocity": -3.2787914582939583
+"time": 1.7010863694332143,
+"velocity": -3.2826214635053037
 },
 {
-"acceleration": 0.1394558736203519,
-"curvature": -0.02153059377754852,
+"acceleration": 0.11801593725021561,
+"curvature": -0.017820415272943715,
 "pose": {
 "rotation": {
-"radians": 3.025190231453577
+"radians": 3.064011946626289
 },
 "translation": {
-"x": 4.955650141835212,
-"y": 1.227802196145058
+"x": 4.761327852163812,
+"y": 1.1374533895222712
 }
 },
-"time": 1.7162068359387304,
-"velocity": -3.2753181593267575
+"time": 1.7282086961010206,
+"velocity": -3.279545001160554
 },
 {
-"acceleration": 0.146900272572347,
-"curvature": -0.024716216740290013,
+"acceleration": 0.12357590459118462,
+"curvature": -0.020619769587503386,
 "pose": {
 "rotation": {
-"radians": 3.027154238485022
+"radians": 3.0657140746960168
 },
 "translation": {
-"x": 5.040071302219876,
-"y": 1.2180128395819345
+"x": 4.849676063656807,
+"y": 1.130659291148187
 }
 },
-"time": 1.742168846131767,
-"velocity": -3.271697604514347
+"time": 1.7552405503591988,
+"velocity": -3.276354811544664
 },
 {
-"acceleration": 0.1556707763566522,
-"curvature": -0.02806235860978595,
+"acceleration": 0.13019581927190793,
+"curvature": -0.023544510696862374,
 "pose": {
 "rotation": {
-"radians": 3.029381640455622
+"radians": 3.0676594787489786
 },
 "translation": {
-"x": 5.1239870706014345,
-"y": 1.2084601878188552
+"x": 4.9375780326133825,
+"y": 1.1240606757759828
 }
 },
-"time": 1.7679984767783345,
-"velocity": -3.2679032247319233
+"time": 1.782158900036229,
+"velocity": -3.273028352133223
 },
 {
-"acceleration": 0.16591174491305632,
-"curvature": -0.03159554713838843,
+"acceleration": 0.13798122166581464,
+"curvature": -0.026616723431076975,
 "pose": {
 "rotation": {
-"radians": 3.031881326733347
+"radians": 3.0698555318097545
 },
 "translation": {
-"x": 5.207325230754795,
-"y": 1.199172665373772
+"x": 5.0249575502984225,
+"y": 1.1176831275410963
 }
 },
-"time": 1.7936740821541515,
-"velocity": -3.263906283309643
+"time": 1.808941020747052,
+"velocity": -3.2695414319854383
 },
 {
-"acceleration": 0.17779531217865624,
-"curvature": -0.035344905669777604,
+"acceleration": 0.1470589144273982,
+"curvature": -0.029860462654203716,
 "pose": {
 "rotation": {
-"radians": 3.034663444589061
+"radians": 3.072310715955775
 },
 "translation": {
-"x": 5.290014286041259,
-"y": 1.190178394317627
+"x": 5.111739043953712,
+"y": 1.1115519941394458
 }
 },
-"time": 1.8191744233234357,
-"velocity": -3.2596754772103687
+"time": 1.8355645428112144,
+"velocity": -3.2658678858859784
 },
 {
-"acceleration": 0.19152554977361444,
-"curvature": -0.039342617916622476,
+"acceleration": 0.15757999670612774,
+"curvature": -0.0333021186892183,
 "pose": {
 "rotation": {
-"radians": 3.0377394893850016
+"radians": 3.075034695140714
 },
 "translation": {
-"x": 5.371983568163706,
-"y": 1.181505151317106
+"x": 5.1978476905822735,
+"y": 1.1056923484802255
 }
 },
-"time": 1.8444787179098574,
-"velocity": -3.255176492254915
+"time": 1.8620074995342637,
+"velocity": -3.261979213376036
 },
 {
-"acceleration": 0.20734344624609133,
-"curvature": -0.0436244601118725,
+"acceleration": 0.169723522159238,
+"curvature": -0.03697083253402264,
 "pose": {
 "rotation": {
-"radians": 3.041122408140598
+"radians": 3.078038399252155
 },
 "translation": {
-"x": 5.453163345921784,
-"y": 1.1731803246773786
+"x": 5.283209530732709,
+"y": 1.1001289503386946
 }
 },
-"time": 1.8695666916748723,
-"velocity": -3.2503715042868646
+"time": 1.8882483769987564,
+"velocity": -3.2578441759916155
 },
 {
-"acceleration": 0.22553282672435188,
-"curvature": -0.04823041363685385,
+"acceleration": 0.18370087899482282,
+"curvature": -0.04089897071598679,
 "pose": {
 "rotation": {
-"radians": 3.0448267179049413
+"radians": 3.081334120540313
 },
 "translation": {
-"x": 5.53348493396712,
-"y": 1.1652308713848472
+"x": 5.367751582283525,
+"y": 1.094886208008976
 }
 },
-"time": 1.8944186321117567,
-"velocity": -3.2452186173107784
+"time": 1.9142661655323507,
+"velocity": -3.2534283452828996
 },
 {
-"acceleration": 0.2464273538619472,
-"curvature": -0.05320537355213269,
+"acceleration": 0.19976100644704156,
+"curvature": -0.045122671327978564,
 "pose": {
 "rotation": {
-"radians": 3.0488686405733585
+"radians": 3.08493562372673
 },
 "translation": {
-"x": 5.612880801558494,
-"y": 1.1576832741498952
+"x": 5.451401954227476,
+"y": 1.0899881399568407
 }
 },
-"time": 1.9190154442925342,
-"velocity": -3.2396712287312397
+"time": 1.9400404130442463,
+"velocity": -3.2486935933595342
 },
 {
-"acceleration": 0.2704187648010441,
-"curvature": -0.05859997123095306,
+"acceleration": 0.2181965785743977,
+"curvature": -0.04968247482350778,
 "pose": {
 "rotation": {
-"radians": 3.0532662560145027
+"radians": 3.0888582712921098
 },
 "translation": {
-"x": 5.691284681317047,
-"y": 1.1505634984496282
+"x": 5.534089960455892,
+"y": 1.085458336472512
 }
 },
-"time": 1.9433387092373844,
-"velocity": -3.2336773109135972
+"time": 1.9655512804476478,
+"velocity": -3.2435975168116937
 },
 {
-"acceleration": 0.29796650773666417,
-"curvature": -0.06447153259115822,
+"acceleration": 0.2393513019491277,
+"curvature": -0.05462405557944993,
 "pose": {
 "rotation": {
-"radians": 3.058039675624797
+"radians": 3.0931191656518964
 },
 "translation": {
-"x": 5.768631677981466,
-"y": 1.1438969495706264
+"x": 5.61574623354303,
+"y": 1.0813199213234483
 }
 },
-"time": 1.967370745111734,
-"velocity": -3.2271785974568012
+"time": 1.9907795994155109,
+"velocity": -3.2380927839297224
 },
 {
-"acceleration": 0.3296089411737697,
-"curvature": -0.07088519731562593,
+"acceleration": 0.2636284867263124,
+"curvature": -0.05999907312570466,
 "pose": {
 "rotation": {
-"radians": 3.063211238698045
+"radians": 3.0977373101575356
 },
 "translation": {
-"x": 5.844858377163181,
-"y": 1.1377084296516848
+"x": 5.696302838530389,
+"y": 1.0775955134071413
 }
 },
-"time": 1.9910946715956608,
-"velocity": -3.220109661932584
+"time": 2.0157069327494606,
+"velocity": -3.2321263942421217
 },
 {
-"acceleration": 0.36597624238594717,
-"curvature": -0.07791522901882693,
+"acceleration": 0.29150105757142175,
+"curvature": -0.06586616536107635,
 "pose": {
 "rotation": {
-"radians": 3.068805734287149
+"radians": 3.1027337911101758
 },
 "translation": {
-"x": 5.919902954101563,
-"y": 1.1320220947265627
+"x": 5.775693386711062,
+"y": 1.0743071884039104
 }
 },
-"time": 2.0144944778129794,
-"velocity": -3.2123968765816224
+"time": 2.0403156376778897,
+"velocity": -3.2256388386015455
 },
 {
-"acceleration": 0.40780512880693387,
-"curvature": -0.08564655162448165,
+"acceleration": 0.32352316701733913,
+"curvature": -0.07229211009442019,
 "pose": {
 "rotation": {
-"radians": 3.0748506515366456
+"radians": 3.1081319832399914
 },
 "translation": {
-"x": 5.9937052824191035,
-"y": 1.1268614117667317
+"x": 5.853853149414062,
+"y": 1.071476440429688
 }
 },
-"time": 2.0375550942533294,
-"velocity": -3.2039572388296795
+"time": 2.0645889324385314,
+"velocity": -3.2185631475080756
 },
 {
-"acceleration": 0.45595541221242947,
-"curvature": -0.09417655330847025,
+"acceleration": 0.3603435513938128,
+"curvature": -0.07935318593560539,
 "pose": {
 "rotation": {
-"radians": 3.081376461769633
+"radians": 3.113957781384233
 },
 "translation": {
-"x": 6.066207042876631,
-"y": 1.1222491157241166
+"x": 5.930719171788658,
+"y": 1.069124143688824
 }
 },
-"time": 2.0602624691687645,
-"velocity": -3.194697054877423
+"time": 2.088510966543132,
+"velocity": -3.2108238152730584
 },
 {
-"acceleration": 0.5114282590837173,
-"curvature": -0.10361720622475955,
+"acceleration": 0.40272071716172203,
+"curvature": -0.0871367689618485,
 "pose": {
 "rotation": {
-"radians": 3.0884169359047617
+"radians": 3.120239861382658
 },
 "translation": {
-"x": 6.137351832128479,
-"y": 1.118207166573848
+"x": 6.0062303865887205,
+"y": 1.067270514126868
 }
 },
-"time": 2.0826036499753213,
-"velocity": -3.184510472573457
+"time": 2.1120668951665813,
+"velocity": -3.2023355882965054
 },
 {
-"acceleration": 1.2899740985830486,
-"curvature": -0.11409755777291637,
+"acceleration": 0.4515399465872042,
+"curvature": -0.09574320771599205,
 "pose": {
 "rotation": {
-"radians": 3.096009501033312
+"radians": 3.127009973487574
 },
 "translation": {
-"x": 6.207085271477698,
-"y": 1.114756706357003
+"x": 6.0803277279570445,
+"y": 1.065935071083369
 }
 },
-"time": 2.1045668702374396,
-"velocity": -3.17327786107093
+"time": 2.1352429581487327,
+"velocity": -3.193002107591348
 },
 {
-"acceleration": 3.099999999999996,
-"curvature": -0.1257666571328097,
+"acceleration": 0.5078319422518147,
+"curvature": -0.10528802590258383,
 "pose": {
 "rotation": {
-"radians": 3.1041956401649946
+"radians": 3.1343032718366834
 },
 "translation": {
-"x": 6.275355115631246,
-"y": 1.1119180162233535
+"x": 6.152954245209692,
+"y": 1.0651365989446653
 }
 },
-"time": 2.1261945166770753,
-"velocity": -3.145378757350488
+"time": 2.1580265641422853,
+"velocity": -3.1827143993579554
 },
 {
-"acceleration": 3.0999999999999837,
-"curvature": -0.13879698868120785,
+"acceleration": 1.6916972207911691,
+"curvature": -0.11590450946828211,
 "pose": {
 "rotation": {
-"radians": 3.113021339197105
+"radians": -3.1410266234502946
 },
 "translation": {
-"x": 6.342111361455171,
-"y": 1.1097104734741157
+"x": 6.224055216620327,
+"y": 1.0648931087966798
 }
 },
-"time": 2.147656703205107,
-"velocity": -3.078845979113589
+"time": 2.180406380481228,
+"velocity": -3.1713492137593113
 },
 {
-"acceleration": 3.1,
-"curvature": -0.15338849080645683,
+"acceleration": 3.1000000000000068,
+"curvature": -0.12774674222621782,
 "pose": {
 "rotation": {
-"radians": 3.122537584991606
+"radians": -3.132565984646297
 },
 "translation": {
-"x": 6.4073063567298245,
-"y": 1.1081525086046904
+"x": 6.293578263204543,
+"y": 1.0652218000777074
 }
 },
-"time": 2.1690687006026663,
-"velocity": -3.0124687871811555
+"time": 2.202458558854016,
+"velocity": -3.134043604893675
 },
 {
-"acceleration": 3.1,
-"curvature": -0.1697732430051363,
+"acceleration": 3.100000000000008,
+"curvature": -0.14099316114088864,
 "pose": {
 "rotation": {
-"radians": 3.132800917937554
+"radians": -3.1234523592165715
 },
 "translation": {
-"x": 6.470894908905029,
-"y": 1.1072615623474125
+"x": 6.361473462504218,
+"y": 1.0661390222312193
 }
 },
-"time": 2.1904136466939197,
-"velocity": -2.9462994542982703
+"time": 2.2243615708152964,
+"velocity": -3.0661442678137054
 },
 {
-"acceleration": 3.1,
-"curvature": -0.20904496945610804,
+"acceleration": 3.1000000000000028,
+"curvature": -0.15585070767331272,
 "pose": {
 "rotation": {
-"radians": -3.1273588188285504
+"radians": -3.113632830072911
 },
 "translation": {
-"x": 6.593084864635021,
-"y": 1.1075452820397915
+"x": 6.427693462371828,
+"y": 1.0676602363586447
 }
 },
-"time": 2.2328327299998367,
-"velocity": -2.814800296049927
+"time": 2.24620564200541,
+"velocity": -2.9984276471243536
 },
 {
-"acceleration": 3.1000000000000023,
-"curvature": -0.2593391548463894,
+"acceleration": 3.1000000000000156,
+"curvature": -0.17255965328057685,
 "pose": {
 "rotation": {
-"radians": -3.1004992926227373
+"radians": -3.103048758376211
 },
 "translation": {
-"x": 6.7083730143308635,
-"y": 1.110679730772973
+"x": 6.492193594754788,
+"y": 1.069799976872166
 }
 },
-"time": 2.274774376542098,
-"velocity": -2.684781191768918
+"time": 2.267973745289946,
+"velocity": -2.930946526942291
 },
 {
-"acceleration": 3.1000000000000028,
-"curvature": -0.3243323376775788,
+"acceleration": 3.099999999999999,
+"curvature": -0.19139917220029992,
 "pose": {
 "rotation": {
-"radians": -3.0690844228082543
+"radians": -3.091635164609091
 },
 "translation": {
-"x": 6.816497458722445,
-"y": 1.1167644212953753
+"x": 6.55493198947981,
+"y": 1.0725718131475164
 }
 },
-"time": 2.316097012204804,
-"velocity": -2.556681021214529
+"time": 2.289648580707943,
+"velocity": -2.8637545371465
 },
 {
-"acceleration": 3.099999999999999,
-"curvature": -0.40893401206940694,
+"acceleration": 3.099999999999996,
+"curvature": -0.23682022696385555,
 "pose": {
 "rotation": {
-"radians": -3.032233935643764
+"radians": -3.06602370643425
 },
 "translation": {
-"x": 6.917246093750002,
-"y": 1.1258789062500005
+"x": 6.674970757365227,
+"y": 1.0800609952211389
 }
 },
-"time": 2.3566615460263716,
-"velocity": -2.4309309663676695
+"time": 2.3326473610499985,
+"velocity": -2.730458318086128
 },
 {
-"acceleration": 3.1,
-"curvature": -0.5195555040508509,
+"acceleration": 3.0999999999999983,
+"curvature": -0.295387815569435,
 "pose": {
 "rotation": {
-"radians": -2.9888858285247495
+"radians": -3.036125126815574
 },
 "translation": {
-"x": 7.010460090730337,
-"y": 1.1380814035423112
+"x": 6.787535086032003,
+"y": 1.0902155796624742
 }
 },
-"time": 2.3963374067946996,
-"velocity": -2.307935797985853
+"time": 2.3750614147294327,
+"velocity": -2.5989747516798825
 },
 {
-"acceleration": 3.1000000000000023,
-"curvature": -0.6642000663918197,
+"acceleration": 3.0999999999999965,
+"curvature": -0.3714831636069596,
 "pose": {
 "rotation": {
-"radians": -2.9377860698216067
+"radians": -3.0011178082090844
 },
 "translation": {
-"x": 7.096037376523018,
-"y": 1.153407421708108
+"x": 6.892402343749998,
+"y": 1.1031054687500017
 }
 },
-"time": 2.4350114130716385,
-"velocity": -2.1880463785273427
+"time": 2.4167510931757787,
+"velocity": -2.4697367484962096
 },
 {
-"acceleration": 3.099999999999999,
-"curvature": -0.8519896041394569,
+"acceleration": 3.100000000000001,
+"curvature": -0.4708607696126368,
 "pose": {
 "rotation": {
-"radians": -2.8775107962228716
+"radians": -2.9600090182047873
 },
 "translation": {
-"x": 7.173936113696541,
-"y": 1.1718683852814147
+"x": 6.989405588153749,
+"y": 1.118781442847105
 }
 },
-"time": 2.4726003785737096,
-"velocity": -2.071520585470922
+"time": 2.4575838058413884,
+"velocity": -2.3431553392328204
+},
+{
+"acceleration": 3.100000000000001,
+"curvature": -0.6007955048777841,
+"pose": {
+"rotation": {
+"radians": -2.911621598753908
+},
+"translation": {
+"x": 7.078437207341194,
+"y": 1.1372739332914374
+}
+},
+"time": 2.497442160927556,
+"velocity": -2.2195944384657005
 },
 {
 "acceleration": 3.100000000000002,
-"curvature": -1.0913695259915541,
+"curvature": -0.7698109959861857,
 "pose": {
 "rotation": {
-"radians": -2.8065568874003324
+"radians": -2.8546067366782104
 },
 "translation": {
-"x": 7.244178180694583,
-"y": 1.1934502601623551
+"x": 7.15945256097242,
+"y": 1.158591795284302
 }
 },
-"time": 2.5090683677162753,
-"velocity": -1.958469819128968
+"time": 2.5362356380987636,
+"velocity": -2.0993346592349567
 },
 {
 "acceleration": 3.1000000000000014,
-"curvature": -1.3858144414647986,
+"curvature": -0.9863744188637547,
 "pose": {
 "rotation": {
-"radians": -2.723556344353506
+"radians": -2.7875132398666844
 },
 "translation": {
-"x": 7.306852652002128,
-"y": 1.2181121789850318
+"x": 7.232473621368401,
+"y": 1.1827210807800315
 }
 },
-"time": 2.544449232446537,
-"velocity": -1.8487891384651567
+"time": 2.573916749757263,
+"velocity": -1.9825232130936088
 },
 {
-"acceleration": 3.1000000000000014,
-"curvature": -1.5515574257460238,
+"acceleration": 3.1,
+"curvature": -1.2555349452361042,
 "pose": {
 "rotation": {
-"radians": -2.67724248627088
+"radians": -2.7089621115884808
 },
 "translation": {
-"x": 7.335399105413238,
-"y": 1.231577708313126
+"x": 7.2975926146097425,
+"y": 1.2096238113753515
 }
 },
-"time": 2.5617730792992215,
-"velocity": -1.7950852132218345
+"time": 2.610502482229281,
+"velocity": -1.8691074424303538
 },
 {
-"acceleration": 3.100000000000003,
-"curvature": -1.7260178757562485,
+"acceleration": 3.0999999999999917,
+"curvature": -1.408946850119294,
 "pose": {
 "rotation": {
-"radians": -2.6276786432456145
+"radians": -2.6650585249340835
 },
 "translation": {
-"x": 7.362119278311731,
-"y": 1.2457850664854062
+"x": 7.32723776276165,
+"y": 1.2240964775491765
 }
 },
-"time": 2.5788844114957334,
-"velocity": -1.7420400834126477
+"time": 2.628418415534889,
+"velocity": -1.8135680491829678
 },
 {
-"acceleration": 3.0999999999999903,
-"curvature": -1.9043547509535583,
+"acceleration": 3.1000000000000028,
+"curvature": -1.5723990457182746,
 "pose": {
 "rotation": {
-"radians": -2.5749462716461715
+"radians": -2.6179881754994527
 },
 "translation": {
-"x": 7.387044753656376,
-"y": 1.2607209860841992
+"x": 7.354975661635391,
+"y": 1.239236751198769
 }
 },
-"time": 2.5958199873896013,
-"velocity": -1.689539798141658
+"time": 2.6461106696850503,
+"velocity": -1.7587220613174683
 },
 {
-"acceleration": 3.0999999999999983,
-"curvature": -2.079826534125965,
+"acceleration": 3.099999999999995,
+"curvature": -1.7421714910555643,
 "pose": {
 "rotation": {
-"radians": -2.5192446695665165
+"radians": -2.5677853315299033
 },
 "translation": {
-"x": 7.410211966689673,
-"y": 1.2763702648691835
+"x": 7.380839219787736,
+"y": 1.2550325800379518
 }
 },
-"time": 2.612626500857439,
-"velocity": -1.637439606391361
+"time": 2.6636122490472105,
+"velocity": -1.704467165294771
 },
 {
-"acceleration": 3.0999999999999965,
-"curvature": -2.2439264286325784,
+"acceleration": 3.099999999999995,
+"curvature": -1.9127827178559593,
 "pose": {
 "rotation": {
-"radians": -2.4609107666809664
+"radians": -2.514587983065682
 },
 "translation": {
-"x": 7.431662313693085,
-"y": 1.2927157228201418
+"x": 7.404866419341415,
+"y": 1.2714701797999464
 }
 },
-"time": 2.6293614263750347,
-"velocity": -1.585561337286815
+"time": 2.6809658536677,
+"velocity": -1.6506709909712531
 },
 {
-"acceleration": 3.0999999999999894,
-"curvature": -2.3869073137216117,
+"acceleration": 3.0999999999999934,
+"curvature": -2.0769586744544335,
 "pose": {
 "rotation": {
-"radians": -2.4004307838491252
+"radians": -2.4586593378274806
 },
 "translation": {
-"x": 7.451442260742191,
-"y": 1.3097381591796884
+"x": 7.427100429769483,
+"y": 1.2885339958901767
 }
 },
-"time": 2.646093743863084,
-"velocity": -1.5336911530738613
+"time": 2.69822484881919,
+"velocity": -1.5971681060016354
 },
 {
-"acceleration": 3.0999999999999965,
-"curvature": -2.498728420358053,
+"acceleration": 3.100000000000005,
+"curvature": -2.2259165683839766,
 "pose": {
 "rotation": {
-"radians": -2.3384391697455427
+"radians": -2.4004038479297507
 },
 "translation": {
-"x": 7.4696034524618895,
-"y": 1.3274163094960416
+"x": 7.4475897216796785,
+"y": 1.3062066650390651
 }
 },
-"time": 2.6629045458005467,
-"velocity": -1.4815776670677279
+"time": 2.7154541538885604,
+"velocity": -1.5437572602865863
 },
 {
-"acceleration": 3.1000000000000036,
-"curvature": -2.5703199925554827,
+"acceleration": 3.100000000000002,
+"curvature": -2.3500400160073234,
 "pose": {
 "rotation": {
-"radians": -2.275701514588112
+"radians": -2.3403733048891473
 },
 "translation": {
-"x": 7.486202820781617,
-"y": 1.3457268026657419
+"x": 7.4663881805987256,
+"y": 1.324468976954817
 }
 },
-"time": 2.6798875850889488,
-"velocity": -1.428930245273682
+"time": 2.732731044595673,
+"velocity": -1.4901988990945363
 },
 {
-"acceleration": 3.0999999999999943,
-"curvature": -2.594910602193435,
+"acceleration": 3.100000000000002,
+"curvature": -2.4399234600320265,
 "pose": {
 "rotation": {
-"radians": -2.2130809613872398
+"radians": -2.279259026753187
 },
 "translation": {
-"x": 7.501302693690523,
-"y": 1.3646441179764244
+"x": 7.483555220756676,
+"y": 1.3432998359762163
 }
 },
-"time": 2.6971499062690376,
-"velocity": -1.3754170496154068
+"time": 2.750145909885525,
+"velocity": -1.4362128166959962
+},
+{
+"acceleration": 3.100000000000002,
+"curvature": -2.4876244504876728,
+"pose": {
+"rotation": {
+"radians": -2.2178680800515775
+},
+"translation": {
+"x": 7.4991558988712494,
+"y": 1.3626762227254243
+}
+},
+"time": 2.7678030815598333,
+"velocity": -1.3814755845056401
 },
 {
 "acceleration": 3.0999999999999983,
-"curvature": -2.5690710889437307,
+"curvature": -2.4878377542999446,
 "pose": {
 "rotation": {
-"radians": -2.1514914284555773
+"radians": -2.1570846830531614
 },
 "translation": {
-"x": 7.514970903992651,
-"y": 1.384140542149546
+"x": 7.513261027932165,
+"y": 1.3825731557607668
 }
 },
-"time": 2.7148128177324002,
-"velocity": -1.320662024078983
+"time": 2.785821966628121,
+"velocity": -1.325617040793949
 },
 {
-"acceleration": 3.1,
-"curvature": -2.4931679319071747,
+"acceleration": 3.099999999999995,
+"curvature": -2.438668492441403,
 "pose": {
 "rotation": {
-"radians": -2.0918444919192845
+"radians": -2.097821586389424
 },
 "translation": {
-"x": 7.52728089806217,
-"y": 1.4041861263831388
+"x": 7.52594729098549,
+"y": 1.4029636532295289
 }
 },
-"time": 2.7330136151372173,
-"velocity": -1.2642395521240506
+"time": 2.8043388683885717,
+"velocity": -1.2682146453365517
 },
 {
-"acceleration": 3.1000000000000036,
-"curvature": -2.3710926583958853,
+"acceleration": 3.1000000000000028,
+"curvature": -2.3417751345338607,
 "pose": {
 "rotation": {
-"radians": -2.03499865053651
+"radians": -2.04096901638014
 },
 "translation": {
-"x": 7.538311844598507,
-"y": 1.4247486433945609
+"x": 7.537297354917966,
+"y": 1.4238186945207438
 }
 },
-"time": 2.751908678329609,
-"velocity": -1.2056648562276355
+"time": 2.823510104542644,
+"velocity": -1.2087838132589275
 },
 {
 "acceleration": 3.100000000000006,
-"curvature": -2.209366599779695,
+"curvature": -2.2018486322760125,
 "pose": {
 "rotation": {
-"radians": -1.9817190947775274
+"radians": -1.987349545508084
 },
 "translation": {
-"x": 7.548148743381606,
-"y": 1.4457935444632328
+"x": 7.547399984241336,
+"y": 1.4451071819179955
 }
 },
-"time": 2.771678895331034,
-"velocity": -1.1443771835232186
+"time": 2.8435173762455226,
+"velocity": -1.146761270980004
 },
 {
-"acceleration": 3.099999999999997,
-"curvature": -2.015899274235798,
+"acceleration": 3.0999999999999988,
+"curvature": -2.025599885881066,
 "pose": {
 "rotation": {
-"radians": -1.9326532965904353
+"radians": -1.9376856854427909
 },
 "translation": {
-"x": 7.5568825340271,
-"y": 1.4672839164733906
+"x": 7.556350154876709,
+"y": 1.4667959022521995
 }
 },
-"time": 2.792538958556939,
-"velocity": -1.0797109875229118
+"time": 2.864576947720082,
+"velocity": -1.0814765994088702
 },
 {
-"acceleration": 3.0999999999999988,
-"curvature": -1.5650029358364153,
+"acceleration": 3.0999999999999996,
+"curvature": -1.5939014131451663,
 "pose": {
 "rotation": {
-"radians": -1.8491370300895777
+"radians": -1.8525339889788117
 },
 "translation": {
-"x": 7.571434901077307,
-"y": 1.5114413411356535
+"x": 7.5712047635205035,
+"y": 1.5112303817085948
 }
 },
-"time": 2.8386527739340606,
-"velocity": -0.9367581598538358
+"time": 2.910985778419941,
+"velocity": -0.9376092242393069
 },
 {
-"acceleration": 3.1000000000000005,
-"curvature": -1.0683838051263088,
+"acceleration": 3.1,
+"curvature": -1.0983938733093968,
 "pose": {
 "rotation": {
-"radians": -1.787356415639369
+"radians": -1.78906088746082
 },
 "translation": {
-"x": 7.582819392085074,
-"y": 1.5568766921758668
+"x": 7.582749540209759,
+"y": 1.55681266129017
 }
 },
-"time": 2.893661867009521,
-"velocity": -0.7662299713199083
+"time": 2.9661706866024633,
+"velocity": -0.7665360088734879
 },
 {
-"acceleration": 3.1000000000000005,
-"curvature": -1.3133514210160322e-14,
+"acceleration": 3.1,
+"curvature": -2.3741352610674924e-14,
 "pose": {
 "rotation": {
-"radians": -1.7359450042095137
+"radians": -1.7359450042095053
 },
 "translation": {
-"x": 7.599999999999998,
-"y": 1.650000000000003
+"x": 7.5999999999999925,
+"y": 1.6500000000000021
 }
 },
-"time": 3.140832825499814,
+"time": 3.2134403668842335,
 "velocity": -0.0
 }
 ]
\ No newline at end of file
diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java
index 4d0a4f5b..7e903906 100644
--- a/src/main/java/com/stuypulse/robot/RobotContainer.java
+++ b/src/main/java/com/stuypulse/robot/RobotContainer.java
@@ -11,8 +11,16 @@
 import com.stuypulse.stuylib.network.SmartNumber;
 import com.stuypulse.robot.commands.BetterShootAnywhere;
 import com.stuypulse.robot.commands.ShootAnywhere;
+import com.stuypulse.robot.commands.SimpleAlignShoot;
 import com.stuypulse.robot.commands.TestAlign;
 import com.stuypulse.robot.commands.auton.*;
+import com.stuypulse.robot.commands.auton.battlecry.BlueFiveBallAuton;
+import com.stuypulse.robot.commands.auton.battlecry.ThreeBallMysteryAuton;
+import com.stuypulse.robot.commands.auton.chezy.ChezyPartnerBallAuton;
+import com.stuypulse.robot.commands.auton.chezy.SixBallAuton;
+import com.stuypulse.robot.commands.auton.chezy.ThreeBallAuton;
+import com.stuypulse.robot.commands.auton.chezy.ThreeBallMeanerAuton;
+import com.stuypulse.robot.commands.auton.chezy.ThreeBallOneMeanAuton;
 import com.stuypulse.robot.commands.climber.*;
 import com.stuypulse.robot.commands.conveyor.*;
 import com.stuypulse.robot.commands.drivetrain.*;
@@ -117,6 +125,8 @@ private void configureButtonBindings() {
                 .whileHeld(new ShooterPadShot(shooter))
                 .whileHeld(new DrivetrainPadAlign(drivetrain, camera).thenShoot(conveyor));
 
+        driver.getRightButton().whileHeld(new SimpleAlignShoot(this));
+
         driver.getTopButton().whileHeld(new DrivetrainAlign(drivetrain, camera).perpetually());
         // driver.getTopButton().whileHeld(new TestAlign(this).thenShoot(conveyor));
 
@@ -127,7 +137,8 @@ private void configureButtonBindings() {
         operator.getRightTriggerButton()
                 .whenPressed(new IntakeExtend(intake))
                 .whileHeld(new IntakeAcquire(intake))
-                .whenReleased(new IntakeRetract(intake));
+                .whenReleased(
+                        new IntakeRetract(intake));
 
         operator.getRightBumper()
                 .whenPressed(new IntakeExtend(intake))
@@ -171,6 +182,11 @@ public void configureAutons() {
         autonChooser.addOption("Four Ball", new FourBallAuton(this));
         autonChooser.addOption("Blue Balls", new BlueFiveBallAuton(this));
         autonChooser.addOption("Mystery Ball", new ThreeBallMysteryAuton(this));
+        autonChooser.addOption("CH/6 Ball AUTON", new SixBallAuton(this));
+        autonChooser.addOption("CH/3 Ball AUTON", new ThreeBallAuton(this));
+        autonChooser.addOption("CH/Partner AUTON", new ChezyPartnerBallAuton(this));
+        autonChooser.addOption("CH/3 Ball 1 Mean AUTON", new ThreeBallOneMeanAuton(this));
+        autonChooser.addOption("CH/3 Ball 2 Mean AUTON", new ThreeBallMeanerAuton(this));
 
         SmartDashboard.putData("Autonomous", autonChooser);
     }
diff --git a/src/main/java/com/stuypulse/robot/commands/SimpleAlignShoot.java b/src/main/java/com/stuypulse/robot/commands/SimpleAlignShoot.java
new file mode 100644
index 00000000..7230ec81
--- /dev/null
+++ b/src/main/java/com/stuypulse/robot/commands/SimpleAlignShoot.java
@@ -0,0 +1,105 @@
+/************************ PROJECT DORCAS ************************/
+/* Copyright (c) 2022 StuyPulse Robotics. All rights reserved.  */
+/* This work is licensed under the terms of the MIT license.    */
+/****************************************************************/
+
+package com.stuypulse.robot.commands;
+
+import com.stuypulse.stuylib.control.Controller;
+import com.stuypulse.stuylib.math.SLMath;
+import com.stuypulse.stuylib.streams.IFuser;
+import com.stuypulse.stuylib.streams.booleans.BStream;
+import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC;
+import com.stuypulse.robot.RobotContainer;
+import com.stuypulse.robot.commands.conveyor.modes.ConveyorMode;
+import com.stuypulse.robot.constants.ShotMap;
+import com.stuypulse.robot.constants.Settings.Alignment;
+import com.stuypulse.robot.constants.Settings.Limelight;
+import com.stuypulse.robot.subsystems.Camera;
+import com.stuypulse.robot.subsystems.Conveyor;
+import com.stuypulse.robot.subsystems.Drivetrain;
+import com.stuypulse.robot.subsystems.Shooter;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class SimpleAlignShoot extends CommandBase {
+
+    // subsystems
+    private final Camera camera;
+    private final Conveyor conveyor;
+    private final Drivetrain drivetrain;
+    private final Shooter shooter;
+
+    // angle control
+    private final Controller angleController;
+    private final IFuser angleError;
+
+    // determine when to switch between shooting and aligning
+    private final BStream readyToShoot;
+
+    public SimpleAlignShoot(RobotContainer robot) {
+        this.camera = robot.camera;
+        this.conveyor = robot.conveyor;
+        this.drivetrain = robot.drivetrain;
+        this.shooter = robot.shooter;
+
+        // find angle error
+        angleError =
+                new IFuser(
+                        Alignment.FUSION_FILTER,
+                        () -> camera.getXAngle()
+                                .add(ShotMap.getAngle(camera.getDistance()))
+                                .toDegrees(),
+                        () -> drivetrain.getRawGyroAngle());
+        
+
+        // handle errors
+        this.angleController = Alignment.Angle.getController();
+
+        // figure out when ready to shoot
+        readyToShoot =
+                BStream.create(camera::hasAnyTarget)
+                        .and(shooter::isReady)
+                        .and(
+                                () ->
+                                        Math.abs(drivetrain.getVelocity())
+                                                < Limelight.MAX_VELOCITY.get())
+                        .and(() -> angleController.isDone(Limelight.MAX_ANGLE_ERROR.get()))
+                        .filtered(new BDebounceRC.Rising(Limelight.DEBOUNCE_TIME));
+
+        addRequirements(drivetrain);
+    }
+
+    @Override
+    public void initialize() {
+        drivetrain.setLowGear();
+        shooter.retractHood();
+
+        angleError.initialize();
+    }
+
+    private double getTurn() {
+        return angleController.update(angleError.get());
+    }
+
+    @Override
+    public void execute() {
+        drivetrain.arcadeDrive(0, getTurn());
+
+        if (readyToShoot.get()) {
+            conveyor.setMode(ConveyorMode.SEMI_AUTO);
+        } else {
+            conveyor.setMode(ConveyorMode.DEFAULT);
+        }
+    }
+
+    @Override
+    public boolean isFinished() {
+        return conveyor.isEmpty();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        conveyor.setMode(ConveyorMode.DEFAULT);
+    }
+}
diff --git a/src/main/java/com/stuypulse/robot/commands/auton/BlueFiveBallAuton.java b/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BlueFiveBallAuton.java
similarity index 98%
rename from src/main/java/com/stuypulse/robot/commands/auton/BlueFiveBallAuton.java
rename to src/main/java/com/stuypulse/robot/commands/auton/battlecry/BlueFiveBallAuton.java
index 58d7eb60..c64b7bda 100644
--- a/src/main/java/com/stuypulse/robot/commands/auton/BlueFiveBallAuton.java
+++ b/src/main/java/com/stuypulse/robot/commands/auton/battlecry/BlueFiveBallAuton.java
@@ -3,7 +3,7 @@
 /* This work is licensed under the terms of the MIT license.    */
 /****************************************************************/
 
-package com.stuypulse.robot.commands.auton;
+package com.stuypulse.robot.commands.auton.battlecry;
 
 import com.stuypulse.robot.RobotContainer;
 import com.stuypulse.robot.commands.conveyor.ConveyorShoot;
diff --git a/src/main/java/com/stuypulse/robot/commands/auton/ThreeBallMysteryAuton.java b/src/main/java/com/stuypulse/robot/commands/auton/battlecry/ThreeBallMysteryAuton.java
similarity index 98%
rename from src/main/java/com/stuypulse/robot/commands/auton/ThreeBallMysteryAuton.java
rename to src/main/java/com/stuypulse/robot/commands/auton/battlecry/ThreeBallMysteryAuton.java
index c3ddc319..1a14730a 100644
--- a/src/main/java/com/stuypulse/robot/commands/auton/ThreeBallMysteryAuton.java
+++ b/src/main/java/com/stuypulse/robot/commands/auton/battlecry/ThreeBallMysteryAuton.java
@@ -3,7 +3,7 @@
 /* This work is licensed under the terms of the MIT license.    */
 /****************************************************************/
 
-package com.stuypulse.robot.commands.auton;
+package com.stuypulse.robot.commands.auton.battlecry;
 
 import com.stuypulse.robot.RobotContainer;
 import com.stuypulse.robot.commands.conveyor.ConveyorShoot;
diff --git a/src/main/java/com/stuypulse/robot/commands/auton/chezy/ChezyPartnerBallAuton.java b/src/main/java/com/stuypulse/robot/commands/auton/chezy/ChezyPartnerBallAuton.java
new file mode 100644
index 00000000..2430f088
--- /dev/null
+++ b/src/main/java/com/stuypulse/robot/commands/auton/chezy/ChezyPartnerBallAuton.java
@@ -0,0 +1,110 @@
+/************************ PROJECT DORCAS ************************/
+/* Copyright (c) 2022 StuyPulse Robotics. All rights reserved.  */
+/* This work is licensed under the terms of the MIT license.    */
+/****************************************************************/
+
+package com.stuypulse.robot.commands.auton.chezy;
+
+import com.stuypulse.robot.RobotContainer;
+import com.stuypulse.robot.commands.conveyor.ConveyorForceEject;
+import com.stuypulse.robot.commands.conveyor.ConveyorShoot;
+import com.stuypulse.robot.commands.conveyor.ConveyorShootTop;
+import com.stuypulse.robot.commands.drivetrain.DrivetrainAlign;
+import com.stuypulse.robot.commands.drivetrain.DrivetrainRamsete;
+import com.stuypulse.robot.commands.intake.IntakeAcquireForever;
+import com.stuypulse.robot.commands.intake.IntakeDeacquire;
+import com.stuypulse.robot.commands.intake.IntakeDisableSafety;
+import com.stuypulse.robot.commands.intake.IntakeExtend;
+import com.stuypulse.robot.commands.leds.LEDSet;
+import com.stuypulse.robot.commands.shooter.ShooterFenderShot;
+import com.stuypulse.robot.commands.shooter.ShooterRingShot;
+import com.stuypulse.robot.util.LEDColor;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+
+/*-
+ * @author Vincent Wang
+ * @author Shaurya Sen
+ */
+ 
+public class ChezyPartnerBallAuton extends SequentialCommandGroup {
+    // Time it takes for the shooter to reach the target speed
+    private static final double SHOOTER_INITIALIZE_DELAY = 1.0;
+    // Time it takes for shooter to go from Fender to Ring shot speed
+    private static final double SHOOTER_TO_RING_DELAY = 0.2;
+    // Time it takes for the conveyor to give the shooter the ball
+    private static final double CONVEYOR_TO_SHOOTER = 1.0;
+    // Time we want to give the drivetrain to align
+    private static final double DRIVETRAIN_ALIGN_TIME = 3.0;
+    // Time to acquire partner auton ball
+    private static final double ACQUIRE_PARTNER_BALL_TIME = 5.0;
+    // Time for our partner to move back
+    private static final double PARTNER_MOVE_BACK_TIME = 1.0;
+
+    private static final double INTAKE_DEACQUIRE_TIME = 3.0;
+
+    private static final double RING_SHOT_DELAY = 4.0;
+
+    private static final String PARTNER_BALL_TO_RING_BALL = "ThreeBallPartnerAuton/output/acquire_first_ball.wpilib.json";
+    private static final String PARTNER_BALL_GET_OPPONENT_BALL = "ThreeBallPartnerAuton/output/opponent_ball.wpilib.json";
+    private static final String PARTNER_BALL_EJECT = "ThreeBallPartnerAuton/output/move_to_hanger.wpilib.json";
+
+    /** Creates a new FiveBallAuton. */
+    public ChezyPartnerBallAuton(RobotContainer robot) {
+
+        // Starting up subsystems
+        addCommands(
+            new LEDSet(robot.leds, LEDColor.YELLOW),
+            new IntakeExtend(robot.intake),
+            new IntakeDisableSafety(robot.intake),
+            new IntakeAcquireForever(robot.intake),
+            new ShooterFenderShot(robot.shooter),
+            new WaitCommand(SHOOTER_INITIALIZE_DELAY)
+        );
+
+        // (acquire ball from partner)
+        // shoot top ball
+        addCommands(
+            new LEDSet(robot.leds, LEDColor.GREEN),
+            new ConveyorShootTop(robot.conveyor).withTimeout(CONVEYOR_TO_SHOOTER),
+            new WaitCommand(ACQUIRE_PARTNER_BALL_TIME).withInterrupt(robot.conveyor::isFull),
+            new ConveyorShootTop(robot.conveyor).withTimeout(CONVEYOR_TO_SHOOTER),
+            new WaitCommand(PARTNER_MOVE_BACK_TIME));
+
+        // enter ring mode
+        // run path to ring shot
+        addCommands(
+            new ShooterRingShot(robot.shooter),
+            new DrivetrainRamsete(robot.drivetrain, PARTNER_BALL_TO_RING_BALL)
+                    .robotRelative(),
+            new WaitCommand(SHOOTER_TO_RING_DELAY)
+        );
+
+        
+        // align 
+        addCommands(
+            new LEDSet(robot.leds, LEDColor.GREEN.pulse()),
+            new DrivetrainAlign(robot.drivetrain, robot.camera)
+                    .withTimeout(DRIVETRAIN_ALIGN_TIME),
+            new LEDSet(robot.leds, LEDColor.RAINBOW),
+            new ConveyorShoot(robot.conveyor).withTimeout(RING_SHOT_DELAY)
+        );
+        
+        addCommands(
+            new LEDSet(robot.leds, LEDColor.RED),
+            new DrivetrainRamsete(robot.drivetrain, PARTNER_BALL_GET_OPPONENT_BALL)
+        );
+
+        addCommands(
+            new LEDSet(robot.leds, LEDColor.RAINBOW),
+            new DrivetrainRamsete(robot.drivetrain, PARTNER_BALL_EJECT),
+
+            new IntakeDeacquire(robot.intake)
+                    .alongWith(new ConveyorForceEject(robot.conveyor))
+                    .withTimeout(INTAKE_DEACQUIRE_TIME)
+        );
+
+        addCommands(new LEDSet(robot.leds, LEDColor.WHITE.pulse()));
+    }
+}
diff --git a/src/main/java/com/stuypulse/robot/commands/auton/chezy/SixBallAuton.java b/src/main/java/com/stuypulse/robot/commands/auton/chezy/SixBallAuton.java
new file mode 100644
index 00000000..3cb11a75
--- /dev/null
+++ b/src/main/java/com/stuypulse/robot/commands/auton/chezy/SixBallAuton.java
@@ -0,0 +1,123 @@
+/************************ PROJECT DORCAS ************************/
+/* Copyright (c) 2022 StuyPulse Robotics. All rights reserved.  */
+/* This work is licensed under the terms of the MIT license.    */
+/****************************************************************/
+
+package com.stuypulse.robot.commands.auton.chezy;
+
+import com.stuypulse.robot.RobotContainer;
+import com.stuypulse.robot.commands.conveyor.ConveyorShoot;
+import com.stuypulse.robot.commands.drivetrain.DrivetrainAlign;
+import com.stuypulse.robot.commands.drivetrain.DrivetrainDriveDistance;
+import com.stuypulse.robot.commands.drivetrain.DrivetrainRamsete;
+import com.stuypulse.robot.commands.intake.IntakeAcquireForever;
+import com.stuypulse.robot.commands.intake.IntakeDisableSafety;
+import com.stuypulse.robot.commands.intake.IntakeEnableSafety;
+import com.stuypulse.robot.commands.intake.IntakeExtend;
+import com.stuypulse.robot.commands.leds.LEDSet;
+import com.stuypulse.robot.commands.shooter.ShooterRingShot;
+import com.stuypulse.robot.util.LEDColor;
+
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+
+/*-
+ * @author Vincent Wang (vinowang921@gmail.com)
+ * @author Ivan Wei (ivanw8288@gmail.com)
+ * @author Ivan Chen (ivanchen07@gmail.com)
+ * @author Eric Lin (ericlin071906@gmail.com)
+ * @author Marc Jiang (mjiang05@gmail.com)
+ * @author Ian Jiang (ijiang05@gmail.com)
+ * @author Carmin Vuong (carminvuong@gmail.com)
+ * @author Samuel Chen(samchen1738@gmail.com)
+ */
+ 
+public class SixBallAuton extends SequentialCommandGroup {
+    // Time it takes for the shooter to reach the target speed
+    private static final double SHOOTER_INITIALIZE_DELAY = 0.3;
+    // Time it takes for the conveyor to give the shooter the ball
+    private static final double CONVEYOR_TO_SHOOTER = 3.0;
+    // Time we want to give the drivetrain to align
+    private static final double DRIVETRAIN_ALIGN_TIME = 3.0;
+
+    private static final String SIX_BALL_TO_SECOND_BALL = "FiveBallAuton/output/FiveBallAcquireSecondBall.wpilib.json";
+    private static final String SIX_BALL_TO_TERMINAL = "FiveBallAuton/output/FiveBallGetTerminalBalls.wpilib.json";
+    private static final String SIX_BALL_TERMINAL_TO_SHOOT = "FiveBallAuton/output/FiveBallShootTerminalBalls.wpilib.json";
+    private static final String SIX_BALL_TO_WALL_BALL = "FiveBallAuton/output/FiveBallGetWallBall.wpilib.json";
+
+    /** Creates a new FiveBallAuton. */
+    public SixBallAuton(RobotContainer robot) {
+
+        // Starting up subsystems
+        addCommands(
+            new LEDSet(robot.leds, LEDColor.YELLOW),
+            new IntakeExtend(robot.intake),
+            new IntakeAcquireForever(robot.intake),
+            new IntakeDisableSafety(robot.intake),
+            new ShooterRingShot(robot.shooter),
+            new WaitCommand(SHOOTER_INITIALIZE_DELAY)
+        );
+
+        // Tarmac to first ball
+        addCommands(
+            new LEDSet(robot.leds, LEDColor.GREEN),
+            new DrivetrainRamsete(robot.drivetrain, SIX_BALL_TO_SECOND_BALL)
+                    .robotRelative());
+        addCommands(
+            new IntakeEnableSafety(robot.intake),
+            new LEDSet(robot.leds, LEDColor.GREEN.pulse()),
+            new DrivetrainAlign(robot.drivetrain, robot.camera)
+                    .withTimeout(DRIVETRAIN_ALIGN_TIME)
+        );
+
+        addCommands(
+                new LEDSet(robot.leds, LEDColor.RAINBOW),
+                new ConveyorShoot(robot.conveyor).withTimeout(15));
+
+        // First ball to terminal to RingShot
+        addCommands(
+                new LEDSet(robot.leds, LEDColor.BLUE),
+                
+                new DrivetrainRamsete(robot.drivetrain, SIX_BALL_TO_TERMINAL)
+                        .fieldRelative());
+
+        addCommands(
+                new DrivetrainDriveDistance(robot.drivetrain, Units.feetToMeters(-1))
+                .fieldRelative(),
+
+                new LEDSet(robot.leds, LEDColor.WHITE.pulse()));
+
+        // Return to Ring to shoot
+        addCommands(
+                new LEDSet(robot.leds, LEDColor.PURPLE),
+                new DrivetrainRamsete(robot.drivetrain, SIX_BALL_TERMINAL_TO_SHOOT)
+                        .fieldRelative());
+        addCommands(
+                new LEDSet(robot.leds, LEDColor.PURPLE.pulse()),
+                new DrivetrainAlign(robot.drivetrain, robot.camera)
+                        .withTimeout(DRIVETRAIN_ALIGN_TIME)
+        );
+
+        addCommands(
+                new LEDSet(robot.leds, LEDColor.RAINBOW),
+                new ConveyorShoot(robot.conveyor).withTimeout(CONVEYOR_TO_SHOOTER));
+
+        // Pick up and shoot fifth ball
+        addCommands(
+                new LEDSet(robot.leds, LEDColor.PINK),
+                new DrivetrainRamsete(robot.drivetrain, SIX_BALL_TO_WALL_BALL)
+                        .fieldRelative());
+        addCommands(  
+                new LEDSet(robot.leds, LEDColor.PINK.pulse()),
+                new DrivetrainAlign(robot.drivetrain, robot.camera)
+                        .withTimeout(DRIVETRAIN_ALIGN_TIME)
+        );
+
+        addCommands(
+                new LEDSet(robot.leds, LEDColor.RAINBOW),
+                new ConveyorShoot(robot.conveyor).withTimeout(CONVEYOR_TO_SHOOTER));
+
+        addCommands(new LEDSet(robot.leds, LEDColor.WHITE.pulse()));
+    }
+}
diff --git a/src/main/java/com/stuypulse/robot/commands/auton/chezy/ThreeBallAuton.java b/src/main/java/com/stuypulse/robot/commands/auton/chezy/ThreeBallAuton.java
new file mode 100644
index 00000000..92b5328f
--- /dev/null
+++ b/src/main/java/com/stuypulse/robot/commands/auton/chezy/ThreeBallAuton.java
@@ -0,0 +1,81 @@
+/************************ PROJECT DORCAS ************************/
+/* Copyright (c) 2022 StuyPulse Robotics. All rights reserved.  */
+/* This work is licensed under the terms of the MIT license.    */
+/****************************************************************/
+
+package com.stuypulse.robot.commands.auton.chezy;
+
+import com.stuypulse.robot.RobotContainer;
+import com.stuypulse.robot.commands.conveyor.ConveyorShoot;
+import com.stuypulse.robot.commands.drivetrain.DrivetrainAlign;
+import com.stuypulse.robot.commands.drivetrain.DrivetrainRamsete;
+import com.stuypulse.robot.commands.intake.IntakeAcquireForever;
+import com.stuypulse.robot.commands.intake.IntakeDisableSafety;
+import com.stuypulse.robot.commands.intake.IntakeEnableSafety;
+import com.stuypulse.robot.commands.intake.IntakeExtend;
+import com.stuypulse.robot.commands.leds.LEDSet;
+import com.stuypulse.robot.commands.shooter.ShooterRingShot;
+import com.stuypulse.robot.util.LEDColor;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+
+/*-
+ * @author Vincent Wang (vinowang921@gmail.com)
+ * @author Ivan Wei (ivanw8288@gmail.com)
+ * @author Ivan Chen (ivanchen07@gmail.com)
+ * @author Eric Lin (ericlin071906@gmail.com)
+ * @author Marc Jiang (mjiang05@gmail.com)
+ * @author Ian Jiang (ijiang05@gmail.com)
+ * @author Carmin Vuong (carminvuong@gmail.com)
+ * @author Samuel Chen(samchen1738@gmail.com)
+ */
+
+public class ThreeBallAuton extends SequentialCommandGroup {
+    // Initial delay for the auton
+    private static final double START_DELAY = 1.0;
+
+    // Time it takes for the shooter to reach the target speed
+    private static final double SHOOTER_INITIALIZE_DELAY = 1.0;
+    // Time it takes for the conveyor to give the shooter the ball
+    private static final double CONVEYOR_TO_SHOOTER = 1.0;
+    // Time we want to give the drivetrain to align
+    private static final double DRIVETRAIN_ALIGN_TIME = 3.0;
+
+    
+    private static final String START =
+            "TwoBallAuton/output/TwoBallGetSecondBall.wpilib.json";
+    private static final String EVACUATE =
+            "TwoBallAuton/output/TwoBallEvacuate.wpilib.json";
+
+    public ThreeBallAuton(RobotContainer robot) {
+
+        addCommands(
+                new LEDSet(robot.leds, LEDColor.RED), new WaitCommand(START_DELAY));
+
+        // Starting up subsystems
+        addCommands(
+            new LEDSet(robot.leds, LEDColor.YELLOW),
+            new IntakeExtend(robot.intake),
+            new IntakeDisableSafety(robot.intake),
+            new IntakeAcquireForever(robot.intake),
+            new ShooterRingShot(robot.shooter),
+            new WaitCommand(SHOOTER_INITIALIZE_DELAY)
+        );
+
+        addCommands(
+                new LEDSet(robot.leds, LEDColor.GREEN),
+                new DrivetrainRamsete(robot.drivetrain, START).robotRelative(),
+                new IntakeEnableSafety(robot.intake),
+                new DrivetrainAlign(robot.drivetrain, robot.camera)
+                        .withTimeout(DRIVETRAIN_ALIGN_TIME),
+                new ConveyorShoot(robot.conveyor).withTimeout(CONVEYOR_TO_SHOOTER));
+
+        addCommands(
+                new LEDSet(robot.leds, LEDColor.BLUE),
+                new DrivetrainRamsete(robot.drivetrain, EVACUATE).fieldRelative()
+        );
+
+        addCommands(new LEDSet(robot.leds, LEDColor.WHITE.pulse()));
+    }
+}
diff --git a/src/main/java/com/stuypulse/robot/commands/auton/chezy/ThreeBallMeanerAuton.java b/src/main/java/com/stuypulse/robot/commands/auton/chezy/ThreeBallMeanerAuton.java
new file mode 100644
index 00000000..575b31a9
--- /dev/null
+++ b/src/main/java/com/stuypulse/robot/commands/auton/chezy/ThreeBallMeanerAuton.java
@@ -0,0 +1,100 @@
+/************************ PROJECT DORCAS ************************/
+/* Copyright (c) 2022 StuyPulse Robotics. All rights reserved.  */
+/* This work is licensed under the terms of the MIT license.    */
+/****************************************************************/
+
+package com.stuypulse.robot.commands.auton.chezy;
+
+import com.stuypulse.robot.RobotContainer;
+import com.stuypulse.robot.commands.conveyor.ConveyorForceEject;
+import com.stuypulse.robot.commands.conveyor.ConveyorShoot;
+import com.stuypulse.robot.commands.drivetrain.DrivetrainAlign;
+import com.stuypulse.robot.commands.drivetrain.DrivetrainRamsete;
+import com.stuypulse.robot.commands.intake.IntakeAcquireForever;
+import com.stuypulse.robot.commands.intake.IntakeDeacquireForever;
+import com.stuypulse.robot.commands.intake.IntakeDisableSafety;
+import com.stuypulse.robot.commands.intake.IntakeEnableSafety;
+import com.stuypulse.robot.commands.intake.IntakeExtend;
+import com.stuypulse.robot.commands.leds.LEDSet;
+import com.stuypulse.robot.commands.shooter.ShooterRingShot;
+import com.stuypulse.robot.util.LEDColor;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+
+/*-
+ * @author Vincent Wang (vinowang921@gmail.com)
+ * @author Eric Lin (ericlin071906@gmail.com)
+ * @author Shaurya Sen (shauryasen12@gmail.com)
+ */
+
+public class ThreeBallMeanerAuton extends SequentialCommandGroup {
+    // Time it takes for the shooter to reach the target speed
+    private static final double SHOOTER_INITIALIZE_DELAY = 0.7;
+    // Time it takes for the conveyor to give the shooter the ball
+    private static final double CONVEYOR_TO_SHOOTER = 1.0;
+    // Time we want to give the drivetrain to align
+    private static final double DRIVETRAIN_ALIGN_TIME = 3.0;
+    // Time it takes for the robot to deacquire two balls
+    private static final double INTAKE_DEACQUIRE_TIME = 0.75;
+
+    private static final String GET_SECOND_BALL = "TwoBallMeanerAuton/output/GetSecondBall.wpilib.json";
+    private static final String FIRST_OPPONENT_BALL = "TwoBallMeanerAuton/output/FirstOpponentBall.wpilib.json";
+    private static final String SETUP_FOR_LAST_BALL = "TwoBallMeanerAuton/output/SetupForLastBall.wpilib.json";
+    private static final String SECOND_OPPONENT_BALL = "TwoBallMeanerAuton/output/LastOpponentBall.wpilib.json";
+    private static final String FINAL_MOVEMENT = "TwoBallMeanerAuton/output/FinalMovement.wpilib.json";
+
+    public  ThreeBallMeanerAuton(RobotContainer robot) {
+
+        // Starting up subsystems
+        addCommands(
+            new LEDSet(robot.leds, LEDColor.YELLOW),
+            new IntakeExtend(robot.intake),
+            new IntakeDisableSafety(robot.intake),
+            new IntakeAcquireForever(robot.intake),
+            new ShooterRingShot(robot.shooter),
+            new WaitCommand(SHOOTER_INITIALIZE_DELAY)
+        );
+
+        // Shoot Two Balls
+        addCommands(
+                new LEDSet(robot.leds, LEDColor.GREEN),
+                new DrivetrainRamsete(robot.drivetrain, GET_SECOND_BALL)
+                        .robotRelative(),
+                new IntakeEnableSafety(robot.intake),
+
+                new LEDSet(robot.leds, LEDColor.GREEN.pulse()),
+                new DrivetrainAlign(robot.drivetrain, robot.camera)
+                        .withTimeout(DRIVETRAIN_ALIGN_TIME),
+
+                new LEDSet(robot.leds, LEDColor.RAINBOW),
+                new ConveyorShoot(robot.conveyor).withTimeout(CONVEYOR_TO_SHOOTER)
+        );
+
+        // Get Wall Blue Ball
+        addCommands(
+                new LEDSet(robot.leds, LEDColor.BLUE),
+                new DrivetrainRamsete(robot.drivetrain, FIRST_OPPONENT_BALL)
+                        .fieldRelative());
+        addCommands(
+                new LEDSet(robot.leds, LEDColor.PURPLE),
+                new DrivetrainRamsete(robot.drivetrain, SETUP_FOR_LAST_BALL)
+                        .fieldRelative());
+        addCommands(
+                new LEDSet(robot.leds, LEDColor.PINK),
+                new DrivetrainRamsete(robot.drivetrain, SECOND_OPPONENT_BALL)
+                        .fieldRelative(),
+
+                new IntakeDeacquireForever(robot.intake)
+                        .alongWith(new ConveyorForceEject(robot.conveyor))
+                        .withTimeout(INTAKE_DEACQUIRE_TIME)
+        );
+                
+        addCommands(
+                new LEDSet(robot.leds, LEDColor.AQUA),
+                new DrivetrainRamsete(robot.drivetrain, FINAL_MOVEMENT)
+                        .fieldRelative());
+
+        addCommands(new LEDSet(robot.leds, LEDColor.WHITE.pulse()));
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/com/stuypulse/robot/commands/auton/chezy/ThreeBallOneMeanAuton.java b/src/main/java/com/stuypulse/robot/commands/auton/chezy/ThreeBallOneMeanAuton.java
new file mode 100644
index 00000000..f8c49bb8
--- /dev/null
+++ b/src/main/java/com/stuypulse/robot/commands/auton/chezy/ThreeBallOneMeanAuton.java
@@ -0,0 +1,72 @@
+package com.stuypulse.robot.commands.auton.chezy;
+
+import com.stuypulse.robot.RobotContainer;
+import com.stuypulse.robot.commands.conveyor.ConveyorForceEject;
+import com.stuypulse.robot.commands.conveyor.ConveyorShoot;
+import com.stuypulse.robot.commands.drivetrain.DrivetrainAlign;
+import com.stuypulse.robot.commands.drivetrain.DrivetrainRamsete;
+import com.stuypulse.robot.commands.intake.IntakeAcquireForever;
+import com.stuypulse.robot.commands.intake.IntakeDeacquireForever;
+import com.stuypulse.robot.commands.intake.IntakeDisableSafety;
+import com.stuypulse.robot.commands.intake.IntakeEnableSafety;
+import com.stuypulse.robot.commands.intake.IntakeExtend;
+import com.stuypulse.robot.commands.leds.LEDSet;
+import com.stuypulse.robot.commands.shooter.ShooterRingShot;
+import com.stuypulse.robot.util.LEDColor;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+
+public class ThreeBallOneMeanAuton extends SequentialCommandGroup {
+
+    private static final double ROBOT_STARTUP = 1;
+    private static final double DRIVETRAIN_ALIGN_TWO = 1;
+    private static final double SHOOT_TWO = 5;
+    private static final double INTAKE_DEACQUIRE_TIME = 3;
+
+    private static final String GET_FIRST_PATH = "TwoBallOneMean/output/GetFirstBall.wpilib.json";
+    private static final String GET_OPPONENT_PATH = "TwoBallOneMean/output/GetOpponent.wpilib.json";
+    private static final String DISCARD_OPPONENT_PATH = "TwoBallOneMean/output/DiscardOpponent.wpilib.json";
+    private static final String SETUP_TELEOP_PATH = "TwoBallOneMean/output/SetupTeleop.wpilib.json";
+
+    public ThreeBallOneMeanAuton(RobotContainer robot) {
+        
+         // Starting up subsystems
+         addCommands(
+            new LEDSet(robot.leds, LEDColor.YELLOW),
+            new IntakeDisableSafety(robot.intake),
+            new IntakeExtend(robot.intake),
+            new IntakeAcquireForever(robot.intake),
+            new ShooterRingShot(robot.shooter),
+            new WaitCommand(ROBOT_STARTUP)
+        );
+
+        // Shoot Two Balls
+        addCommands(
+                new LEDSet(robot.leds, LEDColor.GREEN),
+                new DrivetrainRamsete(robot.drivetrain, GET_FIRST_PATH)
+                        .robotRelative(),
+                new IntakeEnableSafety(robot.intake),
+                new DrivetrainAlign(robot.drivetrain, robot.camera).withTimeout(DRIVETRAIN_ALIGN_TWO),
+                new ConveyorShoot(robot.conveyor).withTimeout(SHOOT_TWO)
+        );
+
+        // get & discard opponent balls 
+        addCommands(
+            new LEDSet(robot.leds, LEDColor.RED),
+            new DrivetrainRamsete(robot.drivetrain, GET_OPPONENT_PATH).fieldRelative(),
+            new DrivetrainRamsete(robot.drivetrain, DISCARD_OPPONENT_PATH).fieldRelative(),
+            
+            new IntakeDeacquireForever(robot.intake)
+                        .alongWith(new ConveyorForceEject(robot.conveyor))
+                        .withTimeout(INTAKE_DEACQUIRE_TIME)
+        );
+
+        // move for the start of teleop
+        addCommands(
+            new LEDSet(robot.leds, LEDColor.WHITE)
+            // new DrivetrainRamsete(robot.drivetrain, SETUP_TELEOP_PATH).fieldRelative()
+         );
+    }
+
+}
diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeDisableSafety.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeDisableSafety.java
new file mode 100644
index 00000000..bd6711cf
--- /dev/null
+++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeDisableSafety.java
@@ -0,0 +1,13 @@
+package com.stuypulse.robot.commands.intake;
+
+import com.stuypulse.robot.subsystems.Intake;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class IntakeDisableSafety extends InstantCommand {
+ 
+    public IntakeDisableSafety(Intake intake) {
+        super(() -> intake.setIgnoreConveyor(true), intake);
+    }
+
+}
diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeEnableSafety.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeEnableSafety.java
new file mode 100644
index 00000000..cdcfb95e
--- /dev/null
+++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeEnableSafety.java
@@ -0,0 +1,13 @@
+package com.stuypulse.robot.commands.intake;
+
+import com.stuypulse.robot.subsystems.Intake;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class IntakeEnableSafety extends InstantCommand {
+
+    public IntakeEnableSafety(Intake intake) {
+        super(() -> intake.setIgnoreConveyor(false), intake);
+    }
+    
+}
diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java
index a912328f..a0824e5c 100644
--- a/src/main/java/com/stuypulse/robot/constants/Settings.java
+++ b/src/main/java/com/stuypulse/robot/constants/Settings.java
@@ -102,7 +102,7 @@ public interface BallRGB {
 
     public interface Conveyor {
         // How long it takes to until ConveyorShootCommand finishes
-        double DEBOUNCE_TIME = 0.2;
+        double DEBOUNCE_TIME = 0.4;
         double SEMI_AUTO_TIME = 0.400694;
 
         SmartNumber SLOW_MUL = new SmartNumber("Conveyor/Slow Mul", 5.0 / 8.0);
diff --git a/src/main/java/com/stuypulse/robot/subsystems/Intake.java b/src/main/java/com/stuypulse/robot/subsystems/Intake.java
index 502a063a..bafb9c43 100644
--- a/src/main/java/com/stuypulse/robot/subsystems/Intake.java
+++ b/src/main/java/com/stuypulse/robot/subsystems/Intake.java
@@ -53,6 +53,8 @@ public class Intake extends SubsystemBase {
     private final IFilter speedFilter;
     private double speed;
 
+    private boolean ignoreConveyor;
+
     public Intake(Conveyor conveyor) {
         this.motor = new CANSparkMax(Ports.Intake.MOTOR, MotorType.kBrushless);
         Motors.INTAKE.configure(motor);
@@ -67,6 +69,8 @@ public Intake(Conveyor conveyor) {
 
         this.speedFilter = new LowPassFilter(Settings.Intake.SPEED_FILTERING);
         this.speed = 0.0;
+
+        ignoreConveyor = false;
     }
 
     /*** Extend / Retract ***/
@@ -102,15 +106,22 @@ public void deacquire() {
     }
 
     /*** Automated Intake Actions ***/
+    public void setIgnoreConveyor(boolean disabled) { 
+        ignoreConveyor = disabled;
+    }
+
     private boolean getShouldStop() {
+        if (ignoreConveyor) return false;
         return conveyor.getGandalfDirection() == Direction.STOPPED && conveyor.hasAnyBall();
     }
 
     private boolean getShouldSlow() {
+        if (ignoreConveyor) return false;
         return conveyor.getGandalfDirection() != Direction.STOPPED && conveyor.hasAnyBall();
     }
 
     public boolean getShouldRetract() {
+        if (ignoreConveyor) return false;
         return Settings.Intake.AUTO_RETRACT.get()
                 && !DriverStation.isAutonomous()
                 && conveyor.isFull();