Committing general unfinished updates

.pathplanner/settings.json

@@ -4,8 +4,8 @@
   "holonomicMode": true,
   "pathFolders": [],
   "autoFolders": [],
-  "defaultMaxVel": 3.0,
-  "defaultMaxAccel": 3.0,
+  "defaultMaxVel": 4.0,
+  "defaultMaxAccel": 4.0,
   "defaultMaxAngVel": 540.0,
   "defaultMaxAngAccel": 720.0,
   "maxModuleSpeed": 4.03

src/main/deploy/pathplanner/autos/Center Sub 4s.auto

@@ -0,0 +1,118 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 1.3216735004898588,
+      "y": 5.55593454044051
+    },
+    "rotation": 180.0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Speaker Note Shot"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Center Sub to Note 1"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Auto Intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Note 1 to Center Sub"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Speaker Note Shot"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Center Sub to Note 2"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Auto Intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Note 2 to Center Sub"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Speaker Note Shot"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Left Sub to Note 3"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Auto Intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Left Note 3 to Sub"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Speaker Note Shot"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Gremlin Auto Left.auto

@@ -2,8 +2,8 @@
   "version": 1.0,
   "startingPose": {
     "position": {
-      "x": 0.4936438362905214,
-      "y": 6.981410202136645
+      "x": 0.4585613729632017,
+      "y": 4.0695657459691
     },
     "rotation": 180.0
   },
@@ -14,13 +14,7 @@
         {
           "type": "path",
           "data": {
-            "pathName": "Left Subwoofer"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "Charge Shooter"
+            "pathName": "Gremlin Start"
           }
         },
         {
@@ -32,7 +26,7 @@
         {
           "type": "path",
           "data": {
-            "pathName": "L Sub to Center"
+            "pathName": "Gremlin Path"
           }
         }
       ]

src/main/deploy/pathplanner/autos/Left Sub 3s.auto

@@ -0,0 +1,111 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.4936438362905214,
+      "y": 6.981410202136645
+    },
+    "rotation": 180.0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Left Subwoofer"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Charge Shooter"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Speaker Note Shot"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "L Sub to Note 1"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Auto Intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Note 1 to Speaker"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Charge Shooter"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Speaker Note Shot"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "L Sub to Note 2"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Auto Intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Note 2 to L Sub"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Charge Shooter"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Speaker Note Shot"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Left Sub 4s.auto

@@ -0,0 +1,148 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.5740411480822968,
+      "y": 7.021880329474945
+    },
+    "rotation": 180.0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Left Subwoofer"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Charge Shooter"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Speaker Note Shot"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "L Sub to Note 1"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Auto Intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Note 1 to Speaker"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Charge Shooter"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Speaker Note Shot"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "L Sub to Note 2"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Auto Intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Note 2 to L Sub (4S)"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Charge Shooter"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Speaker Note Shot"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Left Sub to Note 3"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Auto Intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Left Note 3 to Sub"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Charge Shooter"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Speaker Note Shot"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Left Subwoofer Center 2S.auto

@@ -1,10 +1,28 @@
 {
   "version": 1.0,
-  "startingPose": null,
+  "startingPose": {
+    "position": {
+      "x": 0.4936438362905214,
+      "y": 6.981410202136645
+    },
+    "rotation": 180.0
+  },
   "command": {
     "type": "sequential",
     "data": {
       "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Left Subwoofer"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Charge Shooter"
+          }
+        },
         {
           "type": "named",
           "data": {
@@ -12,34 +30,28 @@
           }
         },
         {
-          "type": "path",
-          "data": {
-            "pathName": "Left Preloaded+Pickup"
-          }
-        },
-        {
-          "type": "deadline",
+          "type": "parallel",
           "data": {
             "commands": [
               {
-                "type": "named",
+                "type": "path",
                 "data": {
-                  "name": "Amp Handoff"
+                  "pathName": "L Sub to Note 1"
                 }
               },
               {
-                "type": "path",
+                "type": "named",
                 "data": {
-                  "pathName": "Note 1 to amp"
+                  "name": "Auto Intake"
                 }
               }
             ]
           }
         },
         {
-          "type": "named",
+          "type": "path",
           "data": {
-            "name": "Amp Note Shot"
+            "pathName": "Note 1 to Speaker"
           }
         }
       ]

src/main/deploy/pathplanner/autos/Right 2S Center.auto

@@ -0,0 +1,62 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.52,
+      "y": 4.034483282641779
+    },
+    "rotation": 180.0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Right Sub"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Charge Shooter"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Speaker Note Shot"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Right Sub to Right Center Note"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Right Sub from Center"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Speaker Note Shot"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Right 2S.auto

@@ -2,41 +2,47 @@
   "version": 1.0,
   "startingPose": {
     "position": {
-      "x": 1.1134340217398382,
-      "y": 6.525338178881486
+      "x": 0.4936438362905214,
+      "y": 4.092954054853979
     },
-    "rotation": 42.79740183823424
+    "rotation": 180.0
   },
   "command": {
     "type": "sequential",
     "data": {
       "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Right Wall to Sub"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Charge Shooter"
+          }
+        },
         {
           "type": "named",
           "data": {
             "name": "Speaker Note Shot"
           }
         },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Left Preloaded+Pickup"
-          }
-        },
         {
           "type": "parallel",
           "data": {
             "commands": [
               {
-                "type": "wait",
+                "type": "named",
                 "data": {
-                  "waitTime": 2.0
+                  "name": "Auto Intake"
                 }
               },
               {
-                "type": "named",
+                "type": "path",
                 "data": {
-                  "name": "Amp Handoff"
+                  "pathName": "Right Sub Note 3"
                 }
               }
             ]
@@ -45,25 +51,19 @@
         {
           "type": "path",
           "data": {
-            "pathName": "Note 1 to amp"
+            "pathName": "Note 3 to Sub"
           }
         },
         {
           "type": "named",
           "data": {
-            "name": "Amp Note Shot"
+            "name": "Charge Shooter"
           }
         },
         {
-          "type": "path",
+          "type": "named",
           "data": {
-            "pathName": "Leftmost Center Note"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Note 1 to amp"
+            "name": "Speaker Note Shot"
           }
         }
       ]

src/main/deploy/pathplanner/autos/Right 3 Amp.auto

@@ -1,43 +0,0 @@
-{
-  "version": 1.0,
-  "startingPose": {
-    "position": {
-      "x": 1.1134340217398382,
-      "y": 6.525338178881486
-    },
-    "rotation": 33.13560954871888
-  },
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Left Preloaded+Pickup"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Note 1 to amp"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Leftmost Center Note"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Note 1 to amp"
-          }
-        }
-      ]
-    }
-  },
-  "folder": null,
-  "choreoAuto": false
-}

src/main/deploy/pathplanner/paths/Center Sub to Note 1.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.3216735004898588,
+        "y": 5.55593454044051
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.8054356108712222,
+        "y": 6.010377735041184
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.5050384003901907,
+        "y": 6.829386194384925
+      },
+      "prevControl": {
+        "x": 2.241168158363992,
+        "y": 6.433580831345627
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -151.8214098900407,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 180.0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Center Sub to Note 2.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.2888463383764373,
-        "y": 6.642279723305886
+        "x": 1.3216735004898588,
+        "y": 5.541275082550166
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.2888463383764366,
-        "y": 6.642279723305886
+        "x": 1.7449183616315946,
+        "y": 5.55472336015897
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 3.159911049166827,
-        "y": 6.864468657712245
+        "x": 2.294543620426272,
+        "y": 5.541275082550166
       },
       "prevControl": {
-        "x": 2.159911049166827,
-        "y": 6.864468657712245
+        "x": 1.2945436204262721,
+        "y": 5.541275082550166
       },
       "nextControl": null,
       "isLocked": false,
@@ -32,20 +32,20 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 0,
+    "rotation": 180.0,
     "rotateFast": false
   },
   "reversed": false,
   "folder": null,
   "previewStartingState": {
-    "rotation": 21.801409486351982,
+    "rotation": 180.0,
     "velocity": 0
   },
   "useDefaultConstraints": true

src/main/deploy/pathplanner/paths/Center Subwoofer to Center.path

@@ -54,8 +54,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0
   },

src/main/deploy/pathplanner/paths/Gremlin Path.path

@@ -0,0 +1,123 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.8912450873334795,
+        "y": 4.4671669970120575
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.8912450873334783,
+        "y": 4.4671669970120575
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.013584323464943,
+        "y": 1.9763121007723505
+      },
+      "prevControl": {
+        "x": 1.9257282443058341,
+        "y": 3.278151773659794
+      },
+      "nextControl": {
+        "x": 5.007587451072338,
+        "y": 1.3565219153230341
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.56261040157408,
+        "y": 1.2278862164561943
+      },
+      "prevControl": {
+        "x": 8.317033158282841,
+        "y": 0.09355323553951994
+      },
+      "nextControl": {
+        "x": 8.686725702060071,
+        "y": 1.801180699653394
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.761411027095559,
+        "y": 6.478561561111727
+      },
+      "prevControl": {
+        "x": 8.843270108192637,
+        "y": 4.794603321400375
+      },
+      "nextControl": {
+        "x": 8.68370806922782,
+        "y": 8.077022408676678
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.30563859418317,
+        "y": 7.203599136543003
+      },
+      "prevControl": {
+        "x": 7.451665729542288,
+        "y": 7.051575128791285
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.7,
+      "rotationDegrees": -178.9636039256695,
+      "rotateFast": true
+    },
+    {
+      "waypointRelativePos": 2.15,
+      "rotationDegrees": 151.60505283278198,
+      "rotateFast": true
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "New Constraints Zone",
+      "minWaypointRelativePos": 1.7,
+      "maxWaypointRelativePos": 2.4,
+      "constraints": {
+        "maxVelocity": 3.0,
+        "maxAcceleration": 4.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0
+      }
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 64.23067237566114,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 132.61405596961126,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Gremlin Start.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.66,
+        "y": 4.38
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 0.8945513262455097,
+        "y": 3.688073587575746
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.9539344375942316,
+        "y": 4.503791885637482
+      },
+      "prevControl": {
+        "x": 1.3164532253098697,
+        "y": 4.117884789036965
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 133.4021151464655,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 119.88721357012417,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Just Backup.path

@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0
   },

