Pid

Finger lake changes

lib/RoboLib

@@ -1 +1 @@
-Subproject commit 79d2801dbafc56f1a6e5273411f14dec7be1879a
+Subproject commit c0478b01a526712b8308ac329017857177a3c876

simgui-ds.json

@@ -1,4 +1,9 @@
 {
+  "Keyboard 0 Settings": {
+    "window": {
+      "visible": true
+    }
+  },
   "keyboardJoysticks": [
     {
       "axisConfig": [
@@ -7,17 +12,20 @@
           "incKey": 68
         },
         {
-          "decKey": 87,
+          "decKey": 83,
-          "incKey": 83
+          "incKey": 87
         },
         {
-          "decKey": 69,
+          "decayRate": 0.009999999776482582,
-          "decayRate": 0.0,
-          "incKey": 82,
           "keyRate": 0.009999999776482582
+        },
+        {},
+        {
+          "decKey": 81,
+          "incKey": 69
         }
       ],
-      "axisCount": 3,
+      "axisCount": 5,
       "buttonCount": 4,
       "buttonKeys": [
         90,
@@ -91,7 +99,7 @@
   ],
   "robotJoysticks": [
     {
-      "guid": "030000004c050000cc09000011810000"
+      "guid": "Keyboard0"
     }
   ]
 }

src/main/deploy/pathplanner/autos/1. Hub to Shoot.auto

@@ -0,0 +1,88 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "1.1 Hub to Shoot"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "ChassisAlignShoot"
+                }
+              },
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 10.0
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "1.2 Shoot to Depot Path"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "RunIntake"
+                }
+              },
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 10.0
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "1.3 Depot to Shoot Path"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "ChassisAlignShoot"
+                }
+              },
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 10.0
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/9. Chaos Monkey.auto

@@ -0,0 +1,25 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "9.1 Chaos Monkey Path"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot preload"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Depot. Hub.auto

@@ -0,0 +1,25 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "S. Hub. shoot 8. E-Hub"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "M. Depot. "
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Depot. L-Tr..auto

@@ -0,0 +1,25 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "S. L-Tre. Preload."
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "M. Depot. "
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Depot. R-Tr.auto

@@ -0,0 +1,25 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "S. R-Trench. E-Hub"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "M. Depot. "
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

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

@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HUB_START"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

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

@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LEFT_TRENCH_START"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

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

@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "RIGHT_TRENCH_START"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Relay. L-Tr..auto

@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Relay. L-Tr."
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Relay. R-Tr..auto

@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Relay. R-Tr."
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Shoot 8. Hub.auto

@@ -0,0 +1,38 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "S. Hub. shoot 8. E-Hub"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 10.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "TurretShoot"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Shoot 8. L-Tr..auto

@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "S. L-Tre. Preload."
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Shoot 8. R-Tr..auto

@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "S. R-Trench. E-Hub"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/1.1 Hub to Shoot Path.path

