diff --git a/.Glass/glass.json b/.Glass/glass.json
index c1bfd22..0549369 100644
--- a/.Glass/glass.json
+++ b/.Glass/glass.json
@@ -1,9 +1,19 @@
 {
   "NetworkTables": {
+    "DriveTrain": {
+      "open": true
+    },
+    "SwerveChassis": {
+      "SwerveModuleFL": {
+        "open": true
+      },
+      "open": true
+    },
     "types": {
       "/FMSInfo": "FMSInfo",
       "/Shuffleboard/Auto/Routines": "String Chooser",
       "/Shuffleboard/BasicDebugging/CommandScheduler": "Scheduler",
+      "/SmartDashboard/ArmDisplay": "Mechanism2d",
       "/SmartDashboard/Field": "Field2d",
       "/SmartDashboard/PPSwerveControllerCommand_field": "Field2d"
     },
@@ -20,7 +30,39 @@
     }
   },
   "NetworkTables Settings": {
-    "mode": "Client (NT4)",
+    "mode": "Client",
     "serverTeam": "2706"
+  },
+  "Plots": {
+    "Plot <0>": {
+      "plots": [
+        {
+          "height": 332,
+          "series": [
+            {
+              "color": [
+                0.2980392277240753,
+                0.44705885648727417,
+                0.6901960968971252,
+                1.0
+              ],
+              "id": "NT:/SwerveChassis/SwerveModuleFL/Current speed (mps)"
+            },
+            {
+              "color": [
+                0.8666667342185974,
+                0.5176470875740051,
+                0.32156863808631897,
+                1.0
+              ],
+              "id": "NT:/SwerveChassis/SwerveModuleFL/Desired speed (mps)"
+            }
+          ]
+        }
+      ],
+      "window": {
+        "visible": false
+      }
+    }
   }
 }
diff --git a/src/main/deploy/pathplanner/Kevin_path.path b/src/main/deploy/pathplanner/Kevin_path.path
new file mode 100644
index 0000000..c05aab6
--- /dev/null
+++ b/src/main/deploy/pathplanner/Kevin_path.path
@@ -0,0 +1,83 @@
+{
+  "waypoints": [
+    {
+      "anchorPoint": {
+        "x": 2.138163250685476,
+        "y": 3.060337060845062
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.1381632506854755,
+        "y": 3.060337060845062
+      },
+      "holonomicAngle": 0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [
+          "ArmPickup",
+          "ArmHomeAfterPickup"
+        ],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 3.9994876570949724,
+        "y": 4.990176476079449
+      },
+      "prevControl": {
+        "x": 3.9994876570949724,
+        "y": 3.9901764760794487
+      },
+      "nextControl": {
+        "x": 3.9994876570949724,
+        "y": 3.9901764760794487
+      },
+      "holonomicAngle": 0,
+      "isReversal": true,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": true,
+      "stopEvent": {
+        "names": [
+          "ArmCubeTop",
+          "GripperOpen",
+          "ArmHome"
+        ],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 5.404045338005206,
+        "y": 2.8776303706453565
+      },
+      "prevControl": {
+        "x": 7.27215828790632,
+        "y": 2.8132043358103807
+      },
+      "nextControl": null,
+      "holonomicAngle": 0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [
+          "BlingBlue"
+        ],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    }
+  ],
+  "markers": []
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/test_path.path b/src/main/deploy/pathplanner/test_path.path
new file mode 100644
index 0000000..afc3c5f
--- /dev/null
+++ b/src/main/deploy/pathplanner/test_path.path
@@ -0,0 +1,49 @@
+{
+  "waypoints": [
+    {
+      "anchorPoint": {
+        "x": 1.2250909062388209,
+        "y": 2.3420415486911743
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.6750269384970293,
+        "y": 2.3420415486911743
+      },
+      "holonomicAngle": 0.0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 7.23,
+        "y": 2.34
+      },
+      "prevControl": {
+        "x": 6.008149009289472,
+        "y": 2.34
+      },
+      "nextControl": null,
+      "holonomicAngle": 0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    }
+  ],
+  "markers": []
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/auto/AutoRoutines.java b/src/main/java/frc/robot/auto/AutoRoutines.java
index 8bbe4fb..559b554 100644
--- a/src/main/java/frc/robot/auto/AutoRoutines.java
+++ b/src/main/java/frc/robot/auto/AutoRoutines.java
@@ -4,6 +4,7 @@
 
 package frc.robot.auto;
 
