From e235e0327a4c8b1b2be006c6080e1000250747ba Mon Sep 17 00:00:00 2001
From: James Ding <jamesding365@gmail.com>
Date: Tue, 5 Mar 2024 18:20:38 -0800
Subject: [PATCH] Sync

---
 .pathplanner/settings.json                    |   20 +-
 .run/Debug Robot via IP.run.xml               |    4 +
 .run/Debug Robot via USB.run.xml              |    4 +
 simgui.json                                   |   33 +-
 .../deploy/pathplanner/autos/1m Test.auto     |   25 +
 .../deploy/pathplanner/autos/2m Test.auto     |   25 +
 .../{Example Auto.auto => Circle Test.auto}   |    4 +-
 src/main/deploy/pathplanner/navgrid.json      | 1634 +----------------
 .../paths/{Example Path.path => 1m Test.path} |   41 +-
 .../deploy/pathplanner/paths/2m Test.path     |   52 +
 .../{Amp Placement Path.path => Amp.path}     |    0
 .../deploy/pathplanner/paths/Circle Test.path |  106 ++
 .../deploy/pathplanner/paths/Playground.path  |   52 +
 .../pathplanner/paths/Source Direct Left.path |   52 +
 .../paths/Source Direct Middle.path           |   52 +
 .../paths/Source Direct Right.path            |   52 +
 src/main/java/org/rambots/AutoConstants.kt    |   11 +
 src/main/java/org/rambots/BuildConstants.java |   10 +-
 src/main/java/org/rambots/RobotContainer.kt   |   19 +-
 .../java/org/rambots/commands/AmpScoring.kt   |   23 +
 .../rambots/commands/PathFinderAndFollow.java |    7 +-
 .../org/rambots/subsystems/drive/Drive.java   |    6 +-
 .../subsystems/drive/DriveConstants.java      |   22 +-
 .../subsystems/drive/ModuleIOTalonFX.java     |   19 +-
 .../vision/AprilTagVisionIOLimelight.java     |   59 +-
 25 files changed, 559 insertions(+), 1773 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/autos/1m Test.auto
 create mode 100644 src/main/deploy/pathplanner/autos/2m Test.auto
 rename src/main/deploy/pathplanner/autos/{Example Auto.auto => Circle Test.auto} (83%)
 rename src/main/deploy/pathplanner/paths/{Example Path.path => 1m Test.path} (52%)
 create mode 100644 src/main/deploy/pathplanner/paths/2m Test.path
 rename src/main/deploy/pathplanner/paths/{Amp Placement Path.path => Amp.path} (100%)
 create mode 100644 src/main/deploy/pathplanner/paths/Circle Test.path
 create mode 100644 src/main/deploy/pathplanner/paths/Playground.path
 create mode 100644 src/main/deploy/pathplanner/paths/Source Direct Left.path
 create mode 100644 src/main/deploy/pathplanner/paths/Source Direct Middle.path
 create mode 100644 src/main/deploy/pathplanner/paths/Source Direct Right.path
 create mode 100644 src/main/java/org/rambots/AutoConstants.kt
 create mode 100644 src/main/java/org/rambots/commands/AmpScoring.kt

diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json
index 5c6a51b..ba0ba64 100644
--- a/.pathplanner/settings.json
+++ b/.pathplanner/settings.json
@@ -1,12 +1,18 @@
 {
-  "robotWidth": 0.9,
-  "robotLength": 0.9,
+  "robotWidth": 0.83,
+  "robotLength": 0.8255,
   "holonomicMode": true,
-  "pathFolders": [],
-  "autoFolders": [],
-  "defaultMaxVel": 4.331208,
-  "defaultMaxAccel": 3.0,
+  "pathFolders": [
+    "Autonomous",
+    "Source On-The-Fly",
+    "Testing"
+  ],
+  "autoFolders": [
+    "Troll"
+  ],
+  "defaultMaxVel": 4.33,
+  "defaultMaxAccel": 8.0,
   "defaultMaxAngVel": 540.0,
   "defaultMaxAngAccel": 720.0,
-  "maxModuleSpeed": 4.5
+  "maxModuleSpeed": 4.331208
 }