@@ -0,0 +1,58 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5,
+        "y": 4.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0,
+        "y": 4.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 4.0
+      },
+      "prevControl": {
+        "x": 2.5,
+        "y": 4.0
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -177.9047474356738
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+<<<<<<< HEAD:src/main/deploy/pathplanner/paths/Hub to Depot.path
+    "rotation": -179.99999999992184
+=======
+    "rotation": -179.69626968995792
+>>>>>>> c2fa27aa5cc22124b930645e802b8176658edb7a:src/main/deploy/pathplanner/paths/1.1 Hub to Shoot Path.path
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/1.1 Hub to Shoot.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5,
+        "y": 4.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.75,
+        "y": 4.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 4.0
+      },
+      "prevControl": {
+        "x": 2.75,
+        "y": 4.0
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 177.4888349270938
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -179.0393266954795
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/1.2 Shoot to Depot Path.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 4.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.2144854970378125,
+        "y": 3.8554998234204434
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.0,
+        "y": 6.0
+      },
+      "prevControl": {
+        "x": 1.2166471375173105,
+        "y": 5.875244167248359
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -178.09121133641756
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.4910281334858283
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/1.3 Depot to Shoot Path.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.0,
+        "y": 6.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.453699105174549,
+        "y": 6.084627986890304
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 4.0
+      },
+      "prevControl": {
+        "x": 2.266577414275921,
+        "y": 3.829411518660614
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 100.0,
+    "maxAngularAcceleration": 100.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -179.02897806892076
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -179.69626968995792
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/9.1 Chaos Monkey Path.path

@@ -0,0 +1,102 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5184925161825644,
+        "y": 7.5345006466771025
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.489963675146704,
+        "y": 7.219438447522688
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.6,
+        "y": 6.812200117368956
+      },
+      "prevControl": {
+        "x": 7.532870181778978,
+        "y": 7.8099443669167
+      },
+      "nextControl": {
+        "x": 7.650893142287796,
+        "y": 6.055780073270545
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.6,
+        "y": 0.9617601315610045
+      },
+      "prevControl": {
+        "x": 7.654625343431328,
+        "y": 1.468826273060325
+      },
+      "nextControl": {
+        "x": 7.527819365080411,
+        "y": 0.2917349428743933
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.5,
+        "y": 0.6692650602433468
+      },
+      "prevControl": {
+        "x": 1.096874747061515,
+        "y": 0.6440356562159093
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.2332261810856255,
+      "rotationDegrees": -90.0
+    },
+    {
+      "waypointRelativePos": 1.7516108262639798,
+      "rotationDegrees": 0.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "",
+      "waypointRelativePos": 0,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -1.2282908533168928
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -0.1533900770684255
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/HUB_START.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.519556041182133,
+        "y": 4.059804750409042
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.769556041182133,
+        "y": 4.059804750409042
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.519556041182133,
+        "y": 4.059804750409042
+      },
+      "prevControl": {
+        "x": 3.269556041182133,
+        "y": 4.059804750409042
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/LEFT_TRENCH_START.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 7.423545951843262
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.25,
+        "y": 7.423545951843262
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 7.423545951843262
+      },
+      "prevControl": {
+        "x": 3.75,
+        "y": 7.423545951843262
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/M. Depot. .path

@@ -0,0 +1,107 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.4727349397590364,
+        "y": 4.013144578313253
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.2818883684167575,
+        "y": 4.174630138679796
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.751734939759036,
+        "y": 4.942
+      },
+      "prevControl": {
+        "x": 1.975638554216867,
+        "y": 6.362602409638553
+      },
+      "nextControl": {
+        "x": 0.40641026098477545,
+        "y": 4.54117671213702
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.751734939759036,
+        "y": 7.138469879518072
+      },
+      "prevControl": {
+        "x": 0.41241207330734936,
+        "y": 6.876706525398199
+      },
+      "nextControl": {
+        "x": 1.1342048192771081,
+        "y": 7.433518072289157
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.4727349397590364,
+        "y": 4.013144578313253
+      },
+      "prevControl": {
+        "x": 2.161409638554217,
+        "y": 4.1989156626506015
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.0308661417322806,
+      "rotationDegrees": 90.0
+    },
+    {
+      "waypointRelativePos": 1.7754330708661374,
+      "rotationDegrees": 90.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Shoot 8",
+      "waypointRelativePos": 3.0,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Shoot preload"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/RIGHT_TRENCH_START.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 0.6236959156237152
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.25,
+        "y": 0.6236959156237152
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 0.6236959156237152
+      },
+      "prevControl": {
+        "x": 3.75,
+        "y": 0.6236959156237152
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Relay. L-Tr..path

@@ -0,0 +1,193 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5383012048192777,
+        "y": 7.4553734939759035
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.538301204819281,
+        "y": 7.4553734939759035
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.67990361445783,
+        "y": 7.269602409638554
+      },
+      "prevControl": {
+        "x": 7.475661648641639,
+        "y": 7.413773209038217
+      },
+      "nextControl": {
+        "x": 7.884145580274021,
+        "y": 7.125431610238891
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.67990361445783,
+        "y": 1.0845180722891563
+      },
+      "prevControl": {
+        "x": 7.619269708198755,
+        "y": 1.3270536973254914
+      },
+      "nextControl": {
+        "x": 7.743952438085447,
+        "y": 0.828322777778649
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.641771084337348,
+        "y": 0.9206024096385546
+      },
+      "prevControl": {
+        "x": 6.662532454300786,
+        "y": 1.1697388491997742
+      },
+      "nextControl": {
+        "x": 6.62100971437391,
+        "y": 0.6714659700773349
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.663626506024096,
+        "y": 7.19310843373494
+      },
+      "prevControl": {
+        "x": 6.649759001118466,
+        "y": 6.943493345433587
+      },
+      "nextControl": {
+        "x": 6.677494010929727,
+        "y": 7.442723522036293
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.876831325301204,
+        "y": 7.19310843373494
+      },
+      "prevControl": {
+        "x": 6.125590622853701,
+        "y": 7.217984363490191
+      },
+      "nextControl": {
+        "x": 5.628072027748707,
+        "y": 7.168232503979689
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.745698795180723,
+        "y": 0.9206024096385546
+      },
+      "prevControl": {
+        "x": 5.857502194055715,
+        "y": 1.1442092073885324
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 1.0672199170124472,
+      "rotationDegrees": -90.0
+    },
+    {
+      "waypointRelativePos": 2.0863070539419084,
+      "rotationDegrees": -90.0
+    },
+    {
+      "waypointRelativePos": 2.962655601659752,
+      "rotationDegrees": 90.0
+    },
+    {
+      "waypointRelativePos": 3.9983402489627107,
+      "rotationDegrees": 90.0
+    },
+    {
+      "waypointRelativePos": 5.02406639004149,
+      "rotationDegrees": -90.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Shoot Headed R-Tr",
+      "waypointRelativePos": 1.0302729528535988,
+      "endWaypointRelativePos": 2.2094292803970026,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Shoot Headed R-Tr"
+        }
+      }
+    },
+    {
+      "name": "Shoot headed L-Tr",
+      "waypointRelativePos": 3.1503722084366608,
+      "endWaypointRelativePos": 4.281885856079398,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Shoot headed L-Tr"
+        }
+      }
+    },
+    {
+      "name": "Shoot Headed R-Tr",
+      "waypointRelativePos": 5.067990074441674,
+      "endWaypointRelativePos": 6.0,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Shoot Headed R-Tr"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Relay. R-Tr..path

@@ -0,0 +1,197 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.4618072289156623,
+        "y": 0.680192771084337
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.4618072289156725,
+        "y": 0.680192771084337
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.395783132530121,
+        "y": 0.8768915662650603
+      },
+      "prevControl": {
+        "x": 6.47916753759839,
+        "y": 0.6593895606880393
+      },
+      "nextControl": {
+        "x": 8.040518072289156,
+        "y": 1.029879518072289
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.668975903614458,
+        "y": 7.138469879518072
+      },
+      "prevControl": {
+        "x": 7.691609840125085,
+        "y": 7.387443181134999
+      },
+      "nextControl": {
+        "x": 7.64634196710383,
+        "y": 6.889496577901146
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.761975903614458,
+        "y": 7.138469879518072
+      },
+      "prevControl": {
+        "x": 7.011975903614458,
+        "y": 7.138469879518072
+      },
+      "nextControl": {
+        "x": 6.511975903614458,
+        "y": 7.138469879518072
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.761975903614458,
+        "y": 1.029879518072289
+      },
+      "prevControl": {
+        "x": 7.011975903614458,
+        "y": 1.029879518072289
+      },
+      "nextControl": {
+        "x": 6.511975903614458,
+        "y": 1.029879518072289
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.8440481927710834,
+        "y": 1.029879518072289
+      },
+      "prevControl": {
+        "x": 6.020824888067715,
+        "y": 0.8531028227756465
+      },
+      "nextControl": {
+        "x": 5.667271497474452,
+        "y": 1.2066562133689316
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.8440481927710834,
+        "y": 6.9964096385542165
+      },
+      "prevControl": {
+        "x": 5.885147939597421,
+        "y": 6.74981115759618
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 1.0406639004149303,
+      "rotationDegrees": 90.0
+    },
+    {
+      "waypointRelativePos": 1.917012448132766,
+      "rotationDegrees": 90.0
+    },
+    {
+      "waypointRelativePos": 3.0904564315352623,
+      "rotationDegrees": -90.0
+    },
+    {
+      "waypointRelativePos": 3.91867219917012,
+      "rotationDegrees": -90.0
+    },
+    {
+      "waypointRelativePos": 5.063900414937716,
+      "rotationDegrees": 90.0
+    },
+    {
+      "waypointRelativePos": 6.0,
+      "rotationDegrees": 90.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Shoot headed L-Tr",
+      "waypointRelativePos": 1.0302729528536159,
+      "endWaypointRelativePos": 2.4357320099255366,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Shoot headed L-Tr"
+        }
+      }
+    },
+    {
+      "name": "Shoot Headed R-Tr",
+      "waypointRelativePos": 2.9836228287841102,
+      "endWaypointRelativePos": 4.103225806451647,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Shoot Headed R-Tr"
+        }
+      }
+    },
+    {
+      "name": "Shoot headed L-Tr",
+      "waypointRelativePos": 5.079900744416814,
+      "endWaypointRelativePos": 6.0,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Shoot headed L-Tr"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/S. Hub. shoot 8. E-Hub.path

@@ -0,0 +1,87 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.48366265060241,
+        "y": 4.00221686746988
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.2365697261897557,
+        "y": 4.040231163533364
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.084915662650602,
+        "y": 4.00221686746988
+      },
+      "prevControl": {
+        "x": 1.084915662650602,
+        "y": 4.00221686746988
+      },
+      "nextControl": {
+        "x": 3.084915662650602,
+        "y": 4.00221686746988
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.677383349903843,
+        "y": 4.00221686746988
+      },
+      "prevControl": {
+        "x": 2.4274139067638107,
+        "y": 4.006125511257969
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.7905511811023622,
+      "rotationDegrees": 180.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Shoot 8",
+      "waypointRelativePos": 2.0,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Shoot preload"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/S. L-Tre. Preload..path

@@ -0,0 +1,82 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.48366265060241,
+        "y": 7.466301204819277
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.3666907204976515,
+        "y": 7.245354225732512
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.1723373493975906,
+        "y": 4.00221686746988
+      },
+      "prevControl": {
+        "x": 1.8772891566265066,
+        "y": 4.024072289156626
+      },
+      "nextControl": {
+        "x": 2.421654288011768,
+        "y": 3.9837489460910525
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.1723373493975906,
+        "y": 4.00221686746988
+      },
+      "prevControl": {
+        "x": 1.8226506024096385,
+        "y": 3.9912891566265065
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Shoot 8",
+      "waypointRelativePos": 2.0,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Shoot preload"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/S. R-Trench. E-Hub.path

@@ -0,0 +1,87 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5273734939759036,
+        "y": 0.5272048192771093
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.478313253012048,
+        "y": 0.8003975903614468
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.128626506024096,
+        "y": 3.892939759036145
+      },
+      "prevControl": {
+        "x": 1.9085087310435527,
+        "y": 3.77441480327739
+      },
+      "nextControl": {
+        "x": 2.3487442810046395,
+        "y": 4.0114647147949
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.473,
+        "y": 4.013
+      },
+      "prevControl": {
+        "x": 3.2270325224750227,
+        "y": 4.057721359549996
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.8686614173228185,
+      "rotationDegrees": 180.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Shoot 8",
+      "waypointRelativePos": 2.0,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Shoot preload"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Spin.path

@@ -0,0 +1,91 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5164457831325304,
+        "y": 0.5599879518072293
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.716570175215442,
+        "y": 2.6055674399847426
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.5164457831325304,
+        "y": 3.958506024096385
+      },
+      "prevControl": {
+        "x": 3.3576574884732078,
+        "y": 2.7315170387830205
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.24787401574803541,
+      "rotationDegrees": 90.0
+    },
+    {
+      "waypointRelativePos": 0.29574803149607193,
+      "rotationDegrees": -90.0
+    },
+    {
+      "waypointRelativePos": 0.3398425196850417,
+      "rotationDegrees": 90.0
+    },
+    {
+      "waypointRelativePos": 0.38267716535433216,
+      "rotationDegrees": -90.0
+    },
+    {
+      "waypointRelativePos": 0.4305511811023681,
+      "rotationDegrees": -90.0
+    },
+    {
+      "waypointRelativePos": 0.465826771653542,
+      "rotationDegrees": 90.0
+    },
+    {
+      "waypointRelativePos": 0.5099212598425222,
+      "rotationDegrees": -90.0
+    },
+    {
+      "waypointRelativePos": 0.5363779527559113,
+      "rotationDegrees": 90.0
+    },
+    {
+      "waypointRelativePos": 0.5565354330708714,
+      "rotationDegrees": -90.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/settings.json

@@ -0,0 +1,32 @@
+{
+  "robotWidth": 0.8763,
+  "robotLength": 0.8763,
+  "holonomicMode": true,
+  "pathFolders": [],
+  "autoFolders": [],
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 3.0,
+  "defaultMaxAngVel": 360.0,
+  "defaultMaxAngAccel": 720.0,
+  "defaultNominalVoltage": 12.0,
+  "robotMass": 44.4521,
+  "robotMOI": 6.883,
+  "robotTrackwidth": 0.546,
+  "driveWheelRadius": 0.051,
+  "driveGearing": 8.14,
+  "maxDriveSpeed": 3.9,
+  "driveMotorType": "krakenX60",
+  "driveCurrentLimit": 60.0,
+  "wheelCOF": 1.2,
+  "flModuleX": 0.273,
+  "flModuleY": 0.273,
+  "frModuleX": 0.273,
+  "frModuleY": -0.273,
+  "blModuleX": -0.273,
+  "blModuleY": 0.273,
+  "brModuleX": -0.273,
+  "brModuleY": -0.273,
+  "bumperOffsetX": 0.0,
+  "bumperOffsetY": 0.0,
+  "robotFeatures": []
+}

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

@@ -1,46 +1,82 @@
 // Copyright (c) FIRST and other WPILib contributors.
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
-
 package frc.robot;
 
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.units.Unit;
-import edu.wpi.first.units.measure.Angle;
-import java.security.PublicKey;
-import java.util.Queue;
-
 import com.ctre.phoenix6.signals.InvertedValue;
 
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.units.measure.Angle;
+
 /**
- * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * The Constants class provides a convenient place for teams to hold robot-wide
- * constants. This class should not be used for any other purpose. All constants should be declared
+ * numerical or boolean constants. This class should not be used for any other
- * globally (i.e. public static). Do not put anything functional in this class.
+ * purpose. All constants should be declared globally (i.e. public static). Do
+ * not put anything functional in this class.
  *
- * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * <p>
- * constants are needed, to reduce verbosity.
+ * It is advised to statically import this class (or one of its inner classes)
+ * wherever the constants are needed, to reduce verbosity.
  */
 public final class Constants {
+
     public static class OperatorConstants {
+
         public static final int kDriverControllerPort = 0;
     }
 
-  public static class AdvantageKitConstants{
+    public static class CameraConstanst {
-    public enum RobotType{
+
+        public static class IDFilters {
+
+            public static final int[] RED_HUB_SHOOTING_IDS = new int[]{8, 9, 10, 11};
+            public static final int[] BLUE_HUB_SHOOTING_IDS = new int[]{27, 26, 25, 24};
+        }
+
+        public static final String BACK_CAMERA_NAME = "limelight-back";
+        public static final int BACK_LOCALIZATION_PIPELINE_NUMBER = 0;
+        public static final int BACK_HUB_TRACKING_2D_PIPELINE_NUMBER = 1;
+        public static final int BACK_HUB_TRACKING_3D_PIPELINE_NUMBER = 2;
+
+        public static final String FRONT_CAMERA_NAME = "limelight-front";
+        public static final int FRONT_LOCALIZATION_PIPELINE_NUMBER = 0;
+        public static final double[] BLUE_ZONE_COOR = new double[]{0, 3.5};
+        public static final double[] RED_ZONE_COOR = new double[]{13, 16};
+        public static final double[] NEUTRAL_ZONE_COOR = new double[]{5.5, 11};
+        public static final double[] NEUTRAL1_ZONE_COOR = new double[]{5.5, 11, 0, 3};
+        public static final double[] NEUTRAL2_ZONE_COOR = new double[]{5.5, 11, 5, 8};
+        public static final double[] BLUE1_ZONE_COOR = new double[]{0, 3.5, 0, 3};
+        public static final double[] BLUE2_ZONE_COOR = new double[]{0, 3.5, 5, 8};
+        public static final double[] RED1_ZONE_COOR = new double[]{13, 16, 0, 3};
+        public static final double[] RED2_ZONE_COOR = new double[]{13, 16, 5, 8};
+
+    }
+
+    public static class AdvantageKitConstants {
+
+        public enum RobotType {
             SIM,
             REAL,
             REPLAY
         }
         public static final RobotType CURRENT_MODE = RobotType.REAL;
-  
     }
     // Swerve offsets as of 3/2/26
+
     public static class SwerveDriveOffsets {
+
         public static final float FRONT_LEFT_MOTOR_OFFSET = -0.336426f;
         public static final float FRONT_RIGHT_MOTOR_OFFSET = 0.108887f;
         public static final float BACK_LEFT_MOTOR_OFFSET = -0.492432f;
         public static final float BACK_RIGHT_MOTOR_OFFSET = 0.470215f;
     }
+
     /*
    * Numbers starting with 1 (E.g. 11, 12, 13) are in the Front Left of the robot
    * Numbers starting with 2 (E.g. 21, 22, 23) are in the Front Right of the robot
@@ -50,6 +86,7 @@ public final class Constants {
    * Single diget numbers (E.g. 1, 2, 3) are extra, non-motor, pieces that require IDs, such as the Pigeon
      */
     public static class SwerveDriveIDs {
+
         public static final int FRONT_LEFT_STEER_ID = 11;
         public static final int FRONT_LEFT_DRIVE_ID = 12;
         public static final int FRONT_LEFT_ENCODER_ID = 13;
@@ -63,50 +100,121 @@ public final class Constants {
         public static final int BACK_RIGHT_DRIVE_ID = 42;
         public static final int BACK_RIGHT_ENCODER_ID = 43;
     }
+
+    public static class RobotChassisLimits {
+        // Max velocity in meters per second
+
+        public static final double MAX_ROBOT_LINEAR_VELOCITY = 3;
+        // In meters per second squared
+        public static final double MAX_ROBOT_LINEAR_VELOCITY_ACCEL = 2;
+
+        // In radians per second
+        public static final double MAX_ROBOT_ANGULAR_VELOCITY = Math.PI;
+        // In radians per second squared
+        public static final double MAX_ROBOT_ANGULAR_VELOCITY_ACCEL = Math.PI;
+    }
+
     //Bird
     public static class ExtraIDEntities {
+
         public static final int PIGEON_ID = 1;
     }
 
     public static class IntakeConstants {
-    public static final int DEPLOY_MOTOR_ID = 17;
-    public static final boolean DEPLOY_MOTOR_INVERTED = false; // TODO: Test this
 
-    public static final int INTAKE_MOTOR_ID = 0; 
+        public static final int DEPLOY_MOTOR_ID = 17;
+        public static final boolean DEPLOY_MOTOR_INVERTED = true; // TODO: Test this
+
+        public static final int INTAKE_MOTOR_ID = 2;
         public static final InvertedValue INTAKE_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; // TODO: Test this
     }
 
     public static class ShooterConstants {
-    public static final int LAUNCHER_MOTOR_ID = 2;
-    public static final boolean LAUNCHER_MOTOR_INVERTED = true; // TODO: Test this
 
-    public static final int TURRET_MOTOR_ID = 0;
+        public static final int LAUNCHER_MOTOR_ID = 3;
+        public static final boolean LAUNCHER_MOTOR_INVERTED = false;
+
+        public static final int TURRET_MOTOR_ID = 4;
         public static final InvertedValue TURRET_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
-    public static final int TURRET_LIMIT_CHANNEL = 0;
+        public static final int TURRET_LIMIT_CHANNEL = 9;
-    public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(265);
+        public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(165);
         public static final Angle LIMIT_AXIMUTH_NEG = edu.wpi.first.units.Units.Degrees.of(5);
-    public static final double HOMING_SPEED = .35;
+        public static final double HOMING_SPEED = .05;
-    public static final double TURRET_GEAR_RATIO  = 0;
+        public static final double TURRET_GEAR_RATIO = 6;
 
-    public static final int HOOD_SERVO_CHANNEL = 9;
+        public static final int HOOD_SERVO_CHANNEL = 0;
-    public static final boolean HOOD_SERVO_INVERTED = true; // TODO: Test this
+        public static final boolean HOOD_SERVO_INVERTED = false; // TODO: Test this
-    public static final int HOOD_ENCODER_ID = 0;
+        public static final int HOOD_ENCODER_ID = 4;
-    public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(.5);
+        public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(1);
         public static final Angle LIMIT_ELEVATION_POS = edu.wpi.first.units.Units.Degrees.of(90);
         public static final Angle LIMIT_ELEVATION_NEG = edu.wpi.first.units.Units.Degrees.of(0);
-    public static final Angle DEFLECTOR_STORED_ANGLE = edu.wpi.first.units.Units.Degrees.of(0);
+        public static final double DEFLECTOR_STORED_ANGLE = 0;
-    public static final Angle DEFLECTOR_SET_ANGLE1 = edu.wpi.first.units.Units.Degrees.of(100);
+        public static final double DEFLECTOR_SET_ANGLE1 = 100;
-    public static final Angle DEFLECTOR_SET_ANGLE2 = edu.wpi.first.units.Units.Degrees.of(200);
+        public static final double DEFLECTOR_SET_ANGLE2 = 200;
-    public static final Angle DEFLECTOR_SET_ANGLE3 = edu.wpi.first.units.Units.Degrees.of(300);
+        public static final double DEFLECTOR_SET_ANGLE3 = 300;
         public static final double DEFLECTOR_SERVO_RATIO = 1.0;
     }
 
     public static class HopperConstants {
+
         public static final int KICKER_MOTOR_ID = 9;
         public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive;
 
         public static final int SPINDEXER_MOTOR_ID = 8;
         public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
-    public static final double SPINDEXER_MOTOR_SPEED = .6;
+    }
+
+    public static class FieldPoses {
+
+        private static AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
+
+        public static Pose3d BLUE_HUB_POSE = fieldTags.getTagPose(26).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
+        public static Pose3d RED_HUB_POSE = fieldTags.getTagPose(10).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
+
+        // These are the field targets for passing fuel to the Alliance Zone or to the Neutral Zone from the opponent's side.
+        // Left/Right are from driver's perspective. Coordinates assume origin at blue side of field.
+        // Rotation are zero because they don't matter.
+        public static Pose3d BLUE_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(1.99, 6.35, 0, new Rotation3d());
+        public static Pose3d BLUE_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(1.99, 1.72, 0, new Rotation3d());
+        public static Pose3d BLUE_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 6.35, 0, new Rotation3d());
+        public static Pose3d BLUE_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 1.72, 0, new Rotation3d());
+
+        public static Pose3d RED_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(14.55, 1.72, 0, new Rotation3d());
+        public static Pose3d RED_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(14.55, 6.35, 0, new Rotation3d());
+        public static Pose3d RED_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 1.72, 0, new Rotation3d());
+        public static Pose3d RED_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 6.35, 0, new Rotation3d());
+
+
+    }
+
+    // These represent the dividing lines for the regions in our shooting strategy.
+    public static class ShootingRegionDimensions {
+
+        public static double BLUE_ALLIANCE_ZONE_REGION_X_MAX = 3.5;
+        public static double RED_ALLIANCE_ZONE_REGION_X_MIN = 13.0;
+        public static double NEUTRAL_ZONE_REGION_X_MIN = 5.5;
+        public static double NEUTRAL_ZONE_REGION_X_MAX = 11.0;
+        public static double BLUE_RIGHT_REGION_Y_MAX = 3.0;
+        public static double RED_LEFT_REGION_Y_MAX = 3.0;               // same meaning as the previous one, but from red perspective
+        public static double BLUE_LEFT_REGION_Y_MIN = 5.0;
+        public static double RED_RIGHT_REGION_Y_MIN = 5.0;              // same meaning as the previous one, but from red perspective
+
+    }
+
+    // Regions that define the AutoTurret shooting behaviors.
+    // Color ID in the name reflects robot's alliance since region definitions vary by alliance.
+    // Left/Right are from driver's perspective.
+    public enum ShootingRegion {
+        NON_SHOOTING_REGION,                // Any part of the field that's not in one of the regions
+        BLUE_OWN_ALLIANCE_ZONE,
+        BLUE_LEFT_NEUTRAL_ZONE,
+        BLUE_RIGHT_NEUTRAL_ZONE,
+        BLUE_LEFT_OPPONENT_ALLIANCE_ZONE,
+        BLUE_RIGHT_OPPONENT_ALLIANCE_ZONE,
+        RED_OWN_ALLIANCE_ZONE,
+        RED_LEFT_NEUTRAL_ZONE,
+        RED_RIGHT_NEUTRAL_ZONE,
+        RED_LEFT_OPPONENT_ALLIANCE_ZONE,
+        RED_RIGHT_OPPONENT_ALLIANCE_ZONE        
     }
 }

