Merge branch 'megatag2'

src/main/deploy/pathplanner/autos/New New Auto.auto

@@ -0,0 +1,31 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 3.9355271535655185,
+      "y": 6.819606105579925
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "test path"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "test"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/conditional auto.auto

@@ -0,0 +1,37 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 1.548236363234494,
+      "y": 4.075371117931965
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "$s!p-c5"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "test"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "$s!score c5"
+          }
+        }
+      ]
+    }
+  },
+  "folder": "limelight autos",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/c5 check.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.8,
+        "y": 0.91
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.099172885934929,
+        "y": 1.5224087509515962
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.33338961685056,
+        "y": 1.885444683870822
+      },
+      "prevControl": {
+        "x": 7.169437905209619,
+        "y": 1.5341195874973776
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 2.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0.5,
+    "rotation": 32.61924307119286,
+    "rotateFast": true
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/java/frc/robot/Constants.java

@@ -38,7 +38,7 @@ public static Mode getMode() {
     };
   }
 
-  public static final Mode currentMode = Mode.REAL;
+  public static final Mode currentMode = Mode.SIM;
   public static final boolean tuningMode = true;
   public static final String CANBUS = "CAN Bus 2";
   public static final double LOOP_PERIOD_SECS = 0.02;
@@ -130,6 +130,8 @@ public static class PivotConstants {
     public static final double CLIMB_SETPOINT_ONE_DEG = 50;
     public static final double CLIMB_SETPOINT_TWO_DEG = 95;
 
+    public static final double TRAP_SETPOINT_DEG = 75;
+
     public static final double THRESHOLD = 1.5;
 
     public static final double[] PID = {0, 0, 0};