diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json
new file mode 100644
index 0000000..70c55ea
--- /dev/null
+++ b/.pathplanner/settings.json
@@ -0,0 +1,14 @@
+{
+  "robotWidth": 0.9,
+  "robotLength": 0.9,
+  "holonomicMode": true,
+  "pathFolders": [
+    "New Folder"
+  ],
+  "autoFolders": [],
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 3.0,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "maxModuleSpeed": 4.5
+}
\ No newline at end of file
diff --git a/gradlew b/gradlew
old mode 100644
new mode 100755
diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
new file mode 100644
index 0000000..bab0da9
--- /dev/null
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -0,0 +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]]}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/New New Path.path b/src/main/deploy/pathplanner/paths/New New Path.path
new file mode 100644
index 0000000..c68786f
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/New New Path.path	
@@ -0,0 +1,116 @@
+{
+  "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": 8.328999269805339,
+        "y": 7.4309552554704394
+      },
+      "prevControl": {
+        "x": 8.511296292046968,
+        "y": 8.095132618708886
+      },
+      "nextControl": {
+        "x": 8.146702247563708,
+        "y": 6.7667778922319926
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.640145802652965,
+        "y": 6.344214763563695
+      },
+      "prevControl": {
+        "x": 6.120236779878946,
+        "y": 6.887756014645622
+      },
+      "nextControl": {
+        "x": 7.286315824867787,
+        "y": 5.668673376702747
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.328999269805339,
+        "y": 5.580559282764361
+      },
+      "prevControl": {
+        "x": 8.885141974325553,
+        "y": 6.454497818438981
+      },
+      "nextControl": {
+        "x": 7.917800164759543,
+        "y": 4.934389260549541
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.772316943560543,
+        "y": 5.800844517610323
+      },
+      "prevControl": {
+        "x": 6.919173766791185,
+        "y": 6.402957492855949
+      },
+      "nextControl": {
+        "x": 6.625460120329902,
+        "y": 5.198731542364694
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 4.053248321165694
+      },
+      "prevControl": {
+        "x": -1.0032220350666101,
+        "y": 2.826993847189841
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path
new file mode 100644
index 0000000..45cfa6c
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/New Path.path	
@@ -0,0 +1,271 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.4267285779652068,
+        "y": 5.521816553472107
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.8191211945930155,
+        "y": 6.300157716594503
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.7631256693640407,
+        "y": 4.038562638842631
+      },
+      "prevControl": {
+        "x": 3.856994878248822,
+        "y": 4.120194669356421
+      },
+      "nextControl": {
+        "x": 1.7791849537187452,
+        "y": 3.9651342272273102
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.968725221886938,
+        "y": 5.521816553472107
+      },
+      "prevControl": {
+        "x": 3.132192798793267,
+        "y": 5.209453385321058
+      },
+      "nextControl": {
+        "x": 2.80525764498061,
+        "y": 5.834179721623156
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.4267285779652068,
+        "y": 5.521816553472107
+      },
+      "prevControl": {
+        "x": 0.696863096921075,
+        "y": 5.087700878079531
+      },
+      "nextControl": {
+        "x": 2.1565940590093406,
+        "y": 5.9559322288646825
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.968725221886938,
+        "y": 7.0050704681015805
+      },
+      "prevControl": {
+        "x": 2.4392551066537314,
+        "y": 6.188183410443437
+      },
+      "nextControl": {
+        "x": 3.609582512967294,
+        "y": 7.9938099867783095
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.685576451653799,
+        "y": 5.698044741348875
+      },
+      "prevControl": {
+        "x": 4.408629078943142,
+        "y": 5.7638539992345725
+      },
+      "nextControl": {
+        "x": 6.962523824364456,
+        "y": 5.632235483463178
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.284942222836145,
+        "y": 7.489697984762696
+      },
+      "prevControl": {
+        "x": 9.075134361161302,
+        "y": 8.140444451618707
+      },
+      "nextControl": {
+        "x": 7.286315824867787,
+        "y": 6.667299774671107
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.685576451653799,
+        "y": 5.698044741348875
+      },
+      "prevControl": {
+        "x": 5.456501835429192,
+        "y": 5.713389868366871
+      },
+      "nextControl": {
+        "x": 5.914651067878406,
+        "y": 5.682699614330879
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.284942222836145,
+        "y": 5.698044741348875
+      },
+      "prevControl": {
+        "x": 7.842296179420121,
+        "y": 6.634411371652005
+      },
+      "nextControl": {
+        "x": 8.666769963235812,
+        "y": 4.890332213580349
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.685576451653799,
+        "y": 5.698044741348875
+      },
+      "prevControl": {
+        "x": 5.793883358786396,
+        "y": 5.687948334751769
+      },
+      "nextControl": {
+        "x": 5.577269544521202,
+        "y": 5.708141147945981
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.284942222836145,
+        "y": 4.038562638842631
+      },
+      "prevControl": {
+        "x": 7.800314706175031,
+        "y": 4.273533556011657
+      },
+      "nextControl": {
+        "x": 8.74165576951133,
+        "y": 3.817125767727388
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.685576451653799,
+        "y": 2.951822146935887
+      },
+      "prevControl": {
+        "x": 5.721250192300502,
+        "y": 3.227578719057887
+      },
+      "nextControl": {
+        "x": 5.649902711007095,
+        "y": 2.676065574813887
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.284942222836145,
+        "y": 2.4965659949208994
+      },
+      "prevControl": {
+        "x": 8.85587151717903,
+        "y": 2.712798350307138
+      },
+      "nextControl": {
+        "x": 7.714012928493259,
+        "y": 2.280333639534661
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.685576451653799,
+        "y": 2.8049653237052454
+      },
+      "prevControl": {
+        "x": 5.73907498416589,
+        "y": 3.1556442508495848
+      },
+      "nextControl": {
+        "x": 5.632077919141708,
+        "y": 2.454286396560905
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.284942222836145,
+        "y": 0.7636554807993345
+      },
+      "prevControl": {
+        "x": 7.048067892200186,
+        "y": 0.843527962547876
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.0,
+      "rotationDegrees": 0,
+      "rotateFast": false
+    },
+    {
+      "waypointRelativePos": 1.0,
+      "rotationDegrees": 0,
+      "rotateFast": false
+    }
+  ],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 70cd557..e1a3ee9 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -51,26 +51,30 @@ public static final class DriveConstants {
     public static final double kBackLeftChassisAngularOffset = Math.PI;
     public static final double kBackRightChassisAngularOffset = Math.PI / 2;
 
-    // SPARK MAX CAN IDs
-    public static final int kFrontLeftDrivingCanId = 8;
-    public static final int kRearLeftDrivingCanId = 2;
-    public static final int kFrontRightDrivingCanId = 9;
-    public static final int kRearRightDrivingCanId = 6;
-
-    public static final int kFrontLeftTurningCanId = 7;
-    public static final int kRearLeftTurningCanId = 1;
-    public static final int kFrontRightTurningCanId = 10;
-    public static final int kRearRightTurningCanId = 5;
-
-    public static final boolean kGyroReversed = false;
+   // SPARK MAX CAN IDs
+   public static final int kFrontLeftDrivingCanId = 1;
+   public static final int kRearLeftDrivingCanId = 3;
+   public static final int kFrontRightDrivingCanId = 5;
+   public static final int kRearRightDrivingCanId = 7;
+
+   public static final int kFrontLeftTurningCanId = 2;
+   public static final int kRearLeftTurningCanId = 4;
+   public static final int kFrontRightTurningCanId = 6;
+   public static final int kRearRightTurningCanId = 8;
+
+    public static final boolean kGyroReversed = true;
   }
 
   public static final class IntakeConstants{
-    public static final int IntakeMotor = 4;
+    public static final int IntakeMotor = 14;
   }
 
   public static final class AmpConstants{
-    public static final int AmpMotor = 3;
+    public static final int AmpMotor = 13;
+  }
+
+  public static final class ElevatorConstants{
+    public static final int ElevatorMotor = 12;
   }
 
   public static final class ModuleConstants {
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index d9bd7f5..7878ea3 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -5,6 +5,7 @@
 package frc.robot;
 
 import edu.wpi.first.math.MathUtil;
+import com.pathplanner.lib.auto.NamedCommands;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -15,16 +16,29 @@
 import edu.wpi.first.math.trajectory.TrajectoryGenerator;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.PS4Controller.Button;
+import edu.wpi.first.wpilibj.interfaces.Gyro;
 import frc.robot.Constants.AutoConstants;
 import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.OIConstants;
+import frc.robot.commands.*;
+/*import frc.robot.commands.AmpCommand;
+import frc.robot.commands.AmpOuttake;
+import frc.robot.commands.intakecommands;
+import frc.robot.commands.outtakecommands;
+import frc.robot.commands.ElevatorDown;
+import frc.robot.commands.ElevatorUp;*/
+import frc.robot.subsystems.Amp;
 import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.Elevator;
+import frc.robot.subsystems.NGNL_intake;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import java.util.List;
 
+import com.pathplanner.lib.auto.NamedCommands;
+
 /*
  * This class is where the bulk of the robot should be declared.  Since Command-based is a
  * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
@@ -32,11 +46,28 @@
  * (including subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
+    private final NGNL_intake intake = new NGNL_intake(4);
+    
   // The robot's subsystems
+  private final Amp amp = new Amp(3);
   private final DriveSubsystem m_robotDrive = new DriveSubsystem();
+  private final Elevator elevator = new Elevator(11);
 
   // The driver's controller
-  XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
+  private final XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
+  private final XboxController xboxController = new XboxController(1);
+
+  private final intakecommands intakeCommand = new intakecommands(intake);
+  private final outtakecommands outtakecommands = new outtakecommands(intake);
+
+  private final AmpCommand ampCommand = new AmpCommand(amp);
+  private final AmpOuttake ampOuttake = new AmpOuttake(amp);
+
+  private final ElevatorDown down = new ElevatorDown(elevator);
+  private final ElevatorUp up = new ElevatorUp(elevator);
+
+  private final zeroGyro reset = new zeroGyro(m_robotDrive);
+  
 
   /**
    * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -57,7 +88,10 @@ public RobotContainer() {
                 true, true),
             m_robotDrive));
   }
-
+  private void nameCommands(){
+    NamedCommands.registerCommand("IntakeCommand", new intakecommands(intake));
+    //NamedCommands.registerCommand("OuttakeCommand", new OuttakeCommand(intake));
+  }
   /**
    * Use this method to define your button->command mappings. Buttons can be
    * created by
@@ -68,10 +102,13 @@ public RobotContainer() {
    * {@link JoystickButton}.
    */
   private void configureButtonBindings() {
-    new JoystickButton(m_driverController, Button.kR1.value)
-        .whileTrue(new RunCommand(
-            () -> m_robotDrive.setX(),
-            m_robotDrive));
+    new JoystickButton(xboxController, XboxController.Button.kLeftBumper.value).whileTrue(intakeCommand);
+    new JoystickButton(xboxController, XboxController.Button.kLeftBumper.value).whileTrue(ampCommand);
+    new JoystickButton(xboxController, XboxController.Button.kRightBumper.value).whileTrue(ampOuttake);
+    new JoystickButton(xboxController, XboxController.Button.kRightBumper.value).whileTrue(outtakecommands); 
+    new JoystickButton(xboxController, XboxController.Button.kA.value).whileTrue(down);
+    new JoystickButton(xboxController, XboxController.Button.kY.value).whileTrue(up);
+    new JoystickButton(m_driverController, XboxController.Button.kY.value).whileTrue(reset);
   }
 
   /**
diff --git a/src/main/java/frc/robot/TrainingBot.code-workspace b/src/main/java/frc/robot/TrainingBot.code-workspace
new file mode 100644
index 0000000..b425ded
--- /dev/null
+++ b/src/main/java/frc/robot/TrainingBot.code-workspace
@@ -0,0 +1,10 @@
+{
+	"folders": [
+		{
+			"path": "../../../../.."
+		},
+		{
+			"path": "../../../../.."
+		}
+	]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/AmpCommand.java b/src/main/java/frc/robot/commands/AmpCommand.java
new file mode 100644
index 0000000..ef49442
--- /dev/null
+++ b/src/main/java/frc/robot/commands/AmpCommand.java
@@ -0,0 +1,25 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Amp;
+
+public class AmpCommand extends Command {
+  private final Amp amp;
+
+  public AmpCommand(Amp amp) {
+    this.amp = amp;
+
+    addRequirements(amp);
+  }
+
+  @Override
+  public void execute() {
+    amp.intake();
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    amp.stopIntake();
+  }
+
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/AmpOuttake.java b/src/main/java/frc/robot/commands/AmpOuttake.java
new file mode 100644
index 0000000..1ce1733
--- /dev/null
+++ b/src/main/java/frc/robot/commands/AmpOuttake.java
@@ -0,0 +1,25 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Amp;
+
+public class AmpOuttake extends Command {
+  private final Amp amp;
+
+  public AmpOuttake(Amp amp) {
+    this.amp = amp;
+
+    addRequirements(amp);
+  }
+
+  @Override
+  public void execute() {
+    amp.outtake();
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    amp.stopIntake();
+  }
+
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/ElevatorDown.java b/src/main/java/frc/robot/commands/ElevatorDown.java
new file mode 100644
index 0000000..a525e1d
--- /dev/null
+++ b/src/main/java/frc/robot/commands/ElevatorDown.java
@@ -0,0 +1,25 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Elevator;
+
+public class ElevatorDown extends Command {
+  private final Elevator elevator;
+
+  public ElevatorDown(Elevator elevator) {
+    this.elevator = elevator;
+
+    addRequirements(elevator);
+  }
+
+  @Override
+  public void execute() {
+    elevator.down();
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    elevator.hold();
+  }
+
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/ElevatorUp.java b/src/main/java/frc/robot/commands/ElevatorUp.java
new file mode 100644
index 0000000..30cecb3
--- /dev/null
+++ b/src/main/java/frc/robot/commands/ElevatorUp.java
@@ -0,0 +1,25 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Elevator;
+
+public class ElevatorUp extends Command {
+  private final Elevator elevator;
+
+  public ElevatorUp(Elevator elevator) {
+    this.elevator = elevator;
+
+    addRequirements(elevator);
+  }
+
+  @Override
+  public void execute() {
+    elevator.up();
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    elevator.hold();
+  }
+
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/intakecommands.java b/src/main/java/frc/robot/commands/intakecommands.java
new file mode 100644
index 0000000..8dd6ae2
--- /dev/null
+++ b/src/main/java/frc/robot/commands/intakecommands.java
@@ -0,0 +1,25 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.NGNL_intake;
+
+public class intakecommands extends Command {
+  private final NGNL_intake intake;
+
+  public intakecommands(NGNL_intake intake) {
+    this.intake = intake;
+
+    addRequirements(intake);
+  }
+
+  @Override
+  public void execute() {
+    intake.intake();
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    intake.stopIntake();
+  }
+
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/outtakecommands.java b/src/main/java/frc/robot/commands/outtakecommands.java
new file mode 100644
index 0000000..563ab5c
--- /dev/null
+++ b/src/main/java/frc/robot/commands/outtakecommands.java
@@ -0,0 +1,25 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.NGNL_intake;
+
+public class outtakecommands extends Command {
+  private final NGNL_intake outtake;
+
+  public outtakecommands(NGNL_intake outtake) {
+    this.outtake = outtake;
+
+    addRequirements(outtake);
+  }
+
+  @Override
+  public void execute() {
+    outtake.outtake();
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    outtake.stopIntake();
+  }
+
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/zeroGyro.java b/src/main/java/frc/robot/commands/zeroGyro.java
new file mode 100644
index 0000000..c1e7ff9
--- /dev/null
+++ b/src/main/java/frc/robot/commands/zeroGyro.java
@@ -0,0 +1,25 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.DriveSubsystem;
+
+public class zeroGyro extends Command {
+  private final DriveSubsystem gyro;
+
+  public zeroGyro(DriveSubsystem gyro) {
+    this.gyro = gyro;
+
+    addRequirements(gyro);
+  }
+
+  @Override
+  public void execute() {
+    gyro.zeroHeading();
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    gyro.getHeading();
+  }
+
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/Amp.java b/src/main/java/frc/robot/subsystems/Amp.java
new file mode 100644
index 0000000..6fca72c
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Amp.java
@@ -0,0 +1,37 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.CANSparkMax;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.AmpConstants;
+
+public class Amp extends SubsystemBase{
+  private final CANSparkMax ampMotor;
+
+  public Amp(int AmpMotorID) {
+    ampMotor = new CANSparkMax(AmpConstants.AmpMotor, CANSparkMax.MotorType.kBrushless);
+
+  }
+
+  // Spins intake motor to intake notes
+  public void intake() {
+    ampMotor.set(.75);
+  }
+
+  public void outtake() {
+    ampMotor.set(-0.75);
+  }
+
+  public void stopIntake() {
+    ampMotor.set(0);
+  }
+
+  @Override
+  public void periodic() {
+
+  }
+
+  public void stop() {
+    ampMotor.stopMotor();
+  }
+
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
index 09b175d..98d8332 100644
--- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
@@ -8,11 +8,14 @@
 import com.kauailabs.navx.frc.AHRS;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.SPI;
+import edu.wpi.first.wpilibj.smartdashboard.*;
 import edu.wpi.first.util.WPIUtilJNI;
 import frc.robot.Constants.DriveConstants;
 import frc.robot.utils.SwerveUtils;
@@ -20,6 +23,7 @@
 
 public class DriveSubsystem extends SubsystemBase {
   AHRS ahrs;
+  private final Field2d m_field = new Field2d();
   // Create MAXSwerveModules
   private final MAXSwerveModule m_frontLeft = new MAXSwerveModule(
       DriveConstants.kFrontLeftDrivingCanId,
@@ -56,7 +60,7 @@ public class DriveSubsystem extends SubsystemBase {
   // Odometry class for tracking robot pose
   SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(
       DriveConstants.kDriveKinematics,
-      Rotation2d.fromDegrees(m_gyro.getAngle()),
+      Rotation2d.fromDegrees(-m_gyro.getAngle()),
       new SwerveModulePosition[] {
           m_frontLeft.getPosition(),
           m_frontRight.getPosition(),
@@ -66,19 +70,23 @@ public class DriveSubsystem extends SubsystemBase {
 
   /** Creates a new DriveSubsystem. */
   public DriveSubsystem() {
+    
   }
 
   @Override
   public void periodic() {
     // Update the odometry in the periodic block
     m_odometry.update(
-        Rotation2d.fromDegrees(m_gyro.getAngle()),
+        Rotation2d.fromDegrees(-m_gyro.getAngle()),
         new SwerveModulePosition[] {
             m_frontLeft.getPosition(),
             m_frontRight.getPosition(),
             m_rearLeft.getPosition(),
             m_rearRight.getPosition()
         });
+
+        m_field.setRobotPose(m_odometry.getPoseMeters());
+        SmartDashboard.putNumber("Gyro Value", -m_gyro.getAngle());
   }
 
   /**
@@ -97,7 +105,7 @@ public Pose2d getPose() {
    */
   public void resetOdometry(Pose2d pose) {
     m_odometry.resetPosition(
-        Rotation2d.fromDegrees(m_gyro.getAngle()),
+        Rotation2d.fromDegrees(-m_gyro.getAngle()),
         new SwerveModulePosition[] {
             m_frontLeft.getPosition(),
             m_frontRight.getPosition(),
@@ -177,7 +185,7 @@ else if (angleDif > 0.85*Math.PI) {
 
     var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(
         fieldRelative
-            ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, Rotation2d.fromDegrees(m_gyro.getAngle()))
+            ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, Rotation2d.fromDegrees(-m_gyro.getAngle()))
             : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
     SwerveDriveKinematics.desaturateWheelSpeeds(
         swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond);
@@ -191,10 +199,10 @@ else if (angleDif > 0.85*Math.PI) {
    * Sets the wheels into an X formation to prevent movement.
    */
   public void setX() {
-    m_frontLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45)));
-    m_frontRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45)));
-    m_rearLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45)));
-    m_rearRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45)));
+    m_frontLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45)));
+    m_frontRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45)));
+    m_rearLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45)));
+    m_rearRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45)));
   }
 
   /**
@@ -230,7 +238,7 @@ public void zeroHeading() {
    * @return the robot's heading in degrees, from -180 to 180
    */
   public double getHeading() {
-    return Rotation2d.fromDegrees(m_gyro.getAngle()).getDegrees();
+    return Rotation2d.fromDegrees(-m_gyro.getAngle()).getDegrees();
   }
 
   /**
diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java
new file mode 100644
index 0000000..4964de5
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Elevator.java
@@ -0,0 +1,37 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.CANSparkMax;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.ElevatorConstants;
+
+public class Elevator extends SubsystemBase{
+  private final CANSparkMax ElevatorMotor;
+
+  public Elevator(int ElevatorMotorID) {
+    ElevatorMotor = new CANSparkMax(ElevatorConstants.ElevatorMotor, CANSparkMax.MotorType.kBrushless);
+
+  }
+
+  // Spins intake motor to intake notes
+  public void up() {
+    ElevatorMotor.set(1);
+  }
+
+  public void down() {
+    ElevatorMotor.set(-1);
+  }
+
+  public void hold() {
+    ElevatorMotor.set(0);
+  }
+
+  @Override
+  public void periodic() {
+
+  }
+
+  public void stop() {
+    ElevatorMotor.stopMotor();
+  }
+
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/NGNL_intake.java b/src/main/java/frc/robot/subsystems/NGNL_intake.java
index bbe237f..865416e 100644
--- a/src/main/java/frc/robot/subsystems/NGNL_intake.java
+++ b/src/main/java/frc/robot/subsystems/NGNL_intake.java
@@ -1,25 +1,37 @@
 package frc.robot.subsystems;
 
+import com.revrobotics.CANSparkMax;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.IntakeConstants;
-import com.revrobotics.CANSparkMax;
-import com.revrobotics.CANSparkLowLevel.MotorType;
-
-public class NGNL_intake extends SubsystemBase {
-
-    private final CANSparkMax intakeMotor = 
-        new CANSparkMax(IntakeConstants.IntakeMotor, MotorType.kBrushless);
-    
-    public void runPercentSpeed(double percentSpeed){
-        percentSpeed = -percentSpeed;
-    }    
-    public NGNL_intake(){
-        intakeMotor.restoreFactoryDefaults();
-        intakeMotor.setCANTimeout(250);
-        intakeMotor.setInverted(false);
-        intakeMotor.setSmartCurrentLimit(40);
-        intakeMotor.burnFlash();
-    }
-
-    
-}
+
+public class NGNL_intake extends SubsystemBase{
+  private final CANSparkMax intakeMotor;
+
+  public NGNL_intake(int intakeMotorID) {
+    intakeMotor = new CANSparkMax(IntakeConstants.IntakeMotor, CANSparkMax.MotorType.kBrushless);
+
+  }
+
+  // Spins intake motor to intake notes
+  public void intake() {
+    intakeMotor.set(.75);
+  }
+
+  public void outtake() {
+    intakeMotor.set(-0.75);
+  }
+
+  public void stopIntake() {
+    intakeMotor.set(0);
+  }
+
+  @Override
+  public void periodic() {
+
+  }
+
+  public void stop() {
+    intakeMotor.stopMotor();
+  }
+
+}
\ No newline at end of file