src/main/java/frc/robot/Robot.java

@@ -14,6 +14,8 @@ import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.NT4Publisher;
 import org.littletonrobotics.junction.wpilog.WPILOGWriter;
 
+import com.ctre.phoenix6.swerve.SwerveModule;
+
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
@@ -55,8 +57,9 @@ public class Robot extends LoggedRobot {
   @Override
   public void robotInit() {
 
+    String swVersion = getVersion();
     Logger.recordMetadata("ProjectName", "MyProject"); // Set a metadata value
-    Logger.recordMetadata("Software Version", getVersion());
+    Logger.recordMetadata("Software Version", swVersion);
 
     if (isReal()) {
       Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs")
@@ -75,7 +78,9 @@ public class Robot extends LoggedRobot {
     Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may
                     // be added.
 
-    Logger.recordOutput("Software Version", getVersion());
+    // Print out software version
+    Logger.recordOutput("Software Version", swVersion);
+    System.out.println("Robot Software Version: " + swVersion);
 
     // Instantiate our RobotContainer. This will perform all our button bindings,
     // and put our
@@ -87,6 +92,9 @@ public class Robot extends LoggedRobot {
 
     // Run all POST tests after robot has been initialized
     PostyManager.getInstance().runAllPOSTs();
+
+
+    m_robotContainer.driveSubsystem.driveSystem.setBrakeMode(false);
   }
 
   /**
@@ -114,6 +122,7 @@ public class Robot extends LoggedRobot {
   /** This function is called once each time the robot enters Disabled mode. */
   @Override
   public void disabledInit() {
+    m_robotContainer.driveSubsystem.driveSystem.setBrakeMode(true);
   }
 
   @Override

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

@@ -1,43 +1,41 @@
 // Copyright (c) FIRST and other WPILib contributors.
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
-
 package frc.robot;
 
-import edu.wpi.first.units.Unit;
+import com.pathplanner.lib.auto.NamedCommands;
-import edu.wpi.first.units.Units;
+
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.OperatorConstants;
-import frc.robot.commands.Autos;
+import frc.robot.commands.AutoShootCommandChassisTurret;
-import frc.robot.commands.ExampleCommand;
+import frc.robot.commands.AutoTurretCommand;
-import frc.robot.commands.ManualHopperCommand;
+import frc.robot.commands.AutoTurretShootCommand;
-import frc.robot.commands.ManualIntake;
+import frc.robot.commands.Reverse;
-import frc.robot.commands.ManualShootCommand;
 import frc.robot.commands.SetDeflectorCommand;
 import frc.robot.subsystems.DriveSubsystem;
-import frc.robot.subsystems.ExampleSubsystem;
 import frc.robot.subsystems.HopperSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.TurretSubsystem;
 
 /**
- * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * This class is where the bulk of the robot should be declared. Since
- * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * Command-based is a "declarative" paradigm, very little robot logic should
- * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * actually be handled in the {@link Robot} periodic methods (other than the
- * subsystems, commands, and trigger mappings) should be declared here.
+ * scheduler calls). Instead, the structure of the robot (including subsystems,
+ * commands, and trigger mappings) should be declared here.
  */
 public class RobotContainer {
     // The robot's subsystems and commands are defined here...
-  private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
-
     // Replace with CommandPS4Controller or CommandJoystick if needed
-  private final CommandXboxController m_driverController =
+
-      new CommandXboxController(OperatorConstants.kDriverControllerPort);
+    private final CommandXboxController m_driverController
+            = new CommandXboxController(OperatorConstants.kDriverControllerPort);
 
     // private final CommandPS4Controller m_driverController = new CommandPS4Controller(OperatorConstants.kDriverControllerPort);
-
     public final DriveSubsystem driveSubsystem = new DriveSubsystem(m_driverController);
 
     public final IntakeSubsystem intakeSubsystem = new IntakeSubsystem();
@@ -46,40 +44,49 @@ public class RobotContainer {
 
     public final HopperSubsystem hopperSubsystem = new HopperSubsystem();
 
-  /** The container for the robot. Contains subsystems, OI devices, and commands. */
+    /**
+     * The container for the robot. Contains subsystems, OI devices, and
+     * commands.
+     */
     public RobotContainer() {
+
+        // turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem));
+        // turretSubsystem.homeTurret();
         // Configure the trigger bindings
         configureBindings();
 
+        NamedCommands.registerCommand("ChassisAlignShoot", new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
+        NamedCommands.registerCommand("RunIntake", intakeSubsystem.intake(m_driverController.getHID()));
+        NamedCommands.registerCommand("TurretShoot", new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem));
+
+        turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem));
     }
 
     /**
-   * Use this method to define your trigger->command mappings. Triggers can be created via the
+     * Use this method to define your trigger->command mappings. Triggers can be
-   * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary
+     * created via the
-   * predicate, or via the named factories in {@link
+     * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor
+     * with an arbitrary predicate, or via the named factories in {@link
      * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link
      * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller
      * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight
      * joysticks}.
+     *
      */
     private void configureBindings() {
-    turretSubsystem.setDefaultCommand(new ManualShootCommand(turretSubsystem, m_driverController.getHID()));
+        // m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE));
-    hopperSubsystem.setDefaultCommand(new ManualHopperCommand(hopperSubsystem, m_driverController.getHID()));
+        // m_driverController.povLeft().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE1));
+        // m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3));
+        // m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2));
 
-    m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE));
+        //m_driverController.back().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem));
-    m_driverController.povLeft().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE1));
+        m_driverController.leftBumper().onTrue(Commands.runOnce(() -> turretSubsystem.homeTurret(), turretSubsystem));
-    m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3));
+        m_driverController.rightBumper().onTrue(Commands.parallel(intakeSubsystem.intake(m_driverController.getHID())));
-    m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2));
+        m_driverController.start().whileTrue(intakeSubsystem.resetDeploy());
-
+        m_driverController.a().whileTrue((new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)));
-    /*m_driverController.y().onTrue(intakeSubsystem.deployCommand());
+        m_driverController.x().whileTrue((new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)));
-    m_driverController.a().onTrue(intakeSubsystem.retractCommand());
+        m_driverController.b().whileTrue(new Reverse(intakeSubsystem, hopperSubsystem, m_driverController.getHID()));
-    m_driverController.b().onTrue(intakeSubsystem.reverseCommand());*/
+        m_driverController.y().onTrue(intakeSubsystem.deploy());
-
-    m_driverController.x().onTrue(intakeSubsystem.killSpeedCommand());
-    m_driverController.a().onTrue(intakeSubsystem.startSpeedCommand());
-    //m_driverController.b().onTrue(intakeSubsystem.revSpeedCommand());
-
-    intakeSubsystem.setDefaultCommand(new ManualIntake(intakeSubsystem, m_driverController.getHID()));
     }
 
     /**
@@ -89,6 +96,8 @@ public class RobotContainer {
      */
     public Command getAutonomousCommand() {
         // An example command will be run in autonomous
-    return Autos.exampleAuto(m_exampleSubsystem);
+        return Commands.parallel(intakeSubsystem.deploy(), Commands.run(() -> {
+            turretSubsystem.homeTurret();
+        }, turretSubsystem), driveSubsystem.getAutoCommand());
     }
 }

src/main/java/frc/robot/Utils.java

