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..21e2a318 --- /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": 5.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..ed319504 100644 --- a/src/main/deploy/pathplanner/paths/SHOP depot-bump.path +++ b/src/main/deploy/pathplanner/paths/SHOP depot-bump.path @@ -105,8 +105,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/SHOP depot-transition-bump.path b/src/main/deploy/pathplanner/paths/SHOP depot-transition-bump.path index 405cf3f1..ed190dff 100644 --- a/src/main/deploy/pathplanner/paths/SHOP depot-transition-bump.path +++ b/src/main/deploy/pathplanner/paths/SHOP depot-transition-bump.path @@ -75,8 +75,8 @@ "eventMarkers": [], "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/SHOP left-bump-bump-hub.path b/src/main/deploy/pathplanner/paths/SHOP left-bump-bump-hub.path index 33bba626..3712fc40 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 @@ -174,8 +174,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/SHOP left-bump-trench-hub.path b/src/main/deploy/pathplanner/paths/SHOP left-bump-trench-hub.path index 465a9e38..87b07883 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 @@ -184,8 +184,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/SHOP left-trench-depot-hub.path b/src/main/deploy/pathplanner/paths/SHOP left-trench-depot-hub.path index e59871f5..ffeccd43 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 @@ -203,8 +203,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/SHOP left-trench-trench-hub.path b/src/main/deploy/pathplanner/paths/SHOP left-trench-trench-hub.path index a7840e46..a01244e4 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 @@ -172,8 +172,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/SHOP right-bump-bump-hub.path b/src/main/deploy/pathplanner/paths/SHOP right-bump-bump-hub.path index 3f329638..fd1d31ce 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 @@ -174,8 +174,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/SHOP right-bump-trench-hub.path b/src/main/deploy/pathplanner/paths/SHOP right-bump-trench-hub.path index fa968254..526362ce 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 @@ -184,8 +184,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/SHOP right-trench-trench-hub.path b/src/main/deploy/pathplanner/paths/SHOP right-trench-trench-hub.path index b8f625e0..196c9245 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 @@ -172,8 +172,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/coasting-test.path b/src/main/deploy/pathplanner/paths/coasting-test.path index 109910e0..62192c1b 100644 --- a/src/main/deploy/pathplanner/paths/coasting-test.path +++ b/src/main/deploy/pathplanner/paths/coasting-test.path @@ -16,12 +16,20 @@ }, { "anchor": { - "x": 9.690602409638554, - "y": 7.411662650602409 + "x": 8.25, +<<<<<<< HEAD + "y": 4.035 }, "prevControl": { - "x": 8.69060240963855, - "y": 7.411662650602409 + "x": 7.2499999999999964, + "y": 4.035 +======= + "y": 7.138469879518072 + }, + "prevControl": { + "x": 7.2499999999999964, + "y": 7.138469879518072 +>>>>>>> e470d1a (hilltoppers) }, "nextControl": null, "isLocked": false, @@ -46,15 +54,15 @@ ], "globalConstraints": { "maxVelocity": 5.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 480.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 5.0, - "rotation": 180.0 + "rotation": 0.3320487358161149 }, "reversed": false, "folder": "Testing", diff --git a/src/main/deploy/pathplanner/paths/depot-bump.path b/src/main/deploy/pathplanner/paths/depot-bump.path index 64998fd4..f77ebf84 100644 --- a/src/main/deploy/pathplanner/paths/depot-bump.path +++ b/src/main/deploy/pathplanner/paths/depot-bump.path @@ -87,8 +87,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-coast-to-mid.path b/src/main/deploy/pathplanner/paths/depot-coast-to-mid.path index 1892f832..9ed9c3e3 100644 --- a/src/main/deploy/pathplanner/paths/depot-coast-to-mid.path +++ b/src/main/deploy/pathplanner/paths/depot-coast-to-mid.path @@ -78,8 +78,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-transition-SOTM.path b/src/main/deploy/pathplanner/paths/depot-transition-SOTM.path index 0d128ca7..f52af8bb 100644 --- a/src/main/deploy/pathplanner/paths/depot-transition-SOTM.path +++ b/src/main/deploy/pathplanner/paths/depot-transition-SOTM.path @@ -103,8 +103,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/left-bump-bump-hub.path b/src/main/deploy/pathplanner/paths/left-bump-bump-hub.path index b07d9799..61ce3d18 100644 --- a/src/main/deploy/pathplanner/paths/left-bump-bump-hub.path +++ b/src/main/deploy/pathplanner/paths/left-bump-bump-hub.path @@ -174,8 +174,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/left-bump-collect.path b/src/main/deploy/pathplanner/paths/left-bump-collect.path index 5b3d23b4..a377ef3a 100644 --- a/src/main/deploy/pathplanner/paths/left-bump-collect.path +++ b/src/main/deploy/pathplanner/paths/left-bump-collect.path @@ -78,8 +78,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/left-bump-far.path b/src/main/deploy/pathplanner/paths/left-bump-far.path index a429c4cd..c7c9e430 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, @@ -149,8 +149,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/left-bump-safe.path b/src/main/deploy/pathplanner/paths/left-bump-safe.path index 9945d587..aaa93c58 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, @@ -149,8 +149,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/left-bump-trench-hub.path b/src/main/deploy/pathplanner/paths/left-bump-trench-hub.path index 7547ce54..2b6fa5f8 100644 --- a/src/main/deploy/pathplanner/paths/left-bump-trench-hub.path +++ b/src/main/deploy/pathplanner/paths/left-bump-trench-hub.path @@ -184,8 +184,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/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..5986e629 100644 --- a/src/main/deploy/pathplanner/paths/left-trench-collect.path +++ b/src/main/deploy/pathplanner/paths/left-trench-collect.path @@ -87,8 +87,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/left-trench-depot-hub.path b/src/main/deploy/pathplanner/paths/left-trench-depot-hub.path index 12372fa4..879b541e 100644 --- a/src/main/deploy/pathplanner/paths/left-trench-depot-hub.path +++ b/src/main/deploy/pathplanner/paths/left-trench-depot-hub.path @@ -203,8 +203,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/left-trench-far.path b/src/main/deploy/pathplanner/paths/left-trench-far.path index 582fb82f..656d6d9b 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, @@ -149,8 +149,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/left-trench-safe.path b/src/main/deploy/pathplanner/paths/left-trench-safe.path index d340f603..0d8510b6 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, @@ -149,8 +149,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/left-trench-trench-hub.path b/src/main/deploy/pathplanner/paths/left-trench-trench-hub.path index 8bb11c26..ef223bdb 100644 --- a/src/main/deploy/pathplanner/paths/left-trench-trench-hub.path +++ b/src/main/deploy/pathplanner/paths/left-trench-trench-hub.path @@ -172,8 +172,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/qual 62 right-bump-far.path b/src/main/deploy/pathplanner/paths/qual 62 right-bump-far.path index 32ea641f..1c98606b 100644 --- a/src/main/deploy/pathplanner/paths/qual 62 right-bump-far.path +++ b/src/main/deploy/pathplanner/paths/qual 62 right-bump-far.path @@ -149,8 +149,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/right-bump-bump-hub.path b/src/main/deploy/pathplanner/paths/right-bump-bump-hub.path index 7e3ac127..05be6424 100644 --- a/src/main/deploy/pathplanner/paths/right-bump-bump-hub.path +++ b/src/main/deploy/pathplanner/paths/right-bump-bump-hub.path @@ -174,8 +174,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/right-bump-collect.path b/src/main/deploy/pathplanner/paths/right-bump-collect.path index 26b12893..5bbe5b48 100644 --- a/src/main/deploy/pathplanner/paths/right-bump-collect.path +++ b/src/main/deploy/pathplanner/paths/right-bump-collect.path @@ -78,8 +78,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/right-bump-far.path b/src/main/deploy/pathplanner/paths/right-bump-far.path index b96c3c44..a5143d12 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, @@ -149,8 +149,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/right-bump-safe.path b/src/main/deploy/pathplanner/paths/right-bump-safe.path index 7f2d85a6..581bb818 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, @@ -149,8 +149,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/right-bump-trench-hub.path b/src/main/deploy/pathplanner/paths/right-bump-trench-hub.path index 4109c6fc..c07f89f6 100644 --- a/src/main/deploy/pathplanner/paths/right-bump-trench-hub.path +++ b/src/main/deploy/pathplanner/paths/right-bump-trench-hub.path @@ -184,8 +184,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/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..9c6c92e0 100644 --- a/src/main/deploy/pathplanner/paths/right-trench-collect.path +++ b/src/main/deploy/pathplanner/paths/right-trench-collect.path @@ -87,8 +87,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/right-trench-far.path b/src/main/deploy/pathplanner/paths/right-trench-far.path index fee85b80..0e5d56fd 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, @@ -149,8 +149,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/right-trench-hub.path b/src/main/deploy/pathplanner/paths/right-trench-hub.path index a92b8af2..283434a6 100644 --- a/src/main/deploy/pathplanner/paths/right-trench-hub.path +++ b/src/main/deploy/pathplanner/paths/right-trench-hub.path @@ -172,8 +172,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/right-trench-safe.path b/src/main/deploy/pathplanner/paths/right-trench-safe.path index 3b99872a..e6152ade 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, @@ -149,8 +149,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/sotm-test.path b/src/main/deploy/pathplanner/paths/sotm-test.path index b7b88e11..2d2f5e4b 100644 --- a/src/main/deploy/pathplanner/paths/sotm-test.path +++ b/src/main/deploy/pathplanner/paths/sotm-test.path @@ -109,8 +109,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/trench-depot.path b/src/main/deploy/pathplanner/paths/trench-depot.path index e852cccb..cb721ec8 100644 --- a/src/main/deploy/pathplanner/paths/trench-depot.path +++ b/src/main/deploy/pathplanner/paths/trench-depot.path @@ -65,8 +65,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/settings.json b/src/main/deploy/pathplanner/settings.json index 6431e63a..efa79ddf 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -5,8 +5,9 @@ "pathFolders": [ "1st Pass", "2nd Pass", - "Depot", "3rd Pass", + "Depot", + "Teleop Pathing", "Testing" ], "autoFolders": [ @@ -14,8 +15,8 @@ "Testing" ], "defaultMaxVel": 5.0, - "defaultMaxAccel": 3.0, - "defaultMaxAngVel": 540.0, + "defaultMaxAccel": 5.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 5c07e504..249ebbe2 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -27,6 +27,7 @@ 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; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.leds.Leds; @@ -48,6 +49,7 @@ public class Autos { private final Intake intake; private final Shooter shooter; private final Booster booster; + private final Hood hood; private final Leds leds; private final Deploy deploy; @@ -72,6 +74,7 @@ public Autos( Intake intake, Shooter shooter, Booster booster, + Hood hood, Leds leds, Deploy deploy) { this.drive = drive; @@ -79,6 +82,7 @@ public Autos( this.intake = intake; this.shooter = shooter; this.booster = booster; + this.hood = hood; this.leds = leds; this.deploy = deploy; @@ -94,9 +98,10 @@ public Autos( indexer, deploy, booster, + hood, leds, () -> RebuiltUtils.getCurrentHubLocation().toTranslation2d(), - Constants.Tolerances.SCORING_ANGLE_TOLERANCE, + inAllianceZoneSupplier, () -> false, () -> false, () -> true, @@ -114,7 +119,8 @@ public Autos( drive .getPose() .getTranslation() - .getDistance(RebuiltUtils.getCurrentHubLocation().toTranslation2d()))) + .getDistance(RebuiltUtils.getCurrentHubLocation().toTranslation2d())), + () -> ShooterCommands.HUB_SETPOINTS) .asProxy(); intakeCommand = @@ -130,11 +136,12 @@ public Autos( indexer, deploy, booster, + hood, leds, () -> 0, () -> 0, () -> RebuiltUtils.getCurrentHubLocation().toTranslation2d(), - Constants.Tolerances.SCORING_ANGLE_TOLERANCE, + inAllianceZoneSupplier, () -> false, () -> false, () -> true, @@ -150,11 +157,12 @@ public Autos( indexer, deploy, booster, + hood, leds, () -> 0, () -> 0, () -> RebuiltUtils.getCurrentHubLocation().toTranslation2d(), - Constants.Tolerances.SCORING_ANGLE_TOLERANCE, + inAllianceZoneSupplier, () -> false, () -> false, () -> true, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f316f9e1..c2d9cbf0 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; /** @@ -46,6 +48,22 @@ public static class Tolerances { public static final AngularVelocity NORMAL_SPEED_TOLERANCE = RotationsPerSecond.of(2); } + 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 { // shooter public static final int SHOOTER_LEFT_TOP_MOTOR_ID = 13; @@ -53,6 +71,8 @@ public static class CanIds { public static final int SHOOTER_RIGHT_TOP_MOTOR_ID = 15; public static final int SHOOTER_RIGHT_BOTTOM_MOTOR_ID = 16; public static final int BOOSTER_MOTOR_ID = 28; + public static final int HOOD_MOTOR_ID = 29; + public static final int HOOD_ENCODER_ID = 30; public static final int SHOOTER_ENCODER_ID = 19; 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 03fc5cf9..bceb5410 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,6 +17,9 @@ import frc.robot.subsystems.drive.ModuleIOTalonFX; import frc.robot.subsystems.fueldetection.FuelDetection; import frc.robot.subsystems.fueldetection.FuelDetectionIO; +import frc.robot.subsystems.hood.Hood; +import frc.robot.subsystems.hood.HoodIO; +import frc.robot.subsystems.hood.HoodIOReal; import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.indexer.IndexerIO; import frc.robot.subsystems.indexer.IndexerIOReal; @@ -43,12 +46,13 @@ */ public class RobotContainer { // Subsystems - private final Drive drive; + public final Drive drive; private final Vision vision; private final Intake intake; private final Indexer indexer; private final Shooter shooter; private final Booster booster; + private final Hood hood; private final FuelDetection fuelDetection; private final Leds leds; private final Deploy deploy; @@ -91,7 +95,8 @@ public RobotContainer() { VisionConstants.RIGHT_SIDE_CAMERA_POSITION)); fuelDetection = new FuelDetection(new FuelDetectionIO() {}); leds = new Leds(new LedsReal()); - deploy = new Deploy(new DeployIOReal()); + deploy = new Deploy(new DeployIO() {}); + hood = new Hood(new HoodIOReal()); break; case SIM: @@ -118,6 +123,7 @@ public RobotContainer() { fuelDetection = new FuelDetection(new FuelDetectionIO() {}); leds = new Leds(new LedsReal()); deploy = new Deploy(new DeployIOReal()); + hood = new Hood(new HoodIOReal()); break; default: @@ -143,11 +149,13 @@ public RobotContainer() { fuelDetection = new FuelDetection(new FuelDetectionIO() {}); leds = new Leds(new LedsIO() {}); deploy = new Deploy(new DeployIO() {}); + hood = new Hood(new HoodIO() {}); break; } - controls = new Controls(drive, intake, shooter, indexer, booster, fuelDetection, leds, deploy); - autos = new Autos(drive, indexer, intake, shooter, booster, leds, deploy); + controls = + new Controls(drive, intake, shooter, indexer, booster, hood, fuelDetection, leds, deploy); + autos = new Autos(drive, indexer, intake, shooter, booster, hood, leds, deploy); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/DriveAtAngleCommand.java b/src/main/java/frc/robot/commands/DriveAtAngleCommand.java index ec58f58f..2c6e4574 100644 --- a/src/main/java/frc/robot/commands/DriveAtAngleCommand.java +++ b/src/main/java/frc/robot/commands/DriveAtAngleCommand.java @@ -111,7 +111,6 @@ public void resetPID() { public double getPIDOutput(boolean flipped) { Logger.recordOutput("Drive/Angle Setpoint Error", angleController.getPositionError()); - return angleController.calculate( drive.getRotation().getRadians(), 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 98dd6536..4faf5b14 100644 --- a/src/main/java/frc/robot/commands/SafeAimAndShootCommand.java +++ b/src/main/java/frc/robot/commands/SafeAimAndShootCommand.java @@ -1,12 +1,12 @@ package frc.robot.commands; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; 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; import frc.robot.subsystems.leds.Leds; import frc.robot.subsystems.shooter.Shooter; @@ -24,11 +24,12 @@ public SafeAimAndShootCommand( Indexer indexer, Deploy deploy, Booster booster, + Hood hood, Leds leds, DoubleSupplier xSupplier, DoubleSupplier ySupplier, Supplier positionSupplier, - Rotation2d angleTolerance, + BooleanSupplier isScoring, BooleanSupplier overrideAngleSafeguard, BooleanSupplier overrideVelocitySafeguard, BooleanSupplier overrideHubActive, @@ -41,9 +42,10 @@ public SafeAimAndShootCommand( indexer, deploy, booster, + hood, leds, positionSupplier, - angleTolerance, + isScoring, overrideAngleSafeguard, overrideVelocitySafeguard, overrideHubActive, diff --git a/src/main/java/frc/robot/commands/SafeShootCommand.java b/src/main/java/frc/robot/commands/SafeShootCommand.java index b064fb30..42a34bea 100644 --- a/src/main/java/frc/robot/commands/SafeShootCommand.java +++ b/src/main/java/frc/robot/commands/SafeShootCommand.java @@ -3,7 +3,6 @@ import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Seconds; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Time; @@ -15,6 +14,7 @@ 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; import frc.robot.subsystems.leds.Leds; import frc.robot.subsystems.shooter.Shooter; @@ -40,9 +40,10 @@ public SafeShootCommand( Indexer indexer, Deploy deploy, Booster booster, + Hood hood, Leds leds, Supplier positionSupplier, - Rotation2d angleTolerance, + BooleanSupplier isScoring, BooleanSupplier overrideAngleSafeguard, BooleanSupplier overrideVelocitySafeguard, BooleanSupplier overrideHubActive, @@ -57,7 +58,14 @@ public SafeShootCommand( shooter.isAtRequestedSpeed(Constants.Tolerances.NORMAL_SPEED_TOLERANCE); BooleanSupplier driveAngleCondition = - () -> drive.isLocked(drive, positionSupplier.get(), true, angleTolerance); + () -> + drive.isLocked( + drive, + positionSupplier.get(), + true, + isScoring.getAsBoolean() + ? Constants.Tolerances.SCORING_ANGLE_TOLERANCE + : Constants.Tolerances.PASSING_ANGLE_TOLERANCE); BooleanSupplier hubActiveCondition = () -> @@ -92,6 +100,16 @@ public SafeShootCommand( .andThen(indexer.indexUntilCancelledCommand(INDEXER_SPEED)), combinedCondition); + Command guardedDeployCommand = + new GuardedCommand( + Commands.waitUntil( + () -> + shooter + .isAtRequestedSpeed(Constants.Tolerances.INITIAL_SPEED_TOLERANCE) + .getAsBoolean()) + .andThen(Commands.runOnce(() -> hasStartedShooting = true)) + .andThen(deploy.crunchCommand()), + combinedCondition); // Command guardedDeployCommand = // Commands.waitUntil(() -> hasStartedShooting) // .andThen(Commands.waitTime(RETRACT_DELAY)) @@ -101,11 +119,17 @@ public SafeShootCommand( // () -> combinedCondition.getAsBoolean() && // notMovingCondition.getAsBoolean())); + Supplier distanceSupplier = + () -> Meters.of(drive.getPose().getTranslation().getDistance(positionSupplier.get())); + Command shootAtDistanceCommand = ShooterCommands.shootAtDistanceCommand( shooter, + distanceSupplier, () -> - Meters.of(drive.getPose().getTranslation().getDistance(positionSupplier.get()))) + isScoring.getAsBoolean() + ? ShooterCommands.HUB_SETPOINTS + : ShooterCommands.PASSING_SETPOINTS) .withInterruptBehavior(InterruptionBehavior.kCancelIncoming); Command boosterCommand = @@ -113,6 +137,15 @@ public SafeShootCommand( .boostCommand(BOOSTER_SPEED) .withInterruptBehavior(InterruptionBehavior.kCancelIncoming); + Command hoodCommand = + hood.angleFromDistance( + distanceSupplier, + () -> + isScoring.getAsBoolean() + ? ShooterCommands.HUB_SETPOINTS + : ShooterCommands.PASSING_SETPOINTS) + .withInterruptBehavior(InterruptionBehavior.kCancelIncoming); + @SuppressWarnings("unused") Trigger indexerRescheduler = new Trigger( @@ -155,8 +188,9 @@ public SafeShootCommand( activityTracker, shootAtDistanceCommand.asProxy(), guardedIndexerCommand.asProxy(), - // guardedDeployCommand.asProxy(), + guardedDeployCommand.asProxy(), boosterCommand.asProxy(), + hoodCommand, loggedGuardCommand); } diff --git a/src/main/java/frc/robot/commands/ShootOnTheMoveCommands.java b/src/main/java/frc/robot/commands/ShootOnTheMoveCommands.java index d371f356..5eb5c5ad 100644 --- a/src/main/java/frc/robot/commands/ShootOnTheMoveCommands.java +++ b/src/main/java/frc/robot/commands/ShootOnTheMoveCommands.java @@ -11,9 +11,11 @@ import edu.wpi.first.units.measure.Time; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.commands.ShooterCommands.ShooterSetpoint; 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; import frc.robot.subsystems.leds.Leds; import frc.robot.subsystems.shooter.Shooter; @@ -32,13 +34,24 @@ public static Command shootOnTheMoveAutoCommand( Indexer indexer, Deploy deploy, Booster booster, + Hood hood, Leds leds, Supplier target, - Rotation2d angleTolerance, + BooleanSupplier isScoring, BooleanSupplier overrideAngleSafeguard, BooleanSupplier overrideVelocitySafeguard, BooleanSupplier overrideHubActive, BooleanSupplier overrideAutoRanging) { + Supplier leadTarget = + () -> + calculateLeadTarget( + drive, + target, + () -> + isScoring.getAsBoolean() + ? ShooterCommands.HUB_SETPOINTS + : ShooterCommands.PASSING_SETPOINTS); + SafeShootCommand shootCommand = new SafeShootCommand( drive, @@ -46,17 +59,17 @@ public static Command shootOnTheMoveAutoCommand( indexer, deploy, booster, + hood, leds, - () -> calculateLeadTarget(drive, target), - angleTolerance, + leadTarget, + isScoring, overrideAngleSafeguard, overrideVelocitySafeguard, overrideHubActive, overrideAutoRanging); DriveAimLockedCommand driveCommand = - new DriveAimLockedCommand( - drive, () -> 0, () -> 0, () -> calculateLeadTarget(drive, target), true); + new DriveAimLockedCommand(drive, () -> 0, () -> 0, leadTarget, true); return shootCommand.alongWith( Commands.startEnd( @@ -74,11 +87,12 @@ public static Command aimAndShootOnTheMoveCommand( Indexer indexer, Deploy deploy, Booster booster, + Hood hood, Leds leds, DoubleSupplier xSupplier, DoubleSupplier ySupplier, Supplier target, - Rotation2d angleTolerance, + BooleanSupplier isScoring, BooleanSupplier overrideAngleSafeguard, BooleanSupplier overrideVelocitySafeguard, BooleanSupplier overrideHubActive, @@ -89,18 +103,27 @@ public static Command aimAndShootOnTheMoveCommand( indexer, deploy, booster, + hood, leds, () -> xSupplier.getAsDouble() * SLOWDOWN_FACTOR, () -> ySupplier.getAsDouble() * SLOWDOWN_FACTOR, - () -> calculateLeadTarget(drive, target), - angleTolerance, + () -> + calculateLeadTarget( + drive, + target, + () -> + isScoring.getAsBoolean() + ? ShooterCommands.HUB_SETPOINTS + : ShooterCommands.PASSING_SETPOINTS), + isScoring, overrideAngleSafeguard, overrideVelocitySafeguard, overrideHubActive, overrideAutoRanging); } - private static Translation2d calculateLeadTarget(Drive drive, Supplier target) { + private static Translation2d calculateLeadTarget( + Drive drive, Supplier target, Supplier setpoints) { Translation2d robotPos = drive.getPose().getTranslation(); Translation2d targetPos = target.get(); @@ -110,8 +133,7 @@ private static Translation2d calculateLeadTarget(Drive drive, Supplier setpoint.distance.gte(distance)).findFirst(); if (firstSetpointOptional.isEmpty() && secondSetpointOptional.isEmpty()) - return new ShooterSetpoint(Meters.of(0), RotationsPerSecond.of(0), Seconds.of(0)); + return new ShooterSetpoint( + Meters.of(0), Rotations.of(0), RotationsPerSecond.of(0), Seconds.of(0)); else if (firstSetpointOptional.isEmpty()) return secondSetpointOptional.get(); else if (secondSetpointOptional.isEmpty()) return firstSetpointOptional.get(); @@ -58,6 +91,9 @@ public static ShooterSetpoint interpolateSetpoints( firstSetpoint.distance.in(Meters), secondSetpoint.distance.in(Meters), distance.in(Meters)); + double lerpedAngle = + Interpolation.lerp( + firstSetpoint.hoodAngle.in(Rotations), secondSetpoint.hoodAngle.in(Rotations), t); double lerpedVelocity = Interpolation.lerp( firstSetpoint.velocity.in(RotationsPerSecond), @@ -68,7 +104,10 @@ public static ShooterSetpoint interpolateSetpoints( Interpolation.lerp(firstSetpoint.time.in(Seconds), secondSetpoint.time.in(Seconds), t); return new ShooterSetpoint( - distance, RotationsPerSecond.of(lerpedVelocity), Seconds.of(lerpedTime)); + distance, + Rotations.of(lerpedAngle), + RotationsPerSecond.of(lerpedVelocity), + Seconds.of(lerpedTime)); } public static Command shootAtSpeedCommand(Shooter shooter, AngularVelocity velocity) { @@ -79,22 +118,18 @@ public static Command shootAtSpeedCommand(Shooter shooter, Supplier shooter.setVelocity(velocity.get()), shooter::setIdle, shooter); } - public static Command shootAtDistanceCommand(Shooter shooter, Supplier distance) { + public static Command shootAtDistanceCommand( + Shooter shooter, Supplier distance, Supplier setpoints) { return shootAtSpeedCommand( shooter, () -> { Logger.recordOutput("Shooter/Distance to Target", distance.get().in(Feet)); - return interpolateSetpoints(SETPOINTS, distance.get()).velocity; + return interpolateSetpoints(setpoints.get(), distance.get()).velocity; }); } - public static Command shootForTimeCommand( - Shooter shooter, Supplier distanceSupplier, Time time) { - return new ParallelDeadlineGroup( - new WaitCommand(time), shootAtDistanceCommand(shooter, distanceSupplier)); - } - - public record ShooterSetpoint(Distance distance, AngularVelocity velocity, Time time) + public record ShooterSetpoint( + Distance distance, Angle hoodAngle, AngularVelocity velocity, Time time) implements Comparable { @Override public int compareTo(ShooterSetpoint o) { diff --git a/src/main/java/frc/robot/controls/CompetitionControllerMapping.java b/src/main/java/frc/robot/controls/CompetitionControllerMapping.java index 7047ea94..5c15fa32 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; @@ -16,23 +17,23 @@ import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Constants; 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; +import frc.robot.subsystems.hood.HoodConstants; import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.leds.Leds; import frc.robot.subsystems.shooter.Shooter; import frc.robot.util.LocationUtils; import frc.robot.util.RebuiltUtils; +import java.util.function.BooleanSupplier; import org.littletonrobotics.junction.Logger; public class CompetitionControllerMapping extends ControllerMapping { @@ -42,6 +43,7 @@ public class CompetitionControllerMapping extends ControllerMapping { private final Shooter shooter; private final Indexer indexer; private final Booster booster; + private final Hood hood; private final FuelDetection fuelDetection; private final Leds leds; private final Deploy deploy; @@ -54,6 +56,7 @@ public CompetitionControllerMapping( Shooter shooter, Indexer indexer, Booster booster, + Hood hood, FuelDetection fuelDetection, Leds leds, Deploy deploy) { @@ -63,6 +66,7 @@ public CompetitionControllerMapping( this.shooter = shooter; this.indexer = indexer; this.booster = booster; + this.hood = hood; this.fuelDetection = fuelDetection; this.leds = leds; this.deploy = deploy; @@ -91,6 +95,9 @@ public void bind() { ? RebuiltUtils.getCurrentHubLocation().toTranslation2d() : RebuiltUtils.getNearestAllianceCorner(drive.getPose().getTranslation()))); + BooleanSupplier inAllianceZoneSupplier = + () -> RebuiltUtils.isInAllianceZone(drive.getPose().getTranslation()); + /* ---Default Commands--- */ // Drive with stick @@ -102,6 +109,9 @@ public void bind() { () -> -driverController.getRightX()) .alongWith(logWithinRangeCommand)); + // Hood down + hood.setDefaultCommand(hood.moveToAngle(HoodConstants.ZERO_POSITION)); + /* ---P1--- */ // Reset Odometry @@ -123,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 @@ -164,32 +178,38 @@ public void bind() { indexer, deploy, booster, + hood, leds, () -> -driverController.getLeftY(), () -> -driverController.getLeftX(), () -> - RebuiltUtils.isInAllianceZone(drive.getPose().getTranslation()) + inAllianceZoneSupplier.getAsBoolean() ? RebuiltUtils.getCurrentHubLocation().toTranslation2d() : RebuiltUtils.getNearestAllianceCorner( drive.getPose().getTranslation()), - RebuiltUtils.isInAllianceZone(drive.getPose().getTranslation()) - ? Constants.Tolerances.SCORING_ANGLE_TOLERANCE - : Constants.Tolerances.PASSING_ANGLE_TOLERANCE, + inAllianceZoneSupplier, operatorController.rightBumper(), () -> operatorController.rightBumper().getAsBoolean() - || !RebuiltUtils.isInAllianceZone(drive.getPose().getTranslation()), + || !inAllianceZoneSupplier.getAsBoolean(), () -> operatorController.rightBumper().getAsBoolean() || operatorController.leftBumper().getAsBoolean() - || !RebuiltUtils.isInAllianceZone(drive.getPose().getTranslation()), + || !inAllianceZoneSupplier.getAsBoolean(), () -> operatorController.rightBumper().getAsBoolean() - || !RebuiltUtils.isInAllianceZone(drive.getPose().getTranslation())) + || !inAllianceZoneSupplier.getAsBoolean()) .alongWith(loggedTargetCommand) .withInterruptBehavior(InterruptionBehavior.kCancelIncoming)); /* ---P2--- */ + // Force through defence? + operatorController + .leftStick() + .whileTrue( + Commands.startEnd( + () -> drive.setHighCurrentLimits(), () -> drive.setLowCurrentLimits())); + // Spool Shooter operatorController .rightTrigger() @@ -198,11 +218,12 @@ public void bind() { shooter, () -> LocationUtils.getDistanceToLocation( - RebuiltUtils.isInAllianceZone(drive.getPose().getTranslation()) + inAllianceZoneSupplier.getAsBoolean() ? RebuiltUtils.getCurrentHubLocation().toTranslation2d() : RebuiltUtils.getNearestAllianceCorner( drive.getPose().getTranslation()), - drive.getPose().getTranslation())) + drive.getPose().getTranslation()), + () -> ShooterCommands.HUB_SETPOINTS) .withInterruptBehavior(InterruptionBehavior.kCancelSelf) .alongWith( new InstantCommand( @@ -217,12 +238,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 @@ -278,4 +293,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 d23eec47..d269ff27 100644 --- a/src/main/java/frc/robot/controls/Controls.java +++ b/src/main/java/frc/robot/controls/Controls.java @@ -8,6 +8,7 @@ import frc.robot.subsystems.deploy.Deploy; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.fueldetection.FuelDetection; +import frc.robot.subsystems.hood.Hood; import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.leds.Leds; @@ -37,6 +38,7 @@ public Controls( Shooter shooter, Indexer indexer, Booster booster, + Hood hood, FuelDetection fuelDetection, Leds leds, Deploy deploy) { @@ -54,6 +56,7 @@ public Controls( shooter, indexer, booster, + hood, fuelDetection, leds, deploy)); @@ -68,7 +71,14 @@ public Controls( mappings.put( ControlStates.SHOOTER_TESTING, new ShooterTestingControllerMapping( - driverController, operatorController, drivetrain, intake, shooter, indexer, booster)); + driverController, + operatorController, + drivetrain, + intake, + shooter, + indexer, + booster, + hood)); Consumer> onChange = (nextState) -> { diff --git a/src/main/java/frc/robot/controls/ShooterTestingControllerMapping.java b/src/main/java/frc/robot/controls/ShooterTestingControllerMapping.java index 682f19db..41f369a5 100644 --- a/src/main/java/frc/robot/controls/ShooterTestingControllerMapping.java +++ b/src/main/java/frc/robot/controls/ShooterTestingControllerMapping.java @@ -1,6 +1,7 @@ package frc.robot.controls; import static edu.wpi.first.units.Units.Feet; +import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; import badgerutils.commands.CommandUtils; @@ -19,6 +20,7 @@ import frc.robot.commands.ShooterCommands; import frc.robot.subsystems.booster.Booster; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.hood.Hood; import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.shooter.Shooter; @@ -35,11 +37,16 @@ public class ShooterTestingControllerMapping extends ControllerMapping { private final Shooter shooter; private final Indexer indexer; private final Booster booster; + private final Hood hood; @AutoLogOutput private final LoggedNetworkNumberPlus targetSpeed = new LoggedNetworkNumberPlus("/Tuning/Shooter RPS", 0.75); + @AutoLogOutput + private final LoggedNetworkNumberPlus hoodAngle = + new LoggedNetworkNumberPlus("/Tuning/Hood Angle", 0); + @AutoLogOutput private final LoggedNetworkNumberPlus boosterPower = new LoggedNetworkNumberPlus("/Tuning/Booster Duty Cycle", 1); @@ -51,13 +58,16 @@ public ShooterTestingControllerMapping( Intake intake, Shooter shooter, Indexer indexer, - Booster booster) { + Booster booster, + Hood hood) { super(driverController, operatorController); this.drive = drive; this.intake = intake; this.shooter = shooter; this.indexer = indexer; this.booster = booster; + + this.hood = hood; } @Override @@ -100,6 +110,9 @@ public void bind() { drive.getPose().getTranslation(), RebuiltUtils.getCurrentHubLocation().toTranslation2d()))))); + hood.setDefaultCommand( + Commands.run(() -> hood.setAngle(Rotations.of(hoodAngle.getAsDouble())), hood)); + /* ---P1--- */ // Reset Odometry @@ -129,7 +142,8 @@ public void bind() { .alongWith( new GuardedCommand( indexer.indexUntilCancelledCommand(1), - shooter.isAtRequestedSpeed(Constants.Tolerances.NORMAL_SPEED_TOLERANCE)))); + shooter.isAtRequestedSpeed(Constants.Tolerances.NORMAL_SPEED_TOLERANCE))) + .alongWith(booster.boostCommand(0.8))); driverController.a().whileTrue(booster.boostCommand(1)); driverController @@ -141,7 +155,7 @@ public void bind() { new GuardedCommand( indexer.indexUntilCancelledCommand(1), shooter.isAtRequestedSpeed(Constants.Tolerances.NORMAL_SPEED_TOLERANCE)) - .alongWith(intake.shakeIntake()) + .alongWith(booster.boostCommand(0.8)) .alongWith( new DriveAimLockedCommand( drive, @@ -155,5 +169,6 @@ public void bind() { public void clear() { super.clear(); CommandUtils.removeAndCancelDefaultCommand(drive); + CommandUtils.removeAndCancelDefaultCommand(hood); } } 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..7eb892e8 100644 --- a/src/main/java/frc/robot/subsystems/deploy/Deploy.java +++ b/src/main/java/frc/robot/subsystems/deploy/Deploy.java @@ -41,18 +41,12 @@ public Command deployCommand() { } 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 Commands.startEnd( + () -> { + setDeployerPosition(DeployerPosition.DUMP); + System.out.println("Crunch"); + }, + () -> setDeployerPosition(DeployerPosition.EXTENDED), + this); } } diff --git a/src/main/java/frc/robot/subsystems/deploy/DeployConstants.java b/src/main/java/frc/robot/subsystems/deploy/DeployConstants.java index 05b67df3..5d96d2c2 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,19 +12,15 @@ 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 = 100; + 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 = 300; + public static final double KD_1 = 30; + public static final double KS_1 = 0; - private static final double ROTOR_TO_SENSOR_RATIO = (15D / 1D) * (21D / 20D); + private static final double ROTOR_TO_SENSOR_RATIO = (25D / 1D) * (18D / 18D); public static final TalonFXConfiguration DEPLOYER_MOTOR_CONFIGS = new TalonFXConfiguration() @@ -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/DeployIOReal.java b/src/main/java/frc/robot/subsystems/deploy/DeployIOReal.java index 9e269941..102641a2 100644 --- a/src/main/java/frc/robot/subsystems/deploy/DeployIOReal.java +++ b/src/main/java/frc/robot/subsystems/deploy/DeployIOReal.java @@ -83,7 +83,9 @@ 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; + + System.out.println("With slot " + slot); motor.setControl(positionRequest.withPosition(position.getAngle()).withSlot(slot)); diff --git a/src/main/java/frc/robot/subsystems/deploy/DeployerPosition.java b/src/main/java/frc/robot/subsystems/deploy/DeployerPosition.java index bd18daf1..7219bfc7 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)), + DUMP(Rotations.of(.2)), ; 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/Hood.java b/src/main/java/frc/robot/subsystems/hood/Hood.java new file mode 100644 index 00000000..4b1d13b4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/hood/Hood.java @@ -0,0 +1,52 @@ +package frc.robot.subsystems.hood; + +import static edu.wpi.first.units.Units.Rotations; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.commands.ShooterCommands; +import frc.robot.commands.ShooterCommands.ShooterSetpoint; +import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; + +public class Hood extends SubsystemBase { + + private final HoodIO io; + private final HoodIOInputsAutoLogged inputs = new HoodIOInputsAutoLogged(); + + public Hood(HoodIO io) { + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Hood", inputs); + } + + public void setAngle(Angle angle) { + io.setAngle(angle); + Logger.recordOutput("Hood/Angle Setpoint", angle.in(Rotations)); + } + + public Command moveToAngle(Angle angle) { + return Commands.startEnd( + () -> setAngle(angle), () -> setAngle(HoodConstants.ZERO_POSITION), this); + } + + public Command moveToAngle(Supplier angleSupplier) { + return Commands.runEnd( + () -> setAngle(angleSupplier.get()), () -> setAngle(HoodConstants.ZERO_POSITION), this); + } + + public Command angleFromDistance( + Supplier distanceSupplier, Supplier setpoints) { + return moveToAngle( + () -> + ShooterCommands.interpolateSetpoints(setpoints.get(), distanceSupplier.get()) + .hoodAngle()); + } +} diff --git a/src/main/java/frc/robot/subsystems/hood/HoodConstants.java b/src/main/java/frc/robot/subsystems/hood/HoodConstants.java new file mode 100644 index 00000000..5cd90aa1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/hood/HoodConstants.java @@ -0,0 +1,45 @@ +package frc.robot.subsystems.hood; + +import static edu.wpi.first.units.Units.Rotations; + +import badgerutils.motor.MotorConfigUtils; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.units.measure.Angle; +import frc.robot.Constants; + +public class HoodConstants { + public static final double KP = 40; + public static final double KD = 1.5; + + public static final double KS = 0; + public static final double KG = 0; + + public static final Angle ZERO_POSITION = Rotations.of(0.02); + public static final Angle MAX_ANGLE = Rotations.of(.88); + + public static final double ROTOR_TO_SENSOR_RATIO = 150.462963; + + public static final TalonFXConfiguration HOOD_MOTOR_CONFIGS = + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withSupplyCurrentLimit(40) + .withSupplyCurrentLimitEnable(true)) + .withMotorOutput( + MotorConfigUtils.createMotorOutputConfig( + InvertedValue.CounterClockwise_Positive, NeutralModeValue.Brake)) + .withSlot0( + MotorConfigUtils.createPidConfig( + KP, 0, KD, KS, 0, KG, 0, GravityTypeValue.Arm_Cosine)) + .withFeedback( + new FeedbackConfigs() + .withFeedbackRemoteSensorID(Constants.CanIds.HOOD_ENCODER_ID) + .withRotorToSensorRatio(ROTOR_TO_SENSOR_RATIO) + .withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder)); +} diff --git a/src/main/java/frc/robot/subsystems/hood/HoodIO.java b/src/main/java/frc/robot/subsystems/hood/HoodIO.java new file mode 100644 index 00000000..a8768030 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/hood/HoodIO.java @@ -0,0 +1,23 @@ +package frc.robot.subsystems.hood; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Temperature; +import org.littletonrobotics.junction.AutoLog; + +public interface HoodIO { + @AutoLog + public static class HoodIOInputs { + public boolean isMotorConnected; + public AngularVelocity motorVelocity; + public Angle motorPosition; + public Current motorSupplyCurrent; + public Current motorStatorCurrent; + public Temperature motorTemperature; + } + + public default void updateInputs(HoodIOInputs inputs) {} + + public default void setAngle(Angle angle) {} +} diff --git a/src/main/java/frc/robot/subsystems/hood/HoodIOReal.java b/src/main/java/frc/robot/subsystems/hood/HoodIOReal.java new file mode 100644 index 00000000..c0bbf27c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/hood/HoodIOReal.java @@ -0,0 +1,106 @@ +package frc.robot.subsystems.hood; + +import static edu.wpi.first.units.Units.Rotations; + +import badgerutils.motor.MotorConfigUtils; +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.GravityTypeValue; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Temperature; +import frc.robot.Constants; +import frc.robot.util.LoggedNetworkNumberPlus; +import java.util.function.DoubleConsumer; +import org.littletonrobotics.junction.AutoLogOutput; + +public class HoodIOReal implements HoodIO { + @AutoLogOutput + private static final LoggedNetworkNumberPlus KP_SUPPLIER = + new LoggedNetworkNumberPlus("Tuning/Hood KP", HoodConstants.KP); + + @AutoLogOutput + private static final LoggedNetworkNumberPlus KD_SUPPLIER = + new LoggedNetworkNumberPlus("Tuning/Hood KD", HoodConstants.KD); + + @AutoLogOutput + private static final LoggedNetworkNumberPlus KS_SUPPLIER = + new LoggedNetworkNumberPlus("Tuning/Hood KS", HoodConstants.KS); + + @AutoLogOutput + private static final LoggedNetworkNumberPlus KG_SUPPLIER = + new LoggedNetworkNumberPlus("Tuning/Hood KG", HoodConstants.KG); + + private final TalonFX motor; + + private final StatusSignal motorVelocity; + private final StatusSignal motorPosition; + private final StatusSignal motorSupplyCurrent; + private final StatusSignal motorStatorCurrent; + private final StatusSignal motorTemperature; + + private final PositionTorqueCurrentFOC positionRequest; + + public HoodIOReal() { + motor = new TalonFX(Constants.CanIds.HOOD_MOTOR_ID); + motor.getConfigurator().apply(HoodConstants.HOOD_MOTOR_CONFIGS); + + motorVelocity = motor.getVelocity(); + motorPosition = motor.getPosition(); + motorSupplyCurrent = motor.getSupplyCurrent(); + motorStatorCurrent = motor.getStatorCurrent(); + motorTemperature = motor.getDeviceTemp(); + + positionRequest = new PositionTorqueCurrentFOC(0); + + DoubleConsumer applySlot0 = + value -> + motor + .getConfigurator() + .apply( + MotorConfigUtils.createPidConfig( + KP_SUPPLIER.get(), + 0, + KD_SUPPLIER.get(), + KS_SUPPLIER.get(), + 0, + KG_SUPPLIER.get(), + 0, + GravityTypeValue.Arm_Cosine)); + + KP_SUPPLIER.addSubscriber(applySlot0); + KD_SUPPLIER.addSubscriber(applySlot0); + KS_SUPPLIER.addSubscriber(applySlot0); + KG_SUPPLIER.addSubscriber(applySlot0); + } + + @Override + public void updateInputs(HoodIOInputs inputs) { + StatusCode motorStatus = + BaseStatusSignal.refreshAll( + motorVelocity, motorPosition, motorSupplyCurrent, motorStatorCurrent, motorTemperature); + + inputs.isMotorConnected = motorStatus.isOK(); + inputs.motorVelocity = motorVelocity.getValue(); + inputs.motorPosition = motorPosition.getValue(); + inputs.motorSupplyCurrent = motorSupplyCurrent.getValue(); + inputs.motorStatorCurrent = motorStatorCurrent.getValue(); + inputs.motorTemperature = motorTemperature.getValue(); + } + + @Override + public void setAngle(Angle angle) { + angle = + Rotations.of( + MathUtil.clamp( + angle.in(Rotations), + HoodConstants.ZERO_POSITION.in(Rotations), + HoodConstants.MAX_ANGLE.in(Rotations))); + motor.setControl(positionRequest.withPosition(angle)); + } +} 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 bc8da2c3..4e4e74ac 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -22,27 +22,27 @@ public class VisionConstants { // Robot to camera transforms public static final Transform3d RIGHT_BACK_CAMERA_POSITION = new Transform3d( - Inches.of(-9.607), - Inches.of(-13.028588), - Inches.of(8.125806), + Inches.of(-11.738), + Inches.of(-10.872), + Inches.of(9.541), new Rotation3d(Degrees.of(0), Degrees.of(-23), Degrees.of(-165))); public static final Transform3d RIGHT_SIDE_CAMERA_POSITION = new Transform3d( - Inches.of(-7.002289), - Inches.of(-13.805643), - Inches.of(10.677571), + Inches.of(-10.25), + Inches.of(-14.051), + Inches.of(7.94), new Rotation3d(Degrees.of(0), Degrees.of(-22), Degrees.of(-90))); public static final Transform3d LEFT_BACK_CAMERA_POSITION = new Transform3d( - Inches.of(-9.487822), - Inches.of(12.996737), - Inches.of(8.415729), + Inches.of(-11.738), + Inches.of(10.872), + Inches.of(9.541), new Rotation3d(Degrees.of(0), Degrees.of(-23), Degrees.of(165))); public static final Transform3d LEFT_SIDE_CAMERA_POSITION = new Transform3d( - Inches.of(-7.002289), - Inches.of(13.805643), - Inches.of(10.677571), + Inches.of(-10.25), + Inches.of(14.051), + Inches.of(7.648), new Rotation3d(Degrees.of(0), Degrees.of(-22), Degrees.of(90))); // Basic filtering thresholds 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; + } } diff --git a/src/test/java/ShooterCommandsTest.java b/src/test/java/ShooterCommandsTest.java index acf125be..3f236415 100644 --- a/src/test/java/ShooterCommandsTest.java +++ b/src/test/java/ShooterCommandsTest.java @@ -1,4 +1,5 @@ import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Seconds; import static org.junit.jupiter.api.Assertions.*; @@ -12,9 +13,11 @@ public class ShooterCommandsTest { private static final ShooterSetpoint[] testSetpoints = new ShooterSetpoint[] { - new ShooterSetpoint(Meters.of(0), RotationsPerSecond.of(0), Seconds.of(1)), - new ShooterSetpoint(Meters.of(5), RotationsPerSecond.of(0.5), Seconds.of(0.5)), - new ShooterSetpoint(Meters.of(10), RotationsPerSecond.of(1), Seconds.of(1)), + new ShooterSetpoint(Meters.of(0), Rotations.of(0), RotationsPerSecond.of(0), Seconds.of(1)), + new ShooterSetpoint( + Meters.of(5), Rotations.of(0), RotationsPerSecond.of(0.5), Seconds.of(0.5)), + new ShooterSetpoint( + Meters.of(10), Rotations.of(0), RotationsPerSecond.of(1), Seconds.of(1)), }; @Test