\ No newline at end of file
diff --git a/.run/Debug Robot via IP.run.xml b/.run/Debug Robot via IP.run.xml
index bf5a423..e5daf11 100644
--- a/.run/Debug Robot via IP.run.xml	
+++ b/.run/Debug Robot via IP.run.xml	
@@ -7,6 +7,10 @@
     <option name="HOST" value="10.22.4.2" />
     <option name="PORT" value="8349" />
     <option name="AUTO_RESTART" value="false" />
+    <RunnerSettings RunnerId="Debug">
+      <option name="DEBUG_PORT" value="8349" />
+      <option name="LOCAL" value="false" />
+    </RunnerSettings>
     <method v="2" />
   </configuration>
 </component>
\ No newline at end of file
diff --git a/.run/Debug Robot via USB.run.xml b/.run/Debug Robot via USB.run.xml
index c5b25da..37afad5 100644
--- a/.run/Debug Robot via USB.run.xml	
+++ b/.run/Debug Robot via USB.run.xml	
@@ -7,6 +7,10 @@
     <option name="HOST" value="172.22.11.2" />
     <option name="PORT" value="8349" />
     <option name="AUTO_RESTART" value="false" />
+    <RunnerSettings RunnerId="Debug">
+      <option name="DEBUG_PORT" value="8349" />
+      <option name="LOCAL" value="false" />
+    </RunnerSettings>
     <method v="2" />
   </configuration>
 </component>
\ No newline at end of file
diff --git a/simgui.json b/simgui.json
index 402cac5..3762b75 100644
--- a/simgui.json
+++ b/simgui.json
@@ -1,16 +1,4 @@
 {
-  "HALProvider": {
-    "PowerDistributions": {
-      "window": {
-        "visible": true
-      }
-    },
-    "RoboRIO": {
-      "window": {
-        "visible": true
-      }
-    }
-  },
   "NTProvider": {
     "types": {
       "/FMSInfo": "FMSInfo",
@@ -22,20 +10,21 @@
         "window": {
           "visible": true
         }
-      }
-    }
-  },
-  "NetworkTables": {
-    "transitory": {
-      "FMSInfo": {
-        "open": true
+      },
+      "/SmartDashboard/VisionSystemSim-main/Sim Field": {
+        "bottom": 1476,
+        "height": 8.210550308227539,
+        "left": 150,
+        "right": 2961,
+        "top": 79,
+        "width": 16.541748046875,
+        "window": {
+          "visible": true
+        }
       }
     }
   },
   "NetworkTables Info": {
-    "Server": {
-      "open": true
-    },
     "visible": true
   }
 }
diff --git a/src/main/deploy/pathplanner/autos/1m Test.auto b/src/main/deploy/pathplanner/autos/1m Test.auto
new file mode 100644
index 0000000..0e6c597
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/1m Test.auto	
@@ -0,0 +1,25 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 2.0,
+      "y": 6.0
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "1m Test"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/2m Test.auto b/src/main/deploy/pathplanner/autos/2m Test.auto
new file mode 100644
index 0000000..4864a84
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/2m Test.auto	
@@ -0,0 +1,25 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 2.0,
+      "y": 6.0
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "2m Test"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Example Auto.auto b/src/main/deploy/pathplanner/autos/Circle Test.auto
similarity index 83%
rename from src/main/deploy/pathplanner/autos/Example Auto.auto
rename to src/main/deploy/pathplanner/autos/Circle Test.auto
index efecee1..eb71777 100644
--- a/src/main/deploy/pathplanner/autos/Example Auto.auto	
+++ b/src/main/deploy/pathplanner/autos/Circle Test.auto	
@@ -5,7 +5,7 @@
       "x": 2.0,
       "y": 7.0
     },
-    "rotation": 180.0
+    "rotation": 0
   },
   "command": {
     "type": "sequential",
@@ -14,7 +14,7 @@
         {
           "type": "path",
           "data": {
-            "pathName": "Example Path"
+            "pathName": "Circle Test"
           }
         }
       ]
diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
index 690f5db..e74ee28 100644
--- a/src/main/deploy/pathplanner/navgrid.json
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -1,1633 +1 @@
-{
-  "field_size": {
-    "x": 16.54,
-    "y": 8.21
-  },
-  "nodeSizeMeters": 0.3,
-  "grid": [
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      false,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true
-    ],
-    [
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true,
-      true
-    ]
-  ]
-}
+{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false]]}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/1m Test.path
similarity index 52%
rename from src/main/deploy/pathplanner/paths/Example Path.path
rename to src/main/deploy/pathplanner/paths/1m Test.path
index 77a1d45..61c69f0 100644
--- a/src/main/deploy/pathplanner/paths/Example Path.path	
+++ b/src/main/deploy/pathplanner/paths/1m Test.path	
@@ -4,40 +4,24 @@
     {
       "anchor": {
         "x": 2.0,
-        "y": 7.0
+        "y": 6.0
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.013282910175676,
-        "y": 6.5274984191074985
+        "x": 3.0000000000000004,
+        "y": 6.0
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.166769560390973,
-        "y": 5.0964860911203305
+        "x": 3.0,
+        "y": 6.0
       },
       "prevControl": {
-        "x": 4.166769560390973,
-        "y": 6.0964860911203305
-      },
-      "nextControl": {
-        "x": 6.166769560390973,
-        "y": 4.0964860911203305
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 7.0,
-        "y": 1.0
-      },
-      "prevControl": {
-        "x": 6.75,
-        "y": 2.5
+        "x": 2.5,
+        "y": 6.0
       },
       "nextControl": null,
       "isLocked": false,
@@ -48,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 4.331208,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 4.33,
+    "maxAcceleration": 8.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0
   },
@@ -59,7 +43,10 @@
     "rotateFast": false
   },
   "reversed": false,