@@ -0,0 +1,173 @@
+package frc.robot;
+
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import frc.robot.Constants;
+import frc.robot.Constants.ShootingRegion;
+import frc.robot.Constants.ShootingRegionDimensions;
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.units.measure.Angle;
+
+public class Utils {
+    
+    /**
+     * Calculates the needed angle of the turret for a distance from the HUB.
+     * @param distanceToTag
+     * @return number of rotations of the turret, robot relative
+     */
+    public static double calculateTurretAngleFromCameraTagDistance(double distanceToTag){
+
+        // Calculate turret Angle
+        double cameraDistanceToTarget = distanceToTag + .6;
+        double cameraToShootDistance = 0.26035;
+        // a^2 + b^2 = c^2
+        double shooterDistanceToTarget = Math
+                .sqrt(Math.pow(cameraDistanceToTarget, 2) + Math.pow(cameraToShootDistance, 2));
+        double angleAtShooter = Math.acos(cameraToShootDistance / shooterDistanceToTarget);
+        double robotTurretAngle = (Math.PI / 2) - angleAtShooter;
+        double robotTurretRotations = (robotTurretAngle / Math.PI);
+
+        return robotTurretRotations;
+    }
+
+
+
+    /**
+     * Determines the region for the robot based on its location and alliance.
+     * Assumes the robot is a 27" square. Thresholds based on observations from simulator.
+     * @param robotPose: the robot's pose
+     * @param alliance: the red/blue alliance of the robot, as a sanity check
+     * @return the robot's shooting region
+     */
+    public static ShootingRegion findRobotShootingRegion(Pose3d robotPose, Alliance alliance){
+        ShootingRegion region = Constants.ShootingRegion.NON_SHOOTING_REGION;   // the default return value if robot isn't in a valid region
+        double robotX = robotPose.getX();
+        double robotY = robotPose.getY();
+
+        // Receommended: Add a sanity check for invalid robot poses.
+
+        if (alliance == Alliance.Blue) {
+            if (robotX < Constants.ShootingRegionDimensions.BLUE_ALLIANCE_ZONE_REGION_X_MAX) {
+                region = Constants.ShootingRegion.BLUE_OWN_ALLIANCE_ZONE;
+                }
+            else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
+                     robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
+                     robotY > Constants.ShootingRegionDimensions.BLUE_LEFT_REGION_Y_MIN) {
+                        region = Constants.ShootingRegion.BLUE_LEFT_NEUTRAL_ZONE;
+                     }
+            else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
+                     robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
+                     robotY < Constants.ShootingRegionDimensions.BLUE_RIGHT_REGION_Y_MAX) {
+                        region = Constants.ShootingRegion.BLUE_RIGHT_NEUTRAL_ZONE;                
+                     }
+            else if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN &&
+                     robotY > Constants.ShootingRegionDimensions.BLUE_LEFT_REGION_Y_MIN) {
+                        region = Constants.ShootingRegion.BLUE_LEFT_OPPONENT_ALLIANCE_ZONE;
+                     }
+            else if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN &&
+                     robotY < Constants.ShootingRegionDimensions.BLUE_RIGHT_REGION_Y_MAX) {
+                        region = Constants.ShootingRegion.BLUE_RIGHT_OPPONENT_ALLIANCE_ZONE;
+                     }
+        }
+        else if (alliance == Alliance.Red) {
+            if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN) {
+                region = Constants.ShootingRegion.RED_OWN_ALLIANCE_ZONE;
+                }
+            else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
+                     robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
+                     robotY < Constants.ShootingRegionDimensions.RED_LEFT_REGION_Y_MAX) {
+                        region = Constants.ShootingRegion.RED_LEFT_NEUTRAL_ZONE;
+                     }
+            else if (robotX > Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MIN &&
+                     robotX < Constants.ShootingRegionDimensions.NEUTRAL_ZONE_REGION_X_MAX &&
+                     robotY > Constants.ShootingRegionDimensions.RED_RIGHT_REGION_Y_MIN) {
+                        region = Constants.ShootingRegion.RED_RIGHT_NEUTRAL_ZONE;
+                     }
+            else if (robotX < Constants.ShootingRegionDimensions.BLUE_ALLIANCE_ZONE_REGION_X_MAX &&
+                     robotY < Constants.ShootingRegionDimensions.RED_LEFT_REGION_Y_MAX) {
+                        region = Constants.ShootingRegion.RED_LEFT_OPPONENT_ALLIANCE_ZONE;
+                     }
+            else if (robotX > Constants.ShootingRegionDimensions.RED_ALLIANCE_ZONE_REGION_X_MIN &&
+                     robotY > Constants.ShootingRegionDimensions.RED_RIGHT_REGION_Y_MIN) {
+                        region = Constants.ShootingRegion.RED_RIGHT_OPPONENT_ALLIANCE_ZONE;
+                     }
+        }
+
+        Logger.recordOutput("Utils-ShootingRegion", region);
+
+        return region;
+    }
+
+    public static double getHoodAngleForDistance(double distanceInMeters){
+        if(distanceInMeters >= 4){
+            return 200;
+        }
+        return 0;
+    }
+
+    public static double getLauncherRPMForDistance(double distanceInMeters){ 
+        return Math.min((667.557*distanceInMeters) + 1500.699, 5000);
+    }
+
+    /**
+     * Determines the appropriate shooting target for the robot based on its alliance
+     * and its current region on the field. There are 10 possible targets:
+     * Scoring hub, 2x passing from Neutral Zone, 2x passing from opponent's Alliance Zone,
+     * and these exist for both red and blue alliances.
+     * Right/left are from driver's perspective.
+     * @param region: the identifier of the robot's current field region
+     * @param alliance: the red/blue alliance of the robot, as a sanity check
+     * @return the shooting target for a robot with the provided properties. Returns all-zero
+     * Pose3d object if the region/alliance combination is invalid.
+     */
+    public static Pose3d findShootingTarget(ShootingRegion region, Alliance alliance){
+
+        Pose3d shootingTarget = new Pose3d();
+        if (alliance == Alliance.Blue) {
+            switch (region) {
+                case BLUE_OWN_ALLIANCE_ZONE:
+                    shootingTarget = Constants.FieldPoses.BLUE_HUB_POSE;
+                    break;
+                case BLUE_LEFT_NEUTRAL_ZONE:
+                    shootingTarget = Constants.FieldPoses.BLUE_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE;
+                    break;
+                case BLUE_RIGHT_NEUTRAL_ZONE:
+                    shootingTarget = Constants.FieldPoses.BLUE_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE;
+                    break;
+                case BLUE_LEFT_OPPONENT_ALLIANCE_ZONE:
+                    shootingTarget = Constants.FieldPoses.BLUE_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE;
+                    break;
+                case BLUE_RIGHT_OPPONENT_ALLIANCE_ZONE:
+                    shootingTarget = Constants.FieldPoses.BLUE_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE;
+                    break;
+                default:
+                    break;
+            }
+        }
+        else if (alliance == Alliance.Red) {
+            switch (region) {
+                case RED_OWN_ALLIANCE_ZONE:
+                    shootingTarget = Constants.FieldPoses.RED_HUB_POSE;
+                    break;
+                case RED_LEFT_NEUTRAL_ZONE:
+                    shootingTarget = Constants.FieldPoses.RED_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE;
+                    break;
+                case RED_RIGHT_NEUTRAL_ZONE:
+                    shootingTarget = Constants.FieldPoses.RED_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE;
+                    break;
+                case RED_LEFT_OPPONENT_ALLIANCE_ZONE:
+                    shootingTarget = Constants.FieldPoses.RED_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE;
+                    break;
+                case RED_RIGHT_OPPONENT_ALLIANCE_ZONE:
+                    shootingTarget = Constants.FieldPoses.RED_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE;
+                    break;
+                default:
+                    break;
+            }
+        }
+        return shootingTarget;
+    }
+
+}

src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java

@@ -0,0 +1,154 @@
+package frc.robot.commands;
+
+import java.util.Arrays;
+import java.util.List;
+
+import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.Utils;
+import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.HopperSubsystem;
+import frc.robot.subsystems.TurretSubsystem;
+
+public class AutoShootCommandChassisTurret extends Command{
+    
+    // For Chassis Control
+    private DriveSubsystem driveSubsystem;
+    
+    private HopperSubsystem hopperSubsystem;
+    private TurretSubsystem turretSubsystem;
+
+    private PIDController cameraAutoCenterController;
+    private PIDController deadreckoningAutoCenterController;
+
+    private int[] tagFilters;
+
+    private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
+
+    private Pose3d closestTag;
+
+    private final double chassisRotateSpeed = Math.PI * 0.1;
+
+    public AutoShootCommandChassisTurret(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem)
+    {
+        this.driveSubsystem = driveSubsystem;
+        this.hopperSubsystem = hopperSubsystem;
+        this.turretSubsystem = turretSubsystem;
+
+        this.addRequirements(driveSubsystem, hopperSubsystem, turretSubsystem);
+
+        cameraAutoCenterController = new PIDController(0.15, 0.0, 0.0);
+        cameraAutoCenterController.setTolerance(0.2); // Units degrees
+        cameraAutoCenterController.setSetpoint(0);
+
+
+        deadreckoningAutoCenterController = new PIDController(Math.PI * 0.1, 0, 0.0);
+        deadreckoningAutoCenterController.setTolerance(0.2); // Units is rotations
+        deadreckoningAutoCenterController.enableContinuousInput(-Math.PI, Math.PI);
+    }
+
+    @Override
+    public void initialize() {
+        DriverStation.getAlliance().ifPresent((aliance) -> {
+            if (aliance == Alliance.Blue) {
+                tagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS;
+            } else {
+                tagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS;
+            }
+        });
+
+        turretSubsystem.setLauncher(3000);
+        turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE);
+        hopperSubsystem.spindexerOn();
+        cameraAutoCenterController.calculate(-100);
+
+        // Change to HUB tracking pipeline
+        LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_HUB_TRACKING_3D_PIPELINE_NUMBER);
+        LimelightHelpers.SetFiducialIDFiltersOverride(Constants.CameraConstanst.BACK_CAMERA_NAME, tagFilters);
+
+        Pose3d botPose = driveSubsystem.getBotPose();
+        // Find closest target to us
+        List<Pose3d> tagPoses = Arrays.stream(tagFilters).mapToObj(tagId -> fieldTags.getTagPose(tagId).get()).toList();
+        closestTag = botPose.nearest(tagPoses);
+        Logger.recordOutput("AutoAlign-ClosestAprilTagPose", closestTag);
+
+        Translation2d delta = botPose.getTranslation().toTranslation2d().minus(closestTag.getTranslation().toTranslation2d());
+        Logger.recordOutput("AutoAlign-ClosestAprilTagTransform", delta);
+
+        deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.kZero).getRadians());
+    }
+
+    @Override
+    public void execute() {
+        // Need to compute shoot speed and angle
+
+        Logger.recordOutput("AutoAlign-CenteringError", cameraAutoCenterController.getError());
+
+        // No april tag in camera view. Rotate towards our theoretical closest tag
+        if(!LimelightHelpers.getTV(Constants.CameraConstanst.BACK_CAMERA_NAME)){ 
+            Logger.recordOutput("AutoAlignState-Tracking", "Deadreckoning");
+
+            Pose3d botPose = driveSubsystem.getBotPose();
+            //driveSubsystem.rotateChassis(-deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians()));
+        }
+        else // We have an april tag, center it to the camera frame
+        {
+            Logger.recordOutput("AutoAlignState-Tracking", "Camera");
+            // [tx, ty, tz, pitch, yaw, roll]
+            double[] targetsPose = LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME);
+            if(targetsPose.length >= 1){
+                Logger.recordOutput("AutoAlign-Target Distance", targetsPose[2]);
+
+                // Calculate turret Angle
+                double turretAngle = Utils.calculateTurretAngleFromCameraTagDistance(targetsPose[2]) * Constants.ShooterConstants.TURRET_GEAR_RATIO;
+                turretSubsystem.setTurretMotorRotation(-turretAngle);
+
+                driveSubsystem.rotateChassis(-cameraAutoCenterController.calculate(targetsPose[0])); // This may need to be negated
+            }
+        }
+
+        // If robot is centered on HUB and shooter is up to speed. Run the kicker
+        if(turretSubsystem.isLauncherUpToSpeed() && cameraAutoCenterController.atSetpoint()){
+            Logger.recordOutput("AutoAlignState-Shoot", "FIRE!");
+            hopperSubsystem.feedKicker();
+        }else{
+            Logger.recordOutput("AutoAlignState-Shoot", "Waiting");
+        }
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        turretSubsystem.setLauncher(0);
+
+        hopperSubsystem.stopKicker();
+        hopperSubsystem.spindexerOff();
+
+        driveSubsystem.stopDrive();
+
+        LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_LOCALIZATION_PIPELINE_NUMBER);
+    }
+
+}

src/main/java/frc/robot/commands/AutoShootCommandChassisTurretPathPlanner.java

@@ -0,0 +1,130 @@
+package frc.robot.commands;
+
+import java.util.Arrays;
+import java.util.List;
+
+import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
+import org.littletonrobotics.junction.Logger;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.PathConstraints;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.Constants;
+import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.HopperSubsystem;
+import frc.robot.subsystems.TurretSubsystem;
+
+public class AutoShootCommandChassisTurretPathPlanner extends Command {
+
+    // For Chassis Control
+    private DriveSubsystem driveSubsystem;
+
+    private HopperSubsystem hopperSubsystem;
+    private TurretSubsystem turretSubsystem;
+
+    private int[] tagFilters;
+
+    private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
+
+    private Pose3d closestTag;
+    private Pose2d targetPPPose;
+
+    private Command pathFollowCommand;
+
+    public AutoShootCommandChassisTurretPathPlanner(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem,
+            TurretSubsystem turretSubsystem) {
+        this.driveSubsystem = driveSubsystem;
+        this.hopperSubsystem = hopperSubsystem;
+        this.turretSubsystem = turretSubsystem;
+
+        this.addRequirements(driveSubsystem, hopperSubsystem, turretSubsystem);
+
+    }
+
+    @Override
+    public void initialize() {
+
+        DriverStation.getAlliance().ifPresent((aliance) -> {
+            Pose3d targetHub;
+            if (aliance == Alliance.Blue) {
+                tagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS;
+                targetHub = Constants.FieldPoses.BLUE_HUB_POSE;
+            } else {
+                tagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS;
+                targetHub = Constants.FieldPoses.RED_HUB_POSE;
+            }
+
+            Pose3d botPose = driveSubsystem.getBotPose();
+            // Find closest target to us
+            List<Pose3d> tagPoses = Arrays.stream(tagFilters).mapToObj(tagId -> fieldTags.getTagPose(tagId).get())
+                    .toList();
+            closestTag = botPose.nearest(tagPoses);
+            Logger.recordOutput("AutoAlignPP-ClosestAprilTagPose", closestTag);
+
+            Translation2d delta = botPose.getTranslation().toTranslation2d()
+                    .minus(closestTag.getTranslation().toTranslation2d());
+            Logger.recordOutput("AutoAlignPP-ClosestAprilTagTransform", delta);
+
+            // Add 180 because we want the back of the robot to face the HUB
+            // deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.k180deg).getRadians());
+
+            // Translate back 1 meter from HUB
+            Translation2d newTrans = targetHub.getTranslation().toTranslation2d()
+                    .plus(new Translation2d(1.5, delta.getAngle()));
+            targetPPPose = new Pose2d(newTrans, delta.getAngle());
+            Logger.recordOutput("AutoAlignPP-PPTargetPose", targetPPPose);
+
+            pathFollowCommand = AutoBuilder.pathfindToPose(targetPPPose,
+                    new PathConstraints(Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY,
+                            Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY_ACCEL,
+                            Constants.RobotChassisLimits.MAX_ROBOT_ANGULAR_VELOCITY,
+                            Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY_ACCEL));
+
+            CommandScheduler.getInstance().schedule(pathFollowCommand);
+        });
+
+        turretSubsystem.setLauncher(5500);
+        hopperSubsystem.spindexerOn();
+
+    }
+
+    @Override
+    public void execute() {
+
+        // If robot is centered on HUB and shooter is up to speed. Run the kicker
+        if (turretSubsystem.isLauncherUpToSpeed() && pathFollowCommand.isFinished()) {
+            Logger.recordOutput("AutoAlignState-Shoot", "FIRE!");
+            hopperSubsystem.feedKicker();
+        } else {
+            Logger.recordOutput("AutoAlignState-Shoot", "Waiting");
+        }
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        CommandScheduler.getInstance().cancel(pathFollowCommand);
+        turretSubsystem.setLauncher(0);
+
+        hopperSubsystem.stopKicker();
+        hopperSubsystem.spindexerOff();
+
+        driveSubsystem.stopDrive();
+
+        LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME,
+                Constants.CameraConstanst.BACK_LOCALIZATION_PIPELINE_NUMBER);
+    }
+
+}

