diff --git a/src/main/deploy/pathplanner/autos/left-trench-far-depot.auto b/src/main/deploy/pathplanner/autos/depot-only.auto similarity index 62% rename from src/main/deploy/pathplanner/autos/left-trench-far-depot.auto rename to src/main/deploy/pathplanner/autos/depot-only.auto index 9f6c34b1..87f770d3 100644 --- a/src/main/deploy/pathplanner/autos/left-trench-far-depot.auto +++ b/src/main/deploy/pathplanner/autos/depot-only.auto @@ -7,19 +7,7 @@ { "type": "path", "data": { - "pathName": "left-trench-far" - } - }, - { - "type": "named", - "data": { - "name": "shoot-small-hopper" - } - }, - { - "type": "path", - "data": { - "pathName": "left-trench-depot-hub" + "pathName": "depot-gather-hub" } }, { @@ -27,15 +15,15 @@ "data": { "commands": [ { - "type": "named", + "type": "wait", "data": { - "name": "shoot-small-hopper" + "waitTime": 8.0 } }, { - "type": "wait", + "type": "named", "data": { - "waitTime": 1.0 + "name": "shoot-until-done" } } ] @@ -44,7 +32,7 @@ { "type": "path", "data": { - "pathName": "depot-coast-to-mid" + "pathName": "depot-gather-hub 2" } } ] diff --git a/src/main/deploy/pathplanner/autos/friendly-left-bump-far-depot.auto b/src/main/deploy/pathplanner/autos/friendly-left-bump-far-depot.auto index b46a2fe6..b9f5a0aa 100644 --- a/src/main/deploy/pathplanner/autos/friendly-left-bump-far-depot.auto +++ b/src/main/deploy/pathplanner/autos/friendly-left-bump-far-depot.auto @@ -25,7 +25,7 @@ { "type": "path", "data": { - "pathName": "depot-bump" + "pathName": "depot-bump-sotm" } }, { @@ -35,7 +35,7 @@ { "type": "wait", "data": { - "waitTime": 0.5 + "waitTime": 1.0 } }, { diff --git a/src/main/deploy/pathplanner/autos/friendly-left-bump-far.auto b/src/main/deploy/pathplanner/autos/friendly-left-bump-far.auto index 48feb465..ba017d69 100644 --- a/src/main/deploy/pathplanner/autos/friendly-left-bump-far.auto +++ b/src/main/deploy/pathplanner/autos/friendly-left-bump-far.auto @@ -25,19 +25,7 @@ { "type": "named", "data": { - "name": "shoot-small-hopper" - } - }, - { - "type": "named", - "data": { - "name": "coast" - } - }, - { - "type": "path", - "data": { - "pathName": "left-bump-collect" + "name": "shoot-until-done" } } ] diff --git a/src/main/deploy/pathplanner/autos/friendly-left-bump-safe-depot.auto b/src/main/deploy/pathplanner/autos/friendly-left-bump-safe-depot.auto index 023ccb0a..9372d318 100644 --- a/src/main/deploy/pathplanner/autos/friendly-left-bump-safe-depot.auto +++ b/src/main/deploy/pathplanner/autos/friendly-left-bump-safe-depot.auto @@ -22,10 +22,16 @@ "pathName": "SHOP left-bump-bump-hub" } }, + { + "type": "named", + "data": { + "name": "shoot-small-hopper" + } + }, { "type": "path", "data": { - "pathName": "depot-bump" + "pathName": "depot-bump-sotm" } }, { diff --git a/src/main/deploy/pathplanner/autos/friendly-left-bump-safe.auto b/src/main/deploy/pathplanner/autos/friendly-left-bump-safe.auto index 46a13242..075132a5 100644 --- a/src/main/deploy/pathplanner/autos/friendly-left-bump-safe.auto +++ b/src/main/deploy/pathplanner/autos/friendly-left-bump-safe.auto @@ -25,19 +25,7 @@ { "type": "named", "data": { - "name": "shoot-small-hopper" - } - }, - { - "type": "named", - "data": { - "name": "coast" - } - }, - { - "type": "path", - "data": { - "pathName": "left-bump-collect" + "name": "shoot-until-done" } } ] diff --git a/src/main/deploy/pathplanner/autos/friendly-left-trench-far-depot.auto b/src/main/deploy/pathplanner/autos/friendly-left-trench-far-depot.auto deleted file mode 100644 index ca7721eb..00000000 --- a/src/main/deploy/pathplanner/autos/friendly-left-trench-far-depot.auto +++ /dev/null @@ -1,56 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "left-trench-far" - } - }, - { - "type": "named", - "data": { - "name": "shoot-small-hopper" - } - }, - { - "type": "path", - "data": { - "pathName": "SHOP left-trench-depot-hub" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "shoot-small-hopper" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "depot-coast-to-mid" - } - } - ] - } - }, - "resetOdom": true, - "folder": "New Autos", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/friendly-left-trench-far.auto b/src/main/deploy/pathplanner/autos/friendly-left-trench-far.auto deleted file mode 100644 index 5dd24258..00000000 --- a/src/main/deploy/pathplanner/autos/friendly-left-trench-far.auto +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "left-trench-far" - } - }, - { - "type": "named", - "data": { - "name": "shoot-small-hopper" - } - }, - { - "type": "path", - "data": { - "pathName": "SHOP left-trench-trench-hub" - } - }, - { - "type": "named", - "data": { - "name": "shoot-small-hopper" - } - }, - { - "type": "named", - "data": { - "name": "coast" - } - }, - { - "type": "path", - "data": { - "pathName": "left-trench-collect" - } - } - ] - } - }, - "resetOdom": true, - "folder": "New Autos", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/friendly-left-trench-safe-depot.auto b/src/main/deploy/pathplanner/autos/friendly-left-trench-safe-depot.auto deleted file mode 100644 index c8b91b11..00000000 --- a/src/main/deploy/pathplanner/autos/friendly-left-trench-safe-depot.auto +++ /dev/null @@ -1,56 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "left-trench-safe" - } - }, - { - "type": "named", - "data": { - "name": "shoot-small-hopper" - } - }, - { - "type": "path", - "data": { - "pathName": "SHOP left-trench-depot-hub" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "shoot-small-hopper" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "depot-coast-to-mid" - } - } - ] - } - }, - "resetOdom": true, - "folder": "New Autos", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/friendly-left-trench-safe.auto b/src/main/deploy/pathplanner/autos/friendly-left-trench-safe.auto deleted file mode 100644 index a3659587..00000000 --- a/src/main/deploy/pathplanner/autos/friendly-left-trench-safe.auto +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "left-trench-safe" - } - }, - { - "type": "named", - "data": { - "name": "shoot-small-hopper" - } - }, - { - "type": "path", - "data": { - "pathName": "SHOP left-trench-trench-hub" - } - }, - { - "type": "named", - "data": { - "name": "shoot-small-hopper" - } - }, - { - "type": "named", - "data": { - "name": "coast" - } - }, - { - "type": "path", - "data": { - "pathName": "left-trench-collect" - } - } - ] - } - }, - "resetOdom": true, - "folder": "New Autos", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/friendly-right-bump-far.auto b/src/main/deploy/pathplanner/autos/friendly-right-bump-far.auto index 58fad154..9eea6443 100644 --- a/src/main/deploy/pathplanner/autos/friendly-right-bump-far.auto +++ b/src/main/deploy/pathplanner/autos/friendly-right-bump-far.auto @@ -25,19 +25,7 @@ { "type": "named", "data": { - "name": "shoot-small-hopper" - } - }, - { - "type": "named", - "data": { - "name": "coast" - } - }, - { - "type": "path", - "data": { - "pathName": "right-bump-collect" + "name": "shoot-until-done" } } ] diff --git a/src/main/deploy/pathplanner/autos/friendly-right-bump-safe.auto b/src/main/deploy/pathplanner/autos/friendly-right-bump-safe.auto index 03272219..43830e59 100644 --- a/src/main/deploy/pathplanner/autos/friendly-right-bump-safe.auto +++ b/src/main/deploy/pathplanner/autos/friendly-right-bump-safe.auto @@ -25,19 +25,7 @@ { "type": "named", "data": { - "name": "shoot-small-hopper" - } - }, - { - "type": "named", - "data": { - "name": "coast" - } - }, - { - "type": "path", - "data": { - "pathName": "right-bump-collect" + "name": "shoot-until-done" } } ] diff --git a/src/main/deploy/pathplanner/autos/friendly-right-trench-far.auto b/src/main/deploy/pathplanner/autos/friendly-right-trench-far.auto deleted file mode 100644 index 507661a9..00000000 --- a/src/main/deploy/pathplanner/autos/friendly-right-trench-far.auto +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "right-trench-far" - } - }, - { - "type": "named", - "data": { - "name": "shoot-small-hopper" - } - }, - { - "type": "path", - "data": { - "pathName": "SHOP right-trench-trench-hub" - } - }, - { - "type": "named", - "data": { - "name": "shoot-small-hopper" - } - }, - { - "type": "named", - "data": { - "name": "coast" - } - }, - { - "type": "path", - "data": { - "pathName": "right-trench-collect" - } - } - ] - } - }, - "resetOdom": true, - "folder": "New Autos", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/friendly-right-trench-safe.auto b/src/main/deploy/pathplanner/autos/friendly-right-trench-safe.auto deleted file mode 100644 index 31a7672e..00000000 --- a/src/main/deploy/pathplanner/autos/friendly-right-trench-safe.auto +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "right-trench-safe" - } - }, - { - "type": "named", - "data": { - "name": "shoot-small-hopper" - } - }, - { - "type": "path", - "data": { - "pathName": "SHOP right-trench-trench-hub" - } - }, - { - "type": "named", - "data": { - "name": "shoot-small-hopper" - } - }, - { - "type": "named", - "data": { - "name": "coast" - } - }, - { - "type": "path", - "data": { - "pathName": "right-trench-collect" - } - } - ] - } - }, - "resetOdom": true, - "folder": "New Autos", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/left-bump-far-depot.auto b/src/main/deploy/pathplanner/autos/left-bump-far-depot.auto index 3e0c2e81..01d6dd4d 100644 --- a/src/main/deploy/pathplanner/autos/left-bump-far-depot.auto +++ b/src/main/deploy/pathplanner/autos/left-bump-far-depot.auto @@ -25,7 +25,7 @@ { "type": "path", "data": { - "pathName": "depot-bump" + "pathName": "depot-bump-sotm" } }, { @@ -35,7 +35,7 @@ { "type": "wait", "data": { - "waitTime": 0.5 + "waitTime": 1.0 } }, { diff --git a/src/main/deploy/pathplanner/autos/left-bump-far.auto b/src/main/deploy/pathplanner/autos/left-bump-far.auto index d5e3c063..97de14b9 100644 --- a/src/main/deploy/pathplanner/autos/left-bump-far.auto +++ b/src/main/deploy/pathplanner/autos/left-bump-far.auto @@ -25,19 +25,7 @@ { "type": "named", "data": { - "name": "shoot-small-hopper" - } - }, - { - "type": "named", - "data": { - "name": "coast" - } - }, - { - "type": "path", - "data": { - "pathName": "left-bump-collect" + "name": "shoot-until-done" } } ] diff --git a/src/main/deploy/pathplanner/autos/left-bump-safe-depot.auto b/src/main/deploy/pathplanner/autos/left-bump-safe-depot.auto index ed3240ef..1864ecea 100644 --- a/src/main/deploy/pathplanner/autos/left-bump-safe-depot.auto +++ b/src/main/deploy/pathplanner/autos/left-bump-safe-depot.auto @@ -25,7 +25,7 @@ { "type": "path", "data": { - "pathName": "depot-bump" + "pathName": "depot-bump-sotm" } }, { diff --git a/src/main/deploy/pathplanner/autos/left-trench-far.auto b/src/main/deploy/pathplanner/autos/left-trench-far.auto deleted file mode 100644 index 3efd471a..00000000 --- a/src/main/deploy/pathplanner/autos/left-trench-far.auto +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "left-trench-far" - } - }, - { - "type": "named", - "data": { - "name": "shoot-small-hopper" - } - }, - { - "type": "path", - "data": { - "pathName": "left-trench-trench-hub" - } - }, - { - "type": "named", - "data": { - "name": "shoot-small-hopper" - } - }, - { - "type": "named", - "data": { - "name": "coast" - } - }, - { - "type": "path", - "data": { - "pathName": "left-trench-collect" - } - } - ] - } - }, - "resetOdom": true, - "folder": "New Autos", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/left-trench-safe-depot.auto b/src/main/deploy/pathplanner/autos/left-trench-safe-depot.auto deleted file mode 100644 index 9ea0a736..00000000 --- a/src/main/deploy/pathplanner/autos/left-trench-safe-depot.auto +++ /dev/null @@ -1,56 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "left-trench-safe" - } - }, - { - "type": "named", - "data": { - "name": "shoot-small-hopper" - } - }, - { - "type": "path", - "data": { - "pathName": "left-trench-depot-hub" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "shoot-small-hopper" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "depot-coast-to-mid" - } - } - ] - } - }, - "resetOdom": true, - "folder": "New Autos", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/left-trench-safe.auto b/src/main/deploy/pathplanner/autos/left-trench-safe.auto deleted file mode 100644 index 5082f408..00000000 --- a/src/main/deploy/pathplanner/autos/left-trench-safe.auto +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "left-trench-safe" - } - }, - { - "type": "named", - "data": { - "name": "shoot-small-hopper" - } - }, - { - "type": "path", - "data": { - "pathName": "left-trench-trench-hub" - } - }, - { - "type": "named", - "data": { - "name": "shoot-small-hopper" - } - }, - { - "type": "named", - "data": { - "name": "coast" - } - }, - { - "type": "path", - "data": { - "pathName": "left-trench-collect" - } - } - ] - } - }, - "resetOdom": true, - "folder": "New Autos", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/qual 62 auto.auto b/src/main/deploy/pathplanner/autos/qual 62 auto.auto deleted file mode 100644 index 2fea88d9..00000000 --- a/src/main/deploy/pathplanner/autos/qual 62 auto.auto +++ /dev/null @@ -1,37 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "qual 62 right-bump-far" - } - }, - { - "type": "named", - "data": { - "name": "shoot-small-hopper" - } - }, - { - "type": "path", - "data": { - "pathName": "right-bump-bump-hub" - } - }, - { - "type": "named", - "data": { - "name": "shoot-small-hopper" - } - } - ] - } - }, - "resetOdom": true, - "folder": "New Autos", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/right-bump-far.auto b/src/main/deploy/pathplanner/autos/right-bump-far.auto index e61816c7..60698e75 100644 --- a/src/main/deploy/pathplanner/autos/right-bump-far.auto +++ b/src/main/deploy/pathplanner/autos/right-bump-far.auto @@ -25,19 +25,7 @@ { "type": "named", "data": { - "name": "shoot-small-hopper" - } - }, - { - "type": "named", - "data": { - "name": "coast" - } - }, - { - "type": "path", - "data": { - "pathName": "right-bump-collect" + "name": "shoot-until-done" } } ] diff --git a/src/main/deploy/pathplanner/autos/right-trench-far.auto b/src/main/deploy/pathplanner/autos/right-trench-far.auto deleted file mode 100644 index 07abd8d1..00000000 --- a/src/main/deploy/pathplanner/autos/right-trench-far.auto +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "right-trench-far" - } - }, - { - "type": "named", - "data": { - "name": "shoot-small-hopper" - } - }, - { - "type": "path", - "data": { - "pathName": "right-trench-hub" - } - }, - { - "type": "named", - "data": { - "name": "shoot-small-hopper" - } - }, - { - "type": "named", - "data": { - "name": "coast" - } - }, - { - "type": "path", - "data": { - "pathName": "right-trench-collect" - } - } - ] - } - }, - "resetOdom": true, - "folder": "New Autos", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/right-trench-safe.auto b/src/main/deploy/pathplanner/autos/right-trench-safe.auto deleted file mode 100644 index c2e70930..00000000 --- a/src/main/deploy/pathplanner/autos/right-trench-safe.auto +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "right-trench-safe" - } - }, - { - "type": "named", - "data": { - "name": "shoot-small-hopper" - } - }, - { - "type": "path", - "data": { - "pathName": "right-trench-hub" - } - }, - { - "type": "named", - "data": { - "name": "shoot-small-hopper" - } - }, - { - "type": "named", - "data": { - "name": "coast" - } - }, - { - "type": "path", - "data": { - "pathName": "right-trench-collect" - } - } - ] - } - }, - "resetOdom": true, - "folder": "New Autos", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/test auto.auto b/src/main/deploy/pathplanner/autos/test auto.auto index 2e76bf5c..4b7cf625 100644 --- a/src/main/deploy/pathplanner/autos/test auto.auto +++ b/src/main/deploy/pathplanner/autos/test auto.auto @@ -7,7 +7,7 @@ { "type": "path", "data": { - "pathName": "sotm-test" + "pathName": "Rotation Testing" } } ] diff --git a/src/main/deploy/pathplanner/paths/Rotation Testing.path b/src/main/deploy/pathplanner/paths/Rotation Testing.path new file mode 100644 index 00000000..b982011a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Rotation Testing.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.0, + "y": 5.5 + }, + "prevControl": null, + "nextControl": { + "x": 1.0, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": { + "x": 1.0, + "y": 7.0 + }, + "nextControl": { + "x": 3.0, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.0, + "y": 5.5 + }, + "prevControl": { + "x": 3.0, + "y": 7.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 480.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Testing", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SHOP depot-bump.path b/src/main/deploy/pathplanner/paths/SHOP depot-bump.path index c212c9ce..b41d4e0e 100644 --- a/src/main/deploy/pathplanner/paths/SHOP depot-bump.path +++ b/src/main/deploy/pathplanner/paths/SHOP depot-bump.path @@ -106,7 +106,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/SHOP depot-transition-bump.path b/src/main/deploy/pathplanner/paths/SHOP depot-transition-bump.path index 405cf3f1..53d5cd0f 100644 --- a/src/main/deploy/pathplanner/paths/SHOP depot-transition-bump.path +++ b/src/main/deploy/pathplanner/paths/SHOP depot-transition-bump.path @@ -16,15 +16,15 @@ }, { "anchor": { - "x": 3.353907563025211, + "x": 3.45, "y": 5.5 }, "prevControl": { - "x": 2.853907563025211, + "x": 2.95, "y": 5.5 }, "nextControl": { - "x": 4.35390756302521, + "x": 4.449999999999999, "y": 5.5 }, "isLocked": false, @@ -76,7 +76,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/SHOP left-bump-bump-hub.path b/src/main/deploy/pathplanner/paths/SHOP left-bump-bump-hub.path index 33bba626..abb9b243 100644 --- a/src/main/deploy/pathplanner/paths/SHOP left-bump-bump-hub.path +++ b/src/main/deploy/pathplanner/paths/SHOP left-bump-bump-hub.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 2.3, - "y": 7.449999999999999 + "y": 7.45 }, "isLocked": false, "linkedName": "bump-left-alliance" @@ -146,7 +146,21 @@ "rotationDegrees": 135.0 } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 6.6, + "maxWaypointRelativePos": 7.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 480.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [ { @@ -175,7 +189,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/SHOP left-bump-trench-hub.path b/src/main/deploy/pathplanner/paths/SHOP left-bump-trench-hub.path index 465a9e38..b018cad0 100644 --- a/src/main/deploy/pathplanner/paths/SHOP left-bump-trench-hub.path +++ b/src/main/deploy/pathplanner/paths/SHOP left-bump-trench-hub.path @@ -185,7 +185,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/SHOP left-trench-depot-hub.path b/src/main/deploy/pathplanner/paths/SHOP left-trench-depot-hub.path index e59871f5..ba4fed77 100644 --- a/src/main/deploy/pathplanner/paths/SHOP left-trench-depot-hub.path +++ b/src/main/deploy/pathplanner/paths/SHOP left-trench-depot-hub.path @@ -204,7 +204,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/SHOP left-trench-trench-hub.path b/src/main/deploy/pathplanner/paths/SHOP left-trench-trench-hub.path index a7840e46..88e20496 100644 --- a/src/main/deploy/pathplanner/paths/SHOP left-trench-trench-hub.path +++ b/src/main/deploy/pathplanner/paths/SHOP left-trench-trench-hub.path @@ -173,7 +173,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/SHOP right-bump-bump-hub.path b/src/main/deploy/pathplanner/paths/SHOP right-bump-bump-hub.path index 3f329638..328f2a13 100644 --- a/src/main/deploy/pathplanner/paths/SHOP right-bump-bump-hub.path +++ b/src/main/deploy/pathplanner/paths/SHOP right-bump-bump-hub.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 2.2, - "y": 0.6000000000000005 + "y": 0.5999999999999999 }, "isLocked": false, "linkedName": "bump-alliance-right" @@ -146,7 +146,21 @@ "rotationDegrees": -135.0 } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 6.6, + "maxWaypointRelativePos": 7.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 480.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [ { @@ -160,6 +174,17 @@ } } }, + { + "name": "sotm-until-done", + "waypointRelativePos": 0, + "endWaypointRelativePos": 0.33, + "command": { + "type": "named", + "data": { + "name": "sotm-until-done" + } + } + }, { "name": "spool-shooter", "waypointRelativePos": 6.0, @@ -175,7 +200,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/SHOP right-bump-trench-hub.path b/src/main/deploy/pathplanner/paths/SHOP right-bump-trench-hub.path index fa968254..0adf9934 100644 --- a/src/main/deploy/pathplanner/paths/SHOP right-bump-trench-hub.path +++ b/src/main/deploy/pathplanner/paths/SHOP right-bump-trench-hub.path @@ -185,7 +185,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/SHOP right-trench-trench-hub.path b/src/main/deploy/pathplanner/paths/SHOP right-trench-trench-hub.path index b8f625e0..e34b4346 100644 --- a/src/main/deploy/pathplanner/paths/SHOP right-trench-trench-hub.path +++ b/src/main/deploy/pathplanner/paths/SHOP right-trench-trench-hub.path @@ -173,7 +173,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/bump-bump.path b/src/main/deploy/pathplanner/paths/bump-bump.path new file mode 100644 index 00000000..25b804e7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/bump-bump.path @@ -0,0 +1,93 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.45, + "y": 5.5 + }, + "prevControl": null, + "nextControl": { + "x": 2.465544967488438, + "y": 5.5 + }, + "isLocked": false, + "linkedName": "start-bump-left-alliance" + }, + { + "anchor": { + "x": 2.3, + "y": 5.5 + }, + "prevControl": { + "x": 2.55, + "y": 5.5 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "bump-left-alliance" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.6, + "y": 4.06 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 1.0, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [ + { + "name": "sotm-small-hopper", + "waypointRelativePos": 0.0, + "endWaypointRelativePos": 1.0, + "command": { + "type": "named", + "data": { + "name": "sotm-small-hopper" + } + } + }, + { + "name": "intake", + "waypointRelativePos": 0.0, + "endWaypointRelativePos": 1.0, + "command": { + "type": "named", + "data": { + "name": "intake" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 480.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 2.0, + "rotation": 145.0 + }, + "reversed": false, + "folder": "Depot", + "idealStartingState": { + "velocity": 0.0, + "rotation": 135.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/coasting-test.path b/src/main/deploy/pathplanner/paths/coasting-test.path index 4850ebb4..62192c1b 100644 --- a/src/main/deploy/pathplanner/paths/coasting-test.path +++ b/src/main/deploy/pathplanner/paths/coasting-test.path @@ -17,11 +17,19 @@ { "anchor": { "x": 8.25, +<<<<<<< HEAD + "y": 4.035 + }, + "prevControl": { + "x": 7.2499999999999964, + "y": 4.035 +======= "y": 7.138469879518072 }, "prevControl": { "x": 7.2499999999999964, "y": 7.138469879518072 +>>>>>>> e470d1a (hilltoppers) }, "nextControl": null, "isLocked": false, @@ -46,8 +54,8 @@ ], "globalConstraints": { "maxVelocity": 5.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/depot-bump-sotm.path b/src/main/deploy/pathplanner/paths/depot-bump-sotm.path new file mode 100644 index 00000000..4f341d3a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/depot-bump-sotm.path @@ -0,0 +1,107 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.3, + "y": 5.5 + }, + "prevControl": null, + "nextControl": { + "x": 1.3155449674884379, + "y": 5.5 + }, + "isLocked": false, + "linkedName": "bump-left-alliance" + }, + { + "anchor": { + "x": 0.7, + "y": 5.803268907563026 + }, + "prevControl": { + "x": 0.9500000000000002, + "y": 5.803268907563026 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "depot-gather-bump" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 0.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.6, + "y": 4.06 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 1.0, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [ + { + "name": "sotm-small-hopper", + "waypointRelativePos": 0.0, + "endWaypointRelativePos": 1.0, + "command": { + "type": "named", + "data": { + "name": "sotm-small-hopper" + } + } + }, + { + "name": "intake", + "waypointRelativePos": 0.0, + "endWaypointRelativePos": 1.0, + "command": { + "type": "named", + "data": { + "name": "intake" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 480.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Depot", + "idealStartingState": { + "velocity": 1.0, + "rotation": 145.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/depot-bump.path b/src/main/deploy/pathplanner/paths/depot-bump.path index 64998fd4..24155a93 100644 --- a/src/main/deploy/pathplanner/paths/depot-bump.path +++ b/src/main/deploy/pathplanner/paths/depot-bump.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 1.3155449674884379, - "y": 5.5 + "x": 1.8662392961243413, + "y": 5.565777040367884 }, "isLocked": false, "linkedName": "bump-left-alliance" @@ -20,20 +20,15 @@ "y": 5.803268907563026 }, "prevControl": { - "x": 0.9500000000000002, - "y": 5.803268907563026 + "x": 0.9494249769327897, + "y": 5.78632249549211 }, "nextControl": null, "isLocked": false, "linkedName": "depot-gather-bump" } ], - "rotationTargets": [ - { - "waypointRelativePos": 1, - "rotationDegrees": 180.0 - } - ], + "rotationTargets": [], "constraintZones": [ { "name": "Constraints Zone", @@ -41,7 +36,7 @@ "maxWaypointRelativePos": 1.0, "constraints": { "maxVelocity": 1.0, - "maxAcceleration": 3.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -49,30 +44,8 @@ } } ], - "pointTowardsZones": [ - { - "fieldPosition": { - "x": 4.6, - "y": 4.06 - }, - "rotationOffset": 180.0, - "minWaypointRelativePos": 0.0, - "maxWaypointRelativePos": 1.0, - "name": "Point Towards Zone" - } - ], + "pointTowardsZones": [], "eventMarkers": [ - { - "name": "sotm-small-hopper", - "waypointRelativePos": 0.0, - "endWaypointRelativePos": 1.0, - "command": { - "type": "named", - "data": { - "name": "sotm-small-hopper" - } - } - }, { "name": "intake", "waypointRelativePos": 0.0, @@ -88,7 +61,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false @@ -100,7 +73,7 @@ "reversed": false, "folder": "Depot", "idealStartingState": { - "velocity": 1.0, + "velocity": 2.0, "rotation": 145.0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/depot-coast-to-mid.path b/src/main/deploy/pathplanner/paths/depot-coast-to-mid.path index 1892f832..c5fe309c 100644 --- a/src/main/deploy/pathplanner/paths/depot-coast-to-mid.path +++ b/src/main/deploy/pathplanner/paths/depot-coast-to-mid.path @@ -79,7 +79,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/depot-gather-hub 2.path b/src/main/deploy/pathplanner/paths/depot-gather-hub 2.path new file mode 100644 index 00000000..ce17fc7d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/depot-gather-hub 2.path @@ -0,0 +1,80 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.7, + "y": 6.3 + }, + "prevControl": null, + "nextControl": { + "x": 1.7, + "y": 6.8 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.7, + "y": 7.542795180722893 + }, + "prevControl": { + "x": 1.7, + "y": 7.292795180722893 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intake", + "waypointRelativePos": 0.0, + "endWaypointRelativePos": 2.0, + "command": { + "type": "named", + "data": { + "name": "intake" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 480.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Depot", + "idealStartingState": { + "velocity": 0.0, + "rotation": 135.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/depot-gather-hub.path b/src/main/deploy/pathplanner/paths/depot-gather-hub.path new file mode 100644 index 00000000..789d7348 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/depot-gather-hub.path @@ -0,0 +1,128 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.55, + "y": 4.8 + }, + "prevControl": null, + "nextControl": { + "x": 2.5500000000000003, + "y": 4.8 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.5878192771084336, + "y": 4.9 + }, + "prevControl": { + "x": 0.7645959724050705, + "y": 4.723223304703364 + }, + "nextControl": { + "x": 0.41104258181179676, + "y": 5.076776695296637 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.5, + "y": 6.0 + }, + "prevControl": { + "x": 0.5, + "y": 5.75 + }, + "nextControl": { + "x": 0.5000000000000001, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.7, + "y": 6.3 + }, + "prevControl": { + "x": 1.1056189135229162, + "y": 6.894381086477083 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 2.0, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 3.0, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "deploy", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "deploy" + } + } + }, + { + "name": "intake", + "waypointRelativePos": 0.5, + "endWaypointRelativePos": 2.0, + "command": { + "type": "named", + "data": { + "name": "intake" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 480.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 135.0 + }, + "reversed": false, + "folder": "Depot", + "idealStartingState": { + "velocity": 0.0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/depot-near-trench.path b/src/main/deploy/pathplanner/paths/depot-near-trench.path new file mode 100644 index 00000000..2ce560ca --- /dev/null +++ b/src/main/deploy/pathplanner/paths/depot-near-trench.path @@ -0,0 +1,80 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.7, + "y": 5.803268907563026 + }, + "prevControl": null, + "nextControl": { + "x": 0.7000000000000001, + "y": 6.803268907563026 + }, + "isLocked": false, + "linkedName": "depot-gather-bump" + }, + { + "anchor": { + "x": 2.5, + "y": 7.4 + }, + "prevControl": { + "x": 1.5, + "y": 7.4 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "intake", + "waypointRelativePos": 0.0, + "endWaypointRelativePos": 1.0, + "command": { + "type": "named", + "data": { + "name": "intake" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 480.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Depot", + "idealStartingState": { + "velocity": 2.0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/depot-transition-SOTM.path b/src/main/deploy/pathplanner/paths/depot-transition-SOTM.path index 0d128ca7..895541a1 100644 --- a/src/main/deploy/pathplanner/paths/depot-transition-SOTM.path +++ b/src/main/deploy/pathplanner/paths/depot-transition-SOTM.path @@ -104,7 +104,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/left-bump-bump-hub.path b/src/main/deploy/pathplanner/paths/left-bump-bump-hub.path index b07d9799..11ca43aa 100644 --- a/src/main/deploy/pathplanner/paths/left-bump-bump-hub.path +++ b/src/main/deploy/pathplanner/paths/left-bump-bump-hub.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 2.3, - "y": 7.449999999999999 + "y": 7.45 }, "isLocked": false, "linkedName": "bump-left-alliance" @@ -146,7 +146,21 @@ "rotationDegrees": 135.0 } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 6.6, + "maxWaypointRelativePos": 7.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 480.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [ { @@ -175,7 +189,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/left-bump-collect.path b/src/main/deploy/pathplanner/paths/left-bump-collect.path index 5b3d23b4..4d2196ae 100644 --- a/src/main/deploy/pathplanner/paths/left-bump-collect.path +++ b/src/main/deploy/pathplanner/paths/left-bump-collect.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 2.3, - "y": 6.9 + "y": 6.5 }, "isLocked": false, "linkedName": "bump-left-alliance" @@ -50,8 +50,33 @@ "rotationDegrees": 0.0 } ], - "constraintZones": [], - "pointTowardsZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.33, + "constraints": { + "maxVelocity": 0.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 480.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.6, + "y": 4.06 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.33, + "name": "Point Towards Zone" + } + ], "eventMarkers": [ { "name": "intake", @@ -74,12 +99,23 @@ "name": "coast" } } + }, + { + "name": "sotm-until-done", + "waypointRelativePos": 0.0, + "endWaypointRelativePos": 0.33, + "command": { + "type": "named", + "data": { + "name": "sotm-until-done" + } + } } ], "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/left-bump-far.path b/src/main/deploy/pathplanner/paths/left-bump-far.path index a429c4cd..5a79d1b3 100644 --- a/src/main/deploy/pathplanner/paths/left-bump-far.path +++ b/src/main/deploy/pathplanner/paths/left-bump-far.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 4.4, + "x": 4.7, "y": 7.5 }, "prevControl": null, "nextControl": { - "x": 5.400000000000004, + "x": 5.700000000000004, "y": 7.5 }, "isLocked": false, @@ -110,7 +110,47 @@ "rotationDegrees": 135.0 } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 5.0, + "maxAcceleration": 100.0, + "maxAngularVelocity": 480.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 4.6, + "maxWaypointRelativePos": 5.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 480.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.0, + "maxWaypointRelativePos": 3.0, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 480.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [ { @@ -150,7 +190,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/left-bump-safe.path b/src/main/deploy/pathplanner/paths/left-bump-safe.path index 9945d587..60bbf9d3 100644 --- a/src/main/deploy/pathplanner/paths/left-bump-safe.path +++ b/src/main/deploy/pathplanner/paths/left-bump-safe.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 4.4, + "x": 4.7, "y": 7.5 }, "prevControl": null, "nextControl": { - "x": 5.400000000000004, + "x": 5.700000000000004, "y": 7.5 }, "isLocked": false, @@ -110,7 +110,47 @@ "rotationDegrees": 135.0 } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 5.0, + "maxAcceleration": 100.0, + "maxAngularVelocity": 480.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 4.6, + "maxWaypointRelativePos": 5.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 480.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.0, + "maxWaypointRelativePos": 4.0, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 480.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [ { @@ -150,7 +190,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/left-bump-trench-hub.path b/src/main/deploy/pathplanner/paths/left-bump-trench-hub.path index 7547ce54..d3dab0b1 100644 --- a/src/main/deploy/pathplanner/paths/left-bump-trench-hub.path +++ b/src/main/deploy/pathplanner/paths/left-bump-trench-hub.path @@ -185,7 +185,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/left-right-alliance-close-hub.path b/src/main/deploy/pathplanner/paths/left-right-alliance-close-hub.path new file mode 100644 index 00000000..ddb7b9e8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/left-right-alliance-close-hub.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.101192771084338, + "y": 5.215192771084337 + }, + "prevControl": null, + "nextControl": { + "x": 3.277969466380975, + "y": 5.0384160757877 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.5, + "y": 4.5 + }, + "prevControl": { + "x": 3.323223304703363, + "y": 4.6767766952966365 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.0, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Teleop Pathing", + "idealStartingState": { + "velocity": 0.0, + "rotation": -90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/left-right-alliance-far-hub.path b/src/main/deploy/pathplanner/paths/left-right-alliance-far-hub.path new file mode 100644 index 00000000..0c204bf9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/left-right-alliance-far-hub.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 13.1, + "y": 5.0 + }, + "prevControl": null, + "nextControl": { + "x": 13.1, + "y": 4.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 13.1, + "y": 3.5 + }, + "prevControl": { + "x": 13.1, + "y": 4.500000000000001 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.0, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 1.5, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Teleop Pathing", + "idealStartingState": { + "velocity": 1.5, + "rotation": -90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/left-right-mid-close-hub.path b/src/main/deploy/pathplanner/paths/left-right-mid-close-hub.path new file mode 100644 index 00000000..119b56b5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/left-right-mid-close-hub.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.8, + "y": 5.0 + }, + "prevControl": null, + "nextControl": { + "x": 5.8, + "y": 4.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.8, + "y": 3.5 + }, + "prevControl": { + "x": 5.8, + "y": 4.500000000000001 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.0, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 1.5, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Teleop Pathing", + "idealStartingState": { + "velocity": 1.5, + "rotation": -90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/left-right-mid-far-hub.path b/src/main/deploy/pathplanner/paths/left-right-mid-far-hub.path new file mode 100644 index 00000000..1a6b6c06 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/left-right-mid-far-hub.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 10.75, + "y": 5.0 + }, + "prevControl": null, + "nextControl": { + "x": 10.75, + "y": 4.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 10.75, + "y": 3.5 + }, + "prevControl": { + "x": 10.75, + "y": 4.500000000000001 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.0, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 1.5, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Teleop Pathing", + "idealStartingState": { + "velocity": 1.5, + "rotation": -90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/left-trench-collect.path b/src/main/deploy/pathplanner/paths/left-trench-collect.path index 2be9a604..924c8d83 100644 --- a/src/main/deploy/pathplanner/paths/left-trench-collect.path +++ b/src/main/deploy/pathplanner/paths/left-trench-collect.path @@ -88,7 +88,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/left-trench-depot-hub.path b/src/main/deploy/pathplanner/paths/left-trench-depot-hub.path index 12372fa4..2356fd96 100644 --- a/src/main/deploy/pathplanner/paths/left-trench-depot-hub.path +++ b/src/main/deploy/pathplanner/paths/left-trench-depot-hub.path @@ -204,7 +204,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/left-trench-far.path b/src/main/deploy/pathplanner/paths/left-trench-far.path index 582fb82f..514cb2b6 100644 --- a/src/main/deploy/pathplanner/paths/left-trench-far.path +++ b/src/main/deploy/pathplanner/paths/left-trench-far.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 4.4, + "x": 4.7, "y": 7.5 }, "prevControl": null, "nextControl": { - "x": 5.400000000000004, + "x": 5.700000000000004, "y": 7.5 }, "isLocked": false, @@ -110,7 +110,21 @@ "rotationDegrees": 90.0 } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 3.0, + "constraints": { + "maxVelocity": 5.0, + "maxAcceleration": 100.0, + "maxAngularVelocity": 480.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [ { @@ -150,7 +164,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/left-trench-safe.path b/src/main/deploy/pathplanner/paths/left-trench-safe.path index d340f603..0dbb8cb6 100644 --- a/src/main/deploy/pathplanner/paths/left-trench-safe.path +++ b/src/main/deploy/pathplanner/paths/left-trench-safe.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 4.4, + "x": 4.7, "y": 7.5 }, "prevControl": null, "nextControl": { - "x": 5.400000000000004, + "x": 5.700000000000004, "y": 7.5 }, "isLocked": false, @@ -110,7 +110,21 @@ "rotationDegrees": 90.0 } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 3.0, + "constraints": { + "maxVelocity": 5.0, + "maxAcceleration": 100.0, + "maxAngularVelocity": 480.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [ { @@ -150,7 +164,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/left-trench-trench-hub.path b/src/main/deploy/pathplanner/paths/left-trench-trench-hub.path index 8bb11c26..a582292c 100644 --- a/src/main/deploy/pathplanner/paths/left-trench-trench-hub.path +++ b/src/main/deploy/pathplanner/paths/left-trench-trench-hub.path @@ -173,7 +173,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/qual 62 right-bump-far.path b/src/main/deploy/pathplanner/paths/qual 62 right-bump-far.path deleted file mode 100644 index 32ea641f..00000000 --- a/src/main/deploy/pathplanner/paths/qual 62 right-bump-far.path +++ /dev/null @@ -1,169 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.5, - "y": 0.55 - }, - "prevControl": null, - "nextControl": { - "x": 4.5000000000000036, - "y": 0.55 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.4, - "y": 1.55 - }, - "prevControl": { - "x": 8.4, - "y": 0.4500000000000006 - }, - "nextControl": { - "x": 8.4, - "y": 2.55 - }, - "isLocked": false, - "linkedName": "fuel-far-right-entrance" - }, - { - "anchor": { - "x": 8.4, - "y": 3.05 - }, - "prevControl": { - "x": 8.4, - "y": 2.55 - }, - "nextControl": { - "x": 8.4, - "y": 4.05 - }, - "isLocked": false, - "linkedName": "fuel-mid-exit-1-right" - }, - { - "anchor": { - "x": 7.3, - "y": 3.05 - }, - "prevControl": { - "x": 7.3, - "y": 4.05 - }, - "nextControl": { - "x": 7.3, - "y": 2.55 - }, - "isLocked": false, - "linkedName": "fuel-mid-exit-2-right" - }, - { - "anchor": { - "x": 6.0, - "y": 2.55 - }, - "prevControl": { - "x": 6.5, - "y": 2.55 - }, - "nextControl": { - "x": 5.5, - "y": 2.55 - }, - "isLocked": false, - "linkedName": "bump-center-right" - }, - { - "anchor": { - "x": 2.2, - "y": 2.55 - }, - "prevControl": { - "x": 2.45, - "y": 2.55 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "bump-alliance-right" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.95, - "rotationDegrees": 100.0 - }, - { - "waypointRelativePos": 2.0, - "rotationDegrees": 100.0 - }, - { - "waypointRelativePos": 3.0, - "rotationDegrees": -90.0 - }, - { - "waypointRelativePos": 4.0, - "rotationDegrees": -135.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "deploy", - "waypointRelativePos": 0.15, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "deploy" - } - } - }, - { - "name": "intake", - "waypointRelativePos": 0.5, - "endWaypointRelativePos": 5.0, - "command": { - "type": "named", - "data": { - "name": "intake" - } - } - }, - { - "name": "spool-shooter", - "waypointRelativePos": 3.5, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "spool-shooter" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 1.0, - "rotation": -145.0 - }, - "reversed": false, - "folder": "1st Pass", - "idealStartingState": { - "velocity": 0.0, - "rotation": 100.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/right-bump-bump-hub.path b/src/main/deploy/pathplanner/paths/right-bump-bump-hub.path index 7e3ac127..a0131e7b 100644 --- a/src/main/deploy/pathplanner/paths/right-bump-bump-hub.path +++ b/src/main/deploy/pathplanner/paths/right-bump-bump-hub.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 2.2, - "y": 0.5999999999999996 + "y": 0.5999999999999999 }, "isLocked": false, "linkedName": "bump-alliance-right" @@ -146,7 +146,21 @@ "rotationDegrees": -135.0 } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 6.6, + "maxWaypointRelativePos": 7.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 480.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [ { @@ -160,6 +174,17 @@ } } }, + { + "name": "sotm-until-done", + "waypointRelativePos": 0, + "endWaypointRelativePos": 0.33, + "command": { + "type": "named", + "data": { + "name": "sotm-until-done" + } + } + }, { "name": "spool-shooter", "waypointRelativePos": 6.0, @@ -175,7 +200,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/right-bump-collect.path b/src/main/deploy/pathplanner/paths/right-bump-collect.path index 26b12893..cc415c43 100644 --- a/src/main/deploy/pathplanner/paths/right-bump-collect.path +++ b/src/main/deploy/pathplanner/paths/right-bump-collect.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 2.2, - "y": 1.15 + "y": 1.5499999999999998 }, "isLocked": false, "linkedName": "bump-alliance-right" @@ -50,8 +50,33 @@ "rotationDegrees": 0.0 } ], - "constraintZones": [], - "pointTowardsZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.33, + "constraints": { + "maxVelocity": 0.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 480.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.6, + "y": 4.06 + }, + "rotationOffset": 180.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.33, + "name": "Point Towards Zone" + } + ], "eventMarkers": [ { "name": "intake", @@ -74,12 +99,23 @@ "name": "coast" } } + }, + { + "name": "sotm-until-done", + "waypointRelativePos": 0, + "endWaypointRelativePos": 0.33, + "command": { + "type": "named", + "data": { + "name": "sotm-until-done" + } + } } ], "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/right-bump-far.path b/src/main/deploy/pathplanner/paths/right-bump-far.path index b96c3c44..f19fe551 100644 --- a/src/main/deploy/pathplanner/paths/right-bump-far.path +++ b/src/main/deploy/pathplanner/paths/right-bump-far.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 4.4, + "x": 4.7, "y": 0.55 }, "prevControl": null, "nextControl": { - "x": 5.400000000000004, + "x": 5.700000000000004, "y": 0.55 }, "isLocked": false, @@ -110,7 +110,47 @@ "rotationDegrees": -135.0 } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 5.0, + "maxAcceleration": 100.0, + "maxAngularVelocity": 480.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 4.6, + "maxWaypointRelativePos": 5.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 480.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.0, + "maxWaypointRelativePos": 3.5, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 480.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [ { @@ -150,7 +190,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/right-bump-safe.path b/src/main/deploy/pathplanner/paths/right-bump-safe.path index 7f2d85a6..99f81d27 100644 --- a/src/main/deploy/pathplanner/paths/right-bump-safe.path +++ b/src/main/deploy/pathplanner/paths/right-bump-safe.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 4.4, + "x": 4.7, "y": 0.55 }, "prevControl": null, "nextControl": { - "x": 5.400000000000004, + "x": 5.700000000000004, "y": 0.55 }, "isLocked": false, @@ -110,7 +110,47 @@ "rotationDegrees": -135.0 } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 5.0, + "maxAcceleration": 100.0, + "maxAngularVelocity": 480.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 4.6, + "maxWaypointRelativePos": 5.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 480.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.0, + "maxWaypointRelativePos": 3.5, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 480.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [ { @@ -150,7 +190,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/right-bump-trench-hub.path b/src/main/deploy/pathplanner/paths/right-bump-trench-hub.path index 4109c6fc..d1415c4c 100644 --- a/src/main/deploy/pathplanner/paths/right-bump-trench-hub.path +++ b/src/main/deploy/pathplanner/paths/right-bump-trench-hub.path @@ -185,7 +185,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/right-left-alliance-close-hub.path b/src/main/deploy/pathplanner/paths/right-left-alliance-close-hub.path new file mode 100644 index 00000000..2945898a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/right-left-alliance-close-hub.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5, + "y": 3.1 + }, + "prevControl": null, + "nextControl": { + "x": 3.5, + "y": 4.1 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.5, + "y": 4.6 + }, + "prevControl": { + "x": 3.5, + "y": 3.5999999999999996 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.0, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 1.0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Teleop Pathing", + "idealStartingState": { + "velocity": 1.0, + "rotation": 90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/right-left-alliance-far-hub.path b/src/main/deploy/pathplanner/paths/right-left-alliance-far-hub.path new file mode 100644 index 00000000..ecde123f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/right-left-alliance-far-hub.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 13.1, + "y": 3.1 + }, + "prevControl": null, + "nextControl": { + "x": 13.1, + "y": 4.1 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 13.1, + "y": 4.6 + }, + "prevControl": { + "x": 13.1, + "y": 3.5999999999999996 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.0, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 1.5, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Teleop Pathing", + "idealStartingState": { + "velocity": 1.5, + "rotation": 90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/right-left-mid-close-hub.path b/src/main/deploy/pathplanner/paths/right-left-mid-close-hub.path new file mode 100644 index 00000000..f3f10f62 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/right-left-mid-close-hub.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.8, + "y": 3.1 + }, + "prevControl": null, + "nextControl": { + "x": 5.8, + "y": 4.1 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.8, + "y": 4.6 + }, + "prevControl": { + "x": 5.8, + "y": 3.5999999999999996 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.0, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 1.5, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Teleop Pathing", + "idealStartingState": { + "velocity": 1.5, + "rotation": 90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/right-left-mid-far-hub.path b/src/main/deploy/pathplanner/paths/right-left-mid-far-hub.path new file mode 100644 index 00000000..97c1b617 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/right-left-mid-far-hub.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 10.75, + "y": 3.1 + }, + "prevControl": null, + "nextControl": { + "x": 10.75, + "y": 4.1 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 10.75, + "y": 4.6 + }, + "prevControl": { + "x": 10.75, + "y": 3.5999999999999996 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.0, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 1.5, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Teleop Pathing", + "idealStartingState": { + "velocity": 1.5, + "rotation": 90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/right-trench-collect.path b/src/main/deploy/pathplanner/paths/right-trench-collect.path index e9d8bb4b..77398a4f 100644 --- a/src/main/deploy/pathplanner/paths/right-trench-collect.path +++ b/src/main/deploy/pathplanner/paths/right-trench-collect.path @@ -88,7 +88,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/right-trench-far.path b/src/main/deploy/pathplanner/paths/right-trench-far.path index fee85b80..dbb1b5cb 100644 --- a/src/main/deploy/pathplanner/paths/right-trench-far.path +++ b/src/main/deploy/pathplanner/paths/right-trench-far.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 4.4, + "x": 4.7, "y": 0.55 }, "prevControl": null, "nextControl": { - "x": 5.400000000000004, + "x": 5.700000000000004, "y": 0.5499999999999997 }, "isLocked": false, @@ -110,7 +110,21 @@ "rotationDegrees": -90.0 } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 3.0, + "constraints": { + "maxVelocity": 5.0, + "maxAcceleration": 100.0, + "maxAngularVelocity": 480.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [ { @@ -150,7 +164,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/right-trench-hub.path b/src/main/deploy/pathplanner/paths/right-trench-hub.path index a92b8af2..d1774bfa 100644 --- a/src/main/deploy/pathplanner/paths/right-trench-hub.path +++ b/src/main/deploy/pathplanner/paths/right-trench-hub.path @@ -173,7 +173,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/right-trench-safe.path b/src/main/deploy/pathplanner/paths/right-trench-safe.path index 3b99872a..2f1da5c9 100644 --- a/src/main/deploy/pathplanner/paths/right-trench-safe.path +++ b/src/main/deploy/pathplanner/paths/right-trench-safe.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 4.4, + "x": 4.7, "y": 0.55 }, "prevControl": null, "nextControl": { - "x": 5.400000000000004, + "x": 5.700000000000004, "y": 0.5499999999999997 }, "isLocked": false, @@ -110,7 +110,21 @@ "rotationDegrees": -90.0 } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 3.0, + "constraints": { + "maxVelocity": 5.0, + "maxAcceleration": 100.0, + "maxAngularVelocity": 480.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [ { @@ -150,7 +164,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/sotm-test.path b/src/main/deploy/pathplanner/paths/sotm-test.path index b7b88e11..d7331fdb 100644 --- a/src/main/deploy/pathplanner/paths/sotm-test.path +++ b/src/main/deploy/pathplanner/paths/sotm-test.path @@ -110,7 +110,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/trench-depot.path b/src/main/deploy/pathplanner/paths/trench-depot.path index e852cccb..d9d767c1 100644 --- a/src/main/deploy/pathplanner/paths/trench-depot.path +++ b/src/main/deploy/pathplanner/paths/trench-depot.path @@ -66,7 +66,7 @@ "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 6431e63a..e2fa83e8 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -5,17 +5,20 @@ "pathFolders": [ "1st Pass", "2nd Pass", - "Depot", "3rd Pass", + "Depot", + "Custom", + "Teleop Pathing", "Testing" ], "autoFolders": [ "New Autos", + "Custom", "Testing" ], "defaultMaxVel": 5.0, "defaultMaxAccel": 3.0, - "defaultMaxAngVel": 540.0, + "defaultMaxAngVel": 480.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, "robotMass": 74.088, diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 249ebbe2..aada7862 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -54,6 +54,7 @@ public class Autos { private final Deploy deploy; private final Command sotmSmallHopperCommand; + private final Command sotmUntilDoneCommand; private final Command shootSmallHopperCommand; private final Command shootUntilDoneCommand; private final Command spoolShooterCommand; @@ -111,6 +112,26 @@ public Autos( Commands.none(), inAllianceZoneSupplier); + sotmUntilDoneCommand = + new ConditionalCommand( + ShootOnTheMoveCommands.shootOnTheMoveAutoCommand( + drive, + shooter, + indexer, + deploy, + booster, + hood, + leds, + () -> RebuiltUtils.getCurrentHubLocation().toTranslation2d(), + inAllianceZoneSupplier, + () -> false, + () -> false, + () -> true, + () -> false) + .alongWith(intake.intakeUntilInterruptedCommand(1).asProxy()), + Commands.none(), + inAllianceZoneSupplier); + spoolShooterCommand = ShooterCommands.shootAtDistanceCommand( shooter, @@ -145,7 +166,8 @@ public Autos( () -> false, () -> false, () -> true, - () -> false), + () -> false, + () -> true), Commands.none(), inAllianceZoneSupplier); @@ -166,7 +188,8 @@ public Autos( () -> false, () -> false, () -> true, - () -> false), + () -> false, + () -> true), Commands.none(), inAllianceZoneSupplier) .withDeadline(Commands.waitTime(SMALL_HOPPER_SHOOT_DURATION)); @@ -193,7 +216,11 @@ public Autos( public Command createCommandFromSelectedAuto() { Auto auto = autoChooser.get(); return new WaitCommand(autoWaitTime.get()) - .andThen(auto.getCommand()) + .andThen( + auto.getCommand() + .alongWith( + Commands.waitTime(Seconds.of(3)) + .andThen(Commands.runOnce(() -> drive.setLowCurrentLimits())))) .andThen(Commands.print("Auto Complete")) .withName(auto.getName()); } @@ -224,6 +251,8 @@ private void bindNamedCommands() { NamedCommands.registerCommand("sotm-small-hopper", sotmSmallHopperCommand); + NamedCommands.registerCommand("sotm-until-done", sotmUntilDoneCommand); + NamedCommands.registerCommand("coast", Commands.runOnce(() -> drive.setCoastMode())); RobotModeTriggers.teleop().onTrue(Commands.runOnce(() -> drive.setBrakeMode())); @@ -240,6 +269,9 @@ private void bindEventMarkers() { new EventTrigger("sotm-small-hopper") .onFalse(Commands.runOnce(() -> sotmSmallHopperCommand.cancel())); + + new EventTrigger("sotm-until-done") + .onFalse(Commands.runOnce(() -> sotmUntilDoneCommand.cancel())); } public static final class Auto { diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 48eeeaf3..6bee3ac6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,5 +1,6 @@ package frc.robot; +import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.RotationsPerSecond; import com.pathplanner.lib.util.FlippingUtil; @@ -7,6 +8,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; import edu.wpi.first.wpilibj.RobotBase; /** @@ -40,10 +42,26 @@ public static class Locations { } public static class Tolerances { - public static final Rotation2d SCORING_ANGLE_TOLERANCE = Rotation2d.fromDegrees(5); + public static final Rotation2d SCORING_ANGLE_TOLERANCE = Rotation2d.fromDegrees(10); public static final Rotation2d PASSING_ANGLE_TOLERANCE = Rotation2d.fromDegrees(15); - public static final AngularVelocity INITIAL_SPEED_TOLERANCE = RotationsPerSecond.of(0.5); - public static final AngularVelocity NORMAL_SPEED_TOLERANCE = RotationsPerSecond.of(2); + public static final AngularVelocity INITIAL_SPEED_TOLERANCE = RotationsPerSecond.of(0.1); + public static final AngularVelocity NORMAL_SPEED_TOLERANCE = RotationsPerSecond.of(0.25); + } + + public static class CurrentLimits { + public static final Current HIGH_DRIVE_STATOR = Amps.of(100); + public static final Current HIGH_DRIVE_SUPPLY = Amps.of(80); + public static final Current LOW_DRIVE_STATOR = Amps.of(60); + public static final Current LOW_DRIVE_SUPPLY = Amps.of(50); + public static final Current STEER_STATOR = Amps.of(60); + + public static final Current SHOOTER_SUPPLY = Amps.of(60); + public static final Current BOOSTER_SUPPLY = Amps.of(60); + public static final Current INTAKE_SUPPLY = Amps.of(60); + public static final Current INDEXER_SUPPLY = Amps.of(40); + + public static final Current DEPLOY_STATOR = Amps.of(60); + public static final Current DEPLOY_SUPPLY = Amps.of(40); } public static class CanIds { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1025538d..6389bb1e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -92,7 +92,11 @@ public void robotPeriodic() { /** This function is called once when the robot is disabled. */ @Override - public void disabledInit() {} + public void disabledInit() { + if (robotContainer.drive != null) { + robotContainer.drive.setHighCurrentLimits(); + } + } /** This function is called periodically when disabled. */ @Override @@ -118,6 +122,10 @@ public void teleopInit() { if (autonomousCommand != null) { autonomousCommand.cancel(); } + + if (robotContainer.drive != null) { + robotContainer.drive.setLowCurrentLimits(); + } } /** This function is called periodically during operator control. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ec530723..85cf9677 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -46,7 +46,7 @@ */ public class RobotContainer { // Subsystems - private final Drive drive; + public final Drive drive; private final Vision vision; private final Intake intake; private final Indexer indexer; @@ -95,7 +95,7 @@ public RobotContainer() { VisionConstants.RIGHT_SIDE_CAMERA_POSITION)); fuelDetection = new FuelDetection(new FuelDetectionIO() {}); leds = new Leds(new LedsReal()); - deploy = new Deploy(new DeployIO() {}); + deploy = new Deploy(new DeployIOReal()); hood = new Hood(new HoodIOReal()); break; diff --git a/src/main/java/frc/robot/commands/DriveAtAngleCommand.java b/src/main/java/frc/robot/commands/DriveAtAngleCommand.java index 2c6e4574..5882e68f 100644 --- a/src/main/java/frc/robot/commands/DriveAtAngleCommand.java +++ b/src/main/java/frc/robot/commands/DriveAtAngleCommand.java @@ -15,9 +15,11 @@ import org.littletonrobotics.junction.Logger; public class DriveAtAngleCommand extends Command { - private static double ANGLE_KP = 12; + private static double ANGLE_KP = 5.8; private static double ANGLE_KI = 0.00; - private static double ANGLE_KD = 0.15; + private static double ANGLE_KD = 0.16; + private static final double ANGLE_MAX_VELOCITY = 1000; + private static final double ANGLE_MAX_ACCELERATION = 1000; @AutoLogOutput private static final LoggedNetworkNumberPlus KP_SUPPLIER = @@ -27,8 +29,14 @@ public class DriveAtAngleCommand extends Command { private static final LoggedNetworkNumberPlus KD_SUPPLIER = new LoggedNetworkNumberPlus("/Tuning/Angle KD", ANGLE_KD); - private static final double ANGLE_MAX_VELOCITY = 8.0; - private static final double ANGLE_MAX_ACCELERATION = 20.0; + @AutoLogOutput + private static final LoggedNetworkNumberPlus MAX_VELOCITY_SUPPLIER = + new LoggedNetworkNumberPlus("/Tuning/Angle Max Velocity", ANGLE_MAX_VELOCITY); + + @AutoLogOutput + private static final LoggedNetworkNumberPlus MAX_ACCELERATION_SUPPLIER = + new LoggedNetworkNumberPlus("/Tuning/Angle Max Acceleration", ANGLE_MAX_ACCELERATION); + private static final Rotation2d INITIAL_TOLERANCE = Rotation2d.fromDegrees(1); public static final Rotation2d ADJUSTMENT_TOLERANCE = Rotation2d.fromDegrees(2); @@ -65,6 +73,16 @@ public DriveAtAngleCommand( KP_SUPPLIER.addSubscriber(value -> angleController.setP(value)); KD_SUPPLIER.addSubscriber(value -> angleController.setD(value)); + MAX_VELOCITY_SUPPLIER.addSubscriber( + value -> + angleController.setConstraints( + new TrapezoidProfile.Constraints( + MAX_VELOCITY_SUPPLIER.get(), MAX_ACCELERATION_SUPPLIER.get()))); + MAX_ACCELERATION_SUPPLIER.addSubscriber( + value -> + angleController.setConstraints( + new TrapezoidProfile.Constraints( + MAX_VELOCITY_SUPPLIER.get(), MAX_ACCELERATION_SUPPLIER.get()))); } @Override @@ -112,10 +130,15 @@ public void resetPID() { public double getPIDOutput(boolean flipped) { Logger.recordOutput("Drive/Angle Setpoint Error", angleController.getPositionError()); - return angleController.calculate( - drive.getRotation().getRadians(), - flipped - ? rotationSupplier.get().getRadians() + Math.PI - : rotationSupplier.get().getRadians()); + double output = + angleController.calculate( + drive.getRotation().getRadians(), + flipped + ? rotationSupplier.get().getRadians() + Math.PI + : rotationSupplier.get().getRadians()); + + Logger.recordOutput("Drive/PID Output", output); + + return output; } } diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index c732c33d..07cc6dc3 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -61,7 +61,7 @@ public static Command joystickDriveCommand( double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), DEADBAND); // Square rotation value for more precise control - omega = Math.copySign(omega * omega, omega); + omega = Math.signum(omega) * Math.pow(Math.abs(omega), .75); // Convert to field relative speeds & send command ChassisSpeeds speeds = diff --git a/src/main/java/frc/robot/commands/SafeAimAndShootCommand.java b/src/main/java/frc/robot/commands/SafeAimAndShootCommand.java index 4faf5b14..c9e44b4b 100644 --- a/src/main/java/frc/robot/commands/SafeAimAndShootCommand.java +++ b/src/main/java/frc/robot/commands/SafeAimAndShootCommand.java @@ -33,7 +33,8 @@ public SafeAimAndShootCommand( BooleanSupplier overrideAngleSafeguard, BooleanSupplier overrideVelocitySafeguard, BooleanSupplier overrideHubActive, - BooleanSupplier overrideAutoRanging) { + BooleanSupplier overrideAutoRanging, + BooleanSupplier additionalDeployCondition) { Command safeShootCommand = new SafeShootCommand( @@ -49,7 +50,8 @@ public SafeAimAndShootCommand( overrideAngleSafeguard, overrideVelocitySafeguard, overrideHubActive, - overrideAutoRanging); + overrideAutoRanging, + additionalDeployCondition); driveCommand = new DriveAimLockedCommand(drive, xSupplier, ySupplier, positionSupplier, true); diff --git a/src/main/java/frc/robot/commands/SafeShootCommand.java b/src/main/java/frc/robot/commands/SafeShootCommand.java index e10fb118..0668b465 100644 --- a/src/main/java/frc/robot/commands/SafeShootCommand.java +++ b/src/main/java/frc/robot/commands/SafeShootCommand.java @@ -28,6 +28,7 @@ public class SafeShootCommand extends ParallelCommandGroup { private static double INDEXER_SPEED = 1; private static Time RETRACT_DELAY = Seconds.of(1); + private static Time LIFT_TIME = Seconds.of(0.5); private static final double BOOSTER_SPEED = 1; private static final Distance MINIMUM_SHOT_DISTANCE = Meters.of(2.2); @@ -47,7 +48,8 @@ public SafeShootCommand( BooleanSupplier overrideAngleSafeguard, BooleanSupplier overrideVelocitySafeguard, BooleanSupplier overrideHubActive, - BooleanSupplier overrideAutoRanging) { + BooleanSupplier overrideAutoRanging, + BooleanSupplier additionalDeployCondition) { Logger.recordOutput( "Shooter/Distance to Target", @@ -83,11 +85,6 @@ public SafeShootCommand( && (hubActiveCondition.getAsBoolean() || overrideHubActive.getAsBoolean()) && (autoRangeCondition.getAsBoolean() || overrideAutoRanging.getAsBoolean()); - BooleanSupplier notMovingCondition = - () -> - drive.getChassisSpeeds().vxMetersPerSecond < 0.025 - && drive.getChassisSpeeds().vyMetersPerSecond < 0.025; - Command guardedIndexerCommand = new GuardedCommand( Commands.waitUntil( @@ -100,18 +97,18 @@ public SafeShootCommand( .andThen(indexer.indexUntilCancelledCommand(INDEXER_SPEED)), combinedCondition); - // Command guardedDeployCommand = - // Commands.waitUntil(() -> hasStartedShooting) - // .andThen(Commands.waitTime(RETRACT_DELAY)) - // .andThen( - // new GuardedCommand( - // deploy.crunchCommand(), - // () -> combinedCondition.getAsBoolean() && - // notMovingCondition.getAsBoolean())); + Command deployCommand = + Commands.waitUntil(() -> hasStartedShooting) + .andThen(Commands.waitTime(RETRACT_DELAY)) + .andThen(new GuardedCommand(deploy.crunchCommand(), additionalDeployCondition)) + .until(() -> deploy.isPastOrAtSetpoint()); Supplier distanceSupplier = () -> Meters.of(drive.getPose().getTranslation().getDistance(positionSupplier.get())); + Command logDeployConditionCommand = + Commands.run(() -> Logger.recordOutput("Deploy/is at target", deploy.isPastOrAtSetpoint())); + Command shootAtDistanceCommand = ShooterCommands.shootAtDistanceCommand( shooter, @@ -152,7 +149,8 @@ public SafeShootCommand( shooterVelocityCondition, driveAngleCondition, hubActiveCondition, - autoRangeCondition)); + autoRangeCondition, + additionalDeployCondition)); Command activityTracker = Commands.startEnd( @@ -178,17 +176,19 @@ public SafeShootCommand( activityTracker, shootAtDistanceCommand.asProxy(), guardedIndexerCommand.asProxy(), - // guardedDeployCommand.asProxy(), + deployCommand.asProxy(), boosterCommand.asProxy(), hoodCommand, - loggedGuardCommand); + loggedGuardCommand, + logDeployConditionCommand); } private void logConditions( BooleanSupplier shooterVelocityCondition, BooleanSupplier driveAngleCondition, BooleanSupplier hubActiveCondition, - BooleanSupplier withinRangeCondition) { + BooleanSupplier withinRangeCondition, + BooleanSupplier additionalDeployCondition) { Logger.recordOutput( "Controls/Ready To Shoot", shooterVelocityCondition.getAsBoolean() && driveAngleCondition.getAsBoolean()); @@ -197,5 +197,7 @@ private void logConditions( Logger.recordOutput("Controls/Drive Angle Condition", driveAngleCondition.getAsBoolean()); Logger.recordOutput("Controls/HubActive", hubActiveCondition.getAsBoolean()); Logger.recordOutput("Controls/Within Range Condition", withinRangeCondition.getAsBoolean()); + Logger.recordOutput( + "Controls/Additional Deploy Condition", additionalDeployCondition.getAsBoolean()); } } diff --git a/src/main/java/frc/robot/commands/ShootOnTheMoveCommands.java b/src/main/java/frc/robot/commands/ShootOnTheMoveCommands.java index 5eb5c5ad..06f6b599 100644 --- a/src/main/java/frc/robot/commands/ShootOnTheMoveCommands.java +++ b/src/main/java/frc/robot/commands/ShootOnTheMoveCommands.java @@ -66,7 +66,8 @@ public static Command shootOnTheMoveAutoCommand( overrideAngleSafeguard, overrideVelocitySafeguard, overrideHubActive, - overrideAutoRanging); + overrideAutoRanging, + () -> false); DriveAimLockedCommand driveCommand = new DriveAimLockedCommand(drive, () -> 0, () -> 0, leadTarget, true); @@ -119,7 +120,8 @@ public static Command aimAndShootOnTheMoveCommand( overrideAngleSafeguard, overrideVelocitySafeguard, overrideHubActive, - overrideAutoRanging); + overrideAutoRanging, + () -> Math.abs(xSupplier.getAsDouble()) < 0.05 && Math.abs(ySupplier.getAsDouble()) < 0.05); } private static Translation2d calculateLeadTarget( diff --git a/src/main/java/frc/robot/commands/ShooterCommands.java b/src/main/java/frc/robot/commands/ShooterCommands.java index 622c345b..4775e2ba 100644 --- a/src/main/java/frc/robot/commands/ShooterCommands.java +++ b/src/main/java/frc/robot/commands/ShooterCommands.java @@ -21,16 +21,56 @@ public class ShooterCommands { - // Starter setpoints; not well done public static final ShooterSetpoint[] HUB_SETPOINTS = Arrays.stream( new ShooterSetpoint[] { new ShooterSetpoint( - Meters.of(2.41), Rotations.of(0), RotationsPerSecond.of(22), Seconds.of(.7)), + Meters.of(2.44), + Rotations.of(0), + RotationsPerSecond.of(22), + Seconds.of(0.7824)), new ShooterSetpoint( - Meters.of(3), Rotations.of(.2), RotationsPerSecond.of(23.5), Seconds.of(.9)), + Meters.of(2.86), + Rotations.of(0), + RotationsPerSecond.of(22.5), + Seconds.of(0.8694)), new ShooterSetpoint( - Meters.of(4.76), Rotations.of(.3), RotationsPerSecond.of(28), Seconds.of(1.3)) + Meters.of(3.15), + Rotations.of(0), + RotationsPerSecond.of(23.25), + Seconds.of(0.897)), + new ShooterSetpoint( + Meters.of(3.34), + Rotations.of(0), + RotationsPerSecond.of(23.5), + Seconds.of(0.9162)), + new ShooterSetpoint( + Meters.of(3.75), + Rotations.of(0), + RotationsPerSecond.of(24.5), + Seconds.of(1.016)), + new ShooterSetpoint( + Meters.of(3.82), + Rotations.of(0), + RotationsPerSecond.of(24.5), + Seconds.of(1.016)), + new ShooterSetpoint( + Meters.of(4.23), + Rotations.of(0), + RotationsPerSecond.of(25.75), + Seconds.of(1.0426)), + new ShooterSetpoint( + Meters.of(4.69), Rotations.of(0), RotationsPerSecond.of(27), Seconds.of(1.096)), + new ShooterSetpoint( + Meters.of(5), + Rotations.of(0.1), + RotationsPerSecond.of(27.25), + Seconds.of(1.1946)), + new ShooterSetpoint( + Meters.of(5.42), + Rotations.of(0.2), + RotationsPerSecond.of(28.25), + Seconds.of(1.1894)), }) .sorted() .toArray(ShooterSetpoint[]::new); @@ -39,16 +79,17 @@ public class ShooterCommands { Arrays.stream( new ShooterSetpoint[] { new ShooterSetpoint( - Meters.of(2.41), Rotations.of(0), RotationsPerSecond.of(22), Seconds.of(.7)), - new ShooterSetpoint( - Meters.of(3), Rotations.of(.2), RotationsPerSecond.of(23.5), Seconds.of(.9)), + Meters.of(6.3), + Rotations.of(.4), + RotationsPerSecond.of(22), + Seconds.of(0.8942)), new ShooterSetpoint( Meters.of(7.25), Rotations.of(.7), - RotationsPerSecond.of(28), - Seconds.of(.86)), // estimated time + RotationsPerSecond.of(27), + Seconds.of(0.9304)), new ShooterSetpoint( - Meters.of(15), Rotations.of(.8), RotationsPerSecond.of(41), Seconds.of(1.66)) + Meters.of(15), Rotations.of(.7), RotationsPerSecond.of(50), Seconds.of(1.66)) }) .sorted() .toArray(ShooterSetpoint[]::new); diff --git a/src/main/java/frc/robot/controls/CompetitionControllerMapping.java b/src/main/java/frc/robot/controls/CompetitionControllerMapping.java index 5410d14e..9115e628 100644 --- a/src/main/java/frc/robot/controls/CompetitionControllerMapping.java +++ b/src/main/java/frc/robot/controls/CompetitionControllerMapping.java @@ -7,6 +7,7 @@ import badgerutils.commands.CommandUtils; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -18,12 +19,10 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.DriveCommands; import frc.robot.commands.FaceforwardCommand; -import frc.robot.commands.FuelCollectionCommand; import frc.robot.commands.ShootOnTheMoveCommands; import frc.robot.commands.ShooterCommands; import frc.robot.subsystems.booster.Booster; import frc.robot.subsystems.deploy.Deploy; -import frc.robot.subsystems.deploy.DeployerPosition; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.fueldetection.FuelDetection; import frc.robot.subsystems.hood.Hood; @@ -134,8 +133,12 @@ public void bind() { .intakeUntilInterruptedCommand(1) .withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); - // Fuel Collection - driverController.b().whileTrue(new FuelCollectionCommand(drive, fuelDetection)); + // Hub Sweep + driverController + .b() + .whileTrue( + Commands.deferredProxy( + () -> getHubCollectingPath(drive, drive.getPose().getTranslation()))); // Robot Relative Drive driverController @@ -200,6 +203,13 @@ public void bind() { .withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); /* ---P2--- */ + // Force through defence? + operatorController + .leftStick() + .whileTrue( + Commands.startEnd( + () -> drive.setHighCurrentLimits(), () -> drive.setLowCurrentLimits())); + // Spool Shooter operatorController .rightTrigger() @@ -217,7 +227,11 @@ public void bind() { .withInterruptBehavior(InterruptionBehavior.kCancelSelf) .alongWith( new InstantCommand( - () -> operatorController.setRumble(RumbleType.kBothRumble, 0.25)))) + () -> operatorController.setRumble(RumbleType.kBothRumble, 0.25))) + .alongWith( + (booster.boostCommand(-0.5).alongWith(indexer.indexUntilCancelledCommand(-0.5))) + .withDeadline(Commands.waitSeconds(0.25)) + .andThen(booster.boostCommand(.8)))) .onFalse( new InstantCommand(() -> operatorController.setRumble(RumbleType.kBothRumble, 0)) .ignoringDisable(true)); @@ -228,12 +242,6 @@ public void bind() { operatorController.povLeft().whileTrue(deploy.deployManuallyCommand(0.2)); operatorController.povRight().whileTrue(deploy.deployManuallyCommand(-0.2)); - operatorController - .back() - .onTrue( - Commands.runOnce( - () -> deploy.setDeployerPosition(DeployerPosition.RETRACTED), - deploy)); // TESTING ONLY // OVERRIDES // Force Indexer @@ -289,4 +297,61 @@ public void clear() { CommandUtils.removeAndCancelDefaultCommand(drive); CommandUtils.removeAndCancelDefaultCommand(intake); } + + private Command getHubCollectingPath(Drive drive, Translation2d position) { + Command collectionPath; + if (RebuiltUtils.isInAllianceZone(position)) { + collectionPath = + RebuiltUtils.isLeftSide(position) + ? drive.leftToRightAllianceCloseHubSweep + : drive.rightToLeftAllianceCloseHubSweep; + } else if (RebuiltUtils.isInOpponentAllianceZone(position)) { + collectionPath = + RebuiltUtils.isLeftSide(position) + ? drive.leftToRightAllianceFarHubSweep + : drive.rightToLeftAllianceFarHubSweep; + } else if (RebuiltUtils.isOurHalf(position)) { + collectionPath = + RebuiltUtils.isLeftSide(position) + ? drive.leftToRightMidCloseHubSweep + : drive.rightToLeftMidCloseHubSweep; + } else if (!RebuiltUtils.isOurHalf(position)) { + collectionPath = + RebuiltUtils.isLeftSide(position) + ? drive.leftToRightMidFarHubSweep + : drive.rightToLeftMidFarHubSweep; + } else { + + System.out.println("Could not find suitable path"); + return Commands.none(); + } + + return collectionPath; + } + /* + private Command pathOnTheFlyToHubCollectingPath(Drive drive, Command hubCollectingPath) { + HashMap map = new HashMap<>(); + map.put( + drive.leftToRightAllianceCloseHubSweep, new Pose2d(3.45, 5, Rotation2d.fromDegrees(-90))); + map.put( + drive.rightToLeftAllianceCloseHubSweep, new Pose2d(3.45, 3.1, Rotation2d.fromDegrees(90))); + map.put(drive.leftToRightMidCloseHubSweep, new Pose2d(5.8, 5, Rotation2d.fromDegrees(-90))); + map.put(drive.rightToLeftMidCloseHubSweep, new Pose2d(5.8, 3.1, Rotation2d.fromDegrees(90))); + map.put(drive.leftToRightMidFarHubSweep, new Pose2d(10.75, 5, Rotation2d.fromDegrees(-90))); + map.put(drive.rightToLeftMidFarHubSweep, new Pose2d(10.75, 3.1, Rotation2d.fromDegrees(90))); + map.put(drive.leftToRightAllianceFarHubSweep, new Pose2d(13.1, 5, Rotation2d.fromDegrees(-90))); + map.put( + drive.rightToLeftAllianceFarHubSweep, new Pose2d(13.1, 3.1, Rotation2d.fromDegrees(90))); + + return AutoBuilder.followPath( + new PathPlannerPath( + PathPlannerPath.waypointsFromPoses(drive.getPose(), map.get(hubCollectingPath)), + new PathConstraints(2.0, 3.0, Degrees.of(540).in(Radians), Degrees.of(720).in(Radians)), + new IdealStartingState( + Math.sqrt( + Math.pow(drive.getChassisSpeeds().vxMetersPerSecond, 2) + + Math.pow(drive.getChassisSpeeds().vyMetersPerSecond, 2)), + drive.getPose().getRotation()), + new GoalEndState(2, map.get(hubCollectingPath).getRotation()))); + }*/ } diff --git a/src/main/java/frc/robot/controls/Controls.java b/src/main/java/frc/robot/controls/Controls.java index d269ff27..1adfb6b1 100644 --- a/src/main/java/frc/robot/controls/Controls.java +++ b/src/main/java/frc/robot/controls/Controls.java @@ -77,6 +77,7 @@ public Controls( intake, shooter, indexer, + deploy, booster, hood)); diff --git a/src/main/java/frc/robot/controls/ShooterTestingControllerMapping.java b/src/main/java/frc/robot/controls/ShooterTestingControllerMapping.java index 41f369a5..6edca9aa 100644 --- a/src/main/java/frc/robot/controls/ShooterTestingControllerMapping.java +++ b/src/main/java/frc/robot/controls/ShooterTestingControllerMapping.java @@ -19,6 +19,7 @@ import frc.robot.commands.GuardedCommand; import frc.robot.commands.ShooterCommands; import frc.robot.subsystems.booster.Booster; +import frc.robot.subsystems.deploy.Deploy; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.hood.Hood; import frc.robot.subsystems.indexer.Indexer; @@ -38,6 +39,7 @@ public class ShooterTestingControllerMapping extends ControllerMapping { private final Indexer indexer; private final Booster booster; private final Hood hood; + private final Deploy deploy; @AutoLogOutput private final LoggedNetworkNumberPlus targetSpeed = @@ -58,6 +60,7 @@ public ShooterTestingControllerMapping( Intake intake, Shooter shooter, Indexer indexer, + Deploy deploy, Booster booster, Hood hood) { super(driverController, operatorController); @@ -66,7 +69,7 @@ public ShooterTestingControllerMapping( this.shooter = shooter; this.indexer = indexer; this.booster = booster; - + this.deploy = deploy; this.hood = hood; } @@ -81,6 +84,8 @@ public void bind() { drive.getPose().getTranslation(), RebuiltUtils.getCurrentHubLocation().toTranslation2d()) .gt(Feet.of(7.5)))); + Command logDeployConditionCommand = + Commands.run(() -> Logger.recordOutput("Deploy/is at target", deploy.isPastOrAtSetpoint())); Command loggedTargetCommand = Commands.run( @@ -101,6 +106,7 @@ public void bind() { () -> -driverController.getLeftX(), () -> -driverController.getRightX()) .alongWith(logWithinRangeCommand) + .alongWith(logDeployConditionCommand) .alongWith( new RunCommand( () -> @@ -163,6 +169,9 @@ public void bind() { () -> -driverController.getLeftX(), () -> RebuiltUtils.getCurrentHubLocation().toTranslation2d(), true)))); + + operatorController.x().onTrue(deploy.deployCommand()); + operatorController.leftTrigger(0.5).whileTrue(deploy.crunchCommand()); } @Override diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 06575741..d18dcbcb 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -12,6 +12,7 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.*; +import frc.robot.Constants; // Generated by the 2026 Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html @@ -51,17 +52,13 @@ public class TunerConstants { // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; - // The stator current at which the wheels start to slip; - // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(60); - // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() - .withSupplyCurrentLimit(Amps.of(50)) + .withSupplyCurrentLimit(Constants.CurrentLimits.HIGH_DRIVE_SUPPLY) .withSupplyCurrentLimitEnable(true)); private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() @@ -70,7 +67,7 @@ public class TunerConstants { // Swerve azimuth does not require much torque output, so we can set a relatively // low // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimit(Constants.CurrentLimits.STEER_STATOR) .withStatorCurrentLimitEnable(true)); private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs @@ -124,7 +121,7 @@ public class TunerConstants { .withDriveMotorGains(driveGains) .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) + .withSlipCurrent(Constants.CurrentLimits.HIGH_DRIVE_STATOR) .withSpeedAt12Volts(kSpeedAt12Volts) .withDriveMotorType(kDriveMotorType) .withSteerMotorType(kSteerMotorType) diff --git a/src/main/java/frc/robot/subsystems/booster/BoosterConstants.java b/src/main/java/frc/robot/subsystems/booster/BoosterConstants.java index 5f67bce1..9b5fb56f 100644 --- a/src/main/java/frc/robot/subsystems/booster/BoosterConstants.java +++ b/src/main/java/frc/robot/subsystems/booster/BoosterConstants.java @@ -1,12 +1,11 @@ package frc.robot.subsystems.booster; -import static edu.wpi.first.units.Units.Amps; - import badgerutils.motor.MotorConfigUtils; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import frc.robot.Constants; public class BoosterConstants { // CONFIGS @@ -16,7 +15,7 @@ public class BoosterConstants { new CurrentLimitsConfigs() .withStatorCurrentLimitEnable(false) .withSupplyCurrentLimitEnable(true) - .withSupplyCurrentLimit(Amps.of(60))) + .withSupplyCurrentLimit(Constants.CurrentLimits.BOOSTER_SUPPLY)) .withMotorOutput( MotorConfigUtils.createMotorOutputConfig( InvertedValue.Clockwise_Positive, NeutralModeValue.Coast)); diff --git a/src/main/java/frc/robot/subsystems/deploy/Deploy.java b/src/main/java/frc/robot/subsystems/deploy/Deploy.java index eeaef10a..3fbbb9ce 100644 --- a/src/main/java/frc/robot/subsystems/deploy/Deploy.java +++ b/src/main/java/frc/robot/subsystems/deploy/Deploy.java @@ -40,19 +40,20 @@ public Command deployCommand() { return Commands.runOnce(() -> setDeployerPosition(DeployerPosition.EXTENDED), this); } + public Command deployToPositionCommand(DeployerPosition position) { + return Commands.startEnd( + () -> { + setDeployerPosition(position); + }, + () -> setDeployerPosition(DeployerPosition.EXTENDED), + this); + } + public Command crunchCommand() { - // return Commands.startEnd( - // () -> setDeployerPosition(DeployerPosition.DUMP), - // () -> setDeployerPosition(DeployerPosition.EXTENDED), - // this); - - return (Commands.runOnce(() -> setDutyCycle(0.2), this) - .andThen(Commands.waitSeconds(.5)) - .andThen( - Commands.runEnd( - () -> setDeployerPosition(DeployerPosition.DUMP), - () -> setDeployerPosition(DeployerPosition.EXTENDED), - this))) - .finallyDo(() -> setDeployerPosition(DeployerPosition.EXTENDED)); + return deployToPositionCommand(DeployerPosition.DUMP); + } + + public boolean isPastOrAtSetpoint() { + return deployIO.isAtSetpoint(); } } diff --git a/src/main/java/frc/robot/subsystems/deploy/DeployConstants.java b/src/main/java/frc/robot/subsystems/deploy/DeployConstants.java index 2a46c4a2..944a55e7 100644 --- a/src/main/java/frc/robot/subsystems/deploy/DeployConstants.java +++ b/src/main/java/frc/robot/subsystems/deploy/DeployConstants.java @@ -1,12 +1,9 @@ package frc.robot.subsystems.deploy; -import static edu.wpi.first.units.Units.Amps; - import badgerutils.motor.MotorConfigUtils; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.Slot1Configs; -import com.ctre.phoenix6.configs.Slot2Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.GravityTypeValue; @@ -15,17 +12,13 @@ import frc.robot.Constants; public class DeployConstants { - public static final double KP_0 = 130; - public static final double KD_0 = 5; - public static final double KS_0 = 5; - - public static final double KP_1 = 130; - public static final double KD_1 = 5; - public static final double KS_1 = 5; + public static final double KP_0 = 180; + public static final double KD_0 = 15; + public static final double KS_0 = 0; - public static final double KP_2 = 130; - public static final double KD_2 = 5; - public static final double KS_2 = 5; + public static final double KP_1 = 380; + public static final double KD_1 = 30; + public static final double KS_1 = 0; private static final double ROTOR_TO_SENSOR_RATIO = (25D / 1D) * (18D / 18D); @@ -37,14 +30,13 @@ public class DeployConstants { .withCurrentLimits( new CurrentLimitsConfigs() .withStatorCurrentLimitEnable(true) - .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimit(Constants.CurrentLimits.DEPLOY_STATOR) .withSupplyCurrentLimitEnable(true) - .withSupplyCurrentLimit(Amps.of(40))) + .withSupplyCurrentLimit(Constants.CurrentLimits.DEPLOY_SUPPLY)) .withSlot0( MotorConfigUtils.createPidConfig( KP_0, 0, KD_0, 0, 0, 0, 0, GravityTypeValue.Arm_Cosine)) - .withSlot1(new Slot1Configs().withKP(KP_1).withKD(KD_1).withKS(KD_2)) - .withSlot2(new Slot2Configs().withKP(KP_2).withKD(KD_2).withKS(KS_2)) + .withSlot1(new Slot1Configs().withKP(KP_1).withKD(KD_1).withKS(KS_1)) .withFeedback( new FeedbackConfigs() .withFeedbackRemoteSensorID(Constants.CanIds.DEPLOYER_ENCODER_ID) diff --git a/src/main/java/frc/robot/subsystems/deploy/DeployIO.java b/src/main/java/frc/robot/subsystems/deploy/DeployIO.java index 4dadf7ca..86266586 100644 --- a/src/main/java/frc/robot/subsystems/deploy/DeployIO.java +++ b/src/main/java/frc/robot/subsystems/deploy/DeployIO.java @@ -29,4 +29,8 @@ public default void setPosition(DeployerPosition position) {} public default void setDutyCycle(double dutyCycle) {} public default void adjustTarget(Angle angle) {} + + public default boolean isAtSetpoint() { + return true; + } } diff --git a/src/main/java/frc/robot/subsystems/deploy/DeployIOReal.java b/src/main/java/frc/robot/subsystems/deploy/DeployIOReal.java index 9e269941..c3259cc4 100644 --- a/src/main/java/frc/robot/subsystems/deploy/DeployIOReal.java +++ b/src/main/java/frc/robot/subsystems/deploy/DeployIOReal.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.deploy; import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Rotations; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusCode; @@ -83,13 +84,21 @@ public void setPosition(DeployerPosition position) { if (lastKnownPosition == DeployerPosition.EXTENDED && position == DeployerPosition.DUMP) slot = 1; if (lastKnownPosition == DeployerPosition.DUMP && position == DeployerPosition.EXTENDED) - slot = 2; + slot = 1; motor.setControl(positionRequest.withPosition(position.getAngle()).withSlot(slot)); lastKnownPosition = position; } + @Override + public boolean isAtSetpoint() { + return deployerMotorPosition + .getValue() + .minus(positionRequest.getPositionMeasure()) + .gt(Rotations.of(-0.05)); + } + @Override public void setDutyCycle(double dutyCycle) { motor.setControl(dutyCycleRequest.withOutput(dutyCycle)); diff --git a/src/main/java/frc/robot/subsystems/deploy/DeployerPosition.java b/src/main/java/frc/robot/subsystems/deploy/DeployerPosition.java index bd18daf1..3bdffe8c 100644 --- a/src/main/java/frc/robot/subsystems/deploy/DeployerPosition.java +++ b/src/main/java/frc/robot/subsystems/deploy/DeployerPosition.java @@ -5,9 +5,8 @@ import edu.wpi.first.units.measure.Angle; public enum DeployerPosition { - RETRACTED(Rotations.of(.7)), - EXTENDED(Rotations.of(0.01)), - DUMP(Rotations.of(.45)), + EXTENDED(Rotations.of(-.01)), + DUMP(Rotations.of(.4)), ; private Angle angle; diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 191da4cd..e7982e36 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -7,6 +7,7 @@ import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.pathfinding.Pathfinding; import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.hal.FRCNetComm.tInstances; @@ -33,6 +34,7 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d; 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.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; @@ -83,6 +85,15 @@ public class Drive extends SubsystemBase { private final Alert gyroDisconnectedAlert = new Alert("Disconnected gyro, using kinematics as fallback.", AlertType.kError); + public Command leftToRightAllianceCloseHubSweep = Commands.print("Path failed to load"); + public Command rightToLeftAllianceCloseHubSweep = Commands.print("Path failed to load"); + public Command leftToRightMidCloseHubSweep = Commands.print("Path failed to load"); + public Command rightToLeftMidCloseHubSweep = Commands.print("Path failed to load"); + public Command leftToRightMidFarHubSweep = Commands.print("Path failed to load"); + public Command rightToLeftMidFarHubSweep = Commands.print("Path failed to load"); + public Command leftToRightAllianceFarHubSweep = Commands.print("Path failed to load"); + public Command rightToLeftAllianceFarHubSweep = Commands.print("Path failed to load"); + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); private Rotation2d rawGyroRotation = Rotation2d.kZero; private SwerveModulePosition[] lastModulePositions = // For delta tracking @@ -143,6 +154,27 @@ public Drive( (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())), new SysIdRoutine.Mechanism( (voltage) -> runCharacterization(voltage.in(Volts)), null, this)); + + try { + leftToRightAllianceCloseHubSweep = + AutoBuilder.followPath(PathPlannerPath.fromPathFile("left-right-alliance-close-hub")); + rightToLeftAllianceCloseHubSweep = + AutoBuilder.followPath(PathPlannerPath.fromPathFile("right-left-alliance-close-hub")); + leftToRightMidCloseHubSweep = + AutoBuilder.followPath(PathPlannerPath.fromPathFile("left-right-mid-close-hub")); + rightToLeftMidCloseHubSweep = + AutoBuilder.followPath(PathPlannerPath.fromPathFile("right-left-mid-close-hub")); + leftToRightMidFarHubSweep = + AutoBuilder.followPath(PathPlannerPath.fromPathFile("left-right-mid-far-hub")); + rightToLeftMidFarHubSweep = + AutoBuilder.followPath(PathPlannerPath.fromPathFile("right-left-mid-far-hub")); + leftToRightAllianceFarHubSweep = + AutoBuilder.followPath(PathPlannerPath.fromPathFile("left-right-alliance-far-hub")); + rightToLeftAllianceFarHubSweep = + AutoBuilder.followPath(PathPlannerPath.fromPathFile("right-left-alliance-far-hub")); + } catch (Exception e) { + System.out.println("Error loading paths: " + e.getMessage()); + } } @Override @@ -219,6 +251,18 @@ public void setCoastMode() { } } + public void setHighCurrentLimits() { + for (Module module : modules) { + module.setHighCurrentLimits(); + } + } + + public void setLowCurrentLimits() { + for (Module module : modules) { + module.setLowCurrentLimits(); + } + } + /** * Runs the drive at the desired velocity. * diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index f0414945..72a0c7b2 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -79,6 +79,14 @@ public void setCoastMode() { io.setCoastMode(); } + public void setHighCurrentLimits() { + io.setHighCurrentLimits(); + } + + public void setLowCurrentLimits() { + io.setLowCurrentLimits(); + } + /** Runs the module with the specified setpoint state. Mutates the state to optimize it. */ public void runSetpoint(SwerveModuleState state) { // Optimize velocity setpoint diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index fa9063cf..8381b61f 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -50,4 +50,8 @@ public default void setTurnPosition(Rotation2d rotation) {} public default void setBrakeMode() {} public default void setCoastMode() {} + + public default void setHighCurrentLimits() {} + + public default void setLowCurrentLimits() {} } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index af2dcd7f..f56a94c1 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -7,7 +7,7 @@ package frc.robot.subsystems.drive; -import static frc.robot.util.PhoenixUtil.*; +import static frc.robot.util.PhoenixUtil.tryUntilOk; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; @@ -34,6 +34,7 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants; import frc.robot.generated.TunerConstants; import java.util.Queue; @@ -293,4 +294,28 @@ public void setCoastMode() { ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive)); } + + @Override + public void setHighCurrentLimits() { + driveTalon + .getConfigurator() + .apply( + driveConfig + .clone() + .CurrentLimits + .withStatorCurrentLimit(Constants.CurrentLimits.HIGH_DRIVE_STATOR) + .withSupplyCurrentLimit(Constants.CurrentLimits.HIGH_DRIVE_SUPPLY)); + } + + @Override + public void setLowCurrentLimits() { + driveTalon + .getConfigurator() + .apply( + driveConfig + .clone() + .CurrentLimits + .withStatorCurrentLimit(Constants.CurrentLimits.LOW_DRIVE_STATOR) + .withSupplyCurrentLimit(Constants.CurrentLimits.LOW_DRIVE_SUPPLY)); + } } diff --git a/src/main/java/frc/robot/subsystems/hood/HoodConstants.java b/src/main/java/frc/robot/subsystems/hood/HoodConstants.java index 5cd90aa1..4573fb27 100644 --- a/src/main/java/frc/robot/subsystems/hood/HoodConstants.java +++ b/src/main/java/frc/robot/subsystems/hood/HoodConstants.java @@ -14,11 +14,11 @@ import frc.robot.Constants; public class HoodConstants { - public static final double KP = 40; - public static final double KD = 1.5; + public static final double KP = 200; + public static final double KD = 3; - public static final double KS = 0; - public static final double KG = 0; + public static final double KS = 3; + public static final double KG = 3; public static final Angle ZERO_POSITION = Rotations.of(0.02); public static final Angle MAX_ANGLE = Rotations.of(.88); @@ -36,7 +36,7 @@ public class HoodConstants { InvertedValue.CounterClockwise_Positive, NeutralModeValue.Brake)) .withSlot0( MotorConfigUtils.createPidConfig( - KP, 0, KD, KS, 0, KG, 0, GravityTypeValue.Arm_Cosine)) + KP, 0, KD, KS, 0, KG, 0, GravityTypeValue.Elevator_Static)) .withFeedback( new FeedbackConfigs() .withFeedbackRemoteSensorID(Constants.CanIds.HOOD_ENCODER_ID) diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java index 66b67156..a0baa3e8 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java @@ -1,12 +1,11 @@ package frc.robot.subsystems.indexer; -import static edu.wpi.first.units.Units.Amps; - import badgerutils.motor.MotorConfigUtils; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import frc.robot.Constants; public class IndexerConstants { // CONFIGS @@ -16,7 +15,7 @@ public class IndexerConstants { new CurrentLimitsConfigs() .withStatorCurrentLimitEnable(false) .withSupplyCurrentLimitEnable(true) - .withSupplyCurrentLimit(Amps.of(40))) + .withSupplyCurrentLimit(Constants.CurrentLimits.INDEXER_SUPPLY)) .withMotorOutput( MotorConfigUtils.createMotorOutputConfig( InvertedValue.Clockwise_Positive, NeutralModeValue.Brake)); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index d082d765..b81fe22d 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -1,12 +1,11 @@ package frc.robot.subsystems.intake; -import static edu.wpi.first.units.Units.Amps; - import badgerutils.motor.MotorConfigUtils; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import frc.robot.Constants; public class IntakeConstants { @@ -17,7 +16,7 @@ public class IntakeConstants { new CurrentLimitsConfigs() .withStatorCurrentLimitEnable(false) .withSupplyCurrentLimitEnable(true) - .withSupplyCurrentLimit(Amps.of(60))) + .withSupplyCurrentLimit(Constants.CurrentLimits.INTAKE_SUPPLY)) .withMotorOutput( MotorConfigUtils.createMotorOutputConfig( InvertedValue.Clockwise_Positive, NeutralModeValue.Brake)); diff --git a/src/main/java/frc/robot/subsystems/leds/LedsConstants.java b/src/main/java/frc/robot/subsystems/leds/LedsConstants.java index 0cec265b..2f9e3494 100644 --- a/src/main/java/frc/robot/subsystems/leds/LedsConstants.java +++ b/src/main/java/frc/robot/subsystems/leds/LedsConstants.java @@ -7,7 +7,7 @@ public class LedsConstants { // 0-7 are onboard, 8-399 are an external strip. public static final int STRIP_START_INDEX = 8; - public static final int STRIP_END_INDEX = STRIP_START_INDEX + 26; + public static final int STRIP_END_INDEX = STRIP_START_INDEX + 40; public static final LEDConfigs CANDLE_CONFIG = new CANdleConfiguration().LED.withStripType(StripTypeValue.GRB).withBrightnessScalar(0.5); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 8713ead8..afb10cb0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -1,7 +1,5 @@ package frc.robot.subsystems.shooter; -import static edu.wpi.first.units.Units.Amps; - import badgerutils.motor.MotorConfigUtils; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; @@ -20,7 +18,7 @@ public class ShooterConstants { public static final double KI = 0; public static final double KD = 0; - public static final double KV = 2.5; + public static final double KV = 3; public static final double KS = 0.058037; public static final double ROTOR_TO_SENSOR_RATIO = (24D / 36D); @@ -45,7 +43,7 @@ public class ShooterConstants { .withCurrentLimits( new CurrentLimitsConfigs() .withStatorCurrentLimitEnable(false) - .withSupplyCurrentLimit(Amps.of(60)) + .withSupplyCurrentLimit(Constants.CurrentLimits.SHOOTER_SUPPLY) .withSupplyCurrentLimitEnable(true)) .withMotorOutput( MotorConfigUtils.createMotorOutputConfig( diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 4e4e74ac..adbccbdb 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -11,7 +11,7 @@ public class VisionConstants { // AprilTag layout public static final AprilTagFieldLayout APRIL_TAG_LAYOUT = - AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark); + AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded); // Camera names, must match names configured on coprocessor public static final String LEFT_BACK_CAMERA_NAME = "leftBack"; diff --git a/src/main/java/frc/robot/util/RebuiltUtils.java b/src/main/java/frc/robot/util/RebuiltUtils.java index 3828db7c..3add99ca 100644 --- a/src/main/java/frc/robot/util/RebuiltUtils.java +++ b/src/main/java/frc/robot/util/RebuiltUtils.java @@ -155,9 +155,23 @@ public static Translation2d getNearestAllianceCorner(Translation2d currentPositi return rightIsCloser ? rightCorner : leftCorner; } + public static boolean isLeftSide(Translation2d position) { + return AllianceTriggers.isBlueAlliance() == position.getY() > 4.05; + } + + public static boolean isOurHalf(Translation2d position) { + return AllianceTriggers.isBlueAlliance() == position.getX() < 8.25; + } + public static boolean isInAllianceZone(Translation2d position) { return AllianceTriggers.isBlueAlliance() ? position.getX() <= 4.5 : position.getX() >= 16.540988 - 4.5; } + + public static boolean isInOpponentAllianceZone(Translation2d position) { + return AllianceTriggers.isRedAlliance() + ? position.getX() <= 4.5 + : position.getX() >= 16.540988 - 4.5; + } }