From 9bf82381371ad260fa2f64b7cc157fd033bf1a2b Mon Sep 17 00:00:00 2001
From: PJ Reiniger <pj.reiniger@gmail.com>
Date: Sat, 4 Nov 2023 21:55:54 -0400
Subject: [PATCH 1/4] Delete pathweaver stuff

---
 PathWeaver/Paths/DriveOffTarmac               |    3 -
 PathWeaver/Paths/FourBallAutoPartOne          |    3 -
 PathWeaver/Paths/FourBallAutoPartTwo          |    3 -
 PathWeaver/Paths/SCurve                       |    4 -
 PathWeaver/Paths/Shoot                        |    3 -
 PathWeaver/Paths/StraightLine                 |    3 -
 PathWeaver/Paths/TwoBallAuto                  |    3 -
 PathWeaver/pathweaver.json                    |    9 -
 .../output/DriveOffTarmac.wpilib.json         |  422 ------
 .../output/FourBallAutoPartOne.wpilib.json    |  332 -----
 .../output/FourBallAutoPartTwo.wpilib.json    |  827 ------------
 .../Trajectories/output/SCurve.wpilib.json    | 1157 -----------------
 .../Trajectories/output/Shoot.wpilib.json     |  692 ----------
 .../output/StraightLine.wpilib.json           | 1127 ----------------
 .../output/TwoBallAuto.wpilib.json            |  302 -----
 .../autos/{curveTest.java => CurveTest.java}  |    4 +-
 .../autos/{lineTest.java => LineTest.java}    |    4 +-
 .../org/littletonrobotics/FieldConstants.java |  198 ---
 18 files changed, 4 insertions(+), 5092 deletions(-)
 delete mode 100644 PathWeaver/Paths/DriveOffTarmac
 delete mode 100644 PathWeaver/Paths/FourBallAutoPartOne
 delete mode 100644 PathWeaver/Paths/FourBallAutoPartTwo
 delete mode 100644 PathWeaver/Paths/SCurve
 delete mode 100644 PathWeaver/Paths/Shoot
 delete mode 100644 PathWeaver/Paths/StraightLine
 delete mode 100644 PathWeaver/Paths/TwoBallAuto
 delete mode 100644 PathWeaver/pathweaver.json
 delete mode 100644 src/main/deploy/Trajectories/output/DriveOffTarmac.wpilib.json
 delete mode 100644 src/main/deploy/Trajectories/output/FourBallAutoPartOne.wpilib.json
 delete mode 100644 src/main/deploy/Trajectories/output/FourBallAutoPartTwo.wpilib.json
 delete mode 100644 src/main/deploy/Trajectories/output/SCurve.wpilib.json
 delete mode 100644 src/main/deploy/Trajectories/output/Shoot.wpilib.json
 delete mode 100644 src/main/deploy/Trajectories/output/StraightLine.wpilib.json
 delete mode 100644 src/main/deploy/Trajectories/output/TwoBallAuto.wpilib.json
 rename src/main/java/frc/robot/autos/{curveTest.java => CurveTest.java} (83%)
 rename src/main/java/frc/robot/autos/{lineTest.java => LineTest.java} (83%)
 delete mode 100644 src/main/java/org/littletonrobotics/FieldConstants.java

diff --git a/PathWeaver/Paths/DriveOffTarmac b/PathWeaver/Paths/DriveOffTarmac
deleted file mode 100644
index 71baaba..0000000
--- a/PathWeaver/Paths/DriveOffTarmac
+++ /dev/null
@@ -1,3 +0,0 @@
-X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
-7.177180893971274,-5.772035605891501,-1.9874058629585587,-2.0713807585765256,true,false,
-5.581657877229897,-7.465529334187175,-0.7417782446253778,-0.7417782446253778,true,false,
diff --git a/PathWeaver/Paths/FourBallAutoPartOne b/PathWeaver/Paths/FourBallAutoPartOne
deleted file mode 100644
index 8a011f7..0000000
--- a/PathWeaver/Paths/FourBallAutoPartOne
+++ /dev/null
@@ -1,3 +0,0 @@
-X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
-6.519377544963864,-5.450131839355961,-1.3575941458238043,-0.8957322199249838,true,false,
-5.035821055713109,-6.359859875217272,-0.2939121346628859,-0.1819456071722616,true,false,
diff --git a/PathWeaver/Paths/FourBallAutoPartTwo b/PathWeaver/Paths/FourBallAutoPartTwo
deleted file mode 100644
index c30fcfd..0000000
--- a/PathWeaver/Paths/FourBallAutoPartTwo
+++ /dev/null
@@ -1,3 +0,0 @@
-X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
-5.036,-6.3595999999999995,-0.294,-0.182,false,false,
-1.2149633050955986,-7.03165904016101,-0.9517154836702958,-1.0076987474156072,true,false,
diff --git a/PathWeaver/Paths/SCurve b/PathWeaver/Paths/SCurve
deleted file mode 100644
index 5f458d1..0000000
--- a/PathWeaver/Paths/SCurve
+++ /dev/null
@@ -1,4 +0,0 @@
-X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
-0.0,0.0,2.0574,0.0,true,false,
-2.1246913409569106,-1.181407978775958,1.6548812506999884,-0.2290022529107028,false,false,
-4.993833607904126,-0.10373015167871144,0.8957322199249838,-0.13995815936327874,true,false,
diff --git a/PathWeaver/Paths/Shoot b/PathWeaver/Paths/Shoot
deleted file mode 100644
index 8c9268d..0000000
--- a/PathWeaver/Paths/Shoot
+++ /dev/null
@@ -1,3 +0,0 @@
-X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
-1.215,-7.031599999999999,-0.4758944367395491,-0.18200464733327415,true,true,
-5.203770846949044,-5.506115103101271,-1.525543937059739,-0.6438075330710822,true,true,
diff --git a/PathWeaver/Paths/StraightLine b/PathWeaver/Paths/StraightLine
deleted file mode 100644
index b1627d2..0000000
--- a/PathWeaver/Paths/StraightLine
+++ /dev/null
@@ -1,3 +0,0 @@
-X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
-0.09529803018936864,-0.005759440124416262,3.540941431890953,-0.013995815936327875,true,false,
-6.095,-0.005599999999999383,0.6857949808800665,0.027991631872655742,true,false,StraightLine
diff --git a/PathWeaver/Paths/TwoBallAuto b/PathWeaver/Paths/TwoBallAuto
deleted file mode 100644
index c9058ef..0000000
--- a/PathWeaver/Paths/TwoBallAuto
+++ /dev/null
@@ -1,3 +0,0 @@
-X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
-7.6250470039337666,-6.0379561086817315,0.0,-0.7557740605617047,true,false,
-7.6250470039337666,-7.717454021041076,0.0,-4.1148,true,false,
diff --git a/PathWeaver/pathweaver.json b/PathWeaver/pathweaver.json
deleted file mode 100644
index 2a920e9..0000000
--- a/PathWeaver/pathweaver.json
+++ /dev/null
@@ -1,9 +0,0 @@
-{
-  "lengthUnit": "Meter",
-  "exportUnit": "Always Meters",
-  "maxVelocity": 2.5,
-  "maxAcceleration": 2.5,
-  "trackWidth": 0.6858,
-  "gameName": "Rapid React",
-  "outputDir": "../src/main/java/frc/robot/Trajectories"
-}
\ No newline at end of file
diff --git a/src/main/deploy/Trajectories/output/DriveOffTarmac.wpilib.json b/src/main/deploy/Trajectories/output/DriveOffTarmac.wpilib.json
deleted file mode 100644
index 5bdc1ca..0000000
--- a/src/main/deploy/Trajectories/output/DriveOffTarmac.wpilib.json
+++ /dev/null
@@ -1,422 +0,0 @@
-[
-{
-"acceleration": 2.4999999999999996,
-"curvature": 0.0,
-"pose": {
-"rotation": {
-"radians": -2.335507786412501
-},
-"translation": {
-"x": 7.177180893971274,
-"y": 2.4575643941084984
-}
-},
-"time": 0.0,
-"velocity": 0.0
-},
-{
-"acceleration": 2.5,
-"curvature": 0.006104343070860575,
-"pose": {
-"rotation": {
-"radians": -2.335223427861308
-},
-"translation": {
-"x": 7.115044667993659,
-"y": 2.3927901814110824
-}
-},
-"time": 0.2679680822058686,
-"velocity": 0.6699202055146714
-},
-{
-"acceleration": 2.4999999999999996,
-"curvature": 0.010827413883599288,
-"pose": {
-"rotation": {
-"radians": -2.334450789487766
-},
-"translation": {
-"x": 7.052750370666024,
-"y": 2.3277811161563964
-}
-},
-"time": 0.3792583849746827,
-"velocity": 0.9481459624367067
-},
-{
-"acceleration": 2.5000000000000004,
-"curvature": 0.014241802863867078,
-"pose": {
-"rotation": {
-"radians": -2.3333067974691097
-},
-"translation": {
-"x": 6.9901945851100376,
-"y": 2.2623725366908864
-}
-},
-"time": 0.4650188008230546,
-"velocity": 1.1625470020576363
-},
-{
-"acceleration": 2.500000000000001,
-"curvature": 0.01649980581226433,
-"pose": {
-"rotation": {
-"radians": -2.3318986583630164
-},
-"translation": {
-"x": 6.927329012742952,
-"y": 2.196470756427413
-}
-},
-"time": 0.5376844296007,
-"velocity": 1.3442110740017499
-},
-{
-"acceleration": 2.5000000000000013,
-"curvature": 0.017787158950744005,
-"pose": {
-"rotation": {
-"radians": -2.3303204018413175
-},
-"translation": {
-"x": 6.864155518036715,
-"y": 2.130046907333851
-}
-},
-"time": 0.6020290093586323,
-"velocity": 1.505072523396581
-},
-{
-"acceleration": 2.499999999999997,
-"curvature": 0.018290399413559772,
-"pose": {
-"rotation": {
-"radians": -2.328651778226205
-},
-"translation": {
-"x": 6.800721173277091,
-"y": 2.063130783421683
-}
-},
-"time": 0.6604563486498747,
-"velocity": 1.651140871624687
-},
-{
-"acceleration": 2.5,
-"curvature": 0.018177179938918035,
-"pose": {
-"rotation": {
-"radians": -2.326958760676729
-},
-"translation": {
-"x": 6.737113303322766,
-"y": 1.9958046842346002
-}
-},
-"time": 0.7143527739126655,
-"velocity": 1.785881934781664
-},
-{
-"acceleration": 2.5000000000000013,
-"curvature": 0.017586585237037763,
-"pose": {
-"rotation": {
-"radians": -2.325295000569299
-},
-"translation": {
-"x": 6.673454530364473,
-"y": 1.9281972583370965
-}
-},
-"time": 0.7645841024177157,
-"velocity": 1.9114602560442895
-},
-{
-"acceleration": 2.500000000000002,
-"curvature": 0.016626064678723306,
-"pose": {
-"rotation": {
-"radians": -2.323703746346959
-},
-"translation": {
-"x": 6.609897818684101,
-"y": 1.8604773468030702
-}
-},
-"time": 0.81171882812006,
-"velocity": 2.0292970703001503
-},
-{
-"acceleration": 2.4999999999999964,
-"curvature": 0.015372090047667335,
-"pose": {
-"rotation": {
-"radians": -2.322219898541515
-},
-"translation": {
-"x": 6.5466215194138115,
-"y": 1.7928478267044152
-}
-},
-"time": 0.856142455603862,
-"velocity": 2.1403561390096555
-},
-{
-"acceleration": 2.499999999999993,
-"curvature": 0.013872423589397522,
-"pose": {
-"rotation": {
-"radians": -2.3208720075352947
-},
-"translation": {
-"x": 6.483824415295151,
-"y": 1.7255394545996232
-}
-},
-"time": 0.8981218769282143,
-"velocity": 2.245304692320536
-},
-{
-"acceleration": 1.056830603209502,
-"curvature": 0.01214860118772051,
-"pose": {
-"rotation": {
-"radians": -2.3196841181679013
-},
-"translation": {
-"x": 6.421720765438169,
-"y": 1.6588047100223795
-}
-},
-"time": 0.9378443238816694,
-"velocity": 2.3446108097041733
-},
-{
-"acceleration": -2.499999999999994,
-"curvature": 0.010197773061180627,
-"pose": {
-"rotation": {
-"radians": -2.318677429701917
-},
-"translation": {
-"x": 6.360535350080534,
-"y": 1.5929116389701594
-}
-},
-"time": 0.9758700932153739,
-"velocity": 2.3847976064466176
-},
-{
-"acceleration": -2.4999999999999947,
-"curvature": 0.007993390662082387,
-"pose": {
-"rotation": {
-"radians": -2.317871777972008
-},
-"translation": {
-"x": 6.300498515346641,
-"y": 1.5281376973928253
-}
-},
-"time": 1.0136521003326344,
-"velocity": 2.2903425886534667
-},
-{
-"acceleration": -2.4999999999999973,
-"curvature": 0.005484423664694723,
-"pose": {
-"rotation": {
-"radians": -2.3172869656917108
-},
-"translation": {
-"x": 6.241841218006737,
-"y": 1.4647635946812265
-}
-},
-"time": 1.0521649930667312,
-"velocity": 2.194060356818225
-},
-{
-"acceleration": -2.4999999999999947,
-"curvature": 0.002592892259422986,
-"pose": {
-"rotation": {
-"radians": -2.3169439718755473
-},
-"translation": {
-"x": 6.1847900702360254,
-"y": 1.4030671371557943
-}
-},
-"time": 1.0913387988056193,
-"velocity": 2.0961258424710048
-},
-{
-"acceleration": -2.5,
-"curvature": -0.000790424376730214,
-"pose": {
-"rotation": {
-"radians": -2.316866064369632
-},
-"translation": {
-"x": 6.12956238437379,
-"y": 1.3433170715551395
-}
-},
-"time": 1.1310980581058403,
-"velocity": 1.9967276942204526
-},
-{
-"acceleration": -2.5000000000000013,
-"curvature": -0.004812098237513517,
-"pose": {
-"rotation": {
-"radians": -2.317079819395371
-},
-"translation": {
-"x": 6.076361217682502,
-"y": 1.285766928524649
-}
-},
-"time": 1.171363956943848,
-"velocity": 1.8960629471254333
-},
-{
-"acceleration": -2.4999999999999987,
-"curvature": -0.009663603940485356,
-"pose": {
-"rotation": {
-"radians": -2.3176160132480543
-},
-"translation": {
-"x": 6.0253704171069415,
-"y": 1.230648866105085
-}
-},
-"time": 1.2120572043677864,
-"velocity": 1.7943298285655873
-},
-{
-"acceleration": -2.5,
-"curvature": -0.01558903677668317,
-"pose": {
-"rotation": {
-"radians": -2.31851028215942
-},
-"translation": {
-"x": 5.976749664033308,
-"y": 1.1781675132211813
-}
-},
-"time": 1.253102016950802,
-"velocity": 1.6917177971080481
-},
-{
-"acceleration": -2.5,
-"curvature": -0.02288973684071636,
-"pose": {
-"rotation": {
-"radians": -2.3198033261680555
-},
-"translation": {
-"x": 5.930629519048337,
-"y": 1.1284938131702402
-}
-},
-"time": 1.2944317538848187,
-"velocity": 1.5883934547730063
-},
-{
-"acceleration": -2.500000000000001,
-"curvature": -0.03191846075993177,
-"pose": {
-"rotation": {
-"radians": -2.3215402284551887
-},
-"translation": {
-"x": 5.887106466698415,
-"y": 1.0817588671107305
-}
-},
-"time": 1.3359970420592895,
-"velocity": 1.4844802343368295
-},
-{
-"acceleration": -2.4999999999999987,
-"curvature": -0.056583884347772805,
-"pose": {
-"rotation": {
-"radians": -2.3265309373962713
-},
-"translation": {
-"x": 5.808037466442201,
-"y": 0.9973934918372898
-}
-},
-"time": 1.41980082953267,
-"velocity": 1.274970765653378
-},
-{
-"acceleration": -2.4999999999999996,
-"curvature": -0.0904352530840693,
-"pose": {
-"rotation": {
-"radians": -2.333751711665249
-},
-"translation": {
-"x": 5.739444719675131,
-"y": 0.925089406458623
-}
-},
-"time": 1.5051043793824759,
-"velocity": 1.0617118910288637
-},
-{
-"acceleration": -2.5000000000000004,
-"curvature": -0.12235540370049369,
-"pose": {
-"rotation": {
-"radians": -2.342881139191291
-},
-"translation": {
-"x": 5.68036793074543,
-"y": 0.8638331390813772
-}
-},
-"time": 1.594714036771536,
-"velocity": 0.8376877475562132
-},
-{
-"acceleration": -2.5000000000000004,
-"curvature": -0.11018646520307893,
-"pose": {
-"rotation": {
-"radians": -2.351909731519342
-},
-"translation": {
-"x": 5.628825884230762,
-"y": 0.8113827184961933
-}
-},
-"time": 1.6986051077385906,
-"velocity": 0.5779600701385765
-},
-{
-"acceleration": -2.5000000000000004,
-"curvature": 1.27836554791396e-13,
-"pose": {
-"rotation": {
-"radians": -2.356194490192309
-},
-"translation": {
-"x": 5.581657877229901,
-"y": 0.7640706658128185
-}
-},
-"time": 1.9297891357940211,
-"velocity": 0.0
-}
-]
\ No newline at end of file
diff --git a/src/main/deploy/Trajectories/output/FourBallAutoPartOne.wpilib.json b/src/main/deploy/Trajectories/output/FourBallAutoPartOne.wpilib.json
deleted file mode 100644
index 880329e..0000000
--- a/src/main/deploy/Trajectories/output/FourBallAutoPartOne.wpilib.json
+++ /dev/null
@@ -1,332 +0,0 @@
-[
-{
-"acceleration": 2.5,
-"curvature": 0.0,
-"pose": {
-"rotation": {
-"radians": -2.558363283480902
-},
-"translation": {
-"x": 6.519377544963864,
-"y": 2.779468160644039
-}
-},
-"time": 0.0,
-"velocity": 0.0
-},
-{
-"acceleration": 2.5000000000000013,
-"curvature": -0.05585830844766544,
-"pose": {
-"rotation": {
-"radians": -2.561641525689503
-},
-"translation": {
-"x": 6.433320313640877,
-"y": 2.7228309919661937
-}
-},
-"time": 0.28708522084312293,
-"velocity": 0.7177130521078073
-},
-{
-"acceleration": 2.5000000000000004,
-"curvature": -0.06949818596684898,
-"pose": {
-"rotation": {
-"radians": -2.56886320671592
-},
-"translation": {
-"x": 6.341066715428874,
-"y": 2.662855012011714
-}
-},
-"time": 0.4128516017379929,
-"velocity": 1.0321290043449824
-},
-{
-"acceleration": 2.5000000000000018,
-"curvature": -0.061000411805305965,
-"pose": {
-"rotation": {
-"radians": -2.5768623320951756
-},
-"translation": {
-"x": 6.239102473470949,
-"y": 2.597694508672647
-}
-},
-"time": 0.5169639513617809,
-"velocity": 1.2924098784044524
-},
-{
-"acceleration": 2.499999999999996,
-"curvature": -0.05436156234028765,
-"pose": {
-"rotation": {
-"radians": -2.580615220522637
-},
-"translation": {
-"x": 6.184080777556258,
-"y": 2.5629823184191967
-}
-},
-"time": 0.5650635125184222,
-"velocity": 1.4126587812960558
-},
-{
-"acceleration": 2.499999999999998,
-"curvature": -0.04773903891000544,
-"pose": {
-"rotation": {
-"radians": -2.584083722344202
-},
-"translation": {
-"x": 6.126428611204658,
-"y": 2.526901087707122
-}
-},
-"time": 0.611315258958773,
-"velocity": 1.5282881473969325
-},
-{
-"acceleration": 2.500000000000003,
-"curvature": -0.04164096770819926,
-"pose": {
-"rotation": {
-"radians": -2.587239866847625
-},
-"translation": {
-"x": 6.066340014641799,
-"y": 2.4895713164825732
-}
-},
-"time": 0.6559713153118392,
-"velocity": 1.6399282882795978
-},
-{
-"acceleration": 2.5000000000000013,
-"curvature": -0.036264499079920354,
-"pose": {
-"rotation": {
-"radians": -2.5900827170936753
-},
-"translation": {
-"x": 6.004109774647408,
-"y": 2.451169003389615
-}
-},
-"time": 0.6991414561082092,
-"velocity": 1.747853640270523
-},
-{
-"acceleration": 2.5,
-"curvature": -0.03163711048137559,
-"pose": {
-"rotation": {
-"radians": -2.592626164187725
-},
-"translation": {
-"x": 5.940119309626706,
-"y": 2.4119176873530437
-}
-},
-"time": 0.7408471223806015,
-"velocity": 1.8521178059515035
-},
-{
-"acceleration": 2.4999999999999987,
-"curvature": -0.02770332054620756,
-"pose": {
-"rotation": {
-"radians": -2.5948911008655937
-},
-"translation": {
-"x": 5.874822554681827,
-"y": 2.372080489161214
-}
-},
-"time": 0.7810545366653541,
-"velocity": 1.952636341663385
-},
-{
-"acceleration": -0.5421170691391302,
-"curvature": -0.024373176080285743,
-"pose": {
-"rotation": {
-"radians": -2.596900644156691
-},
-"translation": {
-"x": 5.808731846683236,
-"y": 2.3319521530488547
-}
-},
-"time": 0.8196960303175803,
-"velocity": 2.0492400757939504
-},
-{
-"acceleration": -2.4999999999999942,
-"curvature": -0.021546867890023177,
-"pose": {
-"rotation": {
-"radians": -2.598677334761523
-},
-"translation": {
-"x": 5.742403809341149,
-"y": 2.291851088279893
-}
-},
-"time": 0.8577100176055118,
-"velocity": 2.028632044419125
-},
-{
-"acceleration": -2.499999999999998,
-"curvature": -0.019125300372678684,
-"pose": {
-"rotation": {
-"radians": -2.600241544566771
-},
-"translation": {
-"x": 5.676425238276946,
-"y": 2.2521114107302704
-}
-},
-"time": 0.8966099608527466,
-"velocity": 1.931382186301038
-},
-{
-"acceleration": -2.499999999999997,
-"curvature": -0.017012467342269477,
-"pose": {
-"rotation": {
-"radians": -2.601610571275439
-},
-"translation": {
-"x": 5.611398986094594,
-"y": 2.2130749844707704
-}
-},
-"time": 0.9369312867545008,
-"velocity": 1.8305788715466527
-},
-{
-"acceleration": -2.5000000000000018,
-"curvature": -0.015112608163024722,
-"pose": {
-"rotation": {
-"radians": -2.6027980717585244
-},
-"translation": {
-"x": 5.547929847452064,
-"y": 2.1750834633498357
-}
-},
-"time": 0.9785208459052614,
-"velocity": 1.726604973669751
-},
-{
-"acceleration": -2.5,
-"curvature": -0.01332303637872251,
-"pose": {
-"rotation": {
-"radians": -2.603813592474663
-},
-"translation": {
-"x": 5.486610444132747,
-"y": 2.1384703325763876
-}
-},
-"time": 1.0212032514186373,
-"velocity": 1.6198989598863114
-},
-{
-"acceleration": -2.4999999999999982,
-"curvature": -0.01152173018772482,
-"pose": {
-"rotation": {
-"radians": -2.6046620094485426
-},
-"translation": {
-"x": 5.428007110116876,
-"y": 2.1035529503026495
-}
-},
-"time": 1.064780561479556,
-"velocity": 1.5109556847340144
-},
-{
-"acceleration": -2.5,
-"curvature": -0.009546608028455987,
-"pose": {
-"rotation": {
-"radians": -2.605342700948862
-},
-"translation": {
-"x": 5.37264577665294,
-"y": 2.0706245892069655
-}
-},
-"time": 1.1090317881558387,
-"velocity": 1.400327618043308
-},
-{
-"acceleration": -2.5000000000000013,
-"curvature": -0.003984971556400902,
-"pose": {
-"rotation": {
-"radians": -2.606162363252346
-},
-"translation": {
-"x": 5.273466133144627,
-"y": 2.0117398433906786
-}
-},
-"time": 1.19855433072587,
-"velocity": 1.17652126161823
-},
-{
-"acceleration": -2.5,
-"curvature": 0.007734403886258118,
-"pose": {
-"rotation": {
-"radians": -2.606085796626112
-},
-"translation": {
-"x": 5.191934541674771,
-"y": 1.9633781472239225
-}
-},
-"time": 1.287540402980037,
-"velocity": 0.9540560809828121
-},
-{
-"acceleration": -2.5000000000000004,
-"curvature": 0.1323262565606135,
-"pose": {
-"rotation": {
-"radians": -2.6007813376618985
-},
-"translation": {
-"x": 5.0850669537180835,
-"y": 1.8997439114672758
-}
-},
-"time": 1.4543770682548225,
-"velocity": 0.5369644177958482
-},
-{
-"acceleration": -2.5000000000000004,
-"curvature": 9.052881833796132e-13,
-"pose": {
-"rotation": {
-"radians": -2.587285157388167
-},
-"translation": {
-"x": 5.0358210557131216,
-"y": 1.8697401247827292
-}
-},
-"time": 1.6691628353731618,
-"velocity": 0.0
-}
-]
\ No newline at end of file
diff --git a/src/main/deploy/Trajectories/output/FourBallAutoPartTwo.wpilib.json b/src/main/deploy/Trajectories/output/FourBallAutoPartTwo.wpilib.json
deleted file mode 100644
index c939bc9..0000000
--- a/src/main/deploy/Trajectories/output/FourBallAutoPartTwo.wpilib.json
+++ /dev/null
@@ -1,827 +0,0 @@
-[
-{
-"acceleration": 2.5,
-"curvature": 0.0,
-"pose": {
-"rotation": {
-"radians": -2.587285157388242
-},
-"translation": {
-"x": 5.036,
-"y": 1.87
-}
-},
-"time": 0.0,
-"velocity": 0.0
-},
-{
-"acceleration": 2.4999999999999987,
-"curvature": -9.947214282890952,
-"pose": {
-"rotation": {
-"radians": -2.6179333553047015
-},
-"translation": {
-"x": 5.031284601294508,
-"y": 1.8671502481013302
-}
-},
-"time": 0.06639056779378304,
-"velocity": 0.1659764194844576
-},
-{
-"acceleration": 2.4999999999999987,
-"curvature": -11.967073187475597,
-"pose": {
-"rotation": {
-"radians": -2.6893014136020374
-},
-"translation": {
-"x": 5.025861913416885,
-"y": 1.864265223564506
-}
-},
-"time": 0.09654851937264183,
-"velocity": 0.24137129843160454
-},
-{
-"acceleration": 2.4999999999999973,
-"curvature": -9.102719122404979,
-"pose": {
-"rotation": {
-"radians": -2.768130315487032
-},
-"translation": {
-"x": 5.019085840566514,
-"y": 1.861311662026611
-}
-},
-"time": 0.12343036744546944,
-"velocity": 0.3085759186136735
-},
-{
-"acceleration": 2.4999999999999996,
-"curvature": -5.780167396849074,
-"pose": {
-"rotation": {
-"radians": -2.8356721332058377
-},
-"translation": {
-"x": 5.010375093760691,
-"y": 1.858258442903968
-}
-},
-"time": 0.1503972442976828,
-"velocity": 0.3759931107442068
-},
-{
-"acceleration": 2.5000000000000013,
-"curvature": -3.459979347837657,
-"pose": {
-"rotation": {
-"radians": -2.8877588071209574
-},
-"translation": {
-"x": 4.999211046288527,
-"y": 1.8550765376190252
-}
-},
-"time": 0.178623183059707,
-"velocity": 0.4465579576492673
-},
-{
-"acceleration": 2.4999999999999996,
-"curvature": -2.064881641055333,
-"pose": {
-"rotation": {
-"radians": -2.926461304724418
-},
-"translation": {
-"x": 4.9851355891648375,
-"y": 1.8517389578272443
-}
-},
-"time": 0.20851580701827993,
-"velocity": 0.5212895175456996
-},
-{
-"acceleration": 2.5,
-"curvature": -0.7884937745428306,
-"pose": {
-"rotation": {
-"radians": -2.9763240221069247
-},
-"translation": {
-"x": 4.946707731374064,
-"y": 1.844498711871359
-}
-},
-"time": 0.2734264571225073,
-"velocity": 0.683566142806268
-},
-{
-"acceleration": 2.4999999999999987,
-"curvature": -0.15928102292050125,
-"pose": {
-"rotation": {
-"radians": -3.0213302408840974
-},
-"translation": {
-"x": 4.820973948391183,
-"y": 1.8271769896727603
-}
-},
-"time": 0.419879861979112,
-"velocity": 1.0496996549477797
-},
-{
-"acceleration": 2.4999999999999996,
-"curvature": -0.07978397657073408,
-"pose": {
-"rotation": {
-"radians": -3.031584373030252
-},
-"translation": {
-"x": 4.731020475466961,
-"y": 1.8168277084875415
-}
-},
-"time": 0.4987349909140112,
-"velocity": 1.2468374772850277
-},
-{
-"acceleration": 2.499999999999998,
-"curvature": -0.04061209618728612,
-"pose": {
-"rotation": {
-"radians": -3.037864589327709
-},
-"translation": {
-"x": 4.622446302932885,
-"y": 1.8052187430587319
-}
-},
-"time": 0.5797335764195815,
-"velocity": 1.4493339410489532
-},
-{
-"acceleration": 2.4999999999999996,
-"curvature": -0.028546596202906997,
-"pose": {
-"rotation": {
-"radians": -3.039968355390107
-},
-"translation": {
-"x": 4.561271525521778,
-"y": 1.7989191947039966
-}
-},
-"time": 0.6207170360163123,
-"velocity": 1.5517925900407803
-},
-{
-"acceleration": 2.499999999999994,
-"curvature": -0.019474674514682797,
-"pose": {
-"rotation": {
-"radians": -3.0415355513917435
-},
-"translation": {
-"x": 4.495627798459982,
-"y": 1.7922804044685599
-}
-},
-"time": 0.6618704550603369,
-"velocity": 1.6546761376508419
-},
-{
-"acceleration": 2.5000000000000013,
-"curvature": -0.012503983283243064,
-"pose": {
-"rotation": {
-"radians": -3.0426472603919765
-},
-"translation": {
-"x": 4.425648589500977,
-"y": 1.785297195683473
-}
-},
-"time": 0.7030888400914066,
-"velocity": 1.7577221002285157
-},
-{
-"acceleration": 2.4999999999999964,
-"curvature": -0.007026336379228899,
-"pose": {
-"rotation": {
-"radians": -3.0433652289536823
-},
-"translation": {
-"x": 4.351497860478477,
-"y": 1.777965707089173
-}
-},
-"time": 0.7442739743843233,
-"velocity": 1.8606849359608075
-},
-{
-"acceleration": 2.4999999999999925,
-"curvature": -0.0026186603863443793,
-"pose": {
-"rotation": {
-"radians": -3.043736501280448
-},
-"translation": {
-"x": 4.273367922760337,
-"y": 1.7702833410623686
-}
-},
-"time": 0.7853337692835295,
-"velocity": 1.9633344232088228
-},
-{
-"acceleration": 2.5000000000000027,
-"curvature": 0.0010194113917965737,
-"pose": {
-"rotation": {
-"radians": -3.043796742550586
-},
-"translation": {
-"x": 4.191477292702439,
-"y": 1.7622487118429282
-}
-},
-"time": 0.8261817005410513,
-"velocity": 2.065454251352627
-},
-{
-"acceleration": 2.5,
-"curvature": 0.004105685858854052,
-"pose": {
-"rotation": {
-"radians": -3.0435726417399693
-},
-"translation": {
-"x": 4.106068547102594,
-"y": 1.7538615937607585
-}
-},
-"time": 0.8667363229686662,
-"velocity": 2.1668408074216643
-},
-{
-"acceleration": 2.5000000000000036,
-"curvature": 0.006801768182756269,
-"pose": {
-"rotation": {
-"radians": -3.043083657519818
-},
-"translation": {
-"x": 4.017406178654429,
-"y": 1.7451228694626915
-}
-},
-"time": 0.9069208551287038,
-"velocity": 2.2673021378217584
-},
-{
-"acceleration": 2.499999999999993,
-"curvature": 0.00923072599807912,
-"pose": {
-"rotation": {
-"radians": -3.042343287146873
-},
-"translation": {
-"x": 3.9257744514012947,
-"y": 1.736034478139369
-}
-},
-"time": 0.9466628257436009,
-"velocity": 2.3666570643590013
-},
-{
-"acceleration": 0.6020032690989593,
-"curvature": 0.011489188615668425,
-"pose": {
-"rotation": {
-"radians": -3.0413599822249666
-},
-"translation": {
-"x": 3.8314752561901457,
-"y": 1.7265993637521255
-}
-},
-"time": 0.9858937741602504,
-"velocity": 2.4647344354006244
-},
-{
-"acceleration": -0.04558286278463284,
-"curvature": 0.013655842819859659,
-"pose": {
-"rotation": {
-"radians": -3.0401377970967505
-},
-"translation": {
-"x": 3.734825966125449,
-"y": 1.716821423259872
-}
-},
-"time": 1.0251189007578718,
-"velocity": 2.488348089843213
-},
-{
-"acceleration": -0.04544145976434352,
-"curvature": 0.015797582035617402,
-"pose": {
-"rotation": {
-"radians": -3.0386768292347055
-},
-"translation": {
-"x": 3.636157292023071,
-"y": 1.7067054548459817
-}
-},
-"time": 1.0649935971522384,
-"velocity": 2.48653048702889
-},
-{
-"acceleration": -0.0466095460463888,
-"curvature": 0.017974134243328128,
-"pose": {
-"rotation": {
-"radians": -3.0369734923480953
-},
-"translation": {
-"x": 3.5358111378641754,
-"y": 1.6962571061451737
-}
-},
-"time": 1.105582714064574,
-"velocity": 2.4846860583058477
-},
-{
-"acceleration": -0.0490316734412526,
-"curvature": 0.02024172782068343,
-"pose": {
-"rotation": {
-"radians": -3.0350206493910705
-},
-"translation": {
-"x": 3.434138456249119,
-"y": 1.685482822470397
-}
-},
-"time": 1.1467474540833125,
-"velocity": 2.4827673884604566
-},
-{
-"acceleration": -0.052735778998391075,
-"curvature": 0.022656190685028964,
-"pose": {
-"rotation": {
-"radians": -3.032807622500527
-},
-"translation": {
-"x": 3.3314971038513455,
-"y": 1.6743897950397137
-}
-},
-"time": 1.1883467920911528,
-"velocity": 2.480727703303884
-},
-{
-"acceleration": -0.05782836663557209,
-"curvature": 0.025275779900682804,
-"pose": {
-"rotation": {
-"radians": -3.0303200889181148
-},
-"translation": {
-"x": 3.2282496968712815,
-"y": 1.6629859092031847
-}
-},
-"time": 1.2302383555749077,
-"velocity": 2.4785185190701076
-},
-{
-"acceleration": -0.06449769509459355,
-"curvature": 0.028163984825450253,
-"pose": {
-"rotation": {
-"radians": -3.027539865318161
-},
-"translation": {
-"x": 3.124761466490232,
-"y": 1.6512796926697524
-}
-},
-"time": 1.2722793215228825,
-"velocity": 2.4760873586775545
-},
-{
-"acceleration": -0.0730245976087693,
-"curvature": 0.03139252476451037,
-"pose": {
-"rotation": {
-"radians": -3.0244445770541053
-},
-"translation": {
-"x": 3.021398114324276,
-"y": 1.6392802637341264
-}
-},
-"time": 1.3143273306297572,
-"velocity": 2.4733753590068446
-},
-{
-"acceleration": -0.08380156870303118,
-"curvature": 0.035044765869979524,
-"pose": {
-"rotation": {
-"radians": -3.0210072031692974
-},
-"translation": {
-"x": 2.918523667878158,
-"y": 1.6269972795036658
-}
-},
-"time": 1.3562414203654412,
-"velocity": 2.4703145994697584
-},
-{
-"acceleration": -0.09736172853910083,
-"curvature": 0.03921980981378354,
-"pose": {
-"rotation": {
-"radians": -3.0171954821942997
-},
-"translation": {
-"x": 2.8164983359991904,
-"y": 1.6144408841252655
-}
-},
-"time": 1.397882979333498,
-"velocity": 2.4668249715049955
-},
-{
-"acceleration": -0.11442035125211146,
-"curvature": 0.04403756013251153,
-"pose": {
-"rotation": {
-"radians": -3.0129711574177676
-},
-"translation": {
-"x": 2.7156763643311397,
-"y": 1.601621657012239
-}
-},
-"time": 1.4391167262987665,
-"velocity": 2.462810382626313
-},
-{
-"acceleration": -0.13593293193516834,
-"curvature": 0.049645155866437245,
-"pose": {
-"rotation": {
-"radians": -3.0082890331426837
-},
-"translation": {
-"x": 2.61640389076813,
-"y": 1.5885505610712016
-}
-},
-"time": 1.4798117183513253,
-"velocity": 2.4581540473414574
-},
-{
-"acceleration": -0.16317535206965983,
-"curvature": 0.05622528439494697,
-"pose": {
-"rotation": {
-"radians": -3.0030958051181615
-},
-"translation": {
-"x": 2.519016800908534,
-"y": 1.575238890928957
-}
-},
-"time": 1.5198423939404977,
-"velocity": 2.4527125602412756
-},
-{
-"acceleration": -0.23250056899390759,
-"curvature": 0.06400705836641218,
-"pose": {
-"rotation": {
-"radians": -2.997328618608302
-},
-"translation": {
-"x": 2.423838583508866,
-"y": 1.5616982211593804
-}
-},
-"time": 1.5590896579969917,
-"velocity": 2.4463083741110863
-},
-{
-"acceleration": -2.499999999999993,
-"curvature": 0.0732803820637495,
-"pose": {
-"rotation": {
-"radians": -2.990913296271385
-},
-"translation": {
-"x": 2.3311781859376857,
-"y": 1.5479403545103008
-}
-},
-"time": 1.5974524723680639,
-"velocity": 2.4373889979416044
-},
-{
-"acceleration": -2.4999999999999973,
-"curvature": 0.08441506201676856,
-"pose": {
-"rotation": {
-"radians": -2.983762165268377
-},
-"translation": {
-"x": 2.2413278696294814,
-"y": 1.5339772701303893
-}
-},
-"time": 1.635500723606274,
-"velocity": 2.3422683698460793
-},
-{
-"acceleration": -2.4999999999999947,
-"curvature": 0.09788636050718462,
-"pose": {
-"rotation": {
-"radians": -2.975771399408439
-},
-"translation": {
-"x": 2.1545610655385756,
-"y": 1.5198210717960379
-}
-},
-"time": 1.6738179778057989,
-"velocity": 2.246475234347267
-},
-{
-"acceleration": -2.5,
-"curvature": 0.11430927312417603,
-"pose": {
-"rotation": {
-"radians": -2.966817779308986
-},
-"translation": {
-"x": 2.0711302295930145,
-"y": 1.5054839361382477
-}
-},
-"time": 1.7123260089429848,
-"velocity": 2.1502051565043026
-},
-{
-"acceleration": -2.4999999999999916,
-"curvature": 0.13448454237642335,
-"pose": {
-"rotation": {
-"radians": -2.9567547651149244
-},
-"translation": {
-"x": 1.9912646981484667,
-"y": 1.4909780608695131
-}
-},
-"time": 1.7509438867867047,
-"velocity": 2.053660461895003
-},
-{
-"acceleration": -2.5,
-"curvature": 0.15946026004465952,
-"pose": {
-"rotation": {
-"radians": -2.9454077796112634
-},
-"translation": {
-"x": 1.915168543442114,
-"y": 1.4763156130107022
-}
-},
-"time": 1.789588361064106,
-"velocity": 1.9570492762015
-},
-{
-"acceleration": -2.5,
-"curvature": 0.19061369836127368,
-"pose": {
-"rotation": {
-"radians": -2.932568628713658
-},
-"translation": {
-"x": 1.843018429046548,
-"y": 1.461508677117946
-}
-},
-"time": 1.8281744740137702,
-"velocity": 1.8605839938273392
-},
-{
-"acceleration": -2.5,
-"curvature": 0.2297582905550648,
-"pose": {
-"rotation": {
-"radians": -2.9179890677530667
-},
-"translation": {
-"x": 1.7749614653236736,
-"y": 1.446569203509518
-}
-},
-"time": 1.8666165027854087,
-"velocity": 1.7644789218982428
-},
-{
-"acceleration": -2.4999999999999964,
-"curvature": 0.27927936475845044,
-"pose": {
-"rotation": {
-"radians": -2.9013737037690057
-},
-"translation": {
-"x": 1.7111130648785862,
-"y": 1.4315089564927215
-}
-},
-"time": 1.9048293749876593,
-"velocity": 1.6689467413926165
-},
-{
-"acceleration": -2.499999999999997,
-"curvature": 0.34229690682494524,
-"pose": {
-"rotation": {
-"radians": -2.882372791380791
-},
-"translation": {
-"x": 1.65155479801349,
-"y": 1.4163394625907717
-}
-},
-"time": 1.9427307595651881,
-"velocity": 1.574193279948794
-},
-{
-"acceleration": -2.5,
-"curvature": 0.52597847218196,
-"pose": {
-"rotation": {
-"radians": -2.835510926634674
-},
-"translation": {
-"x": 1.5454528674409023,
-"y": 1.3857173406651446
-}
-},
-"time": 2.0172976967447784,
-"velocity": 1.3877759369998188
-},
-{
-"acceleration": -2.4999999999999987,
-"curvature": 0.8250157161546879,
-"pose": {
-"rotation": {
-"radians": -2.7734215329290404
-},
-"translation": {
-"x": 1.4565498972134434,
-"y": 1.3547883268582153
-}
-},
-"time": 2.0898689369358103,
-"velocity": 1.2063478365222395
-},
-{
-"acceleration": -2.500000000000003,
-"curvature": 1.28577775082341,
-"pose": {
-"rotation": {
-"radians": -2.6918248489057426
-},
-"translation": {
-"x": 1.384061383141483,
-"y": 1.3236307922707526
-}
-},
-"time": 2.160433349444786,
-"velocity": 1.0299368052498006
-},
-{
-"acceleration": -2.5000000000000018,
-"curvature": 1.5747628433911087,
-"pose": {
-"rotation": {
-"radians": -2.64290675726717
-},
-"translation": {
-"x": 1.353524911671942,
-"y": 1.3079884666051145
-}
-},
-"time": 2.195213986869731,
-"velocity": 0.942985211687437
-},
-{
-"acceleration": -2.5000000000000018,
-"curvature": 1.875971558908881,
-"pose": {
-"rotation": {
-"radians": -2.5889821244783966
-},
-"translation": {
-"x": 1.326455467762877,
-"y": 1.2923143332390081
-}
-},
-"time": 2.2299880905761498,
-"velocity": 0.8560499524213907
-},
-{
-"acceleration": -2.500000000000001,
-"curvature": 2.137530655552861,
-"pose": {
-"rotation": {
-"radians": -2.531427947556427
-},
-"translation": {
-"x": 1.3025335364156287,
-"y": 1.2766154488486494
-}
-},
-"time": 2.265225872272698,
-"velocity": 0.7679554981800195
-},
-{
-"acceleration": -2.4999999999999987,
-"curvature": 2.275815004418493,
-"pose": {
-"rotation": {
-"radians": -2.4729160639600933
-},
-"translation": {
-"x": 1.281384314867612,
-"y": 1.26089811459501
-}
-},
-"time": 2.3017036603745504,
-"velocity": 0.676761027925389
-},
-{
-"acceleration": -2.5,
-"curvature": 2.186762286771549,
-"pose": {
-"rotation": {
-"radians": -2.417616400344057
-},
-"translation": {
-"x": 1.2625755680461537,
-"y": 1.245167824350701
-}
-},
-"time": 2.340750576740486,
-"velocity": 0.5791437370105498
-},
-{
-"acceleration": -2.5,
-"curvature": 1.779982893126663,
-"pose": {
-"rotation": {
-"radians": -2.37101234307858
-},
-"translation": {
-"x": 1.245615484022454,
-"y": 1.2294292129268545
-}
-},
-"time": 2.3849110895560366,
-"velocity": 0.4687424549716732
-},
-{
-"acceleration": -2.5,
-"curvature": -2.031524073818165e-14,
-"pose": {
-"rotation": {
-"radians": -2.327630832353611
-},
-"translation": {
-"x": 1.2149633050955893,
-"y": 1.1979409598390003
-}
-},
-"time": 2.572408071544706,
-"velocity": 0.0
-}
-]
\ No newline at end of file
diff --git a/src/main/deploy/Trajectories/output/SCurve.wpilib.json b/src/main/deploy/Trajectories/output/SCurve.wpilib.json
deleted file mode 100644
index a368288..0000000
--- a/src/main/deploy/Trajectories/output/SCurve.wpilib.json
+++ /dev/null
@@ -1,1157 +0,0 @@
-[
-{
-"acceleration": 2.5,
-"curvature": 0.0,
-"pose": {
-"rotation": {
-"radians": 0.0
-},
-"translation": {
-"x": 0.0,
-"y": 8.2296
-}
-},
-"time": 0.0,
-"velocity": 0.0
-},
-{
-"acceleration": 2.5000000000000004,
-"curvature": -0.4349449116596967,
-"pose": {
-"rotation": {
-"radians": -0.014532076034560268
-},
-"translation": {
-"x": 0.0643598195847667,
-"y": 8.229282597917596
-}
-},
-"time": 0.22691073529980996,
-"velocity": 0.5672768382495249
-},
-{
-"acceleration": 2.4999999999999982,
-"curvature": -0.7648980337273721,
-"pose": {
-"rotation": {
-"radians": -0.054000653656919896
-},
-"translation": {
-"x": 0.12908801146953133,
-"y": 8.227179176047116
-}
-},
-"time": 0.32140063689375925,
-"velocity": 0.8035015922343982
-},
-{
-"acceleration": 2.500000000000002,
-"curvature": -0.9813743785357366,
-"pose": {
-"rotation": {
-"radians": -0.1119444354277113
-},
-"translation": {
-"x": 0.19447841674357666,
-"y": 8.221818992541111
-}
-},
-"time": 0.3946975467193309,
-"velocity": 0.9867438667983272
-},
-{
-"acceleration": 2.500000000000002,
-"curvature": -1.0852830254674435,
-"pose": {
-"rotation": {
-"radians": -0.18179520773206811
-},
-"translation": {
-"x": 0.26074880833286246,
-"y": 8.212054432998801
-}
-},
-"time": 0.45757499617245717,
-"velocity": 1.143937490431143
-},
-{
-"acceleration": 2.5,
-"curvature": -1.0904479372581815,
-"pose": {
-"rotation": {
-"radians": -0.2573293686161901
-},
-"translation": {
-"x": 0.32804665347322837,
-"y": 8.197038117128649
-}
-},
-"time": 0.5143317250164771,
-"velocity": 1.2858293125411928
-},
-{
-"acceleration": 2.499999999999998,
-"curvature": -1.0221157525372802,
-"pose": {
-"rotation": {
-"radians": -0.33321756026253563
-},
-"translation": {
-"x": 0.3964548761835968,
-"y": 8.176200005410953
-}
-},
-"time": 0.5672269559694741,
-"velocity": 1.4180673899236855
-},
-{
-"acceleration": 2.500000000000005,
-"curvature": -0.9098476595867542,
-"pose": {
-"rotation": {
-"radians": -0.40544079815721834
-},
-"translation": {
-"x": 0.46599761973917564,
-"y": 8.149224505760417
-}
-},
-"time": 0.6175916987598651,
-"velocity": 1.5439792468996627
-},
-{
-"acceleration": 2.5000000000000013,
-"curvature": -0.7796616690480549,
-"pose": {
-"rotation": {
-"radians": -0.47140152461054263
-},
-"translation": {
-"x": 0.5366460091446611,
-"y": 8.116027580188735
-}
-},
-"time": 0.6662333118625517,
-"velocity": 1.6655832796563796
-},
-{
-"acceleration": 2.4999999999999982,
-"curvature": -0.6497128875408189,
-"pose": {
-"rotation": {
-"radians": -0.5297441994872781
-},
-"translation": {
-"x": 0.608323913607441,
-"y": 8.076733851467182
-}
-},
-"time": 0.713624727933758,
-"velocity": 1.7840618198343952
-},
-{
-"acceleration": 2.4999999999999956,
-"curvature": -0.5300408617971761,
-"pose": {
-"rotation": {
-"radians": -0.5800238267015563
-},
-"translation": {
-"x": 0.6809137090107966,
-"y": 8.031653709789177
-}
-},
-"time": 0.7600126926676181,
-"velocity": 1.9000317316690454
-},
-{
-"acceleration": 2.499999999999996,
-"curvature": -0.4245453148086314,
-"pose": {
-"rotation": {
-"radians": -0.6223627769386652
-},
-"translation": {
-"x": 0.7542620403871072,
-"y": 7.981260419432882
-}
-},
-"time": 0.805488906039385,
-"velocity": 2.0137222650984627
-},
-{
-"acceleration": 2.5000000000000018,
-"curvature": -0.3333447212499964,
-"pose": {
-"rotation": {
-"radians": -0.6571742245607182
-},
-"translation": {
-"x": 0.8281855843910509,
-"y": 7.926167225423771
-}
-},
-"time": 0.8500403038368909,
-"velocity": 2.125100759592227
-},
-{
-"acceleration": 2.4999999999999933,
-"curvature": -0.25460560800778786,
-"pose": {
-"rotation": {
-"radians": -0.6849712167461403
-},
-"translation": {
-"x": 0.902476811772809,
-"y": 7.867104460197218
-}
-},
-"time": 0.8935855894895454,
-"velocity": 2.2339639737238635
-},
-{
-"acceleration": 1.4040641953200217,
-"curvature": -0.18567278423789899,
-"pose": {
-"rotation": {
-"radians": -0.7062485884843716
-},
-"translation": {
-"x": 0.9769097498512683,
-"y": 7.804896650261076
-}
-},
-"time": 0.9360019412486964,
-"velocity": 2.340004853121741
-},
-{
-"acceleration": 1.141920339611744,
-"curvature": -0.12364714934765747,
-"pose": {
-"rotation": {
-"radians": -0.7214158748764421
-},
-"translation": {
-"x": 1.051245744987224,
-"y": 7.740439622858259
-}
-},
-"time": 0.9775313381950166,
-"velocity": 2.3983147924273016
-},
-{
-"acceleration": 1.190328469679199,
-"curvature": -0.06560842007023977,
-"pose": {
-"rotation": {
-"radians": -0.730761584892689
-},
-"translation": {
-"x": 1.1252392250565821,
-"y": 7.674677612629318
-}
-},
-"time": 1.0184097167759203,
-"velocity": 2.4449946443791846
-},
-{
-"acceleration": -0.8863350895654077,
-"curvature": -0.00863439207713158,
-"pose": {
-"rotation": {
-"radians": -0.734434749645516
-},
-"translation": {
-"x": 1.1986434619235635,
-"y": 7.608580368275032
-}
-},
-"time": 1.0584199949333677,
-"velocity": 2.492620017549778
-},
-{
-"acceleration": -1.3184314017256722,
-"curvature": 0.05028742181363222,
-"pose": {
-"rotation": {
-"radians": -0.7324350465722995
-},
-"translation": {
-"x": 1.2712163339139055,
-"y": 7.543120259218982
-}
-},
-"time": 1.0979063702325162,
-"velocity": 2.457621857562394
-},
-{
-"acceleration": -1.4402922804417295,
-"curvature": 0.11439653958424763,
-"pose": {
-"rotation": {
-"radians": -0.7246072165259388
-},
-"translation": {
-"x": 1.342726088288065,
-"y": 7.479249382270135
-}
-},
-"time": 1.1373370773009364,
-"velocity": 2.4056351751711427
-},
-{
-"acceleration": -1.6263022292721434,
-"curvature": 0.18729561137775247,
-"pose": {
-"rotation": {
-"radians": -0.7106393393093966
-},
-"translation": {
-"x": 1.4129571037144226,
-"y": 7.417876668285421
-}
-},
-"time": 1.1765686487073042,
-"velocity": 2.3491302457249525
-},
-{
-"acceleration": -1.8664865121481775,
-"curvature": 0.27301522160640546,
-"pose": {
-"rotation": {
-"radians": -0.6900687284136348
-},
-"translation": {
-"x": 1.481715652742483,
-"y": 7.359844988832325
-}
-},
-"time": 1.21539157714108,
-"velocity": 2.28599243066623
-},
-{
-"acceleration": -2.1364757277900877,
-"curvature": 0.375871277368872,
-"pose": {
-"rotation": {
-"radians": -0.6623049158013701
-},
-"translation": {
-"x": 1.5488356642760805,
-"y": 7.305908262851453
-}
-},
-"time": 1.2536561699391031,
-"velocity": 2.2145720843158774
-},
-{
-"acceleration": -2.421066683691392,
-"curvature": 0.49985287064678563,
-"pose": {
-"rotation": {
-"radians": -0.6266876048576935
-},
-"translation": {
-"x": 1.6141844860465797,
-"y": 7.256708563319126
-}
-},
-"time": 1.2912755386635182,
-"velocity": 2.134199216141379
-},
-{
-"acceleration": -2.500000000000003,
-"curvature": 0.647095114821546,
-"pose": {
-"rotation": {
-"radians": -0.5826089116853364
-},
-"translation": {
-"x": 1.67766864708608,
-"y": 7.212753223909954
-}
-},
-"time": 1.3282304696575629,
-"velocity": 2.0447288639135834
-},
-{
-"acceleration": -2.470290200398191,
-"curvature": 0.81485286739151,
-"pose": {
-"rotation": {
-"radians": -0.5297406887952426
-},
-"translation": {
-"x": 1.739239620200617,
-"y": 7.174391945659422
-}
-},
-"time": 1.3645136039405092,
-"velocity": 1.9540210282062176
-},
-{
-"acceleration": -2.0573139263852593,
-"curvature": 0.9906545455422358,
-"pose": {
-"rotation": {
-"radians": -0.4684078869187038
-},
-"translation": {
-"x": 1.7988995844433673,
-"y": 7.141793903626468
-}
-},
-"time": 1.4001066927913113,
-"velocity": 1.866095769616179
-},
-{
-"acceleration": -1.1549386169379494,
-"curvature": 1.146665149113696,
-"pose": {
-"rotation": {
-"radians": -0.4001132954220403
-},
-"translation": {
-"x": 1.856707187587848,
-"y": 7.114924853556063
-}
-},
-"time": 1.4349359453723538,
-"velocity": 1.7944410632356105
-},
-{
-"acceleration": 0.4309311608801355,
-"curvature": 1.2370625005016362,
-"pose": {
-"rotation": {
-"radians": -0.32811954291452783
-},
-"translation": {
-"x": 1.912783308601124,
-"y": 7.093524238541798
-}
-},
-"time": 1.4687522340296348,
-"velocity": 1.755385325583796
-},
-{
-"acceleration": 2.500000000000008,
-"curvature": 1.2043685060524827,
-"pose": {
-"rotation": {
-"radians": -0.25784671634874734
-},
-"translation": {
-"x": 1.967316820117008,
-"y": 7.077082295688458
-}
-},
-"time": 1.5010717349652234,
-"velocity": 1.7693128056410357
-},
-{
-"acceleration": 0.823426309284273,
-"curvature": 0.9970396949993284,
-"pose": {
-"rotation": {
-"radians": -0.19677204421663985
-},
-"translation": {
-"x": 2.020570350909262,
-"y": 7.064817162774613
-}
-},
-"time": 1.5313120694531923,
-"velocity": 1.8449136418609582
-},
-{
-"acceleration": -2.499999999999998,
-"curvature": -1.0468734605202166e-15,
-"pose": {
-"rotation": {
-"radians": -0.13750660372215093
-},
-"translation": {
-"x": 2.1246913409569097,
-"y": 7.048192021224041
-}
-},
-"time": 1.58775285656547,
-"velocity": 1.8913884708859203
-},
-{
-"acceleration": -0.48572437863287643,
-"curvature": 1.265000895108805,
-"pose": {
-"rotation": {
-"radians": -0.054077644502723995
-},
-"translation": {
-"x": 2.231477391880091,
-"y": 7.036700217934387
-}
-},
-"time": 1.6468457285332174,
-"velocity": 1.7436562909665516
-},
-{
-"acceleration": 2.072320080480062,
-"curvature": 1.3050653998831883,
-"pose": {
-"rotation": {
-"radians": 0.023475778236697904
-},
-"translation": {
-"x": 2.2905955149946213,
-"y": 7.0357852150536315
-}
-},
-"time": 1.6809161568546627,
-"velocity": 1.7271074533403616
-},
-{
-"acceleration": 2.499999999999997,
-"curvature": 1.1257130265207385,
-"pose": {
-"rotation": {
-"radians": 0.10344339286645375
-},
-"translation": {
-"x": 2.355746094089861,
-"y": 7.039990255573768
-}
-},
-"time": 1.7178965661524517,
-"velocity": 1.8037426981125413
-},
-{
-"acceleration": 2.5000000000000027,
-"curvature": 0.8831715010111089,
-"pose": {
-"rotation": {
-"radians": 0.17668219120867026
-},
-"translation": {
-"x": 2.4280374820347475,
-"y": 7.050291779818997
-}
-},
-"time": 1.75730380525163,
-"velocity": 1.9022607958604874
-},
-{
-"acceleration": 2.500000000000002,
-"curvature": 0.6613819202798487,
-"pose": {
-"rotation": {
-"radians": 0.23951092885808326
-},
-"translation": {
-"x": 2.5081763194664184,
-"y": 7.067340213405863
-}
-},
-"time": 1.7992202183368886,
-"velocity": 2.007051828573634
-},
-{
-"acceleration": 2.3354199515962843,
-"curvature": 0.48653926374773104,
-"pose": {
-"rotation": {
-"radians": 0.29155478017673514
-},
-"translation": {
-"x": 2.5965017347904595,
-"y": 7.091487050216497
-}
-},
-"time": 1.843615182666265,
-"velocity": 2.118039239397075
-},
-{
-"acceleration": 1.326000054114521,
-"curvature": 0.3573244497721603,
-"pose": {
-"rotation": {
-"radians": 0.3339173372116421
-},
-"translation": {
-"x": 2.6930195441811566,
-"y": 7.122811935371848
-}
-},
-"time": 1.8903218009241425,
-"velocity": 2.227118807548113
-},
-{
-"acceleration": 0.9500430090002672,
-"curvature": 0.2641160378425222,
-"pose": {
-"rotation": {
-"radians": 0.3681218044983266
-},
-"translation": {
-"x": 2.7974364515817456,
-"y": 7.161149748204928
-}
-},
-"time": 1.9395451010026639,
-"velocity": 2.292388906115928
-},
-{
-"acceleration": 0.7391484878886384,
-"curvature": 0.19703473250467252,
-"pose": {
-"rotation": {
-"radians": 0.39563094355302925
-},
-"translation": {
-"x": 2.909194248704658,
-"y": 7.206117685234055
-}
-},
-"time": 1.9915351734793187,
-"velocity": 2.341781711009791
-},
-{
-"acceleration": 0.6289840983941416,
-"curvature": 0.1707499237303838,
-"pose": {
-"rotation": {
-"radians": 0.4072709733266554
-},
-"translation": {
-"x": 2.967587520290323,
-"y": 7.23091546653991
-}
-},
-"time": 2.0185110513338542,
-"velocity": 2.3617208903354396
-},
-{
-"acceleration": 0.5382061928982188,
-"curvature": 0.14823004998825418,
-"pose": {
-"rotation": {
-"radians": 0.4176802961888324
-},
-"translation": {
-"x": 3.0275040150317762,
-"y": 7.257142343136084
-}
-},
-"time": 2.046103527140514,
-"velocity": 2.3790761188531535
-},
-{
-"acceleration": 0.4637842280813848,
-"curvature": 0.12882774577208522,
-"pose": {
-"rotation": {
-"radians": 0.42697355647894053
-},
-"translation": {
-"x": 3.08881468001577,
-"y": 7.284701301682252
-}
-},
-"time": 2.07426835729762,
-"velocity": 2.3942346048656344
-},
-{
-"acceleration": 0.403076374422805,
-"curvature": 0.1120007552054922,
-"pose": {
-"rotation": {
-"radians": 0.4352509131438875
-},
-"translation": {
-"x": 3.151380317814678,
-"y": 7.3134868017196535
-}
-},
-"time": 2.1029535564334516,
-"velocity": 2.407538347804207
-},
-{
-"acceleration": 0.35385466103296065,
-"curvature": 0.09729500232905448,
-"pose": {
-"rotation": {
-"radians": 0.4425992140761856
-},
-"translation": {
-"x": 3.215052655236505,
-"y": 7.343385622014008
-}
-},
-"time": 2.1321001590370927,
-"velocity": 2.419286654708425
-},
-{
-"acceleration": 0.3142890057501707,
-"curvature": 0.08432911855493698,
-"pose": {
-"rotation": {
-"radians": 0.4490931700307836
-},
-"translation": {
-"x": 3.279675412074891,
-"y": 7.374277706898424
-}
-},
-"time": 2.1616429761501186,
-"velocity": 2.4297405182439133
-},
-{
-"acceleration": 0.2829132274503553,
-"curvature": 0.07278080291060192,
-"pose": {
-"rotation": {
-"radians": 0.4547964536605559
-},
-"translation": {
-"x": 3.345085369859123,
-"y": 7.4060370126163155
-}
-},
-"time": 2.1915113368935817,
-"velocity": 2.4391278156453637
-},
-{
-"acceleration": 0.25858678104351857,
-"curvature": 0.06237501261821018,
-"pose": {
-"rotation": {
-"radians": 0.459762682238013
-},
-"translation": {
-"x": 3.4111134406041383,
-"y": 7.438532353664314
-}
-},
-"time": 2.2216298094081903,
-"velocity": 2.4476487299103464
-},
-{
-"acceleration": 0.24046010131931117,
-"curvature": 0.05287379261224741,
-"pose": {
-"rotation": {
-"radians": 0.4640362627611798
-},
-"translation": {
-"x": 3.477585735560538,
-"y": 7.471628249135188
-}
-},
-"time": 2.2519188986985745,
-"velocity": 2.4554810880106865
-},
-{
-"acceleration": 0.22794807318508342,
-"curvature": 0.04406746776760697,
-"pose": {
-"rotation": {
-"radians": 0.46765308973068137
-},
-"translation": {
-"x": 3.54432463396459,
-"y": 7.505185769060747
-}
-},
-"time": 2.2822957205357954,
-"velocity": 2.4627855016674234
-},
-{
-"acceleration": 0.22071448485231024,
-"curvature": 0.03576688986239397,
-"pose": {
-"rotation": {
-"radians": 0.47064109184621683
-},
-"translation": {
-"x": 3.6111498517882388,
-"y": 7.539063380754762
-}
-},
-"time": 2.312674651415642,
-"velocity": 2.4697103204269073
-},
-{
-"acceleration": 0.2186698554951737,
-"curvature": 0.027796422313326748,
-"pose": {
-"rotation": {
-"radians": 0.4730206261074194
-},
-"translation": {
-"x": 3.6778795104891127,
-"y": 7.57311779515588
-}
-},
-"time": 2.342967954875558,
-"velocity": 2.4763964912945373
-},
-{
-"acceleration": 0.2219854882980314,
-"curvature": 0.019987341297270065,
-"pose": {
-"rotation": {
-"radians": 0.4748047175249769
-},
-"translation": {
-"x": 3.7443312057605325,
-"y": 7.607204813170533
-}
-},
-"time": 2.3730863844197683,
-"velocity": 2.4829824839307113
-},
-{
-"acceleration": -0.703461243170258,
-"curvature": 0.0121713204692605,
-"pose": {
-"rotation": {
-"radians": 0.4759991405853351
-},
-"translation": {
-"x": 3.8103230762815183,
-"y": 7.641180172015856
-}
-},
-"time": 2.402939762999626,
-"velocity": 2.489609500752107
-},
-{
-"acceleration": -2.500000000000006,
-"curvature": 0.004173639723218532,
-"pose": {
-"rotation": {
-"radians": 0.4766023351718682
-},
-"translation": {
-"x": 3.8756748724667993,
-"y": 7.674900391562597
-}
-},
-"time": 2.4326022345584333,
-"velocity": 2.468743101633846
-},
-{
-"acceleration": -2.5,
-"curvature": -0.00419429125394143,
-"pose": {
-"rotation": {
-"radians": 0.47660514500511586
-},
-"translation": {
-"x": 3.9402090252168165,
-"y": 7.708223620678034
-}
-},
-"time": 2.4624738052189556,
-"velocity": 2.3940641749825398
-},
-{
-"acceleration": -2.5,
-"curvature": -0.013143585009082916,
-"pose": {
-"rotation": {
-"radians": 0.4759903608409663
-},
-"translation": {
-"x": 4.003751714667738,
-"y": 7.741010483568887
-}
-},
-"time": 2.492821374391934,
-"velocity": 2.318195252050094
-},
-{
-"acceleration": -2.5,
-"curvature": -0.022918317784827943,
-"pose": {
-"rotation": {
-"radians": 0.4747320435598793
-},
-"translation": {
-"x": 4.066133938941461,
-"y": 7.7731249261242334
-}
-},
-"time": 2.5235984492712022,
-"velocity": 2.2412525648519233
-},
-{
-"acceleration": -2.5000000000000067,
-"curvature": -0.03380855495634889,
-"pose": {
-"rotation": {
-"radians": 0.47279459371743787
-},
-"translation": {
-"x": 4.127192582895621,
-"y": 7.804435062258419
-}
-},
-"time": 2.554755969838236,
-"velocity": 2.163358763434339
-},
-{
-"acceleration": -2.5,
-"curvature": -0.04616701326488123,
-"pose": {
-"rotation": {
-"radians": 0.47013152390148255
-},
-"translation": {
-"x": 4.1867714868735995,
-"y": 7.8348140202539724
-}
-},
-"time": 2.586242281431123,
-"velocity": 2.0846429844521217
-},
-{
-"acceleration": -2.5000000000000036,
-"curvature": -0.06043077615919049,
-"pose": {
-"rotation": {
-"radians": 0.4666838781910478
-},
-"translation": {
-"x": 4.244722515454535,
-"y": 7.864140789104523
-}
-},
-"time": 2.618003117302842,
-"velocity": 2.0052408947728235
-},
-{
-"acceleration": -2.5000000000000018,
-"curvature": -0.07714998338888823,
-"pose": {
-"rotation": {
-"radians": 0.4623782291515153
-},
-"translation": {
-"x": 4.300906626203325,
-"y": 7.892301064857707
-}
-},
-"time": 2.6499816010828083,
-"velocity": 1.9252946853229072
-},
-{
-"acceleration": -2.5,
-"curvature": -0.12096334574617659,
-"pose": {
-"rotation": {
-"radians": 0.45081118492264965
-},
-"translation": {
-"x": 4.407469801892927,
-"y": 7.944703534590073
-}
-},
-"time": 2.7143509517417304,
-"velocity": 1.7643713086756014
-},
-{
-"acceleration": -2.5,
-"curvature": -0.18609108962081053,
-"pose": {
-"rotation": {
-"radians": 0.4344420700820166
-},
-"translation": {
-"x": 4.505571146677136,
-"y": 7.9912732999431215
-}
-},
-"time": 2.7788460732666,
-"velocity": 1.603133504863427
-},
-{
-"acceleration": -2.4999999999999987,
-"curvature": -0.28711499522644574,
-"pose": {
-"rotation": {
-"radians": 0.41181749668319956
-},
-"translation": {
-"x": 4.594534681063377,
-"y": 8.031423710219563
-}
-},
-"time": 2.842931715609543,
-"velocity": 1.4429193990060702
-},
-{
-"acceleration": -2.5000000000000013,
-"curvature": -0.4494774646558957,
-"pose": {
-"rotation": {
-"radians": 0.3808257614847099
-},
-"translation": {
-"x": 4.673932513332018,
-"y": 8.064756676506006
-}
-},
-"time": 2.9060626753624996,
-"velocity": 1.2850919996236787
-},
-{
-"acceleration": -2.499999999999999,
-"curvature": -0.7168984723804336,
-"pose": {
-"rotation": {
-"radians": 0.3384693676967129
-},
-"translation": {
-"x": 4.743619039536623,
-"y": 8.091089754646195
-}
-},
-"time": 2.9677311616912334,
-"velocity": 1.1309207838018438
-},
-{
-"acceleration": -2.5,
-"curvature": -1.1581588114975483,
-"pose": {
-"rotation": {
-"radians": 0.28075160860854026
-},
-"translation": {
-"x": 4.803765143504194,
-"y": 8.110483228214246
-}
-},
-"time": 3.0275682560088337,
-"velocity": 0.9813280480078428
-},
-{
-"acceleration": -2.500000000000002,
-"curvature": -1.8508023755755092,
-"pose": {
-"rotation": {
-"radians": 0.20323075479464445
-},
-"translation": {
-"x": 4.854892396835428,
-"y": 8.123267191487901
-}
-},
-"time": 3.0855554157821157,
-"velocity": 0.8363601485746381
-},
-{
-"acceleration": -2.4999999999999973,
-"curvature": -2.293209588612769,
-"pose": {
-"rotation": {
-"radians": 0.15612080064004036
-},
-"translation": {
-"x": 4.877341253232068,
-"y": 8.127364202540841
-}
-},
-"time": 3.114053720917878,
-"velocity": 0.7651143857352323
-},
-{
-"acceleration": -2.5,
-"curvature": -2.762404865863379,
-"pose": {
-"rotation": {
-"radians": 0.10375241767782481
-},
-"translation": {
-"x": 4.897907258904963,
-"y": 8.130068632421755
-}
-},
-"time": 3.1424854340095765,
-"velocity": 0.6940351030059861
-},
-{
-"acceleration": -2.5000000000000018,
-"curvature": -3.180991941816036,
-"pose": {
-"rotation": {
-"radians": 0.047421308117279064
-},
-"translation": {
-"x": 4.916769470928166,
-"y": 8.131509687562275
-}
-},
-"time": 3.171230408776405,
-"velocity": 0.622172666088914
-},
-{
-"acceleration": -2.500000000000001,
-"curvature": -3.419771804615093,
-"pose": {
-"rotation": {
-"radians": -0.010257700786406183
-},
-"translation": {
-"x": 4.934135276861628,
-"y": 8.131838515620494
-}
-},
-"time": 3.2009176394868546,
-"velocity": 0.5479545893127904
-},
-{
-"acceleration": -2.500000000000001,
-"curvature": -3.3134081112658187,
-"pose": {
-"rotation": {
-"radians": -0.06511207265007189
-},
-"translation": {
-"x": 4.950241463501213,
-"y": 8.131229051823862
-}
-},
-"time": 3.2326254605596336,
-"velocity": 0.4686850366308429
-},
-{
-"acceleration": -2.5,
-"curvature": -2.7143009734792893,
-"pose": {
-"rotation": {
-"radians": -0.11157434870245536
-},
-"translation": {
-"x": 4.965355285628698,
-"y": 8.129878865312136
-}
-},
-"time": 3.268417910660402,
-"velocity": 0.3792039113789212
-},
-{
-"acceleration": -2.5,
-"curvature": -1.4413328847787678e-14,
-"pose": {
-"rotation": {
-"radians": -0.1549967419239427
-},
-"translation": {
-"x": 4.9938336079041274,
-"y": 8.12586984832129
-}
-},
-"time": 3.4200994752119707,
-"velocity": 0.0
-}
-]
\ No newline at end of file
diff --git a/src/main/deploy/Trajectories/output/Shoot.wpilib.json b/src/main/deploy/Trajectories/output/Shoot.wpilib.json
deleted file mode 100644
index de91a0a..0000000
--- a/src/main/deploy/Trajectories/output/Shoot.wpilib.json
+++ /dev/null
@@ -1,692 +0,0 @@
-[
-{
-"acceleration": -2.4999999999999996,
-"curvature": -0.0,
-"pose": {
-"rotation": {
-"radians": -2.776308690438734
-},
-"translation": {
-"x": 1.215,
-"y": 1.1980000000000004
-}
-},
-"time": 0.0,
-"velocity": -0.0
-},
-{
-"acceleration": -2.4999999999999996,
-"curvature": 0.009470301865523842,
-"pose": {
-"rotation": {
-"radians": -2.780829312391785
-},
-"translation": {
-"x": 1.3243737108181883,
-"y": 1.2394555560590748
-}
-},
-"time": 0.30589742220584215,
-"velocity": -0.7647435555146053
-},
-{
-"acceleration": -2.5,
-"curvature": 0.0038440252282299224,
-"pose": {
-"rotation": {
-"radians": -2.78120628387852
-},
-"translation": {
-"x": 1.38198934617654,
-"y": 1.261178221855042
-}
-},
-"time": 0.37793245524563046,
-"velocity": -0.9448311381140759
-},
-{
-"acceleration": -2.5000000000000013,
-"curvature": 0.001433574065909168,
-"pose": {
-"rotation": {
-"radians": -2.7813992531694343
-},
-"translation": {
-"x": 1.45622490280161,
-"y": 1.2891439495048624
-}
-},
-"time": 0.4541978474718752,
-"velocity": -1.1354946186796877
-},
-{
-"acceleration": -2.499999999999999,
-"curvature": 0.0003153119884129276,
-"pose": {
-"rotation": {
-"radians": -2.7814767198219728
-},
-"translation": {
-"x": 1.5480133106510672,
-"y": 1.3237086369484539
-}
-},
-"time": 0.5336293357569316,
-"velocity": -1.3340733393923287
-},
-{
-"acceleration": -2.499999999999999,
-"curvature": -0.0002516123731138936,
-"pose": {
-"rotation": {
-"radians": -2.7814761064243805
-},
-"translation": {
-"x": 1.6576343210693787,
-"y": 1.364984163698713
-}
-},
-"time": 0.615197266373744,
-"velocity": -1.5379931659343595
-},
-{
-"acceleration": -2.499999999999998,
-"curvature": -0.00043131583506124665,
-"pose": {
-"rotation": {
-"radians": -2.7814533591447717
-},
-"translation": {
-"x": 1.7190566497125566,
-"y": 1.3881126054555146
-}
-},
-"time": 0.6564858643338709,
-"velocity": -1.641214660834677
-},
-{
-"acceleration": -2.499999999999999,
-"curvature": -0.000570442353434645,
-"pose": {
-"rotation": {
-"radians": -2.7814179367798406
-},
-"translation": {
-"x": 1.7847786234206091,
-"y": 1.41286226419275
-}
-},
-"time": 0.6979654816330555,
-"velocity": -1.7449137040826384
-},
-{
-"acceleration": -2.499999999999996,
-"curvature": -0.0006819866747854781,
-"pose": {
-"rotation": {
-"radians": -2.7813709810692333
-},
-"translation": {
-"x": 1.8546800565646744,
-"y": 1.4391891257686549
-}
-},
-"time": 0.7395347578699167,
-"velocity": -1.8488368946747915
-},
-{
-"acceleration": -2.5000000000000075,
-"curvature": -0.0007749876345776465,
-"pose": {
-"rotation": {
-"radians": -2.7813132971891843
-},
-"translation": {
-"x": 1.9286119617212152,
-"y": 1.4670384011431334
-}
-},
-"time": 0.7810980949526695,
-"velocity": -1.9527452373816732
-},
-{
-"acceleration": -2.4999999999999942,
-"curvature": -0.0008558787745902928,
-"pose": {
-"rotation": {
-"radians": -2.78124542199387
-},
-"translation": {
-"x": 2.0063985533167927,
-"y": 1.496345272419985
-}
-},
-"time": 0.8225652927742748,
-"velocity": -2.0564132319356867
-},
-{
-"acceleration": -2.4999999999999987,
-"curvature": -0.0009293623393541781,
-"pose": {
-"rotation": {
-"radians": -2.781167673037722
-},
-"translation": {
-"x": 2.087839251272843,
-"y": 1.5270356388891297
-}
-},
-"time": 0.8638511901560029,
-"velocity": -2.1596279753900065
-},
-{
-"acceleration": -2.5000000000000058,
-"curvature": -0.0009989857689404324,
-"pose": {
-"rotation": {
-"radians": -2.781080183514066
-},
-"translation": {
-"x": 2.1727106846504505,
-"y": 1.5590268630688353
-}
-},
-"time": 0.9048753272124228,
-"velocity": -2.2621883180310562
-},
-{
-"acceleration": -2.4999999999999956,
-"curvature": -0.001067530091013617,
-"pose": {
-"rotation": {
-"radians": -2.780982926688968
-},
-"translation": {
-"x": 2.260768695295124,
-"y": 1.5922285167479444
-}
-},
-"time": 0.9455616369494662,
-"velocity": -2.363904092373665
-},
-{
-"acceleration": -0.8523510946217816,
-"curvature": -0.0011372781767881785,
-"pose": {
-"rotation": {
-"radians": -2.7808757323217153
-},
-"translation": {
-"x": 2.35175034148157,
-"y": 1.6265431270280972
-}
-},
-"time": 0.9858381691330271,
-"velocity": -2.4645954228325673
-},
-{
-"acceleration": 0.0016257363232975107,
-"curvature": -0.0012102058741566798,
-"pose": {
-"rotation": {
-"radians": -2.7807582967968543
-},
-"translation": {
-"x": 2.445375901558471,
-"y": 1.6618669223659621
-}
-},
-"time": 1.0261590635863136,
-"velocity": -2.4989629813559553
-},
-{
-"acceleration": 0.0017294680069879227,
-"curvature": -0.001288123830882504,
-"pose": {
-"rotation": {
-"radians": -2.780630188144678
-},
-"translation": {
-"x": 2.5413508775932554,
-"y": 1.6980905786154592
-}
-},
-"time": 1.067209990665677,
-"velocity": -2.4988962433726973
-},
-{
-"acceleration": 0.0018707685910488298,
-"curvature": -0.0013727884821435146,
-"pose": {
-"rotation": {
-"radians": -2.780490846726981
-},
-"translation": {
-"x": 2.6393679990168755,
-"y": 1.7350999650699883
-}
-},
-"time": 1.109137678536383,
-"velocity": -2.498823730777918
-},
-{
-"acceleration": 0.0020516289002939477,
-"curvature": -0.0014659949298390731,
-"pose": {
-"rotation": {
-"radians": -2.780339582064294
-},
-"translation": {
-"x": 2.7391092262685834,
-"y": 1.7727768905046526
-}
-},
-"time": 1.151806508383016,
-"velocity": -2.498743907271224
-},
-{
-"acceleration": 0.002276293630410435,
-"curvature": -0.0015696609489784805,
-"pose": {
-"rotation": {
-"radians": -2.7801755660459344
-},
-"translation": {
-"x": 2.840247754440704,
-"y": 1.8109998492184882
-}
-},
-"time": 1.1950771322080926,
-"velocity": -2.498655132008851
-},
-{
-"acceleration": 0.0025513390782631426,
-"curvature": -0.0016859093287968537,
-"pose": {
-"rotation": {
-"radians": -2.7799978225711537
-},
-"translation": {
-"x": 2.9424500169234085,
-"y": 1.849644767076689
-}
-},
-"time": 1.238807329496436,
-"velocity": -2.4985555892393068
-},
-{
-"acceleration": 0.002885963904576762,
-"curvature": -0.0018171547297049621,
-"pose": {
-"rotation": {
-"radians": -2.7798052135012234
-},
-"translation": {
-"x": 3.0453776890494924,
-"y": 1.8885857475528312
-}
-},
-"time": 1.2828528643955837,
-"velocity": -2.4984432141448956
-},
-{
-"acceleration": 0.003292502582493258,
-"curvature": -0.0019662009505565545,
-"pose": {
-"rotation": {
-"radians": -2.779596420645328
-},
-"translation": {
-"x": 3.1486896917391496,
-"y": 1.9276958177711023
-}
-},
-"time": 1.3270683434498216,
-"velocity": -2.4983156098683215
-},
-{
-"acceleration": 0.0037871988159726617,
-"curvature": -0.002136354823575126,
-"pose": {
-"rotation": {
-"radians": -2.7793699233475353
-},
-"translation": {
-"x": 3.2520441951447463,
-"y": 1.9668476745485277
-}
-},
-"time": 1.3713080739500803,
-"velocity": -2.4981699504414006
-},
-{
-"acceleration": 0.004391306248139896,
-"curvature": -0.0023315638477448613,
-"pose": {
-"rotation": {
-"radians": -2.7791239710795335
-},
-"translation": {
-"x": 3.3551006222955957,
-"y": 2.0059144304371923
-}
-},
-"time": 1.4154269229812038,
-"velocity": -2.498002863588588
-},
-{
-"acceleration": 0.005132620733169828,
-"curvature": -0.0025565861678775136,
-"pose": {
-"rotation": {
-"radians": -2.778856550267352
-},
-"translation": {
-"x": 3.4575216527427326,
-"y": 2.0447703597664724
-}
-},
-"time": 1.4592811772768943,
-"velocity": -2.4978102861276916
-},
-{
-"acceleration": 0.006047597428118208,
-"curvature": -0.002817203700964137,
-"pose": {
-"rotation": {
-"radians": -2.7785653443842775
-},
-"translation": {
-"x": 3.5589752262036933,
-"y": 2.083291644685259
-}
-},
-"time": 1.5027294040164818,
-"velocity": -2.4975872828583086
-},
-{
-"acceleration": 0.007184273444986679,
-"curvature": -0.0031204922602162356,
-"pose": {
-"rotation": {
-"radians": -2.778247686122804
-},
-"translation": {
-"x": 3.6591365462072813,
-"y": 2.121357121204184
-}
-},
-"time": 1.5456333127309305,
-"velocity": -2.497327817290311
-},
-{
-"acceleration": 0.008606311430250214,
-"curvature": -0.003475166649555502,
-"pose": {
-"rotation": {
-"radians": -2.7779005002146606
-},
-"translation": {
-"x": 3.7576900837383507,
-"y": 2.1588490252378483
-}
-},
-"time": 1.5878586185186474,
-"velocity": -2.4970244591472337
-},
-{
-"acceleration": 0.010398612817812404,
-"curvature": -0.003892024177880086,
-"pose": {
-"rotation": {
-"radians": -2.7775202352041024
-},
-"translation": {
-"x": 3.854331580882577,
-"y": 2.1956537386470467
-}
-},
-"time": 1.629275906808418,
-"velocity": -2.4966680090656155
-},
-{
-"acceleration": 0.012675136836903718,
-"curvature": -0.00438451720138259,
-"pose": {
-"rotation": {
-"radians": -2.7771027822093197
-},
-"translation": {
-"x": 3.94877005447123,
-"y": 2.231662535280993
-}
-},
-"time": 1.6697614999453496,
-"velocity": -2.496247015057885
-},
-{
-"acceleration": 2.4003936999447104,
-"curvature": -0.004969494472149147,
-"pose": {
-"rotation": {
-"radians": -2.776643378460766
-},
-"translation": {
-"x": 4.0407297997259555,
-"y": 2.2667723270195506
-}
-},
-"time": 1.7091983259087289,
-"velocity": -2.495747147892386
-},
-{
-"acceleration": 2.499999999999993,
-"curvature": -0.005668162461644537,
-"pose": {
-"rotation": {
-"radians": -2.7761364932435937
-},
-"translation": {
-"x": 4.129952393903545,
-"y": 2.300886409815453
-}
-},
-"time": 1.7482038649779132,
-"velocity": -2.4021184976477685
-},
-{
-"acceleration": 2.499999999999995,
-"curvature": -0.0065073312562546545,
-"pose": {
-"rotation": {
-"radians": -2.7755756939060676
-},
-"translation": {
-"x": 4.216198699940706,
-"y": 2.3339152097365314
-}
-},
-"time": 1.7874525226655171,
-"velocity": -2.303996853428759
-},
-{
-"acceleration": 2.5000000000000027,
-"curvature": -0.007521023934270008,
-"pose": {
-"rotation": {
-"radians": -2.7749534900267996
-},
-"translation": {
-"x": 4.299250870098854,
-"y": 2.3657770290079503
-}
-},
-"time": 1.826905623866848,
-"velocity": -2.205364100425432
-},
-{
-"acceleration": 2.500000000000008,
-"curvature": -0.00875254021342987,
-"pose": {
-"rotation": {
-"radians": -2.7742611550104406
-},
-"translation": {
-"x": 4.378914349608862,
-"y": 2.396398792054417
-}
-},
-"time": 1.8664932396622007,
-"velocity": -2.1063950609370496
-},
-{
-"acceleration": 2.5,
-"curvature": -0.010257066759571724,
-"pose": {
-"rotation": {
-"radians": -2.7734885269056426
-},
-"translation": {
-"x": 4.455019880315861,
-"y": 2.4257167915424196
-}
-},
-"time": 1.9061451884474412,
-"velocity": -2.0072651889739483
-},
-{
-"acceleration": 2.5,
-"curvature": -0.012104900897479282,
-"pose": {
-"rotation": {
-"radians": -2.772623795129553
-},
-"translation": {
-"x": 4.527425504323997,
-"y": 2.4536774344224552
-}
-},
-"time": 1.9457920000388955,
-"velocity": -1.9081481599953125
-},
-{
-"acceleration": 2.500000000000003,
-"curvature": -0.014385266764878337,
-"pose": {
-"rotation": {
-"radians": -2.7716532887426912
-},
-"translation": {
-"x": 4.596018567641216,
-"y": 2.480237987971244
-}
-},
-"time": 1.9853662641221383,
-"velocity": -1.8092124997872052
-},
-{
-"acceleration": 2.5,
-"curvature": -0.01721048464658507,
-"pose": {
-"rotation": {
-"radians": -2.770561297740816
-},
-"translation": {
-"x": 4.66071772382403,
-"y": 2.50536732583397
-}
-},
-"time": 2.0248045147840203,
-"velocity": -1.7106168731325004
-},
-{
-"acceleration": 2.5,
-"curvature": -0.025081005125496554,
-"pose": {
-"rotation": {
-"radians": -2.7679394998451015
-},
-"translation": {
-"x": 4.778277488624,
-"y": 2.5512703571775948
-}
-},
-"time": 2.1030557082495553,
-"velocity": -1.5149888894686632
-},
-{
-"acceleration": 2.5,
-"curvature": -0.03713811368278128,
-"pose": {
-"rotation": {
-"radians": -2.764595059439821
-},
-"translation": {
-"x": 4.880156316648892,
-"y": 2.591397994588508
-}
-},
-"time": 2.1802476428256354,
-"velocity": -1.322009053028463
-},
-{
-"acceleration": 2.499999999999999,
-"curvature": -0.054727096472794126,
-"pose": {
-"rotation": {
-"radians": -2.7603676512642665
-},
-"translation": {
-"x": 4.9670348798664214,
-"y": 2.625995152799687
-}
-},
-"time": 2.256478445860014,
-"velocity": -1.1314320454425169
-},
-{
-"acceleration": 2.5,
-"curvature": -0.07666361134623467,
-"pose": {
-"rotation": {
-"radians": -2.7552141300512027
-},
-"translation": {
-"x": 5.040287120919804,
-"y": 2.6555640684931383
-}
-},
-"time": 2.332718767692783,
-"velocity": -0.940831240860594
-},
-{
-"acceleration": 2.5000000000000004,
-"curvature": -0.09232262723517723,
-"pose": {
-"rotation": {
-"radians": -2.7494950034586845
-},
-"translation": {
-"x": 5.102044369760608,
-"y": 2.68088817365114
-}
-},
-"time": 2.4120194271746134,
-"velocity": -0.7425795921560175
-},
-{
-"acceleration": 2.5000000000000004,
-"curvature": -4.880412174163198e-14,
-"pose": {
-"rotation": {
-"radians": -2.742250199907076
-},
-"translation": {
-"x": 5.203770846949043,
-"y": 2.723484896898727
-}
-},
-"time": 2.7090512640370203,
-"velocity": -0.0
-}
-]
\ No newline at end of file
diff --git a/src/main/deploy/Trajectories/output/StraightLine.wpilib.json b/src/main/deploy/Trajectories/output/StraightLine.wpilib.json
deleted file mode 100644
index 79c13ae..0000000
--- a/src/main/deploy/Trajectories/output/StraightLine.wpilib.json
+++ /dev/null
@@ -1,1127 +0,0 @@
-[
-{
-"acceleration": 2.4999999999999996,
-"curvature": 0.0,
-"pose": {
-"rotation": {
-"radians": -0.003952548586750195
-},
-"translation": {
-"x": 0.09529803018936864,
-"y": 8.223840559875583
-}
-},
-"time": 0.0,
-"velocity": 0.0
-},
-{
-"acceleration": 2.4999999999999996,
-"curvature": 0.0014757565594660205,
-"pose": {
-"rotation": {
-"radians": -0.0038639698440911196
-},
-"translation": {
-"x": 0.20699779464365825,
-"y": 8.223402461629103
-}
-},
-"time": 0.2989322646817893,
-"velocity": 0.7473306617044732
-},
-{
-"acceleration": 2.5000000000000004,
-"curvature": 0.002197745288810409,
-"pose": {
-"rotation": {
-"radians": -0.0036398014816034236
-},
-"translation": {
-"x": 0.3245524415952006,
-"y": 8.222960582475423
-}
-},
-"time": 0.42825796060667476,
-"velocity": 1.0706449015166868
-},
-{
-"acceleration": 2.5000000000000004,
-"curvature": 0.002292780080821684,
-"pose": {
-"rotation": {
-"radians": -0.0034986867150578467
-},
-"translation": {
-"x": 0.3870535104581222,
-"y": 8.222737469374282
-}
-},
-"time": 0.4831211592296284,
-"velocity": 1.207802898074071
-},
-{
-"acceleration": 2.5000000000000036,
-"curvature": 0.0022646105626719006,
-"pose": {
-"rotation": {
-"radians": -0.0033485382418383075
-},
-"translation": {
-"x": 0.45270711898572513,
-"y": 8.222512706455358
-}
-},
-"time": 0.534723525856003,
-"velocity": 1.3368088146400074
-},
-{
-"acceleration": 2.5000000000000027,
-"curvature": 0.0021562050227736103,
-"pose": {
-"rotation": {
-"radians": -0.0031952105808478984
-},
-"translation": {
-"x": 0.5219297482774266,
-"y": 8.222286261486461
-}
-},
-"time": 0.5842154130457918,
-"velocity": 1.4605385326144797
-},
-{
-"acceleration": 2.4999999999999947,
-"curvature": 0.0020030226767829796,
-"pose": {
-"rotation": {
-"radians": -0.0030429708129027446
-},
-"translation": {
-"x": 0.5950721658560649,
-"y": 8.222058191427937
-}
-},
-"time": 0.6323146901396718,
-"velocity": 1.5807867253491799
-},
-{
-"acceleration": 2.5000000000000013,
-"curvature": 0.0018310197847122341,
-"pose": {
-"rotation": {
-"radians": -0.0028946945437942784
-},
-"translation": {
-"x": 0.672422031657764,
-"y": 8.22182863784712
-}
-},
-"time": 0.6794865947967504,
-"velocity": 1.6987164869918763
-},
-{
-"acceleration": 2.4999999999999987,
-"curvature": 0.0016574187066549124,
-"pose": {
-"rotation": {
-"radians": -0.0027521412047645795
-},
-"translation": {
-"x": 0.7542065040217969,
-"y": 8.221597822332782
-}
-},
-"time": 0.7260371002683128,
-"velocity": 1.8150927506707824
-},
-{
-"acceleration": 2.4999999999999982,
-"curvature": 0.0014925770585591273,
-"pose": {
-"rotation": {
-"radians": -0.0026162314618522037
-},
-"translation": {
-"x": 0.8405948456804494,
-"y": 8.221366041909587
-}
-},
-"time": 0.7721662988241483,
-"velocity": 1.9304157470603711
-},
-{
-"acceleration": 2.4999999999999987,
-"curvature": 0.0013419773077737586,
-"pose": {
-"rotation": {
-"radians": -0.0024872883047609602
-},
-"translation": {
-"x": 0.9317010297488838,
-"y": 8.221133664452527
-}
-},
-"time": 0.8180012086647473,
-"velocity": 2.0450030216618686
-},
-{
-"acceleration": 2.499999999999996,
-"curvature": 0.0012079019882565245,
-"pose": {
-"rotation": {
-"radians": -0.0023652289989504784
-},
-"translation": {
-"x": 1.0275863457150032,
-"y": 8.220901124101388
-}
-},
-"time": 0.8636170770263865,
-"velocity": 2.1590426925659663
-},
-{
-"acceleration": 2.499999999999999,
-"curvature": 0.0010906820876119118,
-"pose": {
-"rotation": {
-"radians": -0.002249709039486769
-},
-"translation": {
-"x": 1.1282620054293144,
-"y": 8.220668916675196
-}
-},
-"time": 0.9090518124597894,
-"velocity": 2.2726295311494735
-},
-{
-"acceleration": 2.499999999999999,
-"curvature": 0.00098955615954012,
-"pose": {
-"rotation": {
-"radians": -0.0021402252941754986
-},
-"translation": {
-"x": 1.2336917490947934,
-"y": 8.22043759508666
-}
-},
-"time": 0.9543160879310667,
-"velocity": 2.385790219827667
-},
-{
-"acceleration": 0.017153497744363047,
-"curvature": 0.0009032267580062521,
-"pose": {
-"rotation": {
-"radians": -0.002036187069267744
-},
-"translation": {
-"x": 1.343794451256746,
-"y": 8.220207764756626
-}
-},
-"time": 0.9994005950139156,
-"velocity": 2.498501487534789
-},
-{
-"acceleration": 0.0011010737497244127,
-"curvature": 0.0008302018931851028,
-"pose": {
-"rotation": {
-"radians": -0.0019369632100680464
-},
-"translation": {
-"x": 1.4584467267926755,
-"y": 8.219980079028533
-}
-},
-"time": 1.0452818751899042,
-"velocity": 2.4992885119707964
-},
-{
-"acceleration": 0.0008823497231114543,
-"curvature": 0.0007689928717578236,
-"pose": {
-"rotation": {
-"radians": -0.0018419119229067457
-},
-"translation": {
-"x": 1.5774855369021437,
-"y": 8.219755234582848
-}
-},
-"time": 1.0929105395072989,
-"velocity": 2.4993409546428107
-},
-{
-"acceleration": 0.0007417877197780542,
-"curvature": 0.0007182206861950503,
-"pose": {
-"rotation": {
-"radians": -0.001750398464876811
-},
-"translation": {
-"x": 1.7007107950966356,
-"y": 8.219533966851534
-}
-},
-"time": 1.1422132903916045,
-"velocity": 2.499384456911402
-},
-{
-"acceleration": 0.0006583271447637858,
-"curvature": 0.0006963606653970598,
-"pose": {
-"rotation": {
-"radians": -0.0017057744172242965
-},
-"translation": {
-"x": 1.7638213262887141,
-"y": 8.21942491354389
-}
-},
-"time": 1.1674636630435462,
-"velocity": 2.499403187327755
-},
-{
-"acceleration": 0.0005817174042701445,
-"curvature": 0.0006766666832309289,
-"pose": {
-"rotation": {
-"radians": -0.0016618044885475896
-},
-"translation": {
-"x": 1.827887973189423,
-"y": 8.21931704543249
-}
-},
-"time": 1.1930963907927288,
-"velocity": 2.4994200620482268
-},
-{
-"acceleration": 0.0005111994034113325,
-"curvature": 0.0006590144388055376,
-"pose": {
-"rotation": {
-"radians": -0.0016184145490667347
-},
-"translation": {
-"x": 1.8928764658052883,
-"y": 8.219210463303185
-}
-},
-"time": 1.2190977758176849,
-"velocity": 2.499435187506431
-},
-{
-"acceleration": 0.00044607321525439684,
-"curvature": 0.0006432909146300825,
-"pose": {
-"rotation": {
-"radians": -0.001575531738812017
-},
-"translation": {
-"x": 1.9587507072854293,
-"y": 8.219105269503999
-}
-},
-"time": 1.2454533893804791,
-"velocity": 2.4994486604803607
-},
-{
-"acceleration": 0.0003856946614932372,
-"curvature": 0.0006293941580971907,
-"pose": {
-"rotation": {
-"radians": -0.0015330843988382373
-},
-"translation": {
-"x": 2.0254728553587436,
-"y": 8.219001567801838
-}
-},
-"time": 1.272148104410426,
-"velocity": 2.4994605682777244
-},
-{
-"acceleration": 0.00032947106562269474,
-"curvature": 0.0006172329886621691,
-"pose": {
-"rotation": {
-"radians": -0.001491001971335141
-},
-"translation": {
-"x": 2.0930034037710907,
-"y": 8.218899463239184
-}
-},
-"time": 1.2991661280935576,
-"velocity": 2.499470988985223
-},
-{
-"acceleration": 0.00027685652941501743,
-"curvature": 0.0006067266663070081,
-"pose": {
-"rotation": {
-"radians": -0.0014492148741947143
-},
-"translation": {
-"x": 2.1613012637224736,
-"y": 8.218799061990806
-}
-},
-"time": 1.3264910344636154,
-"velocity": 2.499479991751243
-},
-{
-"acceleration": 0.00022734697358934807,
-"curvature": 0.0005978045485808431,
-"pose": {
-"rotation": {
-"radians": -0.0014076543544160626
-},
-"translation": {
-"x": 2.230323845304224,
-"y": 8.218700471220453
-}
-},
-"time": 1.3541057969988295,
-"velocity": 2.499487637078559
-},
-{
-"acceleration": 0.0001804751021799258,
-"curvature": 0.0005904057571126299,
-"pose": {
-"rotation": {
-"radians": -0.0013662523237348086
-},
-"translation": {
-"x": 2.300027138936182,
-"y": 8.218603798937563
-}
-},
-"time": 1.3819928212166186,
-"velocity": 2.4994939771091174
-},
-{
-"acceleration": 0.0001358053902187469,
-"curvature": 0.0005844788696252937,
-"pose": {
-"rotation": {
-"radians": -0.001324941179025441
-},
-"translation": {
-"x": 2.370365796803885,
-"y": 8.218509153853956
-}
-},
-"time": 1.4101339772771024,
-"velocity": 2.499499055887133
-},
-{
-"acceleration": 9.292914717832507e-05,
-"curvature": 0.0005799816498422984,
-"pose": {
-"rotation": {
-"radians": -0.0012836536093150126
-},
-"translation": {
-"x": 2.441293214295747,
-"y": 8.218416645240543
-}
-},
-"time": 1.438510632581321,
-"velocity": 2.4995029095898795
-},
-{
-"acceleration": 5.145967677934905e-05,
-"curvature": 0.0005768808250259114,
-"pose": {
-"rotation": {
-"radians": -0.0012423223906417424
-},
-"translation": {
-"x": 2.512761611440239,
-"y": 8.218326382784024
-}
-},
-"time": 1.4671036843768226,
-"velocity": 2.499505566717798
-},
-{
-"acceleration": 1.1027527491912405e-05,
-"curvature": 0.0005751519190300826,
-"pose": {
-"rotation": {
-"radians": -0.0012008801694724542
-},
-"translation": {
-"x": 2.584722114343082,
-"y": 8.218238476443592
-}
-},
-"time": 1.4958935923574865,
-"velocity": 2.4995070482371573
-},
-{
-"acceleration": -2.8724192083308296e-05,
-"curvature": 0.0005747791475400122,
-"pose": {
-"rotation": {
-"radians": -0.00115925923494121
-},
-"translation": {
-"x": 2.6571248366244187,
-"y": 8.21815303630763
-}
-},
-"time": 1.524860411286503,
-"velocity": 2.4995073676695494
-},
-{
-"acceleration": -6.814447245189876e-05,
-"curvature": 0.0005757553814901306,
-"pose": {
-"rotation": {
-"radians": -0.0011173912797729226
-},
-"translation": {
-"x": 2.7299189608560055,
-"y": 8.218070172450417
-}
-},
-"time": 1.5539838235690464,
-"velocity": 2.499506531123061
-},
-{
-"acceleration": -0.00010757909203549777,
-"curvature": 0.0005780821844160411,
-"pose": {
-"rotation": {
-"radians": -0.0010752071493974461
-},
-"translation": {
-"x": 2.8030528199983906,
-"y": 8.217989994788832
-}
-},
-"time": 1.5832431719043767,
-"velocity": 2.4995045372602043
-},
-{
-"acceleration": -0.0001473753999645347,
-"curvature": 0.0005817699296416762,
-"pose": {
-"rotation": {
-"radians": -0.001032636578430109
-},
-"translation": {
-"x": 2.8764739788380993,
-"y": 8.217912612939044
-}
-},
-"time": 1.612617491870285,
-"velocity": 2.499501377197533
-},
-{
-"acceleration": -0.00018788727060218747,
-"curvature": 0.0005868380036887465,
-"pose": {
-"rotation": {
-"radians": -0.0009896079133839845
-},
-"translation": {
-"x": 2.9501293154248174,
-"y": 8.217838136073224
-}
-},
-"time": 1.642085544538218,
-"velocity": 2.499497034331485
-},
-{
-"acceleration": -0.00022948031882314947,
-"curvature": 0.0005933151030971133,
-"pose": {
-"rotation": {
-"radians": -0.0009460478201782372
-},
-"translation": {
-"x": 3.023965102508574,
-"y": 8.21776667277625
-}
-},
-"time": 1.671625849100425,
-"velocity": 2.499491484084288
-},
-{
-"acceleration": -0.00027253747894889513,
-"curvature": 0.000601239632953561,
-"pose": {
-"rotation": {
-"radians": -0.0009018809747073469
-},
-"translation": {
-"x": 3.0979270889769253,
-"y": 8.217698330902394
-}
-},
-"time": 1.7012167154756008,
-"velocity": 2.499484693562838
-},
-{
-"acceleration": -0.00031746506678729245,
-"curvature": 0.0006106602168482402,
-"pose": {
-"rotation": {
-"radians": -0.0008570297344299966
-},
-"translation": {
-"x": 3.171960581292136,
-"y": 8.217633217432034
-}
-},
-"time": 1.7308362769359105,
-"velocity": 2.49947662112223
-},
-{
-"acceleration": -0.00036469946278631286,
-"curvature": 0.000621636329732692,
-"pose": {
-"rotation": {
-"radians": -0.0008114137886163132
-},
-"translation": {
-"x": 3.2460105249283635,
-"y": 8.217571438328358
-}
-},
-"time": 1.760462522727891,
-"velocity": 2.499467215824131
-},
-{
-"acceleration": -0.00041471457813258857,
-"curvature": 0.0006342390672745381,
-"pose": {
-"rotation": {
-"radians": -0.000764949784550372
-},
-"translation": {
-"x": 3.3200215858088433,
-"y": 8.217513098394054
-}
-},
-"time": 1.7900733306993126,
-"velocity": 2.4994564167783713
-},
-{
-"acceleration": -0.00046803029480315994,
-"curvature": 0.0006485520678402742,
-"pose": {
-"rotation": {
-"radians": -0.0007175509266135635
-},
-"translation": {
-"x": 3.39393823174307,
-"y": 8.217458301128024
-}
-},
-"time": 1.8196464999285404,
-"velocity": 2.4994441523539703
-},
-{
-"acceleration": -0.0005252221053360621,
-"curvature": 0.0006646726062540342,
-"pose": {
-"rotation": {
-"radians": -0.0006691265447654191
-},
-"translation": {
-"x": 3.467704813863979,
-"y": 8.217407148582078
-}
-},
-"time": 1.849159783360869,
-"velocity": 2.499430339243225
-},
-{
-"acceleration": -0.0005869322223206138,
-"curvature": 0.0006827128820603373,
-"pose": {
-"rotation": {
-"radians": -0.0006195816284828989
-},
-"translation": {
-"x": 3.5412656480651354,
-"y": 8.217359741217638
-}
-},
-"time": 1.8785909204460665,
-"velocity": 2.4994148813594426
-},
-{
-"acceleration": -0.000653882482722582,
-"curvature": 0.0007028015292678062,
-"pose": {
-"rotation": {
-"radians": -0.0005688163217071386
-},
-"translation": {
-"x": 3.614565096437909,
-"y": 8.217316177762438
-}
-},
-"time": 1.907917669783406,
-"velocity": 2.4993976685452806
-},
-{
-"acceleration": -0.0007268894387097597,
-"curvature": 0.0007250853795999677,
-"pose": {
-"rotation": {
-"radians": -0.0005167253737674342
-},
-"translation": {
-"x": 3.6875476487086662,
-"y": 8.21727655506723
-}
-},
-"time": 1.9371178417734012,
-"velocity": 2.499378575064324
-},
-{
-"acceleration": -0.0008068821113569873,
-"curvature": 0.000749731517290961,
-"pose": {
-"rotation": {
-"radians": -0.00046319754059323667
-},
-"translation": {
-"x": 3.760158003675946,
-"y": 8.217240967962478
-}
-},
-"time": 1.9661693312740591,
-"velocity": 2.499357457843427
-},
-{
-"acceleration": -0.0008949229890613607,
-"curvature": 0.0007769296706392163,
-"pose": {
-"rotation": {
-"radians": -0.0004081149297719887
-},
-"translation": {
-"x": 3.8323411506476486,
-"y": 8.217209509115069
-}
-},
-"time": 1.9950501502666842,
-"velocity": 2.4993341544272205
-},
-{
-"acceleration": -0.000992232982150394,
-"curvature": 0.000806894994118993,
-"pose": {
-"rotation": {
-"radians": -0.0003513522821473879
-},
-"translation": {
-"x": 3.9040424508782157,
-"y": 8.217182268885004
-}
-},
-"time": 2.023738460530208,
-"velocity": 2.4993084805988484
-},
-{
-"acceleration": -0.0011002212118236601,
-"curvature": 0.0008398713051548098,
-"pose": {
-"rotation": {
-"radians": -0.0002927761816596462
-},
-"translation": {
-"x": 3.9752077190058137,
-"y": 8.21715933518211
-}
-},
-"time": 2.052212606324881,
-"velocity": 2.4992802276122523
-},
-{
-"acceleration": -0.0012205207158639795,
-"curvature": 0.0008761348520669918,
-"pose": {
-"rotation": {
-"radians": -0.00023224418398451628
-},
-"translation": {
-"x": 4.045783304489519,
-"y": 8.217140793322734
-}
-},
-"time": 2.0804511470880334,
-"velocity": 2.4992491589707138
-},
-{
-"acceleration": -0.0013550314157301077,
-"curvature": 0.0009159987046688449,
-"pose": {
-"rotation": {
-"radians": -0.0001696038532044953
-},
-"translation": {
-"x": 4.115716173046501,
-"y": 8.217126725886448
-}
-},
-"time": 2.1084328901412093,
-"velocity": 2.4992150066736514
-},
-{
-"acceleration": -0.001505972018899275,
-"curvature": 0.0009598178771233114,
-"pose": {
-"rotation": {
-"radians": -0.0001046916942128764
-},
-"translation": {
-"x": 4.184953988089196,
-"y": 8.217117212572747
-}
-},
-"time": 2.1361369234132015,
-"velocity": 2.4991774668382254
-},
-{
-"acceleration": -0.001675942947165769,
-"curvature": 0.0010079953146798241,
-"pose": {
-"rotation": {
-"radians": -3.7331966772250605e-05
-},
-"translation": {
-"x": 4.253445192162514,
-"y": 8.217112330057759
-}
-},
-"time": 2.1635426481782978,
-"velocity": 2.4991361945835715
-},
-{
-"acceleration": -0.0018680029164790865,
-"curvature": 0.0010609889027242405,
-"pose": {
-"rotation": {
-"radians": 3.26646349199514e-05
-},
-"translation": {
-"x": 4.321139088380994,
-"y": 8.217112151850936
-}
-},
-"time": 2.1906298118140137,
-"velocity": 2.4990907980427175
-},
-{
-"acceleration": -0.0020857624721159584,
-"curvature": 0.0011193196893316852,
-"pose": {
-"rotation": {
-"radians": 0.00010550245571716745
-},
-"translation": {
-"x": 4.387985921866003,
-"y": 8.217116748151762
-}
-},
-"time": 2.217378540579008,
-"velocity": 2.4990408313393724
-},
-{
-"acceleration": -0.0023334986578797093,
-"curvature": 0.001183581552655584,
-"pose": {
-"rotation": {
-"radians": 0.0001814025274335036
-},
-"translation": {
-"x": 4.453936961182917,
-"y": 8.217126185706453
-}
-},
-"time": 2.2437693724160632,
-"velocity": 2.4989857863327187
-},
-{
-"acceleration": -0.0026162961215111086,
-"curvature": 0.0012544525938344446,
-"pose": {
-"rotation": {
-"radians": 0.0002606044158426303
-},
-"translation": {
-"x": 4.5189445797783065,
-"y": 8.217140527664661
-}
-},
-"time": 2.269783289782107,
-"velocity": 2.498925082891459
-},
-{
-"acceleration": -0.0029402214128425846,
-"curvature": 0.0013327085969534077,
-"pose": {
-"rotation": {
-"radians": 0.00034336829528863993
-},
-"translation": {
-"x": 4.582962337417113,
-"y": 8.217159833436167
-}
-},
-"time": 2.2954017525096235,
-"velocity": 2.498858057406786
-},
-{
-"acceleration": -0.003525297674898555,
-"curvature": 0.0014192389728831776,
-"pose": {
-"rotation": {
-"radians": 0.0004299772839612464
-},
-"translation": {
-"x": 4.645945061619834,
-"y": 8.217184158547592
-}
-},
-"time": 2.3206067307040863,
-"velocity": 2.498783949190188
-},
-{
-"acceleration": -0.8521148318481957,
-"curvature": 0.0016213658692083633,
-"pose": {
-"rotation": {
-"radians": 0.0006159939305937456
-},
-"translation": {
-"x": 4.7686315471999166,
-"y": 8.217248068621089
-}
-},
-"time": 2.3697069146992984,
-"velocity": 2.498610856425713
-},
-{
-"acceleration": -2.500000000000004,
-"curvature": 0.0018710376255277107,
-"pose": {
-"rotation": {
-"radians": 0.0008214873002111105
-},
-"translation": {
-"x": 4.8866711064066735,
-"y": 8.217332618989511
-}
-},
-"time": 2.4173358225422445,
-"velocity": 2.4580255576280075
-},
-{
-"acceleration": -2.500000000000004,
-"curvature": 0.002181941090125719,
-"pose": {
-"rotation": {
-"radians": 0.0010498667383886547
-},
-"translation": {
-"x": 4.9997563051968825,
-"y": 8.217438099455512
-}
-},
-"time": 2.46447225345664,
-"velocity": 2.3401844803420193
-},
-{
-"acceleration": -2.5000000000000044,
-"curvature": 0.0025726428550705066,
-"pose": {
-"rotation": {
-"radians": 0.0013052686957486688
-},
-"translation": {
-"x": 5.107607811585841,
-"y": 8.217564723934476
-}
-},
-"time": 2.511753111469766,
-"velocity": 2.221982335309204
-},
-{
-"acceleration": -2.5000000000000044,
-"curvature": 0.003068639087342333,
-"pose": {
-"rotation": {
-"radians": 0.001592753684926861
-},
-"translation": {
-"x": 5.209977001637219,
-"y": 8.217712625868982
-}
-},
-"time": 2.5590845537637508,
-"velocity": 2.1036537295742415
-},
-{
-"acceleration": -2.4999999999999973,
-"curvature": 0.0037054053211421165,
-"pose": {
-"rotation": {
-"radians": 0.001918565967400363
-},
-"translation": {
-"x": 5.306648565452944,
-"y": 8.21788185364324
-}
-},
-"time": 2.6063671786600975,
-"velocity": 1.985447167333374
-},
-{
-"acceleration": -2.4999999999999973,
-"curvature": 0.004533001146344881,
-"pose": {
-"rotation": {
-"radians": 0.0022904789777128216
-},
-"translation": {
-"x": 5.397443113163041,
-"y": 8.218072365997546
-}
-},
-"time": 2.6534956626765003,
-"velocity": 1.8676259572923672
-},
-{
-"acceleration": -2.5000000000000027,
-"curvature": 0.0056231289136125726,
-"pose": {
-"rotation": {
-"radians": 0.002718258768967166
-},
-"translation": {
-"x": 5.482219780915516,
-"y": 8.218284027442738
-}
-},
-"time": 2.70035840714707,
-"velocity": 1.7504690961159433
-},
-{
-"acceleration": -2.5,
-"curvature": 0.0070801229085780395,
-"pose": {
-"rotation": {
-"radians": 0.003214290879821792
-},
-"translation": {
-"x": 5.560878836866208,
-"y": 8.218516603674635
-}
-},
-"time": 2.746837234880306,
-"velocity": 1.6342720267828528
-},
-{
-"acceleration": -2.5000000000000027,
-"curvature": 0.009058322332772345,
-"pose": {
-"rotation": {
-"radians": 0.0037944343157270423
-},
-"translation": {
-"x": 5.633364287168652,
-"y": 8.218769756988495
-}
-},
-"time": 2.792807207513391,
-"velocity": 1.51934709520014
-},
-{
-"acceleration": -2.5,
-"curvature": 0.01563033924242775,
-"pose": {
-"rotation": {
-"radians": 0.005295310719130757
-},
-"translation": {
-"x": 5.759824721370665,
-"y": 8.219335899527024
-}
-},
-"time": 2.8826878274066536,
-"velocity": 1.2946455454669832
-},
-{
-"acceleration": -2.4999999999999987,
-"curvature": 0.029167350108493237,
-"pose": {
-"rotation": {
-"radians": 0.007473785460967018
-},
-"translation": {
-"x": 5.862126920318707,
-"y": 8.219977511158225
-}
-},
-"time": 2.9688820959805837,
-"velocity": 1.0791598740321575
-},
-{
-"acceleration": -2.5,
-"curvature": 0.05920481659829719,
-"pose": {
-"rotation": {
-"radians": 0.010771605221467697
-},
-"translation": {
-"x": 5.941663312124419,
-"y": 8.220687700097777
-}
-},
-"time": 3.050257428221715,
-"velocity": 0.8757215434293284
-},
-{
-"acceleration": -2.5,
-"curvature": 0.2769737286574105,
-"pose": {
-"rotation": {
-"radians": 0.023977875365034578
-},
-"translation": {
-"x": 6.042841693381755,
-"y": 8.22227562291799
-}
-},
-"time": 3.1962192423104945,
-"velocity": 0.5108170082073807
-},
-{
-"acceleration": -2.5,
-"curvature": -1.9894888306118257e-15,
-"pose": {
-"rotation": {
-"radians": 0.040793682867864334
-},
-"translation": {
-"x": 6.095000000000002,
-"y": 8.224
-}
-},
-"time": 3.4005460455934466,
-"velocity": 0.0
-}
-]
\ No newline at end of file
diff --git a/src/main/deploy/Trajectories/output/TwoBallAuto.wpilib.json b/src/main/deploy/Trajectories/output/TwoBallAuto.wpilib.json
deleted file mode 100644
index 4c1e0ae..0000000
--- a/src/main/deploy/Trajectories/output/TwoBallAuto.wpilib.json
+++ /dev/null
@@ -1,302 +0,0 @@
-[
-{
-"acceleration": 2.5000000000000004,
-"curvature": 0.0,
-"pose": {
-"rotation": {
-"radians": -1.5707963267948966
-},
-"translation": {
-"x": 7.6250470039337666,
-"y": 2.191643891318268
-}
-},
-"time": 0.0,
-"velocity": 0.0
-},
-{
-"acceleration": 2.4999999999999987,
-"curvature": 0.0,
-"pose": {
-"rotation": {
-"radians": -1.5707963267948966
-},
-"translation": {
-"x": 7.6250470039337666,
-"y": 2.1031536864720666
-}
-},
-"time": 0.2660679685286472,
-"velocity": 0.6651699213216181
-},
-{
-"acceleration": 2.499999999999999,
-"curvature": 0.0,
-"pose": {
-"rotation": {
-"radians": -1.5707963267948966
-},
-"translation": {
-"x": 7.6250470039337666,
-"y": 2.0350121763219278
-}
-},
-"time": 0.3539849883781405,
-"velocity": 0.8849624709453512
-},
-{
-"acceleration": 2.5000000000000013,
-"curvature": 0.0,
-"pose": {
-"rotation": {
-"radians": -1.5707963267948966
-},
-"translation": {
-"x": 7.6250470039337666,
-"y": 1.972304358550402
-}
-},
-"time": 0.418893335127563,
-"velocity": 1.0472333378189074
-},
-{
-"acceleration": 2.5,
-"curvature": 0.0,
-"pose": {
-"rotation": {
-"radians": -1.5707963267948966
-},
-"translation": {
-"x": 7.6250470039337666,
-"y": 1.87674273817583
-}
-},
-"time": 0.501917246679122,
-"velocity": 1.2547931166978048
-},
-{
-"acceleration": 2.5,
-"curvature": 0.0,
-"pose": {
-"rotation": {
-"radians": -1.5707963267948966
-},
-"translation": {
-"x": 7.6250470039337666,
-"y": 1.8023727975888226
-}
-},
-"time": 0.5580473770062506,
-"velocity": 1.3951184425156264
-},
-{
-"acceleration": 2.4999999999999964,
-"curvature": 0.0,
-"pose": {
-"rotation": {
-"radians": -1.5707963267948966
-},
-"translation": {
-"x": 7.6250470039337666,
-"y": 1.7032740220379177
-}
-},
-"time": 0.6250567137662633,
-"velocity": 1.5626417844156582
-},
-{
-"acceleration": 2.5,
-"curvature": 0.0,
-"pose": {
-"rotation": {
-"radians": -1.5707963267948966
-},
-"translation": {
-"x": 7.6250470039337666,
-"y": 1.6431539953293246
-}
-},
-"time": 0.662413705165552,
-"velocity": 1.6560342629138798
-},
-{
-"acceleration": 2.5,
-"curvature": 0.0,
-"pose": {
-"rotation": {
-"radians": -1.5707963267948966
-},
-"translation": {
-"x": 7.6250470039337666,
-"y": 1.5754821297267094
-}
-},
-"time": 0.7020893171621735,
-"velocity": 1.7552232929054334
-},
-{
-"acceleration": 2.5000000000000027,
-"curvature": 0.0,
-"pose": {
-"rotation": {
-"radians": -1.5707963267948966
-},
-"translation": {
-"x": 7.6250470039337666,
-"y": 1.500025221628123
-}
-},
-"time": 0.7438379768149218,
-"velocity": 1.8595949420373044
-},
-{
-"acceleration": 1.053401544981922,
-"curvature": 0.0,
-"pose": {
-"rotation": {
-"radians": -1.5707963267948966
-},
-"translation": {
-"x": 7.6250470039337666,
-"y": 1.4166858132833218
-}
-},
-"time": 0.7873794907336341,
-"velocity": 1.9684487268340853
-},
-{
-"acceleration": -2.4999999999999933,
-"curvature": 0.0,
-"pose": {
-"rotation": {
-"radians": -1.5707963267948966
-},
-"translation": {
-"x": 7.6250470039337666,
-"y": 1.3255184102688409
-}
-},
-"time": 0.833133683901077,
-"velocity": 2.016646264606071
-},
-{
-"acceleration": -2.5,
-"curvature": 0.0,
-"pose": {
-"rotation": {
-"radians": -1.5707963267948966
-},
-"translation": {
-"x": 7.6250470039337666,
-"y": 1.2267456989630876
-}
-},
-"time": 0.8836971057736805,
-"velocity": 1.8902377099245624
-},
-{
-"acceleration": -2.5000000000000018,
-"curvature": 0.0,
-"pose": {
-"rotation": {
-"radians": -1.5707963267948966
-},
-"translation": {
-"x": 7.6250470039337666,
-"y": 1.1207747640214194
-}
-},
-"time": 0.9420078190028002,
-"velocity": 1.7444609268517632
-},
-{
-"acceleration": -2.499999999999999,
-"curvature": 0.0,
-"pose": {
-"rotation": {
-"radians": -1.5707963267948966
-},
-"translation": {
-"x": 7.6250470039337666,
-"y": 1.0082133058512248
-}
-},
-"time": 1.009828808653901,
-"velocity": 1.5749084527240114
-},
-{
-"acceleration": -2.500000000000001,
-"curvature": 0.0,
-"pose": {
-"rotation": {
-"radians": -1.5707963267948966
-},
-"translation": {
-"x": 7.6250470039337666,
-"y": 0.8898858580870144
-}
-},
-"time": 1.0900723490041722,
-"velocity": 1.3742996018483333
-},
-{
-"acceleration": -2.5,
-"curvature": 0.0,
-"pose": {
-"rotation": {
-"radians": -1.5707963267948966
-},
-"translation": {
-"x": 7.6250470039337666,
-"y": 0.7668500050654998
-}
-},
-"time": 1.1883907931071345,
-"velocity": 1.1285034915909276
-},
-{
-"acceleration": -2.5,
-"curvature": 0.0,
-"pose": {
-"rotation": {
-"radians": -1.5707963267948966
-},
-"translation": {
-"x": 7.6250470039337666,
-"y": 0.640412599300678
-}
-},
-"time": 1.319459087686586,
-"velocity": 0.8008327551422989
-},
-{
-"acceleration": -2.5,
-"curvature": 0.0,
-"pose": {
-"rotation": {
-"radians": -1.5707963267948966
-},
-"translation": {
-"x": 7.6250470039337666,
-"y": 0.5763988558366373
-}
-},
-"time": 1.413071433079771,
-"velocity": 0.5668018916593364
-},
-{
-"acceleration": -2.5,
-"curvature": 0.0,
-"pose": {
-"rotation": {
-"radians": -1.5707963267948966
-},
-"translation": {
-"x": 7.6250470039337666,
-"y": 0.5121459789589169
-}
-},
-"time": 1.6397921897435055,
-"velocity": 0.0
-}
-]
\ No newline at end of file
diff --git a/src/main/java/frc/robot/autos/curveTest.java b/src/main/java/frc/robot/autos/CurveTest.java
similarity index 83%
rename from src/main/java/frc/robot/autos/curveTest.java
rename to src/main/java/frc/robot/autos/CurveTest.java
index a03eb78..0b06f4c 100644
--- a/src/main/java/frc/robot/autos/curveTest.java
+++ b/src/main/java/frc/robot/autos/CurveTest.java
@@ -8,8 +8,8 @@
 import frc.robot.subsystems.DrivetrainSubsystem;
 import java.util.HashMap;
 
-public class curveTest extends SequentialCommandGroup {
-    public curveTest(DrivetrainSubsystem drivetrain, String path){
+public class CurveTest extends SequentialCommandGroup {
+    public CurveTest(DrivetrainSubsystem drivetrain, String path){
         PathPlannerTrajectory curveTest = PathPlanner.loadPath(path, Constants.DEFAULT_PATH_CONSTRAINTS, true);
         Command curveTest1 = drivetrain.ramseteAutoBuilderNoPoseReset(new HashMap<>()).fullAuto(curveTest);
 
diff --git a/src/main/java/frc/robot/autos/lineTest.java b/src/main/java/frc/robot/autos/LineTest.java
similarity index 83%
rename from src/main/java/frc/robot/autos/lineTest.java
rename to src/main/java/frc/robot/autos/LineTest.java
index 7dce922..16d09e9 100644
--- a/src/main/java/frc/robot/autos/lineTest.java
+++ b/src/main/java/frc/robot/autos/LineTest.java
@@ -9,9 +9,9 @@
 
 import java.util.HashMap;
 
-public class lineTest extends SequentialCommandGroup {
+public class LineTest extends SequentialCommandGroup {
     //CONSTRUCTOR
-    public lineTest(DrivetrainSubsystem drivetrain, String path) {
+    public LineTest(DrivetrainSubsystem drivetrain, String path) {
         PathPlannerTrajectory LineTest = PathPlanner.loadPath(path, Constants.DEFAULT_PATH_CONSTRAINTS, true);
         Command LineTest1 = drivetrain.ramseteAutoBuilderNoPoseReset(new HashMap<>()).fullAuto(LineTest);
         // STEP 1: drive!
diff --git a/src/main/java/org/littletonrobotics/FieldConstants.java b/src/main/java/org/littletonrobotics/FieldConstants.java
deleted file mode 100644
index 526d6f2..0000000
--- a/src/main/java/org/littletonrobotics/FieldConstants.java
+++ /dev/null
@@ -1,198 +0,0 @@
-//// Copyright (c) 2023 FRC 6328
-//// http://github.com/Mechanical-Advantage
-////
-//// Use of this source code is governed by an MIT-style
-//// license that can be found in the LICENSE file at
-//// the root directory of this project.
-//
-//package org.littletonrobotics;
-//
-//import edu.wpi.first.math.geometry.Pose2d;
-//import edu.wpi.first.math.geometry.Translation2d;
-//import edu.wpi.first.math.geometry.Translation3d;
-//import edu.wpi.first.math.util.Units;
-//
-///**
-// * Contains various field dimensions and useful reference points. Dimensions are in meters, and sets
-// * of corners start in the lower left moving clockwise.
-// *
-// * <p>All translations and poses are stored with the origin at the rightmost point on the BLUE
-// * ALLIANCE wall. Use the {@link #allianceFlip(Translation2d)} and {@link #allianceFlip(Pose2d)}
-// * methods to flip these values based on the current alliance color.
-// */
-//public final class FieldConstants {
-//    public static final double FIELD_LENGTH = Units.inchesToMeters(651.25);
-//    public static final double FIELD_WIDTH = Units.inchesToMeters(315.5);
-//    public static final double TAPE_WIDTH = Units.inchesToMeters(2.0);
-//    public static final double APRIL_TAG_WIDTH = Units.inchesToMeters(6.0);
-//
-//    // Dimensions for community and charging station, including the tape.
-//    public static final class Community {
-//        // Region dimensions
-//        public static final double INNER_X = 0.0;
-//        public static final double MID_X =
-//                Units.inchesToMeters(132.375); // Tape to the left of charging station
-//        public static final double OUTER_X =
-//                Units.inchesToMeters(193.25); // Tape to the right of charging station
-//        public static final double LEFT_Y = Units.feetToMeters(18.0);
-//        public static final double MID_Y = LEFT_Y - Units.inchesToMeters(59.39) + TAPE_WIDTH;
-//        public static final double RIGHT_Y = 0.0;
-//        public static final Translation2d[] REGION_CORNERS = {
-//                new Translation2d(INNER_X, RIGHT_Y),
-//                new Translation2d(INNER_X, LEFT_Y),
-//                new Translation2d(MID_X, LEFT_Y),
-//                new Translation2d(MID_X, MID_Y),
-//                new Translation2d(OUTER_X, MID_Y),
-//                new Translation2d(OUTER_X, RIGHT_Y),
-//        };
-//
-//        // Charging station dimensions
-//        public static final double CHARGING_STATION_LENGTH = Units.inchesToMeters(76.125);
-//        public static final double CHARGING_STATION_WIDTH = Units.inchesToMeters(97.25);
-//        public static final double CHARGING_STATION_OUTER_X = OUTER_X - TAPE_WIDTH;
-//        public static final double CHARGING_STATION_INNER_X =
-//                CHARGING_STATION_OUTER_X - CHARGING_STATION_LENGTH;
-//        public static final double CHARGING_STATION_LEFT_Y = MID_Y - TAPE_WIDTH;
-//        public static final double CHARGING_STATION_RIGHT_Y = CHARGING_STATION_LEFT_Y - CHARGING_STATION_WIDTH;
-//        public static final Translation2d[] CHARGING_STATION_CORNERS = {
-//                new Translation2d(CHARGING_STATION_INNER_X, CHARGING_STATION_RIGHT_Y),
-//                new Translation2d(CHARGING_STATION_INNER_X, CHARGING_STATION_LEFT_Y),
-//                new Translation2d(CHARGING_STATION_OUTER_X, CHARGING_STATION_RIGHT_Y),
-//                new Translation2d(CHARGING_STATION_OUTER_X, CHARGING_STATION_LEFT_Y)
-//        };
-//
-//        // Cable bump
-//        public static final double CABLE_BUMP_INNER_X =
-//                INNER_X + Grids.OUTER_X + Units.inchesToMeters(95.25);
-//        public static final double CABLE_BUMP_OUTER_X = CABLE_BUMP_INNER_X + Units.inchesToMeters(7);
-//        public static final Translation2d[] CABLE_BUMP_CORNERS = {
-//                new Translation2d(CABLE_BUMP_INNER_X, 0.0),
-//                new Translation2d(CABLE_BUMP_INNER_X, CHARGING_STATION_RIGHT_Y),
-//                new Translation2d(CABLE_BUMP_OUTER_X, 0.0),
-//                new Translation2d(CABLE_BUMP_OUTER_X, CHARGING_STATION_RIGHT_Y)
-//        };
-//    }
-//
-//    // Dimensions for grids and nodes
-//    public static final class Grids {
-//        // X layout
-//        public static final double OUTER_X = Units.inchesToMeters(54.25);
-//        public static final double LOW_X =
-//                OUTER_X - (Units.inchesToMeters(14.25) / 2.0); // Centered when under cube nodes
-//        public static final double MID_X = OUTER_X - Units.inchesToMeters(22.75);
-//        public static final double HIGH_X = OUTER_X - Units.inchesToMeters(39.75);
-//
-//        // Y layout
-//        public static final int NODE_ROW_COUNT = 9;
-//        public static final double NODE_FIRST_Y = Units.inchesToMeters(20.19);
-//        public static final double NODE_SEPARATION_Y = Units.inchesToMeters(22.0);
-//
-//        // Z layout
-//        public static final double CUBE_EDGE_HIGH = Units.inchesToMeters(3.0);
-//        public static final double HIGH_CUBE_Z = Units.inchesToMeters(35.5) - CUBE_EDGE_HIGH;
-//        public static final double MID_CUBE_Z = Units.inchesToMeters(23.5) - CUBE_EDGE_HIGH;
-//        public static final double HIGH_CONE_Z = Units.inchesToMeters(46.0);
-//        public static final double MID_CONE_Z = Units.inchesToMeters(34.0);
-//
-//        // Translations (all nodes in the same column/row have the same X/Y coordinate)
-//        public static final Translation2d[] LOW_TRANSLATIONS = new Translation2d[NODE_ROW_COUNT];
-//        public static final Translation2d[] MID_TRANSLATIONS = new Translation2d[NODE_ROW_COUNT];
-//        public static final Translation3d[] MID_3D_TRANSLATIONS = new Translation3d[NODE_ROW_COUNT];
-//        public static final Translation2d[] HIGH_TRANSLATIONS = new Translation2d[NODE_ROW_COUNT];
-//        public static final Translation3d[] HIGH_3D_TRANSLATIONS = new Translation3d[NODE_ROW_COUNT];
-//
-//        static {
-//            for (int i = 0; i < NODE_ROW_COUNT; i++) {
-//                boolean isCube = i == 1 || i == 4 || i == 7;
-//                LOW_TRANSLATIONS[i] = new Translation2d(LOW_X, NODE_FIRST_Y + NODE_SEPARATION_Y * i);
-//                MID_TRANSLATIONS[i] = new Translation2d(MID_X, NODE_FIRST_Y + NODE_SEPARATION_Y * i);
-//                MID_3D_TRANSLATIONS[i] =
-//                        new Translation3d(MID_X, NODE_FIRST_Y + NODE_SEPARATION_Y * i, isCube ? MID_CUBE_Z : MID_CONE_Z);
-//                HIGH_3D_TRANSLATIONS[i] =
-//                        new Translation3d(
-//                                HIGH_X, NODE_FIRST_Y + NODE_SEPARATION_Y * i, isCube ? HIGH_CUBE_Z : HIGH_CONE_Z);
-//                HIGH_TRANSLATIONS[i] = new Translation2d(HIGH_X, NODE_FIRST_Y + NODE_SEPARATION_Y * i);
-//            }
-//        }
-//
-//        // Complex low layout (shifted to account for cube vs cone rows and wide edge nodes)
-//        public static final double COMPLEX_LOW_X_CONES =
-//                OUTER_X - Units.inchesToMeters(16.0) / 2.0; // Centered X under cone nodes
-//        public static final double COMPLEX_LOW_X_CUBES = LOW_X; // Centered X under cube nodes
-//        public static final double COMPLEX_LOW_OUTER_Y_OFFSET =
-//                NODE_FIRST_Y - Units.inchesToMeters(3.0) - (Units.inchesToMeters(25.75) / 2.0);
-//
-//        public static final Translation2d[] COMPLEX_LOW_TRANSLATIONS = {
-//                new Translation2d(COMPLEX_LOW_X_CONES, NODE_FIRST_Y - COMPLEX_LOW_OUTER_Y_OFFSET),
-//                new Translation2d(COMPLEX_LOW_X_CUBES, NODE_FIRST_Y + NODE_SEPARATION_Y * 1),
-//                new Translation2d(COMPLEX_LOW_X_CONES, NODE_FIRST_Y + NODE_SEPARATION_Y * 2),
-//                new Translation2d(COMPLEX_LOW_X_CONES, NODE_FIRST_Y + NODE_SEPARATION_Y * 3),
-//                new Translation2d(COMPLEX_LOW_X_CUBES, NODE_FIRST_Y + NODE_SEPARATION_Y * 4),
-//                new Translation2d(COMPLEX_LOW_X_CONES, NODE_FIRST_Y + NODE_SEPARATION_Y * 5),
-//                new Translation2d(COMPLEX_LOW_X_CONES, NODE_FIRST_Y + NODE_SEPARATION_Y * 6),
-//                new Translation2d(COMPLEX_LOW_X_CUBES, NODE_FIRST_Y + NODE_SEPARATION_Y * 7),
-//                new Translation2d(
-//                        COMPLEX_LOW_X_CONES, NODE_FIRST_Y + NODE_SEPARATION_Y * 8 + COMPLEX_LOW_OUTER_Y_OFFSET),
-//        };
-//    }
-//
-//    // Dimensions for loading zone and substations, including the tape
-//    public static final class LoadingZone {
-//        // Region dimensions
-//        public static final double WIDTH = Units.inchesToMeters(99.0);
-//        public static final double INNER_X = FieldConstants.FIELD_LENGTH;
-//        public static final double MID_X = FIELD_LENGTH - Units.inchesToMeters(132.25);
-//        public static final double OUTER_X = FIELD_LENGTH - Units.inchesToMeters(264.25);
-//        public static final double LEFT_Y = FieldConstants.FIELD_WIDTH;
-//        public static final double MID_Y = LEFT_Y - Units.inchesToMeters(50.5);
-//        public static final double RIGHT_Y = LEFT_Y - WIDTH;
-//        public static final Translation2d[] REGION_CORNERS = {
-//                new Translation2d(
-//                        MID_X, RIGHT_Y), // Start at lower left next to border with opponent community
-//                new Translation2d(MID_X, MID_Y),
-//                new Translation2d(OUTER_X, MID_Y),
-//                new Translation2d(OUTER_X, LEFT_Y),
-//                new Translation2d(INNER_X, LEFT_Y),
-//                new Translation2d(INNER_X, RIGHT_Y),
-//        };
-//
-//        // Double substation dimensions
-//        public static final double DOUBLE_SUBSTATION_LENGTH = Units.inchesToMeters(14.0);
-//        public static final double DOUBLE_SUBSTATION_X = INNER_X - DOUBLE_SUBSTATION_LENGTH;
-//        public static final double DOUBLE_SUBSTATION_SHELF_Z = Units.inchesToMeters(37.375);
-//        public static final double DOUBLE_SUBSTATION_CENTER_Y = Units.inchesToMeters(265.74);
-//
-//        // Single substation dimensions
-//        public static final double SINGLE_SUBSTATION_WIDTH = Units.inchesToMeters(22.75);
-//        public static final double SINGLE_SUBSTATION_LEFT_X =
-//                FieldConstants.FIELD_LENGTH - DOUBLE_SUBSTATION_LENGTH - Units.inchesToMeters(88.77);
-//        public static final double SINGLE_SUBSTATION_CENTER_X =
-//                SINGLE_SUBSTATION_LEFT_X + (SINGLE_SUBSTATION_WIDTH / 2.0);
-//        public static final double SINGLE_SUBSTATION_RIGHT_X =
-//                SINGLE_SUBSTATION_LEFT_X + SINGLE_SUBSTATION_WIDTH;
-//        public static final Translation2d SINGLE_SUBSTATION_TRANSLATION =
-//                new Translation2d(SINGLE_SUBSTATION_CENTER_X, LEFT_Y);
-//
-//        public static final double SINGLE_SUBSTATION_HEIGHT = Units.inchesToMeters(18.0);
-//        public static final double SINGLE_SUBSTATION_LOW_Z = Units.inchesToMeters(27.125);
-//        public static final double SINGLE_SUBSTATION_CENTER_Z =
-//                SINGLE_SUBSTATION_LOW_Z + (SINGLE_SUBSTATION_HEIGHT / 2.0);
-//        public static final double SINGLE_SUBSTATION_HIGH_Z =
-//                SINGLE_SUBSTATION_LOW_Z + SINGLE_SUBSTATION_HEIGHT;
-//    }
-//
-//    // Locations of staged game pieces
-//    public static final class StagingLocations {
-//        public static final double CENTER_OFFSET_X = Units.inchesToMeters(47.36);
-//        public static final double POSITION_X = FIELD_LENGTH / 2.0 - Units.inchesToMeters(47.36);
-//        public static final double FIRST_Y = Units.inchesToMeters(36.19);
-//        public static final double SEPARATION_Y = Units.inchesToMeters(48.0);
-//        public static final Translation2d[] TRANSLATIONS = new Translation2d[4];
-//
-//        static {
-//            for (int i = 0; i < TRANSLATIONS.length; i++) {
-//                TRANSLATIONS[i] = new Translation2d(POSITION_X, FIRST_Y + (i * SEPARATION_Y));
-//            }
-//        }
-//    }
-//}

From 98dcb6c06071d45afb89b642ac4503331bfe7f47 Mon Sep 17 00:00:00 2001
From: PJ Reiniger <pj.reiniger@gmail.com>
Date: Sat, 4 Nov 2023 22:35:12 -0400
Subject: [PATCH 2/4] Fix styleguide

---
 build.gradle                                  |   3 +
 networktables.json                            |   1 +
 src/main/java/frc/robot/Constants.java        |  56 ++--
 src/main/java/frc/robot/Main.java             |  19 +-
 src/main/java/frc/robot/Robot.java            | 178 +++++++-----
 src/main/java/frc/robot/RobotContainer.java   | 221 +++++++-------
 .../java/frc/robot/ShooterLookupTable.java    |  46 ++-
 src/main/java/frc/robot/TunableNumber.java    |  81 ------
 .../frc/robot/autos/AutonomousFactory.java    |  32 +--
 src/main/java/frc/robot/autos/CurveTest.java  |   3 +-
 src/main/java/frc/robot/autos/LineTest.java   |   6 +-
 src/main/java/frc/robot/autos/SCurveTest.java |   6 +-
 src/main/java/frc/robot/autos/Shoot.java      |   2 +-
 .../java/frc/robot/autos/ShootEngage.java     |   4 +-
 .../frc/robot/autos/ShootLeaveCommunity.java  |   8 +-
 .../frc/robot/commands/AutoAimCommand.java    | 106 ++++---
 .../java/frc/robot/commands/AutoClimb.java    |  16 --
 .../frc/robot/commands/ClimbAutoCommand.java  |  59 ++--
 .../frc/robot/commands/ClimbPIDComand.java    |  47 +--
 .../frc/robot/commands/ClimberCommand.java    |  77 ++---
 .../java/frc/robot/commands/DriveCommand.java |  77 ++---
 .../robot/commands/DriveCommandWithPID.java   |   2 +-
 .../frc/robot/commands/ExampleCommand.java    |  43 ---
 .../frc/robot/commands/HopperCommand.java     |  79 ++---
 .../commands/KickIfShootSetRPMCommand.java    | 100 +++----
 .../KickIfShooterDistanceGoBrrCommand.java    | 108 +++----
 .../commands/RunClimberToBottomCommand.java   |  72 ++---
 .../frc/robot/commands/RunIntakeCommand.java  | 115 ++++----
 .../commands/SetShooterSpeedCommand.java      |  78 ++---
 .../commands/ShootFromDistanceCommand.java    |  92 +++---
 .../frc/robot/commands/ShooterPIDCommand.java |  79 ++---
 .../frc/robot/commands/StopIntakeCommand.java |  91 +++---
 .../frc/robot/commands/TowerDownCommand.java  |  79 ++---
 .../robot/commands/TowerKickerCommand.java    |  80 +++---
 .../frc/robot/commands/TowerUpCommand.java    |  83 +++---
 .../frc/robot/commands/TrajectoryCommand.java |  92 +++---
 .../robot/subsystems/ClimberSubsystem.java    | 125 ++++----
 .../robot/subsystems/DrivetrainSubsystem.java | 270 +++++++++---------
 .../robot/subsystems/ExampleSubsystem.java    |  22 --
 .../frc/robot/subsystems/HopperSubsystem.java |  42 +--
 .../frc/robot/subsystems/IntakeSubsystem.java |  75 ++---
 .../robot/subsystems/LimelightSubsystem.java  |  49 ++--
 .../frc/robot/subsystems/ShooterSubsytem.java | 126 ++++----
 .../frc/robot/subsystems/TowerSubsystem.java  |  82 +++---
 styleguide/checkstyle.xml                     | 244 ++++++++++++++++
 styleguide/pmd-ruleset.xml                    | 115 ++++++++
 styleguide/spotless.gradle                    |  47 +++
 styleguide/styleguide.gradle                  |  18 ++
 vendordeps/GirlsOfSteelLib.json               |  29 ++
 49 files changed, 1936 insertions(+), 1549 deletions(-)
 create mode 100644 networktables.json
 delete mode 100644 src/main/java/frc/robot/TunableNumber.java
 delete mode 100644 src/main/java/frc/robot/commands/AutoClimb.java
 delete mode 100644 src/main/java/frc/robot/commands/ExampleCommand.java
 delete mode 100644 src/main/java/frc/robot/subsystems/ExampleSubsystem.java
 create mode 100644 styleguide/checkstyle.xml
 create mode 100644 styleguide/pmd-ruleset.xml
 create mode 100644 styleguide/spotless.gradle
 create mode 100644 styleguide/styleguide.gradle
 create mode 100644 vendordeps/GirlsOfSteelLib.json

diff --git a/build.gradle b/build.gradle
index b233ef1..670c473 100644
--- a/build.gradle
+++ b/build.gradle
@@ -1,6 +1,7 @@
 plugins {
     id "java"
     id "edu.wpi.first.GradleRIO" version "2023.4.3"
+    id 'com.diffplug.spotless' version '6.12.0' apply false
 }
 
 sourceCompatibility = JavaVersion.VERSION_11
@@ -97,3 +98,5 @@ wpi.java.configureTestTasks(test)
 tasks.withType(JavaCompile) {
     options.compilerArgs.add '-XDstringConcat=inline'
 }
+
+apply from: "styleguide/styleguide.gradle"
diff --git a/networktables.json b/networktables.json
new file mode 100644
index 0000000..fe51488
--- /dev/null
+++ b/networktables.json
@@ -0,0 +1 @@
+[]
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 8334f08..f16ec4a 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -16,32 +16,32 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
-  // CAN IDs
-  public static final int DRIVE_LEFT_LEADER = 2;
-  public static final int DRIVE_LEFT_FOLLOWER = 3;
-  public static final int DRIVE_RIGHT_LEADER = 4;
-  public static final int DRIVE_RIGHT_FOLLOWER = 5;
-  public static final int TOWER_SPARK = 8;
-  public static final int TOWER_KICKER_SPARK = 9;
-  public static final int SHOOTER_SPARK = 10;
-  public static final int HOPPER_SPARK = 7;
-  public static final int SHOOTER_HOOD_SPARK = 11;
-  public static final int CLIMBER_LEFT = 12;
-  public static final int CLIMBER_RIGHT = 13;
-
-  // path constraints
-  public static final PathConstraints DEFAULT_PATH_CONSTRAINTS = new PathConstraints(Units.inchesToMeters(48), Units.inchesToMeters(48));
-
-
-  // DIO
-  public static final int LEFT_LIMIT_SWITCH = 1;
-  public static final int RIGHT_LIMIT_SWITCH = 0;
-
-  // Odometry
-  public static final double DRIVE_CONVERSION_FACTOR = 1;
-  public static final double DRIVE_TRACK = 0; // TODO
-
-  // Auto Aim
-  public static final double MAX_TURN_VELOCITY = 0.5;
-  public static final double MAX_TURN_ACCELERATION = 0.5;
+    // CAN IDs
+    public static final int DRIVE_LEFT_LEADER = 2;
+    public static final int DRIVE_LEFT_FOLLOWER = 3;
+    public static final int DRIVE_RIGHT_LEADER = 4;
+    public static final int DRIVE_RIGHT_FOLLOWER = 5;
+    public static final int TOWER_SPARK = 8;
+    public static final int TOWER_KICKER_SPARK = 9;
+    public static final int SHOOTER_SPARK = 10;
+    public static final int HOPPER_SPARK = 7;
+    public static final int SHOOTER_HOOD_SPARK = 11;
+    public static final int CLIMBER_LEFT = 12;
+    public static final int CLIMBER_RIGHT = 13;
+
+    // path constraints
+    public static final PathConstraints DEFAULT_PATH_CONSTRAINTS =
+            new PathConstraints(Units.inchesToMeters(48), Units.inchesToMeters(48));
+
+    // DIO
+    public static final int LEFT_LIMIT_SWITCH = 1;
+    public static final int RIGHT_LIMIT_SWITCH = 0;
+
+    // Odometry
+    public static final double DRIVE_CONVERSION_FACTOR = 1;
+    public static final double DRIVE_TRACK = 0; // TODO
+
+    // Auto Aim
+    public static final double MAX_TURN_VELOCITY = 0.5;
+    public static final double MAX_TURN_ACCELERATION = 0.5;
 }
diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java
index 8776e5d..e9c0192 100644
--- a/src/main/java/frc/robot/Main.java
+++ b/src/main/java/frc/robot/Main.java
@@ -12,14 +12,15 @@
  * call.
  */
 public final class Main {
-  private Main() {}
+    private Main() {
+    }
 
-  /**
-   * Main initialization function. Do not perform any initialization here.
-   *
-   * <p>If you change your main robot class, change the parameter type.
-   */
-  public static void main(String... args) {
-    RobotBase.startRobot(Robot::new);
-  }
+    /**
+     * Main initialization function. Do not perform any initialization here.
+     *
+     * <p>If you change your main robot class, change the parameter type.
+     */
+    public static void main(String... args) {
+        RobotBase.startRobot(Robot::new);
+    }
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 687a0a0..1e32f4a 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -14,90 +14,112 @@
  * the package after creating this project, you must also update the build.gradle file in the
  * project.
  */
+@SuppressWarnings("PMD.TooManyMethods")
 public class Robot extends TimedRobot {
-  private Command m_autonomousCommand;
-
-  private RobotContainer m_robotContainer;
-
-  /**
-   * This function is run when the robot is first started up and should be used for any
-   * initialization code.
-   */
-  @Override
-  public void robotInit() {
-    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
-    // autonomous chooser on the dashboard.
-    m_robotContainer = new RobotContainer();
-  }
-
-  /**
-   * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
-   * that you want ran during disabled, autonomous, teleoperated and test.
-   *
-   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
-   * SmartDashboard integrated updating.
-   */
-  @Override
-  public void robotPeriodic() {
-    // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
-    // commands, running already-scheduled commands, removing finished or interrupted commands,
-    // and running subsystem periodic() methods.  This must be called from the robot's periodic
-    // block in order for anything in the Command-based framework to work.
-    CommandScheduler.getInstance().run();
-  }
-
-  /** This function is called once each time the robot enters Disabled mode. */
-  @Override
-  public void disabledInit() {}
-
-  @Override
-  public void disabledPeriodic() {}
-
-  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
-  @Override
-  public void autonomousInit() {
-    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
-
-    // schedule the autonomous command (example)
-    if (m_autonomousCommand != null) {
-      m_autonomousCommand.schedule();
+    private Command m_autonomousCommand;
+
+    private RobotContainer m_robotContainer;
+
+    /**
+     * This function is run when the robot is first started up and should be used for any
+     * initialization code.
+     */
+    @Override
+    public void robotInit() {
+        // Instantiate our RobotContainer.    This will perform all our button bindings, and put our
+        // autonomous chooser on the dashboard.
+        m_robotContainer = new RobotContainer();
     }
-  }
-
-  /** This function is called periodically during autonomous. */
-  @Override
-  public void autonomousPeriodic() {}
-
-  @Override
-  public void teleopInit() {
-    // This makes sure that the autonomous stops running when
-    // teleop starts running. If you want the autonomous to
-    // continue until interrupted by another command, remove
-    // this line or comment it out.
-    if (m_autonomousCommand != null) {
-      m_autonomousCommand.cancel();
+
+    /**
+     * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
+     * that you want ran during disabled, autonomous, teleoperated and test.
+     *
+     * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+     * SmartDashboard integrated updating.
+     */
+    @Override
+    public void robotPeriodic() {
+        // Runs the Scheduler.    This is responsible for polling buttons, adding newly-scheduled
+        // commands, running already-scheduled commands, removing finished or interrupted commands,
+        // and running subsystem periodic() methods.    This must be called from the robot's periodic
+        // block in order for anything in the Command-based framework to work.
+        CommandScheduler.getInstance().run();
+    }
+
+    /**
+     * This function is called once each time the robot enters Disabled mode.
+     */
+    @Override
+    public void disabledInit() {
+    }
+
+    @Override
+    public void disabledPeriodic() {
+    }
+
+    /**
+     * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+     */
+    @Override
+    public void autonomousInit() {
+        m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+        // schedule the autonomous command (example)
+        if (m_autonomousCommand != null) {
+            m_autonomousCommand.schedule();
+        }
+    }
+
+    /**
+     * This function is called periodically during autonomous.
+     */
+    @Override
+    public void autonomousPeriodic() {
+    }
+
+    @Override
+    public void teleopInit() {
+        // This makes sure that the autonomous stops running when
+        // teleop starts running. If you want the autonomous to
+        // continue until interrupted by another command, remove
+        // this line or comment it out.
+        if (m_autonomousCommand != null) {
+            m_autonomousCommand.cancel();
+        }
     }
-  }
 
-  /** This function is called periodically during operator control. */
-  @Override
-  public void teleopPeriodic() {}
+    /**
+     * This function is called periodically during operator control.
+     */
+    @Override
+    public void teleopPeriodic() {
+    }
 
-  @Override
-  public void testInit() {
-    // Cancels all running commands at the start of test mode.
-    CommandScheduler.getInstance().cancelAll();
-  }
+    @Override
+    public void testInit() {
+        // Cancels all running commands at the start of test mode.
+        CommandScheduler.getInstance().cancelAll();
+    }
 
-  /** This function is called periodically during test mode. */
-  @Override
-  public void testPeriodic() {}
+    /**
+     * This function is called periodically during test mode.
+     */
+    @Override
+    public void testPeriodic() {
+    }
 
-  /** This function is called once when the robot is first started up. */
-  @Override
-  public void simulationInit() {}
+    /**
+     * This function is called once when the robot is first started up.
+     */
+    @Override
+    public void simulationInit() {
+    }
 
-  /** This function is called periodically whilst in simulation. */
-  @Override
-  public void simulationPeriodic() {}
+    /**
+     * This function is called periodically whilst in simulation.
+     */
+    @Override
+    public void simulationPeriodic() {
+    }
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index fdb32b2..15bdee9 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -20,116 +20,117 @@
  * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
  * subsystems, commands, and button mappings) should be declared here.
  */
+@SuppressWarnings("PMD.TooManyFields")
 public class RobotContainer {
 
-  private final XboxController m_operator = new XboxController(0);
-
-  private final CommandXboxController m_operatorCommand = new CommandXboxController(0); 
-
-  private final XboxController m_driver  = new XboxController(1);
-
-  private final CommandXboxController m_driverCommand = new CommandXboxController(1); 
-
-  // The robot's subsystems and commands are defined here...
-  private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem();
-  private final DrivetrainSubsystem m_drivetrainSubsystem = new DrivetrainSubsystem();
-  private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem();
-  private final ShooterSubsytem m_shooterSubsystem = new ShooterSubsytem();
-  private final TowerSubsystem m_towerSubsystem = new TowerSubsystem();
-  private final HopperSubsystem m_hopperSubsystem = new HopperSubsystem();
-
-  private final LimelightSubsystem m_limelightSubsystem = new LimelightSubsystem();
-
-  private final ShooterLookupTable m_shooterLookupTable = new ShooterLookupTable();
-
-  private final AutonomousFactory m_autonomousFactory = new AutonomousFactory(m_drivetrainSubsystem, m_shooterSubsystem);
-
-  private final RunIntakeCommand m_intakeInCommand =
-      new RunIntakeCommand(m_intakeSubsystem, 0.5, m_hopperSubsystem, m_towerSubsystem, 0.5);
-  private final RunIntakeCommand m_intakeOutCommand =
-      new RunIntakeCommand(m_intakeSubsystem, -0.5, m_hopperSubsystem, m_towerSubsystem, -0.5);
-  private final StopIntakeCommand m_intakeStopCommand =
-      new StopIntakeCommand(m_intakeSubsystem, m_hopperSubsystem);
-  private final DriveCommand m_driveCommand = new DriveCommand(m_drivetrainSubsystem, m_operator);
-  // private final KickIfShootSetRPMCommand m_kickIfShootSetRPMCommand =
-  //     new KickIfShootSetRPMCommand(m_shooterSubsystem, m_towerSubsystem);
-
-  public RobotContainer() {
-    CameraServer.startAutomaticCapture();
-    // Configure the button bindings
-    configureButtonBindings();
-    m_intakeSubsystem.setDefaultCommand(m_intakeStopCommand);
-    m_hopperSubsystem.setDefaultCommand(new HopperCommand(m_hopperSubsystem, 0));
-    m_shooterSubsystem.setDefaultCommand(new SetShooterSpeedCommand(m_shooterSubsystem, 0));
-    m_drivetrainSubsystem.setDefaultCommand(m_driveCommand);
-    m_climberSubsystem.setDefaultCommand(new ClimberCommand(m_climberSubsystem, 0));
-//    ShuffleboardTab testCommands = Shuffleboard.getTab("test commands");
-
-    // testCommands.add("ClimbUp", new ClimberCommand(m_climberSubsystem, 0.25));
-    // testCommands.add("ClimbDown", new ClimberCommand(m_climberSubsystem, -0.25));
-    // testCommands.add("IntakeIn", new RunIntakeCommand(m_intakeSubsystem, 0.75,
-    // m_hopperSubsystem));
-    // testCommands.add(
-    //     "IntakeOut", new RunIntakeCommand(m_intakeSubsystem, -0.75, m_hopperSubsystem));
-    // testCommands.add(
-    //     "Shoot RPM", new ShooterPIDCommand(m_shooterSubsystem, m_tunableShooterGoal.get()));
-    // testCommands.add("Tower Up", new TowerUpCommand(m_towerSubsystem));
-    // testCommands.add("Tower Down", new TowerDownCommand(m_towerSubsystem));
-    // testCommands.add("Tower Kicker", new TowerKickerCommand(m_towerSubsystem));
-    // testCommands.add("Hopper Up", new HopperCommand(m_hopperSubsystem, 0.5));
-    // testCommands.add("Hopper Down", new HopperCommand(m_hopperSubsystem, -0.5));
-  }
-
-  /**
-   * Use this method to define your button->command mappings. Buttons can be created by
-   * instantiating a {@link GenericHID} or one of its subclasses ({@link
-   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
-   * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
-   */
-  private void configureButtonBindings() {
-    /* FOR SIENNA :)
-    moving drivetrain
-    intake out/in
-    shooter + kicker logic
-    conveyor up/down
-    climber up/down
-    kicker separately
-    */
-
-
-
-    // driver joystick
-    m_operatorCommand.rightTrigger().whileTrue(m_intakeInCommand); 
-    new JoystickButton(m_operator, XboxController.Button.kRightBumper.value)
-        .whileTrue(m_intakeOutCommand);
-
-    new JoystickButton(m_driver, XboxController.Button.kLeftBumper.value)
-        .whileTrue(new ClimberCommand(m_climberSubsystem, 0.75));
-    m_driverCommand.leftTrigger().whileTrue(new ClimberCommand(m_climberSubsystem, -0.75)); 
-
-    new JoystickButton(m_operator, XboxController.Button.kB.value)
-        .whileTrue(new KickIfShootSetRPMCommand(m_shooterSubsystem, m_towerSubsystem, 1000));
-    new JoystickButton(m_operator, XboxController.Button.kA.value)
-        .whileTrue(new KickIfShootSetRPMCommand(m_shooterSubsystem, m_towerSubsystem, 2000));
-
-    //testing: individually shoot out by addressing shooter and tower
-    new JoystickButton(m_operator, XboxController.Button.kX.value)
-            .whileTrue(new ShootFromDistanceCommand(m_shooterSubsystem, m_limelightSubsystem, m_shooterLookupTable));
-
-    new JoystickButton(m_operator, XboxController.Button.kLeftBumper.value)
-            .whileTrue(new TowerUpCommand(m_towerSubsystem));
-    new JoystickButton(m_operator, XboxController.Button.kRightBumper.value)
-            .whileTrue(new TowerDownCommand(m_towerSubsystem));
-  }
-
-
-  /**
-   * Use this to pass the autonomous command to the main {@link Robot} class.
-   *
-   * @return the command to run in autonomous
-   */
-  public Command getAutonomousCommand() {
-    // An ExampleCommand will run in autonomous
-    return m_autonomousFactory.getAutonomousCommand();
-  }
+    private final XboxController m_operator = new XboxController(0);
+
+    private final CommandXboxController m_operatorCommand = new CommandXboxController(0);
+
+    private final XboxController m_driver = new XboxController(1);
+
+    private final CommandXboxController m_driverCommand = new CommandXboxController(1);
+
+    // The robot's subsystems and commands are defined here...
+    private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem();
+    private final DrivetrainSubsystem m_drivetrainSubsystem = new DrivetrainSubsystem();
+    private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem();
+    private final ShooterSubsytem m_shooterSubsystem = new ShooterSubsytem();
+    private final TowerSubsystem m_towerSubsystem = new TowerSubsystem();
+    private final HopperSubsystem m_hopperSubsystem = new HopperSubsystem();
+
+    private final LimelightSubsystem m_limelightSubsystem = new LimelightSubsystem();
+
+    private final ShooterLookupTable m_shooterLookupTable = new ShooterLookupTable();
+
+    private final AutonomousFactory m_autonomousFactory =
+            new AutonomousFactory(m_shooterSubsystem);
+
+    private final RunIntakeCommand m_intakeInCommand =
+            new RunIntakeCommand(m_intakeSubsystem, 0.5, m_hopperSubsystem, m_towerSubsystem, 0.5);
+    private final RunIntakeCommand m_intakeOutCommand =
+            new RunIntakeCommand(m_intakeSubsystem, -0.5, m_hopperSubsystem, m_towerSubsystem, -0.5);
+    private final StopIntakeCommand m_intakeStopCommand =
+            new StopIntakeCommand(m_intakeSubsystem, m_hopperSubsystem);
+    private final DriveCommand m_driveCommand = new DriveCommand(m_drivetrainSubsystem, m_operator);
+    // private final KickIfShootSetRPMCommand m_kickIfShootSetRPMCommand =
+    //         new KickIfShootSetRPMCommand(m_shooterSubsystem, m_towerSubsystem);
+
+    public RobotContainer() {
+        CameraServer.startAutomaticCapture();
+        // Configure the button bindings
+        configureButtonBindings();
+        m_intakeSubsystem.setDefaultCommand(m_intakeStopCommand);
+        m_hopperSubsystem.setDefaultCommand(new HopperCommand(m_hopperSubsystem, 0));
+        m_shooterSubsystem.setDefaultCommand(new SetShooterSpeedCommand(m_shooterSubsystem, 0));
+        m_drivetrainSubsystem.setDefaultCommand(m_driveCommand);
+        m_climberSubsystem.setDefaultCommand(new ClimberCommand(m_climberSubsystem, 0));
+        //        ShuffleboardTab testCommands = Shuffleboard.getTab("test commands");
+
+        // testCommands.add("ClimbUp", new ClimberCommand(m_climberSubsystem, 0.25));
+        // testCommands.add("ClimbDown", new ClimberCommand(m_climberSubsystem, -0.25));
+        // testCommands.add("IntakeIn", new RunIntakeCommand(m_intakeSubsystem, 0.75,
+        // m_hopperSubsystem));
+        // testCommands.add(
+        //         "IntakeOut", new RunIntakeCommand(m_intakeSubsystem, -0.75, m_hopperSubsystem));
+        // testCommands.add(
+        //         "Shoot RPM", new ShooterPIDCommand(m_shooterSubsystem, m_tunableShooterGoal.get()));
+        // testCommands.add("Tower Up", new TowerUpCommand(m_towerSubsystem));
+        // testCommands.add("Tower Down", new TowerDownCommand(m_towerSubsystem));
+        // testCommands.add("Tower Kicker", new TowerKickerCommand(m_towerSubsystem));
+        // testCommands.add("Hopper Up", new HopperCommand(m_hopperSubsystem, 0.5));
+        // testCommands.add("Hopper Down", new HopperCommand(m_hopperSubsystem, -0.5));
+    }
+
+    /**
+     * Use this method to define your button->command mappings. Buttons can be created by
+     * instantiating a {@link GenericHID} or one of its subclasses ({@link
+     * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
+     * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+     */
+    private void configureButtonBindings() {
+        /* FOR SIENNA :)
+        moving drivetrain
+        intake out/in
+        shooter + kicker logic
+        conveyor up/down
+        climber up/down
+        kicker separately
+        */
+
+        // driver joystick
+        m_operatorCommand.rightTrigger().whileTrue(m_intakeInCommand);
+        new JoystickButton(m_operator, XboxController.Button.kRightBumper.value)
+                .whileTrue(m_intakeOutCommand);
+
+        new JoystickButton(m_driver, XboxController.Button.kLeftBumper.value)
+                .whileTrue(new ClimberCommand(m_climberSubsystem, 0.75));
+        m_driverCommand.leftTrigger().whileTrue(new ClimberCommand(m_climberSubsystem, -0.75));
+
+        new JoystickButton(m_operator, XboxController.Button.kB.value)
+                .whileTrue(new KickIfShootSetRPMCommand(m_shooterSubsystem, m_towerSubsystem, 1000));
+        new JoystickButton(m_operator, XboxController.Button.kA.value)
+                .whileTrue(new KickIfShootSetRPMCommand(m_shooterSubsystem, m_towerSubsystem, 2000));
+
+        // testing: individually shoot out by addressing shooter and tower
+        new JoystickButton(m_operator, XboxController.Button.kX.value)
+                .whileTrue(
+                        new ShootFromDistanceCommand(
+                                m_shooterSubsystem, m_limelightSubsystem, m_shooterLookupTable));
+
+        new JoystickButton(m_operator, XboxController.Button.kLeftBumper.value)
+                .whileTrue(new TowerUpCommand(m_towerSubsystem));
+        new JoystickButton(m_operator, XboxController.Button.kRightBumper.value)
+                .whileTrue(new TowerDownCommand(m_towerSubsystem));
+    }
+
+    /**
+     * Use this to pass the autonomous command to the main {@link Robot} class.
+     *
+     * @return the command to run in autonomous
+     */
+    public Command getAutonomousCommand() {
+        // An ExampleCommand will run in autonomous
+        return m_autonomousFactory.getAutonomousCommand();
+    }
 }
diff --git a/src/main/java/frc/robot/ShooterLookupTable.java b/src/main/java/frc/robot/ShooterLookupTable.java
index e531042..48e1ca6 100644
--- a/src/main/java/frc/robot/ShooterLookupTable.java
+++ b/src/main/java/frc/robot/ShooterLookupTable.java
@@ -5,39 +5,33 @@
 import java.util.TreeMap;
 
 public class ShooterLookupTable {
-  private final NavigableMap<Double, Double> m_list = new TreeMap<>();
+    private final NavigableMap<Double, Double> m_list = new TreeMap<>();
 
-  public ShooterLookupTable() {
-    // fender at 1400\
-  }
+    public double getRpmTable(double distance) {
 
-  public double getRpmTable(double distance) {
+        Map.Entry<Double, Double> floor = m_list.floorEntry(distance);
+        if (floor == null) {
+            return 0;
+        }
 
-    Map.Entry<Double, Double> floor = m_list.floorEntry(distance);
+        Map.Entry<Double, Double> ceiling = m_list.ceilingEntry(distance);
+        if (ceiling == null) {
+            return 0;
+        }
 
-    Map.Entry<Double, Double> ceiling = m_list.ceilingEntry(distance);
+        if (floor.equals(ceiling)) {
+            return floor.getValue();
+        }
 
-    if (floor == null) {
-      return 0;
-    }
+        double d1 = floor.getKey();
+        double v1 = floor.getValue();
+        double d2 = ceiling.getKey();
+        double v2 = ceiling.getValue();
 
-    if (ceiling == null) {
-      return 0;
+        return interpolate(distance, d1, v1, d2, v2);
     }
 
-    if (floor.equals(ceiling)) {
-      return floor.getValue();
+    private double interpolate(double distance, double d1, double v1, double d2, double v2) {
+        return v1 + (v2 - v1) * ((distance - d1) / (d2 - d1));
     }
-
-    double d1 = floor.getKey();
-    double v1 = floor.getValue();
-    double d2 = ceiling.getKey();
-    double v2 = ceiling.getValue();
-
-    return interpolate(distance, d1, v1, d2, v2);
-  }
-
-  private double interpolate(double distance, double d1, double v1, double d2, double v2) {
-    return v1 + (v2 - v1) * ((distance - d1) / (d2 - d1));
-  }
 }
diff --git a/src/main/java/frc/robot/TunableNumber.java b/src/main/java/frc/robot/TunableNumber.java
deleted file mode 100644
index 6b096b9..0000000
--- a/src/main/java/frc/robot/TunableNumber.java
+++ /dev/null
@@ -1,81 +0,0 @@
-package frc.robot;
-
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
-/**
- * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or
- * value not in dashboard.
- */
-public class TunableNumber {
-  private static final String tableKey = "TunableNumbers";
-
-  private String key;
-  private double defaultValue;
-
-  private double lastHasChangedValue = defaultValue;
-
-  /**
-   * Create a new TunableNumber
-   *
-   * @param dashboardKey Key on dashboard
-   */
-  public TunableNumber(String dashboardKey) {
-    this.key = tableKey + "/" + dashboardKey;
-  }
-
-  /**
-   * Create a new TunableNumber wit1h the default value
-   *
-   * @param dashboardKey Key on dashboard
-   * @param defaultValue Default value
-   */
-  public TunableNumber(String dashboardKey, double defaultValue) {
-    this(dashboardKey);
-    setDefault(defaultValue);
-  }
-
-  /**
-   * Get the default value for the number that has been set
-   *
-   * @return The default value
-   */
-  public double getDefault() {
-    return defaultValue;
-  }
-
-  /**
-   * Set the default value of the number
-   *
-   * @param defaultValue The default value
-   */
-  public void setDefault(double defaultValue) {
-    this.defaultValue = defaultValue;
-    SmartDashboard.putNumber(key, SmartDashboard.getNumber(key, defaultValue));
-  }
-
-  /**
-   * Get the current value, from dashboard if available and in tuning mode
-   *
-   * @return The current value
-   */
-  public double get() {
-    // System.out.println("place 1");
-    return SmartDashboard.getNumber(key, defaultValue);
-  }
-
-  /**
-   * Checks whether the number has changed since our last check
-   *
-   * @return True if the number has changed since the last time this method was called, false
-   *     otherwise
-   */
-  public boolean hasChanged() {
-    // System.out.println("place 2");
-    double currentValue = get();
-    if (currentValue != lastHasChangedValue) {
-      lastHasChangedValue = currentValue;
-      return true;
-    }
-    return false;
-  }
-}
diff --git a/src/main/java/frc/robot/autos/AutonomousFactory.java b/src/main/java/frc/robot/autos/AutonomousFactory.java
index cd26c40..a879fbc 100644
--- a/src/main/java/frc/robot/autos/AutonomousFactory.java
+++ b/src/main/java/frc/robot/autos/AutonomousFactory.java
@@ -8,7 +8,7 @@
 import java.util.HashMap;
 import java.util.Map;
 
-public class AutonomousFactory{
+public class AutonomousFactory {
     private static final AutonMode DEFAULT_MODE = AutonMode.SHOOT_4;
 
     private final SendableChooser<AutonMode> m_chooseAutoOption;
@@ -32,24 +32,23 @@ public enum AutonMode {
     private final Map<AutonMode, Command> m_autoOptions = new HashMap<>();
 
     //in parameters, pass in all subsystems you're going to use
-public AutonomousFactory(DrivetrainSubsystem drivetrainSubsystem, ShooterSubsytem shooterSubsytem) {
+    public AutonomousFactory(ShooterSubsytem shooterSubsytem) {
         m_chooseAutoOption = new SendableChooser<>();
 
         //m_autoOptions.put(AutonMode.LINE_TEST, new lineTest(drivetrainSubsystem, "lineTest"));
-//        m_autoOptions.put(AutonMode.CURVE_TEST, new curveTest(drivetrainSubsystem, "curveTest"));
-//        m_autoOptions.put(AutonMode.S_CURVE_TEST, new SCurveTest(drivetrainSubsystem,"sCurveTest"));
-//        m_autoOptions.put(AutonMode.SHOOT_ENGAGE_4,new ShootEngage(drivetrainSubsystem, shooterSubsytem, "dockAndEngage4"));
-//        m_autoOptions.put(AutonMode.SHOOT_LEAVE_COMMUNITY_1,new ShootLeaveCommunity(drivetrainSubsystem,shooterSubsytem,"leaveCommunity1"));
-//        m_autoOptions.put(AutonMode.SHOOT_LEAVE_COMMUNITY_7,new ShootLeaveCommunity(drivetrainSubsystem,shooterSubsytem,"leaveCommunity7"));
-        m_autoOptions.put(AutonMode.SHOOT_4,new Shoot(shooterSubsytem));
-        m_autoOptions.put(AutonMode.SHOOT_1,new Shoot(shooterSubsytem));
-        m_autoOptions.put(AutonMode.SHOOT_7,new Shoot(shooterSubsytem));
-
-        for (AutonMode auto: AutonMode.values()) {
+        //        m_autoOptions.put(AutonMode.CURVE_TEST, new curveTest(drivetrainSubsystem, "curveTest"));
+        //        m_autoOptions.put(AutonMode.S_CURVE_TEST, new SCurveTest(drivetrainSubsystem,"sCurveTest"));
+        //        m_autoOptions.put(AutonMode.SHOOT_ENGAGE_4,new ShootEngage(drivetrainSubsystem, shooterSubsytem, "dockAndEngage4"));
+        //        m_autoOptions.put(AutonMode.SHOOT_LEAVE_COMMUNITY_1,new ShootLeaveCommunity(drivetrainSubsystem,shooterSubsytem,"leaveCommunity1"));
+        //        m_autoOptions.put(AutonMode.SHOOT_LEAVE_COMMUNITY_7,new ShootLeaveCommunity(drivetrainSubsystem,shooterSubsytem,"leaveCommunity7"));
+        m_autoOptions.put(AutonMode.SHOOT_4, new Shoot(shooterSubsytem));
+        m_autoOptions.put(AutonMode.SHOOT_1, new Shoot(shooterSubsytem));
+        m_autoOptions.put(AutonMode.SHOOT_7, new Shoot(shooterSubsytem));
+
+        for (AutonMode auto : AutonMode.values()) {
             if (auto == DEFAULT_MODE) {
                 m_chooseAutoOption.setDefaultOption(auto.toString(), auto);
-            }
-            else {
+            } else {
                 m_chooseAutoOption.addOption(auto.toString(), auto);
             }
         }
@@ -62,9 +61,4 @@ public Command getAutonomousCommand() {
     }
 
 
-
-
-
-
-
 }
diff --git a/src/main/java/frc/robot/autos/CurveTest.java b/src/main/java/frc/robot/autos/CurveTest.java
index 0b06f4c..767b501 100644
--- a/src/main/java/frc/robot/autos/CurveTest.java
+++ b/src/main/java/frc/robot/autos/CurveTest.java
@@ -6,10 +6,11 @@
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.Constants;
 import frc.robot.subsystems.DrivetrainSubsystem;
+
 import java.util.HashMap;
 
 public class CurveTest extends SequentialCommandGroup {
-    public CurveTest(DrivetrainSubsystem drivetrain, String path){
+    public CurveTest(DrivetrainSubsystem drivetrain, String path) {
         PathPlannerTrajectory curveTest = PathPlanner.loadPath(path, Constants.DEFAULT_PATH_CONSTRAINTS, true);
         Command curveTest1 = drivetrain.ramseteAutoBuilderNoPoseReset(new HashMap<>()).fullAuto(curveTest);
 
diff --git a/src/main/java/frc/robot/autos/LineTest.java b/src/main/java/frc/robot/autos/LineTest.java
index 16d09e9..104c945 100644
--- a/src/main/java/frc/robot/autos/LineTest.java
+++ b/src/main/java/frc/robot/autos/LineTest.java
@@ -12,10 +12,10 @@
 public class LineTest extends SequentialCommandGroup {
     //CONSTRUCTOR
     public LineTest(DrivetrainSubsystem drivetrain, String path) {
-        PathPlannerTrajectory LineTest = PathPlanner.loadPath(path, Constants.DEFAULT_PATH_CONSTRAINTS, true);
-        Command LineTest1 = drivetrain.ramseteAutoBuilderNoPoseReset(new HashMap<>()).fullAuto(LineTest);
+        PathPlannerTrajectory trajectory = PathPlanner.loadPath(path, Constants.DEFAULT_PATH_CONSTRAINTS, true);
+        Command lineTest1 = drivetrain.ramseteAutoBuilderNoPoseReset(new HashMap<>()).fullAuto(trajectory);
         // STEP 1: drive!
-        addCommands(LineTest1);
+        addCommands(lineTest1);
 
     }
 }
diff --git a/src/main/java/frc/robot/autos/SCurveTest.java b/src/main/java/frc/robot/autos/SCurveTest.java
index 039600c..6620bcd 100644
--- a/src/main/java/frc/robot/autos/SCurveTest.java
+++ b/src/main/java/frc/robot/autos/SCurveTest.java
@@ -12,9 +12,9 @@
 public class SCurveTest extends SequentialCommandGroup {
 
     public SCurveTest(DrivetrainSubsystem drivetrain, String path) {
-        PathPlannerTrajectory SCurveTest = PathPlanner.loadPath(path, Constants.DEFAULT_PATH_CONSTRAINTS, true);
-        Command SCurveTest1 = drivetrain.ramseteAutoBuilderNoPoseReset(new HashMap<>()).fullAuto(SCurveTest);
+        PathPlannerTrajectory trajectory = PathPlanner.loadPath(path, Constants.DEFAULT_PATH_CONSTRAINTS, true);
+        Command curveTest1 = drivetrain.ramseteAutoBuilderNoPoseReset(new HashMap<>()).fullAuto(trajectory);
 
-        addCommands(SCurveTest1);
+        addCommands(curveTest1);
     }
 }
diff --git a/src/main/java/frc/robot/autos/Shoot.java b/src/main/java/frc/robot/autos/Shoot.java
index b4dbfbb..669b01b 100644
--- a/src/main/java/frc/robot/autos/Shoot.java
+++ b/src/main/java/frc/robot/autos/Shoot.java
@@ -5,7 +5,7 @@
 import frc.robot.subsystems.ShooterSubsytem;
 
 public class Shoot extends SequentialCommandGroup {
-    public Shoot(ShooterSubsytem shooterSubsytem){
+    public Shoot(ShooterSubsytem shooterSubsytem) {
         addCommands(new ShooterPIDCommand(shooterSubsytem));
     }
 }
diff --git a/src/main/java/frc/robot/autos/ShootEngage.java b/src/main/java/frc/robot/autos/ShootEngage.java
index de98518..1c4a41f 100644
--- a/src/main/java/frc/robot/autos/ShootEngage.java
+++ b/src/main/java/frc/robot/autos/ShootEngage.java
@@ -12,7 +12,7 @@
 import java.util.HashMap;
 
 public class ShootEngage extends SequentialCommandGroup {
-    public ShootEngage(DrivetrainSubsystem drivetrain, ShooterSubsytem shooter, String path){
+    public ShootEngage(DrivetrainSubsystem drivetrain, ShooterSubsytem shooter, String path) {
         PathPlannerTrajectory shootAndEngage = PathPlanner.loadPath(path, Constants.DEFAULT_PATH_CONSTRAINTS, true);
         Command shootEngage = drivetrain.ramseteAutoBuilderNoPoseReset(new HashMap<>()).fullAuto(shootAndEngage);
 
@@ -23,8 +23,6 @@ public ShootEngage(DrivetrainSubsystem drivetrain, ShooterSubsytem shooter, Stri
         addCommands(shootEngage);
 
 
-
-
     }
 
 
diff --git a/src/main/java/frc/robot/autos/ShootLeaveCommunity.java b/src/main/java/frc/robot/autos/ShootLeaveCommunity.java
index 548530e..5cd2f05 100644
--- a/src/main/java/frc/robot/autos/ShootLeaveCommunity.java
+++ b/src/main/java/frc/robot/autos/ShootLeaveCommunity.java
@@ -11,14 +11,14 @@
 import java.util.HashMap;
 
 public class ShootLeaveCommunity extends SequentialCommandGroup {
-   //CONSTRUCTOR
+    //CONSTRUCTOR
     public ShootLeaveCommunity(DrivetrainSubsystem drivetrain, ShooterSubsytem shooter, String path) {
-        PathPlannerTrajectory ShootLeaveCommunity = PathPlanner.loadPath(path, Constants.DEFAULT_PATH_CONSTRAINTS, true);
-        Command ShootAndLeaveCommunity = drivetrain.ramseteAutoBuilderNoPoseReset(new HashMap<>()).fullAuto(ShootLeaveCommunity);
+        PathPlannerTrajectory trajectory = PathPlanner.loadPath(path, Constants.DEFAULT_PATH_CONSTRAINTS, true);
+        Command shootAndLeaveCommunity = drivetrain.ramseteAutoBuilderNoPoseReset(new HashMap<>()).fullAuto(trajectory);
         //shoot a ball
         addCommands(new ShooterPIDCommand(shooter));
 
         //leave community
-        addCommands(ShootAndLeaveCommunity);
+        addCommands(shootAndLeaveCommunity);
     }
 }
diff --git a/src/main/java/frc/robot/commands/AutoAimCommand.java b/src/main/java/frc/robot/commands/AutoAimCommand.java
index 3670bb7..2e151a9 100644
--- a/src/main/java/frc/robot/commands/AutoAimCommand.java
+++ b/src/main/java/frc/robot/commands/AutoAimCommand.java
@@ -4,65 +4,77 @@
 
 package frc.robot.commands;
 
+import com.gos.lib.properties.PidProperty;
+import com.gos.lib.properties.WpiProfiledPidPropertyBuilder;
+
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.Constants;
-import frc.robot.TunableNumber;
 import frc.robot.subsystems.DrivetrainSubsystem;
 import frc.robot.subsystems.LimelightSubsystem;
 
-/** An example command that uses an example subsystem. */
+/**
+ * An example command that uses an example subsystem.
+ */
 public class AutoAimCommand extends CommandBase {
-  private final DrivetrainSubsystem m_driveTrainSubsystem;
-  private final LimelightSubsystem m_limelightSubsystem;
-  private final ProfiledPIDController m_controller;
-  private final TunableNumber m_P = new TunableNumber("AutoAim(kP)", 0);
-  private final TunableNumber m_I = new TunableNumber("AutoAim(kI)", 0);
-  private final TunableNumber m_D = new TunableNumber("AutoAim(kD)", 0);
+    private final DrivetrainSubsystem m_driveTrainSubsystem;
+    private final LimelightSubsystem m_limelightSubsystem;
+    private final ProfiledPIDController m_controller;
+    private final PidProperty m_pidProperty;
 
-  /**
-   * Creates a new AutoAimCommand.
-   *
-   * @param subsystem The subsystem used by this command.
-   */
-  public AutoAimCommand(
-      DrivetrainSubsystem driveTrainSubsystem, LimelightSubsystem limelightSubsystem) {
-    m_driveTrainSubsystem = driveTrainSubsystem;
-    m_limelightSubsystem = limelightSubsystem;
-    m_controller =
-        new ProfiledPIDController(
-            m_P.get(),
-            m_I.get(),
-            m_D.get(),
-            new TrapezoidProfile.Constraints(
-                Constants.MAX_TURN_VELOCITY, Constants.MAX_TURN_ACCELERATION));
-    m_controller.setTolerance(5);
-    // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(driveTrainSubsystem);
-    addRequirements(limelightSubsystem);
-  }
+    /**
+     * Creates a new AutoAimCommand.
+     *
+     * @param subsystem The subsystem used by this command.
+     */
+    public AutoAimCommand(
+            DrivetrainSubsystem driveTrainSubsystem, LimelightSubsystem limelightSubsystem) {
+        m_driveTrainSubsystem = driveTrainSubsystem;
+        m_limelightSubsystem = limelightSubsystem;
+        m_controller =
+                new ProfiledPIDController(
+                        0,
+                        0,
+                        0,
+                        new TrapezoidProfile.Constraints(
+                                Constants.MAX_TURN_VELOCITY, Constants.MAX_TURN_ACCELERATION));
+        m_pidProperty = new WpiProfiledPidPropertyBuilder("AutoAim", false, m_controller)
+                .addP(0)
+                .addI(0)
+                .addD(0)
+                .addMaxVelocity(Constants.MAX_TURN_VELOCITY)
+                .addMaxAcceleration(Constants.MAX_TURN_ACCELERATION)
+                .build();
+        m_controller.setTolerance(5);
+        // Use addRequirements() here to declare subsystem dependencies.
+        addRequirements(driveTrainSubsystem);
+        addRequirements(limelightSubsystem);
+    }
 
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {}
+    // Called when the command is initially scheduled.
+    @Override
+    public void initialize() {
+    }
 
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    double turnSpeed = m_controller.calculate(m_limelightSubsystem.limelightAngle());
-    ChassisSpeeds chassisSpeed = new ChassisSpeeds(0, 0, turnSpeed);
-    m_driveTrainSubsystem.applyChassisSpeed(chassisSpeed);
-  }
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {
+        m_pidProperty.updateIfChanged();
+        double turnSpeed = m_controller.calculate(m_limelightSubsystem.limelightAngle());
+        ChassisSpeeds chassisSpeed = new ChassisSpeeds(0, 0, turnSpeed);
+        m_driveTrainSubsystem.applyChassisSpeed(chassisSpeed);
+    }
 
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {}
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+    }
 
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return m_controller.atGoal();
-  }
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return m_controller.atGoal();
+    }
 }
diff --git a/src/main/java/frc/robot/commands/AutoClimb.java b/src/main/java/frc/robot/commands/AutoClimb.java
deleted file mode 100644
index 2e638a4..0000000
--- a/src/main/java/frc/robot/commands/AutoClimb.java
+++ /dev/null
@@ -1,16 +0,0 @@
-package frc.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import frc.robot.subsystems.ClimberSubsystem;
-
-public class AutoClimb extends SequentialCommandGroup {
-
-  public AutoClimb(ClimberSubsystem climber) {
-    // new ClimberCommand(climber, -0.5),
-    // new WaitCommand(0.1),
-    // new ClimbPIDComand(climber, 1),
-    // new WaitCommand(0.6),
-    // new ClimbPIDComand(climber, 0),
-    // new
-  }
-}
diff --git a/src/main/java/frc/robot/commands/ClimbAutoCommand.java b/src/main/java/frc/robot/commands/ClimbAutoCommand.java
index f64e08b..f29c15e 100644
--- a/src/main/java/frc/robot/commands/ClimbAutoCommand.java
+++ b/src/main/java/frc/robot/commands/ClimbAutoCommand.java
@@ -3,37 +3,42 @@
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.ClimberSubsystem;
 
-/** An example command that uses an example subsystem. */
+/**
+ * An example command that uses an example subsystem.
+ */
 public class ClimbAutoCommand extends CommandBase {
-  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-  private final ClimberSubsystem m_subsystem;
+    @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+    private final ClimberSubsystem m_subsystem;
 
-  /**
-   * Creates a new ClimbAutoCommand.
-   *
-   * @param subsystem The subsystem used by this command.
-   */
-  public ClimbAutoCommand(ClimberSubsystem subsystem) {
-    m_subsystem = subsystem;
-    // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(subsystem);
-  }
+    /**
+     * Creates a new ClimbAutoCommand.
+     *
+     * @param subsystem The subsystem used by this command.
+     */
+    public ClimbAutoCommand(ClimberSubsystem subsystem) {
+        m_subsystem = subsystem;
+        // Use addRequirements() here to declare subsystem dependencies.
+        addRequirements(subsystem);
+    }
 
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {}
+    // Called when the command is initially scheduled.
+    @Override
+    public void initialize() {
+    }
 
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {}
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {
+    }
 
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {}
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+    }
 
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
 }
diff --git a/src/main/java/frc/robot/commands/ClimbPIDComand.java b/src/main/java/frc/robot/commands/ClimbPIDComand.java
index efb6b76..7eae37e 100644
--- a/src/main/java/frc/robot/commands/ClimbPIDComand.java
+++ b/src/main/java/frc/robot/commands/ClimbPIDComand.java
@@ -4,32 +4,33 @@
 import frc.robot.subsystems.ClimberSubsystem;
 
 public class ClimbPIDComand extends CommandBase {
-  private final ClimberSubsystem m_climberSubsystem;
-  private final double m_goal;
+    private final ClimberSubsystem m_climberSubsystem;
+    private final double m_goal;
 
-  public ClimbPIDComand(ClimberSubsystem climberSubsystem, double goal) {
-    m_climberSubsystem = climberSubsystem;
-    m_goal = goal;
-    addRequirements(climberSubsystem);
-  }
+    public ClimbPIDComand(ClimberSubsystem climberSubsystem, double goal) {
+        m_climberSubsystem = climberSubsystem;
+        m_goal = goal;
+        addRequirements(climberSubsystem);
+    }
 
-  @Override
-  public void initialize() {}
+    @Override
+    public void initialize() {
+    }
 
-  @Override
-  public void execute() {
-    m_climberSubsystem.RunClimberPID(m_goal);
-  }
+    @Override
+    public void execute() {
+        m_climberSubsystem.runClimberPID(m_goal);
+    }
 
-  @Override
-  public boolean isFinished() {
-    return Math.abs(m_climberSubsystem.getPIDPosition()) < 100
-        || (m_climberSubsystem.leftLimitSwitchPress()
-            || m_climberSubsystem.rightLimitSwitchPress());
-  }
+    @Override
+    public boolean isFinished() {
+        return Math.abs(m_climberSubsystem.getPIDPosition()) < 100
+                || (m_climberSubsystem.leftLimitSwitchPress()
+                || m_climberSubsystem.rightLimitSwitchPress());
+    }
 
-  @Override
-  public void end(boolean interrupted) {
-    m_climberSubsystem.set(0);
-  }
+    @Override
+    public void end(boolean interrupted) {
+        m_climberSubsystem.set(0);
+    }
 }
diff --git a/src/main/java/frc/robot/commands/ClimberCommand.java b/src/main/java/frc/robot/commands/ClimberCommand.java
index e3d1f04..b4b5343 100644
--- a/src/main/java/frc/robot/commands/ClimberCommand.java
+++ b/src/main/java/frc/robot/commands/ClimberCommand.java
@@ -7,41 +7,46 @@
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.ClimberSubsystem;
 
-/** An example command that uses an example subsystem. */
+/**
+ * An example command that uses an example subsystem.
+ */
 public class ClimberCommand extends CommandBase {
-  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-  private final ClimberSubsystem m_subsystem;
-
-  private final double m_speed;
-  /**
-   * Creates a new ClimberCommand.
-   *
-   * @param subsystem The subsystem used by this command.
-   */
-  public ClimberCommand(ClimberSubsystem subsystem, double speed) {
-    m_subsystem = subsystem;
-    m_speed = speed;
-    // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(subsystem);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {}
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    m_subsystem.set(m_speed);
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {}
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
+    @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+    private final ClimberSubsystem m_subsystem;
+
+    private final double m_speed;
+
+    /**
+     * Creates a new ClimberCommand.
+     *
+     * @param subsystem The subsystem used by this command.
+     */
+    public ClimberCommand(ClimberSubsystem subsystem, double speed) {
+        m_subsystem = subsystem;
+        m_speed = speed;
+        // Use addRequirements() here to declare subsystem dependencies.
+        addRequirements(subsystem);
+    }
+
+    // Called when the command is initially scheduled.
+    @Override
+    public void initialize() {
+    }
+
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {
+        m_subsystem.set(m_speed);
+    }
+
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+    }
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
 }
diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/DriveCommand.java
index 7e82952..5530ed3 100644
--- a/src/main/java/frc/robot/commands/DriveCommand.java
+++ b/src/main/java/frc/robot/commands/DriveCommand.java
@@ -8,41 +8,46 @@
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.DrivetrainSubsystem;
 
-/** An example command that uses an example subsystem. */
+/**
+ * An example command that uses an example subsystem.
+ */
 public class DriveCommand extends CommandBase {
-  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-  private final DrivetrainSubsystem m_subsystem;
-
-  private final XboxController m_joystick;
-  /**
-   * Creates a new DriveCommand.
-   *
-   * @param subsystem The subsystem used by this command.
-   */
-  public DriveCommand(DrivetrainSubsystem subsystem, XboxController joystick) {
-    m_subsystem = subsystem;
-    // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(subsystem);
-    m_joystick = joystick;
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {}
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    m_subsystem.control(-0.5 * m_joystick.getLeftY(), 0.5 * m_joystick.getRightX());
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {}
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
+    @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+    private final DrivetrainSubsystem m_subsystem;
+
+    private final XboxController m_joystick;
+
+    /**
+     * Creates a new DriveCommand.
+     *
+     * @param subsystem The subsystem used by this command.
+     */
+    public DriveCommand(DrivetrainSubsystem subsystem, XboxController joystick) {
+        m_subsystem = subsystem;
+        // Use addRequirements() here to declare subsystem dependencies.
+        addRequirements(subsystem);
+        m_joystick = joystick;
+    }
+
+    // Called when the command is initially scheduled.
+    @Override
+    public void initialize() {
+    }
+
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {
+        m_subsystem.control(-0.5 * m_joystick.getLeftY(), 0.5 * m_joystick.getRightX());
+    }
+
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+    }
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
 }
diff --git a/src/main/java/frc/robot/commands/DriveCommandWithPID.java b/src/main/java/frc/robot/commands/DriveCommandWithPID.java
index c401a6b..6c1c779 100644
--- a/src/main/java/frc/robot/commands/DriveCommandWithPID.java
+++ b/src/main/java/frc/robot/commands/DriveCommandWithPID.java
@@ -5,7 +5,7 @@
 import frc.robot.subsystems.DrivetrainSubsystem;
 
 public class DriveCommandWithPID extends CommandBase {
-    DrivetrainSubsystem m_drivetrain;
+    private final DrivetrainSubsystem m_drivetrain;
 
     public DriveCommandWithPID(DrivetrainSubsystem drivetrainSubsystem) {
         m_drivetrain = drivetrainSubsystem;
diff --git a/src/main/java/frc/robot/commands/ExampleCommand.java b/src/main/java/frc/robot/commands/ExampleCommand.java
deleted file mode 100644
index 5150ec7..0000000
--- a/src/main/java/frc/robot/commands/ExampleCommand.java
+++ /dev/null
@@ -1,43 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc.robot.subsystems.ExampleSubsystem;
-
-/** An example command that uses an example subsystem. */
-public class ExampleCommand extends CommandBase {
-  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-  private final ExampleSubsystem m_subsystem;
-
-  /**
-   * Creates a new ExampleCommand.
-   *
-   * @param subsystem The subsystem used by this command.
-   */
-  public ExampleCommand(ExampleSubsystem subsystem) {
-    m_subsystem = subsystem;
-    // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(subsystem);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {}
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {}
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {}
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
-}
diff --git a/src/main/java/frc/robot/commands/HopperCommand.java b/src/main/java/frc/robot/commands/HopperCommand.java
index 0a15095..c0e50fc 100644
--- a/src/main/java/frc/robot/commands/HopperCommand.java
+++ b/src/main/java/frc/robot/commands/HopperCommand.java
@@ -7,42 +7,47 @@
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.HopperSubsystem;
 
-/** An example command that uses an example subsystem. */
+/**
+ * An example command that uses an example subsystem.
+ */
 public class HopperCommand extends CommandBase {
-  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-  private final HopperSubsystem m_hopperSubsystem;
-
-  private double m_speed;
-  /**
-   * Creates a new HopperCommand.
-   *
-   * @param towersubsystem The subsystem used by this command.
-   */
-  public HopperCommand(HopperSubsystem subsystem, double speed) {
-    m_hopperSubsystem = subsystem;
-    m_speed = speed;
-
-    // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(subsystem);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {}
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    m_hopperSubsystem.setHopperSpeed(m_speed);
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {}
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
+    @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+    private final HopperSubsystem m_hopperSubsystem;
+
+    private final double m_speed;
+
+    /**
+     * Creates a new HopperCommand.
+     *
+     * @param towersubsystem The subsystem used by this command.
+     */
+    public HopperCommand(HopperSubsystem subsystem, double speed) {
+        m_hopperSubsystem = subsystem;
+        m_speed = speed;
+
+        // Use addRequirements() here to declare subsystem dependencies.
+        addRequirements(subsystem);
+    }
+
+    // Called when the command is initially scheduled.
+    @Override
+    public void initialize() {
+    }
+
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {
+        m_hopperSubsystem.setHopperSpeed(m_speed);
+    }
+
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+    }
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
 }
diff --git a/src/main/java/frc/robot/commands/KickIfShootSetRPMCommand.java b/src/main/java/frc/robot/commands/KickIfShootSetRPMCommand.java
index 2b451ba..dbdce8b 100644
--- a/src/main/java/frc/robot/commands/KickIfShootSetRPMCommand.java
+++ b/src/main/java/frc/robot/commands/KickIfShootSetRPMCommand.java
@@ -5,54 +5,58 @@
 import frc.robot.subsystems.ShooterSubsytem;
 import frc.robot.subsystems.TowerSubsystem;
 
-/** An example command that uses an example subsystem. */
+/**
+ * An example command that uses an example subsystem.
+ */
 public class KickIfShootSetRPMCommand extends CommandBase {
-  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-  private final ShooterSubsytem m_shooterSubsystem;
-
-  private final TowerSubsystem m_towerSubsystem;
-
-  private final double m_rpm;
-  /**
-   * Creates a new KickIfShooterGoBrrCommand.
-   *
-   * @param subsystem The subsystem used by this command.
-   */
-  public KickIfShootSetRPMCommand(ShooterSubsytem subsystem, TowerSubsystem towerSubsystem, double rpm) {
-    m_shooterSubsystem = subsystem;
-    m_towerSubsystem = towerSubsystem;
-    m_rpm = rpm;
-    // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(subsystem);
-    addRequirements(towerSubsystem);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {}
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    m_shooterSubsystem.setPidRpm(m_rpm);
-    if (m_shooterSubsystem.checkAtSpeed(m_rpm)) {
-      m_towerSubsystem.setKickerSpeed(1);
-      m_towerSubsystem.setTowerSpeed(0.75);
+    @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+    private final ShooterSubsytem m_shooterSubsystem;
+
+    private final TowerSubsystem m_towerSubsystem;
+
+    private final double m_rpm;
+
+    /**
+     * Creates a new KickIfShooterGoBrrCommand.
+     *
+     * @param subsystem The subsystem used by this command.
+     */
+    public KickIfShootSetRPMCommand(ShooterSubsytem subsystem, TowerSubsystem towerSubsystem, double rpm) {
+        m_shooterSubsystem = subsystem;
+        m_towerSubsystem = towerSubsystem;
+        m_rpm = rpm;
+        // Use addRequirements() here to declare subsystem dependencies.
+        addRequirements(subsystem);
+        addRequirements(towerSubsystem);
+    }
+
+    // Called when the command is initially scheduled.
+    @Override
+    public void initialize() {
+    }
+
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {
+        m_shooterSubsystem.setPidRpm(m_rpm);
+        if (m_shooterSubsystem.checkAtSpeed(m_rpm)) {
+            m_towerSubsystem.setKickerSpeed(1);
+            m_towerSubsystem.setTowerSpeed(0.75);
+        }
+        SmartDashboard.putBoolean(
+                "kickIfShootRPM atSpeed", m_shooterSubsystem.checkAtSpeed(m_rpm));
+    }
+
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+        m_towerSubsystem.setKickerSpeed(0);
+        m_towerSubsystem.setTowerSpeed(0);
+    }
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return false;
     }
-    SmartDashboard.putBoolean(
-        "kickIfShootRPM atSpeed", m_shooterSubsystem.checkAtSpeed(m_rpm));
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {
-    m_towerSubsystem.setKickerSpeed(0);
-    m_towerSubsystem.setTowerSpeed(0);
-  }
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
 }
diff --git a/src/main/java/frc/robot/commands/KickIfShooterDistanceGoBrrCommand.java b/src/main/java/frc/robot/commands/KickIfShooterDistanceGoBrrCommand.java
index def51cf..c4ac381 100644
--- a/src/main/java/frc/robot/commands/KickIfShooterDistanceGoBrrCommand.java
+++ b/src/main/java/frc/robot/commands/KickIfShooterDistanceGoBrrCommand.java
@@ -7,63 +7,67 @@
 import frc.robot.subsystems.ShooterSubsytem;
 import frc.robot.subsystems.TowerSubsystem;
 
-/** An example command that uses an example subsystem. */
+/**
+ * An example command that uses an example subsystem.
+ */
 public class KickIfShooterDistanceGoBrrCommand extends CommandBase {
-  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-  private final ShooterSubsytem m_shooterSubsystem;
+    @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+    private final ShooterSubsytem m_shooterSubsystem;
 
-  private final LimelightSubsystem m_limelightSubsystem;
-  private final TowerSubsystem m_towerSubsystem;
-  private final ShooterLookupTable m_shooterLookupTable;
-  /**
-   * Creates a new KickIfShooterGoBrrCommand.
-   *
-   * @param subsystem The subsystem used by this command.
-   */
-  public KickIfShooterDistanceGoBrrCommand(
-      ShooterSubsytem subsystem,
-      TowerSubsystem towerSubsystem,
-      LimelightSubsystem limelightSubsystem,
-      ShooterLookupTable shooterLookupTable) {
-    m_shooterSubsystem = subsystem;
-    m_towerSubsystem = towerSubsystem;
-    m_limelightSubsystem = limelightSubsystem;
-    m_shooterLookupTable = shooterLookupTable;
-    // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(subsystem);
-    addRequirements(towerSubsystem);
-    addRequirements(limelightSubsystem);
-  }
+    private final LimelightSubsystem m_limelightSubsystem;
+    private final TowerSubsystem m_towerSubsystem;
+    private final ShooterLookupTable m_shooterLookupTable;
 
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {}
+    /**
+     * Creates a new KickIfShooterGoBrrCommand.
+     *
+     * @param subsystem The subsystem used by this command.
+     */
+    public KickIfShooterDistanceGoBrrCommand(
+            ShooterSubsytem subsystem,
+            TowerSubsystem towerSubsystem,
+            LimelightSubsystem limelightSubsystem,
+            ShooterLookupTable shooterLookupTable) {
+        m_shooterSubsystem = subsystem;
+        m_towerSubsystem = towerSubsystem;
+        m_limelightSubsystem = limelightSubsystem;
+        m_shooterLookupTable = shooterLookupTable;
+        // Use addRequirements() here to declare subsystem dependencies.
+        addRequirements(subsystem);
+        addRequirements(towerSubsystem);
+        addRequirements(limelightSubsystem);
+    }
 
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    double distance = m_limelightSubsystem.limelightDistance();
-    m_shooterSubsystem.shootFromDistance(distance);
-    if (m_shooterSubsystem.checkAtSpeed(m_shooterLookupTable.getRpmTable(distance))) {
-      m_towerSubsystem.setKickerSpeed(1);
-      m_towerSubsystem.setTowerSpeed(0.75);
+    // Called when the command is initially scheduled.
+    @Override
+    public void initialize() {
     }
-    SmartDashboard.putBoolean(
-        "kickIfShootDist atSpeed",
-        m_shooterSubsystem.checkAtSpeed(m_shooterLookupTable.getRpmTable(distance)));
-  }
 
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {
-    m_towerSubsystem.setTowerSpeed(0);
-    m_towerSubsystem.setKickerSpeed(0);
-  }
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {
+        double distance = m_limelightSubsystem.limelightDistance();
+        m_shooterSubsystem.shootFromDistance(distance);
+        if (m_shooterSubsystem.checkAtSpeed(m_shooterLookupTable.getRpmTable(distance))) {
+            m_towerSubsystem.setKickerSpeed(1);
+            m_towerSubsystem.setTowerSpeed(0.75);
+        }
+        SmartDashboard.putBoolean(
+                "kickIfShootDist atSpeed",
+                m_shooterSubsystem.checkAtSpeed(m_shooterLookupTable.getRpmTable(distance)));
+    }
 
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return m_shooterSubsystem.checkAtSpeed(
-        m_shooterLookupTable.getRpmTable(m_limelightSubsystem.limelightDistance()));
-  }
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+        m_towerSubsystem.setTowerSpeed(0);
+        m_towerSubsystem.setKickerSpeed(0);
+    }
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return m_shooterSubsystem.checkAtSpeed(
+                m_shooterLookupTable.getRpmTable(m_limelightSubsystem.limelightDistance()));
+    }
 }
diff --git a/src/main/java/frc/robot/commands/RunClimberToBottomCommand.java b/src/main/java/frc/robot/commands/RunClimberToBottomCommand.java
index 2f0fbc6..d7a68c2 100644
--- a/src/main/java/frc/robot/commands/RunClimberToBottomCommand.java
+++ b/src/main/java/frc/robot/commands/RunClimberToBottomCommand.java
@@ -7,39 +7,43 @@
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.ClimberSubsystem;
 
-/** An example command that uses an example subsystem. */
+/**
+ * An example command that uses an example subsystem.
+ */
 public class RunClimberToBottomCommand extends CommandBase {
-  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-  private final ClimberSubsystem m_subsystem;
-
-  /**
-   * Creates a new RunClimberToBottomCommand.
-   *
-   * @param subsystem The subsystem used by this command.
-   */
-  public RunClimberToBottomCommand(ClimberSubsystem subsystem) {
-    m_subsystem = subsystem;
-    // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(subsystem);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {}
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    m_subsystem.set(-0.5);
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {}
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
+    @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+    private final ClimberSubsystem m_subsystem;
+
+    /**
+     * Creates a new RunClimberToBottomCommand.
+     *
+     * @param subsystem The subsystem used by this command.
+     */
+    public RunClimberToBottomCommand(ClimberSubsystem subsystem) {
+        m_subsystem = subsystem;
+        // Use addRequirements() here to declare subsystem dependencies.
+        addRequirements(subsystem);
+    }
+
+    // Called when the command is initially scheduled.
+    @Override
+    public void initialize() {
+    }
+
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {
+        m_subsystem.set(-0.5);
+    }
+
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+    }
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
 }
diff --git a/src/main/java/frc/robot/commands/RunIntakeCommand.java b/src/main/java/frc/robot/commands/RunIntakeCommand.java
index ada7223..59efdde 100644
--- a/src/main/java/frc/robot/commands/RunIntakeCommand.java
+++ b/src/main/java/frc/robot/commands/RunIntakeCommand.java
@@ -9,69 +9,72 @@
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.TowerSubsystem;
 
-/** An example command that uses an example subsystem. */
+/**
+ * An example command that uses an example subsystem.
+ */
 public class RunIntakeCommand extends CommandBase {
-  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-  private final IntakeSubsystem m_intakeSubsystem;
+    @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+    private final IntakeSubsystem m_intakeSubsystem;
 
-  private final HopperSubsystem m_hopperSubsystem;
-  private final TowerSubsystem m_towerSubsystem;
+    private final HopperSubsystem m_hopperSubsystem;
+    private final TowerSubsystem m_towerSubsystem;
 
-  private final double m_intakeSpeed;
-  private final double m_towerSpeed;
+    private final double m_intakeSpeed;
+    private final double m_towerSpeed;
 
-  /**
-   * Creates a new IntakeCommand.
-   *
-   * @param subsystem The subsystem used by this command.
-   */
-  public RunIntakeCommand(
-      IntakeSubsystem intakeSubsystem,
-      double intakeSpeed,
-      HopperSubsystem hopperSubsystem,
-      TowerSubsystem towerSubsystem,
-      double towerSpeed) {
-    m_intakeSubsystem = intakeSubsystem;
-    m_hopperSubsystem = hopperSubsystem;
-    m_towerSubsystem = towerSubsystem;
-    m_intakeSpeed = intakeSpeed;
-    m_towerSpeed = towerSpeed;
-    // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(intakeSubsystem);
-    addRequirements(hopperSubsystem);
-    addRequirements(towerSubsystem);
-  }
+    /**
+     * Creates a new IntakeCommand.
+     *
+     * @param subsystem The subsystem used by this command.
+     */
+    public RunIntakeCommand(
+            IntakeSubsystem intakeSubsystem,
+            double intakeSpeed,
+            HopperSubsystem hopperSubsystem,
+            TowerSubsystem towerSubsystem,
+            double towerSpeed) {
+        m_intakeSubsystem = intakeSubsystem;
+        m_hopperSubsystem = hopperSubsystem;
+        m_towerSubsystem = towerSubsystem;
+        m_intakeSpeed = intakeSpeed;
+        m_towerSpeed = towerSpeed;
+        // Use addRequirements() here to declare subsystem dependencies.
+        addRequirements(intakeSubsystem);
+        addRequirements(hopperSubsystem);
+        addRequirements(towerSubsystem);
+    }
 
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {}
+    // Called when the command is initially scheduled.
+    @Override
+    public void initialize() {
+    }
 
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    m_intakeSubsystem.set(m_intakeSpeed);
-    m_hopperSubsystem.setHopperSpeed(m_intakeSpeed);
-    if (m_towerSubsystem.getBeamBreak() || m_intakeSpeed < 0) {
-      m_towerSubsystem.setTowerSpeed(m_towerSpeed);
-      m_towerSubsystem.setKickerSpeed(-0.5);
-    } else {
-      m_towerSubsystem.setTowerSpeed(0);
-      m_towerSubsystem.setKickerSpeed(0);
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {
+        m_intakeSubsystem.set(m_intakeSpeed);
+        m_hopperSubsystem.setHopperSpeed(m_intakeSpeed);
+        if (m_towerSubsystem.getBeamBreak() || m_intakeSpeed < 0) {
+            m_towerSubsystem.setTowerSpeed(m_towerSpeed);
+            m_towerSubsystem.setKickerSpeed(-0.5);
+        } else {
+            m_towerSubsystem.setTowerSpeed(0);
+            m_towerSubsystem.setKickerSpeed(0);
+        }
     }
-  }
 
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {
-    m_intakeSubsystem.set(0);
-    m_hopperSubsystem.setHopperSpeed(0);
-    m_towerSubsystem.setTowerSpeed(0);
-    m_towerSubsystem.setKickerSpeed(0);
-  }
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+        m_intakeSubsystem.set(0);
+        m_hopperSubsystem.setHopperSpeed(0);
+        m_towerSubsystem.setTowerSpeed(0);
+        m_towerSubsystem.setKickerSpeed(0);
+    }
 
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
 }
diff --git a/src/main/java/frc/robot/commands/SetShooterSpeedCommand.java b/src/main/java/frc/robot/commands/SetShooterSpeedCommand.java
index 36b4eaa..703e80d 100644
--- a/src/main/java/frc/robot/commands/SetShooterSpeedCommand.java
+++ b/src/main/java/frc/robot/commands/SetShooterSpeedCommand.java
@@ -7,42 +7,46 @@
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.ShooterSubsytem;
 
-/** An example command that uses an example subsystem. */
+/**
+ * An example command that uses an example subsystem.
+ */
 public class SetShooterSpeedCommand extends CommandBase {
-  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-  private final ShooterSubsytem m_shooterSubsystem;
-
-  private double m_goal;
-
-  /**
-   * Creates a new ExampleCommand.
-   *
-   * @param subsystem The subsystem used by this command.
-   */
-  public SetShooterSpeedCommand(ShooterSubsytem shooterSubsystem, double goal) {
-    m_shooterSubsystem = shooterSubsystem;
-    m_goal = goal;
-    // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(shooterSubsystem);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {}
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    m_shooterSubsystem.setShooterSpeed(m_goal);
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {}
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
+    @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+    private final ShooterSubsytem m_shooterSubsystem;
+
+    private final double m_goal;
+
+    /**
+     * Creates a new ExampleCommand.
+     *
+     * @param subsystem The subsystem used by this command.
+     */
+    public SetShooterSpeedCommand(ShooterSubsytem shooterSubsystem, double goal) {
+        m_shooterSubsystem = shooterSubsystem;
+        m_goal = goal;
+        // Use addRequirements() here to declare subsystem dependencies.
+        addRequirements(shooterSubsystem);
+    }
+
+    // Called when the command is initially scheduled.
+    @Override
+    public void initialize() {
+    }
+
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {
+        m_shooterSubsystem.setShooterSpeed(m_goal);
+    }
+
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+    }
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
 }
diff --git a/src/main/java/frc/robot/commands/ShootFromDistanceCommand.java b/src/main/java/frc/robot/commands/ShootFromDistanceCommand.java
index 84347c7..8f299a0 100644
--- a/src/main/java/frc/robot/commands/ShootFromDistanceCommand.java
+++ b/src/main/java/frc/robot/commands/ShootFromDistanceCommand.java
@@ -5,49 +5,53 @@
 import frc.robot.subsystems.LimelightSubsystem;
 import frc.robot.subsystems.ShooterSubsytem;
 
-/** An example command that uses an example subsystem. */
+/**
+ * An example command that uses an example subsystem.
+ */
 public class ShootFromDistanceCommand extends CommandBase {
-  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-  private final ShooterSubsytem m_shooterSubsystem;
-
-  private final ShooterLookupTable m_shooterLookupTable;
-  private final LimelightSubsystem m_limelightSubsystem;
-
-  /**
-   * Creates a new ShootFromDistanceCommand.
-   *
-   * @param subsystem The subsystem used by this command.
-   */
-  public ShootFromDistanceCommand(
-      ShooterSubsytem subsystem,
-      LimelightSubsystem limelightsubsystem,
-      ShooterLookupTable shooterLookupTable) {
-    m_shooterSubsystem = subsystem;
-    m_limelightSubsystem = limelightsubsystem;
-    m_shooterLookupTable = shooterLookupTable;
-    // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(subsystem);
-    addRequirements(limelightsubsystem);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {}
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    m_shooterSubsystem.shootFromDistance(m_limelightSubsystem.limelightDistance());
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {}
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return m_shooterSubsystem.checkAtSpeed(
-        m_shooterLookupTable.getRpmTable(m_limelightSubsystem.limelightDistance()));
-  }
+    @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+    private final ShooterSubsytem m_shooterSubsystem;
+
+    private final ShooterLookupTable m_shooterLookupTable;
+    private final LimelightSubsystem m_limelightSubsystem;
+
+    /**
+     * Creates a new ShootFromDistanceCommand.
+     *
+     * @param subsystem The subsystem used by this command.
+     */
+    public ShootFromDistanceCommand(
+            ShooterSubsytem subsystem,
+            LimelightSubsystem limelightsubsystem,
+            ShooterLookupTable shooterLookupTable) {
+        m_shooterSubsystem = subsystem;
+        m_limelightSubsystem = limelightsubsystem;
+        m_shooterLookupTable = shooterLookupTable;
+        // Use addRequirements() here to declare subsystem dependencies.
+        addRequirements(subsystem);
+        addRequirements(limelightsubsystem);
+    }
+
+    // Called when the command is initially scheduled.
+    @Override
+    public void initialize() {
+    }
+
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {
+        m_shooterSubsystem.shootFromDistance(m_limelightSubsystem.limelightDistance());
+    }
+
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+    }
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return m_shooterSubsystem.checkAtSpeed(
+                m_shooterLookupTable.getRpmTable(m_limelightSubsystem.limelightDistance()));
+    }
 }
diff --git a/src/main/java/frc/robot/commands/ShooterPIDCommand.java b/src/main/java/frc/robot/commands/ShooterPIDCommand.java
index fbb1b2c..cb40eca 100644
--- a/src/main/java/frc/robot/commands/ShooterPIDCommand.java
+++ b/src/main/java/frc/robot/commands/ShooterPIDCommand.java
@@ -4,45 +4,50 @@
 
 package frc.robot.commands;
 
+import com.gos.lib.properties.GosDoubleProperty;
+
 import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc.robot.TunableNumber;
 import frc.robot.subsystems.ShooterSubsytem;
 
-/** An example command that uses an example subsystem. */
+/**
+ * An example command that uses an example subsystem.
+ */
 public class ShooterPIDCommand extends CommandBase {
-  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-  private final ShooterSubsytem m_shooterSubsystem;
-
-  private final TunableNumber m_tunableShooterGoal = new TunableNumber("Shooter Goal", 1000);
-
-  /**
-   * Creates a new ExampleCommand.
-   *
-   * @param subsystem The subsystem used by this command.
-   */
-  public ShooterPIDCommand(ShooterSubsytem shooterSubsystem) {
-    m_shooterSubsystem = shooterSubsystem;
-    // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(shooterSubsystem);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {}
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    m_shooterSubsystem.setPidRpm(m_tunableShooterGoal.get());
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {}
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return m_shooterSubsystem.checkAtSpeed(m_tunableShooterGoal.get());
-  }
+    @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+    private final ShooterSubsytem m_shooterSubsystem;
+
+    private final GosDoubleProperty m_tunableShooterGoal = new GosDoubleProperty(false, "Shooter Goal", 1000);
+
+    /**
+     * Creates a new ExampleCommand.
+     *
+     * @param subsystem The subsystem used by this command.
+     */
+    public ShooterPIDCommand(ShooterSubsytem shooterSubsystem) {
+        m_shooterSubsystem = shooterSubsystem;
+        // Use addRequirements() here to declare subsystem dependencies.
+        addRequirements(shooterSubsystem);
+    }
+
+    // Called when the command is initially scheduled.
+    @Override
+    public void initialize() {
+    }
+
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {
+        m_shooterSubsystem.setPidRpm(m_tunableShooterGoal.getValue());
+    }
+
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+    }
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return m_shooterSubsystem.checkAtSpeed(m_tunableShooterGoal.getValue());
+    }
 }
diff --git a/src/main/java/frc/robot/commands/StopIntakeCommand.java b/src/main/java/frc/robot/commands/StopIntakeCommand.java
index 718bf86..3a83051 100644
--- a/src/main/java/frc/robot/commands/StopIntakeCommand.java
+++ b/src/main/java/frc/robot/commands/StopIntakeCommand.java
@@ -8,49 +8,52 @@
 import frc.robot.subsystems.HopperSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 
-/** An example command that uses an example subsystem. */
+/**
+ * An example command that uses an example subsystem.
+ */
 public class StopIntakeCommand extends CommandBase {
-  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-  private final IntakeSubsystem m_intakeSubsystem;
-  private final HopperSubsystem m_hopperSubsystem;
-
-
-  /**
-   * Creates a new IntakeCommand.
-   *
-   * @param subsystem The subsystem used by this command.
-   */
-  public StopIntakeCommand(
-      IntakeSubsystem intakeSubsystem,
-      HopperSubsystem hopperSubsystem) {
-    m_intakeSubsystem = intakeSubsystem;
-    m_hopperSubsystem = hopperSubsystem;
-    // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(intakeSubsystem);
-    addRequirements(hopperSubsystem);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {}
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    m_intakeSubsystem.set(0);
-    m_hopperSubsystem.setHopperSpeed(0);
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {
-    m_intakeSubsystem.set(0);
-    m_hopperSubsystem.setHopperSpeed(0);
-  }
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
+    @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+    private final IntakeSubsystem m_intakeSubsystem;
+    private final HopperSubsystem m_hopperSubsystem;
+
+
+    /**
+     * Creates a new IntakeCommand.
+     *
+     * @param subsystem The subsystem used by this command.
+     */
+    public StopIntakeCommand(
+            IntakeSubsystem intakeSubsystem,
+            HopperSubsystem hopperSubsystem) {
+        m_intakeSubsystem = intakeSubsystem;
+        m_hopperSubsystem = hopperSubsystem;
+        // Use addRequirements() here to declare subsystem dependencies.
+        addRequirements(intakeSubsystem);
+        addRequirements(hopperSubsystem);
+    }
+
+    // Called when the command is initially scheduled.
+    @Override
+    public void initialize() {
+    }
+
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {
+        m_intakeSubsystem.set(0);
+        m_hopperSubsystem.setHopperSpeed(0);
+    }
+
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+        m_intakeSubsystem.set(0);
+        m_hopperSubsystem.setHopperSpeed(0);
+    }
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
 }
diff --git a/src/main/java/frc/robot/commands/TowerDownCommand.java b/src/main/java/frc/robot/commands/TowerDownCommand.java
index 73de9f3..7edb36f 100644
--- a/src/main/java/frc/robot/commands/TowerDownCommand.java
+++ b/src/main/java/frc/robot/commands/TowerDownCommand.java
@@ -7,43 +7,46 @@
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.TowerSubsystem;
 
-/** An example command that uses an example subsystem. */
+/**
+ * An example command that uses an example subsystem.
+ */
 public class TowerDownCommand extends CommandBase {
-  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-  private final TowerSubsystem m_towerSubsystem;
-
-  private final double m_speed = -0.5;
-
-  /**
-   * Creates a new TowerDownCommand.
-   *
-   * @param subsystem The subsystem used by this command.
-   */
-  public TowerDownCommand(TowerSubsystem towerSubsystem) {
-    m_towerSubsystem = towerSubsystem;
-    // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(towerSubsystem);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {}
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    m_towerSubsystem.setTowerSpeed(m_speed);
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {
-    m_towerSubsystem.setTowerSpeed(0);
-  }
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
+    @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+    private final TowerSubsystem m_towerSubsystem;
+
+    private final double m_speed = -0.5;
+
+    /**
+     * Creates a new TowerDownCommand.
+     *
+     * @param subsystem The subsystem used by this command.
+     */
+    public TowerDownCommand(TowerSubsystem towerSubsystem) {
+        m_towerSubsystem = towerSubsystem;
+        // Use addRequirements() here to declare subsystem dependencies.
+        addRequirements(towerSubsystem);
+    }
+
+    // Called when the command is initially scheduled.
+    @Override
+    public void initialize() {
+    }
+
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {
+        m_towerSubsystem.setTowerSpeed(m_speed);
+    }
+
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+        m_towerSubsystem.setTowerSpeed(0);
+    }
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
 }
diff --git a/src/main/java/frc/robot/commands/TowerKickerCommand.java b/src/main/java/frc/robot/commands/TowerKickerCommand.java
index 84c014d..1b168bc 100644
--- a/src/main/java/frc/robot/commands/TowerKickerCommand.java
+++ b/src/main/java/frc/robot/commands/TowerKickerCommand.java
@@ -7,44 +7,46 @@
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.TowerSubsystem;
 
-/** An example command that uses an example subsystem. */
+/**
+ * An example command that uses an example subsystem.
+ */
 public class TowerKickerCommand extends CommandBase {
-  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-  private final TowerSubsystem m_towerSubsystem;
-
-  private final double m_speed = 0.75;
-
-  /**
-   * Creates a new TowerDownCommand.
-   *
-   * @param subsystem The subsystem used by this command.
-   */
-  public TowerKickerCommand(TowerSubsystem towerSubsystem) {
-    m_towerSubsystem = towerSubsystem;
-    // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(towerSubsystem);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {}
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    m_towerSubsystem.setKickerSpeed(m_speed);
-    ;
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {
-    m_towerSubsystem.setKickerSpeed(0);
-  }
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
+    @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+    private final TowerSubsystem m_towerSubsystem;
+
+    private static final double SPEED = 0.75;
+
+    /**
+     * Creates a new TowerDownCommand.
+     *
+     * @param subsystem The subsystem used by this command.
+     */
+    public TowerKickerCommand(TowerSubsystem towerSubsystem) {
+        m_towerSubsystem = towerSubsystem;
+        // Use addRequirements() here to declare subsystem dependencies.
+        addRequirements(towerSubsystem);
+    }
+
+    // Called when the command is initially scheduled.
+    @Override
+    public void initialize() {
+    }
+
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {
+        m_towerSubsystem.setKickerSpeed(SPEED);
+    }
+
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+        m_towerSubsystem.setKickerSpeed(0);
+    }
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
 }
diff --git a/src/main/java/frc/robot/commands/TowerUpCommand.java b/src/main/java/frc/robot/commands/TowerUpCommand.java
index 4f5e3cc..ab3eff2 100644
--- a/src/main/java/frc/robot/commands/TowerUpCommand.java
+++ b/src/main/java/frc/robot/commands/TowerUpCommand.java
@@ -7,45 +7,48 @@
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.TowerSubsystem;
 
-/** An example command that uses an example subsystem. */
+/**
+ * An example command that uses an example subsystem.
+ */
 public class TowerUpCommand extends CommandBase {
-  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-  private final TowerSubsystem m_towerSubsystem;
-
-  private final double m_speed = 0.5;
-
-  /**
-   * Creates a new TowerUpCommand.
-   *
-   * @param subsystem The subsystem used by this command.
-   */
-  public TowerUpCommand(TowerSubsystem towerSubsystem) {
-    m_towerSubsystem = towerSubsystem;
-    // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(towerSubsystem);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {}
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    m_towerSubsystem.setTowerSpeed(m_speed);
-    m_towerSubsystem.setKickerSpeed(-0.5);
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {
-    m_towerSubsystem.setTowerSpeed(0);
-    m_towerSubsystem.setKickerSpeed(0);
-  }
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
+    @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+    private final TowerSubsystem m_towerSubsystem;
+
+    private static final double SPEED = 0.5;
+
+    /**
+     * Creates a new TowerUpCommand.
+     *
+     * @param subsystem The subsystem used by this command.
+     */
+    public TowerUpCommand(TowerSubsystem towerSubsystem) {
+        m_towerSubsystem = towerSubsystem;
+        // Use addRequirements() here to declare subsystem dependencies.
+        addRequirements(towerSubsystem);
+    }
+
+    // Called when the command is initially scheduled.
+    @Override
+    public void initialize() {
+    }
+
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {
+        m_towerSubsystem.setTowerSpeed(SPEED);
+        m_towerSubsystem.setKickerSpeed(-0.5);
+    }
+
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+        m_towerSubsystem.setTowerSpeed(0);
+        m_towerSubsystem.setKickerSpeed(0);
+    }
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
 }
diff --git a/src/main/java/frc/robot/commands/TrajectoryCommand.java b/src/main/java/frc/robot/commands/TrajectoryCommand.java
index e99ad95..4a1e442 100644
--- a/src/main/java/frc/robot/commands/TrajectoryCommand.java
+++ b/src/main/java/frc/robot/commands/TrajectoryCommand.java
@@ -13,58 +13,62 @@
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.DrivetrainSubsystem;
+
 import java.io.IOException;
 import java.nio.file.Path;
 
-/** An example command that uses an example subsystem. */
+/**
+ * An example command that uses an example subsystem.
+ */
 public class TrajectoryCommand extends CommandBase {
-  private final DrivetrainSubsystem m_subsystem;
-  private Trajectory m_trajectory = null;
-  private final Timer m_timer;
-  private final RamseteController m_controller;
+    private final DrivetrainSubsystem m_subsystem;
+    private Trajectory m_trajectory;
+    private final Timer m_timer;
+    private final RamseteController m_controller;
 
-  /**
-   * Creates a new TrajectoryCommand.
-   *
-   * @param subsystem The subsystem used by this command.
-   */
-  public TrajectoryCommand(DrivetrainSubsystem subsystem, String trajectoryPathName) {
-    m_subsystem = subsystem;
-    try {
-      Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryPathName);
-      m_trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
-    } catch (IOException ex) {
-      DriverStation.reportError(
-          "Unable to open trajectory: " + trajectoryPathName, ex.getStackTrace());
-    }
-    m_timer = new Timer();
-    m_controller = new RamseteController();
+    /**
+     * Creates a new TrajectoryCommand.
+     *
+     * @param subsystem The subsystem used by this command.
+     */
+    public TrajectoryCommand(DrivetrainSubsystem subsystem, String trajectoryPathName) {
+        m_subsystem = subsystem;
+        try {
+            Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryPathName);
+            m_trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
+        } catch (IOException ex) {
+            DriverStation.reportError(
+                    "Unable to open trajectory: " + trajectoryPathName, ex.getStackTrace());
+        }
+        m_timer = new Timer();
+        m_controller = new RamseteController();
 
-    // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(subsystem);
-  }
+        // Use addRequirements() here to declare subsystem dependencies.
+        addRequirements(subsystem);
+    }
 
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-    m_timer.reset();
-  }
+    // Called when the command is initially scheduled.
+    @Override
+    public void initialize() {
+        m_timer.reset();
+    }
 
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    Trajectory.State goal = m_trajectory.sample(m_timer.get());
-    ChassisSpeeds adjustedSpeeds = m_controller.calculate(m_subsystem.getPose(), goal);
-    m_subsystem.applyChassisSpeed(adjustedSpeeds);
-  }
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {
+        Trajectory.State goal = m_trajectory.sample(m_timer.get());
+        ChassisSpeeds adjustedSpeeds = m_controller.calculate(m_subsystem.getPose(), goal);
+        m_subsystem.applyChassisSpeed(adjustedSpeeds);
+    }
 
-  @Override
-  // Called once the command ends or is interrupted.
-  public void end(boolean interrupted) {}
+    @Override
+    // Called once the command ends or is interrupted.
+    public void end(boolean interrupted) {
+    }
 
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return m_timer.hasElapsed(m_trajectory.getTotalTimeSeconds());
-  }
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return m_timer.hasElapsed(m_trajectory.getTotalTimeSeconds());
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
index 12cb4b1..ae44b1c 100644
--- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
@@ -15,77 +15,76 @@
 import frc.robot.Constants;
 
 public class ClimberSubsystem extends SubsystemBase {
+    private final CANSparkMax m_leftClimber = new CANSparkMax(Constants.CLIMBER_LEFT, MotorType.kBrushless);
+    private final CANSparkMax m_rightClimber =
+            new CANSparkMax(Constants.CLIMBER_RIGHT, MotorType.kBrushless);
+    private final DigitalInput m_leftLimitSwitch = new DigitalInput(Constants.LEFT_LIMIT_SWITCH);
+    private final DigitalInput m_rightLimitSwitch = new DigitalInput(Constants.RIGHT_LIMIT_SWITCH);
+    private final RelativeEncoder m_leftEncoder = m_leftClimber.getEncoder();
+    private final RelativeEncoder m_rightEncoder = m_rightClimber.getEncoder();
+    private final ProfiledPIDController m_climberPID =
+            new ProfiledPIDController(3.5, 0.0, 0.0, new Constraints(20.0, 150.0), 0.02);
+
+    /**
+     * Creates a new ClimberSubsystem.
+     */
+    public ClimberSubsystem() {
+        m_leftClimber.restoreFactoryDefaults();
+        m_rightClimber.restoreFactoryDefaults();
+        m_leftClimber.setInverted(true);
+        m_rightClimber.setInverted(false);
+        m_leftClimber.burnFlash();
+        m_rightClimber.burnFlash();
+
+        m_rightClimber.follow(m_leftClimber, true);
+        m_leftClimber.setSmartCurrentLimit(50);
+        m_rightClimber.setSmartCurrentLimit(50);
 
-
-  private CANSparkMax m_leftClimber = new CANSparkMax(Constants.CLIMBER_LEFT, MotorType.kBrushless);
-  private CANSparkMax m_rightClimber =
-      new CANSparkMax(Constants.CLIMBER_RIGHT, MotorType.kBrushless);
-  private DigitalInput m_leftLimitSwitch = new DigitalInput(Constants.LEFT_LIMIT_SWITCH);
-  private DigitalInput m_rightLimitSwitch = new DigitalInput(Constants.RIGHT_LIMIT_SWITCH);
-  private RelativeEncoder m_leftEncoder = m_leftClimber.getEncoder();
-  private RelativeEncoder m_rightEncoder = m_rightClimber.getEncoder();
-  private ProfiledPIDController m_climberPID =
-      new ProfiledPIDController(3.5, 0.0, 0.0, new Constraints(20.0, 150.0), 0.02);
-
-  private final double m_encoderReduction = 12 * 50 / 20;
-
-  /** Creates a new ClimberSubsystem. */
-  public ClimberSubsystem() {
-    m_leftClimber.restoreFactoryDefaults();
-    m_rightClimber.restoreFactoryDefaults();
-    m_leftClimber.setInverted(true);
-    m_rightClimber.setInverted(false);
-    m_leftClimber.burnFlash();
-    m_rightClimber.burnFlash();
-
-    m_rightClimber.follow(m_leftClimber, true);
-    m_leftClimber.setSmartCurrentLimit(50);
-    m_rightClimber.setSmartCurrentLimit(50);
-
-  }
-
-  public void RunClimberPID(double goal) {
-    m_climberPID.setGoal(goal);
-    set(m_climberPID.calculate(m_leftEncoder.getPosition()));
-  }
-
-  public void set(double speed) {
-    if (speed < 0 && (leftLimitSwitchPress() || rightLimitSwitchPress())) {
-      speed = 0;
     }
-    if (speed > 0 && m_leftEncoder.getPosition() > 150) {
-      speed = 0;
+
+    public void runClimberPID(double goal) {
+        m_climberPID.setGoal(goal);
+        set(m_climberPID.calculate(m_leftEncoder.getPosition()));
     }
-    m_leftClimber.set(speed);
-  }
 
-  public boolean leftLimitSwitchPress() {
-    return m_leftLimitSwitch.get();
-  }
+    @SuppressWarnings("PMD.AvoidReassigningParameters")
+    public void set(double speed) {
+        if (speed < 0 && (leftLimitSwitchPress() || rightLimitSwitchPress())) {
+            speed = 0;
+        }
+        if (speed > 0 && m_leftEncoder.getPosition() > 150) {
+            speed = 0;
+        }
+        m_leftClimber.set(speed);
+    }
 
-  public boolean rightLimitSwitchPress() {
-    return !m_rightLimitSwitch.get();
-  }
+    public boolean leftLimitSwitchPress() {
+        return m_leftLimitSwitch.get();
+    }
 
-  public double getPIDPosition() {
-    return m_leftEncoder.getPosition();
-  }
+    public boolean rightLimitSwitchPress() {
+        return !m_rightLimitSwitch.get();
+    }
 
-  @Override
-  public void periodic() {
-    // This method will be called once per scheduler run
-    if (leftLimitSwitchPress() || rightLimitSwitchPress()) {
-      m_leftEncoder.setPosition(0);
-      m_rightEncoder.setPosition(0);
+    public double getPIDPosition() {
+        return m_leftEncoder.getPosition();
     }
 
-    SmartDashboard.putBoolean("left limit", leftLimitSwitchPress());
-    SmartDashboard.putBoolean("right limit", rightLimitSwitchPress());
-    SmartDashboard.putNumber("climb left encoder", m_leftEncoder.getPosition());
-  }
+    @Override
+    public void periodic() {
+        // This method will be called once per scheduler run
+        if (leftLimitSwitchPress() || rightLimitSwitchPress()) {
+            m_leftEncoder.setPosition(0);
+            m_rightEncoder.setPosition(0);
+        }
+
+        SmartDashboard.putBoolean("left limit", leftLimitSwitchPress());
+        SmartDashboard.putBoolean("right limit", rightLimitSwitchPress());
+        SmartDashboard.putNumber("climb left encoder", m_leftEncoder.getPosition());
+    }
 
-  @Override
-  public void simulationPeriodic() {
-    // This method will be called once per scheduler run during simulation
-  }
+    @Override
+    public void simulationPeriodic() {
+        // This method will be called once per scheduler run during simulation
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java
index b32ad1f..8ed9c64 100644
--- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java
@@ -4,6 +4,8 @@
 
 package frc.robot.subsystems;
 
+import com.gos.lib.properties.PidProperty;
+import com.gos.lib.rev.RevPidPropertyBuilder;
 import com.kauailabs.navx.frc.AHRS;
 import com.pathplanner.lib.auto.RamseteAutoBuilder;
 import com.revrobotics.CANSparkMax;
@@ -25,144 +27,148 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
-import frc.robot.TunableNumber;
 
 import java.util.Map;
 import java.util.function.Consumer;
 
 // Drive train
+@SuppressWarnings("PMD.TooManyMethods")
 public class DrivetrainSubsystem extends SubsystemBase {
-  private final CANSparkMax m_leftLeader =
-      new CANSparkMax(Constants.DRIVE_LEFT_LEADER, MotorType.kBrushless);
-  private final CANSparkMax m_leftFollower =
-      new CANSparkMax(Constants.DRIVE_LEFT_FOLLOWER, MotorType.kBrushless);
-  private final CANSparkMax m_rightLeader =
-      new CANSparkMax(Constants.DRIVE_RIGHT_LEADER, MotorType.kBrushless);
-  private final CANSparkMax m_rightFollower =
-      new CANSparkMax(Constants.DRIVE_RIGHT_FOLLOWER, MotorType.kBrushless);
-  private DifferentialDrive m_drive = new DifferentialDrive(m_leftLeader, m_rightLeader);
-  private DifferentialDriveKinematics m_kinematics =
-      new DifferentialDriveKinematics(Constants.DRIVE_TRACK);
-
-  // odometry
-  private RelativeEncoder m_leftEncoder = m_leftLeader.getEncoder();
-  private RelativeEncoder m_rightEncoder = m_rightLeader.getEncoder();
-  private AHRS m_gyro = new AHRS(SPI.Port.kMXP);
-  private DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(0), 0, 0);
-  private Field2d m_field = new Field2d();
-
-  // PID
-  private SparkMaxPIDController m_leftController = m_leftLeader.getPIDController();
-  private SparkMaxPIDController m_rightController = m_rightLeader.getPIDController();
-  private TunableNumber m_velocityP = new TunableNumber("Drive Train Velocity P", 1);
-  private TunableNumber m_velocityI = new TunableNumber("Drive Train Velocity I");
-  private TunableNumber m_velocityD = new TunableNumber("Drive Train Velocity D");
-
-  /** Creates a new DrivetrainSubsystem. */
-  public DrivetrainSubsystem() {
-    m_gyro.calibrate();
-
-    m_leftLeader.restoreFactoryDefaults();
-    m_leftFollower.restoreFactoryDefaults();
-    m_rightLeader.restoreFactoryDefaults();
-    m_rightFollower.restoreFactoryDefaults();
-
-    m_leftLeader.setSmartCurrentLimit(50);
-    m_leftFollower.setSmartCurrentLimit(50);
-    m_rightLeader.setSmartCurrentLimit(50);
-    m_rightFollower.setSmartCurrentLimit(50);
-
-    m_leftLeader.setInverted(true);
-
-    m_leftFollower.follow(m_leftLeader);
-    m_rightFollower.follow(m_rightLeader);
-
-    m_leftEncoder.setPositionConversionFactor(Constants.DRIVE_CONVERSION_FACTOR);
-    m_rightEncoder.setPositionConversionFactor(Constants.DRIVE_CONVERSION_FACTOR);
-    m_leftEncoder.setVelocityConversionFactor(Constants.DRIVE_CONVERSION_FACTOR);
-    m_rightEncoder.setVelocityConversionFactor(Constants.DRIVE_CONVERSION_FACTOR);
-
-    m_leftLeader.burnFlash();
-    m_leftFollower.burnFlash();
-    m_rightLeader.burnFlash();
-    m_rightFollower.burnFlash();
-
-    SmartDashboard.putData(m_field);
-    if (m_velocityP.hasChanged() || m_velocityI.hasChanged() || m_velocityD.hasChanged()) {
-      updateDrivePID();
+    private final CANSparkMax m_leftLeader =
+            new CANSparkMax(Constants.DRIVE_LEFT_LEADER, MotorType.kBrushless);
+    private final CANSparkMax m_leftFollower =
+            new CANSparkMax(Constants.DRIVE_LEFT_FOLLOWER, MotorType.kBrushless);
+    private final CANSparkMax m_rightLeader =
+            new CANSparkMax(Constants.DRIVE_RIGHT_LEADER, MotorType.kBrushless);
+    private final CANSparkMax m_rightFollower =
+            new CANSparkMax(Constants.DRIVE_RIGHT_FOLLOWER, MotorType.kBrushless);
+    private final DifferentialDrive m_drive = new DifferentialDrive(m_leftLeader, m_rightLeader);
+    private final DifferentialDriveKinematics m_kinematics =
+            new DifferentialDriveKinematics(Constants.DRIVE_TRACK);
+
+    // odometry
+    private final RelativeEncoder m_leftEncoder = m_leftLeader.getEncoder();
+    private final RelativeEncoder m_rightEncoder = m_rightLeader.getEncoder();
+    private final AHRS m_gyro = new AHRS(SPI.Port.kMXP);
+    private final DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(0), 0, 0);
+    private final Field2d m_field = new Field2d();
+
+    // PID
+    private final SparkMaxPIDController m_leftController = m_leftLeader.getPIDController();
+    private final SparkMaxPIDController m_rightController = m_rightLeader.getPIDController();
+    private final PidProperty m_leftPidProperty;
+    private final PidProperty m_rightPidProperty;
+
+    /**
+     * Creates a new DrivetrainSubsystem.
+     */
+    public DrivetrainSubsystem() {
+        m_gyro.calibrate();
+
+        m_leftLeader.restoreFactoryDefaults();
+        m_leftFollower.restoreFactoryDefaults();
+        m_rightLeader.restoreFactoryDefaults();
+        m_rightFollower.restoreFactoryDefaults();
+
+        m_leftLeader.setSmartCurrentLimit(50);
+        m_leftFollower.setSmartCurrentLimit(50);
+        m_rightLeader.setSmartCurrentLimit(50);
+        m_rightFollower.setSmartCurrentLimit(50);
+
+        m_leftLeader.setInverted(true);
+
+        m_leftFollower.follow(m_leftLeader);
+        m_rightFollower.follow(m_rightLeader);
+
+        m_leftEncoder.setPositionConversionFactor(Constants.DRIVE_CONVERSION_FACTOR);
+        m_rightEncoder.setPositionConversionFactor(Constants.DRIVE_CONVERSION_FACTOR);
+        m_leftEncoder.setVelocityConversionFactor(Constants.DRIVE_CONVERSION_FACTOR);
+        m_rightEncoder.setVelocityConversionFactor(Constants.DRIVE_CONVERSION_FACTOR);
+
+        m_leftLeader.burnFlash();
+        m_leftFollower.burnFlash();
+        m_rightLeader.burnFlash();
+        m_rightFollower.burnFlash();
+
+        SmartDashboard.putData(m_field);
+
+        m_leftPidProperty = createDrivetrainProperty(m_leftController);
+        m_rightPidProperty = createDrivetrainProperty(m_rightController);
+
+    }
+
+    private PidProperty createDrivetrainProperty(SparkMaxPIDController pidController) {
+        return new RevPidPropertyBuilder("Drivetrain", false, pidController, 0)
+                .addP(1)
+                .addI(0)
+                .addD(0)
+                .build();
+    }
+
+    public void control(double speed, double rotation) {
+        m_drive.curvatureDrive(speed, rotation, Math.abs(speed) < 0.05);
+    }
+
+    public void smartVelocityControl(double leftVelocity, double rightVelocity) {
+        m_leftController.setReference(leftVelocity, ControlType.kVelocity);
+        m_rightController.setReference(rightVelocity, ControlType.kVelocity);
+    }
+
+    public void applyChassisSpeed(ChassisSpeeds speeds) {
+        DifferentialDriveWheelSpeeds wheelSpeeds = m_kinematics.toWheelSpeeds(speeds);
+        applyWheelSpeed(wheelSpeeds);
+    }
+
+    public void applyWheelSpeed(DifferentialDriveWheelSpeeds speeds) {
+        m_leftController.setReference(speeds.leftMetersPerSecond, ControlType.kSmartVelocity);
+        m_rightController.setReference(speeds.rightMetersPerSecond, ControlType.kSmartVelocity);
+    }
+
+    public Pose2d getPose() {
+        return m_odometry.getPoseMeters();
     }
 
-  }
-
-  public void control(double speed, double rotation) {
-    m_drive.curvatureDrive(speed, rotation, Math.abs(speed) < 0.05);
-  }
-
-  public void smartVelocityControl(double leftVelocity, double rightVelocity){
-    m_leftController.setReference(leftVelocity, ControlType.kVelocity);
-    m_rightController.setReference(rightVelocity, ControlType.kVelocity);
-  }
-
-  public void applyChassisSpeed(ChassisSpeeds speeds) {
-    DifferentialDriveWheelSpeeds wheelSpeeds = m_kinematics.toWheelSpeeds(speeds);
-    applyWheelSpeed(wheelSpeeds);
-  }
-
-  public void applyWheelSpeed(DifferentialDriveWheelSpeeds speeds) {
-    m_leftController.setReference(speeds.leftMetersPerSecond, ControlType.kSmartVelocity);
-    m_rightController.setReference(speeds.rightMetersPerSecond, ControlType.kSmartVelocity);
-  }
-
-  public Pose2d getPose() {
-    return m_odometry.getPoseMeters();
-  }
-
-  public void updateDrivePID() {
-    m_leftController.setP(m_velocityP.get());
-    m_leftController.setI(m_velocityI.get());
-    m_leftController.setD(m_velocityD.get());
-    m_rightController.setP(m_velocityP.get());
-    m_rightController.setI(m_velocityI.get());
-    m_rightController.setD(m_velocityD.get());
-  }
-
-  public double leftVelocity() {
-    return m_leftEncoder.getVelocity();
-  }
-
-  public double rightVelocity() {
-    return m_rightEncoder.getVelocity();
-  }
-
-  private RamseteAutoBuilder createRamseteAutoBuilder(Map<String, Command> eventMap, Consumer<Pose2d> poseSetter) {
-    return new RamseteAutoBuilder(
-            this::getPose, // Pose supplier
-            poseSetter,
-            new RamseteController(),
-            m_kinematics, // DifferentialDriveKinematics
-            this::smartVelocityControl, // DifferentialDriveWheelSpeeds supplier
-            eventMap,
-            true, // Should the path be automatically mirrored depending on alliance color. Optional, defaults to true
-            this);
-  }
-
-  public RamseteAutoBuilder ramseteAutoBuilderNoPoseReset(Map<String, Command> eventMap) {
-    return createRamseteAutoBuilder(eventMap, (Pose2d pose) -> {});
-  }
-
-  @Override
-  public void periodic() {
-    // This method will be called once per scheduler run
-    Rotation2d gyroAngle = Rotation2d.fromDegrees(-m_gyro.getAngle());
-    m_odometry.update(gyroAngle, m_leftEncoder.getPosition(), m_rightEncoder.getPosition());
-    m_field.setRobotPose(m_odometry.getPoseMeters());
-    SmartDashboard.putNumber("Chassis Left Velocity", leftVelocity());
-    SmartDashboard.putNumber("Chassis Right Velocity", rightVelocity());
-
-  }
-
-  @Override
-  public void simulationPeriodic() {
-    // This method will be called once per scheduler run during simulation
-  }
+    public double leftVelocity() {
+        return m_leftEncoder.getVelocity();
+    }
+
+    public double rightVelocity() {
+        return m_rightEncoder.getVelocity();
+    }
+
+    private RamseteAutoBuilder createRamseteAutoBuilder(Map<String, Command> eventMap, Consumer<Pose2d> poseSetter) {
+        return new RamseteAutoBuilder(
+                this::getPose, // Pose supplier
+                poseSetter,
+                new RamseteController(),
+                m_kinematics, // DifferentialDriveKinematics
+                this::smartVelocityControl, // DifferentialDriveWheelSpeeds supplier
+                eventMap,
+                true, // Should the path be automatically mirrored depending on alliance color. Optional, defaults to true
+                this);
+    }
+
+    public RamseteAutoBuilder ramseteAutoBuilderNoPoseReset(Map<String, Command> eventMap) {
+        return createRamseteAutoBuilder(eventMap, (Pose2d pose) -> {
+        });
+    }
+
+    @Override
+    public void periodic() {
+        m_leftPidProperty.updateIfChanged();
+        m_rightPidProperty.updateIfChanged();
+
+        // This method will be called once per scheduler run
+        Rotation2d gyroAngle = Rotation2d.fromDegrees(-m_gyro.getAngle());
+        m_odometry.update(gyroAngle, m_leftEncoder.getPosition(), m_rightEncoder.getPosition());
+        m_field.setRobotPose(m_odometry.getPoseMeters());
+        SmartDashboard.putNumber("Chassis Left Velocity", leftVelocity());
+        SmartDashboard.putNumber("Chassis Right Velocity", rightVelocity());
+
+    }
+
+    @Override
+    public void simulationPeriodic() {
+        // This method will be called once per scheduler run during simulation
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java b/src/main/java/frc/robot/subsystems/ExampleSubsystem.java
deleted file mode 100644
index f5e9470..0000000
--- a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java
+++ /dev/null
@@ -1,22 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot.subsystems;
-
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
-public class ExampleSubsystem extends SubsystemBase {
-  /** Creates a new ExampleSubsystem. */
-  public ExampleSubsystem() {}
-
-  @Override
-  public void periodic() {
-    // This method will be called once per scheduler run
-  }
-
-  @Override
-  public void simulationPeriodic() {
-    // This method will be called once per scheduler run during simulation
-  }
-}
diff --git a/src/main/java/frc/robot/subsystems/HopperSubsystem.java b/src/main/java/frc/robot/subsystems/HopperSubsystem.java
index 8ef2cd7..19445be 100644
--- a/src/main/java/frc/robot/subsystems/HopperSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/HopperSubsystem.java
@@ -6,28 +6,30 @@
 import frc.robot.Constants;
 
 public class HopperSubsystem extends SubsystemBase {
-  private final CANSparkMax m_hopperMotor;
+    private final CANSparkMax m_hopperMotor;
 
-  /** Creates a new HopperSubsystem. */
-  public HopperSubsystem() {
-    m_hopperMotor = new CANSparkMax(Constants.HOPPER_SPARK, MotorType.kBrushless);
-    m_hopperMotor.setIdleMode(CANSparkMax.IdleMode.kCoast);
-    m_hopperMotor.setSmartCurrentLimit(30);
-    m_hopperMotor.restoreFactoryDefaults();
-    m_hopperMotor.burnFlash();
-  }
+    /**
+     * Creates a new HopperSubsystem.
+     */
+    public HopperSubsystem() {
+        m_hopperMotor = new CANSparkMax(Constants.HOPPER_SPARK, MotorType.kBrushless);
+        m_hopperMotor.setIdleMode(CANSparkMax.IdleMode.kCoast);
+        m_hopperMotor.setSmartCurrentLimit(30);
+        m_hopperMotor.restoreFactoryDefaults();
+        m_hopperMotor.burnFlash();
+    }
 
-  @Override
-  public void periodic() {
-    // This method will be called once per scheduler run
-  }
+    @Override
+    public void periodic() {
+        // This method will be called once per scheduler run
+    }
 
-  @Override
-  public void simulationPeriodic() {
-    // This method will be called once per scheduler run during simulation
-  }
+    @Override
+    public void simulationPeriodic() {
+        // This method will be called once per scheduler run during simulation
+    }
 
-  public void setHopperSpeed(double speed) {
-    m_hopperMotor.set(speed);
-  }
+    public void setHopperSpeed(double speed) {
+        m_hopperMotor.set(speed);
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
index 6ddadb4..fdda996 100644
--- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -12,46 +12,49 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class IntakeSubsystem extends SubsystemBase {
-  private CANSparkMax m_motor = new CANSparkMax(6, MotorType.kBrushless);
-  private DoubleSolenoid m_solenoid = new DoubleSolenoid(PneumaticsModuleType.CTREPCM, 1, 7);
-  /** Creates a new IntakeSubsystem. */
-  public IntakeSubsystem() {
-    m_motor.restoreFactoryDefaults();
-    m_motor.setInverted(true);
-    m_motor.setSmartCurrentLimit(50);
-    retract();
-
-    m_motor.burnFlash();
-  }
-
-  public void set(double speed) {
-    m_motor.set(speed);
-    if (speed == 0) {
-      retract();
-    } else {
-      extend();
+    private final CANSparkMax m_motor = new CANSparkMax(6, MotorType.kBrushless);
+    private final DoubleSolenoid m_solenoid = new DoubleSolenoid(PneumaticsModuleType.CTREPCM, 1, 7);
+
+    /**
+     * Creates a new IntakeSubsystem.
+     */
+    public IntakeSubsystem() {
+        m_motor.restoreFactoryDefaults();
+        m_motor.setInverted(true);
+        m_motor.setSmartCurrentLimit(50);
+        retract();
+
+        m_motor.burnFlash();
     }
-  }
 
-  public void extend() {
-    m_solenoid.set(Value.kForward);
-  }
+    public void set(double speed) {
+        m_motor.set(speed);
+        if (speed == 0) {
+            retract();
+        } else {
+            extend();
+        }
+    }
 
-  public void retract() {
-    m_solenoid.set(Value.kReverse);
-  }
+    public void extend() {
+        m_solenoid.set(Value.kForward);
+    }
 
-  public void toggle() {
-    m_solenoid.toggle();
-  }
+    public final void retract() {
+        m_solenoid.set(Value.kReverse);
+    }
 
-  @Override
-  public void periodic() {
-    // This method will be called once per scheduler run
-  }
+    public void toggle() {
+        m_solenoid.toggle();
+    }
 
-  @Override
-  public void simulationPeriodic() {
-    // This method will be called once per scheduler run during simulation
-  }
+    @Override
+    public void periodic() {
+        // This method will be called once per scheduler run
+    }
+
+    @Override
+    public void simulationPeriodic() {
+        // This method will be called once per scheduler run during simulation
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/robot/subsystems/LimelightSubsystem.java
index 68f2afa..efe5572 100644
--- a/src/main/java/frc/robot/subsystems/LimelightSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/LimelightSubsystem.java
@@ -7,38 +7,35 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class LimelightSubsystem extends SubsystemBase {
-  NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight");
-  NetworkTableEntry m_horizontalAngle = table.getEntry("tx");
-  NetworkTableEntry m_verticalAngle = table.getEntry("ty");
-  NetworkTableEntry m_targetArea = table.getEntry("ta");
+    private final NetworkTable m_table = NetworkTableInstance.getDefault().getTable("limelight");
+    private final NetworkTableEntry m_horizontalAngle = m_table.getEntry("tx");
+    private final NetworkTableEntry m_verticalAngle = m_table.getEntry("ty");
 
-  public LimelightSubsystem() {}
+    public double limelightDistance() {
+        double targetOffsetAngleVertical = m_verticalAngle.getDouble(0.0);
 
-  public double limelightDistance() {
-    double targetOffsetAngle_Vertical = m_verticalAngle.getDouble(0.0);
+        // how many degrees back is your limelight rotated from perfectly vertical?
+        double limelightMountAngleDegrees = 25.0;
 
-    // how many degrees back is your limelight rotated from perfectly vertical?
-    double limelightMountAngleDegrees = 25.0;
+        // distance from the center of the Limelight lens to the floor
+        double limelightLensHeightInches = 37.4;
 
-    // distance from the center of the Limelight lens to the floor
-    double limelightLensHeightInches = 37.4;
+        // distance from the target to the floor
+        double goalHeightInches = 104;
 
-    // distance from the target to the floor
-    double goalHeightInches = 104;
+        double angleToGoalDegrees = limelightMountAngleDegrees + targetOffsetAngleVertical;
+        double angleToGoalRadians = angleToGoalDegrees * (Math.PI / 180.0);
 
-    double angleToGoalDegrees = limelightMountAngleDegrees + targetOffsetAngle_Vertical;
-    double angleToGoalRadians = angleToGoalDegrees * (Math.PI / 180.0);
+        // calculate distance
+        return (goalHeightInches - limelightLensHeightInches) / Math.tan(angleToGoalRadians);
+    }
 
-    // calculate distance
-    return (goalHeightInches - limelightLensHeightInches) / Math.tan(angleToGoalRadians);
-  }
+    public double limelightAngle() {
+        return m_horizontalAngle.getDouble(0);
+    }
 
-  public double limelightAngle() {
-    return m_horizontalAngle.getDouble(0);
-  }
-
-  @Override
-  public void periodic() {
-    SmartDashboard.putNumber("limelight dist (in)", limelightDistance());
-  }
+    @Override
+    public void periodic() {
+        SmartDashboard.putNumber("limelight dist (in)", limelightDistance());
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsytem.java b/src/main/java/frc/robot/subsystems/ShooterSubsytem.java
index 5ca0a47..9e9475b 100644
--- a/src/main/java/frc/robot/subsystems/ShooterSubsytem.java
+++ b/src/main/java/frc/robot/subsystems/ShooterSubsytem.java
@@ -4,6 +4,9 @@
 
 package frc.robot.subsystems;
 
+import com.gos.lib.properties.GosDoubleProperty;
+import com.gos.lib.properties.PidProperty;
+import com.gos.lib.rev.RevPidPropertyBuilder;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import com.revrobotics.RelativeEncoder;
@@ -12,83 +15,76 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 import frc.robot.ShooterLookupTable;
-import frc.robot.TunableNumber;
 
 public class ShooterSubsytem extends SubsystemBase {
-  /** Creates a new Shooter. */
-  private final CANSparkMax m_shooterMotor;
+    private static final double ENCODER_REDUCTION = 0.5;
 
-  private final CANSparkMax m_hoodMotor;
+    /**
+     * Creates a new Shooter.
+     */
+    private final CANSparkMax m_shooterMotor;
 
-  private final ShooterLookupTable m_shooterLookupTable;
-  private final RelativeEncoder m_encoder;
-  private final SparkMaxPIDController m_PIDController;
-  private final TunableNumber m_tunableNumberkP = new TunableNumber("Shooter(kP)", 0);
-  private final TunableNumber m_tunableNumberkD = new TunableNumber("Shooter(kD)", 0);
-  private final TunableNumber m_tunableNumberkFF = new TunableNumber("Shooter(kFF)", 0.00045);
-  private final TunableNumber m_tunableNumberkI = new TunableNumber("Shooter(kI)", 0);
-  private final TunableNumber m_tunableAllowableError =
-      new TunableNumber("Shooter(AllowableError))", 50);
-  private final double m_afterEncoderReduction = 0.5;
-  public static final double FENDER_RPM = 1500;
+    private final CANSparkMax m_hoodMotor;
 
-  public ShooterSubsytem() {
-    m_shooterMotor = new CANSparkMax(Constants.SHOOTER_SPARK, MotorType.kBrushless);
-    m_hoodMotor = new CANSparkMax(Constants.SHOOTER_HOOD_SPARK, MotorType.kBrushless);
-    m_shooterMotor.setSmartCurrentLimit(50);
-    m_hoodMotor.setSmartCurrentLimit(30);
-    m_shooterLookupTable = new ShooterLookupTable();
-    m_encoder = m_shooterMotor.getEncoder();
-    m_PIDController = m_shooterMotor.getPIDController();
-    m_shooterMotor.setIdleMode(CANSparkMax.IdleMode.kCoast);
-    m_shooterMotor.restoreFactoryDefaults();
-    m_shooterMotor.setInverted(true);
-    m_shooterMotor.burnFlash();
-  }
+    private final ShooterLookupTable m_shooterLookupTable;
+    private final RelativeEncoder m_encoder;
+    private final SparkMaxPIDController m_pidController;
+    private final PidProperty m_pidProperty;
+    private final GosDoubleProperty m_tunableAllowableError =
+            new GosDoubleProperty(false, "Shooter(AllowableError))", 50);
+    public static final double FENDER_RPM = 1500;
 
-  @Override
-  public void periodic() {
-    // This method will be called once per scheduler run
-    if (m_tunableNumberkP.hasChanged()
-        || m_tunableNumberkI.hasChanged()
-        || m_tunableNumberkD.hasChanged()
-        || m_tunableNumberkFF.hasChanged()
-        || m_tunableAllowableError.hasChanged()) {
-      configurePID();
+    public ShooterSubsytem() {
+        m_shooterMotor = new CANSparkMax(Constants.SHOOTER_SPARK, MotorType.kBrushless);
+        m_hoodMotor = new CANSparkMax(Constants.SHOOTER_HOOD_SPARK, MotorType.kBrushless);
+        m_shooterMotor.setSmartCurrentLimit(50);
+        m_hoodMotor.setSmartCurrentLimit(30);
+        m_shooterLookupTable = new ShooterLookupTable();
+        m_encoder = m_shooterMotor.getEncoder();
+        m_pidController = m_shooterMotor.getPIDController();
+        m_pidProperty = new RevPidPropertyBuilder("Shooter", false, m_pidController, 0)
+                .addP(0)
+                .addI(0)
+                .addD(0)
+                .addFF(0.00045)
+                .build();
+
+        m_shooterMotor.setIdleMode(CANSparkMax.IdleMode.kCoast);
+        m_shooterMotor.restoreFactoryDefaults();
+        m_shooterMotor.setInverted(true);
+        m_shooterMotor.burnFlash();
     }
-    SmartDashboard.putNumber("shooterRpm", getRPM());
-  }
 
-  @Override
-  public void simulationPeriodic() {
-    // This method will be called once per scheduler run during simulation
-  }
+    @Override
+    public void periodic() {
+        // This method will be called once per scheduler run
+        m_pidProperty.updateIfChanged();
+        SmartDashboard.putNumber("shooterRpm", getRPM());
+    }
 
-  public void setPidRpm(double rpm) {
-    m_PIDController.setReference(rpm, CANSparkMax.ControlType.kVelocity);
-  }
+    @Override
+    public void simulationPeriodic() {
+        // This method will be called once per scheduler run during simulation
+    }
 
-  public void configurePID() {
-    m_PIDController.setP(m_tunableNumberkP.get());
-    m_PIDController.setD(m_tunableNumberkD.get());
-    m_PIDController.setFF(m_tunableNumberkFF.get());
-    m_PIDController.setI(m_tunableNumberkI.get());
-  }
+    public void setPidRpm(double rpm) {
+        m_pidController.setReference(rpm, CANSparkMax.ControlType.kVelocity);
+    }
 
-  public boolean checkAtSpeed(double goal) {
-    double error = Math.abs(goal - getRPM());
-    return m_tunableAllowableError.get() > error;
-  }
+    public boolean checkAtSpeed(double goal) {
+        double error = Math.abs(goal - getRPM());
+        return m_tunableAllowableError.getValue() > error;
+    }
 
-  public double getRPM() {
-    return m_encoder.getVelocity() * m_afterEncoderReduction;
-  }
+    public double getRPM() {
+        return m_encoder.getVelocity() * ENCODER_REDUCTION;
+    }
 
-  public void shootFromDistance(double distance) {
-    setPidRpm(m_shooterLookupTable.getRpmTable(distance));
-  }
+    public void shootFromDistance(double distance) {
+        setPidRpm(m_shooterLookupTable.getRpmTable(distance));
+    }
 
-  public void setShooterSpeed(double speed) {
-    m_shooterMotor.set(speed);
-  }
+    public void setShooterSpeed(double speed) {
+        m_shooterMotor.set(speed);
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/TowerSubsystem.java b/src/main/java/frc/robot/subsystems/TowerSubsystem.java
index 0e7f843..24ada1a 100644
--- a/src/main/java/frc/robot/subsystems/TowerSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/TowerSubsystem.java
@@ -12,44 +12,46 @@
 import frc.robot.Constants;
 
 public class TowerSubsystem extends SubsystemBase {
-  private final CANSparkMax m_towerMotor;
-  private final CANSparkMax m_towerKicker;
-  private final DigitalInput m_beamBreak = new DigitalInput(9);
-
-  /** Creates a new Tower. */
-  public TowerSubsystem() {
-    m_towerMotor = new CANSparkMax(Constants.TOWER_SPARK, MotorType.kBrushless);
-    m_towerKicker = new CANSparkMax(Constants.TOWER_KICKER_SPARK, MotorType.kBrushless);
-    m_towerMotor.setSmartCurrentLimit(30);
-    m_towerKicker.setSmartCurrentLimit(30);
-    m_towerMotor.setIdleMode(CANSparkMax.IdleMode.kBrake);
-    m_towerKicker.setIdleMode(CANSparkMax.IdleMode.kCoast);
-    m_towerMotor.restoreFactoryDefaults();
-    m_towerKicker.restoreFactoryDefaults();
-    m_towerKicker.burnFlash();
-    m_towerMotor.burnFlash();
-  }
-
-  @Override
-  public void periodic() {
-    // This method will be called once per scheduler run
-    SmartDashboard.putBoolean("beam break", m_beamBreak.get());
-  }
-
-  @Override
-  public void simulationPeriodic() {
-    // This method will be called once per scheduler run during simulation
-  }
-
-  public void setTowerSpeed(double speed) {
-    m_towerMotor.set(speed);
-  }
-
-  public boolean getBeamBreak() {
-    return m_beamBreak.get();
-  }
-
-  public void setKickerSpeed(double speed) {
-    m_towerKicker.set(speed);
-  }
+    private final CANSparkMax m_towerMotor;
+    private final CANSparkMax m_towerKicker;
+    private final DigitalInput m_beamBreak = new DigitalInput(9);
+
+    /**
+     * Creates a new Tower.
+     */
+    public TowerSubsystem() {
+        m_towerMotor = new CANSparkMax(Constants.TOWER_SPARK, MotorType.kBrushless);
+        m_towerKicker = new CANSparkMax(Constants.TOWER_KICKER_SPARK, MotorType.kBrushless);
+        m_towerMotor.setSmartCurrentLimit(30);
+        m_towerKicker.setSmartCurrentLimit(30);
+        m_towerMotor.setIdleMode(CANSparkMax.IdleMode.kBrake);
+        m_towerKicker.setIdleMode(CANSparkMax.IdleMode.kCoast);
+        m_towerMotor.restoreFactoryDefaults();
+        m_towerKicker.restoreFactoryDefaults();
+        m_towerKicker.burnFlash();
+        m_towerMotor.burnFlash();
+    }
+
+    @Override
+    public void periodic() {
+        // This method will be called once per scheduler run
+        SmartDashboard.putBoolean("beam break", m_beamBreak.get());
+    }
+
+    @Override
+    public void simulationPeriodic() {
+        // This method will be called once per scheduler run during simulation
+    }
+
+    public void setTowerSpeed(double speed) {
+        m_towerMotor.set(speed);
+    }
+
+    public boolean getBeamBreak() {
+        return m_beamBreak.get();
+    }
+
+    public void setKickerSpeed(double speed) {
+        m_towerKicker.set(speed);
+    }
 }
diff --git a/styleguide/checkstyle.xml b/styleguide/checkstyle.xml
new file mode 100644
index 0000000..b6983ec
--- /dev/null
+++ b/styleguide/checkstyle.xml
@@ -0,0 +1,244 @@
+<?xml version="1.0"?>
+<!DOCTYPE module PUBLIC
+        "-//Checkstyle//DTD Checkstyle Configuration 1.3//EN"
+        "https://checkstyle.org/dtds/configuration_1_3.dtd">
+
+
+<module name="Checker">
+    <property name="charset" value="UTF-8" />
+    <property name="severity" value="error" />
+    <property name="fileExtensions" value="java, properties, xml" />
+    <module name="FileTabCharacter">
+        <property name="eachLine" value="true" />
+    </module>
+    <module name="NewlineAtEndOfFile">
+        <property name="lineSeparator" value="lf_cr_crlf" />
+    </module>
+    <module name="TreeWalker">
+        <module name="SuppressWarningsHolder" />
+        <module name="OuterTypeFilename" />
+        <module name="IllegalTokenText">
+            <property name="tokens" value="STRING_LITERAL, CHAR_LITERAL" />
+            <property name="format"
+                value="\\u00(08|09|0(a|A)|0(c|C)|0(d|D)|22|27|5(C|c))|\\(0(10|11|12|14|15|42|47)|134)" />
+            <property name="message"
+                value="Avoid using corresponding octal or Unicode escape." />
+        </module>
+        <module name="RedundantImport" />
+        <module name="UnusedImports" />
+        <module name="OneTopLevelClass" />
+        <module name="NoLineWrap">
+            <property name="tokens"
+                value="PACKAGE_DEF, IMPORT, STATIC_IMPORT" />
+        </module>
+        <module name="EmptyBlock">
+            <property name="option" value="TEXT" />
+            <property name="tokens"
+                value="LITERAL_TRY, LITERAL_FINALLY, LITERAL_IF, LITERAL_ELSE, LITERAL_SWITCH" />
+        </module>
+        <module name="NeedBraces" />
+        <module name="LeftCurly">
+        </module>
+        <module name="RightCurly">
+            <property name="option" value="alone" />
+            <property name="tokens"
+                value="CLASS_DEF, METHOD_DEF, CTOR_DEF, LITERAL_FOR, LITERAL_WHILE, LITERAL_DO, STATIC_INIT, INSTANCE_INIT" />
+        </module>
+        <module name="WhitespaceAround">
+            <property name="allowEmptyConstructors" value="true" />
+            <property name="allowEmptyLambdas" value="true" />
+            <property name="allowEmptyMethods" value="true" />
+            <property name="allowEmptyTypes" value="true" />
+            <property name="allowEmptyLoops" value="true" />
+            <message key="ws.notFollowed"
+                value="WhitespaceAround: ''{0}'' is not followed by whitespace. Empty blocks may only be represented as '{}' when not part of a multi-block statement (4.1.3)" />
+            <message key="ws.notPreceded"
+                value="WhitespaceAround: ''{0}'' is not preceded with whitespace." />
+        </module>
+        <module name="WhitespaceAfter" />
+        <module name="OneStatementPerLine" />
+        <module name="MultipleVariableDeclarations" />
+        <module name="ArrayTypeStyle" />
+        <module name="MissingSwitchDefault" />
+        <module name="FallThrough" />
+        <module name="SimplifyBooleanExpression" />
+        <module name="SimplifyBooleanReturn" />
+        <module name="StringLiteralEquality" />
+        <module name="UpperEll" />
+        <module name="ModifierOrder" />
+        <module name="EmptyLineSeparator">
+            <property name="tokens"
+                value="PACKAGE_DEF, IMPORT, STATIC_IMPORT, CLASS_DEF, INTERFACE_DEF, ENUM_DEF,
+                    STATIC_INIT, INSTANCE_INIT, METHOD_DEF, CTOR_DEF, VARIABLE_DEF" />
+            <property name="allowNoEmptyLineBetweenFields" value="true" />
+        </module>
+        <module name="SeparatorWrap">
+            <property name="id" value="SeparatorWrapDot" />
+            <property name="tokens" value="DOT" />
+            <property name="option" value="nl" />
+        </module>
+        <module name="SeparatorWrap">
+            <property name="id" value="SeparatorWrapComma" />
+            <property name="tokens" value="COMMA" />
+            <property name="option" value="EOL" />
+        </module>
+        <module name="SeparatorWrap">
+            <!-- ELLIPSIS is EOL until https://github.com/google/styleguide/issues/258 -->
+            <property name="id" value="SeparatorWrapEllipsis" />
+            <property name="tokens" value="ELLIPSIS" />
+            <property name="option" value="EOL" />
+        </module>
+        <module name="SeparatorWrap">
+            <!-- ARRAY_DECLARATOR is EOL until https://github.com/google/styleguide/issues/259 -->
+            <property name="id" value="SeparatorWrapArrayDeclarator" />
+            <property name="tokens" value="ARRAY_DECLARATOR" />
+            <property name="option" value="EOL" />
+        </module>
+        <module name="SeparatorWrap">
+            <property name="id" value="SeparatorWrapMethodRef" />
+            <property name="tokens" value="METHOD_REF" />
+            <property name="option" value="nl" />
+        </module>
+        <module name="PackageName">
+            <property name="format" value="^[a-z]+(\.[a-z][a-z0-9_]*)*$" />
+            <message key="name.invalidPattern"
+                value="Package name ''{0}'' must match pattern ''{1}''." />
+        </module>
+        <module name="TypeName">
+            <property name="tokens"
+                value="CLASS_DEF, INTERFACE_DEF, ENUM_DEF, ANNOTATION_DEF" />
+            <message key="name.invalidPattern"
+                value="Type name ''{0}'' must match pattern ''{1}''." />
+        </module>
+        <module name="MemberName">
+            <property name="format" value="^m_[a-z][a-zA-Z0-9]*$" />
+            <message key="name.invalidPattern"
+                value="Member name ''{0}'' must match pattern ''{1}''." />
+        </module>
+        <module name="ParameterName">
+            <property name="format" value="^[a-z]([a-zA-Z0-9]+)?$" />
+            <message key="name.invalidPattern"
+                value="Parameter name ''{0}'' must match pattern ''{1}''." />
+        </module>
+        <module name="LambdaParameterName">
+            <property name="format"
+                value="^[a-z]([a-z0-9][a-zA-Z0-9]*)?$" />
+            <message key="name.invalidPattern"
+                value="Lambda parameter name ''{0}'' must match pattern ''{1}''." />
+        </module>
+        <module name="CatchParameterName">
+            <property name="format"
+                value="^[a-z]([a-z0-9][a-zA-Z0-9]*)?$" />
+            <message key="name.invalidPattern"
+                value="Catch parameter name ''{0}'' must match pattern ''{1}''." />
+        </module>
+        <module name="LocalVariableName">
+            <property name="format" value="^[a-z]([a-zA-Z0-9]*)?$" />
+            <message key="name.invalidPattern"
+                value="Local variable name ''{0}'' must match pattern ''{1}''." />
+        </module>
+        <module name="ClassTypeParameterName">
+            <property name="format"
+                value="(^[A-Z][0-9]?)$|([A-Z][a-zA-Z0-9]*$)" />
+            <message key="name.invalidPattern"
+                value="Class type name ''{0}'' must match pattern ''{1}''." />
+        </module>
+        <module name="MethodTypeParameterName">
+            <property name="format"
+                value="(^[A-Z][0-9]?)$|([A-Z][a-zA-Z0-9]*$)" />
+            <message key="name.invalidPattern"
+                value="Method type name ''{0}'' must match pattern ''{1}''." />
+        </module>
+        <module name="InterfaceTypeParameterName">
+            <property name="format"
+                value="(^[A-Z][0-9]?)$|([A-Z][a-zA-Z0-9]*$)" />
+            <message key="name.invalidPattern"
+                value="Interface type name ''{0}'' must match pattern ''{1}''." />
+        </module>
+        <module name="NoFinalizer" />
+        <module name="GenericWhitespace">
+            <message key="ws.followed"
+                value="GenericWhitespace ''{0}'' is followed by whitespace." />
+            <message key="ws.preceded"
+                value="GenericWhitespace ''{0}'' is preceded with whitespace." />
+            <message key="ws.illegalFollow"
+                value="GenericWhitespace ''{0}'' should followed by whitespace." />
+            <message key="ws.notPreceded"
+                value="GenericWhitespace ''{0}'' is not preceded with whitespace." />
+        </module>
+        <module name="Indentation">
+            <property name="basicOffset" value="4" />
+            <property name="braceAdjustment" value="0" />
+            <property name="caseIndent" value="0" />
+            <property name="throwsIndent" value="4" />
+            <property name="lineWrappingIndentation" value="4" />
+            <property name="arrayInitIndent" value="4" />
+        </module>
+        <module name="AbbreviationAsWordInName">
+            <property name="ignoreFinal" value="true" />
+            <property name="allowedAbbreviationLength" value="3" />
+            <property name="tokens"
+                value="CLASS_DEF, INTERFACE_DEF, ENUM_DEF, ANNOTATION_DEF, ANNOTATION_FIELD_DEF,
+                    PARAMETER_DEF, VARIABLE_DEF, METHOD_DEF" />
+        </module>
+        <module name="OverloadMethodsDeclarationOrder" />
+        <module name="MethodParamPad">
+            <property name="tokens"
+                value="CTOR_DEF, LITERAL_NEW, METHOD_CALL, METHOD_DEF,
+                    SUPER_CTOR_CALL, ENUM_CONSTANT_DEF" />
+        </module>
+        <module name="NoWhitespaceBefore">
+            <property name="tokens"
+                value="COMMA, SEMI, POST_INC, POST_DEC, DOT, ELLIPSIS, METHOD_REF" />
+            <property name="allowLineBreaks" value="true" />
+        </module>
+        <module name="TypecastParenPad" />
+        <module name="ParenPad">
+            <property name="tokens"
+                value="ANNOTATION, ANNOTATION_FIELD_DEF, CTOR_CALL, CTOR_DEF, DOT, ENUM_CONSTANT_DEF,
+                    EXPR, LITERAL_CATCH, LITERAL_DO, LITERAL_FOR, LITERAL_IF, LITERAL_NEW,
+                    LITERAL_SWITCH, LITERAL_SYNCHRONIZED, LITERAL_WHILE, METHOD_CALL,
+                    METHOD_DEF, QUESTION, RESOURCE_SPECIFICATION, SUPER_CTOR_CALL, LAMBDA" />
+        </module>
+        <module name="OperatorWrap">
+            <property name="option" value="NL" />
+            <property name="tokens"
+                value="BAND, BOR, BSR, BXOR, DIV, EQUAL, GE, GT, LAND, LE, LITERAL_INSTANCEOF, LOR,
+                    LT, MINUS, MOD, NOT_EQUAL, PLUS, QUESTION, SL, SR, STAR, METHOD_REF " />
+        </module>
+        <module name="AnnotationLocation">
+            <property name="id" value="AnnotationLocationMostCases" />
+            <property name="tokens"
+                value="CLASS_DEF, INTERFACE_DEF, ENUM_DEF, METHOD_DEF, CTOR_DEF" />
+        </module>
+        <module name="AnnotationLocation">
+            <property name="id" value="AnnotationLocationVariables" />
+            <property name="tokens" value="VARIABLE_DEF" />
+            <property name="allowSamelineMultipleAnnotations"
+                value="true" />
+        </module>
+        <module name="MissingOverride" />
+        <module name="NonEmptyAtclauseDescription" />
+        <module name="AtclauseOrder">
+            <property name="tagOrder"
+                value="@param, @return, @throws, @deprecated" />
+            <property name="target"
+                value="CLASS_DEF, INTERFACE_DEF, ENUM_DEF, METHOD_DEF, CTOR_DEF, VARIABLE_DEF" />
+        </module>
+        <module name="MethodName">
+            <property name="format" value="^[a-z][a-z0-9][a-zA-Z0-9_]*$" />
+            <message key="name.invalidPattern"
+                value="Method name ''{0}'' must match pattern ''{1}''." />
+        </module>
+        <module name="SingleLineJavadoc">
+            <property name="ignoreInlineTags" value="false" />
+        </module>
+        <module name="EmptyCatchBlock">
+            <property name="exceptionVariableName" value="expected" />
+        </module>
+        <module name="CommentsIndentation">
+            <property name="tokens"
+                value="SINGLE_LINE_COMMENT, BLOCK_COMMENT_BEGIN" />
+        </module>
+    </module>
+</module>
diff --git a/styleguide/pmd-ruleset.xml b/styleguide/pmd-ruleset.xml
new file mode 100644
index 0000000..9ca94c3
--- /dev/null
+++ b/styleguide/pmd-ruleset.xml
@@ -0,0 +1,115 @@
+<?xml version="1.0"?>
+<ruleset name="Ruleset"
+    xmlns="http://pmd.sourceforge.net/ruleset/2.0.0"
+    xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
+    xsi:schemaLocation="http://pmd.sourceforge.net/ruleset/2.0.0 http://pmd.sourceforge.net/ruleset_2_0_0.xsd">
+
+    <description>PMD Ruleset for Girls of Steel</description>
+
+    <rule ref="category/java/bestpractices.xml">
+        <exclude name="GuardLogStatement" />
+        <exclude name="JUnitTestsShouldIncludeAssert" />
+        <exclude name="JUnitAssertionsShouldIncludeMessage" />
+        <exclude name="JUnitTestContainsTooManyAsserts" />
+        <exclude name="JUnit5TestShouldBePackagePrivate" />
+        <exclude name="SystemPrintln" />
+    </rule>
+
+    <rule ref="category/java/codestyle.xml/ClassNamingConventions">
+        <properties>
+            <property name="utilityClassPattern"
+                value="[A-Z][a-zA-Z0-9]*" />
+        </properties>
+    </rule>
+
+    <rule ref="category/java/codestyle.xml/FieldNamingConventions">
+        <properties>
+            <property name="publicConstantPattern"
+                value="[A-Z][A-Z_0-9]*" />
+            <property name="constantPattern" value="[A-Z][A-Z_0-9]*" />
+            <property name="enumConstantPattern" value="[A-Z][A-Z_0-9]*" />
+            <property name="finalFieldPattern"
+                value="m_[a-z][a-zA-Z0-9]*" />
+            <property name="defaultFieldPattern"
+                value="m_[a-z][a-zA-Z0-9]*" />
+        </properties>
+    </rule>
+
+    <rule
+        ref="category/java/codestyle.xml/FormalParameterNamingConventions" />
+    <rule
+        ref="category/java/codestyle.xml/LocalVariableNamingConventions" />
+    <rule ref="category/java/codestyle.xml/MethodNamingConventions" />
+
+    <rule ref="category/java/codestyle.xml">
+        <exclude name="MethodArgumentCouldBeFinal" />
+        <exclude name="LocalVariableCouldBeFinal" />
+        <exclude name="AtLeastOneConstructor" />
+        <exclude name="LongVariable" />
+        <exclude name="ShortVariable" />
+        <exclude name="OnlyOneReturn" />
+        <exclude name="UseUnderscoresInNumericLiterals" />
+        <exclude name="UselessParentheses" />
+        <exclude name="CallSuperInConstructor" />
+        <exclude name="ShortClassName" />
+        <exclude name="ConfusingTernary" />
+        <exclude name="BooleanGetMethodName" />
+    </rule>
+
+    <rule ref="category/java/design.xml">
+        <exclude name="SingularField" />
+        <exclude name="UseUtilityClass" />
+        <exclude name="AvoidThrowingRawExceptionTypes" />
+        <!-- <exclude name="CyclomaticComplexity" /> -->
+        <!-- <exclude name="DataClass" /> -->
+        <!-- <exclude name="ExcessiveMethodLength" /> -->
+        <exclude name="ExcessiveImports" />
+        <!-- <exclude name="ExcessiveParameterList" /> -->
+        <!-- <exclude name="GodClass" /> -->
+        <exclude name="LawOfDemeter" />
+        <exclude name="LoosePackageCoupling" />
+        <!-- <exclude name="NcssConstructorCount" /> -->
+        <!-- <exclude name="NcssCount" /> -->
+        <!-- <exclude name="NcssMethodCount" /> -->
+        <!-- <exclude name="NPathComplexity" /> -->
+        <!-- <exclude name="SignatureDeclareThrowsException" /> -->
+        <!-- <exclude name="TooManyFields" /> -->
+        <!-- <exclude name="TooManyMethods" /> -->
+    </rule>
+
+    <!-- <rule ref="category/java/documentation.xml"> -->
+    <!-- </rule> -->
+
+    <rule ref="category/java/errorprone.xml">
+        <!-- <exclude name="AssignmentToNonFinalStatic" /> -->
+        <exclude name="AssignmentInOperand" />
+        <!-- <exclude name="AvoidCatchingThrowable" /> -->
+        <!-- <exclude name="AvoidDuplicateLiterals" /> -->
+        <exclude name="AvoidLiteralsInIfCondition" />
+        <exclude name="BeanMembersShouldSerialize" />
+        <!-- <exclude name="ConstructorCallsOverridableMethod" /> -->
+        <!-- <exclude name="DataflowAnomalyAnalysis" /> -->
+        <!-- <exclude name="DoNotCallSystemExit" /> -->
+        <!-- <exclude name="FinalizeDoesNotCallSuperFinalize" /> -->
+        <!-- <exclude name="NullAssignment" /> -->
+        <!-- <exclude name="MissingSerialVersionUID" /> -->
+        <!-- <exclude name="CloseResource"/> -->
+    </rule>
+
+    <rule ref="category/java/multithreading.xml">
+        <!-- <exclude name="AvoidSynchronizedAtMethodLevel" /> -->
+        <!-- <exclude name="AvoidUsingVolatile" /> -->
+        <!-- <exclude name="DoNotUseThreads" /> -->
+        <exclude name="UseConcurrentHashMap" />
+    </rule>
+
+    <rule ref="category/java/performance.xml">
+        <exclude name="AvoidInstantiatingObjectsInLoops" />
+        <!-- <exclude name="AvoidUsingShortType" /> -->
+        <!-- <exclude name="AvoidFileStream" /> -->
+    </rule>
+
+    <rule ref="category/java/security.xml">
+    </rule>
+
+</ruleset>
diff --git a/styleguide/spotless.gradle b/styleguide/spotless.gradle
new file mode 100644
index 0000000..7c0d87b
--- /dev/null
+++ b/styleguide/spotless.gradle
@@ -0,0 +1,47 @@
+
+apply plugin: 'com.diffplug.spotless'
+
+
+spotless {
+    java {
+        target fileTree('.') {
+            include '**/*.java'
+            exclude '**/build/**', '**/build-*/**', 'bazel-*/**', '**/venv/**'
+        }
+        toggleOffOn()
+        indentWithSpaces(4)
+        removeUnusedImports()
+        trimTrailingWhitespace()
+        endWithNewline()
+    }
+    groovyGradle {
+        target fileTree('.') {
+            include '**/*.gradle'
+            exclude '**/build/**', '**/build-*/**', 'bazel-*/**', '**/venv/**'
+        }
+        greclipse()
+        indentWithSpaces(4)
+        trimTrailingWhitespace()
+        endWithNewline()
+    }
+    format 'xml', {
+        target fileTree('.') {
+            include '**/*.xml'
+            include '**/*.fxml'
+            exclude '**/build/**', '**/build-*/**', '**/bazel-*/**', '**/.idea/**', '**/venv/**'
+        }
+        eclipseWtp('xml')
+        trimTrailingWhitespace()
+        indentWithSpaces(4)
+        endWithNewline()
+    }
+    format 'misc', {
+        target fileTree('.') {
+            include '**/*.md', '**/.gitignore', "**/*.yml", "**/*.yaml"
+            exclude '**/build/**', '**/build-*/**', 'bazel-*/**', '**/venv/**'
+        }
+        trimTrailingWhitespace()
+        indentWithSpaces(4)
+        endWithNewline()
+    }
+}
diff --git a/styleguide/styleguide.gradle b/styleguide/styleguide.gradle
new file mode 100644
index 0000000..e30b28a
--- /dev/null
+++ b/styleguide/styleguide.gradle
@@ -0,0 +1,18 @@
+
+apply plugin: 'checkstyle'
+apply plugin: 'pmd'
+
+checkstyle {
+    toolVersion = "10.0"
+    configDirectory.set(file("${project.rootDir}/styleguide"))
+}
+
+pmd {
+    toolVersion = '6.43.0'
+    consoleOutput  = true
+    reportsDir = file("${project.buildDir}/reports/pmd")
+    ruleSetFiles = files(file("$rootDir/styleguide/pmd-ruleset.xml"))
+    ruleSets = []
+}
+
+apply from: "${rootDir}/styleguide/spotless.gradle"
diff --git a/vendordeps/GirlsOfSteelLib.json b/vendordeps/GirlsOfSteelLib.json
new file mode 100644
index 0000000..5bef57d
--- /dev/null
+++ b/vendordeps/GirlsOfSteelLib.json
@@ -0,0 +1,29 @@
+{
+    "fileName": "GirlsOfSteelLib.json",
+    "name": "GirlsOfSteelLib",
+    "version": "2023.02.17-00",
+    "uuid": "7e6313c0-ae5c-11ed-8ccb-c1ff8c491d6a",
+    "mavenUrls": [
+        "https://raw.githubusercontent.com/snobotsim/maven_repo/master/release"
+    ],
+    "jsonUrl": "http://raw.githubusercontent.com/snobotsim/maven_repo/master/release/GirlsOfSteelLib.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.gos.lib",
+            "artifactId": "GirlsOfSteelLib",
+            "version": "2023.02.17-00"
+        },
+        {
+            "groupId": "com.gos.lib.ctre",
+            "artifactId": "GirlsOfSteelLibCtre",
+            "version": "2023.02.17-00"
+        },
+        {
+            "groupId": "com.gos.lib.rev",
+            "artifactId": "GirlsOfSteelLibRev",
+            "version": "2023.02.17-00"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": []
+}
\ No newline at end of file

From adcc07364debfed36dbf49810714de15f9fbb3ec Mon Sep 17 00:00:00 2001
From: PJ Reiniger <pj.reiniger@gmail.com>
Date: Sat, 4 Nov 2023 23:23:21 -0400
Subject: [PATCH 3/4] Make it more similar to GOS version

---
 src/main/java/frc/robot/Robot.java            | 28 +------
 src/main/java/frc/robot/RobotContainer.java   | 23 +++++-
 .../frc/robot/commands/AutoAimCommand.java    | 38 +++++----
 .../frc/robot/commands/ClimbAutoCommand.java  |  2 +-
 .../frc/robot/commands/ClimbPIDComand.java    |  8 +-
 .../frc/robot/commands/ClimberCommand.java    |  2 +-
 .../java/frc/robot/commands/DriveCommand.java |  4 +-
 .../frc/robot/commands/HopperCommand.java     |  2 +-
 .../commands/KickIfShootSetRPMCommand.java    |  7 +-
 .../KickIfShooterDistanceGoBrrCommand.java    | 19 +++--
 .../commands/RunClimberToBottomCommand.java   |  2 +-
 .../frc/robot/commands/RunIntakeCommand.java  |  9 +--
 .../commands/SetShooterSpeedCommand.java      |  2 +-
 .../commands/ShootFromDistanceCommand.java    | 10 +--
 .../frc/robot/commands/ShooterPIDCommand.java |  6 +-
 .../frc/robot/commands/TowerDownCommand.java  |  2 +-
 .../robot/commands/TowerKickerCommand.java    |  7 +-
 .../frc/robot/commands/TowerUpCommand.java    | 10 +--
 .../frc/robot/commands/TrajectoryCommand.java |  4 +-
 .../robot/subsystems/ClimberSubsystem.java    | 10 +--
 .../robot/subsystems/DrivetrainSubsystem.java | 81 ++++++++++++-------
 .../frc/robot/subsystems/HopperSubsystem.java |  2 +-
 .../frc/robot/subsystems/ShooterSubsytem.java | 69 +++++++++++-----
 .../frc/robot/subsystems/TowerSubsystem.java  |  2 +-
 vendordeps/SnobotSim.json                     | 22 +++++
 25 files changed, 213 insertions(+), 158 deletions(-)
 create mode 100644 vendordeps/SnobotSim.json

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 1e32f4a..73c1d5c 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -14,7 +14,6 @@
  * the package after creating this project, you must also update the build.gradle file in the
  * project.
  */
-@SuppressWarnings("PMD.TooManyMethods")
 public class Robot extends TimedRobot {
     private Command m_autonomousCommand;
 
@@ -26,7 +25,7 @@ public class Robot extends TimedRobot {
      */
     @Override
     public void robotInit() {
-        // Instantiate our RobotContainer.    This will perform all our button bindings, and put our
+        // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
         // autonomous chooser on the dashboard.
         m_robotContainer = new RobotContainer();
     }
@@ -40,9 +39,9 @@ public void robotInit() {
      */
     @Override
     public void robotPeriodic() {
-        // Runs the Scheduler.    This is responsible for polling buttons, adding newly-scheduled
+        // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
         // commands, running already-scheduled commands, removing finished or interrupted commands,
-        // and running subsystem periodic() methods.    This must be called from the robot's periodic
+        // and running subsystem periodic() methods.  This must be called from the robot's periodic
         // block in order for anything in the Command-based framework to work.
         CommandScheduler.getInstance().run();
     }
@@ -101,25 +100,4 @@ public void testInit() {
         // Cancels all running commands at the start of test mode.
         CommandScheduler.getInstance().cancelAll();
     }
-
-    /**
-     * This function is called periodically during test mode.
-     */
-    @Override
-    public void testPeriodic() {
-    }
-
-    /**
-     * This function is called once when the robot is first started up.
-     */
-    @Override
-    public void simulationInit() {
-    }
-
-    /**
-     * This function is called periodically whilst in simulation.
-     */
-    @Override
-    public void simulationPeriodic() {
-    }
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 15bdee9..5fc2236 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,15 +4,30 @@
 
 package frc.robot;
 
+import frc.robot.commands.ClimberCommand;
+import frc.robot.commands.DriveCommand;
+import frc.robot.commands.HopperCommand;
+import frc.robot.commands.KickIfShootSetRPMCommand;
+import frc.robot.commands.RunIntakeCommand;
+import frc.robot.commands.SetShooterSpeedCommand;
+import frc.robot.commands.StopIntakeCommand;
+import frc.robot.commands.ShootFromDistanceCommand;
+import frc.robot.commands.TowerDownCommand;
+import frc.robot.commands.TowerUpCommand;
+import frc.robot.subsystems.ClimberSubsystem;
+import frc.robot.subsystems.DrivetrainSubsystem;
+import frc.robot.subsystems.HopperSubsystem;
+import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.LimelightSubsystem;
+import frc.robot.subsystems.ShooterSubsytem;
+import frc.robot.subsystems.TowerSubsystem;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.autos.AutonomousFactory;
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-import frc.robot.autos.AutonomousFactory;
-import frc.robot.commands.*;
-import frc.robot.subsystems.*;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
diff --git a/src/main/java/frc/robot/commands/AutoAimCommand.java b/src/main/java/frc/robot/commands/AutoAimCommand.java
index 2e151a9..9a36166 100644
--- a/src/main/java/frc/robot/commands/AutoAimCommand.java
+++ b/src/main/java/frc/robot/commands/AutoAimCommand.java
@@ -6,14 +6,13 @@
 
 import com.gos.lib.properties.PidProperty;
 import com.gos.lib.properties.WpiProfiledPidPropertyBuilder;
-
+import frc.robot.Constants;
+import frc.robot.subsystems.DrivetrainSubsystem;
+import frc.robot.subsystems.LimelightSubsystem;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc.robot.Constants;
-import frc.robot.subsystems.DrivetrainSubsystem;
-import frc.robot.subsystems.LimelightSubsystem;
 
 /**
  * An example command that uses an example subsystem.
@@ -22,7 +21,7 @@ public class AutoAimCommand extends CommandBase {
     private final DrivetrainSubsystem m_driveTrainSubsystem;
     private final LimelightSubsystem m_limelightSubsystem;
     private final ProfiledPIDController m_controller;
-    private final PidProperty m_pidProperty;
+    private final PidProperty m_pidProperties;
 
     /**
      * Creates a new AutoAimCommand.
@@ -30,23 +29,21 @@ public class AutoAimCommand extends CommandBase {
      * @param subsystem The subsystem used by this command.
      */
     public AutoAimCommand(
-            DrivetrainSubsystem driveTrainSubsystem, LimelightSubsystem limelightSubsystem) {
+        DrivetrainSubsystem driveTrainSubsystem, LimelightSubsystem limelightSubsystem) {
         m_driveTrainSubsystem = driveTrainSubsystem;
         m_limelightSubsystem = limelightSubsystem;
         m_controller =
-                new ProfiledPIDController(
-                        0,
-                        0,
-                        0,
-                        new TrapezoidProfile.Constraints(
-                                Constants.MAX_TURN_VELOCITY, Constants.MAX_TURN_ACCELERATION));
-        m_pidProperty = new WpiProfiledPidPropertyBuilder("AutoAim", false, m_controller)
-                .addP(0)
-                .addI(0)
-                .addD(0)
-                .addMaxVelocity(Constants.MAX_TURN_VELOCITY)
-                .addMaxAcceleration(Constants.MAX_TURN_ACCELERATION)
-                .build();
+            new ProfiledPIDController(
+                0,
+                0,
+                0,
+                new TrapezoidProfile.Constraints(
+                    Constants.MAX_TURN_VELOCITY, Constants.MAX_TURN_ACCELERATION));
+        m_pidProperties = new WpiProfiledPidPropertyBuilder("AutoAim", false, m_controller)
+            .addP(0)
+            .addI(0)
+            .addD(0)
+            .build();
         m_controller.setTolerance(5);
         // Use addRequirements() here to declare subsystem dependencies.
         addRequirements(driveTrainSubsystem);
@@ -61,7 +58,8 @@ public void initialize() {
     // Called every time the scheduler runs while the command is scheduled.
     @Override
     public void execute() {
-        m_pidProperty.updateIfChanged();
+        m_pidProperties.updateIfChanged();
+
         double turnSpeed = m_controller.calculate(m_limelightSubsystem.limelightAngle());
         ChassisSpeeds chassisSpeed = new ChassisSpeeds(0, 0, turnSpeed);
         m_driveTrainSubsystem.applyChassisSpeed(chassisSpeed);
diff --git a/src/main/java/frc/robot/commands/ClimbAutoCommand.java b/src/main/java/frc/robot/commands/ClimbAutoCommand.java
index f29c15e..4d3eecb 100644
--- a/src/main/java/frc/robot/commands/ClimbAutoCommand.java
+++ b/src/main/java/frc/robot/commands/ClimbAutoCommand.java
@@ -1,7 +1,7 @@
 package frc.robot.commands;
 
-import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.ClimberSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 
 /**
  * An example command that uses an example subsystem.
diff --git a/src/main/java/frc/robot/commands/ClimbPIDComand.java b/src/main/java/frc/robot/commands/ClimbPIDComand.java
index 7eae37e..1113322 100644
--- a/src/main/java/frc/robot/commands/ClimbPIDComand.java
+++ b/src/main/java/frc/robot/commands/ClimbPIDComand.java
@@ -1,7 +1,7 @@
 package frc.robot.commands;
 
-import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.ClimberSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 
 public class ClimbPIDComand extends CommandBase {
     private final ClimberSubsystem m_climberSubsystem;
@@ -24,13 +24,13 @@ public void execute() {
 
     @Override
     public boolean isFinished() {
-        return Math.abs(m_climberSubsystem.getPIDPosition()) < 100
-                || (m_climberSubsystem.leftLimitSwitchPress()
-                || m_climberSubsystem.rightLimitSwitchPress());
+        return Math.abs(m_climberSubsystem.getPIDPosition()) < 100 || (m_climberSubsystem.leftLimitSwitchPress() || m_climberSubsystem.rightLimitSwitchPress());
     }
 
     @Override
     public void end(boolean interrupted) {
         m_climberSubsystem.set(0);
     }
+
+
 }
diff --git a/src/main/java/frc/robot/commands/ClimberCommand.java b/src/main/java/frc/robot/commands/ClimberCommand.java
index b4b5343..9f457fe 100644
--- a/src/main/java/frc/robot/commands/ClimberCommand.java
+++ b/src/main/java/frc/robot/commands/ClimberCommand.java
@@ -4,8 +4,8 @@
 
 package frc.robot.commands;
 
-import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.ClimberSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 
 /**
  * An example command that uses an example subsystem.
diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/DriveCommand.java
index 5530ed3..71a02cb 100644
--- a/src/main/java/frc/robot/commands/DriveCommand.java
+++ b/src/main/java/frc/robot/commands/DriveCommand.java
@@ -4,9 +4,9 @@
 
 package frc.robot.commands;
 
+import frc.robot.subsystems.DrivetrainSubsystem;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc.robot.subsystems.DrivetrainSubsystem;
 
 /**
  * An example command that uses an example subsystem.
@@ -37,7 +37,7 @@ public void initialize() {
     // Called every time the scheduler runs while the command is scheduled.
     @Override
     public void execute() {
-        m_subsystem.control(-0.5 * m_joystick.getLeftY(), 0.5 * m_joystick.getRightX());
+        m_subsystem.control(-m_joystick.getLeftY(), m_joystick.getRightX());
     }
 
     // Called once the command ends or is interrupted.
diff --git a/src/main/java/frc/robot/commands/HopperCommand.java b/src/main/java/frc/robot/commands/HopperCommand.java
index c0e50fc..42dbb8d 100644
--- a/src/main/java/frc/robot/commands/HopperCommand.java
+++ b/src/main/java/frc/robot/commands/HopperCommand.java
@@ -4,8 +4,8 @@
 
 package frc.robot.commands;
 
-import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.HopperSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 
 /**
  * An example command that uses an example subsystem.
diff --git a/src/main/java/frc/robot/commands/KickIfShootSetRPMCommand.java b/src/main/java/frc/robot/commands/KickIfShootSetRPMCommand.java
index dbdce8b..2d7d572 100644
--- a/src/main/java/frc/robot/commands/KickIfShootSetRPMCommand.java
+++ b/src/main/java/frc/robot/commands/KickIfShootSetRPMCommand.java
@@ -1,9 +1,9 @@
 package frc.robot.commands;
 
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.ShooterSubsytem;
 import frc.robot.subsystems.TowerSubsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 
 /**
  * An example command that uses an example subsystem.
@@ -43,8 +43,7 @@ public void execute() {
             m_towerSubsystem.setKickerSpeed(1);
             m_towerSubsystem.setTowerSpeed(0.75);
         }
-        SmartDashboard.putBoolean(
-                "kickIfShootRPM atSpeed", m_shooterSubsystem.checkAtSpeed(m_rpm));
+        SmartDashboard.putBoolean("kickIfShootRPM atSpeed", m_shooterSubsystem.checkAtSpeed(m_rpm));
     }
 
     // Called once the command ends or is interrupted.
diff --git a/src/main/java/frc/robot/commands/KickIfShooterDistanceGoBrrCommand.java b/src/main/java/frc/robot/commands/KickIfShooterDistanceGoBrrCommand.java
index c4ac381..a1ef175 100644
--- a/src/main/java/frc/robot/commands/KickIfShooterDistanceGoBrrCommand.java
+++ b/src/main/java/frc/robot/commands/KickIfShooterDistanceGoBrrCommand.java
@@ -1,11 +1,11 @@
 package frc.robot.commands;
 
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.ShooterLookupTable;
 import frc.robot.subsystems.LimelightSubsystem;
 import frc.robot.subsystems.ShooterSubsytem;
 import frc.robot.subsystems.TowerSubsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 
 /**
  * An example command that uses an example subsystem.
@@ -24,10 +24,10 @@ public class KickIfShooterDistanceGoBrrCommand extends CommandBase {
      * @param subsystem The subsystem used by this command.
      */
     public KickIfShooterDistanceGoBrrCommand(
-            ShooterSubsytem subsystem,
-            TowerSubsystem towerSubsystem,
-            LimelightSubsystem limelightSubsystem,
-            ShooterLookupTable shooterLookupTable) {
+        ShooterSubsytem subsystem,
+        TowerSubsystem towerSubsystem,
+        LimelightSubsystem limelightSubsystem,
+        ShooterLookupTable shooterLookupTable) {
         m_shooterSubsystem = subsystem;
         m_towerSubsystem = towerSubsystem;
         m_limelightSubsystem = limelightSubsystem;
@@ -52,9 +52,8 @@ public void execute() {
             m_towerSubsystem.setKickerSpeed(1);
             m_towerSubsystem.setTowerSpeed(0.75);
         }
-        SmartDashboard.putBoolean(
-                "kickIfShootDist atSpeed",
-                m_shooterSubsystem.checkAtSpeed(m_shooterLookupTable.getRpmTable(distance)));
+        SmartDashboard.putBoolean("kickIfShootDist atSpeed", m_shooterSubsystem.checkAtSpeed(m_shooterLookupTable.getRpmTable(distance)));
+
     }
 
     // Called once the command ends or is interrupted.
@@ -68,6 +67,6 @@ public void end(boolean interrupted) {
     @Override
     public boolean isFinished() {
         return m_shooterSubsystem.checkAtSpeed(
-                m_shooterLookupTable.getRpmTable(m_limelightSubsystem.limelightDistance()));
+            m_shooterLookupTable.getRpmTable(m_limelightSubsystem.limelightDistance()));
     }
 }
diff --git a/src/main/java/frc/robot/commands/RunClimberToBottomCommand.java b/src/main/java/frc/robot/commands/RunClimberToBottomCommand.java
index d7a68c2..f523641 100644
--- a/src/main/java/frc/robot/commands/RunClimberToBottomCommand.java
+++ b/src/main/java/frc/robot/commands/RunClimberToBottomCommand.java
@@ -4,8 +4,8 @@
 
 package frc.robot.commands;
 
-import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.ClimberSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 
 /**
  * An example command that uses an example subsystem.
diff --git a/src/main/java/frc/robot/commands/RunIntakeCommand.java b/src/main/java/frc/robot/commands/RunIntakeCommand.java
index 59efdde..bfa99fd 100644
--- a/src/main/java/frc/robot/commands/RunIntakeCommand.java
+++ b/src/main/java/frc/robot/commands/RunIntakeCommand.java
@@ -4,9 +4,9 @@
 
 package frc.robot.commands;
 
-import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.HopperSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.TowerSubsystem;
 
 /**
@@ -27,12 +27,7 @@ public class RunIntakeCommand extends CommandBase {
      *
      * @param subsystem The subsystem used by this command.
      */
-    public RunIntakeCommand(
-            IntakeSubsystem intakeSubsystem,
-            double intakeSpeed,
-            HopperSubsystem hopperSubsystem,
-            TowerSubsystem towerSubsystem,
-            double towerSpeed) {
+    public RunIntakeCommand(IntakeSubsystem intakeSubsystem, double intakeSpeed, HopperSubsystem hopperSubsystem, TowerSubsystem towerSubsystem, double towerSpeed) {
         m_intakeSubsystem = intakeSubsystem;
         m_hopperSubsystem = hopperSubsystem;
         m_towerSubsystem = towerSubsystem;
diff --git a/src/main/java/frc/robot/commands/SetShooterSpeedCommand.java b/src/main/java/frc/robot/commands/SetShooterSpeedCommand.java
index 703e80d..e515b29 100644
--- a/src/main/java/frc/robot/commands/SetShooterSpeedCommand.java
+++ b/src/main/java/frc/robot/commands/SetShooterSpeedCommand.java
@@ -4,8 +4,8 @@
 
 package frc.robot.commands;
 
-import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.ShooterSubsytem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 
 /**
  * An example command that uses an example subsystem.
diff --git a/src/main/java/frc/robot/commands/ShootFromDistanceCommand.java b/src/main/java/frc/robot/commands/ShootFromDistanceCommand.java
index 8f299a0..b9e3469 100644
--- a/src/main/java/frc/robot/commands/ShootFromDistanceCommand.java
+++ b/src/main/java/frc/robot/commands/ShootFromDistanceCommand.java
@@ -1,9 +1,9 @@
 package frc.robot.commands;
 
-import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.ShooterLookupTable;
 import frc.robot.subsystems.LimelightSubsystem;
 import frc.robot.subsystems.ShooterSubsytem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 
 /**
  * An example command that uses an example subsystem.
@@ -21,9 +21,9 @@ public class ShootFromDistanceCommand extends CommandBase {
      * @param subsystem The subsystem used by this command.
      */
     public ShootFromDistanceCommand(
-            ShooterSubsytem subsystem,
-            LimelightSubsystem limelightsubsystem,
-            ShooterLookupTable shooterLookupTable) {
+        ShooterSubsytem subsystem,
+        LimelightSubsystem limelightsubsystem,
+        ShooterLookupTable shooterLookupTable) {
         m_shooterSubsystem = subsystem;
         m_limelightSubsystem = limelightsubsystem;
         m_shooterLookupTable = shooterLookupTable;
@@ -52,6 +52,6 @@ public void end(boolean interrupted) {
     @Override
     public boolean isFinished() {
         return m_shooterSubsystem.checkAtSpeed(
-                m_shooterLookupTable.getRpmTable(m_limelightSubsystem.limelightDistance()));
+            m_shooterLookupTable.getRpmTable(m_limelightSubsystem.limelightDistance()));
     }
 }
diff --git a/src/main/java/frc/robot/commands/ShooterPIDCommand.java b/src/main/java/frc/robot/commands/ShooterPIDCommand.java
index cb40eca..4454bbc 100644
--- a/src/main/java/frc/robot/commands/ShooterPIDCommand.java
+++ b/src/main/java/frc/robot/commands/ShooterPIDCommand.java
@@ -5,15 +5,13 @@
 package frc.robot.commands;
 
 import com.gos.lib.properties.GosDoubleProperty;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.ShooterSubsytem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 
 /**
  * An example command that uses an example subsystem.
  */
 public class ShooterPIDCommand extends CommandBase {
-    @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
     private final ShooterSubsytem m_shooterSubsystem;
 
     private final GosDoubleProperty m_tunableShooterGoal = new GosDoubleProperty(false, "Shooter Goal", 1000);
@@ -21,7 +19,7 @@ public class ShooterPIDCommand extends CommandBase {
     /**
      * Creates a new ExampleCommand.
      *
-     * @param subsystem The subsystem used by this command.
+     * @param shooterSubsystem The subsystem used by this command.
      */
     public ShooterPIDCommand(ShooterSubsytem shooterSubsystem) {
         m_shooterSubsystem = shooterSubsystem;
diff --git a/src/main/java/frc/robot/commands/TowerDownCommand.java b/src/main/java/frc/robot/commands/TowerDownCommand.java
index 7edb36f..5e19b08 100644
--- a/src/main/java/frc/robot/commands/TowerDownCommand.java
+++ b/src/main/java/frc/robot/commands/TowerDownCommand.java
@@ -4,8 +4,8 @@
 
 package frc.robot.commands;
 
-import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.TowerSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 
 /**
  * An example command that uses an example subsystem.
diff --git a/src/main/java/frc/robot/commands/TowerKickerCommand.java b/src/main/java/frc/robot/commands/TowerKickerCommand.java
index 1b168bc..a4b7c96 100644
--- a/src/main/java/frc/robot/commands/TowerKickerCommand.java
+++ b/src/main/java/frc/robot/commands/TowerKickerCommand.java
@@ -4,22 +4,23 @@
 
 package frc.robot.commands;
 
-import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.TowerSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 
 /**
  * An example command that uses an example subsystem.
  */
 public class TowerKickerCommand extends CommandBase {
+    private static final double SPEED = 0.75;
+
     @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
     private final TowerSubsystem m_towerSubsystem;
 
-    private static final double SPEED = 0.75;
 
     /**
      * Creates a new TowerDownCommand.
      *
-     * @param subsystem The subsystem used by this command.
+     * @param towerSubsystem The subsystem used by this command.
      */
     public TowerKickerCommand(TowerSubsystem towerSubsystem) {
         m_towerSubsystem = towerSubsystem;
diff --git a/src/main/java/frc/robot/commands/TowerUpCommand.java b/src/main/java/frc/robot/commands/TowerUpCommand.java
index ab3eff2..62f6aea 100644
--- a/src/main/java/frc/robot/commands/TowerUpCommand.java
+++ b/src/main/java/frc/robot/commands/TowerUpCommand.java
@@ -4,22 +4,21 @@
 
 package frc.robot.commands;
 
-import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.TowerSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 
 /**
  * An example command that uses an example subsystem.
  */
 public class TowerUpCommand extends CommandBase {
-    @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-    private final TowerSubsystem m_towerSubsystem;
-
     private static final double SPEED = 0.5;
 
+    private final TowerSubsystem m_towerSubsystem;
+
     /**
      * Creates a new TowerUpCommand.
      *
-     * @param subsystem The subsystem used by this command.
+     * @param towerSubsystem The subsystem used by this command.
      */
     public TowerUpCommand(TowerSubsystem towerSubsystem) {
         m_towerSubsystem = towerSubsystem;
@@ -39,6 +38,7 @@ public void execute() {
         m_towerSubsystem.setKickerSpeed(-0.5);
     }
 
+
     // Called once the command ends or is interrupted.
     @Override
     public void end(boolean interrupted) {
diff --git a/src/main/java/frc/robot/commands/TrajectoryCommand.java b/src/main/java/frc/robot/commands/TrajectoryCommand.java
index 4a1e442..cc224e9 100644
--- a/src/main/java/frc/robot/commands/TrajectoryCommand.java
+++ b/src/main/java/frc/robot/commands/TrajectoryCommand.java
@@ -4,6 +4,7 @@
 
 package frc.robot.commands;
 
+import frc.robot.subsystems.DrivetrainSubsystem;
 import edu.wpi.first.math.controller.RamseteController;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.trajectory.Trajectory;
@@ -12,7 +13,6 @@
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc.robot.subsystems.DrivetrainSubsystem;
 
 import java.io.IOException;
 import java.nio.file.Path;
@@ -38,7 +38,7 @@ public TrajectoryCommand(DrivetrainSubsystem subsystem, String trajectoryPathNam
             m_trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
         } catch (IOException ex) {
             DriverStation.reportError(
-                    "Unable to open trajectory: " + trajectoryPathName, ex.getStackTrace());
+                "Unable to open trajectory: " + trajectoryPathName, ex.getStackTrace());
         }
         m_timer = new Timer();
         m_controller = new RamseteController();
diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
index ae44b1c..52cb135 100644
--- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
@@ -7,23 +7,23 @@
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import com.revrobotics.RelativeEncoder;
+import frc.robot.Constants;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.Constants;
 
 public class ClimberSubsystem extends SubsystemBase {
     private final CANSparkMax m_leftClimber = new CANSparkMax(Constants.CLIMBER_LEFT, MotorType.kBrushless);
     private final CANSparkMax m_rightClimber =
-            new CANSparkMax(Constants.CLIMBER_RIGHT, MotorType.kBrushless);
+        new CANSparkMax(Constants.CLIMBER_RIGHT, MotorType.kBrushless);
     private final DigitalInput m_leftLimitSwitch = new DigitalInput(Constants.LEFT_LIMIT_SWITCH);
     private final DigitalInput m_rightLimitSwitch = new DigitalInput(Constants.RIGHT_LIMIT_SWITCH);
     private final RelativeEncoder m_leftEncoder = m_leftClimber.getEncoder();
     private final RelativeEncoder m_rightEncoder = m_rightClimber.getEncoder();
-    private final ProfiledPIDController m_climberPID =
-            new ProfiledPIDController(3.5, 0.0, 0.0, new Constraints(20.0, 150.0), 0.02);
+    private final ProfiledPIDController m_climberPID = new ProfiledPIDController(
+        3.5, 0.0, 0.0, new Constraints(20.0, 150.0), 0.02);
 
     /**
      * Creates a new ClimberSubsystem.
@@ -39,7 +39,6 @@ public ClimberSubsystem() {
         m_rightClimber.follow(m_leftClimber, true);
         m_leftClimber.setSmartCurrentLimit(50);
         m_rightClimber.setSmartCurrentLimit(50);
-
     }
 
     public void runClimberPID(double goal) {
@@ -47,6 +46,7 @@ public void runClimberPID(double goal) {
         set(m_climberPID.calculate(m_leftEncoder.getPosition()));
     }
 
+
     @SuppressWarnings("PMD.AvoidReassigningParameters")
     public void set(double speed) {
         if (speed < 0 && (leftLimitSwitchPress() || rightLimitSwitchPress())) {
diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java
index 8ed9c64..168c10f 100644
--- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java
@@ -8,12 +8,16 @@
 import com.gos.lib.rev.RevPidPropertyBuilder;
 import com.kauailabs.navx.frc.AHRS;
 import com.pathplanner.lib.auto.RamseteAutoBuilder;
-import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMax.ControlType;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SimableCANSparkMax;
 import com.revrobotics.SparkMaxPIDController;
 import edu.wpi.first.math.controller.RamseteController;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
@@ -24,40 +28,45 @@
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.Constants;
+import org.snobotv2.module_wrappers.navx.NavxWrapper;
+import org.snobotv2.module_wrappers.rev.RevEncoderSimWrapper;
+import org.snobotv2.module_wrappers.rev.RevMotorControllerSimWrapper;
+import org.snobotv2.sim_wrappers.DifferentialDrivetrainSimWrapper;
 
 import java.util.Map;
 import java.util.function.Consumer;
 
 // Drive train
-@SuppressWarnings("PMD.TooManyMethods")
+@SuppressWarnings({"PMD.TooManyMethods", "PMD.TooManyFields"})
 public class DrivetrainSubsystem extends SubsystemBase {
-    private final CANSparkMax m_leftLeader =
-            new CANSparkMax(Constants.DRIVE_LEFT_LEADER, MotorType.kBrushless);
-    private final CANSparkMax m_leftFollower =
-            new CANSparkMax(Constants.DRIVE_LEFT_FOLLOWER, MotorType.kBrushless);
-    private final CANSparkMax m_rightLeader =
-            new CANSparkMax(Constants.DRIVE_RIGHT_LEADER, MotorType.kBrushless);
-    private final CANSparkMax m_rightFollower =
-            new CANSparkMax(Constants.DRIVE_RIGHT_FOLLOWER, MotorType.kBrushless);
+    private final SimableCANSparkMax m_leftLeader =
+        new SimableCANSparkMax(Constants.DRIVE_LEFT_LEADER, MotorType.kBrushless);
+    private final SimableCANSparkMax m_leftFollower =
+        new SimableCANSparkMax(Constants.DRIVE_LEFT_FOLLOWER, MotorType.kBrushless);
+    private final SimableCANSparkMax m_rightLeader =
+        new SimableCANSparkMax(Constants.DRIVE_RIGHT_LEADER, MotorType.kBrushless);
+    private final SimableCANSparkMax m_rightFollower =
+        new SimableCANSparkMax(Constants.DRIVE_RIGHT_FOLLOWER, MotorType.kBrushless);
     private final DifferentialDrive m_drive = new DifferentialDrive(m_leftLeader, m_rightLeader);
     private final DifferentialDriveKinematics m_kinematics =
-            new DifferentialDriveKinematics(Constants.DRIVE_TRACK);
+        new DifferentialDriveKinematics(Constants.DRIVE_TRACK);
 
     // odometry
     private final RelativeEncoder m_leftEncoder = m_leftLeader.getEncoder();
     private final RelativeEncoder m_rightEncoder = m_rightLeader.getEncoder();
     private final AHRS m_gyro = new AHRS(SPI.Port.kMXP);
-    private final DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(0), 0, 0);
+    private final DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(new Rotation2d(), 0, 0);
     private final Field2d m_field = new Field2d();
 
     // PID
     private final SparkMaxPIDController m_leftController = m_leftLeader.getPIDController();
+    private final PidProperty m_leftProperties;
     private final SparkMaxPIDController m_rightController = m_rightLeader.getPIDController();
-    private final PidProperty m_leftPidProperty;
-    private final PidProperty m_rightPidProperty;
+    private final PidProperty m_rightProperties;
+
+    // Simulation
+    private DifferentialDrivetrainSimWrapper m_simulator;
 
     /**
      * Creates a new DrivetrainSubsystem.
@@ -65,6 +74,9 @@ public class DrivetrainSubsystem extends SubsystemBase {
     public DrivetrainSubsystem() {
         m_gyro.calibrate();
 
+        m_leftProperties = setupVelocityPidValues(m_leftController);
+        m_rightProperties = setupVelocityPidValues(m_rightController);
+
         m_leftLeader.restoreFactoryDefaults();
         m_leftFollower.restoreFactoryDefaults();
         m_rightLeader.restoreFactoryDefaults();
@@ -92,17 +104,30 @@ public DrivetrainSubsystem() {
 
         SmartDashboard.putData(m_field);
 
-        m_leftPidProperty = createDrivetrainProperty(m_leftController);
-        m_rightPidProperty = createDrivetrainProperty(m_rightController);
 
+        if (RobotBase.isSimulation()) {
+            DifferentialDrivetrainSim drivetrainSim = DifferentialDrivetrainSim.createKitbotSim(
+                DifferentialDrivetrainSim.KitbotMotor.kDoubleNEOPerSide,
+                DifferentialDrivetrainSim.KitbotGearing.k5p95,
+                DifferentialDrivetrainSim.KitbotWheelSize.kSixInch,
+                null);
+            m_simulator = new DifferentialDrivetrainSimWrapper(
+                drivetrainSim,
+                new RevMotorControllerSimWrapper(m_leftLeader),
+                new RevMotorControllerSimWrapper(m_rightLeader),
+                RevEncoderSimWrapper.create(m_leftLeader),
+                RevEncoderSimWrapper.create(m_rightLeader),
+                new NavxWrapper().getYawGyro());
+            m_simulator.setRightInverted(false);
+        }
     }
 
-    private PidProperty createDrivetrainProperty(SparkMaxPIDController pidController) {
-        return new RevPidPropertyBuilder("Drivetrain", false, pidController, 0)
-                .addP(1)
-                .addI(0)
-                .addD(0)
-                .build();
+    private PidProperty setupVelocityPidValues(SparkMaxPIDController pidController) {
+        return new RevPidPropertyBuilder("ChassisVelocity", false, pidController, 0)
+            .addP(1)
+            .addI(0)
+            .addD(0)
+            .build();
     }
 
     public void control(double speed, double rotation) {
@@ -155,20 +180,20 @@ public RamseteAutoBuilder ramseteAutoBuilderNoPoseReset(Map<String, Command> eve
 
     @Override
     public void periodic() {
-        m_leftPidProperty.updateIfChanged();
-        m_rightPidProperty.updateIfChanged();
 
         // This method will be called once per scheduler run
-        Rotation2d gyroAngle = Rotation2d.fromDegrees(-m_gyro.getAngle());
+        var gyroAngle = Rotation2d.fromDegrees(-m_gyro.getAngle());
         m_odometry.update(gyroAngle, m_leftEncoder.getPosition(), m_rightEncoder.getPosition());
         m_field.setRobotPose(m_odometry.getPoseMeters());
         SmartDashboard.putNumber("Chassis Left Velocity", leftVelocity());
         SmartDashboard.putNumber("Chassis Right Velocity", rightVelocity());
 
+        m_leftProperties.updateIfChanged();
+        m_rightProperties.updateIfChanged();
     }
 
     @Override
     public void simulationPeriodic() {
-        // This method will be called once per scheduler run during simulation
+        m_simulator.update();
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/HopperSubsystem.java b/src/main/java/frc/robot/subsystems/HopperSubsystem.java
index 19445be..7b920de 100644
--- a/src/main/java/frc/robot/subsystems/HopperSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/HopperSubsystem.java
@@ -2,8 +2,8 @@
 
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class HopperSubsystem extends SubsystemBase {
     private final CANSparkMax m_hopperMotor;
diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsytem.java b/src/main/java/frc/robot/subsystems/ShooterSubsytem.java
index 9e9475b..0636924 100644
--- a/src/main/java/frc/robot/subsystems/ShooterSubsytem.java
+++ b/src/main/java/frc/robot/subsystems/ShooterSubsytem.java
@@ -10,61 +10,86 @@
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SimableCANSparkMax;
 import com.revrobotics.SparkMaxPIDController;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.simulation.FlywheelSim;
 import frc.robot.Constants;
 import frc.robot.ShooterLookupTable;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.snobotv2.module_wrappers.rev.RevEncoderSimWrapper;
+import org.snobotv2.module_wrappers.rev.RevMotorControllerSimWrapper;
+import org.snobotv2.sim_wrappers.FlywheelSimWrapper;
+import org.snobotv2.sim_wrappers.ISimWrapper;
+import org.snobotv2.sim_wrappers.InstantaneousMotorSim;
 
 public class ShooterSubsytem extends SubsystemBase {
-    private static final double ENCODER_REDUCTION = 0.5;
+    private static final double AFTER_ENCODER_REDUCTION = 0.5;
+
+    public static final double FENDER_RPM = 1500;
 
     /**
      * Creates a new Shooter.
      */
-    private final CANSparkMax m_shooterMotor;
+    private final SimableCANSparkMax m_shooterMotor;
 
-    private final CANSparkMax m_hoodMotor;
+    private final SimableCANSparkMax m_hoodMotor;
 
     private final ShooterLookupTable m_shooterLookupTable;
     private final RelativeEncoder m_encoder;
     private final SparkMaxPIDController m_pidController;
-    private final PidProperty m_pidProperty;
-    private final GosDoubleProperty m_tunableAllowableError =
-            new GosDoubleProperty(false, "Shooter(AllowableError))", 50);
-    public static final double FENDER_RPM = 1500;
+    private final PidProperty m_pidProperties;
+    private final GosDoubleProperty m_tunableAllowableError = new GosDoubleProperty(false, "Shooter(AllowableError))", 50);
+
+    // Simulation
+    private ISimWrapper m_shooterSimulator;
+    private ISimWrapper m_hoodSimulator;
 
     public ShooterSubsytem() {
-        m_shooterMotor = new CANSparkMax(Constants.SHOOTER_SPARK, MotorType.kBrushless);
-        m_hoodMotor = new CANSparkMax(Constants.SHOOTER_HOOD_SPARK, MotorType.kBrushless);
+        m_shooterMotor = new SimableCANSparkMax(Constants.SHOOTER_SPARK, MotorType.kBrushless);
+        m_hoodMotor = new SimableCANSparkMax(Constants.SHOOTER_HOOD_SPARK, MotorType.kBrushless);
         m_shooterMotor.setSmartCurrentLimit(50);
         m_hoodMotor.setSmartCurrentLimit(30);
         m_shooterLookupTable = new ShooterLookupTable();
         m_encoder = m_shooterMotor.getEncoder();
         m_pidController = m_shooterMotor.getPIDController();
-        m_pidProperty = new RevPidPropertyBuilder("Shooter", false, m_pidController, 0)
-                .addP(0)
-                .addI(0)
-                .addD(0)
-                .addFF(0.00045)
-                .build();
-
+        m_pidProperties = new RevPidPropertyBuilder("Shooter", false, m_pidController, 0)
+            .addP(0)
+            .addI(0)
+            .addD(0)
+            .addFF(0.00045)
+            .build();
         m_shooterMotor.setIdleMode(CANSparkMax.IdleMode.kCoast);
         m_shooterMotor.restoreFactoryDefaults();
         m_shooterMotor.setInverted(true);
         m_shooterMotor.burnFlash();
+
+        if (RobotBase.isSimulation()) {
+            FlywheelSim shooterFlywheelSim = new FlywheelSim(DCMotor.getNeo550(2), 1, 0.01);
+            m_shooterSimulator = new FlywheelSimWrapper(shooterFlywheelSim,
+                new RevMotorControllerSimWrapper(m_shooterMotor),
+                RevEncoderSimWrapper.create(m_shooterMotor));
+
+            m_hoodSimulator = new InstantaneousMotorSim(
+                new RevMotorControllerSimWrapper(m_hoodMotor),
+                RevEncoderSimWrapper.create(m_hoodMotor),
+                1
+            );
+        }
     }
 
     @Override
     public void periodic() {
-        // This method will be called once per scheduler run
-        m_pidProperty.updateIfChanged();
+        m_pidProperties.updateIfChanged();
         SmartDashboard.putNumber("shooterRpm", getRPM());
     }
 
     @Override
     public void simulationPeriodic() {
-        // This method will be called once per scheduler run during simulation
+        m_hoodSimulator.update();
+        m_shooterSimulator.update();
     }
 
     public void setPidRpm(double rpm) {
@@ -77,7 +102,7 @@ public boolean checkAtSpeed(double goal) {
     }
 
     public double getRPM() {
-        return m_encoder.getVelocity() * ENCODER_REDUCTION;
+        return m_encoder.getVelocity() * AFTER_ENCODER_REDUCTION;
     }
 
     public void shootFromDistance(double distance) {
diff --git a/src/main/java/frc/robot/subsystems/TowerSubsystem.java b/src/main/java/frc/robot/subsystems/TowerSubsystem.java
index 24ada1a..e7dbce0 100644
--- a/src/main/java/frc/robot/subsystems/TowerSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/TowerSubsystem.java
@@ -8,8 +8,8 @@
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class TowerSubsystem extends SubsystemBase {
     private final CANSparkMax m_towerMotor;
diff --git a/vendordeps/SnobotSim.json b/vendordeps/SnobotSim.json
new file mode 100644
index 0000000..da79e1b
--- /dev/null
+++ b/vendordeps/SnobotSim.json
@@ -0,0 +1,22 @@
+{
+  "fileName": "SnobotSim.json",
+  "name": "SnobotSim",
+  "version": "2023.10.28-18",
+  "uuid": "88e2fcb0-75bd-11ee-b0bf-396770175b4a",
+  "mavenUrls": ["https://raw.githubusercontent.com/snobotsim/maven_repo/master/release"],
+  "jsonUrl": "http://raw.githubusercontent.com/snobotsim/maven_repo/master/release/SnobotSim.json",
+  "javaDependencies": [
+    {
+      "groupId": "org.snobotv2",
+      "artifactId": "snobot_sim_java",
+      "version": "2023.10.28-18"
+    },
+    {
+      "groupId": "org.snobotv2",
+      "artifactId": "snobot_swerve_sim",
+      "version": "2023.10.28-18"
+    }
+  ],
+  "jniDependencies": [],
+  "cppDependencies": []
+}

From f9cce259bc59214e1397a03c805ac568e37c8adc Mon Sep 17 00:00:00 2001
From: PJ Reiniger <pj.reiniger@gmail.com>
Date: Sat, 4 Nov 2023 23:25:04 -0400
Subject: [PATCH 4/4] preferences

---
 networktables.json | 75 +++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 74 insertions(+), 1 deletion(-)

diff --git a/networktables.json b/networktables.json
index fe51488..4ebff17 100644
--- a/networktables.json
+++ b/networktables.json
@@ -1 +1,74 @@
-[]
+[
+  {
+    "name": "/Preferences/ChassisVelocity.mm.kp",
+    "type": "double",
+    "value": 1.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/ChassisVelocity.mm.ki",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/ChassisVelocity.mm.kd",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/Shooter(AllowableError))",
+    "type": "double",
+    "value": 50.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/Shooter.mm.kp",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/Shooter.mm.ki",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/Shooter.mm.kd",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/Shooter.mm.kff",
+    "type": "double",
+    "value": 0.00045,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/Shooter Goal",
+    "type": "double",
+    "value": 1000.0,
+    "properties": {
+      "persistent": true
+    }
+  }
+]