src/main/java/frc/robot/commands/AutoTurretCommand.java

@@ -0,0 +1,96 @@
+package frc.robot.commands;
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.Constants.ShootingRegion;
+import frc.robot.Utils;
+import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.TurretSubsystem;
+
+public class AutoTurretCommand extends Command {
+
+    private final Translation2d CENTER_TURRET_TO_ROBOT = new Translation2d(-0.1335024, -0.1824736);
+
+    private TurretSubsystem turretSubsystem;
+    private DriveSubsystem driveSubsystem;
+
+    private Alliance alliance;
+
+    public AutoTurretCommand(TurretSubsystem turretSubsystem, DriveSubsystem driveSubsystem) {
+        this.turretSubsystem = turretSubsystem;
+        this.driveSubsystem = driveSubsystem;
+
+        addRequirements(turretSubsystem); // Not adding drive as we don't wan't the drive command to be de-scheduled
+    }
+
+    @Override
+    public void initialize() {
+        // Perform auto-home
+        turretSubsystem.homeTurret();
+
+        DriverStation.getAlliance().ifPresent((aliance) -> {
+            if (aliance == Alliance.Blue) {
+                turretSubsystem.setTurretTargetPose(Constants.FieldPoses.BLUE_HUB_POSE);
+                aliance = Alliance.Blue;
+            } else {
+                turretSubsystem.setTurretTargetPose(Constants.FieldPoses.RED_HUB_POSE);
+                aliance = Alliance.Red;
+            }
+        });
+
+    }
+
+    @Override
+    public void execute() {
+
+        ShootingRegion shootingRegion = Utils.findRobotShootingRegion(driveSubsystem.getBotPose(), DriverStation.getAlliance().get());
+
+        if(shootingRegion != ShootingRegion.NON_SHOOTING_REGION){
+            Pose3d targetPose = Utils.findShootingTarget(shootingRegion, DriverStation.getAlliance().get());
+
+            turretSubsystem.setTurretTargetPose(targetPose);
+        }
+
+        Logger.recordOutput("AutoTurret-ScoringTarget", turretSubsystem.getTurretTargetPose());
+
+        Pose3d botPose = driveSubsystem.getBotPose();
+        Pose3d turretPose = botPose.plus(new Transform3d(new Translation3d(CENTER_TURRET_TO_ROBOT),
+                new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngleRobotRelative() + 0.5))));
+        Logger.recordOutput("AutoTurret-TurretPose", turretPose);
+
+        Translation2d targetTranslation = turretSubsystem.getTurretTargetPose().getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d());
+        Logger.recordOutput("AutoTurret-Translation", targetTranslation);
+
+        if(turretSubsystem.isTurretReady()){
+            turretSubsystem.setTurretFieldRelative(botPose, targetTranslation.getAngle().getRotations());
+            // turretSubsystem.setTurretRobotRelative(0.25);
+            // turretSubsystem.setTurretMotorRotation(0);
+        }
+
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+    }
+
+}

src/main/java/frc/robot/commands/AutoTurretShootCommand.java

@@ -0,0 +1,90 @@
+package frc.robot.commands;
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.Constants;
+import frc.robot.Utils;
+import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.HopperSubsystem;
+import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.TurretSubsystem;
+
+public class AutoTurretShootCommand extends Command{
+
+    private HopperSubsystem hopperSubsystem;
+    private TurretSubsystem turretSubsystem;
+    private IntakeSubsystem intakeSubsystem;
+    private DriveSubsystem driveSubsystem;
+
+    private Command waitCommand = Commands.waitSeconds(1.5);
+
+    private Alliance alliance;
+    public AutoTurretShootCommand(HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem, IntakeSubsystem intakeSubsystem, DriveSubsystem driveSubsystem){
+        this.hopperSubsystem = hopperSubsystem;
+        this.turretSubsystem = turretSubsystem;
+        this.intakeSubsystem = intakeSubsystem;
+        this.driveSubsystem = driveSubsystem;
+
+        addRequirements(hopperSubsystem, intakeSubsystem);
+    }
+    
+    @Override
+    public void initialize() {
+        hopperSubsystem.spindexerOn();
+        CommandScheduler.getInstance().schedule(waitCommand);
+        alliance = DriverStation.getAlliance().get();
+        Logger.recordOutput("AutoTurretShoot-State", "SHOOTING");
+    }
+
+    @Override
+    public void execute() {
+        double distance = turretSubsystem.getTurretTargetPose().toPose2d().getTranslation().getDistance(driveSubsystem.getBotPose().getTranslation().toTranslation2d());
+        Logger.recordOutput("AutoTurretShoot-DistanceToTarget(Meters)", distance);
+
+        turretSubsystem.setTargetElevation(Utils.getHoodAngleForDistance(distance));
+        turretSubsystem.setLauncher(Utils.getLauncherRPMForDistance(distance));
+
+        if(turretSubsystem.isLauncherUpToSpeed() && waitCommand.isFinished()){
+            hopperSubsystem.feedKicker();
+        }else{
+            hopperSubsystem.stopKicker();
+        }
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        waitCommand.cancel();
+        hopperSubsystem.spindexerOff();
+        hopperSubsystem.stopKicker();
+
+        turretSubsystem.setLauncher(0);
+        turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE);
+
+        Logger.recordOutput("AutoTurretShoot-State", "NOT SHOOTING");
+
+        DriverStation.getAlliance().ifPresent((aliance) -> {
+            if (aliance == Alliance.Blue) {
+                turretSubsystem.setTurretTargetPose(Constants.FieldPoses.BLUE_HUB_POSE);
+            } else {
+                turretSubsystem.setTurretTargetPose(Constants.FieldPoses.RED_HUB_POSE);
+            }
+        });
+
+        // turretSubsystem.setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
+    }
+
+}

src/main/java/frc/robot/commands/Autos.java

@@ -4,15 +4,7 @@
 
 package frc.robot.commands;
 
