diff --git a/PathWeaver/Autos/1BallHangarSide b/PathWeaver/Autos/1BallHangarSide
new file mode 100644
index 0000000..dc11a68
--- /dev/null
+++ b/PathWeaver/Autos/1BallHangarSide
@@ -0,0 +1 @@
+HangarSideFirstBall
diff --git a/PathWeaver/Paths/HangarSideFirstBall b/PathWeaver/Paths/HangarSideFirstBall
new file mode 100644
index 0000000..8613fca
--- /dev/null
+++ b/PathWeaver/Paths/HangarSideFirstBall
@@ -0,0 +1,3 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
+7.296058027886137,-3.663979009231006,-0.8863404117260814,0.9927012611332127,true,false,
+4.944301468772932,-1.9503875465605802,-0.5554399913483445,0.5436221191919977,true,false,
diff --git a/PathWeaver/Paths/LinearTest b/PathWeaver/Paths/LinearTest
new file mode 100644
index 0000000..8e06f70
--- /dev/null
+++ b/PathWeaver/Paths/LinearTest
@@ -0,0 +1,3 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
+7.343329516511528,-3.7112504978563976,-2.4581174085203363,0.02363574431269555,true,false,
+5.15,-3.7116,-1.7892934001776695,0.023985246456297737,true,false,
diff --git a/PathWeaver/Paths/NonLinearTest b/PathWeaver/Paths/NonLinearTest
new file mode 100644
index 0000000..c3c9d0b
--- /dev/null
+++ b/PathWeaver/Paths/NonLinearTest
@@ -0,0 +1,3 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
+7.236968667104398,-3.687614753543702,-1.914495289328337,0.7090723293808661,true,false,
+3.10071341238268,-3.923972196670657,-2.0326740108918155,0.8272510509443438,true,false,
diff --git a/PathWeaver/pathweaver.json b/PathWeaver/pathweaver.json
new file mode 100644
index 0000000..5d37410
--- /dev/null
+++ b/PathWeaver/pathweaver.json
@@ -0,0 +1,9 @@
+{
+  "lengthUnit": "Meter",
+  "exportUnit": "Always Meters",
+  "maxVelocity": 4.0,
+  "maxAcceleration": 1.0,
+  "trackWidth": 1.2406,
+  "gameName": "Rapid React",
+  "outputDir": "../src/main/deploy"
+}
\ No newline at end of file
diff --git a/src/main/deploy/output/HangarSideFirstBall.wpilib.json b/src/main/deploy/output/HangarSideFirstBall.wpilib.json
new file mode 100644
index 0000000..c92c9da
--- /dev/null
+++ b/src/main/deploy/output/HangarSideFirstBall.wpilib.json
@@ -0,0 +1,557 @@
+[
+{
+"acceleration": 1.0000000000000002,
+"curvature": -0.0,
+"pose": {
+"rotation": {
+"radians": 2.2996510532475267
+},
+"translation": {
+"x": 7.296058027886137,
+"y": 4.565620990768993
+}
+},
+"time": 0.0,
+"velocity": 0.0
+},
+{
+"acceleration": 0.9999999999999999,
+"curvature": 0.7046526549936946,
+"pose": {
+"rotation": {
+"radians": 2.340071458330768
+},
+"translation": {
+"x": 7.237122350294481,
+"y": 4.629655803575388
+}
+},
+"time": 0.41720017192887204,
+"velocity": 0.41720017192887215
+},
+{
+"acceleration": 1.0,
+"curvature": 0.5167714112787369,
+"pose": {
+"rotation": {
+"radians": 2.408387003016788
+},
+"translation": {
+"x": 7.159692305068378,
+"y": 4.704062631315389
+}
+},
+"time": 0.6235609903276057,
+"velocity": 0.6235609903276058
+},
+{
+"acceleration": 1.0,
+"curvature": 0.3798327946085926,
+"pose": {
+"rotation": {
+"radians": 2.437621774262191
+},
+"translation": {
+"x": 7.110188157019157,
+"y": 4.747292080731512
+}
+},
+"time": 0.7212997247365294,
+"velocity": 0.7212997247365295
+},
+{
+"acceleration": 1.0,
+"curvature": 0.27231786672071406,
+"pose": {
+"rotation": {
+"radians": 2.461827180519893
+},
+"translation": {
+"x": 7.052310222072237,
+"y": 4.795177423025944
+}
+},
+"time": 0.818847516710207,
+"velocity": 0.8188475167102071
+},
+{
+"acceleration": 1.0,
+"curvature": 0.19496235127775563,
+"pose": {
+"rotation": {
+"radians": 2.4814294171577522
+},
+"translation": {
+"x": 6.985657913541941,
+"y": 4.847919970104026
+}
+},
+"time": 0.9167895596364871,
+"velocity": 0.9167895596364872
+},
+{
+"acceleration": 1.0,
+"curvature": 0.1409791525409567,
+"pose": {
+"rotation": {
+"radians": 2.497178545789836
+},
+"translation": {
+"x": 6.910176803843516,
+"y": 4.905523305953324
+}
+},
+"time": 1.0150879346181663,
+"velocity": 1.0150879346181663
+},
+{
+"acceleration": 1.0,
+"curvature": 0.10344784821657113,
+"pose": {
+"rotation": {
+"radians": 2.5098174403962026
+},
+"translation": {
+"x": 6.82612362989756,
+"y": 4.967813573365698
+}
+},
+"time": 1.1133913740328822,
+"velocity": 1.1133913740328822
+},
+{
+"acceleration": 0.9999999999999991,
+"curvature": 0.0770914021308612,
+"pose": {
+"rotation": {
+"radians": 2.519974120995346
+},
+"translation": {
+"x": 6.734031298534453,
+"y": 5.034459760659368
+}
+},
+"time": 1.2111963357394289,
+"velocity": 1.2111963357394289
+},
+{
+"acceleration": 1.0000000000000018,
+"curvature": 0.05824813214526938,
+"pose": {
+"rotation": {
+"radians": 2.5281456318085227
+},
+"translation": {
+"x": 6.634673891898785,
+"y": 5.104993988400992
+}
+},
+"time": 1.3079345789097343,
+"velocity": 1.3079345789097343
+},
+{
+"acceleration": 1.0,
+"curvature": 0.05083925731030608,
+"pose": {
+"rotation": {
+"radians": 2.5316099349397763
+},
+"translation": {
+"x": 6.582570020447651,
+"y": 5.141540472776556
+}
+},
+"time": 1.3557209501478797,
+"velocity": 1.3557209501478797
+},
+{
+"acceleration": 1.0,
+"curvature": 0.044456822630151084,
+"pose": {
+"rotation": {
+"radians": 2.534712911904704
+},
+"translation": {
+"x": 6.529031672853788,
+"y": 5.178831796127727
+}
+},
+"time": 1.4030219651777522,
+"velocity": 1.4030219651777522
+},
+{
+"acceleration": 1.0,
+"curvature": 0.0389168945314462,
+"pose": {
+"rotation": {
+"radians": 2.5374875852321135
+},
+"translation": {
+"x": 6.474207856411549,
+"y": 5.216780013401131
+}
+},
+"time": 1.449766557319599,
+"velocity": 1.449766557319599
+},
+{
+"acceleration": 1.0,
+"curvature": 0.03406770246119906,
+"pose": {
+"rotation": {
+"radians": 2.5399623294680955
+},
+"translation": {
+"x": 6.418256090385759,
+"y": 5.255292429069307
+}
+},
+"time": 1.4958853834417933,
+"velocity": 1.4958853834417933
+},
+{
+"acceleration": 1.0,
+"curvature": 0.02978308043612628,
+"pose": {
+"rotation": {
+"radians": 2.542161388401129
+},
+"translation": {
+"x": 6.361341312430599,
+"y": 5.294272231090778
+}
+},
+"time": 1.5413111263606993,
+"velocity": 1.5413111263606993
+},
+{
+"acceleration": 0.9999999999999969,
+"curvature": 0.025957101092007828,
+"pose": {
+"rotation": {
+"radians": 2.544105319919419
+},
+"translation": {
+"x": 6.303634785008495,
+"y": 5.333619124870108
+}
+},
+"time": 1.585978725676304,
+"velocity": 1.585978725676304
+},
+{
+"acceleration": 0.9999999999999969,
+"curvature": 0.022499669282720083,
+"pose": {
+"rotation": {
+"radians": 2.5458113709251373
+},
+"translation": {
+"x": 6.245313001809008,
+"y": 5.3732299672179735
+}
+},
+"time": 1.629825556366533,
+"velocity": 1.6298255563665327
+},
+{
+"acceleration": 0.6345170575781763,
+"curvature": 0.019332867264386294,
+"pose": {
+"rotation": {
+"radians": 2.5472937861561915
+},
+"translation": {
+"x": 6.186556594167721,
+"y": 5.412999400311222
+}
+},
+"time": 1.6727915699838285,
+"velocity": 1.672791569983828
+},
+{
+"acceleration": -0.9999999999999969,
+"curvature": 0.016387870032603457,
+"pose": {
+"rotation": {
+"radians": 2.548564055017322
+},
+"translation": {
+"x": 6.127549237485125,
+"y": 5.452820485652941
+}
+},
+"time": 1.7150093362619303,
+"velocity": 1.6995794628201324
+},
+{
+"acceleration": -1.0,
+"curvature": 0.013602272471953643,
+"pose": {
+"rotation": {
+"radians": 2.5496311000579794
+},
+"translation": {
+"x": 6.068476557645514,
+"y": 5.492585338032521
+}
+},
+"time": 1.7574373576963098,
+"velocity": 1.657151441385753
+},
+{
+"acceleration": -1.0,
+"curvature": 0.010917687239448186,
+"pose": {
+"rotation": {
+"radians": 2.550501409825501
+},
+"translation": {
+"x": 6.009525037435866,
+"y": 5.532185759485722
+}
+},
+"time": 1.8008614233459463,
+"velocity": 1.6137273757361166
+},
+{
+"acceleration": -1.0,
+"curvature": 0.008277482516533353,
+"pose": {
+"rotation": {
+"radians": 2.5511791176175995
+},
+"translation": {
+"x": 5.950880922964737,
+"y": 5.571513873254737
+}
+},
+"time": 1.8452274050615463,
+"velocity": 1.5693613940205167
+},
+{
+"acceleration": -1.0,
+"curvature": 0.005624531040495701,
+"pose": {
+"rotation": {
+"radians": 2.551666026227246
+},
+"translation": {
+"x": 5.892729130081147,
+"y": 5.6104627577482535
+}
+},
+"time": 1.8904777400074857,
+"velocity": 1.5241110590745772
+},
+{
+"acceleration": -1.0,
+"curvature": 0.002898835090000706,
+"pose": {
+"rotation": {
+"radians": 2.5519615771311823
+},
+"translation": {
+"x": 5.835252150793467,
+"y": 5.6489270805015215
+}
+},
+"time": 1.9365514192419802,
+"velocity": 1.4780373798400828
+},
+{
+"acceleration": -1.0,
+"curvature": 3.487470383652936e-05,
+"pose": {
+"rotation": {
+"radians": 2.5520627606980106
+},
+"translation": {
+"x": 5.778628959688312,
+"y": 5.686803732136422
+}
+},
+"time": 1.9833839667003454,
+"velocity": 1.4312048323817175
+},
+{
+"acceleration": -1.0,
+"curvature": -0.0030415042170831874,
+"pose": {
+"rotation": {
+"radians": 2.5519639618332066
+},
+"translation": {
+"x": 5.723033920349424,
+"y": 5.723992460321524
+}
+},
+"time": 2.0309074115449537,
+"velocity": 1.3836813875371095
+},
+{
+"acceleration": -0.9999999999999982,
+"curvature": -0.0064168930401514186,
+"pose": {
+"rotation": {
+"radians": 2.5516567329653497
+},
+"translation": {
+"x": 5.668635691776565,
+"y": 5.760396503732153
+}
+},
+"time": 2.0790502579221286,
+"velocity": 1.3355385411599343
+},
+{
+"acceleration": -1.0,
+"curvature": -0.010195116519283036,
+"pose": {
+"rotation": {
+"radians": 2.5511294833261564
+},
+"translation": {
+"x": 5.6155961348044,
+"y": 5.795923226010456
+}
+},
+"time": 2.127737457709116,
+"velocity": 1.286851341372947
+},
+{
+"acceleration": -1.0,
+"curvature": -0.019500633137893245,
+"pose": {
+"rotation": {
+"radians": 2.549350272146696
+},
+"translation": {
+"x": 5.5141999266886845,
+"y": 5.863998590333166
+}
+},
+"time": 2.2264268763361903,
+"velocity": 1.188161922745873
+},
+{
+"acceleration": -0.9999999999999984,
+"curvature": -0.03242352373537361,
+"pose": {
+"rotation": {
+"radians": 2.546452086242235
+},
+"translation": {
+"x": 5.4199626632954985,
+"y": 5.927584052245708
+}
+},
+"time": 2.3263042730827133,
+"velocity": 1.08828452599935
+},
+{
+"acceleration": -1.0,
+"curvature": -0.05138202898256643,
+"pose": {
+"rotation": {
+"radians": 2.5421698138048328
+},
+"translation": {
+"x": 5.3338229520854235,
+"y": 5.986151683617407
+}
+},
+"time": 2.4266442374230612,
+"velocity": 0.9879445616590021
+},
+{
+"acceleration": -1.0,
+"curvature": -0.08052589009449668,
+"pose": {
+"rotation": {
+"radians": 2.536109882662837
+},
+"translation": {
+"x": 5.256505646090824,
+"y": 6.039300415952937
+}
+},
+"time": 2.526676373322581,
+"velocity": 0.8879124257594825
+},
+{
+"acceleration": -1.000000000000001,
+"curvature": -0.1271600889636688,
+"pose": {
+"rotation": {
+"radians": 2.527695331058469
+},
+"translation": {
+"x": 5.188486849320288,
+"y": 6.086776327114388
+}
+},
+"time": 2.625608007325845,
+"velocity": 0.7889807917562185
+},
+{
+"acceleration": -0.9999999999999998,
+"curvature": -0.20421415446353952,
+"pose": {
+"rotation": {
+"radians": 2.5160936461654595
+},
+"translation": {
+"x": 5.12995892216304,
+"y": 6.128492928043334
+}
+},
+"time": 2.7226756640110015,
+"velocity": 0.6919131350710617
+},
+{
+"acceleration": -1.0,
+"curvature": -0.5474240067782133,
+"pose": {
+"rotation": {
+"radians": 2.478436783130011
+},
+"translation": {
+"x": 5.040516432575136,
+"y": 6.1952611286998565
+}
+},
+"time": 2.9091054677973287,
+"velocity": 0.5054833312847344
+},
+{
+"acceleration": -0.9999999999999999,
+"curvature": -1.1809181090136762,
+"pose": {
+"rotation": {
+"radians": 2.415177928221663
+},
+"translation": {
+"x": 4.9827123934221556,
+"y": 6.243032662483474
+}
+},
+"time": 3.0897282424707315,
+"velocity": 0.32486055661133184
+},
+{
+"acceleration": -0.9999999999999999,
+"curvature": -8.961282879756587e-14,
+"pose": {
+"rotation": {
+"radians": 2.3669467639834165
+},
+"translation": {
+"x": 4.944301468772939,
+"y": 6.279212453439419
+}
+},
+"time": 3.4145887990820634,
+"velocity": 0.0
+}
+]
\ No newline at end of file
diff --git a/src/main/deploy/output/LinearTest.wpilib.json b/src/main/deploy/output/LinearTest.wpilib.json
new file mode 100644
index 0000000..db34575
--- /dev/null
+++ b/src/main/deploy/output/LinearTest.wpilib.json
@@ -0,0 +1,422 @@
+[
+{
+"acceleration": 1.0,
+"curvature": -0.0,
+"pose": {
+"rotation": {
+"radians": 3.1319775652900907
+},
+"translation": {
+"x": 7.343329516511528,
+"y": 4.518349502143602
+}
+},
+"time": 0.0,
+"velocity": 0.0
+},
+{
+"acceleration": 1.0000000000000004,
+"curvature": 0.006792988534224084,
+"pose": {
+"rotation": {
+"radians": 3.1322473500147394
+},
+"translation": {
+"x": 7.266513175402384,
+"y": 4.519081097897949
+}
+},
+"time": 0.39196893977576813,
+"velocity": 0.39196893977576813
+},
+{
+"acceleration": 1.0000000000000004,
+"curvature": 0.012251764052837165,
+"pose": {
+"rotation": {
+"radians": 3.132987014464534
+},
+"translation": {
+"x": 7.189700919427855,
+"y": 4.519773225995339
+}
+},
+"time": 0.554319761593559,
+"velocity": 0.5543197615935591
+},
+{
+"acceleration": 1.0000000000000007,
+"curvature": 0.016484396994587324,
+"pose": {
+"rotation": {
+"radians": 3.134097860480685
+},
+"translation": {
+"x": 7.112910235088217,
+"y": 4.520393500148893
+}
+},
+"time": 0.6788643288125953,
+"velocity": 0.6788643288125955
+},
+{
+"acceleration": 1.0,
+"curvature": 0.019603399516883686,
+"pose": {
+"rotation": {
+"radians": 3.1354892959584815
+},
+"translation": {
+"x": 7.036171870399583,
+"y": 4.520916787914282
+}
+},
+"time": 0.7837965773270686,
+"velocity": 0.783796577327069
+},
+{
+"acceleration": 1.0,
+"curvature": 0.02171946236983872,
+"pose": {
+"rotation": {
+"radians": 3.1370789252840883
+},
+"translation": {
+"x": 6.9595283409225335,
+"y": 4.521324692272403
+}
+},
+"time": 0.8761428562028188,
+"velocity": 0.8761428562028192
+},
+{
+"acceleration": 1.0000000000000016,
+"curvature": 0.02293711709496013,
+"pose": {
+"rotation": {
+"radians": 3.1387923525453036
+},
+"translation": {
+"x": 6.883032435790746,
+"y": 4.521605033212085
+}
+},
+"time": 0.9594890005227997,
+"velocity": 0.9594890005228001
+},
+{
+"acceleration": 1.0000000000000016,
+"curvature": 0.02335198368105572,
+"pose": {
+"rotation": {
+"radians": 3.1405627893995374
+},
+"translation": {
+"x": 6.806745723739625,
+"y": 4.521751329312785
+}
+},
+"time": 1.0359502144313282,
+"velocity": 1.0359502144313286
+},
+{
+"acceleration": 1.0,
+"curvature": 0.02304927912486566,
+"pose": {
+"rotation": {
+"radians": -3.1408547633587385
+},
+"translation": {
+"x": 6.73073705913493,
+"y": 4.521762279327276
+}
+},
+"time": 1.1068921255331041,
+"velocity": 1.1068921255331046
+},
+{
+"acceleration": 1.0,
+"curvature": 0.022103305458939092,
+"pose": {
+"rotation": {
+"radians": -3.1391428556582324
+},
+"translation": {
+"x": 6.655081088001409,
+"y": 4.521641243764344
+}
+},
+"time": 1.1732528770340305,
+"velocity": 1.173252877034031
+},
+{
+"acceleration": 1.0,
+"curvature": 0.020577692356382085,
+"pose": {
+"rotation": {
+"radians": -3.137534007699137
+},
+"translation": {
+"x": 6.579856754051422,
+"y": 4.521395726471484
+}
+},
+"time": 1.23570699710173,
+"velocity": 1.2357069971017305
+},
+{
+"acceleration": 1.0,
+"curvature": 0.018526230218381994,
+"pose": {
+"rotation": {
+"radians": -3.1360700260974177
+},
+"translation": {
+"x": 6.505145804713577,
+"y": 4.521036856217594
+}
+},
+"time": 1.2947568903724644,
+"velocity": 1.2947568903724649
+},
+{
+"acceleration": 1.0000000000000016,
+"curvature": 0.015994189414299368,
+"pose": {
+"rotation": {
+"radians": -3.134787792265017
+},
+"translation": {
+"x": 6.431031297161356,
+"y": 4.5205788682756705
+}
+},
+"time": 1.3507876407357542,
+"velocity": 1.3507876407357546
+},
+{
+"acceleration": 1.0,
+"curvature": 0.013020077398458608,
+"pose": {
+"rotation": {
+"radians": -3.133719652988036
+},
+"translation": {
+"x": 6.357596104341745,
+"y": 4.5200385860055015
+}
+},
+"time": 1.4041017096173773,
+"velocity": 1.4041017096173778
+},
+{
+"acceleration": 0.06531293042661161,
+"curvature": 0.00963783651458339,
+"pose": {
+"rotation": {
+"radians": -3.1328936964154956
+},
+"translation": {
+"x": 6.2849214210038635,
+"y": 4.51943490243636
+}
+},
+"time": 1.4549419205353997,
+"velocity": 1.4549419205354002
+},
+{
+"acceleration": -0.9999999999999984,
+"curvature": 0.005879530503789499,
+"pose": {
+"rotation": {
+"radians": -3.132333890723757
+},
+"translation": {
+"x": 6.213085269727597,
+"y": 4.5187882618497035
+}
+},
+"time": 1.5042632170116836,
+"velocity": 1.458163238940706
+},
+{
+"acceleration": -1.0,
+"curvature": 0.001778605447874432,
+"pose": {
+"rotation": {
+"radians": -3.1320600626178665
+},
+"translation": {
+"x": 6.142161006952222,
+"y": 4.518120141361863
+}
+},
+"time": 1.5537443725280706,
+"velocity": 1.408682083424319
+},
+{
+"acceleration": -1.0,
+"curvature": -0.002626162287175235,
+"pose": {
+"rotation": {
+"radians": -3.1320876919371035
+},
+"translation": {
+"x": 6.072215829005039,
+"y": 4.517452532506742
+}
+},
+"time": 1.6043069898318778,
+"velocity": 1.3581194661205118
+},
+{
+"acceleration": -0.9999999999999983,
+"curvature": -0.007285908999027022,
+"pose": {
+"rotation": {
+"radians": -3.1324274976770723
+},
+"translation": {
+"x": 6.00330927813,
+"y": 4.516807422818509
+}
+},
+"time": 1.6560309021252135,
+"velocity": 1.306395553827176
+},
+{
+"acceleration": -1.0,
+"curvature": -0.012136013907648223,
+"pose": {
+"rotation": {
+"radians": -3.1330847907292436
+},
+"translation": {
+"x": 5.93549174851634,
+"y": 4.516206277414293
+}
+},
+"time": 1.7090195167311424,
+"velocity": 1.2534069392212472
+},
+{
+"acceleration": -0.9999999999999983,
+"curvature": -0.017089476220590122,
+"pose": {
+"rotation": {
+"radians": -3.1340585709615096
+},
+"translation": {
+"x": 5.868802992327207,
+"y": 4.515669520576875
+}
+},
+"time": 1.7634072222481934,
+"velocity": 1.1990192337041963
+},
+{
+"acceleration": -1.0,
+"curvature": -0.022029429075529343,
+"pose": {
+"rotation": {
+"radians": -3.1353403528105246
+},
+"translation": {
+"x": 5.803270625728289,
+"y": 4.515216017337387
+}
+},
+"time": 1.8193694795699276,
+"velocity": 1.143056976382462
+},
+{
+"acceleration": -1.0000000000000009,
+"curvature": -0.02680110711501685,
+"pose": {
+"rotation": {
+"radians": -3.136912716795311
+},
+"translation": {
+"x": 5.738908634916444,
+"y": 4.514862555058008
+}
+},
+"time": 1.8771369422702715,
+"velocity": 1.0852895136821181
+},
+{
+"acceleration": -1.0000000000000004,
+"curvature": -0.034984922731860914,
+"pose": {
+"rotation": {
+"radians": -3.1408044329720473
+},
+"translation": {
+"x": 5.613674611769049,
+"y": 4.514509403979665
+}
+},
+"time": 1.9994185191447422,
+"velocity": 0.9630079368076472
+},
+{
+"acceleration": -0.9999999999999993,
+"curvature": -0.03939180259312632,
+"pose": {
+"rotation": {
+"radians": 3.137838306007713
+},
+"translation": {
+"x": 5.492883442171243,
+"y": 4.51468311300253
+}
+},
+"time": 2.1342950066346003,
+"velocity": 0.8281314493177891
+},
+{
+"acceleration": -1.0000000000000002,
+"curvature": -0.03698190804363222,
+"pose": {
+"rotation": {
+"radians": 3.133291505157105
+},
+"translation": {
+"x": 5.376003951740285,
+"y": 4.515390306376448
+}
+},
+"time": 2.290088417784008,
+"velocity": 0.6723380381683811
+},
+{
+"acceleration": -1.0000000000000002,
+"curvature": -0.024514843391959057,
+"pose": {
+"rotation": {
+"radians": 3.1296790220459405
+},
+"translation": {
+"x": 5.262143465340612,
+"y": 4.516554597586655
+}
+},
+"time": 2.4888173771651547,
+"velocity": 0.4736090787872346
+},
+{
+"acceleration": -1.0000000000000002,
+"curvature": -1.7512130721932426e-14,
+"pose": {
+"rotation": {
+"radians": 3.128188585834304
+},
+"translation": {
+"x": 5.149999999999993,
+"y": 4.5180000000000025
+}
+},
+"time": 2.9624264559523894,
+"velocity": 0.0
+}
+]
\ No newline at end of file
diff --git a/src/main/deploy/output/NonLinearTest.wpilib.json b/src/main/deploy/output/NonLinearTest.wpilib.json
new file mode 100644
index 0000000..e6e50e2
--- /dev/null
+++ b/src/main/deploy/output/NonLinearTest.wpilib.json
@@ -0,0 +1,752 @@
+[
+{
+"acceleration": 1.0,
+"curvature": -0.0,
+"pose": {
+"rotation": {
+"radians": 2.7868870015788527
+},
+"translation": {
+"x": 7.236968667104398,
+"y": 4.5419852464562975
+}
+},
+"time": 0.0,
+"velocity": 0.0
+},
+{
+"acceleration": 1.0,
+"curvature": 0.6431183760589534,
+"pose": {
+"rotation": {
+"radians": 2.8090801026846557
+},
+"translation": {
+"x": 7.176507694980693,
+"y": 4.563854942153977
+}
+},
+"time": 0.35859373496271835,
+"velocity": 0.35859373496271835
+},
+{
+"acceleration": 1.0,
+"curvature": 0.9632813874118717,
+"pose": {
+"rotation": {
+"radians": 2.865058989164917
+},
+"translation": {
+"x": 7.112487424513783,
+"y": 4.584101946626071
+}
+},
+"time": 0.5127189635471717,
+"velocity": 0.5127189635471717
+},
+{
+"acceleration": 0.9999999999999987,
+"curvature": 0.9846391624647143,
+"pose": {
+"rotation": {
+"radians": 2.937228290128217
+},
+"translation": {
+"x": 7.041984010753304,
+"y": 4.601396909012926
+}
+},
+"time": 0.6388021292414763,
+"velocity": 0.6388021292414763
+},
+{
+"acceleration": 1.0,
+"curvature": 0.8414278195016,
+"pose": {
+"rotation": {
+"radians": 3.0111515012815344
+},
+"translation": {
+"x": 6.962724643471328,
+"y": 4.614710890997543
+}
+},
+"time": 0.7541934886899907,
+"velocity": 0.7541934886899906
+},
+{
+"acceleration": 1.0,
+"curvature": 0.6539572176092259,
+"pose": {
+"rotation": {
+"radians": 3.0783805486860056
+},
+"translation": {
+"x": 6.873041141285629,
+"y": 4.623293812163382
+}
+},
+"time": 0.8654446002897318,
+"velocity": 0.8654446002897317
+},
+{
+"acceleration": 1.0000000000000009,
+"curvature": 0.4845927846268833,
+"pose": {
+"rotation": {
+"radians": 3.1355966992513338
+},
+"translation": {
+"x": 6.771823545782939,
+"y": 4.626652895352158
+}
+},
+"time": 0.9754696271836718,
+"velocity": 0.9754696271836717
+},
+{
+"acceleration": 0.9999999999999991,
+"curvature": 0.35179549035683494,
+"pose": {
+"rotation": {
+"radians": -3.100666535108479
+},
+"translation": {
+"x": 6.658473715642223,
+"y": 4.624531112021641
+}
+},
+"time": 1.085486235654601,
+"velocity": 1.085486235654601
+},
+{
+"acceleration": 1.0000000000000016,
+"curvature": 0.2538039075752756,
+"pose": {
+"rotation": {
+"radians": -3.0629880475283797
+},
+"translation": {
+"x": 6.532858920757936,
+"y": 4.616885627603462
+}
+},
+"time": 1.1958155652744749,
+"velocity": 1.1958155652744749
+},
+{
+"acceleration": 1.0,
+"curvature": 0.2155131233596031,
+"pose": {
+"rotation": {
+"radians": -3.047173305180617
+},
+"translation": {
+"x": 6.465526524061978,
+"y": 4.611031375885881
+}
+},
+"time": 1.2510586326385922,
+"velocity": 1.2510586326385922
+},
+{
+"acceleration": 1.0000000000000016,
+"curvature": 0.18300899591126926,
+"pose": {
+"rotation": {
+"radians": -3.033139655276441
+},
+"translation": {
+"x": 6.39526543636329,
+"y": 4.603866246860908
+}
+},
+"time": 1.306291957328082,
+"velocity": 1.306291957328082
+},
+{
+"acceleration": 1.0000000000000016,
+"curvature": 0.15537527096696074,
+"pose": {
+"rotation": {
+"radians": -3.0207242497683353
+},
+"translation": {
+"x": 6.322169298186051,
+"y": 4.595435529711038
+}
+},
+"time": 1.3614551474403225,
+"velocity": 1.3614551474403225
+},
+{
+"acceleration": 1.0,
+"curvature": 0.13180645445618613,
+"pose": {
+"rotation": {
+"radians": -3.0097788902368285
+},
+"translation": {
+"x": 6.2463521371535204,
+"y": 4.585793859246722
+}
+},
+"time": 1.416480022600105,
+"velocity": 1.416480022600105
+},
+{
+"acceleration": 1.0,
+"curvature": 0.11161006606833378,
+"pose": {
+"rotation": {
+"radians": -3.0001709969858354
+},
+"translation": {
+"x": 6.167946917804394,
+"y": 4.575004542323795
+}
+},
+"time": 1.4712932557854306,
+"velocity": 1.4712932557854306
+},
+{
+"acceleration": 1.0000000000000027,
+"curvature": 0.09419872886168623,
+"pose": {
+"rotation": {
+"radians": -2.991783533531322
+},
+"translation": {
+"x": 6.087104091409148,
+"y": 4.563138884260908
+}
+},
+"time": 1.5258184008529496,
+"velocity": 1.5258184008529496
+},
+{
+"acceleration": 1.0,
+"curvature": 0.07907784651353172,
+"pose": {
+"rotation": {
+"radians": -2.9845143428798155
+},
+"translation": {
+"x": 6.003990145786397,
+"y": 4.550275515256961
+}
+},
+"time": 1.5799774488762637,
+"velocity": 1.579977448876264
+},
+{
+"acceleration": 1.0000000000000024,
+"curvature": 0.06583207073549241,
+"pose": {
+"rotation": {
+"radians": -2.97827519157477
+},
+"translation": {
+"x": 5.918786155119244,
+"y": 4.53649971680853
+}
+},
+"time": 1.6336920229520415,
+"velocity": 1.6336920229520417
+},
+{
+"acceleration": 1.0,
+"curvature": 0.05411221264305731,
+"pose": {
+"rotation": {
+"radians": -2.9729907081288602
+},
+"translation": {
+"x": 5.831686329771632,
+"y": 4.521902748127303
+}
+},
+"time": 1.6868842958747983,
+"velocity": 1.6868842958747987
+},
+{
+"acceleration": 1.0000000000000024,
+"curvature": 0.04362334802636727,
+"pose": {
+"rotation": {
+"radians": -2.9685973297553883
+},
+"translation": {
+"x": 5.742896566104696,
+"y": 4.50658117255751
+}
+},
+"time": 1.739477695853489,
+"velocity": 1.7394776958534894
+},
+{
+"acceleration": 0.9999999999999976,
+"curvature": 0.034114361273183846,
+"pose": {
+"rotation": {
+"radians": -2.9650423240787243
+},
+"translation": {
+"x": 5.652632996293115,
+"y": 4.490636183993351
+}
+},
+"time": 1.7913974507258665,
+"velocity": 1.7913974507258672
+},
+{
+"acceleration": 1.0000000000000024,
+"curvature": 0.02536890538189909,
+"pose": {
+"rotation": {
+"radians": -2.9622829225596625
+},
+"translation": {
+"x": 5.561120538141468,
+"y": 4.47417293329643
+}
+},
+"time": 1.842571009826413,
+"velocity": 1.8425710098264134
+},
+{
+"acceleration": 1.0000000000000024,
+"curvature": 0.017197625277016158,
+"pose": {
+"rotation": {
+"radians": -2.960285583950223
+},
+"translation": {
+"x": 5.468591444900579,
+"y": 4.45729985471319
+}
+},
+"time": 1.8929283739721758,
+"velocity": 1.8929283739721765
+},
+{
+"acceleration": 1.0,
+"curvature": 0.00943143720793463,
+"pose": {
+"rotation": {
+"radians": -2.9590253952308134
+},
+"translation": {
+"x": 5.3752838550838735,
+"y": 4.440127992292334
+}
+},
+"time": 1.9424023573406726,
+"velocity": 1.9424023573406735
+},
+{
+"acceleration": 0.9999999999999977,
+"curvature": 0.0019156420552107058,
+"pose": {
+"rotation": {
+"radians": -2.9584856115029057
+},
+"translation": {
+"x": 5.28144034228373,
+"y": 4.4227703263022695
+}
+},
+"time": 1.990928799864229,
+"velocity": 1.99092879986423
+},
+{
+"acceleration": -0.5907384964225079,
+"curvature": -0.005495346094425059,
+"pose": {
+"rotation": {
+"radians": -2.958657333421031
+},
+"translation": {
+"x": 5.187306464987833,
+"y": 4.405341099648527
+}
+},
+"time": 2.0384467448037187,
+"velocity": 2.0384467448037196
+},
+{
+"acceleration": -1.0,
+"curvature": -0.012942864471820118,
+"pose": {
+"rotation": {
+"radians": -2.9595393197514297
+},
+"translation": {
+"x": 5.093129316395519,
+"y": 4.3879551442912
+}
+},
+"time": 2.085752117456783,
+"velocity": 2.010501640089942
+},
+{
+"acceleration": -0.9999999999999977,
+"curvature": -0.02056865689236139,
+"pose": {
+"rotation": {
+"radians": -2.9611379327276395
+},
+"translation": {
+"x": 4.999156074234137,
+"y": 4.370727207662373
+}
+},
+"time": 2.1338475569368542,
+"velocity": 1.9624062006098706
+},
+{
+"acceleration": -1.0,
+"curvature": -0.028519509234597767,
+"pose": {
+"rotation": {
+"radians": -2.963467214454633
+},
+"translation": {
+"x": 4.905632550575397,
+"y": 4.353771279083553
+}
+},
+"time": 2.1828949900049506,
+"velocity": 1.9133587675417743
+},
+{
+"acceleration": -1.0,
+"curvature": -0.03695204986439015,
+"pose": {
+"rotation": {
+"radians": -2.9665490932308898
+},
+"translation": {
+"x": 4.81280174165172,
+"y": 4.3371999161831
+}
+},
+"time": 2.2328307846931734,
+"velocity": 1.8634229728535514
+},
+{
+"acceleration": -1.0000000000000024,
+"curvature": -0.04603789770824941,
+"pose": {
+"rotation": {
+"radians": -2.9704137188760846
+},
+"translation": {
+"x": 4.720902377672591,
+"y": 4.321123571313661
+}
+},
+"time": 2.2835884996485083,
+"velocity": 1.8126652578982168
+},
+{
+"acceleration": -0.9999999999999976,
+"curvature": -0.055969338892937814,
+"pose": {
+"rotation": {
+"radians": -2.9750999254745647
+},
+"translation": {
+"x": 4.630167472640914,
+"y": 4.3056499179696
+}
+},
+"time": 2.3350991335193827,
+"velocity": 1.7611546240273421
+},
+{
+"acceleration": -1.0,
+"curvature": -0.06696570648279973,
+"pose": {
+"rotation": {
+"radians": -2.9806558177207405
+},
+"translation": {
+"x": 4.5408228741693595,
+"y": 4.2908831772044245
+}
+},
+"time": 2.3872914314888956,
+"velocity": 1.7089623260578295
+},
+{
+"acceleration": -1.0,
+"curvature": -0.07928061827859845,
+"pose": {
+"rotation": {
+"radians": -2.987139472361781
+},
+"translation": {
+"x": 4.45308581329672,
+"y": 4.276923444048224
+}
+},
+"time": 2.440092268850438,
+"velocity": 1.656161488696287
+},
+{
+"acceleration": -0.9999999999999973,
+"curvature": -0.09321018023134439,
+"pose": {
+"rotation": {
+"radians": -2.99461973773569
+},
+"translation": {
+"x": 4.3671634543042614,
+"y": 4.263866013925099
+}
+},
+"time": 2.493427136826554,
+"velocity": 1.6028266207201707
+},
+{
+"acceleration": -0.9999999999999973,
+"curvature": -0.10910216327385203,
+"pose": {
+"rotation": {
+"radians": -3.0031771001370617
+},
+"translation": {
+"x": 4.283251444532073,
+"y": 4.251800709070589
+}
+},
+"time": 2.5472207626561962,
+"velocity": 1.5490329948905288
+},
+{
+"acceleration": -0.9999999999999972,
+"curvature": -0.12736596915457302,
+"pose": {
+"rotation": {
+"radians": -3.0129045628568396
+},
+"translation": {
+"x": 4.201532464195421,
+"y": 4.240811204949109
+}
+},
+"time": 2.6013979047533122,
+"velocity": 1.494855852793413
+},
+{
+"acceleration": -1.0,
+"curvature": -0.1484828518979495,
+"pose": {
+"rotation": {
+"radians": -3.0239084481545406
+},
+"translation": {
+"x": 4.1221747762011,
+"y": 4.230974356671376
+}
+},
+"time": 2.6558843750382577,
+"velocity": 1.4403693825084676
+},
+{
+"acceleration": -1.0,
+"curvature": -0.1730152556816665,
+"pose": {
+"rotation": {
+"radians": -3.036308978425141
+},
+"translation": {
+"x": 4.045330775963787,
+"y": 4.222359525411843
+}
+},
+"time": 2.7106083551142848,
+"velocity": 1.3856454024324405
+},
+{
+"acceleration": -1.0,
+"curvature": -0.20161311696298229,
+"pose": {
+"rotation": {
+"radians": -3.050240412715741
+},
+"translation": {
+"x": 3.971135541222391,
+"y": 4.21502790482613
+}
+},
+"time": 2.765502091820772,
+"velocity": 1.330751665725953
+},
+{
+"acceleration": -1.0,
+"curvature": -0.23501334520635073,
+"pose": {
+"rotation": {
+"radians": -3.0658503986852508
+},
+"translation": {
+"x": 3.8997053818564047,
+"y": 4.209031847468453
+}
+},
+"time": 2.820504082262278,
+"velocity": 1.275749675284447
+},
+{
+"acceleration": -1.0000000000000018,
+"curvature": -0.2740261654922649,
+"pose": {
+"rotation": {
+"radians": -3.0832980366298806
+},
+"translation": {
+"x": 3.8311363897022597,
+"y": 4.20441419120906
+}
+},
+"time": 2.8755618907593004,
+"velocity": 1.2206918667874245
+},
+{
+"acceleration": -0.9999999999999982,
+"curvature": -0.3194982756810784,
+"pose": {
+"rotation": {
+"radians": -3.1027499303529567
+},
+"translation": {
+"x": 3.7655029883696747,
+"y": 4.201207585651656
+}
+},
+"time": 2.9306357834886034,
+"velocity": 1.1656179740581212
+},
+{
+"acceleration": -1.0,
+"curvature": -0.3722376471963132,
+"pose": {
+"rotation": {
+"radians": -3.1243732142188465
+},
+"translation": {
+"x": 3.702856483058013,
+"y": 4.199433818550841
+}
+},
+"time": 2.985703426127596,
+"velocity": 1.110550331419129
+},
+{
+"acceleration": -1.0,
+"curvature": -0.5016590681055971,
+"pose": {
+"rotation": {
+"radians": 3.10845429675366
+},
+"translation": {
+"x": 3.5866050881412077,
+"y": 4.200213599996414
+}
+},
+"time": 3.095846831197659,
+"velocity": 1.0004069263490658
+},
+{
+"acceleration": -1.0,
+"curvature": -0.6604348922884946,
+"pose": {
+"rotation": {
+"radians": 3.0480641246132625
+},
+"translation": {
+"x": 3.4822751713609237,
+"y": 4.206685004462784
+}
+},
+"time": 3.206448632783195,
+"velocity": 0.8898051247635298
+},
+{
+"acceleration": -1.0,
+"curvature": -0.8263119869696386,
+"pose": {
+"rotation": {
+"radians": 2.9783973732100404
+},
+"translation": {
+"x": 3.3892969928070222,
+"y": 4.218562601996826
+}
+},
+"time": 3.3188956503943015,
+"velocity": 0.7773581071524234
+},
+{
+"acceleration": -1.0,
+"curvature": -0.938420346976681,
+"pose": {
+"rotation": {
+"radians": 2.9033011960514283
+},
+"translation": {
+"x": 3.306591700373408,
+"y": 4.2353225091330815
+}
+},
+"time": 3.4363197596968202,
+"velocity": 0.6599339978499048
+},
+{
+"acceleration": -1.0000000000000002,
+"curvature": -0.8924026125215841,
+"pose": {
+"rotation": {
+"radians": 2.831426518084741
+},
+"translation": {
+"x": 3.2325249238813107,
+"y": 4.256180834251557
+}
+},
+"time": 3.56557742481103,
+"velocity": 0.5306763327356951
+},
+{
+"acceleration": -0.9999999999999998,
+"curvature": -0.5832724205082608,
+"pose": {
+"rotation": {
+"radians": 2.7766044122760505
+},
+"translation": {
+"x": 3.1648603692025192,
+"y": 4.280072122935529
+}
+},
+"time": 3.7246352381447707,
+"velocity": 0.3716185194019546
+},
+{
+"acceleration": -0.9999999999999998,
+"curvature": -5.466050154457545e-15,
+"pose": {
+"rotation": {
+"radians": 2.755086348475869
+},
+"translation": {
+"x": 3.1007134123826825,
+"y": 4.305627803329346
+}
+},
+"time": 4.096253757546726,
+"velocity": 0.0
+}
+]
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index c72215c..ccd9fe0 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -298,6 +298,11 @@ private void createAutoChooser() {
                                 new FirePoseVision()))));
 
         autoChooser.addOption("Drive Straight Trajectory", new DriveTrajectory("StraightLine"));