+import java.nio.file.Path;
 import java.util.HashMap;
 import java.util.List;
 import java.util.Map;
@@ -62,6 +63,10 @@ public class AutoRoutines {
     List<PathPlannerTrajectory> cone_0p5_top_charge;
     List<PathPlannerTrajectory> cube_0p5_bottom;
 
+    List<PathPlannerTrajectory> test_path;
+
+    List<PathPlannerTrajectory> Kevin_path;
+
     List<PathPlannerTrajectory> cube_3p0_top;
 
 
@@ -224,6 +229,11 @@ public AutoRoutines() {
         cube_0p5_bottom = PathPlanner.loadPathGroup("cube_0p5_bottom", 2.5, 3);
         cube_3p0_top = PathPlanner.loadPathGroup("cube_3p0_top", 2.6, 3.1);
 
+
+        test_path = PathPlanner.loadPathGroup("test_path", 2, 1);
+
+        Kevin_path = PathPlanner.loadPathGroup("Kevin_path", 2,1);
+
         cone_0p5_top_charge = PathPlanner.loadPathGroup("cone_0p5_top_charge", 2.5, 3);
 
         cone_2p0_bot_P1 = PathPlanner.loadPathGroup("cone_2p0_bot_P1", 2.7, 3.8);
@@ -319,7 +329,9 @@ public Command getAutonomousCommand(int selectAuto) {
                 return (autoBuilder.fullAuto(cube_0p5_middle_charge)); 
       
             case 4:
-                return (autoBuilder.fullAuto(cube_1p0_top));
+                //return (autoBuilder.fullAuto(cube_1p0_top));
+                return (autoBuilder.fullAuto(test_path));
+                //return (autoBuilder.fullAuto(Kevin_path));
             
             case 5:
                 //return (autoBuilder.fullAuto(cube_0p5_bottom));
diff --git a/src/main/java/frc/robot/config/Config.java b/src/main/java/frc/robot/config/Config.java
index 051b90b..aee0c86 100644
--- a/src/main/java/frc/robot/config/Config.java
+++ b/src/main/java/frc/robot/config/Config.java
@@ -72,7 +72,7 @@ public class Config {
      */
     public static Double DRIVER_JOYSTICK_DEADBAND = 0.1; // TODO: Investigate if this can be better tuned
         
-    public static double drivetrainWheelDiameter = robotSpecific(0.0986536,0.1524, 0.1016, 0.1524, 0.01524, 0.1524); // Diameter of wheel is 0.1524
+    public static double drivetrainWheelDiameter = robotSpecific(0.09267,0.1524, 0.1016, 0.1524, 0.01524, 0.1524); // Diameter of wheel is 0.1524
 
     public static final double kWheelBase = robotSpecific(0.52, -0.0, -0.0, -0.0, -0.0, -0.0);
     public static final double kTrackWidth = robotSpecific(0.655, 1.2267, 0.3136, 0.569, 0.52, 0.51762);
@@ -231,8 +231,8 @@ public static class Swerve{
         public static final double teleopRateLimit = 3;
 
         public static final double driveKS = 0.667;
-        public static final double driveKV = 2.9;
-        public static final double driveKA = 0.5;
+        public static final double driveKV = 3.0;
+        public static final double driveKA = 0.55;
 
         public static DoubleSubscriber sub_kA = NetworkTableInstance.getDefault().getTable("SwerveChassis/DrivePID").getDoubleTopic("Drive kA").subscribe(driveKA);
         public static DoubleSubscriber sub_kV = NetworkTableInstance.getDefault().getTable("SwerveChassis/DrivePID").getDoubleTopic("Drive kV").subscribe(driveKV);