-  "folder": null,
-  "previewStartingState": null,
+  "folder": "Testing",
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
   "useDefaultConstraints": true
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/2m Test.path b/src/main/deploy/pathplanner/paths/2m Test.path
new file mode 100644
index 0000000..1bf07e9
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/2m Test.path	
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 6.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0,
+        "y": 6.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 6.0
+      },
+      "prevControl": {
+        "x": 3.0,
+        "y": 6.0
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.33,
+    "maxAcceleration": 8.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "Testing",
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Amp Placement Path.path b/src/main/deploy/pathplanner/paths/Amp.path
similarity index 100%
rename from src/main/deploy/pathplanner/paths/Amp Placement Path.path
rename to src/main/deploy/pathplanner/paths/Amp.path
diff --git a/src/main/deploy/pathplanner/paths/Circle Test.path b/src/main/deploy/pathplanner/paths/Circle Test.path
new file mode 100644
index 0000000..5fb237d
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Circle Test.path	
@@ -0,0 +1,106 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 7.0
+      },
+      "prevControl": {
+        "x": 3.4393407139914594,
+        "y": 7.828046595918608
+      },
+      "nextControl": {
+        "x": 4.642893006716247,
+        "y": 6.050501117102414
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 5.0
+      },
+      "prevControl": {
+        "x": 4.963984020881579,
+        "y": 5.839753235663662
+      },
+      "nextControl": {
+        "x": 2.822268917265107,
+        "y": 3.9740457662733513
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 5.0
+      },
+      "prevControl": {
+        "x": 2.6309545284564635,
+        "y": 4.082619685811823
+      },
+      "nextControl": {
+        "x": 1.5235685111119892,
+        "y": 5.692710566694004
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": {
+        "x": 1.858822007894463,
+        "y": 6.36421043101478
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.95,
+      "rotationDegrees": 180.0,
+      "rotateFast": true
+    }
+  ],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.33,
+    "maxAcceleration": 8.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "Testing",
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Playground.path b/src/main/deploy/pathplanner/paths/Playground.path
new file mode 100644
index 0000000..3787a86
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Playground.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.82,
+        "y": 7.7
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.820000000000001,
+        "y": 7.7
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 6.0
+      },
+      "prevControl": {
+        "x": 3.0,
+        "y": 6.0
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.33,
+    "maxAcceleration": 8.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": -59.9781887324857,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Source Direct Left.path b/src/main/deploy/pathplanner/paths/Source Direct Left.path
new file mode 100644
index 0000000..a22cfe9
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Source Direct Left.path	
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 15.332117209688965,
+        "y": 2.8732537465074937
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 15.57466460387466,
+        "y": 2.91056873022837
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 15.91982820329277,
+        "y": 1.2127369709284874
+      },
+      "prevControl": {
+        "x": 15.667952063176854,
+        "y": 1.3433394139515555
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.33,
+    "maxAcceleration": 8.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -59.886266849017446,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "Source On-The-Fly",
+  "previewStartingState": {
+    "rotation": -60.124007308310574,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Source Direct Middle.path b/src/main/deploy/pathplanner/paths/Source Direct Middle.path
new file mode 100644
index 0000000..d6a50d9
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Source Direct Middle.path	
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 14.632461264922528,
+        "y": 2.0056803749971146
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 14.93098113468954,
+        "y": 1.5858868081372544
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 15.444062160851594,
+        "y": 0.9422033389521326
+      },
+      "prevControl": {
+        "x": 15.229501004456553,
+        "y": 1.2593807005795834
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.33,
+    "maxAcceleration": 8.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 120.0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "Source On-The-Fly",
+  "previewStartingState": {
+    "rotation": 120.0,
+    "velocity": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Source Direct Right.path b/src/main/deploy/pathplanner/paths/Source Direct Right.path
new file mode 100644
index 0000000..c938362
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Source Direct Right.path	
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 14.026092779458287,
+        "y": 1.315353176160898
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 14.268640173643982,
+        "y": 1.3526681598817742
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 14.930981134689542,
+        "y": 0.6250259773246829
+      },
+      "prevControl": {
+        "x": 14.632461264922531,
+        "y": 1.1381070034867355
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.33,
+    "maxAcceleration": 8.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 120.0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "Source On-The-Fly",
+  "previewStartingState": {
+    "rotation": 120.0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/java/org/rambots/AutoConstants.kt b/src/main/java/org/rambots/AutoConstants.kt
new file mode 100644
index 0000000..d7e394f
--- /dev/null
+++ b/src/main/java/org/rambots/AutoConstants.kt
@@ -0,0 +1,11 @@
+package org.rambots
+
+import edu.wpi.first.math.geometry.Pose2d
+import edu.wpi.first.math.geometry.Rotation2d
+import edu.wpi.first.math.geometry.Translation2d
+
+object AutoConstants {
+
+    val ampScorePose = Pose2d(Translation2d(1.82, 7.7), Rotation2d.fromDegrees(-90.0))
+
+}
\ No newline at end of file
diff --git a/src/main/java/org/rambots/BuildConstants.java b/src/main/java/org/rambots/BuildConstants.java
index 8f83f66..e9ee355 100644
--- a/src/main/java/org/rambots/BuildConstants.java
+++ b/src/main/java/org/rambots/BuildConstants.java
@@ -7,12 +7,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "frc-2024";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 5;
-  public static final String GIT_SHA = "5e8a73aaa4496c06ad1a4e91d3936537282b250a";
-  public static final String GIT_DATE = "2024-03-02 21:55:19 PST";
+  public static final int GIT_REVISION = 8;
+  public static final String GIT_SHA = "cd46b92a3c9db0369f3c1f26dd9d6a937b0f47bd";
+  public static final String GIT_DATE = "2024-03-05 09:15:24 PST";
   public static final String GIT_BRANCH = "james/swerve";
-  public static final String BUILD_DATE = "2024-03-02 23:37:18 PST";
-  public static final long BUILD_UNIX_TIME = 1709451438465L;
+  public static final String BUILD_DATE = "2024-03-05 16:08:22 PST";
+  public static final long BUILD_UNIX_TIME = 1709683702503L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}
diff --git a/src/main/java/org/rambots/RobotContainer.kt b/src/main/java/org/rambots/RobotContainer.kt
index 8f8fb8a..4f14b62 100644
--- a/src/main/java/org/rambots/RobotContainer.kt
+++ b/src/main/java/org/rambots/RobotContainer.kt
@@ -43,7 +43,7 @@ object RobotContainer {
     private var drive: Drive
 //    private var flywheel: Flywheel
     private var aprilTagVision: AprilTagVision
-    private val driveMode = DriveController()
+    private val driveController = DriveController()
 
     // Controller
     private val controller = CommandXboxController(0)
@@ -93,7 +93,7 @@ object RobotContainer {
                         AprilTagVisionIOPhotonVisionSIM(
                             "photonCamera1",
                             Transform3d(Translation3d(0.5, 0.0, 0.5), Rotation3d(0.0, 0.0, 0.0))
-                        ) { drive.getDrive() })
+                        ) { drive.drive })
             }
 
             else -> {
@@ -152,8 +152,8 @@ object RobotContainer {
                 visionData
             )
         })