src/main/deploy/pathplanner/paths/L Sub to Note 1.path

@@ -3,37 +3,43 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 2.353014392638472,
-        "y": 6.993104356579084
+        "x": 1.4408703461281565,
+        "y": 6.583808951093687
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.768306670516475,
-        "y": 7.004798511021523
+        "x": 1.8852482149408742,
+        "y": 7.121740055445923
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 1.8384715971711143,
-        "y": 7.624588696470841
+        "x": 2.7272273347965497,
+        "y": 6.876162812154685
       },
       "prevControl": {
-        "x": 1.7950792079196904,
-        "y": 7.315863019194643
+        "x": 2.259461157098952,
+        "y": 6.864468657712245
       },
       "nextControl": null,
       "isLocked": false,
       "linkedName": null
     }
   ],
-  "rotationTargets": [],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.45,
+      "rotationDegrees": 179.22679798216734,
+      "rotateFast": true
+    }
+  ],
   "constraintZones": [],
   "eventMarkers": [
     {
       "name": "New Event Marker",
-      "waypointRelativePos": 0,
+      "waypointRelativePos": 0.1,
       "command": {
         "type": "parallel",
         "data": {
@@ -43,20 +49,20 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0
   },
   "goalEndState": {
-    "velocity": 0.0,
-    "rotation": 90.0,
-    "rotateFast": true
+    "velocity": 0,
+    "rotation": 180.0,
+    "rotateFast": false
   },
   "reversed": false,
   "folder": null,
   "previewStartingState": {
-    "rotation": -178.36342295838335,
+    "rotation": -145.66978280449663,
     "velocity": 0
   },
   "useDefaultConstraints": true

src/main/deploy/pathplanner/paths/L Sub to Note 2.path

@@ -0,0 +1,58 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.5578118905525558,
+        "y": 6.4083966344570875
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.9671072960379536,
+        "y": 6.349925862244889
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.6687565625843503,
+        "y": 5.613194132371171
+      },
+      "prevControl": {
+        "x": 2.306237774868712,
+        "y": 5.999101228971689
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.7,
+      "rotationDegrees": 136.8500052941468,
+      "rotateFast": true
+    }
+  ],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 139.08561677997477,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": -148.523160650416,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Left Note 3 to Sub.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.2594611570989525,
+        "y": 4.560720232551577
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.130825458232113,
+        "y": 4.946627329152095
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.3707054194735169,
+        "y": 5.5196408968316515
+      },
+      "prevControl": {
+        "x": 1.8267774427286747,
+        "y": 5.285757807982852
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 144.46232220802574,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Left Preloaded+Pickup.path

@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0
   },

src/main/deploy/pathplanner/paths/Left Sub to Note 3.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.5812001994374358,
+        "y": 6.221290163378048
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.932024832710634,
+        "y": 5.70674736791069
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.2243786937716323,
+        "y": 4.151424827066179
+      },
+      "prevControl": {
+        "x": 0.7041386162544403,
+        "y": 4.350225452587658
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 171.86989764584402,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": -160.6410058243053,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Left Subwoofer.path

@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 1.148516485067158,
-        "y": 6.560420642208806
+        "x": 1.382399573915957,
+        "y": 5.964018765644369
       },
       "prevControl": {
-        "x": 1.5812001994374358,
-        "y": 6.864468657712245
+        "x": 1.8150832882862347,
+        "y": 6.268066781147808
       },
       "nextControl": null,
       "isLocked": false,
@@ -32,14 +32,14 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -146.30993247402017,
+    "rotation": -164.4071890607336,
     "rotateFast": false
   },
   "reversed": false,

src/main/deploy/pathplanner/paths/Leftmost Center Note.path

@@ -82,8 +82,8 @@
   ],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0
   },

src/main/deploy/pathplanner/paths/Note 1 to Center Sub.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.230559889691208,
+        "y": 6.640734424325992
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.8200950687615667,
+        "y": 5.863783156137741
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.3070140425995143,
+        "y": 5.570593998330854
+      },
+      "prevControl": {
+        "x": 1.8494139845422553,
+        "y": 5.629231829892232
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 180.0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Note 1 to Speaker.path

@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 1.2654580294915574,
-        "y": 6.291455090032687
+        "x": 1.3707054194735169,
+        "y": 5.940630456759489
       },
       "prevControl": {
-        "x": 1.7098358983042754,
-        "y": 6.525338178881486
+        "x": 1.8150832882862349,
+        "y": 6.1745135456082885
       },
       "nextControl": null,
       "isLocked": false,
@@ -32,14 +32,14 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -145.88552705465875,
+    "rotation": -165.25643716352937,
     "rotateFast": false
   },
   "reversed": false,

src/main/deploy/pathplanner/paths/Note 2 to Center Sub.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.503225806451614,
+        "y": 5.535411299394028
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.016531804492181,
+        "y": 5.541275082550166
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.3480605246924788,
+        "y": 5.535411299394028
+      },
+      "prevControl": {
+        "x": 1.6881599477484674,
+        "y": 5.55300264886244
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -178.53119928561404,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 180.0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Note 2 to L Sub (4S).path

@@ -0,0 +1,58 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.1776020760018726,
+        "y": 5.964018765644369
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.8267774427286747,
+        "y": 5.928936302317049
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.3005404928188775,
+        "y": 5.5430292057165325
+      },
+      "prevControl": {
+        "x": 1.674753434976956,
+        "y": 5.683359059025812
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5,
+      "rotationDegrees": -164.3011806576524,
+      "rotateFast": false
+    }
+  ],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 150.25511870305772,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Note 2 to L Sub.path

