Merge pull request #6 from StuyPulse/ic/alignment

Add More Alignment Commands
StuyPulse · Jan 26, 2024 · 26ac943 · 26ac943
2 parents 6b04bd9 + 6480319
commit 26ac943
Show file tree

Hide file tree

Showing 18 changed files with 461 additions and 53 deletions.
diff --git a/simgui-ds.json b/simgui-ds.json
@@ -0,0 +1,97 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "Keyboard0"
+    }
+  ]
+}
diff --git a/simgui.json b/simgui.json
@@ -0,0 +1,100 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/Autonomous": "String Chooser",
+      "/SmartDashboard/Field": "Field2d"
+    },
+    "windows": {
+      "/SmartDashboard/Autonomous": {
+        "window": {
+          "visible": true
+        }
+      },
+      "/SmartDashboard/Field": {
+        "Back Left-2d": {
+          "arrowSize": 25,
+          "arrowWeight": 2.0,
+          "length": 0.4000000059604645,
+          "selectable": false,
+          "weight": 2.0,
+          "width": 0.4000000059604645
+        },
+        "Back Right-2d": {
+          "arrowSize": 25,
+          "arrowWeight": 2.0,
+          "length": 0.4000000059604645,
+          "weight": 2.0,
+          "width": 0.4000000059604645
+        },
+        "Estimator Pose": {
+          "arrowColor": [
+            0.0,
+            0.029412031173706055,
+            1.0,
+            255.0
+          ],
+          "arrowSize": 25,
+          "arrowWeight": 2.0,
+          "length": 0.6600000262260437,
+          "weight": 2.0,
+          "width": 0.6600000262260437
+        },
+        "Front Left-2d": {
+          "arrowSize": 25,
+          "arrowWeight": 2.0,
+          "length": 0.4000000059604645,
+          "weight": 2.0,
+          "width": 0.4000000059604645
+        },
+        "Front Right-2d": {
+          "arrowSize": 25,
+          "arrowWeight": 2.0,
+          "length": 0.4000000059604645,
+          "weight": 2.0,
+          "width": 0.4000000059604645
+        },
+        "Odometry Pose": {
+          "arrowSize": 25,
+          "arrowWeight": 2.0,
+          "length": 0.6600000262260437,
+          "weight": 2.0,
+          "width": 0.6600000262260437
+        },
+        "Robot": {
+          "arrowSize": 25,
+          "arrows": false,
+          "style": "Hidden"
+        },
+        "Target Pose": {
+          "arrowSize": 25,
+          "arrowWeight": 2.0,
+          "length": 0.6600000262260437,
+          "selectable": false,
+          "style": "Box/Image",
+          "weight": 2.0,
+          "width": 0.6600000262260437
+        },
+        "bottom": 544,
+        "builtin": "2024 Crescendo",
+        "height": 8.013679504394531,
+        "left": 46,
+        "path": {
+          "arrows": false,
+          "selectable": false,
+          "style": "Line",
+          "weight": 2.0
+        },
+        "right": 1088,
+        "top": 36,
+        "width": 16.541748046875,
+        "window": {
+          "visible": true
+        }
+      }
+    }
+  },
+  "NetworkTables Info": {
+    "visible": true
+  }
+}
diff --git a/src/main/deploy/pathplanner/autos/4 Piece ABC.auto b/src/main/deploy/pathplanner/autos/4 Piece ABC.auto
@@ -11,18 +11,36 @@
     "type": "sequential",
     "data": {
       "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "intake acquire"
+          }
+        },
         {
           "type": "path",
           "data": {
             "pathName": "Start To C (ABC)"
           }
         },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 1.0
+          }
+        },
         {
           "type": "path",
           "data": {
             "pathName": "C To B (ABC)"
           }
         },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 1.0
+          }
+        },
         {
           "type": "path",
           "data": {

diff --git a/src/main/deploy/pathplanner/paths/B To A (ABC).path b/src/main/deploy/pathplanner/paths/B To A (ABC).path
@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 2.6,
-        "y": 5.54
+        "x": 2.75,
+        "y": 5.64
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.166987298107781,
-        "y": 5.79
+        "x": 2.316987298107781,
+        "y": 5.89
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 2.6,
-        "y": 6.78
+        "x": 2.75,
+        "y": 6.88
       },
       "prevControl": {
-        "x": 2.166987298107781,
-        "y": 6.53
+        "x": 2.316987298107781,
+        "y": 6.63
       },
       "nextControl": null,
       "isLocked": false,

diff --git a/src/main/deploy/pathplanner/paths/C To B (ABC).path b/src/main/deploy/pathplanner/paths/C To B (ABC).path
@@ -8,20 +8,20 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.1669872981077813,
-        "y": 4.55
+        "x": 2.0254666676607664,
+        "y": 4.782090707264905
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 2.6,
-        "y": 5.54
+        "x": 2.75,
+        "y": 5.64
       },
       "prevControl": {
-        "x": 2.166987298107781,
-        "y": 5.29
+        "x": 2.257596123493896,
+        "y": 5.553175911166535
       },
       "nextControl": null,
       "isLocked": false,

diff --git a/src/main/deploy/pathplanner/paths/Start To C (ABC).path b/src/main/deploy/pathplanner/paths/Start To C (ABC).path
@@ -3,12 +3,12 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.41,
+        "x": 1.5,
         "y": 4.3
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.91,
+        "x": 2.0,
         "y": 4.3
       },
       "isLocked": false,

diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java
@@ -5,6 +5,8 @@
 
 package com.stuypulse.robot;
 
+import com.pathplanner.lib.pathfinding.LocalADStar;
+import com.pathplanner.lib.pathfinding.Pathfinding;
 import com.stuypulse.robot.constants.Settings.RobotType;
 
 import edu.wpi.first.wpilibj.TimedRobot;
@@ -50,6 +52,8 @@ public void robotInit() {
         state = MatchState.DISABLE;
         SmartDashboard.putString("Match State", state.name());
         SmartDashboard.putString("Robot Type", robotType.name());
+
+        Pathfinding.setPathfinder(new LocalADStar());
     }
 
     @Override