-import frc.robot.subsystems.ExampleSubsystem;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-
 public final class Autos {
-  /** Example static factory for an autonomous command. */
-  public static Command exampleAuto(ExampleSubsystem subsystem) {
-    return Commands.sequence(subsystem.exampleMethodCommand(), new ExampleCommand(subsystem));
-  }
 
   private Autos() {
     throw new UnsupportedOperationException("This is a utility class!");

src/main/java/frc/robot/commands/ManualHopperCommand.java

@@ -1,59 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot.commands;
-
-import frc.robot.Constants;
-import frc.robot.subsystems.HopperSubsystem;
-import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj2.command.Command;
-
-/** An example command that uses an example subsystem. */
-public class ManualHopperCommand extends Command {
-  private final HopperSubsystem m_subsystem;
-  private final XboxController m_xboxController;
-
-  public ManualHopperCommand(HopperSubsystem subsystem, XboxController xboxController) {
-    m_subsystem = subsystem;
-    m_xboxController = xboxController;
-    addRequirements(subsystem);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-    
-  }
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    if(m_xboxController.getLeftTriggerAxis()>.5 && m_xboxController.getRightTriggerAxis()>.2)
-    {
-      m_subsystem.feed();
-    }
-    else
-    {
-      m_subsystem.stop();
-    }
-    if(m_xboxController.getRightBumper())
-    {
-      m_subsystem.spin(Constants.HopperConstants.SPINDEXER_MOTOR_SPEED);
-    }
-    if(m_xboxController.getLeftBumper())
-    {
-      m_subsystem.spin(0);
-    }
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {}
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
-}

src/main/java/frc/robot/commands/ManualIntake.java

@@ -1,62 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot.commands;
-
-import frc.robot.subsystems.IntakeSubsystem;
-
-import javax.lang.model.util.ElementScanner14;
-
-import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj2.command.Command;
-
-/** An example command that uses an example subsystem. */
-public class ManualIntake extends Command {
-  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-  private final IntakeSubsystem m_subsystem;
-  private final XboxController m_XboxController;
-
-  /**
-   * Creates a new ExampleCommand.
-   *
-   * @param subsystem The subsystem used by this command.
-   */
-  public ManualIntake(IntakeSubsystem subsystem, XboxController xboxController) {
-    m_subsystem = subsystem;
-    m_XboxController = xboxController;
-    // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(subsystem);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {}
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    if(m_XboxController.getLeftBumper()==true)
-    {
-      m_subsystem.extend();
-    }
-    else if(m_XboxController.getRightBumper()==true)
-    {
-      m_subsystem.retract();
-    }
-    else
-    {
-      m_subsystem.neutral();
-    }
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {}
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
-}

src/main/java/frc/robot/commands/ManualShootCommand.java

@@ -1,53 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot.commands;
-
-import frc.robot.subsystems.TurretSubsystem;
-import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj.GenericHID.RumbleType;
-import edu.wpi.first.wpilibj2.command.Command;
-
-/** An example command that uses an example subsystem. */
-public class ManualShootCommand extends Command {
-  private final TurretSubsystem m_subsystem;
-  private final XboxController m_controller;
-
-  public ManualShootCommand(TurretSubsystem subsystem, XboxController controller) {
-    m_subsystem = subsystem;
-    m_controller = controller;
-    // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(subsystem);
-  }
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    if(m_controller.getRightTriggerAxis()>.8)
-    {
-      m_subsystem.setLauncher(1);
-      m_controller.setRumble(RumbleType.kBothRumble, 1);
-    }
-    else if(m_controller.getRightTriggerAxis()>.5)
-    {
-      m_subsystem.setLauncher(.7);
-      m_controller.setRumble(RumbleType.kBothRumble, .67);
-    }
-    else if(m_controller.getRightTriggerAxis()>.2)
-    {
-      m_subsystem.setLauncher(.5);
-      m_controller.setRumble(RumbleType.kBothRumble, .33);
-    }
-    else
-    {
-      m_subsystem.setLauncher(0);
-      m_controller.setRumble(RumbleType.kBothRumble, 0);
-    }
-  }
-
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
-}

src/main/java/frc/robot/commands/Reverse.java

@@ -0,0 +1,57 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.HopperSubsystem;
+import frc.robot.subsystems.IntakeSubsystem;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class Reverse extends Command {
+
+    private IntakeSubsystem intakeSubsystem;
+    private HopperSubsystem hopperSubsystem;
+    private XboxController m_XboxController;
+
+    /**
+     * Creates a new ExampleCommand.
+     *
+     * @param subsystem The subsystem used by this command.
+     */
+    public Reverse(IntakeSubsystem intakeSubsystem, HopperSubsystem hopperSubsystem, XboxController m_XboxController) {
+        this.intakeSubsystem = intakeSubsystem;
+        this.hopperSubsystem = hopperSubsystem;
+        this.m_XboxController = m_XboxController;
+    }
+
+    // Called when the command is initially scheduled.
+    @Override
+    public void initialize() {
+    }
+
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {
+        intakeSubsystem.reverseIntake();
+        hopperSubsystem.spindexerReverse();
+        hopperSubsystem.reverseKicker();
+    }
+
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+        hopperSubsystem.stopKicker();
+        hopperSubsystem.spindexerOff();
+        intakeSubsystem.stopIntake(m_XboxController);
+    }
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+}

src/main/java/frc/robot/commands/SetDeflectorCommand.java

@@ -5,15 +5,14 @@
 package frc.robot.commands;
 
 import frc.robot.subsystems.TurretSubsystem;
-import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj2.command.Command;
 
 /** An example command that uses an example subsystem. **/
 public class SetDeflectorCommand extends Command {
   private final TurretSubsystem m_subsystem;
-  private Angle deployAngle;
+  private double deployAngle;
 
-  public SetDeflectorCommand(TurretSubsystem subsystem, Angle deployAngle) {
+  public SetDeflectorCommand(TurretSubsystem subsystem, double deployAngle) {
     m_subsystem = subsystem;
     this.deployAngle = deployAngle;
   }

src/main/java/frc/robot/commands/ZoneCheck.java

@@ -0,0 +1,86 @@
+package frc.robot.commands;
+
+import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.subsystems.DriveSubsystem;
+
+public class ZoneCheck extends Command {
+    private Zone zone;
+    private DriveSubsystem driveSubsystem;
+
+    public ZoneCheck(DriveSubsystem driveSubsystem)
+    {
+        this.driveSubsystem = driveSubsystem;
+    }
+
+    public enum Zone {
+        Blue,
+        Red,
+        Neutral,
+        INIT;
+    }
+
+    @Override
+    public void initialize() {
+        zone = Zone.INIT;
+    }
+
+    @Override
+    public void execute() {
+        getZone();
+        Logger.recordOutput("FieldZone", zone);
+        DriverStation.getAlliance().ifPresent((aliance) -> {
+            if (aliance == Alliance.Blue) {
+                switch (zone) {
+                    case Blue:
+                        System.out.println("BLUE");
+                            break;
+                    case Neutral:
+                        System.out.println("NEUTRAL");
+                            break;
+                    case Red:
+                        System.out.println("RED");
+                            break;
+                    default:
+                        System.out.println("****");
+                }
+            } else {
+                switch (zone) {
+                    case Blue:
+                        System.out.println("BLUE");
+                            break;
+                    case Neutral:
+                        System.out.println("NEUTRAL");
+                            break;
+                    case Red:
+                        System.out.println("RED");
+                            break;
+                    default:
+                        System.out.println("****");
+                }
+            }
+        });
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+
+    private void getZone() {
+        if (driveSubsystem.getBotPose().getX() >= Constants.CameraConstanst.RED_ZONE_COOR[0]) {
+            zone = Zone.Red;
+        } else if (driveSubsystem.getBotPose().getX() >= Constants.CameraConstanst.NEUTRAL_ZONE_COOR[0]) {
+            zone = Zone.Neutral;
+        } else {
+            zone = Zone.Blue;
+        }
+    }
+}

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

@@ -1,25 +1,41 @@
 package frc.robot.subsystems;
 
+import static org.fairportrobotics.frc.posty.assertions.Assertions.assertThat;
+
 import org.fairportrobotics.frc.posty.TestableSubsystem;
 import org.fairportrobotics.frc.posty.test.PostTest;
-import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
 import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveBuilder;
 import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveDriveSystem;
 import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveModule;
+import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
 import org.littletonrobotics.junction.Logger;
 
-import com.fasterxml.jackson.annotation.JsonCreator.Mode;
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+import com.pathplanner.lib.util.PathPlannerLogging;
 
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotState;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+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.button.CommandXboxController;
-
 import frc.robot.Constants;
 
 public class DriveSubsystem extends TestableSubsystem {
 
+    private SendableChooser<Command> autoChooser;
+
     private SwerveBuilder swerveBuilder = new SwerveBuilder();
-    private SwerveDriveSystem driveSystem;
+    public SwerveDriveSystem driveSystem;
 
     private double driveP = 0.1;
     private double driveI = 0.0;
@@ -36,8 +52,8 @@ public class DriveSubsystem extends TestableSubsystem {
         driveSystem = swerveBuilder
                 .withCanbusName("Drive")
                 .withPigeonId(Constants.ExtraIDEntities.PIGEON_ID)
-                .withMaxLinearVelocity(3.0)
+                .withMaxLinearVelocity(Constants.RobotChassisLimits.MAX_ROBOT_LINEAR_VELOCITY)
-                .withMaxAngularVelocity(Math.PI)
+                .withMaxAngularVelocity(Constants.RobotChassisLimits.MAX_ROBOT_ANGULAR_VELOCITY)
                 .withSwerveModule(
                         swerveBuilder.new SwerveModuleBuilder()
                                 .withDriveMotorId(Constants.SwerveDriveIDs.FRONT_LEFT_DRIVE_ID)
@@ -50,7 +66,7 @@ public class DriveSubsystem extends TestableSubsystem {
                                 .withSteerKI(steerI)
                                 .withSteerKD(steerD)
                                 .withSteerKV(steerKV)
-                                //If the offsets don't work, contact Tyler W or Dom G
+                                // If the offsets don't work, contact Tyler W or Dom G
                                 .withSteerOffset(Constants.SwerveDriveOffsets.FRONT_LEFT_MOTOR_OFFSET)
                                 .withSteerEncoderId(Constants.SwerveDriveIDs.FRONT_LEFT_ENCODER_ID)
                                 .withModuleLocation(new Translation2d(1, 1))
@@ -119,6 +135,9 @@ public class DriveSubsystem extends TestableSubsystem {
                                 .build())
                 .build();
 
+        // Set vision measurement confidence values
+        driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 0.1));
+
         this.setDefaultCommand(Commands.run(new Runnable() {
 
             @Override
@@ -128,21 +147,95 @@ public class DriveSubsystem extends TestableSubsystem {
                         -(xboxController.getLeftX() * Math.abs(xboxController.getLeftX())),
                         -(xboxController.getRawAxis(4) * Math.abs(xboxController.getRawAxis(4))));
 
-                // driveSubsystem.setChassisSpeed(new ChassisSpeeds(1, 0, 0));
-
                 Logger.recordOutput("RequestedSwerveState", driveSystem.getRequestedModuleStates());
                 Logger.recordOutput("ActualSwerveState", driveSystem.getActualModuleStates());
-                Logger.recordOutput("Pose Estimation", driveSystem.getRobotPose());
             }
 
         }, this));
 
+        RobotConfig config;
+        try {
+            config = RobotConfig.fromGUISettings();
+
+            AutoBuilder.configure(
+                    driveSystem::getRobotPose2d,
+                    driveSystem::setPose2d,
+                    driveSystem::getRobotRelativeSpeeds,
+                    (speeds, feedforwards) -> {driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(-speeds.vxMetersPerSecond, -speeds.vyMetersPerSecond, -speeds.omegaRadiansPerSecond, driveSystem.getRobotHeading()), new Translation2d());},
+                    new PPHolonomicDriveController(
+                            new PIDConstants(5, 0, 0),
+                            new PIDConstants(Math.PI, 0, 0)),
+                    config,
+                    () -> {
+                        var alliance = DriverStation.getAlliance();
+                        if (alliance.isPresent()) {
+                            return alliance.get() == DriverStation.Alliance.Red;
+                        }
+                        return false;
+                    },
+                    this);
+
+            autoChooser = AutoBuilder.buildAutoChooser();
+            autoChooser.setDefaultOption("Do Nothing", Commands.none());
+            SmartDashboard.putData("Auto Chooser", autoChooser);
+
+            // PathPlannerLogging.setLogActivePathCallback((List<Pose2d> pose) -> {});
+            PathPlannerLogging.setLogTargetPoseCallback((Pose2d targetPose) -> { Logger.recordOutput("PathPlanner-TargetPose", targetPose);});
+
+        } catch (Exception e) {
+            e.printStackTrace();
+        }
+
+        
+
     }
 
     @Override
     public void periodic() {
         super.periodic();
         driveSystem.periodic();
+
+        Logger.recordOutput("RobotPose", driveSystem.getRobotPose3d());
+
+        LimelightHelpers.SetRobotOrientation(Constants.CameraConstanst.FRONT_CAMERA_NAME, driveSystem.getRobotPose2d().getRotation().getDegrees(), 0, 0, 0, 0, 0);
+        LimelightHelpers.SetRobotOrientation(Constants.CameraConstanst.BACK_CAMERA_NAME, driveSystem.getRobotPose2d().getRotation().getDegrees(), 0, 0, 0, 0, 0);
+
+        // MegaTag 2 impl
+        if(RobotState.isDisabled()){
+            LimelightHelpers.SetIMUMode(Constants.CameraConstanst.FRONT_CAMERA_NAME, 1);
+            LimelightHelpers.SetIMUMode(Constants.CameraConstanst.BACK_CAMERA_NAME, 1);
+        }else{
+            LimelightHelpers.SetIMUMode(Constants.CameraConstanst.FRONT_CAMERA_NAME, 4);
+            LimelightHelpers.SetIMUMode(Constants.CameraConstanst.BACK_CAMERA_NAME, 4);
+        }
+
+        LimelightHelpers.PoseEstimate frontTag = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.CameraConstanst.FRONT_CAMERA_NAME);
+        LimelightHelpers.PoseEstimate backTag = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.CameraConstanst.BACK_CAMERA_NAME);
+
+        if(frontTag.tagCount >= 2){
+            driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(frontTag.pose), frontTag.timestampSeconds);
+            Logger.recordOutput("FrontPose-Estimate", frontTag.pose);
+            Logger.recordOutput("FrontPose-IsMegaTag2", frontTag.isMegaTag2);
+        }
+
+        if(backTag.tagCount >= 2){
+            driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backTag.pose), backTag.timestampSeconds);
+            Logger.recordOutput("BackPose-Estimate", backTag.pose);
+            Logger.recordOutput("BackPose-IsMegaTag2", frontTag.isMegaTag2);
+        }
+
+
+        // MegaTag 1 Impl
+        // LimelightHelpers.PoseEstimate frontCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.FRONT_CAMERA_NAME);
+        // if (frontCameraPose.tagCount >= 2) {
+        //     driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(frontCameraPose.pose), frontCameraPose.timestampSeconds);
+        //     Logger.recordOutput("FrontPoseEstimate", frontCameraPose.pose);
+        // }
+        // LimelightHelpers.PoseEstimate backCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.BACK_CAMERA_NAME);
+        // if (backCameraPose.tagCount >= 2) {
+        //     driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backCameraPose.pose), backCameraPose.timestampSeconds);
+        //     Logger.recordOutput("BackPoseEstimate", backCameraPose.pose);
+        // }
     }
 
     @Override
@@ -151,19 +244,37 @@ public class DriveSubsystem extends TestableSubsystem {
         driveSystem.simulationPeriodic();
     }
 
+    public Command getAutoCommand() {
+        return autoChooser.getSelected();
+    }
+
+    public void stopDrive() {
+        driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0, 0, 0, driveSystem.getGyro().getRotation2d()), Translation2d.kZero);
+    }
+
+    public void rotateChassis(double rotSpeed) {
+        driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0, 0, rotSpeed, driveSystem.getGyro().getRotation2d()), Translation2d.kZero);
+    }
+
+    public Pose3d getBotPose() {
+        return driveSystem.getRobotPose3d();
+    }
+
     @PostTest()