-        driveMode.setPoseSupplier { drive.pose }
-        driveMode.disableHeadingControl()
+        driveController.setPoseSupplier { drive.pose }
+        driveController.disableHeadingControl()
         configureButtonBindings()
     }
 
@@ -164,19 +164,20 @@ object RobotContainer {
     private fun configureButtonBindings() {
         drive.defaultCommand = DriveCommands.joystickDrive(
             drive,
-            driveMode,
+            driveController,
             { -controller.leftY },
             { -controller.leftX },
             { -controller.rightX })
-        controller.leftBumper().whileTrue(Commands.runOnce({ driveMode.toggleDriveMode() }))
 
-        controller.a().whileTrue(PathFinderAndFollow(driveMode.driveModeType))
+        controller.leftBumper().whileTrue(Commands.runOnce({ driveController.toggleDriveMode() }))
+
+        controller.a().whileTrue(PathFinderAndFollow(driveController.driveModeType))
 
         controller
             .b()
             .whileTrue(
                 Commands.startEnd(
-                    { driveMode.enableHeadingControl() }, { driveMode.disableHeadingControl() })
+                    { driveController.enableHeadingControl() }, { driveController.disableHeadingControl() })
             )
 
         controller
@@ -184,7 +185,7 @@ object RobotContainer {
             .whileTrue(
                 Commands.runOnce(
                     {
-                        drive!!.setAutoStartPose(
+                        drive.setAutoStartPose(
                             Pose2d(Translation2d(4.0, 5.0), Rotation2d.fromDegrees(0.0))
                         )
                     })
diff --git a/src/main/java/org/rambots/commands/AmpScoring.kt b/src/main/java/org/rambots/commands/AmpScoring.kt
new file mode 100644
index 0000000..e55019d
--- /dev/null
+++ b/src/main/java/org/rambots/commands/AmpScoring.kt
@@ -0,0 +1,23 @@
+package org.rambots.commands
+
+import edu.wpi.first.wpilibj2.command.Command
+
+class AmpScoring: Command() {
+
+    override fun initialize() {
+        super.initialize()
+    }
+
+    override fun execute() {
+        super.execute()
+    }
+
+    override fun end(interrupted: Boolean) {
+        super.end(interrupted)
+    }
+
+    override fun isFinished(): Boolean {
+        return super.isFinished()
+    }
+
+}
\ No newline at end of file
diff --git a/src/main/java/org/rambots/commands/PathFinderAndFollow.java b/src/main/java/org/rambots/commands/PathFinderAndFollow.java
index 931b6df..99c0518 100644
--- a/src/main/java/org/rambots/commands/PathFinderAndFollow.java
+++ b/src/main/java/org/rambots/commands/PathFinderAndFollow.java
@@ -63,14 +63,13 @@ public boolean isFinished() {
     public void runNewAutonPath() {
         driveMode = driveModeSupplier.get();
         String pathName =
-                driveMode == DriveController.DriveModeType.SPEAKER
-                        ? "Speaker Placement Path"
-                        : "Amp Placement Path";
+                driveMode == DriveController.DriveModeType.SPEAKER ? "Speaker Placement Path" : "Source Direct Left";
         PathPlannerPath ampPath = PathPlannerPath.fromPathFile(pathName);
         PathConstraints constraints =
-                new PathConstraints(3.0, 4.0, Units.degreesToRadians(540), Units.degreesToRadians(720));
+                new PathConstraints(4.33, 8.0, Units.degreesToRadians(540), Units.degreesToRadians(720));
         pathRun = AutoBuilder.pathfindThenFollowPath(ampPath, constraints, 0.0);
         scoreCommand = Commands.sequence(pathRun);
         scoreCommand.schedule();
     }
+
 }
diff --git a/src/main/java/org/rambots/subsystems/drive/Drive.java b/src/main/java/org/rambots/subsystems/drive/Drive.java
index a544f1f..6d0692f 100644
--- a/src/main/java/org/rambots/subsystems/drive/Drive.java
+++ b/src/main/java/org/rambots/subsystems/drive/Drive.java
@@ -38,12 +38,10 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
-import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 import org.rambots.util.VisionHelpers;
 
 import java.util.List;
-import java.util.Queue;
 import java.util.concurrent.locks.Lock;
 import java.util.concurrent.locks.ReentrantLock;
 
@@ -107,9 +105,9 @@ public Drive(
                 this::runVelocity,
                 new HolonomicPathFollowerConfig(
                         new PIDConstants(
-                                PPtranslationConstants.kP, PPtranslationConstants.kI, PPtranslationConstants.kD),
+                                PPTranslationConstants.kP, PPTranslationConstants.kI, PPTranslationConstants.kD),
                         new PIDConstants(
-                                PProtationConstants.kP, PProtationConstants.kI, PProtationConstants.kD),
+                                PPRotationConstants.kP, PPRotationConstants.kI, PPRotationConstants.kD),
                         drivetrainConfig.maxLinearVelocity(),
                         drivetrainConfig.driveBaseRadius(),
                         new ReplanningConfig()),
diff --git a/src/main/java/org/rambots/subsystems/drive/DriveConstants.java b/src/main/java/org/rambots/subsystems/drive/DriveConstants.java
index 7c354cb..c865cef 100644
--- a/src/main/java/org/rambots/subsystems/drive/DriveConstants.java
+++ b/src/main/java/org/rambots/subsystems/drive/DriveConstants.java
@@ -56,12 +56,12 @@ public final class DriveConstants {
                         SwerveXReductions.OldGen736.reduction,
                         SwerveXReductions.TURN.reduction);
             };
-    public static final PIDConstants PPtranslationConstants =
+    public static final PIDConstants PPTranslationConstants =
             switch (Constants.getRobot()) {
                 case COMPBOT -> new PIDConstants(10, 0.0, 0.0);
                 case SIMBOT -> new PIDConstants(10, 0.0, 0.0);
             };
-    public static final PIDConstants PProtationConstants =
+    public static final PIDConstants PPRotationConstants =
             switch (Constants.getRobot()) {
                 case COMPBOT -> new PIDConstants(10, 0.0, 0.0);
                 case SIMBOT -> new PIDConstants(10, 0.0, 0.0);
@@ -77,17 +77,13 @@ public final class DriveConstants {
                         12.93,
                         29.89);
             };
-    public static final Translation2d[] moduleTranslations =
-            new Translation2d[]{
-                    new Translation2d(
-                            drivetrainConfig.trackwidthX() / 2.0, drivetrainConfig.trackwidthY() / 2.0),
-                    new Translation2d(
-                            drivetrainConfig.trackwidthX() / 2.0, -drivetrainConfig.trackwidthY() / 2.0),
-                    new Translation2d(
-                            -drivetrainConfig.trackwidthX() / 2.0, drivetrainConfig.trackwidthY() / 2.0),
-                    new Translation2d(
-                            -drivetrainConfig.trackwidthX() / 2.0, -drivetrainConfig.trackwidthY() / 2.0)
-            };
+    public static final Translation2d[] moduleTranslations = new Translation2d[]{
+            new Translation2d(drivetrainConfig.trackwidthX() / 2.0, drivetrainConfig.trackwidthY() / 2.0),
+            new Translation2d(drivetrainConfig.trackwidthX() / 2.0, -drivetrainConfig.trackwidthY() / 2.0),
+            new Translation2d(-drivetrainConfig.trackwidthX() / 2.0, drivetrainConfig.trackwidthY() / 2.0),
+            new Translation2d(-drivetrainConfig.trackwidthX() / 2.0, -drivetrainConfig.trackwidthY() / 2.0)
+    };
+
     public static final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(moduleTranslations);
     public static ModuleConfig[] moduleConfigs =
             switch (Constants.getRobot()) {
diff --git a/src/main/java/org/rambots/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/org/rambots/subsystems/drive/ModuleIOTalonFX.java
index 457bef1..df4a7b5 100644
--- a/src/main/java/org/rambots/subsystems/drive/ModuleIOTalonFX.java
+++ b/src/main/java/org/rambots/subsystems/drive/ModuleIOTalonFX.java
@@ -88,31 +88,20 @@ public ModuleIOTalonFX(DriveConstants.ModuleConfig config) {
         timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
 
         drivePosition = driveTalon.getPosition();
-        drivePositionQueue =
-                PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition());
+        drivePositionQueue = PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition());
         driveVelocity = driveTalon.getVelocity();
         driveAppliedVolts = driveTalon.getMotorVoltage();
         driveCurrent = driveTalon.getStatorCurrent();
 
         turnAbsolutePosition = cancoder.getAbsolutePosition();
         turnPosition = turnTalon.getPosition();
-        turnPositionQueue =
-                PhoenixOdometryThread.getInstance().registerSignal(turnTalon, turnTalon.getPosition());
+        turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(turnTalon, turnTalon.getPosition());
         turnVelocity = turnTalon.getVelocity();
         turnAppliedVolts = turnTalon.getMotorVoltage();
         turnCurrent = turnTalon.getStatorCurrent();
 
-        BaseStatusSignal.setUpdateFrequencyForAll(
-                DriveConstants.odometryFrequency, drivePosition, turnPosition);
-        BaseStatusSignal.setUpdateFrequencyForAll(
-                50.0,
-                driveVelocity,
-                driveAppliedVolts,
-                driveCurrent,
-                turnAbsolutePosition,
-                turnVelocity,
-                turnAppliedVolts,
-                turnCurrent);
+        BaseStatusSignal.setUpdateFrequencyForAll(DriveConstants.odometryFrequency, drivePosition, turnPosition);
+        BaseStatusSignal.setUpdateFrequencyForAll(50.0, driveVelocity, driveAppliedVolts, driveCurrent, turnAbsolutePosition, turnVelocity, turnAppliedVolts, turnCurrent);
         driveTalon.optimizeBusUtilization();
         turnTalon.optimizeBusUtilization();
     }
diff --git a/src/main/java/org/rambots/subsystems/vision/AprilTagVisionIOLimelight.java b/src/main/java/org/rambots/subsystems/vision/AprilTagVisionIOLimelight.java
index 8389631..1242876 100644
--- a/src/main/java/org/rambots/subsystems/vision/AprilTagVisionIOLimelight.java
+++ b/src/main/java/org/rambots/subsystems/vision/AprilTagVisionIOLimelight.java
@@ -31,13 +31,12 @@ public class AprilTagVisionIOLimelight implements AprilTagVisionIO {
      * @param identifier The identifier of the Limelight camera.
      */
     public AprilTagVisionIOLimelight(String identifier) {
+        limelightName = identifier;
+
         NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable(identifier);
         LimelightHelpers.setPipelineIndex(limelightName, 0);
 
-        observationSubscriber =
-                limelightTable
-                        .getStringTopic("json")
-                        .subscribe("", PubSubOption.keepDuplicates(true), PubSubOption.sendAll(true));
+        observationSubscriber = limelightTable.getStringTopic("json").subscribe("", PubSubOption.keepDuplicates(true), PubSubOption.sendAll(true));
     }
 
     /**
@@ -47,42 +46,38 @@ public AprilTagVisionIOLimelight(String identifier) {
      */
     @Override
     public void updateInputs(AprilTagVisionIOInputs inputs) {
-        TimestampedString[] queue =
-                observationSubscriber.readQueue(); // Reads the queue of timestamped strings
-        ArrayList<VisionHelpers.PoseEstimate> poseEstimates =
-                new ArrayList<>(); // Creates an empty ArrayList to store pose estimates
+        TimestampedString[] queue = observationSubscriber.readQueue(); // Reads the queue of timestamped strings
+        ArrayList<VisionHelpers.PoseEstimate> poseEstimates = new ArrayList<>(); // Creates an empty ArrayList to store pose estimates
 
         // Iterates over each timestamped string in the queue
         for (int i = 0; i < Math.min(queue.length, 3); i++) {
             TimestampedString timestampedString = queue[i];
-            double timestamp = timestampedString.timestamp / 1e6; // Converts the timestamp to seconds
-            LimelightHelpers.Results results =
-                    LimelightHelpers.parseJsonDump(timestampedString.value)
-                            .targetingResults; // Parses the JSON dump and retrieves the targeting results
-            Optional<Alliance> allianceOptional =
-                    DriverStation.getAlliance(); // Retrieves the alliance information from the DriverStation
+
+            // Converts the timestamp to seconds
+            double timestamp = timestampedString.timestamp / 1e6;
+
+            // Parses the JSON dump and retrieves the targeting results
+            LimelightHelpers.Results results = LimelightHelpers.parseJsonDump(timestampedString.value).targetingResults;
+
+            // Retrieves the alliance information from the DriverStation
+            Optional<Alliance> allianceOptional = DriverStation.getAlliance();
 
             // Checks if there are no targets or if the alliance information is not present
             if (results.targets_Fiducials.length == 0 || !allianceOptional.isPresent()) {
                 continue; // Skips to the next iteration of the loop
             }
 
-            double latencyMS =
-                    results.latency_capture
-                            + results.latency_pipeline; // Calculates the total latency in milliseconds
-            Pose3d poseEstimation =
-                    results.getBotPose3d_wpiBlue(); // Retrieves the pose estimation for the robot
+            double latencyMS = results.latency_capture + results.latency_pipeline; // Calculates the total latency in milliseconds
+            Pose3d poseEstimation = results.getBotPose3d_wpiBlue(); // Retrieves the pose estimation for the robot
             double averageTagDistance = 0.0; // Initializes the average tag distance to 0.0
             timestamp -= (latencyMS / 1e3); // Adjusts the timestamp by subtracting the latency in seconds
 
-            int[] tagIDs =
-                    new int[results.targets_Fiducials.length]; // Creates an array to store the tag IDs
+            int[] tagIDs = new int[results.targets_Fiducials.length]; // Creates an array to store the tag IDs
 
             // Iterates over each target in the targeting results
             for (int aprilTags = 0; aprilTags < results.targets_Fiducials.length; aprilTags++) {
-                tagIDs[aprilTags] =
-                        (int)
-                                results.targets_Fiducials[aprilTags].fiducialID; // Retrieves and stores the tag ID
+                // Retrieves and stores the tag ID
+                tagIDs[aprilTags] = (int) results.targets_Fiducials[aprilTags].fiducialID;
                 averageTagDistance +=
                         results
                                 .targets_Fiducials[aprilTags]
@@ -92,16 +87,16 @@ public void updateInputs(AprilTagVisionIOInputs inputs) {
             }
 
             averageTagDistance /= tagIDs.length; // Calculates the average tag distance
-            poseEstimates.add(
-                    new VisionHelpers.PoseEstimate(
-                            poseEstimation,
-                            timestamp,
-                            averageTagDistance,
-                            tagIDs)); // Creates a new PoseEstimate object and adds it to the poseEstimates
+            poseEstimates.add(new VisionHelpers.PoseEstimate(
+                    poseEstimation,
+                    timestamp,
+                    averageTagDistance,
+                    tagIDs
+            )); // Creates a new PoseEstimate object and adds it to the poseEstimates
             // ArrayList
         }
 
-        inputs.poseEstimates =
-                poseEstimates; // Assigns the poseEstimates ArrayList to the inputs.poseEstimates variable
+        // Assigns the poseEstimates ArrayList to the inputs.poseEstimates variable
+        inputs.poseEstimates = poseEstimates;
     }
 }