ryan

SouthwestRoboticsProgramming · Apr 13, 2024 · 63077da · 63077da
1 parent 73294dc
commit 63077da
Show file tree

Hide file tree

Showing 13 changed files with 243 additions and 20 deletions.
diff --git a/Robot/src/main/deploy/pathplanner/autos/Amp Alternate.auto b/Robot/src/main/deploy/pathplanner/autos/Amp Alternate.auto
@@ -24,7 +24,7 @@
               {
                 "type": "path",
                 "data": {
-                  "pathName": "Start Center"
+                  "pathName": "Center Start"
                 }
               },
               {

diff --git a/Robot/src/main/deploy/pathplanner/autos/Big Amp.auto b/Robot/src/main/deploy/pathplanner/autos/Big Amp.auto
@@ -24,7 +24,7 @@
               {
                 "type": "path",
                 "data": {
-                  "pathName": "Start Center"
+                  "pathName": "Center Start"
                 }
               },
               {

diff --git a/Robot/src/main/deploy/pathplanner/autos/Center Through 4 Note.auto b/Robot/src/main/deploy/pathplanner/autos/Center Through 4 Note.auto
@@ -0,0 +1,86 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 1.3576779647621067,
+      "y": 5.52811290895741
+    },
+    "rotation": 180.0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
+        {
+          "type": "race",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Center Start"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Intake Until Note"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Wait and Shoot"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Center Grab Middle A"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Center Middle Score A"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Center End Grab"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Center End Score"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "folder": "Center",
+  "choreoAuto": false
+}
diff --git a/Robot/src/main/deploy/pathplanner/autos/Source Alternate.auto b/Robot/src/main/deploy/pathplanner/autos/Source Alternate.auto
@@ -24,7 +24,7 @@
               {
                 "type": "path",
                 "data": {
-                  "pathName": "Start Center"
+                  "pathName": "Center Start"
                 }
               },
               {

diff --git a/Robot/src/main/deploy/pathplanner/paths/Center End Grab.path b/Robot/src/main/deploy/pathplanner/paths/Center End Grab.path
@@ -0,0 +1,82 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.243606396951512,
+        "y": 5.118623063849453
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.958827207460963,
+        "y": 4.315933188227984
+      },
+      "isLocked": false,
+      "linkedName": "Score Location Center"
+    },
+    {
+      "anchor": {
+        "x": 8.016137678480716,
+        "y": 2.6635356237602354
+      },
+      "prevControl": {
+        "x": 7.281615905307705,
+        "y": 3.240182568534779
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "End of Center End Grab"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [
+    {
+      "name": "New Constraints Zone",
+      "minWaypointRelativePos": 0.85,
+      "maxWaypointRelativePos": 1.0,
+      "constraints": {
+        "maxVelocity": 1.0,
+        "maxAcceleration": 1.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 640.0
+      }
+    }
+  ],
+  "eventMarkers": [
+    {
+      "name": "Intake Until Note",
+      "waypointRelativePos": 0.45,
+      "command": {
+        "type": "parallel",
+        "data": {
+          "commands": [
+            {
+              "type": "named",
+              "data": {
+                "name": "Intake Until Note"
+              }
+            }
+          ]
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 5.7,
+    "maxAcceleration": 3.8,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 640.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 142.99955995556826,
+    "rotateFast": true
+  },
+  "reversed": false,
+  "folder": "Center",
+  "previewStartingState": {
+    "rotation": 173.6824153406948,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
diff --git a/Robot/src/main/deploy/pathplanner/paths/Center End Score.path b/Robot/src/main/deploy/pathplanner/paths/Center End Score.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.016137678480716,
+        "y": 2.6635356237602354
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.782222613428363,
+        "y": 3.631085294970689
+      },
+      "isLocked": false,
+      "linkedName": "End of Center End Grab"
+    },
+    {
+      "anchor": {
+        "x": 4.243606396951512,
+        "y": 5.118623063849453
+      },
+      "prevControl": {
+        "x": 4.298154262975193,
+        "y": 4.346758318516637
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Score Location Center"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 5.7,
+    "maxAcceleration": 3.8,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 640.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 174.56598213220494,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "Center",
+  "previewStartingState": {
+    "rotation": 141.77166803538606,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
diff --git a/Robot/src/main/deploy/pathplanner/paths/Center Grab Middle A.path b/Robot/src/main/deploy/pathplanner/paths/Center Grab Middle A.path
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 7.785620631426448,
-        "y": 4.086239052107734
+        "x": 7.9203263103326815,
+        "y": 4.051814946077808
       },
       "prevControl": {
-        "x": 3.037488379817527,
-        "y": 3.481754042662655
+        "x": 3.1721940587237607,
+        "y": 3.4473299366327295
       },
       "nextControl": null,
       "isLocked": false,

diff --git a/Robot/src/main/deploy/pathplanner/paths/Center Middle Score A.path b/Robot/src/main/deploy/pathplanner/paths/Center Middle Score A.path
@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 7.785620631426448,
-        "y": 4.086239052107734
+        "x": 7.9203263103326815,
+        "y": 4.051814946077808
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.240933736827007,
-        "y": 4.271484458228
+        "x": 5.37563941573324,
+        "y": 4.237060352198074
       },
       "isLocked": false,
       "linkedName": "End of Center Grab Middle A"

diff --git a/Robot/src/main/deploy/pathplanner/paths/Source Center A Score.path b/Robot/src/main/deploy/pathplanner/paths/Source Center A Score.path
@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 4.75,
-    "maxAcceleration": 3.9,
+    "maxVelocity": 5.7,
+    "maxAcceleration": 3.8,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 640.0
   },

diff --git a/Robot/src/main/deploy/pathplanner/paths/Source Center B Grab.path b/Robot/src/main/deploy/pathplanner/paths/Source Center B Grab.path
@@ -66,8 +66,8 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 4.75,
-    "maxAcceleration": 3.9,
+    "maxVelocity": 5.7,
+    "maxAcceleration": 3.8,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 640.0
   },

diff --git a/Robot/src/main/deploy/pathplanner/paths/Source Center B Score.path b/Robot/src/main/deploy/pathplanner/paths/Source Center B Score.path
@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 4.75,
-    "maxAcceleration": 3.9,
+    "maxVelocity": 5.7,
+    "maxAcceleration": 3.8,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 640.0
   },

diff --git a/Robot/src/main/deploy/pathplanner/paths/Source End Grab.path b/Robot/src/main/deploy/pathplanner/paths/Source End Grab.path
@@ -101,8 +101,8 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 4.75,
-    "maxAcceleration": 3.9,
+    "maxVelocity": 5.7,
+    "maxAcceleration": 3.8,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 640.0
   },

diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/ShooterSubsystem.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/ShooterSubsystem.java
@@ -107,7 +107,10 @@ public void periodic() {
 
             // Relative to the target
             Translation2d robotVelocity = new Translation2d(robotSpeeds.vxMetersPerSecond, robotSpeeds.vyMetersPerSecond).rotateBy(angleToTarget);
-            aim = tableAimCalculator.calculateAim(distToSpeaker, robotVelocity.getX());
+            if (aimCalculator instanceof TableAimCalculator)
+                aim = ((TableAimCalculator) aimCalculator).calculateAim(distToSpeaker, robotVelocity.getX());
+            else
+                aim = aimCalculator.calculateAim(distToSpeaker);
 
             if (RobotBase.isSimulation())
                 SimView.lobTrajectory.update(