-    public void DriveSubsystem_CanDevicesConnected(){
+    public void DriveSubsystem_CanDevicesConnected() {
 
         SwerveModule[] modules = driveSystem.getModules();
 
-        for(int i=0;i < modules.length; i++){
+        for (int i = 0; i < modules.length; i++) {
-            assertThat(modules[i].getDriveMotor().isConnected()).as(modules[i].getModuleName() + " drive motor is not connected").isTrue();
+            assertThat(modules[i].getDriveMotor().isConnected())
-            assertThat(modules[i].getSteerMotor().isConnected()).as(modules[i].getModuleName() + " steer motor is not connected").isTrue();
+                    .as(modules[i].getModuleName() + " drive motor is not connected").isTrue();
-            assertThat(modules[i].getSteerEncoder().isConnected()).as(modules[i].getModuleName() + " CANCoder is not connected").isTrue();
+            assertThat(modules[i].getSteerMotor().isConnected())
+                    .as(modules[i].getModuleName() + " steer motor is not connected").isTrue();
+            assertThat(modules[i].getSteerEncoder().isConnected())
+                    .as(modules[i].getModuleName() + " CANCoder is not connected").isTrue();
         }
 
         assertThat(driveSystem.getGyro().isConnected()).as("Pigeon is not connected").isTrue();
-
     }
 
 }

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

@@ -1,46 +1,114 @@
 // Copyright (c) FIRST and other WPILib contributors.
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
-
 package frc.robot.subsystems;
 
 import org.fairportrobotics.frc.posty.TestableSubsystem;
+import static org.fairportrobotics.frc.posty.assertions.Assertions.assertThat;
 import org.fairportrobotics.frc.posty.test.PostTest;
+import org.littletonrobotics.junction.Logger;
 
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 
-import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
+import edu.wpi.first.units.Units;
-import static org.fairportrobotics.frc.robolib.motors.Utils.*;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants;
 
 public class HopperSubsystem extends TestableSubsystem {
 
     private TalonFX spindexerMotor;
+
     private TalonFX kickerMotor;
-  /** Creates a new HopperSubsystem. */
+    private VelocityVoltage kickerControl = new VelocityVoltage(0);
+    private StatusSignal<AngularVelocity> kickerVelocity;
+
+    /**
+     * Creates a new HopperSubsystem.
+     */
     public HopperSubsystem() {
         spindexerMotor = new TalonFX(Constants.HopperConstants.SPINDEXER_MOTOR_ID);
-    SetMotorDirection(spindexerMotor, Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION);
+        spindexerMotor.getConfigurator().apply(
+                new TalonFXConfiguration()
+                        .withMotorOutput(
+                                new MotorOutputConfigs()
+                                        .withInverted(Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION))
+                        .withSlot0(
+                                new Slot0Configs()
+                                        .withKP(0)
+                                        .withKI(0)
+                                        .withKD(0)
+                                        .withKS(0)));
 
         kickerMotor = new TalonFX(Constants.HopperConstants.KICKER_MOTOR_ID);
-    SetMotorDirection(kickerMotor, Constants.HopperConstants.KICKER_MOTOR_DIRECTION);
+        kickerMotor.getConfigurator().apply(
+                new TalonFXConfiguration()
+                        .withMotorOutput(
+                                new MotorOutputConfigs()
+                                        .withInverted(Constants.HopperConstants.KICKER_MOTOR_DIRECTION)
+                        )
+                        .withSlot0(new Slot0Configs()
+                                .withKP(0)
+                                .withKI(0)
+                                .withKD(0)
+                                .withKS(0)
+                        ));
+        kickerVelocity = kickerMotor.getVelocity();
     }
 
-  public void feed() {kickerMotor.set(0.5);}
+    public void feedKicker() {
+        //kickerMotor.setControl(kickerControl.withVelocity(1000));
+        kickerMotor.set(0.75);
+    }
 
-  public void reverse() {kickerMotor.set(-0.5);} 
+    public void reverseKicker() {
+        //kickerMotor.setControl(kickerControl.withVelocity(-500));
+        kickerMotor.set(-0.5);
+    }
 
-  public void stop() {kickerMotor.set(0);}
+    public void stopKicker() {
+        kickerMotor.stopMotor();
+    }
 
-  public void spin(double speed) {spindexerMotor.set(speed);}
+    public void spindexerOn() {
+        spindexerMotor.set(0.75);
+    }
 
-  @PostTest(name = "A friendly name", enabled = true)
+    public void spindexerReverse() {
-  public void myFailingPostTest(){
+        spindexerMotor.set(-0.75);
-    assertThat(true).isFalse();
+    }
+
+    public void spindexerOff() {
+        spindexerMotor.stopMotor();
+    }
+
+    public Command spinny() {
+        return this.runOnce(() -> {
+            if (spindexerMotor.get() == 0) {
+                spindexerOn();
+            } else {
+                spindexerOff();
+            }
+        });
+    }
+
+
+    @Override
+    public void periodic() {
+        Logger.recordOutput("HopperSubsystem-KickerSpeed(Rotations/sec)", kickerVelocity.refresh().getValue().in(Units.RotationsPerSecond));
     }
 
     @PostTest(enabled = true)
-  public void myPassingPostTest(){
+    public void hopperSystem_CANDevicesConnected() {
-    assertThat("Hello World").endWith("World");
+        assertThat(kickerMotor.isConnected()).as("Kicker motor not connected!").isTrue();
+        assertThat(spindexerMotor.isConnected()).as("Spindexer motor not connected!").isTrue();
     }
+
 }

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

@@ -1,33 +1,34 @@
 // Copyright (c) FIRST and other WPILib contributors.
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
-
 package frc.robot.subsystems;
 
 import org.fairportrobotics.frc.posty.TestableSubsystem;
+import static org.fairportrobotics.frc.posty.assertions.Assertions.assertThat;
 import org.fairportrobotics.frc.posty.test.PostTest;
+import static org.fairportrobotics.frc.robolib.motors.Utils.SetMotorDirection;
 
 import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
 import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 import com.ctre.phoenix6.hardware.TalonFX;
-import frc.robot.Constants;
-
-import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
-import static org.fairportrobotics.frc.robolib.motors.Utils.*;
 
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.FunctionalCommand;
+import frc.robot.Constants;
 
 public class IntakeSubsystem extends TestableSubsystem {
-  /** Creates a new ExampleSubsystem. */
+
+    /**
+     * Creates a new ExampleSubsystem.
+     */
 
     private TalonFX intakeMotor;
     private WPI_TalonSRX deployMotor;
 
-  public IntakeSubsystem() 
+    public IntakeSubsystem() {
-  {
         intakeMotor = new TalonFX(Constants.IntakeConstants.INTAKE_MOTOR_ID);
         SetMotorDirection(intakeMotor, Constants.IntakeConstants.INTAKE_MOTOR_DIRECTION);
 
@@ -37,63 +38,56 @@ public class IntakeSubsystem extends TestableSubsystem {
         deployMotor.setInverted(Constants.IntakeConstants.DEPLOY_MOTOR_INVERTED);
     }
 
-  public void neutral()
+    public Command deploy() {
-  {
+        return Commands.sequence(
-    deployMotor.set(0);
+                Commands.deadline(
-  }
+                        Commands.waitSeconds(2),
-
+                        this.runOnce(
-  public void extend()
+                                () -> {
-  {
                                     deployMotor.set(1);
+                                } // Bump this speed up if intake doesn't deploy
+                        )
+                ),
+                this.runOnce(() -> deployMotor.stopMotor())
+        );
     }
 
-  public void retract()
+    public Command resetDeploy() {
-  {
+        return this.runEnd(() -> deployMotor.set(-1), () -> deployMotor.stopMotor());
-    deployMotor.set(-1);
     }
 
-  public void setSpeed(double speed)
+    public void setSpeed(double speed) {
-  {
         intakeMotor.set(speed);
     }
 
-  public boolean isDeployed()
+    public Command stopIntake(XboxController m_XboxController) {
-  {
+        return this.runOnce(() -> {
-    return deployMotor.isFwdLimitSwitchClosed()==1;
+            intakeMotor.stopMotor();
+            m_XboxController.setRumble(RumbleType.kBothRumble, .0);
+        });
     }
 
-  public boolean isRetracted()
+    public Command intake(XboxController m_XboxController) {
-  {
+        return this.runOnce(() -> {
-    return deployMotor.isRevLimitSwitchClosed()==1;
+            if (intakeMotor.get() == 0) {
+                setSpeed(-.5);
+                m_XboxController.setRumble(RumbleType.kBothRumble, .5);
+            } else {
+                intakeMotor.stopMotor();
+                m_XboxController.setRumble(RumbleType.kBothRumble, .0);
+            }
+        });
     }
 
-  public Command deployCommand()
+    public Command reverseIntake() {
-  {
+        return this.runEnd(() -> setSpeed(.5), () -> {
-    return new FunctionalCommand(this::extend, this::extend, interrupted -> neutral(), this::isDeployed, this).andThen(Commands.startEnd(() -> setSpeed(.5), () -> setSpeed(0), this));
+            intakeMotor.stopMotor();
+        });
     }
 
-  public Command reverseCommand()
+    @PostTest(enabled = true)
-  {
+    public void IntakeSubsystem_CANDevicesConnected() {
-    return new FunctionalCommand(this::extend, this::extend, interrupted -> neutral(), this::isDeployed, this).andThen(Commands.startEnd(() -> setSpeed(-.5), () -> setSpeed(0), this));
+        assertThat(intakeMotor.isConnected()).as("Intake motor not connected!").isTrue();
-  }
+        assertThat(deployMotor.isAlive()).as("Deploy controller not connected!").isTrue();
-
-  public Command retractCommand()
-  {
-    return this.runOnce(() -> setSpeed(0)).andThen(new FunctionalCommand(this::retract, this::retract, interrupted -> neutral(), this::isRetracted, this));
-  }
-
-  public Command startSpeedCommand()
-  {
-    return this.runOnce(() -> setSpeed(-.5));
-  }
-
-  public Command killSpeedCommand()
-  {
-    return this.runOnce(() -> setSpeed(0));
-  }
-
-  public Command revSpeedCommand()
-  {
-    return this.runOnce(() -> setSpeed(.5));
     }
 }

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

@@ -5,130 +5,314 @@
 package frc.robot.subsystems;
 
 import org.fairportrobotics.frc.posty.TestableSubsystem;
+import org.fairportrobotics.frc.posty.test.PostTest;
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.ConsoleSource.RoboRIO;
 
-import com.ctre.phoenix6.hardware.CANcoder;
+import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
+
+import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.FeedbackConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.ForwardLimitValue;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.ReverseLimitValue;
+import com.ctre.phoenix6.sim.TalonFXSimState;
+import com.revrobotics.PersistMode;
+import com.revrobotics.ResetMode;
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.FeedbackSensor;
+import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 import com.revrobotics.spark.config.SparkMaxConfig;
 
 import frc.robot.Constants;
-import static org.fairportrobotics.frc.robolib.motors.Utils.*;
+import frc.robot.Robot;
-
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.units.AngleUnit;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Servo;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 
 public class TurretSubsystem extends TestableSubsystem {
   public enum TurretState {
     INIT,
     HOMING,
-    MANUAL;
+    READY;
   }
 
-  private TurretState currentState;
+  private final boolean LOGGING = true;
+  private TurretState turretState;
   private DigitalInput turretLimitSwitch;
-  // TODO: Reinsert the turret once motor is installed
+  private TalonFX turretMotor;
-  //private TalonFX turretMotor;
+  private PositionVoltage turretControlMode = new PositionVoltage(0);
+  private TalonFXSimState turretSim;
+  private StatusSignal<Angle> turretMotorPosition;
+  private StatusSignal<ForwardLimitValue> turretMotorForwardLimit;
+  private StatusSignal<ReverseLimitValue> turretMotorReverseLimit;
+  private StatusSignal<Double> turretPositionError;
+  private double forwardLimit = 0;
+  private double reverseLimit = 0;
+
   private SparkMax launcherMotor;
+  private SparkClosedLoopController launcherControler;
+
+  private double turretOffsetInRotations = 0;
+
   private Servo hood;
-  private Angle limitPos;
+
-  private Angle limitNeg;
+  private Pose3d turretTarget;
-  private boolean turretGoingPos;
 
   public TurretSubsystem() {
+
+    // Turret configuration
     turretLimitSwitch = new DigitalInput(Constants.ShooterConstants.TURRET_LIMIT_CHANNEL);
-    //turretMotor = new TalonFX(Constants.ShooterConstants.TURRET_MOTOR_ID);
+    turretMotor = new TalonFX(Constants.ShooterConstants.TURRET_MOTOR_ID);
-    //SetMotorDirection(turretMotor, Constants.ShooterConstants.TURRET_MOTOR_DIRECTION);
+    turretMotor.getConfigurator().apply(
+      new TalonFXConfiguration()
+        .withSlot0(
+          new Slot0Configs()
+            .withKP(6)
+            .withKI(0)
+            .withKD(0.3)
+            .withKS(0)
+        )
+        .withMotorOutput(
+          new MotorOutputConfigs()
+            .withInverted(InvertedValue.Clockwise_Positive)
+        )
+        .withCurrentLimits(
+          new CurrentLimitsConfigs()
+            .withStatorCurrentLimit(50)
+        )
+        .withSoftwareLimitSwitch(new SoftwareLimitSwitchConfigs().withForwardSoftLimitEnable(false).withReverseSoftLimitEnable(false))
+        .withFeedback(
+          new FeedbackConfigs()
+            .withFeedbackRotorOffset(0)
+        )
+    );
+    turretMotorPosition = turretMotor.getPosition();
+    turretMotorForwardLimit = turretMotor.getForwardLimit();
+    turretMotorReverseLimit = turretMotor.getReverseLimit();
+    turretPositionError = turretMotor.getClosedLoopError();
 
+    turretSim = turretMotor.getSimState();
+
+    // Launcher configuration
     launcherMotor = new SparkMax(Constants.ShooterConstants.LAUNCHER_MOTOR_ID, MotorType.kBrushless);
-    SparkMaxConfig config = new SparkMaxConfig()
+    SparkMaxConfig config = new SparkMaxConfig().apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO);
-      .apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO_2);
     config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED);
+    config.voltageCompensation(10);
+    config.closedLoop.p(0.0002).i(0.000001).d(0.0005); // I = 0.0000001
+    config.closedLoop.feedForward.kS(0.2).kV(0.00005).kA(0.001);
+    // config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0);
+    config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
+    config.closedLoop.positionWrappingEnabled(false);
+    // config.closedLoopRampRate(0.5);
+    // config.smartCurrentLimit(5, 50);
+    launcherMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+    launcherControler = launcherMotor.getClosedLoopController();
 
+    // Hood configuration
     hood = new Servo(Constants.ShooterConstants.HOOD_SERVO_CHANNEL);
-    currentState = TurretState.INIT;
+    turretState = TurretState.INIT;
-    limitNeg = Constants.ShooterConstants.LIMIT_AXIMUTH_NEG;
-    limitPos = Constants.ShooterConstants.LIMIT_AXIMUTH_POS;
-    turretGoingPos = false;
   }
 
-  public void setLauncher(double speed) {launcherMotor.set(speed);}
+  public void setLauncher(double speed) {
-
+    if(speed == 0)
-  public void setTargetElevation(Angle elev)
     {
-    if(Constants.ShooterConstants.HOOD_SERVO_INVERTED){
+        launcherMotor.stopMotor();
-      elev=Units.Degrees.of(300).minus(elev);
+        return;
     }
-    elev=elev.div(1.5);
+    launcherControler.setSetpoint(speed, ControlType.kVelocity, ClosedLoopSlot.kSlot0);
-    elev=elev.div(Constants.ShooterConstants.DEFLECTOR_SERVO_RATIO);
-    hood.setAngle(elev.in(Units.Degrees));
   }
 
-  public void startHoming()
+  public boolean isLauncherUpToSpeed() {
+    // Valid if velcity is within 10% of setpoint.
+    // May need to lower this
+  return true;
+    // return Math.abs(launcherControler.getSetpoint() - launcherMotor.getEncoder().getVelocity()) >= launcherControler.getSetpoint() * 0.03;
+  }
+
+  public void setTargetElevation(double elev)
   {
-    //turretMotor.set(Constants.ShooterConstants.HOMING_SPEED);
+    double pos = Math.abs((elev - 300)/300);
-    //currentState = TurretState.HOMING;
+    Logger.recordOutput("ShooterHood-Angle", pos);
+    hood.set(pos);
   }
 
-  public void startRotate(double speed)
+  public Command revSpeedCommand()
   {
-    /*turretGoingPos=speed>0;
+    return this.runOnce(() -> setLauncher(-.5));
+  }
 
-    if(!passedLimit())
+  public Command setShooter(int speedRPM){
+    return Commands.runEnd(() -> {
+      setLauncher(speedRPM);
+    }, () -> {
+      launcherMotor.stopMotor();
+    }, this);
+  }
+
+  public void homeTurret()
+  {
+    if(turretState != TurretState.INIT){
+      return; 
+    }
+    turretMotor.set(Constants.ShooterConstants.HOMING_SPEED);
+    turretState = TurretState.HOMING;
+  }
+
+  public boolean isTurretReady(){
+    return turretState == TurretState.READY;
+  }
+
+  public void turretRotateDir(double speed)
   {
     turretMotor.set(speed);
   }
-    else
+
+  public double getTurretAngleRobotRelative()
   {
-      turretMotor.set(0);
+    return getTurretMotorPosition() / Constants.ShooterConstants.TURRET_GEAR_RATIO;
-    }*/
   }
 
-  public boolean passedLimit(){
+  public void setTurretTargetPose(Pose3d targetPose){
-    return (getTurretAngle().gt(limitPos) && turretGoingPos) || (getTurretAngle().lt(limitNeg) && !turretGoingPos);
+    turretTarget = targetPose;
   }
 
-  public Angle getTurretAngle()
+  public Pose3d getTurretTargetPose(){
-  {
+    return turretTarget;
-    // return turretMotor.getPosition().refresh().getValue().times(Constants.ShooterConstants.TURRET_GEAR_RATIO);
+  }
-    return Units.Degrees.of(0.0);
+
+  private double getTurretMotorPosition(){
+    return turretMotorPosition.refresh().getValueAsDouble() - turretOffsetInRotations;
   }
 
   @Override
   public void periodic() {
-    // This method will be called once per scheduler run
+    if (turretState == TurretState.HOMING) {
-    switch (currentState) {
+      if (turretLimitSwitch.get()) { // Turret has tripped the switch
-      case HOMING:
+        turretMotor.stopMotor();
-        periodicHoming();
+        turretOffsetInRotations = (turretMotorPosition.refresh().getValueAsDouble() - 1.323);
-        break;
+        if (!Robot.isReal()) {
-      case MANUAL:
+          turretOffsetInRotations = 0; // Flip turret around
-        periodicManual();
+        }
-        break;
+        turretState = TurretState.READY;
-
+        forwardLimit = (1.5 + turretOffsetInRotations);
-      default:
+        reverseLimit = (-1.5 + turretOffsetInRotations);
-        break;
+        Logger.recordOutput("TurretSubsystem-TurretForwardLimit", forwardLimit);
+        Logger.recordOutput("TurretSubsystem-TurretReverseLimit", reverseLimit);
+        turretMotor.getConfigurator().apply(
+            new SoftwareLimitSwitchConfigs()
+                .withForwardSoftLimitEnable(true)
+                .withForwardSoftLimitThreshold(forwardLimit)
+                .withReverseSoftLimitEnable(true)
+                .withReverseSoftLimitThreshold(reverseLimit));
+        // setTurretRobotRelative(0);
+        // setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY
       }
     }
 
-  private void periodicHoming()
+    if (LOGGING) {
+      Logger.recordOutput("TurretSubsystem-TurretLimitSwitch", turretLimitSwitch.get());
+      Logger.recordOutput("TurretSubsystem-Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity());
+      Logger.recordOutput("TurretSubsystem-TurretState", turretState);
+      Logger.recordOutput("TurretSubsystem-TurretPositionRobotRelative", getTurretAngleRobotRelative());
+      Logger.recordOutput("TurretSubsystem-TurretMotorPositionWithOffset", getTurretMotorPosition());
+      Logger.recordOutput("TurretSubsystem-TurretMotorPositionRaw",
+          turretMotorPosition.refresh().getValue().in(Units.Rotations));
+      Logger.recordOutput("TurretSubsystem-TurretRequestedPosRaw",
+          turretControlMode.getPositionMeasure().in(Units.Rotations));
+      Logger.recordOutput("TurretSubsystem-TurretRequestedPos", getRequestedPosTurretRelative());
+      Logger.recordOutput("TurretSubsystem-TurretOffset", turretOffsetInRotations);
+      Logger.recordOutput("TurretSubsystem-TurretForwardLimitHit", turretMotorForwardLimit.refresh().getValue());
+      Logger.recordOutput("TurretSubsystem-TurretReverseLimitHit", turretMotorReverseLimit.refresh().getValue());
+      Logger.recordOutput("TurretSubsystem-TurretPositionError", turretPositionError.refresh().getValueAsDouble());
+      Logger.recordOutput("TurretSubsystem-LauncherSetpoint(RPM)", launcherControler.getSetpoint());
+    }
+
+  }
+
+  public void setTurretFieldRelative(Pose3d robotPose, double pos)
   {
-    if(turretLimitSwitch.get())
+    if(turretState != TurretState.READY)
     {
-      currentState = TurretState.MANUAL;
+      // Not ready to set angle
-      //turretMotor.set(0);
+      return;
-      //turretMotor.setPosition(0);
+    }
+
+    double correctedPos = -pos; // Flip angle to match CCW positive. Add 0.5 to flip around
+
+    double robotRotations = robotPose.getRotation().toRotation2d().getRotations();
+
+    Rotation2d turretAngle = Rotation2d.fromRotations(correctedPos).plus(Rotation2d.fromRotations(robotRotations)).minus(Rotation2d.fromRotations(0.5));
+
+    Logger.recordOutput("TurretSubsystem-TurretFieldTarget", turretAngle.getRotations());
+
+    setTurretRobotRelative(((turretAngle.getRotations())));
+  }
+
+  public void setTurretRobotRelative(double pos)
+  {
+    if(turretState != TurretState.READY)
+    {
+      // Not ready to set angle
+      return;
+    }
+
+    double correctedPos = -pos; // Flip angle to match CCW positive
+
+    setTurretMotorRotation(correctedPos * Constants.ShooterConstants.TURRET_GEAR_RATIO);
+  }
+
+  public void setTurretMotorRotation(double pos){
+    double posWithOffset = pos + turretOffsetInRotations;
+
+    Logger.recordOutput("Blah", posWithOffset);
+
+    if(Robot.isSimulation()){
+      if(posWithOffset >= forwardLimit){
+        turretSim.setForwardLimit(true);
+      } else {
+        turretSim.setForwardLimit(false);
+      }
+
+      if(posWithOffset <= reverseLimit){
+        turretSim.setReverseLimit(true);
+      } else {
+        turretSim.setReverseLimit(false);
       }
     }
 
-  private void periodicManual()
+    turretMotor.setControl(turretControlMode.withPosition(posWithOffset));
-  {
+    turretSim.setRawRotorPosition(-posWithOffset);
-    //If the turret move, it will be a uh-oh, so DONT MOVE IT
-    
-    /*if(passedLimit())
-    {
-      //turretMotor.set(0);
-    }*/
   }
+
+  public boolean isTurretAtTarget(){
+    return Math.abs(turretPositionError.refresh().getValueAsDouble()) <= 0.1;
+  }
+
+  private double getRequestedPosTurretRelative() {
+    return (turretControlMode.getPositionMeasure().in(Units.Rotations) - turretOffsetInRotations) / Constants.ShooterConstants.TURRET_GEAR_RATIO;
+  }
+
+  @PostTest()
+  public void TurretSubsystem_CANDevicesConnected(){
+    assertThat(launcherMotor.getFaults().can).as("Launcher motor not connected!").isFalse();
+    assertThat(turretMotor.isConnected()).as("Turret motor not connected!").isTrue();
+  }
+
 }

vendordeps/AdvantageKit.json

@@ -1,7 +1,7 @@
 {
     "fileName": "AdvantageKit.json",
     "name": "AdvantageKit",
-    "version": "26.0.1",
+    "version": "26.0.2",
     "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
     "frcYear": "2026",
     "mavenUrls": [
@@ -12,14 +12,14 @@
         {
             "groupId": "org.littletonrobotics.akit",
             "artifactId": "akit-java",
-            "version": "26.0.1"
+            "version": "26.0.2"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "org.littletonrobotics.akit",
             "artifactId": "akit-wpilibio",
-            "version": "26.0.1",
+            "version": "26.0.2",
             "skipInvalidPlatforms": false,
             "isJar": false,
             "validPlatforms": [

vendordeps/PathplannerLib-2026.1.2.json

@@ -0,0 +1,38 @@
+{
+    "fileName": "PathplannerLib-2026.1.2.json",
+    "name": "PathplannerLib",
+    "version": "2026.1.2",
+    "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
+    "frcYear": "2026",
+    "mavenUrls": [
+        "https://3015rangerrobotics.github.io/pathplannerlib/repo"
+    ],
+    "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.pathplanner.lib",
+            "artifactId": "PathplannerLib-java",
+            "version": "2026.1.2"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "com.pathplanner.lib",
+            "artifactId": "PathplannerLib-cpp",
+            "version": "2026.1.2",
+            "libName": "PathplannerLib",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal",
+                "linuxathena",
+                "linuxarm32",
+                "linuxarm64"
+            ]
+        }
+    ]
+}

vendordeps/Phoenix6-26.1.2.json

@@ -1,7 +1,7 @@
 {
-    "fileName": "Phoenix6-26.1.1.json",
+    "fileName": "Phoenix6-26.1.2.json",
     "name": "CTRE-Phoenix (v6)",
-    "version": "26.1.1",
+    "version": "26.1.2",
     "frcYear": "2026",
     "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
     "mavenUrls": [
@@ -19,14 +19,14 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-java",
-            "version": "26.1.1"
+            "version": "26.1.2"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "api-cpp",
-            "version": "26.1.1",
+            "version": "26.1.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -40,7 +40,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "26.1.1",
+            "version": "26.1.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -54,7 +54,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "api-cpp-sim",
-            "version": "26.1.1",
+            "version": "26.1.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -68,7 +68,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "26.1.1",
+            "version": "26.1.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -82,7 +82,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "26.1.1",
+            "version": "26.1.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -96,7 +96,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "26.1.1",
+            "version": "26.1.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -110,7 +110,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "26.1.1",
+            "version": "26.1.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -124,7 +124,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "26.1.1",
+            "version": "26.1.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -138,7 +138,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFXS",
-            "version": "26.1.1",
+            "version": "26.1.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -152,7 +152,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "26.1.1",
+            "version": "26.1.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -166,7 +166,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "26.1.1",
+            "version": "26.1.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -180,7 +180,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "26.1.1",
+            "version": "26.1.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -194,7 +194,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANdi",
-            "version": "26.1.1",
+            "version": "26.1.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -208,7 +208,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANdle",
-            "version": "26.1.1",
+            "version": "26.1.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -224,7 +224,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-cpp",
-            "version": "26.1.1",
+            "version": "26.1.2",
             "libName": "CTRE_Phoenix6_WPI",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -240,7 +240,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "26.1.1",
+            "version": "26.1.2",
             "libName": "CTRE_PhoenixTools",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -256,7 +256,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "wpiapi-cpp-sim",
-            "version": "26.1.1",
+            "version": "26.1.2",
             "libName": "CTRE_Phoenix6_WPISim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -272,7 +272,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "26.1.1",
+            "version": "26.1.2",
             "libName": "CTRE_PhoenixTools_Sim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -288,7 +288,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "26.1.1",
+            "version": "26.1.2",
             "libName": "CTRE_SimTalonSRX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -304,7 +304,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "26.1.1",
+            "version": "26.1.2",
             "libName": "CTRE_SimVictorSPX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -320,7 +320,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "26.1.1",
+            "version": "26.1.2",
             "libName": "CTRE_SimPigeonIMU",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -336,7 +336,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "26.1.1",
+            "version": "26.1.2",
             "libName": "CTRE_SimProTalonFX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -352,7 +352,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFXS",
-            "version": "26.1.1",
+            "version": "26.1.2",
             "libName": "CTRE_SimProTalonFXS",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -368,7 +368,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "26.1.1",
+            "version": "26.1.2",
             "libName": "CTRE_SimProCANcoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -384,7 +384,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "26.1.1",
+            "version": "26.1.2",
             "libName": "CTRE_SimProPigeon2",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -400,7 +400,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "26.1.1",
+            "version": "26.1.2",
             "libName": "CTRE_SimProCANrange",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -416,7 +416,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANdi",
-            "version": "26.1.1",
+            "version": "26.1.2",
             "libName": "CTRE_SimProCANdi",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -432,7 +432,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANdle",
-            "version": "26.1.1",
+            "version": "26.1.2",
             "libName": "CTRE_SimProCANdle",
             "headerClassifier": "headers",
             "sharedLibrary": true,

vendordeps/REVLib.json

@@ -1,7 +1,7 @@
 {
     "fileName": "REVLib.json",
     "name": "REVLib",
-    "version": "2026.0.4",
+    "version": "2026.0.5",
     "frcYear": "2026",
     "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
     "mavenUrls": [
@@ -12,14 +12,14 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-java",
-            "version": "2026.0.4"
+            "version": "2026.0.5"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2026.0.4",
+            "version": "2026.0.5",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
@@ -34,7 +34,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "RevLibBackendDriver",
-            "version": "2026.0.4",
+            "version": "2026.0.5",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
@@ -49,7 +49,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "RevLibWpiBackendDriver",
-            "version": "2026.0.4",
+            "version": "2026.0.5",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
@@ -66,7 +66,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-cpp",
-            "version": "2026.0.4",
+            "version": "2026.0.5",
             "libName": "REVLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
@@ -83,7 +83,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2026.0.4",
+            "version": "2026.0.5",
             "libName": "REVLibDriver",
             "headerClassifier": "headers",
             "sharedLibrary": false,
@@ -100,7 +100,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "RevLibBackendDriver",
-            "version": "2026.0.4",
+            "version": "2026.0.5",
             "libName": "BackendDriver",
             "sharedLibrary": true,
             "skipInvalidPlatforms": true,
@@ -116,7 +116,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "RevLibWpiBackendDriver",
-            "version": "2026.0.4",
+            "version": "2026.0.5",
             "libName": "REVLibWpi",
             "sharedLibrary": true,
             "skipInvalidPlatforms": true,