From 6f128261ad8d0a95a7947cb43e242dda05f71adc Mon Sep 17 00:00:00 2001 From: Programming Date: Fri, 1 Mar 2024 18:02:54 -0800 Subject: [PATCH] feat:New Auton Paths --- .../deploy/pathplanner/autos/3NoteBot.auto | 31 ++ .../deploy/pathplanner/autos/3NoteMid.auto | 37 +++ .../autos/{3Note_Up.auto => 3NoteUp.auto} | 2 +- ...Bottom_Disrupt.auto => BottomDisrupt.auto} | 0 src/main/deploy/pathplanner/autos/Sample.auto | 19 ++ .../deploy/pathplanner/autos/TopDisrupt.auto | 19 ++ src/main/deploy/pathplanner/paths/2Mid_1.path | 99 ++++++ src/main/deploy/pathplanner/paths/2mid_2.path | 76 +++++ src/main/deploy/pathplanner/paths/2mid_3.path | 76 +++++ src/main/deploy/pathplanner/paths/2mid_4.path | 76 +++++ .../deploy/pathplanner/paths/3NoteBot_2.path | 92 ++++++ .../deploy/pathplanner/paths/3NoteBot_3.path | 92 ++++++ .../paths/{Shoot_Top1.path => 3Note_Up.path} | 42 +-- .../pathplanner/paths/3NotesBottom_1.path | 149 +++++++++ .../deploy/pathplanner/paths/Bot_Dis_1.path | 46 +-- src/main/deploy/pathplanner/paths/Sample.path | 84 +++++ .../deploy/pathplanner/paths/TopDisrupt1.path | 129 ++++++++ src/main/java/frc/robot/RobotContainer.java | 292 +++++++++--------- 18 files changed, 1170 insertions(+), 191 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/3NoteBot.auto create mode 100644 src/main/deploy/pathplanner/autos/3NoteMid.auto rename src/main/deploy/pathplanner/autos/{3Note_Up.auto => 3NoteUp.auto} (87%) rename src/main/deploy/pathplanner/autos/{Bottom_Disrupt.auto => BottomDisrupt.auto} (100%) create mode 100644 src/main/deploy/pathplanner/autos/Sample.auto create mode 100644 src/main/deploy/pathplanner/autos/TopDisrupt.auto create mode 100644 src/main/deploy/pathplanner/paths/2Mid_1.path create mode 100644 src/main/deploy/pathplanner/paths/2mid_2.path create mode 100644 src/main/deploy/pathplanner/paths/2mid_3.path create mode 100644 src/main/deploy/pathplanner/paths/2mid_4.path create mode 100644 src/main/deploy/pathplanner/paths/3NoteBot_2.path create mode 100644 src/main/deploy/pathplanner/paths/3NoteBot_3.path rename src/main/deploy/pathplanner/paths/{Shoot_Top1.path => 3Note_Up.path} (87%) create mode 100644 src/main/deploy/pathplanner/paths/3NotesBottom_1.path create mode 100644 src/main/deploy/pathplanner/paths/Sample.path create mode 100644 src/main/deploy/pathplanner/paths/TopDisrupt1.path diff --git a/src/main/deploy/pathplanner/autos/3NoteBot.auto b/src/main/deploy/pathplanner/autos/3NoteBot.auto new file mode 100644 index 0000000..2ff3c86 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3NoteBot.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "3NotesBottom_1" + } + }, + { + "type": "path", + "data": { + "pathName": "3NoteBot_2" + } + }, + { + "type": "path", + "data": { + "pathName": "3NoteBot_3" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3NoteMid.auto b/src/main/deploy/pathplanner/autos/3NoteMid.auto new file mode 100644 index 0000000..44ad690 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3NoteMid.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "2Mid_1" + } + }, + { + "type": "path", + "data": { + "pathName": "2mid_2" + } + }, + { + "type": "path", + "data": { + "pathName": "2mid_3" + } + }, + { + "type": "path", + "data": { + "pathName": "2mid_4" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3Note_Up.auto b/src/main/deploy/pathplanner/autos/3NoteUp.auto similarity index 87% rename from src/main/deploy/pathplanner/autos/3Note_Up.auto rename to src/main/deploy/pathplanner/autos/3NoteUp.auto index e3a5def..243628f 100644 --- a/src/main/deploy/pathplanner/autos/3Note_Up.auto +++ b/src/main/deploy/pathplanner/autos/3NoteUp.auto @@ -8,7 +8,7 @@ { "type": "path", "data": { - "pathName": "Shoot_Top1" + "pathName": "3Note_Up" } } ] diff --git a/src/main/deploy/pathplanner/autos/Bottom_Disrupt.auto b/src/main/deploy/pathplanner/autos/BottomDisrupt.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/Bottom_Disrupt.auto rename to src/main/deploy/pathplanner/autos/BottomDisrupt.auto diff --git a/src/main/deploy/pathplanner/autos/Sample.auto b/src/main/deploy/pathplanner/autos/Sample.auto new file mode 100644 index 0000000..9707e22 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Sample.auto @@ -0,0 +1,19 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Sample" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TopDisrupt.auto b/src/main/deploy/pathplanner/autos/TopDisrupt.auto new file mode 100644 index 0000000..dd59797 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/TopDisrupt.auto @@ -0,0 +1,19 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TopDisrupt1" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/2Mid_1.path b/src/main/deploy/pathplanner/paths/2Mid_1.path new file mode 100644 index 0000000..e316b55 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/2Mid_1.path @@ -0,0 +1,99 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.339330858774288, + "y": 5.537039516855769 + }, + "prevControl": null, + "nextControl": { + "x": 2.0016193128071067, + "y": 5.5524415739263 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.3173614827529843, + "y": 5.537039516855769 + }, + "prevControl": { + "x": 1.3173614827529843, + "y": 5.537039516855769 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Mid1_end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "shoot", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + } + ] + } + } + }, + { + "name": "intake", + "waypointRelativePos": 1.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.9093804491991806, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0.7345210342548477, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/2mid_2.path b/src/main/deploy/pathplanner/paths/2mid_2.path new file mode 100644 index 0000000..efde419 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/2mid_2.path @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.3173614827529843, + "y": 5.537039516855769 + }, + "prevControl": null, + "nextControl": { + "x": 1.5087534865506418, + "y": 5.506235402714682 + }, + "isLocked": false, + "linkedName": "Mid1_end" + }, + { + "anchor": { + "x": 0.7617537186293887, + "y": 6.707595854216099 + }, + "prevControl": { + "x": 1.477949372409065, + "y": 6.707595854216073 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "4mid_2end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "shoot", + "waypointRelativePos": 1.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -0.8814039965842145, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/2mid_3.path b/src/main/deploy/pathplanner/paths/2mid_3.path new file mode 100644 index 0000000..3a7c3e9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/2mid_3.path @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7617537186293887, + "y": 6.707595854216099 + }, + "prevControl": null, + "nextControl": { + "x": 4.974216327419523, + "y": 7.454595622136836 + }, + "isLocked": false, + "linkedName": "4mid_2end" + }, + { + "anchor": { + "x": 7.654174257691856, + "y": 5.7834724299842595 + }, + "prevControl": { + "x": 5.628803752917074, + "y": 6.653688654469242 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "4mid_3end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "intake", + "waypointRelativePos": 1.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 59.62087398863165, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/2mid_4.path b/src/main/deploy/pathplanner/paths/2mid_4.path new file mode 100644 index 0000000..137180d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/2mid_4.path @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.654174257691856, + "y": 5.7834724299842595 + }, + "prevControl": null, + "nextControl": { + "x": 5.2899584973654, + "y": 6.800008196639283 + }, + "isLocked": false, + "linkedName": "4mid_3end" + }, + { + "anchor": { + "x": 1.3701349729153494, + "y": 5.537039516855769 + }, + "prevControl": { + "x": 5.890638723116095, + "y": 6.72299791128663 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "shoot", + "waypointRelativePos": 1.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3NoteBot_2.path b/src/main/deploy/pathplanner/paths/3NoteBot_2.path new file mode 100644 index 0000000..6ec8654 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3NoteBot_2.path @@ -0,0 +1,92 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7386506330235927, + "y": 4.297173922678052 + }, + "prevControl": null, + "nextControl": { + "x": 1.7386506330235927, + "y": 4.297173922678052 + }, + "isLocked": false, + "linkedName": "3bot_end" + }, + { + "anchor": { + "x": 2.81792833754523, + "y": 4.982565462316666 + }, + "prevControl": { + "x": 2.3434239790419866, + "y": 5.068838982044528 + }, + "nextControl": { + "x": 3.3261962208727422, + "y": 4.8901531198934824 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.5540608867334065, + "y": 4.112349237831684 + }, + "prevControl": { + "x": 7.084298146082222, + "y": 4.027637923943765 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "3NoteBot2_end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "intake", + "waypointRelativePos": 2.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -59.470294100065914, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3NoteBot_3.path b/src/main/deploy/pathplanner/paths/3NoteBot_3.path new file mode 100644 index 0000000..657c18f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3NoteBot_3.path @@ -0,0 +1,92 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.5540608867334065, + "y": 4.112349237831684 + }, + "prevControl": null, + "nextControl": { + "x": 7.5540608867334065, + "y": 4.112349237831684 + }, + "isLocked": false, + "linkedName": "3NoteBot2_end" + }, + { + "anchor": { + "x": 3.734350733241804, + "y": 3.2190299277409062 + }, + "prevControl": { + "x": 4.123319695522049, + "y": 3.5302050975651023 + }, + "nextControl": { + "x": 3.041258165067925, + "y": 2.664555873201803 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.7540526900941233, + "y": 4.220163637325399 + }, + "prevControl": { + "x": 0.4588182018765541, + "y": 4.227639973103586 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "shoot", + "waypointRelativePos": 2.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -56.30993247402024, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot_Top1.path b/src/main/deploy/pathplanner/paths/3Note_Up.path similarity index 87% rename from src/main/deploy/pathplanner/paths/Shoot_Top1.path rename to src/main/deploy/pathplanner/paths/3Note_Up.path index fb8b55c..70e04c0 100644 --- a/src/main/deploy/pathplanner/paths/Shoot_Top1.path +++ b/src/main/deploy/pathplanner/paths/3Note_Up.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 1.1307886445560125, - "y": 7.095801425645404 + "x": 0.746351661558858, + "y": 6.6459876259339765 }, "prevControl": null, "nextControl": { - "x": 1.7785079541790287, - "y": 7.037579465229853 + "x": 1.3940709711818746, + "y": 6.587765665518425 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.106006481516509, - "y": 6.993912994918189 + "x": 2.240351197400331, + "y": 6.984832881485651 }, "prevControl": { - "x": 1.7785534081698255, - "y": 7.003008913622263 + "x": 1.9128981240536476, + "y": 6.993928800189725 }, "nextControl": { - "x": 2.6300041252564776, - "y": 6.979357504814301 + "x": 2.7643488411402997, + "y": 6.970277391381763 }, "isLocked": false, "linkedName": null @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 1.6256753080882047, - "y": 6.993912994918189 + "x": 0.9234753178699605, + "y": 6.6459876259339765 }, "prevControl": { - "x": 3.6197774523208617, - "y": 7.081245935541516 + "x": 2.917577462102619, + "y": 6.733320566557304 }, "nextControl": null, "isLocked": false, @@ -63,17 +63,17 @@ "rotationTargets": [ { "waypointRelativePos": 1, - "rotationDegrees": 180.0, + "rotationDegrees": 0.8303154862580476, "rotateFast": false }, { - "waypointRelativePos": 0.5, - "rotationDegrees": 0, - "rotateFast": false + "waypointRelativePos": 1.0, + "rotationDegrees": 0.0, + "rotateFast": true }, { "waypointRelativePos": 2, - "rotationDegrees": 180.0, + "rotationDegrees": -0.7493724547088232, "rotateFast": false } ], @@ -239,13 +239,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -134.99999999999997, + "rotation": 55.864059922087705, "rotateFast": false }, "reversed": false, "folder": null, "previewStartingState": { - "rotation": -101.30993247402026, + "rotation": 57.61932229343076, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/3NotesBottom_1.path b/src/main/deploy/pathplanner/paths/3NotesBottom_1.path new file mode 100644 index 0000000..939844f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3NotesBottom_1.path @@ -0,0 +1,149 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7386506330235927, + "y": 4.297173922678052 + }, + "prevControl": null, + "nextControl": { + "x": 1.7386506330235902, + "y": 4.297173922678052 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.3558666254239116, + "y": 4.112349237829486 + }, + "prevControl": { + "x": 1.698980848189649, + "y": 4.140301824094775 + }, + "nextControl": { + "x": 2.717814966581382, + "y": 4.096947180758955 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.7386506330235927, + "y": 4.297173922678052 + }, + "prevControl": { + "x": 1.6088668575031753, + "y": 3.9968338098005063 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "3bot_end" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0, + "rotationDegrees": -0.8425242607404516, + "rotateFast": false + }, + { + "waypointRelativePos": 2.0, + "rotationDegrees": 0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "shoot", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + } + }, + { + "name": "intake", + "waypointRelativePos": 1.15, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + } + }, + { + "name": "shoot", + "waypointRelativePos": 2.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -54.162347046354384, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -56.929322177238376, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bot_Dis_1.path b/src/main/deploy/pathplanner/paths/Bot_Dis_1.path index d9e8f8e..7c0c9a7 100644 --- a/src/main/deploy/pathplanner/paths/Bot_Dis_1.path +++ b/src/main/deploy/pathplanner/paths/Bot_Dis_1.path @@ -3,57 +3,57 @@ "waypoints": [ { "anchor": { - "x": 0.9779559984651882, - "y": 2.0814350848559857 + "x": 0.6924444618120008, + "y": 4.397287293636501 }, "prevControl": null, "nextControl": { - "x": 1.9779559984651898, - "y": 2.0814350848559857 + "x": 1.6396709716496356, + "y": 3.8505142676326636 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.55703752542257, - "y": 0.8442184260255036 + "x": 7.808194828397162, + "y": 0.7701028535265327 }, "prevControl": { - "x": 7.552006394673018, - "y": 0.7486269417840178 + "x": 7.80316369764761, + "y": 0.674511369285047 }, "nextControl": { - "x": 7.564315270474514, - "y": 0.9824955820124404 + "x": 7.815472573449106, + "y": 0.9083800095134696 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.55703752542257, - "y": 2.328878416622081 + "x": 7.808194828397162, + "y": 2.418122960073312 }, "prevControl": { - "x": 7.418760369435634, - "y": 1.0843840127396558 + "x": 7.8048591650352, + "y": 2.0211790199998867 }, "nextControl": { - "x": 7.696398823292165, - "y": 3.583130097448444 + "x": 7.8158958569324275, + "y": 3.3345453557698868 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.666203701201729, - "y": 7.328689267307612 + "x": 7.808194828397162, + "y": 7.308276079966795 }, "prevControl": { - "x": 7.640731593519926, - "y": 6.5172206940158555 + "x": 7.782722720715359, + "y": 6.496807506675038 }, "nextControl": null, "isLocked": false, @@ -63,12 +63,12 @@ "rotationTargets": [ { "waypointRelativePos": 0.5, - "rotationDegrees": -178.2336207977557, + "rotationDegrees": -37.27045562549002, "rotateFast": false }, { "waypointRelativePos": 0.8999999999999999, - "rotationDegrees": 180.0, + "rotationDegrees": -2.7570826639951256, "rotateFast": false } ], @@ -149,7 +149,7 @@ "reversed": false, "folder": null, "previewStartingState": { - "rotation": 91.78991060824606, + "rotation": -59.19855402386322, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Sample.path b/src/main/deploy/pathplanner/paths/Sample.path new file mode 100644 index 0000000..c64df04 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Sample.path @@ -0,0 +1,84 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.849866161270784, + "y": 6.861616424921405 + }, + "prevControl": { + "x": 2.849866161270784, + "y": 6.861616424921405 + }, + "nextControl": { + "x": 4.849866161270781, + "y": 6.861616424921405 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.267989121344096, + "y": 7.0 + }, + "prevControl": { + "x": 6.267989121344096, + "y": 7.292874022896264 + }, + "nextControl": { + "x": 6.267989121344096, + "y": 6.484834043212011 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.864134508756823, + "y": 5.714163173166872 + }, + "prevControl": { + "x": 3.4275647022213382, + "y": 6.19321003745197 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TopDisrupt1.path b/src/main/deploy/pathplanner/paths/TopDisrupt1.path new file mode 100644 index 0000000..2417420 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TopDisrupt1.path @@ -0,0 +1,129 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7617537186293887, + "y": 6.707595854216099 + }, + "prevControl": null, + "nextControl": { + "x": 1.7397843426080857, + "y": 6.9617297958798545 + }, + "isLocked": false, + "linkedName": "4mid_2end" + }, + { + "anchor": { + "x": 2.517588224669883, + "y": 6.930925681738793 + }, + "prevControl": { + "x": 1.5235838687344545, + "y": 6.8215852025858945 + }, + "nextControl": { + "x": 3.2876910781964153, + "y": 7.015636995626712 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.64647322915659, + "y": 7.44689459360157 + }, + "prevControl": { + "x": 7.292225916536219, + "y": 7.693327506728355 + }, + "nextControl": { + "x": 8.008366044344408, + "y": 7.195143069993092 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.64647322915659, + "y": 0.7161956537796751 + }, + "prevControl": { + "x": 7.785091742793199, + "y": 1.7250303918977279 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.9, + "rotationDegrees": 0, + "rotateFast": false + }, + { + "waypointRelativePos": 2.0, + "rotationDegrees": -90.81845546149127, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "shoot", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + } + ] + } + } + }, + { + "name": "intake", + "waypointRelativePos": 0.9, + "command": { + "type": "parallel", + "data": { + "commands": [] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -88.05851360886201, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 60.64224645720876, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 01122d3..5133c71 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -47,152 +47,152 @@ * (including subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { - // The robot's subsystems and commands are defined here - - private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort); - private final XboxController m_operatorController = new XboxController(IOConstants.kOperatorControllerPort); - private final DriveSubsystem m_robotDrive = new DriveSubsystem(); - private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem(); - private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem(); - private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem(); - private final VisionSubsystem m_visionSubsystem = new VisionSubsystem(); - private final SendableChooser autoChooser; - - /** - * The container for the robot. Contains subsystems, IO devices, and commands. - */ - public RobotContainer() { - // Registering Named Commands for autonpaths - NamedCommands.registerCommand("shoot", new SequentialCommandGroup( - new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem), - new WaitCommand(0.5), - new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem))); - - NamedCommands.registerCommand("intake", new SequentialCommandGroup( - new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Extended), - new NoteIntakeCommand(m_intakeSubsystem), - new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted), - new WaitCommand(0.5), - new NoteOuttakeCommand(m_intakeSubsystem), - new WaitCommand(0.5))); - - // All paths automatically - - autoChooser = AutoBuilder.buildAutoChooser(); - SmartDashboard.putData("Auto Chooser", autoChooser); - - AutoBuilder.configureHolonomic(m_robotDrive::getPose, m_robotDrive::resetOdometry, - m_robotDrive::getChassisSpeeds, - m_robotDrive::autonDrive, - new HolonomicPathFollowerConfig( - new PIDConstants(5, 0.0, 0.0), // Translation PID constants - new PIDConstants(5, 0.0, 0.0), // Rotation PID constants - DriveConstants.kMaxSpeedMetersPerSecond, // Max module speed, in m/s - Math.hypot(DriveConstants.kTrackWidth, DriveConstants.kWheelBase), // Drive base radius in - // meters. Distance - // from robot center to - // furthest module. - new ReplanningConfig(true, true)), - () -> false, m_robotDrive); - - m_visionSubsystem.addConsumer(m_robotDrive::addVisionMeasurement); - - // Configure the trigger bindings - configureBindings(); - - m_robotDrive.setDefaultCommand( - new RunCommand( - () -> m_robotDrive.drive( - MathUtil.applyDeadband( - -m_driverController.getLeftY(), - IOConstants.kControllerDeadband) - * DriveConstants.kMaxSpeedMetersPerSecond - * (1 - m_driverController - .getLeftTriggerAxis() - * IOConstants.kSlowModeScalar), - // * 0.8, - MathUtil.applyDeadband( - -m_driverController.getLeftX(), - IOConstants.kControllerDeadband) - * DriveConstants.kMaxSpeedMetersPerSecond - * (1 - m_driverController - .getLeftTriggerAxis() - * IOConstants.kSlowModeScalar), - // * 0.8, - MathUtil.applyDeadband( - -m_driverController.getRightX(), - IOConstants.kControllerDeadband) - * DriveConstants.kMaxAngularSpeedRadiansPerSecond - * (1 - m_driverController - .getLeftTriggerAxis() - * IOConstants.kSlowModeScalar) - / 2, - !m_driverController.getRightBumper()), - m_robotDrive)); + // The robot's subsystems and commands are defined here + + private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort); + private final XboxController m_operatorController = new XboxController(IOConstants.kOperatorControllerPort); + private final DriveSubsystem m_robotDrive = new DriveSubsystem(); + private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem(); + private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem(); + private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem(); + private final VisionSubsystem m_visionSubsystem = new VisionSubsystem(); + private final SendableChooser autoChooser; + + /** + * The container for the robot. Contains subsystems, IO devices, and commands. + */ + public RobotContainer() { + // Registering Named Commands for autonpaths + NamedCommands.registerCommand("shoot", new SequentialCommandGroup( + new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem), + new WaitCommand(0.5), + new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem))); + + NamedCommands.registerCommand("intake", new SequentialCommandGroup( + new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Extended), + new NoteIntakeCommand(m_intakeSubsystem), + new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted), + new WaitCommand(0.5), + new NoteOuttakeCommand(m_intakeSubsystem), + new WaitCommand(0.5))); + + // All paths automatically + + autoChooser = AutoBuilder.buildAutoChooser(); + SmartDashboard.putData("Auto Chooser", autoChooser); + + AutoBuilder.configureHolonomic(m_robotDrive::getPose, m_robotDrive::resetOdometry, + m_robotDrive::getChassisSpeeds, + m_robotDrive::autonDrive, + new HolonomicPathFollowerConfig( + new PIDConstants(5, 0.0, 0.0), // Translation PID constants + new PIDConstants(5, 0.0, 0.0), // Rotation PID constants + DriveConstants.kMaxSpeedMetersPerSecond, // Max module speed, in m/s + Math.hypot(DriveConstants.kTrackWidth, DriveConstants.kWheelBase), // Drive base radius in + // meters. Distance + // from robot center to + // furthest module. + new ReplanningConfig(true, true)), + () -> false, m_robotDrive); + + m_visionSubsystem.addConsumer(m_robotDrive::addVisionMeasurement); + + // Configure the trigger bindings + configureBindings(); + + m_robotDrive.setDefaultCommand( + new RunCommand( + () -> m_robotDrive.drive( + MathUtil.applyDeadband( + -m_driverController.getLeftY(), + IOConstants.kControllerDeadband) + * DriveConstants.kMaxSpeedMetersPerSecond + * (1 - m_driverController + .getLeftTriggerAxis() + * IOConstants.kSlowModeScalar), + // * 0.8, + MathUtil.applyDeadband( + -m_driverController.getLeftX(), + IOConstants.kControllerDeadband) + * DriveConstants.kMaxSpeedMetersPerSecond + * (1 - m_driverController + .getLeftTriggerAxis() + * IOConstants.kSlowModeScalar), + // * 0.8, + MathUtil.applyDeadband( + -m_driverController.getRightX(), + IOConstants.kControllerDeadband) + * DriveConstants.kMaxAngularSpeedRadiansPerSecond + * (1 - m_driverController + .getLeftTriggerAxis() + * IOConstants.kSlowModeScalar) + / 2, + !m_driverController.getRightBumper()), + m_robotDrive)); + } + + /** + * Use this method to define your button->command mappings. + */ + private void configureBindings() { + new JoystickButton(m_driverController, Button.kStart.value) + .onTrue(new InstantCommand(() -> m_robotDrive.zeroHeading(), m_robotDrive)); + + // new JoystickButton(m_driverController, Button.kA.value).whileTrue( + // AutoBuilder.pathfindToPose(new Pose2d(2.8, 5.5, new Rotation2d()), new + // PathConstraints( + // DriveConstants.kMaxSpeedMetersPerSecond - 1, 5, + // DriveConstants.kMaxAngularSpeedRadiansPerSecond - 1, 5))); + + new JoystickButton(m_operatorController, Button.kX.value) + .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem)) + .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem)); + new JoystickButton(m_operatorController, Button.kY.value) + .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(-0.75), m_shooterSubsystem)) + .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem)); + + new JoystickButton(m_operatorController, Button.kA.value) + .onTrue(new InstantCommand(() -> m_climberSubsystem.forward(), m_robotDrive)); + new JoystickButton(m_operatorController, Button.kB.value) + .onTrue(new InstantCommand(() -> m_climberSubsystem.reverse(), m_robotDrive)); + + new Trigger(() -> { + return m_driverController.getLeftTriggerAxis() > 0.5; + }).whileTrue( + new SequentialCommandGroup( + new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Extended), + new NoteIntakeCommand(m_intakeSubsystem), + new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted))) + .onFalse(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted)); + + new Trigger(() -> { + return m_driverController.getRightTriggerAxis() > 0.5; + }).whileTrue( + new SequentialCommandGroup( + new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Amp), + new NoteOuttakeCommand(m_intakeSubsystem))) + .onFalse(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted)); + } + + // adding auton Path options + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + Command follow = autoChooser.getSelected(); + PathPlannerPath path = PathPlannerPath.fromPathFile(follow.getName()); + + var alliance = DriverStation.getAlliance(); + PathPlannerPath autonPath = path; + if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) { + autonPath = autonPath.flipPath(); } + m_robotDrive.resetOdometry(autonPath.getPreviewStartingHolonomicPose()); - /** - * Use this method to define your button->command mappings. - */ - private void configureBindings() { - new JoystickButton(m_driverController, Button.kStart.value) - .onTrue(new InstantCommand(() -> m_robotDrive.zeroHeading(), m_robotDrive)); - - // new JoystickButton(m_driverController, Button.kA.value).whileTrue( - // AutoBuilder.pathfindToPose(new Pose2d(2.8, 5.5, new Rotation2d()), new - // PathConstraints( - // DriveConstants.kMaxSpeedMetersPerSecond - 1, 5, - // DriveConstants.kMaxAngularSpeedRadiansPerSecond - 1, 5))); - - new JoystickButton(m_operatorController, Button.kX.value) - .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem)) - .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem)); - new JoystickButton(m_operatorController, Button.kY.value) - .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(-0.75), m_shooterSubsystem)) - .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem)); - - new JoystickButton(m_operatorController, Button.kA.value) - .onTrue(new InstantCommand(() -> m_climberSubsystem.forward(), m_robotDrive)); - new JoystickButton(m_operatorController, Button.kB.value) - .onTrue(new InstantCommand(() -> m_climberSubsystem.reverse(), m_robotDrive)); - - new Trigger(() -> { - return m_driverController.getLeftTriggerAxis() > 0.5; - }).whileTrue( - new SequentialCommandGroup( - new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Extended), - new NoteIntakeCommand(m_intakeSubsystem), - new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted))) - .onFalse(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted)); - - new Trigger(() -> { - return m_driverController.getRightTriggerAxis() > 0.5; - }).whileTrue( - new SequentialCommandGroup( - new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Amp), - new NoteOuttakeCommand(m_intakeSubsystem))) - .onFalse(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted)); - } - - // adding auton Path options - - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - Command follow = autoChooser.getSelected(); - PathPlannerPath path = PathPlannerPath.fromPathFile(follow.getName()); - - var alliance = DriverStation.getAlliance(); - PathPlannerPath autonPath = path; - if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) { - autonPath = autonPath.flipPath(); - } - m_robotDrive.resetOdometry(autonPath.getPreviewStartingHolonomicPose()); - - // return AutoBuilder.followPath(autonPath); - return null; - } + // return AutoBuilder.followPath(autonPath); + return null; + } }