@@ -0,0 +1,58 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.1776020760018726,
+        "y": 5.964018765644369
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.9671072960379539,
+        "y": 6.209596008935608
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.2537638750491173,
+        "y": 6.385008325572207
+      },
+      "prevControl": {
+        "x": 1.6279768172071958,
+        "y": 6.525338178881487
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5,
+      "rotationDegrees": -164.3011806576524,
+      "rotateFast": false
+    }
+  ],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -152.10272896905224,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 150.25511870305772,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Note 3 to Sub.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.142519612674553,
+        "y": 4.163118981508619
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.756612516074035,
+        "y": 3.964318355987141
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.171904793952038,
+        "y": 4.701050085860856
+      },
+      "prevControl": {
+        "x": 1.8501657516135543,
+        "y": 3.9643183559871393
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 140.0131137550358,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 178.40885972880548,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Right Sub Note 3.path

@@ -0,0 +1,58 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.3005404928188775,
+        "y": 4.397002070357417
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.7683066705164747,
+        "y": 4.361919607030098
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.3647085470809115,
+        "y": 4.081259900411539
+      },
+      "prevControl": {
+        "x": 1.8029020887081402,
+        "y": 4.081259900411539
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.0,
+      "rotationDegrees": 180.0,
+      "rotateFast": true
+    }
+  ],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 131.18592516570968,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Right Sub from Center.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.335023775460655,
+        "y": 1.1577212898015548
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.803089543501023,
+        "y": 0.9472265098376356
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.0432690950851984,
+        "y": 4.478861151454497
+      },
+      "prevControl": {
+        "x": 2.013883913807713,
+        "y": 1.5436283864020721
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 143.130102354156,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 180.0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Right Sub to Right Center Note.path

@@ -0,0 +1,76 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.335622956146197,
+        "y": 4.315142989260338
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.8735540604984342,
+        "y": 1.4149926875352343
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.603689737294005,
+        "y": 0.8068966565283563
+      },
+      "prevControl": {
+        "x": 6.603689737294005,
+        "y": 0.8068966565283563
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.6,
+      "rotationDegrees": 180.0,
+      "rotateFast": false
+    }
+  ],
+  "constraintZones": [],
+  "eventMarkers": [
+    {
+      "name": "Note Pickup",
+      "waypointRelativePos": 0.6,
+      "command": {
+        "type": "parallel",
+        "data": {
+          "commands": [
+            {
+              "type": "named",
+              "data": {
+                "name": "Note Pickup"
+              }
+            }
+          ]
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 139.21417852273407,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Right Sub.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.52,
+        "y": 4.034483282641779
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.0783515584179242,
+        "y": 3.5199404871715854
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.2186814117217974,
+        "y": 4.677661776975976
+      },
+      "prevControl": {
+        "x": 1.9904956049228333,
+        "y": 4.174813135951059
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 143.9726266148963,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": -178.76146677965983,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Right Subwoofer to Center.path

@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 7.0095431619434665,
-        "y": 0.8355890997496291
+        "x": 7.639899851228273,
+        "y": 0.7769512681882514
       },
       "prevControl": {
-        "x": 6.0095431619434665,
-        "y": 0.8355890997496291
+        "x": 6.639899851228273,
+        "y": 0.7769512681882514
       },
       "nextControl": null,
       "isLocked": false,
@@ -32,14 +32,14 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 0,
+    "rotation": 180.0,
     "rotateFast": false
   },
   "reversed": false,

src/main/deploy/pathplanner/paths/Right Wall to Sub.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.4936438362905214,
+        "y": 4.092954054853979
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.359011265031077,
+        "y": 4.034483282641779
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.2186814117217974,
+        "y": 4.736132549188175
+      },
+      "prevControl": {
+        "x": 1.2303755661642373,
+        "y": 4.350225452587658
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 141.76617482255304,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 180.0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/java/frc/robot/Commands/AmpHandoff.java

@@ -1,13 +0,0 @@
-package frc.robot.Commands;
-
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import frc.robot.subsystems.Indexer;
-import frc.robot.subsystems.Shooter;
-
-public class AmpHandoff extends ParallelCommandGroup{
-
-    AmpHandoff(Indexer indexer, Shooter shooter){
-        //addCommands(indexer.shootNote(() -> 1), shooter.ampHandoff());
-    }
-    
-}

src/main/java/frc/robot/Commands/SpeakerShot.java

@@ -1,13 +0,0 @@
-package frc.robot.Commands;
-
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import frc.robot.constants.ShooterConstants;
-import frc.robot.subsystems.Indexer;
-import frc.robot.subsystems.Shooter;
-
-public class SpeakerShot extends ParallelCommandGroup{
-    
-    SpeakerShot(Indexer indexer, Shooter shooter){
-        //addCommands(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterLoadAngle, 0, 0), indexer.shootNote(() -> 1.0));
-    }
-}

src/main/java/frc/robot/RobotContainer.java

@@ -9,12 +9,16 @@ import java.util.Optional;
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
+import com.pathplanner.lib.util.PPLibTelemetry;
+import com.pathplanner.lib.util.PathPlannerLogging;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@@ -23,8 +27,13 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.PrintCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.Commands.autoIntaking;
+import frc.robot.constants.IntakeRollerConstants;
 import frc.robot.constants.OIConstants;
 import frc.robot.constants.PhotonConstants;
 import frc.robot.constants.ShooterConstants;
@@ -32,7 +41,7 @@ import frc.robot.subsystems.Climber;
 import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.Indexer;
 import frc.robot.utilities.PhotonVision;
-import frc.robot.subsystems.Intake;
+import frc.robot.subsystems.IntakeRoller;
 import frc.robot.subsystems.Shooter;
 
 public class RobotContainer {
@@ -42,7 +51,7 @@ public class RobotContainer {
     //private PhotonVision photonvision;
 
     private Drivetrain drivetrain;
-    private Intake intake;
+    private IntakeRoller intake;
     private Shooter shooter;
     private Climber climber;
     private Indexer indexer;
@@ -51,6 +60,8 @@ public class RobotContainer {
     private CommandXboxController operator;
 
     private Trigger isTeleopTrigger;
+    private Trigger isEnabledTrigger;
+    private Trigger indexerBeamBreakWatcher;
 
     private SendableChooser<Command> autoChooser;
 
@@ -63,6 +74,8 @@ public class RobotContainer {
     private int speakerTag;
 
     private boolean setAmpAngle;
+    private boolean intakeDown;
+    private boolean invertXYDrive;
 
     public RobotContainer() {
         /*try {
@@ -82,7 +95,7 @@ public class RobotContainer {
         // TODO Need to provide a real VisualPoseProvider, null means we're not using one at all
         drivetrain = new Drivetrain(new Pose2d(), null, null);
 
-        intake = new Intake();
+        intake = new IntakeRoller();
 
         indexer = new Indexer();
 
@@ -90,13 +103,81 @@ public class RobotContainer {
 
         climber = new Climber(shooter::getShooterAngle);
 
-        NamedCommands.registerCommand("Charge Shooter 2 Sec", shooter.angleSpeedsSetpoints(() -> ShooterConstants.kShooterLoadAngle, 1.0, 1.0).withTimeout(2.0));
-        NamedCommands.registerCommand("Speaker Note Shot", Commands.parallel(shooter.angleSpeedsSetpoints(() -> ShooterConstants.kShooterLoadAngle, 1.0, 1.0), indexer.shootNote(() -> 1.0)).withTimeout(2.0));
+        NamedCommands.registerCommand(
+            "Charge Shooter 2 Sec", 
+            shooter.angleSpeedsSetpoints(
+                () -> ShooterConstants.kShooterLoadAngle, 
+                1.0, 
+                1.0
+            ).withTimeout(2.0)
+        );
+
+        /*
+        NamedCommands.registerCommand(
+            "Speaker Note Shot", 
+            Commands.parallel(
+                shooter.angleSpeedsSetpoints(
+                    () -> ShooterConstants.kShooterLoadAngle, 
+                    1.0, 
+                    1.0
+                ), 
+                indexer.shootNote(() -> 1.0)
+            ).withTimeout(1.0)
+        );
+        */
+
+        NamedCommands.registerCommand(
+            "Speaker Note Shot", 
+            shooter.angleSpeedsSetpoints(
+                () -> ShooterConstants.kShooterLoadAngle, 
+                1.0, 
+                1.0
+            ).raceWith(
+                new WaitCommand(1.0)
+            ).andThen(
+                shooter.angleSpeedsSetpoints(
+                    () -> ShooterConstants.kShooterLoadAngle, 
+                    1.0, 
+                    1.0
+                ).alongWith(indexer.shootNote(() -> 1.0))
+                .withTimeout(0.75)
+            )
+        );
+
+        /* 
+        NamedCommands.registerCommand(
+            "Auto Intake", 
+            Commands.parallel(
+                intake.intakeControl(
+                    () -> IntakeConstants.kDownAngle, 
+                    () -> 1.0
+                ), 
+                indexer.advanceNote()
+            ).until(() -> !indexer.getBeamBreak())
+            .withTimeout(1.0)
+        );*/
+
+        NamedCommands.registerCommand(
+            "Auto Intake", 
+            intake.intakeControl(
+                () -> IntakeRollerConstants.kDownAngle, 
+                () -> 1.0
+            ).alongWith(
+                indexer.advanceNote()
+            ).until(() -> !indexer.getBeamBreak())
+            .andThen(
+                intake.stopAll()
+            )
+            .withTimeout(2.5)
+        );
+
+        //NamedCommands.registerCommand("Auto Intake", new PrintCommand("Intake Note"));
 
         // An example Named Command, doesn't need to remain once we start actually adding real things
         // ALL Named Commands need to be defined AFTER subsystem initialization and BEFORE auto/controller configuration
         NamedCommands.registerCommand("Set Drivetrain X", drivetrain.setXCommand());
 
+
         // TODO Specify a default auto string once we have one
         autoChooser = AutoBuilder.buildAutoChooser();
 
@@ -104,20 +185,11 @@ public class RobotContainer {
         operator = new CommandXboxController(OIConstants.kSecondaryXboxUSB);
 
         isTeleopTrigger = new Trigger(DriverStation::isTeleopEnabled);
-
-        Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
-
-        if (alliance.isEmpty() || alliance.get() == DriverStation.Alliance.Red) {
-            ampTagID = 5;
-            sourceTagID = 9;
-            speakerTag = 4; 
-        } else {
-            ampTagID = 6;
-            sourceTagID = 1;
-            speakerTag = 8;
-        }
+        isEnabledTrigger = new Trigger(DriverStation::isEnabled);
+        indexerBeamBreakWatcher = new Trigger(indexer::getBeamBreak);
 
         setAmpAngle = false;
+        intakeDown = false;
 
         configureBindings();
         shuffleboardSetup();
@@ -132,17 +204,71 @@ public class RobotContainer {
     private void configureBindings() {
         isTeleopTrigger.onTrue(new InstantCommand(() -> Shuffleboard.selectTab(kTeleopTabName)));
 
+        isEnabledTrigger.onTrue(
+            new InstantCommand(
+                () -> {
+                    Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
+
+                    if (alliance.isEmpty() || alliance.get() == DriverStation.Alliance.Red) {
+                        ampTagID = 5;
+                        sourceTagID = 9;
+                        speakerTag = 4;
+                        invertXYDrive = true;
+                    } else {
+                        ampTagID = 6;
+                        sourceTagID = 1;
+                        speakerTag = 8;
+                        invertXYDrive = false;
+                    }
+                }
+            )
+        );
+
         drivetrain.setDefaultCommand(
             drivetrain.teleopCommand(
-                driver::getLeftY, 
-                driver::getLeftX, 
-                () -> { return -driver.getRightX(); }, 
+            () -> { return driver.getLeftY();  /*(invertXYDrive ? -1 : 1);*/ }, 
+            () -> { return driver.getLeftX(); /*(invertXYDrive ? -1 : 1);*/ }, 
+            () -> { return driver.getRightX(); /*(invertXYDrive ? -1 : 1);*/ }, 
                 OIConstants.kTeleopDriveDeadband
             )
         );
 
-        //intake.setDefaultCommand(intake.intakeUpCommand());
-        intake.setDefaultCommand(intake.manualPivot(operator::getLeftY, () -> 0.0));
+        intake.setDefaultCommand(
+            intake.intakeControl(
+                () -> {
+                    if (intakeDown && indexer.getBeamBreak()) {
+                        return IntakeRollerConstants.kDownAngle;
+                    } else {
+                        return IntakeRollerConstants.kUpAngle;
+                    }
+                }, 
+                () -> {
+                    if(intakeDown && indexer.getBeamBreak()){
+                        return 1.0;
+                    }else if(operator.leftBumper().getAsBoolean()){
+                        return -1.0;
+                    }else{
+                        return 0.0;
+                    }
+                }
+            )
+        );
+
+        driver.leftTrigger().whileFalse(
+            new InstantCommand(
+                () -> {
+                    intakeDown = false;
+                }
+            )
+        );
+
+        driver.leftTrigger().whileTrue(Commands.parallel(
+            new InstantCommand(
+                () -> {
+                    intakeDown = true;
+                }
+            ), indexer.advanceNote()
+        ));
 
         shooter.setDefaultCommand(
             shooter.angleSpeedsSetpoints(
@@ -154,30 +280,54 @@ public class RobotContainer {
                     }
                 },
                 () -> {
-                    if (driver.getLeftTriggerAxis() > .25) {
-                        return -1.0;
-                    }else if(driver.getRightTriggerAxis() > 0.25){
+                    if(driver.getRightTriggerAxis() > 0.25){
                         return 1.0;
-                    } else {
-                        return 0;
+                    } else if(operator.getRightTriggerAxis() > 0.25){
+                        return -1.0;
+                    }else{
+                        return 0.0;
                     }
                 }
             )
         );
-        //shooter.setDefaultCommand(shooter.manualPivot(operator::getRightY, operator::getLeftTriggerAxis));
+
+        driver.leftBumper().onTrue(
+            shooter.angleSpeedsSetpoints(
+                () -> ShooterConstants.kShooterLoadAngle, 
+                () -> 1.0
+            ).withTimeout(.5).andThen(
+                shooter.angleSpeedsSetpoints(
+                    () -> ShooterConstants.kShooterLoadAngle, 
+                    () -> 1.0
+                ).alongWith(
+                    indexer.shootNote(() -> 1.0)
+                ).withTimeout(2)
+            )
+        );
 
         climber.setDefaultCommand(climber.stop());
 
         indexer.setDefaultCommand(indexer.shootNote(
             () -> {
-                if (driver.getLeftTriggerAxis() > .25) {
-                        return -1.0;
+                if (driver.getRightTriggerAxis() > .25) {
+                        return 1.0;
                     }else {
                         return 0.0;
                     }
             }
         ));
 
+        indexerBeamBreakWatcher.onFalse(
+            new RunCommand(() -> {
+                driver.getHID().setRumble(RumbleType.kBothRumble, 1);
+            }).withTimeout(.5).andThen(
+                new InstantCommand(() -> {
+                    driver.getHID().setRumble(RumbleType.kBothRumble, 0);
+                })
+            )
+        );
+
+        /*
         driver.povCenter().onFalse(
             drivetrain.driveCardinal(
                 driver::getLeftY, 
@@ -222,7 +372,7 @@ public class RobotContainer {
             ).until(
                 () -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
             )
-        );
+        ); */
 
         // This was originally a run while held, not sure that's really necessary, change it if need be
         driver.y().onTrue(drivetrain.zeroHeadingCommand());
@@ -230,6 +380,8 @@ public class RobotContainer {
         // This was originally a run while held, not sure that's really necessary, change it if need be
         driver.rightBumper().onTrue(drivetrain.setXCommand());
 
+        operator.rightTrigger().whileTrue(indexer.shootNote(() -> -1.0));
+
         /*
         * This has been added because interest has been expressed in trying field relative vs
         * robot relative control. This should <i>default</i> to field relative, but give the option
@@ -257,18 +409,20 @@ public class RobotContainer {
         
         //operator.a().whileTrue(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterAmpAngle, 0, 0));
 
-        operator.y().whileTrue(Commands.parallel(climber.setSpeedWithSupplier(operator::getRightTriggerAxis), shooter.climbState()));
+        operator.y().toggleOnTrue(Commands.parallel(climber.setSpeedWithSupplier(operator::getRightTriggerAxis), shooter.climbState(), intake.climbingStateCommand()));
 
         driver.a().whileTrue(indexer.shootNote(() -> 1.0));
 
         operator.back().toggleOnTrue(shooter.manualPivot(
-        () -> MathUtil.clamp(-operator.getLeftY(), -0.75, 0.75),
-        () -> MathUtil.clamp(operator.getRightTriggerAxis()-operator.getLeftTriggerAxis(), -0.75, 0.75)
+        () -> MathUtil.clamp(-operator.getLeftY(), -0.25, 0.25),
+        () -> MathUtil.clamp(driver.getRightTriggerAxis(), -1, 1)
         ));
 
         driver.povDown().whileTrue(indexer.shootNote(() -> -1.0));
 
-        operator.start().onTrue(shooter.zeroEncoder());
+        operator.leftTrigger().whileTrue(Commands.parallel( indexer.shootNote(() -> -1), intake.intakeControl(() -> IntakeRollerConstants.kDownAngle, () -> -1.0)));
+
+        operator.start().toggleOnTrue(intake.manualPivot(() -> -operator.getRightY()*0.2, () -> driver.getLeftTriggerAxis()));
         
   }
 
@@ -292,7 +446,10 @@ public class RobotContainer {
     teleopTab.addBoolean("indexer beam break", indexer::getBeamBreak);
     teleopTab.addBoolean("shooter beam break", shooter::getBeamBreak);
     teleopTab.addDouble("shooter angle", shooter::getShooterAngle);
-    teleopTab.addDouble("intake angle", intake::getIntakeAngle);
+    teleopTab.addDouble("intake angle", intake::getIntakeDegrees);
+    //teleopTab.addDouble("intake pid", intake::getIntakePID);
+    teleopTab.addDouble("heading swerve", drivetrain::getHeading);
+    teleopTab.addDouble("steering encoder", drivetrain::getEncoderSteering);
   }
 
   public Command getAutonomousCommand() {

src/main/java/frc/robot/constants/AutoConstants.java

@@ -8,14 +8,14 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
 
 public final class AutoConstants {
-    public static final double kMaxSpeedMetersPerSecond = 3.895; // max capable = 4.6
-    public static final double kMaxAccelerationMetersPerSecondSquared = 3; // max capable = 7.4
+    public static final double kMaxSpeedMetersPerSecond = 4; // max capable = 4.6
+    public static final double kMaxAccelerationMetersPerSecondSquared = 4; // max capable = 7.4
     public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
     public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
 
-    public static final double kPXController = 1.05;
-    public static final double kPYController = 1.05;
-    public static final double kPThetaController = 0.95; // needs to be separate from heading control
+    public static final double kPXController = 5.5;//5;
+    public static final double kPYController = 5.5;//5;
+    public static final double kPThetaController = 5;//0.5; // needs to be separate from heading control
 
     public static final double kPHeadingController = 0.02; // for heading control NOT PATHING
     public static final double kDHeadingController = 0.0025;
@@ -24,7 +24,7 @@ public final class AutoConstants {
         new PIDConstants(kPXController), 
         new PIDConstants(kPThetaController), 
         kMaxSpeedMetersPerSecond, 
-        Units.inchesToMeters(Math.sqrt(Math.pow(14-17.5, 2)+ Math.pow(14-1.75, 2))), 
+        Units.inchesToMeters(Math.sqrt(Math.pow(14-1.75, 2)+ Math.pow(14-1.75, 2))), 
         new ReplanningConfig()
     );
 

src/main/java/frc/robot/constants/ClimberConstants.java

@@ -1,6 +1,6 @@
 package frc.robot.constants;
 
 public class ClimberConstants {
-    public static final int kClimberMotor1CANID = 14;
-    public static final int kClimberMotor2CANID = 15;
+    public static final int kMotor1CANID = 14;
+    public static final int kMotor2CANID = 15;
 }

src/main/java/frc/robot/constants/DrivetrainConstants.java

@@ -7,7 +7,7 @@ import edu.wpi.first.math.util.Units;
 public final class DrivetrainConstants {
     // Driving Parameters - Note that these are not the maximum capable speeds of
     // the robot, rather the allowed maximum speeds
-    public static final double kMaxSpeedMetersPerSecond = 4.1;
+    public static final double kMaxSpeedMetersPerSecond = 5.2;
     public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second
 
     public static final double kDirectionSlewRate = 4.8; // radians per second
@@ -26,21 +26,29 @@ public final class DrivetrainConstants {
         new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
 
     // Angular offsets of the modules relative to the chassis in radians
-    public static final double kFrontLeftChassisAngularOffset = Math.PI;
-    public static final double kFrontRightChassisAngularOffset = -Math.PI / 2;
-    public static final double kBackLeftChassisAngularOffset = Math.PI / 2;
-    public static final double kBackRightChassisAngularOffset = 0;
+    
+    public static final double kFrontLeftChassisAngularOffset = 0.0;// Math.PI
+    public static final double kFrontRightChassisAngularOffset = Math.PI / 2;//-Math.PI / 2;
+    public static final double kBackLeftChassisAngularOffset = -Math.PI/2;//Math.PI / 2;
+    public static final double kBackRightChassisAngularOffset = Math.PI; //0;
+
+    /*
+    public static final double kFrontLeftChassisAngularOffset = -2*Math.PI;
+    public static final double kFrontRightChassisAngularOffset = -2*Math.PI;
+    public static final double kBackLeftChassisAngularOffset = -2*Math.PI;
+    public static final double kBackRightChassisAngularOffset = -2*Math.PI-(4/180.0);
+    */
 
     // SPARK MAX CAN IDs
-    public static final int kFrontLeftDrivingCanId = 8;
-    public static final int kRearLeftDrivingCanId = 3;
-    public static final int kFrontRightDrivingCanId = 6;
-    public static final int kRearRightDrivingCanId = 1;
+    public static final int kFrontLeftDrivingCanId = 1;//8
+    public static final int kRearLeftDrivingCanId = 6;//3
+    public static final int kFrontRightDrivingCanId = 3;//6
+    public static final int kRearRightDrivingCanId = 8;//1
 
-    public static final int kFrontLeftTurningCanId = 4;
-    public static final int kRearLeftTurningCanId = 7;
-    public static final int kFrontRightTurningCanId = 2;
-    public static final int kRearRightTurningCanId = 5;
+    public static final int kFrontLeftTurningCanId = 5;//4
+    public static final int kRearLeftTurningCanId = 2;//7
+    public static final int kFrontRightTurningCanId = 7;//2
+    public static final int kRearRightTurningCanId = 4;//5
 
     public static final double kTurnToleranceDeg = 0;
     public static final double kTurnRateToleranceDegPerS = 0;

src/main/java/frc/robot/constants/HoodConstants.java

@@ -0,0 +1,42 @@
+package frc.robot.constants;
+
+public class HoodConstants {
+    public enum HoodPositions {
+        kLoadAngle(Math.toRadians(-20.0)),
+        KAmpAngle(Math.toRadians(105.0));
+
+        private double angle;
+
+        private HoodPositions(double angle) {
+            this.angle = angle;
+        }
+
+        public double getAngle() {
+            return angle;
+        }
+    }
+
+
+    public static final int kMotorCANID = 16;
+
+    public static final double kP = 3.0;
+    public static final double kI = 0.0;
+    public static final double kD = 0.0;
+
+    public static final double kLoadAngle = Math.toRadians(-20.0);
+    public static final double KAmpAngle = Math.toRadians(105.0);
+
+    public static final double kEncoderStepConversion = 1/(40.0*(28.0/15.0));
+
+    public static final double kSFF = 0.0;
+    public static final double kGFF = 0.33;
+    public static final double kVFF = 1.44;
+
+    public static final double kMaxVelocity = 0.0;
+    public static final double kMaxAcceleration = 0.0;
+
+    public static final int kMotorCurrentLimit = 50;
+
+    public static final int kEncoderChannelA = 0;
+    public static final int kEncoderChannelB = 1;
+}

src/main/java/frc/robot/constants/IndexerConstants.java

@@ -1,7 +1,9 @@
 package frc.robot.constants;
 
 public class IndexerConstants {
-    public static final int kIndexerID = 13;
+    public static final int kMotorCANID = 13;
 
-    public static final int kIndexerBeamBreakChannel = 2;
+    public static final int kMotorCurrentLimit = 40;
+
+    public static final int kBeamBreakChannel = 2;
 }

src/main/java/frc/robot/constants/IntakeConstants.java

@@ -1,22 +0,0 @@
-package frc.robot.constants;
-
-public class IntakeConstants {
-    public static final int kIntakePivotID = 10;
-    public static final int kIntakeRollerID = 12;
-
-    public static final double kIntakePivotConversionFactor = 1/(20.0*(28.0/15.0));
-
-    public static final int kPivotCurrentLimit = 40;
-
-    public static final double kPIntake = 0;
-    public static final double kIIntake = 0;
-    public static final double kDIntake = 0;
-
-    public static final double kSFeedForward = 0;
-    public static final double kGFeedForward = 1.11;
-    public static final double kVFeedForward = 0.73;
-
-    public static final double kStartingAngle = Math.toRadians(105.0);
-    public static final double kUpAngle = Math.toRadians(90.0);
-    public static final double kDownAngle = Math.toRadians(-34.0);
-}

src/main/java/frc/robot/constants/IntakePivotConstants.java

@@ -0,0 +1,21 @@
+package frc.robot.constants;
+
+public class IntakePivotConstants {
+    public static final int kIntakePivotID = 10;
+
+    public static final double kIntakePivotConversionFactor = (20.0*(28.0/15.0));
+
+    public static final int kPivotCurrentLimit = 40;
+
+    public static final double kPIntake = 3;//4; //2.5;
+    public static final double kIIntake = 0;
+    public static final double kDIntake = 0.01;
+
+    public static final double kSFeedForward = 0;
+    public static final double kGFeedForward = 1;//1.11;
+    public static final double kVFeedForward = .5;//0.73;
+
+    public static final double kStartingAngle = Math.toRadians(95.0);
+    public static final double kUpAngle = Math.toRadians(90.0);
+    public static final double kDownAngle = Math.toRadians(-34.0);
+}

src/main/java/frc/robot/constants/IntakeRollerConstants.java

@@ -0,0 +1,7 @@
+package frc.robot.constants;
+
+public class IntakeRollerConstants {
+    public static final int kMotorCANID = 12;
+
+    public static final int kMotorCurrentLimit = 60;
+}

src/main/java/frc/robot/constants/ShooterConstants.java

@@ -1,34 +1,10 @@
 package frc.robot.constants;
 
 public class ShooterConstants {
-    
-    public static final int kTopShooterID = 9;
-    public static final int kBottomShooterID = 11;
-    public static final int kShooterPivotID = 16;
+    public static final int kTopMotorCANID = 9;
+    public static final int kBottomMotorCANID = 11;
 
-    public static final double kShooterP = 0.0;
-    public static final double kShooterI = 0.0;
-    public static final double kShooterD = 0.0;
+    public static final int kMotorCurrentLimit = 40;
 
-    public static final double kShooterFF = 0.0;
-
-    public static final double kShooterPivotP = 3.0;
-    public static final double kShooterPivotI = 0.0;
-    public static final double kShooterPivotD = 0.0;
-
-    public static final double kShooterLoadAngle = Math.toRadians(-20.0);
-    public static final double kShooterAmpAngle = Math.toRadians(105.0);
-
-    public static final double kPivotConversion = 1/(40.0*(28.0/15.0));
-
-    public static final double kSShooterPivotFF = 0.0;
-    public static final double kGShooterPivotFF = 0.33;
-    public static final double kVShooterPivotFF = 1.44;
-
-    public static final double kMaxPivotSpeed = 0.0;
-    public static final double kMaxPivotAcceleration = 0.0;
-
-    public static final int kShooterEncoderChannelA = 0;
-    public static final int kShooterEncoderChannelB = 1;
-    public static final int kShooterBeamBreakChannel = 3;
+    public static final int kBeamBreakChannel = 3;
 }

src/main/java/frc/robot/constants/SwerveModuleConstants.java

@@ -16,8 +16,8 @@ public class SwerveModuleConstants {
     public static final double kDrivingMotorFreeSpeedRps = NeoMotorConstants.kFreeSpeedRpm / 60;
     public static final double kWheelDiameterMeters = 0.0762;
     public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI;
-    // 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 teeth on the bevel pinion
-    public static final double kDrivingMotorReduction = (45.0 * 22) / (kDrivingMotorPinionTeeth * 15);
+    // 45 teeth on the wheel's bevel gear, 20 teeth on the first-stage spur gear, 15 teeth on the bevel pinion
+    public static final double kDrivingMotorReduction = (45.0 * 20) / (kDrivingMotorPinionTeeth * 15);
     public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters)
         / kDrivingMotorReduction;
 
@@ -49,6 +49,6 @@ public class SwerveModuleConstants {
     public static final IdleMode kDrivingMotorIdleMode = IdleMode.kBrake;
     public static final IdleMode kTurningMotorIdleMode = IdleMode.kBrake;
 
-    public static final int kDrivingMotorCurrentLimit = 65; // amps
+    public static final int kDrivingMotorCurrentLimit = 70; // amps
     public static final int kTurningMotorCurrentLimit = 30; // amps
 }

src/main/java/frc/robot/subsystems/Climber.java

@@ -8,30 +8,23 @@ import com.ctre.phoenix.motorcontrol.can.VictorSPX;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ClimberConstants;
-import frc.robot.constants.ShooterConstants;
 
 public class Climber extends SubsystemBase {
     private VictorSPX motor1;
     private VictorSPX motor2;
 
-    private DoubleSupplier shooterAngle;
-
-    public Climber(DoubleSupplier shooterAngle) {
-        motor1 = new VictorSPX(ClimberConstants.kClimberMotor1CANID);
-        motor2 = new VictorSPX(ClimberConstants.kClimberMotor2CANID);
+    public Climber() {
+        motor1 = new VictorSPX(ClimberConstants.kMotor1CANID);
+        motor2 = new VictorSPX(ClimberConstants.kMotor2CANID);
         
-
         motor2.follow(motor1);
         motor1.setInverted(true);
         motor2.setInverted(true);
-        this.shooterAngle = shooterAngle;
     }
 
     public Command setSpeedWithSupplier(DoubleSupplier speed) {
         return run(() -> {
-
             motor1.set(VictorSPXControlMode.PercentOutput, speed.getAsDouble());
-            
         });
     }
 

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -43,6 +43,7 @@ public class Drivetrain extends SubsystemBase {
 
     // The gyro sensor
     private final AHRS m_gyro;
+    //private final ADXRS450_Gyro m_gyro;
 
     private final SlewRateLimiter m_magLimiter;
     private final SlewRateLimiter m_rotLimiter;
@@ -92,6 +93,8 @@ public class Drivetrain extends SubsystemBase {
 
         // TODO The NavX is not centered in the robot, it's output probably needs to be translated
         m_gyro = new AHRS(SPI.Port.kMXP);
+       // m_gyro = new ADXRS450_Gyro();
+        //m_gyro.calibrate();
 
         m_magLimiter = new SlewRateLimiter(DrivetrainConstants.kMagnitudeSlewRate);
         m_rotLimiter = new SlewRateLimiter(DrivetrainConstants.kRotationalSlewRate);
@@ -202,6 +205,8 @@ public class Drivetrain extends SubsystemBase {
      * @param pose The pose to which to set the odometry.
      */
     public void resetOdometry(Pose2d pose) {
+        m_gyro.setAngleAdjustment(pose.getRotation().getDegrees());
+
         m_poseEstimator.resetPosition(
             Rotation2d.fromDegrees(getGyroAngle()),
             new SwerveModulePosition[] {
@@ -299,12 +304,21 @@ public class Drivetrain extends SubsystemBase {
     }
 
     public void driveWithChassisSpeeds(ChassisSpeeds speeds) {
+        //speeds.omegaRadiansPerSecond = -speeds.omegaRadiansPerSecond;
+        ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.2);
+        SwerveModuleState[] newStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(discreteSpeeds);
+        SwerveDriveKinematics.desaturateWheelSpeeds(newStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
+        setModuleStates(newStates);
+        
+        /*
+        //speeds.omegaRadiansPerSecond = -speeds.omegaRadiansPerSecond;
         SwerveModuleState[] states = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(speeds);
 
         m_frontLeft.setDesiredState(states[0]);
         m_frontRight.setDesiredState(states[1]);
         m_rearLeft.setDesiredState(states[2]);
         m_rearRight.setDesiredState(states[3]);
+        */
     }
 
     public Command teleopCommand(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rotation, double deadband) {
@@ -419,7 +433,17 @@ public class Drivetrain extends SubsystemBase {
 
     /** Zeroes the heading of the robot. */
     public void zeroHeading() {
-        m_gyro.reset();
+        Pose2d tempPose = m_poseEstimator.getEstimatedPosition();
+
+        Rotation2d tempRotation = Rotation2d.fromDegrees(0);
+
+        resetOdometry(
+            new Pose2d(
+                tempPose.getX(), 
+                tempPose.getY(), 
+                tempRotation
+            )
+        );
     }
 
     public Command zeroHeadingCommand() {
@@ -451,4 +475,8 @@ public class Drivetrain extends SubsystemBase {
     public double getTurnRate() {
         return m_gyro.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
     }
+
+    public double getEncoderSteering(){
+        return m_frontLeft.steeringEncoder();
+    }
 }

src/main/java/frc/robot/subsystems/Hood.java

@@ -0,0 +1,109 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.CANSparkLowLevel.MotorType;
+
+import java.util.function.DoubleSupplier;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkBase.IdleMode;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.HoodConstants;
+import frc.robot.constants.HoodConstants.HoodPositions;
+
+public class Hood extends SubsystemBase {
+    private CANSparkMax shooterPivot;
+
+    private Encoder pivotEncoder;
+
+    private PIDController shooterPivotPID;
+    private ArmFeedforward shooterPivotFF;
+
+    private HoodPositions targetPosition;
+
+    private boolean pidControl;
+
+    public Hood() {
+        shooterPivot = new CANSparkMax(HoodConstants.kMotorCANID, MotorType.kBrushless);
+        shooterPivot.setInverted(true);
+
+        shooterPivot.setSmartCurrentLimit(HoodConstants.kMotorCurrentLimit);
+
+        pivotEncoder = new Encoder(0, 1);
+        pivotEncoder.setDistancePerPulse(HoodConstants.kEncoderStepConversion);
+
+        shooterPivot.burnFlash();
+
+        shooterPivotPID = new PIDController(
+            HoodConstants.kP,
+            HoodConstants.kI,
+            HoodConstants.kD
+        );
+
+        shooterPivotFF = new ArmFeedforward(
+            HoodConstants.kSFF,
+            HoodConstants.kGFF,
+            HoodConstants.kVFF
+        );
+
+        shooterPivot.setIdleMode(IdleMode.kBrake);
+
+        targetPosition = HoodPositions.kLoadAngle;
+
+        pidControl = true;
+    }
+
+    @Override
+    public void periodic() {
+        if(pidControl) {
+            shooterPivot.setVoltage(
+                shooterPivotPID.calculate(
+                    getShooterAngle(),
+                    targetPosition.getAngle()
+                ) + shooterPivotFF.calculate(
+                    targetPosition.getAngle(), 
+                    0.0
+                )
+            );
+        }
+    }
+
+    public Command setHoodAngle(HoodPositions newPosition) {
+        return runOnce(() -> {
+            pidControl = true;
+
+            targetPosition = newPosition;
+        });
+    }
+
+    public Command setSpeed(DoubleSupplier speed) {
+        return runOnce(() -> { pidControl = false; }).andThen(run(() -> {
+            shooterPivot.set(speed.getAsDouble());
+        }));
+    }
+
+    public Command climbState() {
+        return runOnce(() -> {
+            pidControl = false;
+            shooterPivot.setIdleMode(IdleMode.kCoast);
+        }).andThen(run(() -> {
+            shooterPivot.set(0);
+        })).finallyDo(() -> {
+            // finallyDo will reset break mode if we back out of climbing and interrupt
+            // the climbState command with something else
+            shooterPivot.setIdleMode(IdleMode.kBrake);
+        });
+    }
+
+    public Command stop() {
+        return setSpeed(() -> 0);
+    }
+
+    public double getShooterAngle() {
+        return pivotEncoder.getDistance() + HoodPositions.kLoadAngle.getAngle();
+    }
+}

src/main/java/frc/robot/subsystems/Indexer.java

@@ -13,48 +13,52 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.IndexerConstants;
 
 public class Indexer extends SubsystemBase{
-    
     private CANSparkMax indexerMotor;
+
     private DigitalInput indexerBeamBreak;
 
-    public Indexer(){
-        indexerMotor = new CANSparkMax(IndexerConstants.kIndexerID, MotorType.kBrushed);
+    private boolean autoIndex;
 
-        indexerMotor.setSmartCurrentLimit(40);
+    public Indexer(){
+        indexerMotor = new CANSparkMax(IndexerConstants.kMotorCANID, MotorType.kBrushed);
+
+        indexerMotor.setSmartCurrentLimit(IndexerConstants.kMotorCurrentLimit);
         indexerMotor.setIdleMode(IdleMode.kBrake);
         indexerMotor.burnFlash();
 
-        indexerBeamBreak = new DigitalInput(IndexerConstants.kIndexerBeamBreakChannel);
+        indexerBeamBreak = new DigitalInput(IndexerConstants.kBeamBreakChannel);
+
+        autoIndex = false;
     }
 
-    public Command autoIndexing(){
-     return run(() -> {
-        if(!indexerBeamBreak.get()){
-            indexerMotor.set(0.75);
-        }else if(indexerBeamBreak.get()){
-            indexerMotor.set(0.0);
-        }
-
-     });
-    }
-
-    public Command advanceNote(){
-        return run(() -> {
-            if(indexerBeamBreak.get()){
-                indexerMotor.set(0.75);
-            }else{
-                indexerMotor.set(0.75);
+    @Override
+    public void periodic() {
+        if(autoIndex) {
+            if(!indexerBeamBreak.get()) {
+                indexerMotor.set(.75);
+            } else {
+                indexerMotor.set(0);
             }
-        });
-    }
-    
-    public Command shootNote(DoubleSupplier indexerSpeed){
-        return run(() -> {
-            indexerMotor.set(indexerSpeed.getAsDouble());
-        });
+        }
     }
 
     public boolean getBeamBreak(){
         return indexerBeamBreak.get();
     }
+
+    public Command autoIndex() {
+        return runOnce(() -> {
+            this.autoIndex = true;
+        });
+    }
+    
+    public Command setSpeed(DoubleSupplier speed){
+        return runOnce(() -> { autoIndex = false; }).andThen(run(() -> {
+            indexerMotor.set(speed.getAsDouble());
+        }));
+    }
+
+    public Command stop() {
+        return setSpeed(() -> 0);
+    }
 }

src/main/java/frc/robot/subsystems/Intake.java

@@ -1,115 +0,0 @@
-package frc.robot.subsystems;
-
-import java.util.function.DoubleSupplier;
-
-import com.revrobotics.CANSparkMax;
-import com.revrobotics.RelativeEncoder;
-import com.revrobotics.CANSparkBase.IdleMode;
-import com.revrobotics.CANSparkLowLevel.MotorType;
-
-import edu.wpi.first.math.controller.ArmFeedforward;
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.IntakeConstants;
-
-public class Intake extends SubsystemBase{
-    private PIDController intakeAnglePID;
-
-    private CANSparkMax intakeRoller;
-    private CANSparkMax intakePivot;
-
-    private RelativeEncoder intakeEncoder;
-
-    private ArmFeedforward intakeFeedForward;
-
-    public Intake(){
-        intakeRoller = new CANSparkMax(IntakeConstants.kIntakeRollerID, MotorType.kBrushless);
-        
-        intakeRoller.setSmartCurrentLimit(60);
-        intakeRoller.setIdleMode(IdleMode.kBrake);
-        intakeRoller.burnFlash();
-
-        intakePivot = new CANSparkMax(IntakeConstants.kIntakePivotID, MotorType.kBrushless);
-
-        intakePivot.setIdleMode(IdleMode.kBrake);
-        intakePivot.setSmartCurrentLimit(IntakeConstants.kPivotCurrentLimit);
-        intakePivot.burnFlash();
-
-        intakeFeedForward = new ArmFeedforward(
-            IntakeConstants.kSFeedForward, 
-            IntakeConstants.kGFeedForward, 
-            IntakeConstants.kVFeedForward
-        );
-
-        intakeEncoder = intakePivot.getEncoder();
-        intakeEncoder.setPosition(IntakeConstants.kStartingAngle);
-        intakeEncoder.setPositionConversionFactor(IntakeConstants.kIntakePivotConversionFactor);
-
-        intakeAnglePID = new PIDController(
-            IntakeConstants.kPIntake, 
-            IntakeConstants.kIIntake, 
-            IntakeConstants.kDIntake
-        );
-
-    }
-
-    public Command intakeDownCommand() {
-        return run(() -> {
-            intakeRoller.set(1.0);
-        
-            intakePivot.setVoltage(
-                intakeAnglePID.calculate(
-                    intakeEncoder.getPosition(), 
-                    IntakeConstants.kDownAngle
-                ) + intakeFeedForward.calculate(
-                    IntakeConstants.kDownAngle, 
-                    intakeEncoder.getVelocity()
-                )
-            );
-        });
-    }
-
-    public Command manualPivot(DoubleSupplier pivotPower, DoubleSupplier rollerSpinny){
-        return run(() ->{
-            intakePivot.set(pivotPower.getAsDouble());
-            intakeRoller.set(rollerSpinny.getAsDouble());
-        });
-    }
-
-    public Command climbingStateCommand() {
-        return run(() -> {
-            intakeRoller.set(0.0);
-
-            intakePivot.setVoltage(
-                intakeAnglePID.calculate(
-                    intakeEncoder.getPosition(), 
-                    IntakeConstants.kDownAngle
-                ) + intakeFeedForward.calculate(
-                    IntakeConstants.kDownAngle, 
-                    intakeEncoder.getVelocity()
-                )
-            );
-        });
-    }
-
-    public Command intakeUpCommand() {
-        return run(() -> {
-            intakeRoller.set(0.0);
-
-            intakePivot.setVoltage(
-                intakeAnglePID.calculate(
-                    intakeEncoder.getPosition(), 
-                    IntakeConstants.kUpAngle
-                ) + intakeFeedForward.calculate(
-                    IntakeConstants.kUpAngle, 
-                    intakeEncoder.getVelocity()
-                )
-            );
-        });
-    }
-
-    public double getIntakeAngle(){
-        return intakeEncoder.getPosition();
-    }
-}

src/main/java/frc/robot/subsystems/IntakePivot.java

@@ -0,0 +1,48 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.CANSparkBase.IdleMode;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.IntakeRollerConstants;
+import frc.robot.constants.IntakePivotConstants;
+
+public class IntakePivot extends SubsystemBase {
+    private CANSparkMax intakePivot;
+
+    private RelativeEncoder intakeEncoder;
+
+    private PIDController intakeAnglePID;
+
+    private ArmFeedforward intakeFeedForward;
+
+    private double armOffset;
+
+    public IntakePivot() {
+        intakePivot = new CANSparkMax(IntakePivotConstants.kIntakePivotID, MotorType.kBrushless);
+
+        intakePivot.setIdleMode(IdleMode.kBrake);
+        intakePivot.setSmartCurrentLimit(IntakePivotConstants.kPivotCurrentLimit);
+        intakePivot.setInverted(true);
+        intakePivot.burnFlash();
+
+        intakeEncoder = intakePivot.getEncoder();
+        intakeEncoder.setPosition(Units.radiansToRotations(IntakePivotConstants.kStartingAngle));
+
+        intakeAnglePID = new PIDController(
+            IntakePivotConstants.kP
+        );
+
+        intakeFeedForward = new ArmFeedforward(
+            IntakeRollerConstants.kSFeedForward, 
+            IntakeRollerConstants.kGFeedForward, 
+            IntakeRollerConstants.kVFeedForward
+        );
+
+    }
+}

src/main/java/frc/robot/subsystems/IntakeRoller.java

@@ -0,0 +1,155 @@
+package frc.robot.subsystems;
+
+import java.util.function.DoubleSupplier;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.CANSparkBase.IdleMode;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.util.function.BooleanConsumer;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.IntakeRollerConstants;
+
+public class IntakeRoller extends SubsystemBase{
+    
+    private CANSparkMax intakeRoller;
+
+    public IntakeRoller(){
+        armOffset = 0;
+
+        intakeRoller = new CANSparkMax(IntakeRollerConstants.kMotorCANID, MotorType.kBrushless);
+        
+        intakeRoller.setSmartCurrentLimit(IntakeRollerConstants.kMotorCurrentLimit);
+        intakeRoller.setIdleMode(IdleMode.kBrake);
+        intakeRoller.burnFlash();
+
+        
+
+        armOffset = getIntakeAngle()-IntakeRollerConstants.kStartingAngle;
+    }
+
+    
+    /* 
+    public Command autoIntaking(BooleanSupplier beamBreak){
+        return run(() -> {
+            intakeRoller.set(1.0);
+
+            intakePivot.setVoltage(
+                intakeAnglePID.calculate(
+                    getIntakeAngle(), 
+                    IntakeConstants.kDownAngle
+                ) + intakeFeedForward.calculate(
+                    IntakeConstants.kDownAngle, 
+                    0.0
+                )
+            );
+        return isFinished(() -> {
+            if(!beamBreak.getAsBoolean()){
+                return true
+            }
+            });
+        });
+    }
+*/
+    public Command intakeControl(DoubleSupplier pivotAngle, DoubleSupplier intakeSpeed) {
+        return run(() -> {
+            intakeRoller.set(intakeSpeed.getAsDouble());
+
+            intakePivot.setVoltage(
+                intakeAnglePID.calculate(
+                    getIntakeAngle(),
+                    pivotAngle.getAsDouble()
+                ) + intakeFeedForward.calculate(
+                    pivotAngle.getAsDouble(), 
+                    0.0
+                )
+            );
+        });
+    }
+
+    public Command intakeDownCommand() {
+        return run(() -> {
+            intakeRoller.set(0.0);
+        
+            intakePivot.setVoltage(
+                intakeAnglePID.calculate(
+                    getIntakeAngle(), 
+                    IntakeRollerConstants.kDownAngle
+                ) + intakeFeedForward.calculate(
+                    IntakeRollerConstants.kDownAngle, 
+                    0.0
+                )
+            );
+        });
+    }
+    
+
+    public Command manualPivot(DoubleSupplier pivotPower, DoubleSupplier rollerSpinny){
+        return run(() ->{
+            intakePivot.set(pivotPower.getAsDouble());
+            intakeRoller.set(rollerSpinny.getAsDouble());
+        });
+    }
+
+    public Command climbingStateCommand() {
+        return run(() -> {
+            intakeRoller.set(0.0);
+
+            intakePivot.setVoltage(
+                intakeAnglePID.calculate(
+                    getIntakeAngle(), 
+                    IntakeRollerConstants.kDownAngle
+                ) + intakeFeedForward.calculate(
+                    IntakeRollerConstants.kDownAngle, 
+                    0.0
+                )
+            );
+        });
+    }
+
+    public Command intakeUpCommand() {
+        return run(() -> {
+            intakeRoller.set(0.0);
+
+            intakePivot.setVoltage(
+                intakeAnglePID.calculate(
+                    getIntakeAngle(), 
+                    IntakeRollerConstants.kUpAngle
+                ) + intakeFeedForward.calculate(
+                    IntakeRollerConstants.kUpAngle, 
+                    0.0
+                )
+            );
+        });
+    }
+
+    public Command stopAll() {
+        return runOnce(() -> {
+            intakeRoller.set(0);
+            intakePivot.setVoltage(0);
+        });
+    }
+
+    public double getIntakeAngle(){
+        return Units.rotationsToRadians(intakeEncoder.getPosition()/IntakeRollerConstants.kIntakePivotConversionFactor)-armOffset;
+    }
+
+    public double getIntakePID(){
+        return intakeAnglePID.calculate(
+                    getIntakeAngle(), 
+                    IntakeRollerConstants.kDownAngle
+                ) + intakeFeedForward.calculate(
+                    IntakeRollerConstants.kDownAngle, 
+                    0.0
+                );
+    }
+
+    public double getIntakeDegrees(){
+        return Math.toDegrees(getIntakeAngle());
+    }
+}

src/main/java/frc/robot/subsystems/Shooter.java

@@ -1,151 +1,53 @@
 package frc.robot.subsystems;
 
-import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkBase.IdleMode;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 
-import edu.wpi.first.math.controller.ArmFeedforward;
-import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.DigitalInput;
-import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ShooterConstants;
 
-public class Shooter extends SubsystemBase{
+public class Shooter extends SubsystemBase {
     private CANSparkMax topShooter;
     private CANSparkMax bottomShooter;
 
-    private CANSparkMax shooterPivot;
-
-    private Encoder pivotEncoder;
-
-    private PIDController shooterPivotPID;
-    private ArmFeedforward shooterPivotFF;
-
     private DigitalInput shooterBeamBreak;
 
-    private boolean indexerBeamBreak;
+    public Shooter(){
+        topShooter = new CANSparkMax(ShooterConstants.kTopMotorCANID, MotorType.kBrushless);
+        topShooter.setInverted(true);
 
-    public Shooter(BooleanSupplier indexerBeamBreak){
-        topShooter = new CANSparkMax(ShooterConstants.kTopShooterID, MotorType.kBrushless);
-        bottomShooter = new CANSparkMax(ShooterConstants.kBottomShooterID, MotorType.kBrushless);
+        bottomShooter = new CANSparkMax(ShooterConstants.kBottomMotorCANID, MotorType.kBrushless);
+        bottomShooter.setInverted(true);
+        
+        topShooter.setSmartCurrentLimit(ShooterConstants.kMotorCurrentLimit);
+        bottomShooter.setSmartCurrentLimit(ShooterConstants.kMotorCurrentLimit);
 
-        shooterPivot = new CANSparkMax(ShooterConstants.kShooterPivotID, MotorType.kBrushless);
-        shooterPivot.setInverted(true);
-
-        /* 
-        topPID = topShooter.getPIDController();
-        topPID.setFeedbackDevice(topEncoder);
-        bottomPID = bottomShooter.getPIDController();
-        bottomPID.setFeedbackDevice(bottomEncoder);
-        */
-
-        shooterPivot.setSmartCurrentLimit(50);
-        topShooter.setSmartCurrentLimit(40);
-        bottomShooter.setSmartCurrentLimit(40);
-
-        pivotEncoder = new Encoder(0, 1);
-        pivotEncoder.setDistancePerPulse(ShooterConstants.kPivotConversion);
-
-        shooterBeamBreak = new DigitalInput(ShooterConstants.kShooterBeamBreakChannel);
+        shooterBeamBreak = new DigitalInput(ShooterConstants.kBeamBreakChannel);
 
         topShooter.setIdleMode(IdleMode.kCoast);
         bottomShooter.setIdleMode(IdleMode.kCoast);
 
-        bottomShooter.burnFlash();
-        shooterPivot.burnFlash();
         topShooter.burnFlash();
-
-        shooterPivotPID = new PIDController(
-            ShooterConstants.kShooterPivotP,
-            ShooterConstants.kShooterPivotI,
-            ShooterConstants.kShooterPivotD
-        );
-        shooterPivotFF = new ArmFeedforward(ShooterConstants.kSShooterPivotFF, ShooterConstants.kGShooterPivotFF, ShooterConstants.kVShooterPivotFF);
-
+        bottomShooter.burnFlash();
     }
-
-    /*
-    public Command angleSpeedsSetpoints(double setpointAngle, double topRPM, double bottomRPM){
-        return run(()-> {
-            angleAndSpeedControl(setpointAngle, topRPM, bottomRPM);
-        });
-    }*/
-
-    public Command angleSpeedsSetpoints(DoubleSupplier setpointAngle, DoubleSupplier speed){
-        return run(()-> {
-            angleAndSpeedControl(setpointAngle.getAsDouble(), speed.getAsDouble(), speed.getAsDouble());
-        });
-    }
-
-    public Command angleSpeedsSetpoints(DoubleSupplier setpointAngle, double topRPM, double bottomRPM){
-        return run(()-> {
-            angleAndSpeedControl(setpointAngle.getAsDouble(), topRPM, bottomRPM);
-        });
-    }
-
-    private void angleAndSpeedControl(double setpointAngle, double topRPM, double bottomRPM){
-        shooterPivot.setIdleMode(IdleMode.kBrake);
-
-            shooterPivot.setVoltage(
-                shooterPivotPID.calculate(getShooterAngle(), setpointAngle) + 
-                shooterPivotFF.calculate(setpointAngle, 0.0));
-
-            //topPID.setReference(topRPM, CANSparkMax.ControlType.kVelocity);
-            //bottomPID.setReference(bottomRPM, CANSparkMax.ControlType.kVelocity);
-            topShooter.set(topRPM);
-            bottomShooter.set(bottomRPM);
-    }
-
-    public Command ampHandoff(DoubleSupplier shootNoteSpeed, BooleanSupplier ampAngleOrNah){
-        return run(() -> {
-            
-            shooterPivot.setIdleMode(IdleMode.kBrake);
-
-            shooterPivot.setVoltage(
-                shooterPivotPID.calculate(getShooterAngle(), ShooterConstants.kShooterLoadAngle) + 
-                shooterPivotFF.calculate(ShooterConstants.kShooterLoadAngle, 0.0));
-            
-            if(shooterBeamBreak.get() || indexerBeamBreak){
-                angleAndSpeedControl(ShooterConstants.kShooterLoadAngle, 0.25, 0.25);
-            }else{
-                angleAndSpeedControl(ShooterConstants.kShooterLoadAngle, shootNoteSpeed.getAsDouble(), shootNoteSpeed.getAsDouble());
-            }
-        });
-    }
-
-    public Command climbState(){
-        return run(() -> {
-            shooterPivot.setIdleMode(IdleMode.kCoast);
-            shooterPivot.set(0.0);
-        });
-    }
-
-    public double getShooterAngle(){
-        return pivotEncoder.getDistance() + ShooterConstants.kShooterLoadAngle;
-    }
-
-    public Command zeroEncoder(){
-        return run(() -> {
-            pivotEncoder.reset();
-        });
-    }
-
-    public Boolean getBeamBreak(){
+ 
+    public boolean getBeamBreak(){
         return shooterBeamBreak.get();
     }
 
-    public Command manualPivot(DoubleSupplier pivotPower, DoubleSupplier spinny){
-        return run(() ->{
-            shooterPivot.set(pivotPower.getAsDouble());
-            topShooter.set(spinny.getAsDouble());
-            bottomShooter.set(spinny.getAsDouble());
+    public Command setSpeed(DoubleSupplier speed) {
+        return run(() -> {
+            topShooter.set(speed.getAsDouble());
+            bottomShooter.set(speed.getAsDouble());
         });
     }
 
-
+    public Command stop() {
+        return setSpeed(() -> 0);
+    }
 }

src/main/java/frc/robot/utilities/MAXSwerveModule.java

@@ -6,6 +6,7 @@ import edu.wpi.first.math.kinematics.SwerveModuleState;
 
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
 import com.revrobotics.SparkAbsoluteEncoder.Type;
 import com.revrobotics.SparkPIDController;
 import com.revrobotics.AbsoluteEncoder;
@@ -36,6 +37,13 @@ public class MAXSwerveModule {
 		m_drivingSparkMax = new CANSparkMax(drivingCANId, MotorType.kBrushless);
 		m_turningSparkMax = new CANSparkMax(turningCANId, MotorType.kBrushless);
 
+		// The periodic frame update rate for the absolute encoders for position is available to be changed at Status 5
+		// The update rate for absolute encoder velocity is on Status 6.
+		// The default update rate for the absolute encoder on both of those status frames is only 200ms which might be causing the swerve jitter
+		// TODO: test if these changes break the swerve drive or fix the jitter.
+		m_turningSparkMax.setPeriodicFramePeriod(PeriodicFrame.kStatus5, 20);
+		m_turningSparkMax.setPeriodicFramePeriod(PeriodicFrame.kStatus6, 20);
+
 		// Factory reset, so we get the SPARKS MAX to a known state before configuring
 		// them. This is useful in case a SPARK MAX is swapped out.
 		m_drivingSparkMax.restoreFactoryDefaults();
@@ -157,4 +165,8 @@ public class MAXSwerveModule {
 	public void resetEncoders() {
 		m_drivingEncoder.setPosition(0);
 	}
+
+	public double steeringEncoder(){
+		return m_turningEncoder.getPosition();
+	}
 }

vendordeps/PathplannerLib.json

@@ -1,7 +1,7 @@
 {
     "fileName": "PathplannerLib.json",
     "name": "PathplannerLib",
-    "version": "2024.2.4",
+    "version": "2024.2.7",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
     "frcYear": "2024",
     "mavenUrls": [
@@ -12,7 +12,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2024.2.4"
+            "version": "2024.2.7"
         }
     ],
     "jniDependencies": [],
@@ -20,7 +20,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2024.2.4",
+            "version": "2024.2.7",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,

vendordeps/Phoenix5.json

@@ -1,7 +1,7 @@
 {
     "fileName": "Phoenix5.json",
     "name": "CTRE-Phoenix (v5)",
-    "version": "5.33.0",
+    "version": "5.33.1",
     "frcYear": 2024,
     "uuid": "ab676553-b602-441f-a38d-f1296eff6537",
     "mavenUrls": [
@@ -20,19 +20,19 @@
         {
             "groupId": "com.ctre.phoenix",
             "artifactId": "api-java",
-            "version": "5.33.0"
+            "version": "5.33.1"
         },
         {
             "groupId": "com.ctre.phoenix",
             "artifactId": "wpiapi-java",
-            "version": "5.33.0"
+            "version": "5.33.1"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.ctre.phoenix",
             "artifactId": "cci",
-            "version": "5.33.0",
+            "version": "5.33.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -45,7 +45,7 @@
         {
             "groupId": "com.ctre.phoenix.sim",
             "artifactId": "cci-sim",
-            "version": "5.33.0",
+            "version": "5.33.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -60,7 +60,7 @@
         {
             "groupId": "com.ctre.phoenix",
             "artifactId": "wpiapi-cpp",
-            "version": "5.33.0",
+            "version": "5.33.1",
             "libName": "CTRE_Phoenix_WPI",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -75,7 +75,7 @@
         {
             "groupId": "com.ctre.phoenix",
             "artifactId": "api-cpp",
-            "version": "5.33.0",
+            "version": "5.33.1",
             "libName": "CTRE_Phoenix",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -90,7 +90,7 @@
         {
             "groupId": "com.ctre.phoenix",
             "artifactId": "cci",
-            "version": "5.33.0",
+            "version": "5.33.1",
             "libName": "CTRE_PhoenixCCI",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -105,7 +105,7 @@
         {
             "groupId": "com.ctre.phoenix.sim",
             "artifactId": "wpiapi-cpp-sim",
-            "version": "5.33.0",
+            "version": "5.33.1",
             "libName": "CTRE_Phoenix_WPISim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -120,7 +120,7 @@
         {
             "groupId": "com.ctre.phoenix.sim",
             "artifactId": "api-cpp-sim",
-            "version": "5.33.0",
+            "version": "5.33.1",
             "libName": "CTRE_PhoenixSim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -135,7 +135,7 @@
         {
             "groupId": "com.ctre.phoenix.sim",
             "artifactId": "cci-sim",
-            "version": "5.33.0",
+            "version": "5.33.1",
             "libName": "CTRE_PhoenixCCISim",
             "headerClassifier": "headers",
             "sharedLibrary": true,

vendordeps/REVLib.json

@@ -1,7 +1,7 @@
 {
     "fileName": "REVLib.json",
     "name": "REVLib",
-    "version": "2024.2.2",
+    "version": "2024.2.4",
     "frcYear": "2024",
     "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
     "mavenUrls": [
@@ -12,14 +12,14 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-java",
-            "version": "2024.2.2"
+            "version": "2024.2.4"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2024.2.2",
+            "version": "2024.2.4",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
@@ -37,7 +37,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-cpp",
-            "version": "2024.2.2",
+            "version": "2024.2.4",
             "libName": "REVLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
@@ -55,7 +55,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2024.2.2",
+            "version": "2024.2.4",
             "libName": "REVLibDriver",
             "headerClassifier": "headers",
             "sharedLibrary": false,