+        autoChooser.addOption("Linear Test Trajectory", new DriveTrajectory("LinearTest"));
+        autoChooser.addOption("Linear Auto Test Trajectory", new PKParallelCommandGroup(new TurretVisionAlign(),
+                new PKSequentialCommandGroup(new PKParallelCommandGroup(new IntakeIngestTimed(5.0),
+                        new PKSequentialCommandGroup(new WaitCommand(1.0), new DriveTrajectory("LinearTest")),
+                        new FirePoseVision()))));
 
         SmartDashboard.putData("Auto Mode", autoChooser);
     }
diff --git a/src/main/java/frc/robot/commands/drive/DriveTrajectory.java b/src/main/java/frc/robot/commands/drive/DriveTrajectory.java
index 9e5c625..14b0d8c 100644
--- a/src/main/java/frc/robot/commands/drive/DriveTrajectory.java
+++ b/src/main/java/frc/robot/commands/drive/DriveTrajectory.java
@@ -46,7 +46,7 @@ public DriveTrajectory(String pathName) {
             Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON);
             trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
         } catch (IOException ex) {
-            DriverStation.reportError("Unable to open trajectory" + trajectoryJSON, ex.getStackTrace());
+            DriverStation.reportError("Unable to open trajectory " + trajectoryJSON, ex.getStackTrace());
             return;
         }
 
diff --git a/src/main/java/frc/robot/config/BuildVersionInfo.java b/src/main/java/frc/robot/config/BuildVersionInfo.java
index 3469492..083ca78 100644
--- a/src/main/java/frc/robot/config/BuildVersionInfo.java
+++ b/src/main/java/frc/robot/config/BuildVersionInfo.java
@@ -19,6 +19,6 @@
  **/
 class BuildVersionInfo {
     public static final String programmer = "Caleb Hagner";
-    public static final String commitSHA = "bb6a93a";
-    public static final String timeStamp = "20220320-170850";
+    public static final String commitSHA = "3856698";
+    public static final String timeStamp = "20220320-171732";
 }
diff --git a/src/main/java/frc/robot/sensors/gyro/GyroSensor.java b/src/main/java/frc/robot/sensors/gyro/GyroSensor.java
index daec360..6fa7ddf 100644
--- a/src/main/java/frc/robot/sensors/gyro/GyroSensor.java
+++ b/src/main/java/frc/robot/sensors/gyro/GyroSensor.java
@@ -13,7 +13,7 @@
 
 /**
  * Provides implementation of <code>IGyroSensor</code> for the
- * <i>Suitcase-Bot</i> which is based on the navX-MXP sensor.
+ * <i>Real-Bot</i> which is based on the navX-MXP sensor.
  */
 class GyroSensor extends SuitcaseGyroSensor {
 
diff --git a/src/main/java/frc/robot/sensors/gyro/SuitcaseGyroSensor.java b/src/main/java/frc/robot/sensors/gyro/SuitcaseGyroSensor.java
index 21e4ba4..763e5e0 100644
--- a/src/main/java/frc/robot/sensors/gyro/SuitcaseGyroSensor.java
+++ b/src/main/java/frc/robot/sensors/gyro/SuitcaseGyroSensor.java
@@ -43,12 +43,12 @@ public double getPitch() {
 
     @Override
     public double getYaw() {
-        return mySensor.ahrs.getYaw();
+        return -mySensor.ahrs.getYaw();
     }
 
     @Override
     public double getAngle() {
-        return mySensor.ahrs.getAngle();
+        return -mySensor.ahrs.getAngle();
     }
 
 }
diff --git a/src/main/java/frc/robot/sensors/vision/LimelightVision.java b/src/main/java/frc/robot/sensors/vision/LimelightVision.java
index d2bcc0b..c886b8c 100644
--- a/src/main/java/frc/robot/sensors/vision/LimelightVision.java
+++ b/src/main/java/frc/robot/sensors/vision/LimelightVision.java
@@ -8,7 +8,6 @@
 
 package frc.robot.sensors.vision;
 
-
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -16,7 +15,6 @@
 import riolog.PKLogger;
 import riolog.RioLogger;
 
-
 /**
  * Wrapper for the Limelight Vision Sensor to provide ease when switching from
  * Limelight and RPi vision. Additionally, provides access to the values sent
@@ -42,7 +40,7 @@ class LimelightVision {
 
     protected boolean isLocked() {
         return (table.getEntry("tv").getDouble(0.0) == 1)
-                && (Math.abs((table.getEntry("tx").getDouble(0.15))) < 0.15);
+                && (Math.abs((table.getEntry("tx").getDouble(0.25))) < 0.25);
     }
 
     protected boolean isEnabled() {
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
index 588be71..b427c3f 100644
--- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
@@ -50,15 +50,18 @@ class DriveSubsystem extends BaseDriveSubsystem {
     // private static final double v = 2.1836; // VoltSeconds Per Meter
     // private static final double a = 0.44028; // VoltSecondsSquared Per Meter
     // private static final double p = 2.3834; // Drive Velocity
-    private static final double s = 6; // Volts // 0.1 // 0.224 // 0.324 // 0.524 // 0.75
-    private static final double v = 5; // VoltSeconds Per Meter // 0.01 // 0.99 // 1.25 // 2.5 // 2.75
-    private static final double a = 0.1; // VoltSecondsSquared Per Meter // 0.2 // 0.5
-    private static final double p = 0.01; // Drive Velocity // 0.03 // 0.00025 // 0.0003 // 0.0005 // 0.00075 // 0.001
-                                          // // 0.005
-                                          // // 0.003
-    private static final double trackWidth = 0.56411; // Meters // 0.6731 * 1.7
-    private static final double ramseteB = 3; // Proportional constant that makes larger values give more aggressive
-                                              // convergence (b > 0)
+    private static final double s = 0.20814; // Volts // 0.1 // 0.224 // 0.324 // 0.524 // 0.75 // 0.20814 // 0.5
+    private static final double v = 1.25; // VoltSeconds Per Meter // 0.01 // 0.99 // 1.25 // 2.5 // 2.75
+    private static final double a = 0.21697; // VoltSecondsSquared Per Meter // 0.2 // 0.5
+    private static final double p = 0.004; // Drive Velocity // 0.03 // 0.00025 // 0.0003 // 0.0005 // 0.00075 //
+                                           // 0.001
+    // // 0.005
+    // // 0.003
+    // 0.01
+    // 0.0048044 // 0.008
+    private static final double trackWidth = 1.2406; // Meters // 0.6731 * 1.7
+    private static final double ramseteB = 1.5; // Proportional constant that makes larger values give more aggressive
+                                                // convergence (b > 0)
     private static final double ramseteZeta = 0.7; // Larger values provide more damping (0 < zeta < 1)
     private static final double maxSpeed = 5; // Meters Per Second
     private static final double maxAcceleration = 0.1; // Meters Per Second Squared
@@ -140,7 +143,7 @@ class DriveSubsystem extends BaseDriveSubsystem {
         leftEncoder = leftFrontMotor.getEncoder();
         checkError(leftEncoder.setPositionConversionFactor(Math.PI * wheelDiameter / gearRatio),
                 "L Encoder setting position conversion factor");
-        checkError(leftEncoder.setVelocityConversionFactor(Math.PI * wheelDiameter / gearRatio / 81 / 60),
+        checkError(leftEncoder.setVelocityConversionFactor(Math.PI * wheelDiameter / gearRatio / 60),
                 "L Encoder setting velocity conversion factor");
         rightEncoder = rightFrontMotor.getEncoder();
         checkError(rightEncoder.setPositionConversionFactor(Math.PI * wheelDiameter / gearRatio),
@@ -164,7 +167,7 @@ class DriveSubsystem extends BaseDriveSubsystem {
         driveOdometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(nav.getAngle()));
 
         autoVoltageConstraint = new DifferentialDriveVoltageConstraint(new SimpleMotorFeedforward(s, v, a),
-                driveKinematics, 4);
+                driveKinematics, 8);
 
         trajectoryConfig = new TrajectoryConfig(maxSpeed, maxAcceleration).setKinematics(driveKinematics)
                 .addConstraint(autoVoltageConstraint);
@@ -294,7 +297,9 @@ public RamseteCommand followTrajectory(Trajectory trajectory) {
      */
     private void tankDriveVolts(final double leftVolts, final double rightVolts) {
         leftFrontMotor.setVoltage(leftVolts * (leftReversed ? -1 : 1)); // 1 // 0.4 // 0.6
+        SmartDashboard.putNumber("Trajectory Left Voltage", leftVolts);
         rightFrontMotor.setVoltage(rightVolts * (rightReversed ? -1 : 1)); // 1 // 0.4 // 0.6
+        SmartDashboard.putNumber("Trajectory Right Voltage", rightVolts);
     }
 
     /**