diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 4c3a845..6b4d937 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -4,9 +4,10 @@ "holonomicMode": true, "pathFolders": [ "Amp Side", + "Midline", "Four Note", - "Two Note", - "Source Side" + "Source Side", + "Two Note" ], "autoFolders": [], "defaultMaxVel": 3.0, diff --git a/src/main/deploy/pathplanner/autos/Ampside Score.auto b/src/main/deploy/pathplanner/autos/Ampside Score.auto index 1ffc82b..c08f8d3 100644 --- a/src/main/deploy/pathplanner/autos/Ampside Score.auto +++ b/src/main/deploy/pathplanner/autos/Ampside Score.auto @@ -56,7 +56,7 @@ { "type": "path", "data": { - "pathName": "Ampside Auth path 4" + "pathName": "Ampside Auto path 4" } }, { diff --git a/src/main/deploy/pathplanner/autos/Copy of Ampside Score.auto b/src/main/deploy/pathplanner/autos/Copy of Ampside Score.auto index 8430520..29b82c0 100644 --- a/src/main/deploy/pathplanner/autos/Copy of Ampside Score.auto +++ b/src/main/deploy/pathplanner/autos/Copy of Ampside Score.auto @@ -80,7 +80,7 @@ { "type": "path", "data": { - "pathName": "Ampside Auth path 4" + "pathName": "Ampside Auto path 4" } }, { diff --git a/src/main/deploy/pathplanner/autos/Midline Auto.auto b/src/main/deploy/pathplanner/autos/Midline Auto.auto new file mode 100644 index 0000000..62f19b8 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Midline Auto.auto @@ -0,0 +1,103 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.4055808813372015, + "y": 5.537433881885308 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AutoScore" + } + }, + { + "type": "path", + "data": { + "pathName": "Midline Path 1" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + }, + { + "type": "path", + "data": { + "pathName": "Midline Path 2" + } + }, + { + "type": "named", + "data": { + "name": "AutoScore" + } + }, + { + "type": "path", + "data": { + "pathName": "Midline Path 3" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + }, + { + "type": "named", + "data": { + "name": "AutoScore" + } + }, + { + "type": "path", + "data": { + "pathName": "Midline Path 4" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + }, + { + "type": "named", + "data": { + "name": "AutoScore" + } + }, + { + "type": "path", + "data": { + "pathName": "Midline Path 5" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + }, + { + "type": "named", + "data": { + "name": "AutoScore" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Prototype Ampside Smart Auto.auto b/src/main/deploy/pathplanner/autos/Prototype Ampside Smart Auto.auto new file mode 100644 index 0000000..d60f847 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Prototype Ampside Smart Auto.auto @@ -0,0 +1,290 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7987428243044774, + "y": 6.68 + }, + "rotation": 60.20283245949364 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AutoScore" + } + }, + { + "type": "path", + "data": { + "pathName": "Ampside Auto path 1" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + }, + { + "type": "named", + "data": { + "name": "AutoScore" + } + }, + { + "type": "path", + "data": { + "pathName": "Ampside Missed Note 1" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + }, + { + "type": "named", + "data": { + "name": "Check Note" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim" + } + }, + { + "type": "path", + "data": { + "pathName": "Ampside Auto path 4" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AutoScore" + } + }, + { + "type": "path", + "data": { + "pathName": "Ampside Auto path 5" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Got Note" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Ampside Missed Note 2" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Not Got Note" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Check Note" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim" + } + }, + { + "type": "path", + "data": { + "pathName": "Ampside Auto Path 6" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AutoScore" + } + }, + { + "type": "path", + "data": { + "pathName": "Ampside Auto Path 7" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Ampside Missed Note 3" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Not Got Note" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Check Note" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim" + } + }, + { + "type": "path", + "data": { + "pathName": "Ampside Auto Path 8" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AutoScore" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Got Note" + } + } + ] + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Prototype Sourceside Smart Auto.auto b/src/main/deploy/pathplanner/autos/Prototype Sourceside Smart Auto.auto new file mode 100644 index 0000000..2a4c4b5 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Prototype Sourceside Smart Auto.auto @@ -0,0 +1,278 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7036808379666589, + "y": 4.401663085651201 + }, + "rotation": -59.47029410006589 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AutoScore" + } + }, + { + "type": "path", + "data": { + "pathName": "Sourceside Auto Path 1" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + }, + { + "type": "named", + "data": { + "name": "Check Note" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim" + } + }, + { + "type": "path", + "data": { + "pathName": "Sourceside Auto Path 2" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AutoScore" + } + }, + { + "type": "path", + "data": { + "pathName": "Sourceside Auto Path 3" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Got Note" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Sourceside missed note 1" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Not Got Note" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Check Note" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim" + } + }, + { + "type": "path", + "data": { + "pathName": "Sourceside Auto Path 4" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AutoScore" + } + }, + { + "type": "path", + "data": { + "pathName": "Sourceside Auto Path 5" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Got Note" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Sourceside missed Note 2" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Not Got Note" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Check Note" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim" + } + }, + { + "type": "path", + "data": { + "pathName": "Sourceside missed Note 2 Path 2" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AutoScore" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Got Note" + } + } + ] + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Stationary.auto b/src/main/deploy/pathplanner/autos/Stationary.auto new file mode 100644 index 0000000..ee6c837 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Stationary.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2, + "y": 2 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Check Note" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Ampside Auto (Inside Center) Note 2.path b/src/main/deploy/pathplanner/paths/Ampside Auto (Inside Center) Note 2.path index 7c1ca18..2a4a15b 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Auto (Inside Center) Note 2.path +++ b/src/main/deploy/pathplanner/paths/Ampside Auto (Inside Center) Note 2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.69, - "y": 6.86 + "x": 1.9306931140552677, + "y": 6.8946404084978985 }, "prevControl": null, "nextControl": { - "x": 2.69, - "y": 6.86 + "x": 2.9306931140552677, + "y": 6.8946404084978985 }, "isLocked": false, "linkedName": "Ampside Auto path 2 Endpoint" @@ -33,11 +33,18 @@ "eventMarkers": [ { "name": "Intake", - "waypointRelativePos": 0.35, + "waypointRelativePos": 0.65, "command": { "type": "parallel", "data": { - "commands": [] + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] } } } diff --git a/src/main/deploy/pathplanner/paths/Ampside Auto Path 6.path b/src/main/deploy/pathplanner/paths/Ampside Auto Path 6.path index 6185060..95cfe81 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Auto Path 6.path +++ b/src/main/deploy/pathplanner/paths/Ampside Auto Path 6.path @@ -8,24 +8,24 @@ }, "prevControl": null, "nextControl": { - "x": 4.33469113851808, - "y": 6.629350542651702 + "x": 4.881365492268351, + "y": 7.1867861885189965 }, "isLocked": false, "linkedName": "Ampside Auto Path 5 Endpoint" }, { "anchor": { - "x": 1.9863466887427603, - "y": 6.610786554906443 + "x": 2.583152022769052, + "y": 6.77778209648946 }, "prevControl": { - "x": 4.0469493284665585, - "y": 6.6943244997601115 + "x": 5.709111868994794, + "y": 7.128357032514777 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Ampside Auto Path 6 Endpoint" } ], "rotationTargets": [], @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 29.47588900324585, + "rotation": 22.83365417791754, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Ampside Auto Path 7.path b/src/main/deploy/pathplanner/paths/Ampside Auto Path 7.path new file mode 100644 index 0000000..3cac3cc --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Ampside Auto Path 7.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.583152022769052, + "y": 6.77778209648946 + }, + "prevControl": null, + "nextControl": { + "x": 3.5667094821734135, + "y": 6.923854986500009 + }, + "isLocked": false, + "linkedName": "Ampside Auto Path 6 Endpoint" + }, + { + "anchor": { + "x": 8.31703315828284, + "y": 4.128036518181299 + }, + "prevControl": { + "x": 7.40355739311716, + "y": 7.498408353874834 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Ampside Auto Path 7 Endpoint" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.6, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -39.80557109226524, + "rotateFast": false + }, + "reversed": false, + "folder": "Amp Side", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Ampside Auto Path 8.path b/src/main/deploy/pathplanner/paths/Ampside Auto Path 8.path new file mode 100644 index 0000000..535d48e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Ampside Auto Path 8.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.31703315828284, + "y": 4.128036518181299 + }, + "prevControl": null, + "nextControl": { + "x": 5.930370540348576, + "y": 2.8832685148577237 + }, + "isLocked": false, + "linkedName": "Ampside Auto Path 7 Endpoint" + }, + { + "anchor": { + "x": 2.47848475647076, + "y": 6.419299455137156 + }, + "prevControl": { + "x": 3.2191731551407905, + "y": 5.750622428560045 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Ampside Auto Path 8 Endpoint" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 22.89055165624832, + "rotateFast": false + }, + "reversed": false, + "folder": "Amp Side", + "previewStartingState": { + "rotation": -31.429565614838474, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Ampside Auto path 1.path b/src/main/deploy/pathplanner/paths/Ampside Auto path 1.path index 71489f7..943b52a 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Auto path 1.path +++ b/src/main/deploy/pathplanner/paths/Ampside Auto path 1.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.086934844340443, - "y": 6.785397528912033 + "x": 1.1822800587957067, + "y": 6.820505671086531 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.54, - "y": 7.03 + "x": 2.63367409925703, + "y": 6.876162812154685 }, "prevControl": { - "x": 1.1581313038590775, - "y": 6.7137636416806785 + "x": 1.7183097294806722, + "y": 6.635806466629964 }, "nextControl": null, "isLocked": false, @@ -51,13 +51,13 @@ ], "globalConstraints": { "maxVelocity": 5.75, - "maxAcceleration": 3.5, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 12.80426606528676, + "rotation": 28.395068251547436, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Ampside Auto path 2.path b/src/main/deploy/pathplanner/paths/Ampside Auto path 2.path index d1c0f72..8625268 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Auto path 2.path +++ b/src/main/deploy/pathplanner/paths/Ampside Auto path 2.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.54, - "y": 7.03 + "x": 2.63367409925703, + "y": 6.876162812154685 }, "prevControl": null, "nextControl": { - "x": 1.9581156423368469, - "y": 6.9328152351026535 + "x": 2.0517897415938773, + "y": 6.778978047257338 }, "isLocked": false, "linkedName": "Ampside Auto Path 1 Endpoint" }, { "anchor": { - "x": 1.69, - "y": 6.86 + "x": 1.9306931140552677, + "y": 6.8946404084978985 }, "prevControl": { - "x": 1.6548674903626555, - "y": 6.824867490362656 + "x": 1.8955606044179232, + "y": 6.859507898860554 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 29.744881296942317, + "rotation": 26.56505117707805, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Ampside Auto path 3.path b/src/main/deploy/pathplanner/paths/Ampside Auto path 3.path index 2fba58b..9ffcb74 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Auto path 3.path +++ b/src/main/deploy/pathplanner/paths/Ampside Auto path 3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.6887664017838722, - "y": 6.862550215827963 + "x": 1.9306931140552677, + "y": 6.8946404084978985 }, "prevControl": null, "nextControl": { - "x": 7.274835434121652, - "y": 7.576911245120637 + "x": 7.516762146393047, + "y": 7.609001437790572 }, "isLocked": false, "linkedName": "Ampside Auto path 2 Endpoint" diff --git a/src/main/deploy/pathplanner/paths/Ampside Auth path 4.path b/src/main/deploy/pathplanner/paths/Ampside Auto path 4.path similarity index 79% rename from src/main/deploy/pathplanner/paths/Ampside Auth path 4.path rename to src/main/deploy/pathplanner/paths/Ampside Auto path 4.path index 7a33bdb..e70babb 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Auth path 4.path +++ b/src/main/deploy/pathplanner/paths/Ampside Auto path 4.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 6.6555989865066145, - "y": 7.121296217901078 + "x": 6.361570777708579, + "y": 7.225738959188476 }, "isLocked": false, "linkedName": "Ampside Auto path 3 Endpoint" }, { "anchor": { - "x": 1.69, - "y": 6.86 + "x": 2.85582141745541, + "y": 7.001760527838968 }, "prevControl": { - "x": 4.438379168232951, - "y": 6.949241269692627 + "x": 5.611729942321095, + "y": 7.177047995851627 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 46.81201520995728, + "rotation": 24.1791071466892, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Ampside Auto path 5.path b/src/main/deploy/pathplanner/paths/Ampside Auto path 5.path index 7b5b094..3e7e0b1 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Auto path 5.path +++ b/src/main/deploy/pathplanner/paths/Ampside Auto path 5.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.69, - "y": 6.86 + "x": 2.85582141745541, + "y": 7.001760527838968 }, "prevControl": null, "nextControl": { - "x": 2.69, - "y": 6.86 + "x": 3.85582141745541, + "y": 7.001760527838968 }, "isLocked": false, "linkedName": "Ampside Auto Path 4 Endpoint" diff --git a/src/main/deploy/pathplanner/paths/Ampside Missed Note 1.path b/src/main/deploy/pathplanner/paths/Ampside Missed Note 1.path new file mode 100644 index 0000000..cee352c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Ampside Missed Note 1.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.63367409925703, + "y": 6.876162812154685 + }, + "prevControl": null, + "nextControl": { + "x": 3.63367409925703, + "y": 6.876162812154685 + }, + "isLocked": false, + "linkedName": "Ampside Auto Path 1 Endpoint" + }, + { + "anchor": { + "x": 8.149590620168896, + "y": 7.325500083098932 + }, + "prevControl": { + "x": 7.149590620168896, + "y": 7.325500083098932 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Ampside Auto path 3 Endpoint" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.45, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Amp Side", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Ampside Missed Note 2.path b/src/main/deploy/pathplanner/paths/Ampside Missed Note 2.path new file mode 100644 index 0000000..0377efe --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Ampside Missed Note 2.path @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.149590620168896, + "y": 7.325500083098932 + }, + "prevControl": null, + "nextControl": { + "x": 8.23517407718576, + "y": 6.4902557155541665 + }, + "isLocked": false, + "linkedName": "Ampside Auto path 3 Endpoint" + }, + { + "anchor": { + "x": 8.281950694955519, + "y": 5.81199475789265 + }, + "prevControl": { + "x": 8.23517407718576, + "y": 6.127736927838529 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.15, + "rotationDegrees": -90.0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.96093859731644, + "rotateFast": false + }, + "reversed": false, + "folder": "Amp Side", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Ampside Missed Note 3.path b/src/main/deploy/pathplanner/paths/Ampside Missed Note 3.path new file mode 100644 index 0000000..e322524 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Ampside Missed Note 3.path @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.211827384508862, + "y": 5.7844864444177295 + }, + "prevControl": null, + "nextControl": { + "x": 8.23517407718576, + "y": 5.063568873576495 + }, + "isLocked": false, + "linkedName": "Ampside Auto Path 5 Endpoint" + }, + { + "anchor": { + "x": 8.31703315828284, + "y": 4.128036518181299 + }, + "prevControl": { + "x": 8.281950694955519, + "y": 4.6542734680910955 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Ampside Auto Path 7 Endpoint" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2, + "rotationDegrees": -90.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -91.64239797493919, + "rotateFast": false + }, + "reversed": false, + "folder": "Amp Side", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Ampside Smart 1.path b/src/main/deploy/pathplanner/paths/Ampside Smart 1.path new file mode 100644 index 0000000..65a456b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Ampside Smart 1.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7223726295842452, + "y": 6.691938312786578 + }, + "prevControl": null, + "nextControl": { + "x": 1.9284777906097335, + "y": 7.470070674738507 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.726063461610459, + "y": 6.857291439701363 + }, + "prevControl": { + "x": 1.909024481560935, + "y": 6.3125987863350135 + }, + "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": 30.173520029644234, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 58.29857033049431, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Four Piece Auto Path 4.path b/src/main/deploy/pathplanner/paths/Four Piece Auto Path 4.path index dbe1b87..5c75498 100644 --- a/src/main/deploy/pathplanner/paths/Four Piece Auto Path 4.path +++ b/src/main/deploy/pathplanner/paths/Four Piece Auto Path 4.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.104707265167793, - "y": 5.68710451774403 + "x": 8.065754494498313, + "y": 5.823439215087209 }, "prevControl": { - "x": 5.682106864411436, - "y": 6.541047954025964 + "x": 5.621468134988465, + "y": 7.508146546542204 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Four Piece Auto Path 5.path b/src/main/deploy/pathplanner/paths/Four Piece Auto Path 5.path index 75ba5d7..7c79122 100644 --- a/src/main/deploy/pathplanner/paths/Four Piece Auto Path 5.path +++ b/src/main/deploy/pathplanner/paths/Four Piece Auto Path 5.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.104707265167793, - "y": 5.68710451774403 + "x": 8.065754494498313, + "y": 5.823439215087209 }, "prevControl": null, "nextControl": { - "x": 5.2365711585252095, - "y": 6.494637984662815 + "x": 5.485133437645286, + "y": 7.663957629220123 }, "isLocked": false, "linkedName": "Four Piece Auto Path 4 Endpoint" diff --git a/src/main/deploy/pathplanner/paths/Midline Path 1.path b/src/main/deploy/pathplanner/paths/Midline Path 1.path new file mode 100644 index 0000000..1942edf --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Midline Path 1.path @@ -0,0 +1,102 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.4055808813372015, + "y": 5.537433881885308 + }, + "prevControl": null, + "nextControl": { + "x": 1.5325365738450776, + "y": 4.748494935586363 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.8947741881248894, + "y": 4.898310911687067 + }, + "prevControl": { + "x": 1.9053384691709299, + "y": 4.753338645173666 + }, + "nextControl": { + "x": 5.553300786316876, + "y": 5.287838618381864 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.367638467186781, + "y": 4.099779112962734 + }, + "prevControl": { + "x": 3.8732829572795318, + "y": 3.7826179223564056 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Midline Path 1 Endpoint" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.45, + "rotationDegrees": -26.07474124855111, + "rotateFast": false + }, + { + "waypointRelativePos": 0.95, + "rotationDegrees": 0, + "rotateFast": false + }, + { + "waypointRelativePos": 1.8, + "rotationDegrees": 0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 1.55, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.57, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Midline", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Midline Path 2.path b/src/main/deploy/pathplanner/paths/Midline Path 2.path new file mode 100644 index 0000000..0d27c54 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Midline Path 2.path @@ -0,0 +1,92 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.367638467186781, + "y": 4.099779112962734 + }, + "prevControl": null, + "nextControl": { + "x": 7.016467168351424, + "y": 4.335553970481109 + }, + "isLocked": false, + "linkedName": "Midline Path 1 Endpoint" + }, + { + "anchor": { + "x": 4.579473194032577, + "y": 4.694085353082987 + }, + "prevControl": { + "x": 5.486823135797703, + "y": 4.14349539819615 + }, + "nextControl": { + "x": 3.761473335520811, + "y": 5.190456691708164 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.8045844863619553, + "y": 4.694085353082987 + }, + "prevControl": { + "x": 2.923988766126999, + "y": 4.8009289850133685 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Midline Path 2 Endpoint" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.85, + "rotationDegrees": 0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 1.35, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.57, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -14.892153959046919, + "rotateFast": false + }, + "reversed": false, + "folder": "Midline", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Midline Path 3.path b/src/main/deploy/pathplanner/paths/Midline Path 3.path new file mode 100644 index 0000000..a9160f8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Midline Path 3.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.8045844863619553, + "y": 4.694085353082987 + }, + "prevControl": null, + "nextControl": { + "x": 2.1219737176316458, + "y": 4.49458355057061 + }, + "isLocked": false, + "linkedName": "Midline Path 2 Endpoint" + }, + { + "anchor": { + "x": 2.6513193714406413, + "y": 4.2361138103059135 + }, + "prevControl": { + "x": 2.3339301401709505, + "y": 4.535366514074479 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Midline Path 3 Endpoint" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.57, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -31.639920128540666, + "rotateFast": false + }, + "reversed": false, + "folder": "Midline", + "previewStartingState": { + "rotation": -36.027373385103694, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Midline Path 4.path b/src/main/deploy/pathplanner/paths/Midline Path 4.path new file mode 100644 index 0000000..e08b17b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Midline Path 4.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.6513193714406413, + "y": 4.2361138103059135 + }, + "prevControl": null, + "nextControl": { + "x": 1.5119508293583617, + "y": 4.742499829009148 + }, + "isLocked": false, + "linkedName": "Midline Path 3 Endpoint" + }, + { + "anchor": { + "x": 2.85582141745541, + "y": 5.62867536173981 + }, + "prevControl": { + "x": 1.5022126366909916, + "y": 5.356005967053452 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Midline Path 4 Endpoint" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.57, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -2.3552351424074494, + "rotateFast": false + }, + "reversed": false, + "folder": "Midline", + "previewStartingState": { + "rotation": -31.653238844011977, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Midline Path 5.path b/src/main/deploy/pathplanner/paths/Midline Path 5.path new file mode 100644 index 0000000..6956419 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Midline Path 5.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.85582141745541, + "y": 5.62867536173981 + }, + "prevControl": null, + "nextControl": { + "x": 1.8820021507184188, + "y": 6.302868543802893 + }, + "isLocked": false, + "linkedName": "Midline Path 4 Endpoint" + }, + { + "anchor": { + "x": 2.7779158761164506, + "y": 6.962807757169489 + }, + "prevControl": { + "x": 1.7554056460426093, + "y": 6.553803665139952 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.57, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 43.15238973400542, + "rotateFast": false + }, + "reversed": false, + "folder": "Midline", + "previewStartingState": { + "rotation": -0.5809166420702049, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Position Locator (DO NOT USE).path b/src/main/deploy/pathplanner/paths/Position Locator (DO NOT USE).path new file mode 100644 index 0000000..f6b1c19 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Position Locator (DO NOT USE).path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.0, + "y": 5.375482352388193 + }, + "prevControl": null, + "nextControl": { + "x": 4.000000000000005, + "y": 5.375482352388193 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.655631889462832, + "y": 7.0 + }, + "prevControl": { + "x": 5.655631889462832, + "y": 7.0 + }, + "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.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 1.path b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 1.path index 5b32b63..6835069 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 1.path +++ b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.036539916496203, - "y": 0.7887936060569642 + "x": 8.279994733180452, + "y": 0.7693172207222235 }, "prevControl": { - "x": 7.036539916496203, - "y": 0.7887936060569642 + "x": 7.279994733180452, + "y": 0.7693172207222235 }, "nextControl": null, "isLocked": false, @@ -30,9 +30,9 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.65, + "waypointRelativePos": 0.55, "rotationDegrees": 0, - "rotateFast": false + "rotateFast": true } ], "constraintZones": [], @@ -57,7 +57,7 @@ ], "globalConstraints": { "maxVelocity": 5.75, - "maxAcceleration": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 2.path b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 2.path index 119da31..90791bd 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 2.path +++ b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.036539916496203, - "y": 0.7887936060569642 + "x": 8.279994733180452, + "y": 0.7693172207222235 }, "prevControl": null, "nextControl": { - "x": 8.075492687165683, - "y": 0.7887936060569642 + "x": 8.318947503849932, + "y": 0.7693172207222235 }, "isLocked": false, "linkedName": "Loadside Auto Path 1 Endpoint" @@ -21,25 +21,31 @@ }, "prevControl": { "x": 1.5509036000278411, - "y": 1.3730851660991583 + "y": 1.3730851660991585 }, "nextControl": null, "isLocked": false, - "linkedName": "Loadside Auto Path 2 Endpoint" + "linkedName": "Sourceside Auto Path 2 Endpoint" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2, + "rotationDegrees": -29.89890183861451, + "rotateFast": true } ], - "rotationTargets": [], "constraintZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 5.75, - "maxAcceleration": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": -30.52970589993407, + "rotation": -43.451842301022026, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 3.path b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 3.path index edce25d..7d0aec7 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 3.path +++ b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 3.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 4.9658667218569015, - "y": 0.7909763967676083 + "x": 2.534461059432202, + "y": 0.2142402386821383 }, "isLocked": false, - "linkedName": "Loadside Auto Path 2 Endpoint" + "linkedName": "Sourceside Auto Path 2 Endpoint" }, { "anchor": { - "x": 8.163136421167144, - "y": 2.395595396177869 + "x": 8.241041962510971, + "y": 2.4440782784662045 }, "prevControl": { - "x": 6.8566562791000525, - "y": 1.924590227639699 + "x": 7.24774631043924, + "y": 0.16534119430164596 }, "nextControl": null, "isLocked": false, @@ -30,16 +30,16 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.7, - "rotationDegrees": 23.19029156666067, - "rotateFast": false + "waypointRelativePos": 0.35, + "rotationDegrees": 30.707075654360423, + "rotateFast": true } ], "constraintZones": [], "eventMarkers": [ { "name": "Intake", - "waypointRelativePos": 0.7, + "waypointRelativePos": 0.5, "command": { "type": "parallel", "data": { @@ -57,13 +57,13 @@ ], "globalConstraints": { "maxVelocity": 5.75, - "maxAcceleration": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 26.075355583948827, + "rotation": 34.62415507994909, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 4.path b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 4.path index 96be0d7..e353772 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 4.path +++ b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 4.path @@ -3,43 +3,49 @@ "waypoints": [ { "anchor": { - "x": 8.163136421167144, - "y": 2.395595396177869 + "x": 8.241041962510971, + "y": 2.4440782784662045 }, "prevControl": null, "nextControl": { - "x": 4.721797688395219, - "y": 1.5161824972439915 + "x": 6.517381860386496, + "y": 0.0095301116237263 }, "isLocked": false, "linkedName": "Sourceside Auto path 3 Endpoint" }, { "anchor": { - "x": 2.3576264436479493, - "y": 3.909726337971194 + "x": 1.385354324682553, + "y": 4.52825959032701 }, "prevControl": { - "x": 2.4597283762468765, - "y": 2.4246073183504393 + "x": 1.1029467373288258, + "y": 1.4217761294360076 }, "nextControl": null, "isLocked": false, "linkedName": "Sourceside Auto Path 4 Endpoint" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.05, + "rotationDegrees": -39.370000000000005, + "rotateFast": true + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 5.75, - "maxAcceleration": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": -26.56505117707797, + "rotation": -45.0, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 5.path b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 5.path index 932eb17..fedf3ec 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 5.path +++ b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 5.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.3576264436479493, - "y": 3.909726337971194 + "x": 1.385354324682553, + "y": 4.52825959032701 }, "prevControl": null, "nextControl": { - "x": 4.540528748910404, - "y": 0.15581108267791854 + "x": 5.1637730796220795, + "y": 0.2629312020189867 }, "isLocked": false, "linkedName": "Sourceside Auto Path 4 Endpoint" }, { "anchor": { - "x": 8.153398228504642, - "y": 4.060826342293255 + "x": 8.279994733180452, + "y": 4.099779112962734 }, "prevControl": { - "x": 6.21346150912503, - "y": 1.0627423214338552 + "x": 5.825970181003233, + "y": 0.7206262573853753 }, "nextControl": null, "isLocked": false, @@ -51,13 +51,13 @@ ], "globalConstraints": { "maxVelocity": 5.75, - "maxAcceleration": 4.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 25.942295489871594, + "rotation": 42.089162173832335, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Sourceside missed Note 2 Path 2.path b/src/main/deploy/pathplanner/paths/Sourceside missed Note 2 Path 2.path new file mode 100644 index 0000000..80d9dbb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Sourceside missed Note 2 Path 2.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.241041962510971, + "y": 4.119255498297474 + }, + "prevControl": null, + "nextControl": { + "x": 3.2356109314828365, + "y": 4.5574741683291204 + }, + "isLocked": false, + "linkedName": "Sourceside Missed Note 2 Endpoint" + }, + { + "anchor": { + "x": 2.057289618731077, + "y": 3.5933930942594987 + }, + "prevControl": { + "x": 4.725554409590433, + "y": 1.6165399827834053 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.55, + "rotationDegrees": -0.03070135861799204, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -55.124671655397854, + "rotateFast": false + }, + "reversed": false, + "folder": "Source Side", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Sourceside missed Note 2.path b/src/main/deploy/pathplanner/paths/Sourceside missed Note 2.path new file mode 100644 index 0000000..91425bb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Sourceside missed Note 2.path @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.241041962510971, + "y": 2.4440782784662045 + }, + "prevControl": null, + "nextControl": { + "x": 8.23517407718576, + "y": 2.8884561472826653 + }, + "isLocked": false, + "linkedName": "Sourceside Auto path 3 Endpoint" + }, + { + "anchor": { + "x": 8.241041962510971, + "y": 4.119255498297474 + }, + "prevControl": { + "x": 8.25856238607064, + "y": 3.2392807805558634 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Sourceside Missed Note 2 Endpoint" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.0, + "rotationDegrees": 90.0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.0, + "maxAngularVelocity": 700.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.67246599163494, + "rotateFast": false + }, + "reversed": false, + "folder": "Source Side", + "previewStartingState": { + "rotation": 87.76461147672893, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Sourceside missed note 1.path b/src/main/deploy/pathplanner/paths/Sourceside missed note 1.path new file mode 100644 index 0000000..eec9ee7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Sourceside missed note 1.path @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.279994733180452, + "y": 0.7693172207222235 + }, + "prevControl": null, + "nextControl": { + "x": 8.293644849397962, + "y": 1.567016695286953 + }, + "isLocked": false, + "linkedName": "Loadside Auto Path 1 Endpoint" + }, + { + "anchor": { + "x": 8.241041962510971, + "y": 2.4440782784662045 + }, + "prevControl": { + "x": 8.25856238607064, + "y": 1.9351536710761752 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Sourceside Auto path 3 Endpoint" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.0, + "rotationDegrees": 90.0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.0, + "maxAngularVelocity": 700.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 89.47604771489111, + "rotateFast": false + }, + "reversed": false, + "folder": "Source Side", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/frc/lib/util/FieldRelativeAccel.java b/src/main/java/frc/lib/util/FieldRelativeAccel.java new file mode 100644 index 0000000..9929ed0 --- /dev/null +++ b/src/main/java/frc/lib/util/FieldRelativeAccel.java @@ -0,0 +1,41 @@ +package frc.lib.util; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class FieldRelativeAccel { + public double ax; + public double ay; + public double alpha; + + public FieldRelativeAccel(double ax, double ay, double alpha) { + this.ax = ax; + this.ay = ay; + this.alpha = alpha; + } + + public FieldRelativeAccel(FieldRelativeSpeed newSpeed, FieldRelativeSpeed oldSpeed, double time) { + this.ax = (newSpeed.vx - oldSpeed.vx) / time; + this.ay = (newSpeed.vy - oldSpeed.vy) / time; + this.alpha = (newSpeed.omega - oldSpeed.omega) / time; + + if (Math.abs(this.ax) > 6.0) { + this.ax = 6.0 * Math.signum(this.ax); + } + if (Math.abs(this.ay) > 6.0) { + this.ay = 6.0 * Math.signum(this.ay); + } + if (Math.abs(this.alpha) > 4 * Math.PI) { + this.alpha = 4 * Math.PI * Math.signum(this.alpha); + } + + SmartDashboard.putNumber("fieldRelativeAccelX", this.ax); + SmartDashboard.putNumber("fieldRelativeAccelY", this.ay); + } + + public FieldRelativeAccel() { + this.ax = 0.0; + this.ay = 0.0; + this.alpha = 0.0; + } + +} \ No newline at end of file diff --git a/src/main/java/frc/lib/util/FieldRelativeSpeed.java b/src/main/java/frc/lib/util/FieldRelativeSpeed.java new file mode 100644 index 0000000..273e995 --- /dev/null +++ b/src/main/java/frc/lib/util/FieldRelativeSpeed.java @@ -0,0 +1,29 @@ +package frc.lib.util; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +public class FieldRelativeSpeed { + public double vx; + public double vy; + public double omega; + + public FieldRelativeSpeed(double vx, double vy, double omega) { + this.vx = vx; + this.vy = vy; + this.omega = omega; + } + + public FieldRelativeSpeed(ChassisSpeeds chassisSpeed, Rotation2d gyro) { + this(chassisSpeed.vxMetersPerSecond * gyro.getCos() - chassisSpeed.vyMetersPerSecond * gyro.getSin(), + chassisSpeed.vyMetersPerSecond * gyro.getCos() + chassisSpeed.vxMetersPerSecond * gyro.getSin(), + chassisSpeed.omegaRadiansPerSecond); + } + + public FieldRelativeSpeed() { + this.vx = 0.0; + this.vy = 0.0; + this.omega = 0.0; + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 850e8fa..3494e81 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -19,7 +19,7 @@ public final class Constants { public static final double ELEVATOR_GEAR_RATIO = 20.0; public static final double ELEVATOR_ROTATIONS_TO_IN = (1.0 / ELEVATOR_GEAR_RATIO) * ELEVATOR_SPROCKET_DIAMETER * Math.PI * 2.0; public static final double ELEVATOR_TOLERANCE = 1.0; - public static final double ELEVATOR_HIGH_LEVEL = 16.0; + public static final double ELEVATOR_HIGH_LEVEL = 16.5; public static final double ELEVATOR_SAFE_LEVEL = 7.0; @@ -33,7 +33,7 @@ public final class Constants { /* slow mode */ public static final double SLOW_MODE_PERCENT_TRANSLATION = 0.5; public static final double SLOW_MODE_PERCENT_STRAFE = 0.5; - public static final double SLOW_MODE_PERCENT_ROTATION = 0.5; + public static final double SLOW_MODE_PERCENT_ROTATION = 1.0; public static final double AUTO_ROTATE_DEADBAND = 1.0; @@ -89,7 +89,7 @@ public static final class Swerve { /* These values are used by the drive falcon to ramp in open loop and closed loop driving. * We found a small open loop ramp (0.25) helps with tread wear, tipping, etc */ - public static final double openLoopRamp = 0.0; + public static final double openLoopRamp = 0.1; public static final double closedLoopRamp = 0.0; /* Angle Motor PID Values */ @@ -178,7 +178,8 @@ public static final class AutoConstants { //TODO: The below constants are used i } public static final class Positions { - + + //Speaker positions public static final double speakerBlueX = 0; public static final double speakerBlueY = 5.5; public static final double speakerBlueR = 0; @@ -187,6 +188,7 @@ public static final class Positions { public static final double speakerRedY = 5.5; public static final double speakerRedR = 180; + // amp positions (used for Feeding) public static final double ampBlueX = 1.9; public static final double ampBlueY = 8.0; public static final double ampBlueR = 270; @@ -195,7 +197,34 @@ public static final class Positions { public static final double ampRedY = 8.0; public static final double ampRedR = 270; + public static final double distanceLimit = 3.0; + + // blue trap positions + public static final double blueTrapLeftX = 4.3; + public static final double blueTrapLeftY = 5.0; + public static final double blueTrapLeftR = -60.0 + 180; + + public static final double blueTrapRightX = 4.25; + public static final double blueTrapRightY = 2.95; + public static final double blueTrapRightR = 60.0 + 180; + + public static final double blueTrapCenterX = 6.1; + public static final double blueTrapCenterY = 4.1; + public static final double blueTrapCenterR = 180.0 + 180; + // red trap positions + public static final double redTrapLeftX = 12.3; + public static final double redTrapLeftY = 3.0; + public static final double redTrapLeftR = 121.64 + 180; + + public static final double redTrapRightX = 12.3; + public static final double redTrapRightY = 5.0; + public static final double redTrapRightR = -121.64 + 180; + + public static final double redTrapCenterX = 10.4; + public static final double redTrapCenterY = 4.1; + public static final double redTrapCenterR = 0.0 + 180; + } } diff --git a/src/main/java/frc/robot/LimelightHelpers.java b/src/main/java/frc/robot/LimelightHelpers.java index 21c4d8b..3f0b9af 100644 --- a/src/main/java/frc/robot/LimelightHelpers.java +++ b/src/main/java/frc/robot/LimelightHelpers.java @@ -1,4 +1,4 @@ -//LimelightHelpers v1.2.1 (March 1, 2023) +//LimelightHelpers v1.5.0 (March 27, 2024) package frc.robot; @@ -303,6 +303,18 @@ public static class Results { @JsonProperty("botpose_wpiblue") public double[] botpose_wpiblue; + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + @JsonProperty("t6c_rs") public double[] camerapose_robotspace; @@ -362,9 +374,60 @@ public Results() { public static class LimelightResults { @JsonProperty("Results") public Results targetingResults; + + public String error; public LimelightResults() { targetingResults = new Results(); + error = ""; + } + + + } + + public static class RawFiducial { + public int id; + public double txnc; + public double tync; + public double ta; + public double distToCamera; + public double distToRobot; + public double ambiguity; + + + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + } + + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + public RawFiducial[] rawFiducials; + + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; } } @@ -385,7 +448,7 @@ static final String sanitizeName(String name) { private static Pose3d toPose3D(double[] inData){ if(inData.length < 6) { - System.err.println("Bad LL 3D Pose Data!"); + //System.err.println("Bad LL 3D Pose Data!"); return new Pose3d(); } return new Pose3d( @@ -397,7 +460,7 @@ private static Pose3d toPose3D(double[] inData){ private static Pose2d toPose2D(double[] inData){ if(inData.length < 6) { - System.err.println("Bad LL 2D Pose Data!"); + //System.err.println("Bad LL 2D Pose Data!"); return new Pose2d(); } Translation2d tran2d = new Translation2d(inData[0], inData[1]); @@ -405,6 +468,86 @@ private static Pose2d toPose2D(double[] inData){ return new Pose2d(tran2d, r2d); } + private static double extractBotPoseEntry(double[] inData, int position){ + if(inData.length < position+1) + { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { + var poseEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, entryName); + var poseArray = poseEntry.getDoubleArray(new double[0]); + var pose = toPose2D(poseArray); + double latency = extractBotPoseEntry(poseArray,6); + int tagCount = (int)extractBotPoseEntry(poseArray,7); + double tagSpan = extractBotPoseEntry(poseArray,8); + double tagDist = extractBotPoseEntry(poseArray,9); + double tagArea = extractBotPoseEntry(poseArray,10); + //getlastchange() in microseconds, ll latency in milliseconds + var timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency/1000.0); + + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial*tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } + else{ + for(int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int)poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate(pose, timestamp,latency,tagCount,tagSpan,tagDist,tagArea,rawFiducials); + } + + private static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } + } + public static NetworkTable getLimelightNTTable(String tableName) { return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); } @@ -544,8 +687,8 @@ public static double getFiducialID(String limelightName) { return getLimelightNTDouble(limelightName, "tid"); } - public static double getNeuralClassID(String limelightName) { - return getLimelightNTDouble(limelightName, "tclass"); + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); } ///// @@ -604,6 +747,28 @@ public static Pose2d getBotPose2d_wpiBlue(String limelightName) { return toPose2D(result); } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) @@ -618,6 +783,26 @@ public static Pose2d getBotPose2d_wpiRed(String limelightName) { } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) @@ -643,6 +828,11 @@ public static void setPipelineIndex(String limelightName, int pipelineIndex) { setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); } + + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + /** * The LEDs will be controlled by Limelight pipeline settings, and not by robot * code. @@ -696,6 +886,28 @@ public static void setCropWindow(String limelightName, double cropXMin, double c setLimelightNTDoubleArray(limelightName, "crop", entries); } + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + } + + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { double[] entries = new double[6]; entries[0] = forward; @@ -765,7 +977,7 @@ public static LimelightResults getLatestResults(String limelightName) { try { results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); } catch (JsonProcessingException e) { - System.err.println("lljson error: " + e.getMessage()); + results.error = "lljson error: " + e.getMessage(); } long end = System.nanoTime(); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4126cb3..d183b0c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,8 @@ package frc.robot; +import com.pathplanner.lib.commands.PathfindingCommand; + import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.cscore.UsbCamera; import edu.wpi.first.cscore.VideoMode; @@ -39,8 +41,12 @@ public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); + DataLogManager.start(); + // DO THIS AFTER CONFIGURATION OF YOUR DESIRED PATHFINDER + PathfindingCommand.warmupCommand().schedule(); + final UsbCamera usbCamera = CameraServer.startAutomaticCapture(); @@ -76,6 +82,7 @@ public void disabledPeriodic() {} @Override public void autonomousInit() { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + @@ -100,6 +107,7 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } + } /** 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 f9493a2..ee57102 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,6 +7,7 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.path.PathPlannerTrajectory; import edu.wpi.first.math.geometry.Pose2d; @@ -25,11 +26,14 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.shuffleboard.SimpleWidget; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.PrintCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -123,11 +127,12 @@ public class RobotContainer { private final Elevator s_Elevator = new Elevator(); private final Intake s_Intake = new Intake(); private final Shooter s_Shooter = new Shooter(); - private final Eyes s_Eyes = new Eyes(s_Swerve); - - + private final Eyes s_Eyes = new Eyes(s_Swerve, s_Shooter); + private final Blower s_Blower = new Blower(); + /* Commands */ private final SendableChooser autoChooser; + @@ -135,6 +140,8 @@ public class RobotContainer { public RobotContainer() { + //Default Commands + //Swerve Drive if (DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue) { s_Swerve.setDefaultCommand( new TeleopSwerve( @@ -142,7 +149,7 @@ public RobotContainer() { () -> -driver.getRawAxis(leftY), () -> -driver.getRawAxis(leftX), () -> driver.getRawAxis(rightX), - () -> driverDpadUp.getAsBoolean(), + () -> false, () -> s_Swerve.getGyroYaw().getDegrees(), () -> driverLeftTrigger.getAsBoolean(), rotationSpeed, @@ -158,7 +165,7 @@ public RobotContainer() { () -> driver.getRawAxis(leftY), () -> driver.getRawAxis(leftX), () -> driver.getRawAxis(rightX), - () -> driverDpadUp.getAsBoolean(), + () -> false, () -> s_Swerve.getGyroYaw().getDegrees(), () -> driverLeftTrigger.getAsBoolean(), rotationSpeed, @@ -173,44 +180,52 @@ public RobotContainer() { s_Shooter.setDefaultCommand( new ConditionalCommand( new InstantCommand (() -> s_Shooter.setShooterVoltage(0, 0), s_Shooter), - new InstantCommand(() -> s_Shooter.shootingMotorsSetControl(40, 40), s_Shooter), + new InstantCommand(() -> s_Shooter.shootingMotorsSetControl(35.0, 35.0), s_Shooter), //s_Shooter.shootingMotorsSetControl(20, 20) () -> s_Shooter.getBreakBeamOutput()) ); // Configure the button bindings configureButtonBindings(); - //Command ElevatorAtPosition = new s_Elevator.ElevatorAtPosition(); + + //Auto Commands Command AimThenShootAuto = new ParallelRaceGroup( - new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false), + + new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false, true, false), + new SequentialCommandGroup( - new PrintCommand("pre release:" + Timer.getFPGATimestamp()), - new WaitCommand(1.0).until(() -> prepareShot()), - new PrintCommand("post release:" + Timer.getFPGATimestamp()), + new WaitCommand(2.0).until(() -> prepareShot()), new InstantCommand(() -> s_Shooter.setLoaderVoltage(s_Shooter.runLoaderVoltage)), - new WaitCommand(1.0)).until(() -> s_Shooter.getBreakBeamOutput()) + new WaitCommand(1.5)).until(() -> s_Shooter.getBreakBeamOutput()) + ); - Command AimThenShootFar = new ParallelRaceGroup( - new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false), - new SequentialCommandGroup( - new WaitCommand(1.5), - new InstantCommand(() -> s_Shooter.setLoaderVoltage(s_Shooter.runLoaderVoltage)), - new WaitCommand(1.0)) - ); - NamedCommands.registerCommand("Intake", new RunIntake(s_Intake, s_ShooterPivot, s_Shooter, s_Eyes) - .until(() -> !s_Shooter.getBreakBeamOutput()) + + NamedCommands.registerCommand("Intake", new RunIntake(s_Intake, s_ShooterPivot, s_Shooter, s_Eyes, s_Elevator) + .until(() -> !s_Shooter.getBreakBeamOutput()) //TODO Make rollers spin after at position/0.25s ); - NamedCommands.registerCommand("Confirm Intake", new RunIntake(s_Intake, s_ShooterPivot, s_Shooter, s_Eyes) - .until(() -> !s_Shooter.getBreakBeamOutput()) - .withTimeout(2) + NamedCommands.registerCommand("Confirm Intake", new RunIntake(s_Intake, s_ShooterPivot, s_Shooter, s_Eyes, s_Elevator) + .until(() -> !s_Shooter.getBreakBeamOutput()) //TODO Make rollers spin after at position/0.25s + .withTimeout(1.25) ); NamedCommands.registerCommand("AutoScore", AimThenShootAuto); - NamedCommands.registerCommand("Score Far", AimThenShootFar); - NamedCommands.registerCommand("Aim", new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false)); + NamedCommands.registerCommand("Aim", new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false, false, false)); NamedCommands.registerCommand("Fire", new InstantCommand(() -> s_Shooter.setLoaderVoltage(s_Shooter.runLoaderVoltage))); + NamedCommands.registerCommand("Check Note", new InstantCommand(() -> s_Shooter.checkNote())); + NamedCommands.registerCommand("Got Note", new ConditionalCommand( + new WaitCommand (15), + new InstantCommand (), + () -> s_Shooter.gotNote + )); + NamedCommands.registerCommand("Not Got Note", new ConditionalCommand( + new InstantCommand (), + new WaitCommand (15), + () -> s_Shooter.gotNote + )); + + //TODO Shoot on the Move in Auto? autoChooser = AutoBuilder.buildAutoChooser(); @@ -234,7 +249,7 @@ private boolean prepareShot() { */ private void configureButtonBindings() { - + // zero gyro driverY.onTrue(new InstantCommand(() -> s_Swerve.zeroHeading())); @@ -244,13 +259,13 @@ private void configureButtonBindings() { s_Swerve, () -> -driver.getRawAxis(leftY), () -> -driver.getRawAxis(leftX), - () -> driver.getRawAxis(rightX), - () -> driverDpadUp.getAsBoolean(), - () -> s_Eyes.getTargetRotation(), - () -> driverLeftTrigger.getAsBoolean(), + () -> 0, //driver.getRawAxis(rightX), + () -> false, + () -> s_Eyes.getMovingTargetRotation(), + () -> true, rotationSpeed, true - ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false)) + ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false, true, false)) ); } else { @@ -258,13 +273,13 @@ private void configureButtonBindings() { s_Swerve, () -> driver.getRawAxis(leftY), () -> driver.getRawAxis(leftX), - () -> driver.getRawAxis(rightX), - () -> driverDpadUp.getAsBoolean(), - () -> s_Eyes.getTargetRotation(), - () -> driverLeftTrigger.getAsBoolean(), + () -> 0,//driver.getRawAxis(rightX), + () -> false, + () -> s_Eyes.getMovingTargetRotation(), + () -> true, rotationSpeed, true - ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false)) + ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false ,true, false)) ); } @@ -279,12 +294,12 @@ private void configureButtonBindings() { () -> -driver.getRawAxis(leftY), () -> -driver.getRawAxis(leftX), () -> driver.getRawAxis(rightX), - () -> driverDpadUp.getAsBoolean(), + () -> false, () -> s_Eyes.getTargetRotation(), - () -> driverB.getAsBoolean(), + () -> false,//driverB.getAsBoolean(), rotationSpeed, - true - ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, true)), + false //TODO Determine if we want auto rotation aiming + ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, true, false, false)), new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_HIGH_LEVEL)) ) ).onFalse( @@ -301,12 +316,12 @@ private void configureButtonBindings() { () -> driver.getRawAxis(leftY), () -> driver.getRawAxis(leftX), () -> driver.getRawAxis(rightX), - () -> driverDpadUp.getAsBoolean(), - () -> s_Eyes.getTargetRotation(), - () -> driverB.getAsBoolean(), + () -> false, + () -> s_Eyes.getTargetRotation(), + () -> false,//driverB.getAsBoolean(), rotationSpeed, - true - ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, true)), + false //TODO: Determine if we want auto rotation aiming + ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, true, false, false)), new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_HIGH_LEVEL)) ) ).onFalse( @@ -317,8 +332,6 @@ private void configureButtonBindings() { ); } - - // shoot speaker driverRightTrigger.onTrue( new ParallelCommandGroup( @@ -333,18 +346,30 @@ private void configureButtonBindings() { // intake - driverX.whileTrue( - new RunIntake(s_Intake, s_ShooterPivot, s_Shooter, s_Eyes) + driverX.whileTrue(new SequentialCommandGroup( + new ParallelCommandGroup( + new InstantCommand(() -> s_Intake.setIntakePivotPosition(s_Intake.intakeGroundPosition)), + new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotIntakePosition)), + new InstantCommand(() -> s_Shooter.setLoaderVoltage(s_Shooter.runLoaderVoltage)) + ), + s_Intake.IntakeAtPosition().withTimeout(0.25), //TODO figure out if this actually is working or just time delay + new InstantCommand(() -> s_Intake.setIntakeVoltage(s_Intake.runIntakeVoltage)).repeatedly()) .until(() -> !s_Shooter.getBreakBeamOutput()) .andThen(new ParallelCommandGroup( + new InstantCommand(() -> s_Intake.setIntakePivotPosition(s_Intake.intakeSafePosition)), + new InstantCommand(() -> s_Intake.setIntakeVoltage(s_Intake.stopIntakeVoltage)), + new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition)), + new InstantCommand(() -> s_Shooter.setLoaderVoltage(s_Shooter.stopLoaderVoltage)), new InstantCommand(() -> s_Eyes.limelight.setLEDMode_ForceBlink("")), new InstantCommand(() -> driver.setRumble(RumbleType.kBothRumble, 1)) ))) .onFalse(new ParallelCommandGroup( + new InstantCommand(() -> s_Intake.setIntakePivotPosition(s_Intake.intakeSafePosition)), + new InstantCommand(() -> s_Intake.setIntakeVoltage(s_Intake.stopIntakeVoltage)), + new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition)), + new InstantCommand(() -> s_Shooter.setLoaderVoltage(s_Shooter.stopLoaderVoltage)), new InstantCommand(() -> s_Eyes.limelight.setLEDMode_ForceOff("")), - new InstantCommand(() -> driver.setRumble(RumbleType.kBothRumble, 0)) - ) - ); + new InstantCommand(() -> driver.setRumble(RumbleType.kBothRumble, 0)))); // outake driverA.whileTrue(new SequentialCommandGroup( @@ -364,33 +389,38 @@ private void configureButtonBindings() { + //Climbing + driverRB.onTrue(new ConditionalCommand( + //climb pull up + new ParallelCommandGroup( + new InstantCommand(() -> s_Shooter.setShooterVoltage(0,0)), + new InstantCommand(() -> s_Elevator.SetElevatorPosition(2.0)), + new InstantCommand(() -> SmartDashboard.putString("ClimbCommand", "up")), + new InstantCommand(() -> s_Elevator.isClimbed(true))), - // climb reach - driverLB.onTrue( + //Reach for Climb new SequentialCommandGroup( + new InstantCommand(() -> s_Elevator.resetElevatorReverseSoftlimit()), new InstantCommand(() -> s_Shooter.setShooterVoltage(0,0)), - new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_SAFE_LEVEL)), - s_Elevator.ElevatorAtPosition(), - + new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_HIGH_LEVEL)), + s_Elevator.ElevatorAtPosition(Constants.ELEVATOR_SAFE_LEVEL), new ParallelCommandGroup( new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotClimbPosition)), - new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_HIGH_LEVEL)) - ) - ) - ); - - - // climb pull up - driverRB.onTrue( - new ParallelCommandGroup( - new InstantCommand(() -> s_Shooter.setShooterVoltage(0,0)), - new InstantCommand(() -> s_Elevator.SetElevatorPosition(0)), - new InstantCommand(() -> SmartDashboard.putString("ClimbCommand", "up")) - - ) - ); - - // escape climb + new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_HIGH_LEVEL)), + new InstantCommand(() -> s_Elevator.isClimbed(false)) + )), + () -> { + if(s_Elevator.isClimbed){ + return false; + } else{ + return true; + } + })); + //climb down more + driverDpadDown.onTrue( + new InstantCommand(() -> s_Elevator.SetElevatorPosition(0.0))); + + // abort climb driverSelect.onTrue( new SequentialCommandGroup( @@ -401,36 +431,113 @@ private void configureButtonBindings() { new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition)), new InstantCommand(() -> SmartDashboard.putString("ShooterPivot", "return")), s_ShooterPivot.ShooterPivotAtPosition(), - new InstantCommand(() -> s_Elevator.SetElevatorPosition(0)) + new InstantCommand(() -> s_Elevator.SetElevatorPosition(0)), + new InstantCommand(() -> s_Elevator.isClimbed(true)) ) - ); + + // generate and run path to closest trap + driverStart.whileTrue(new ParallelCommandGroup(new ConditionalCommand(new InstantCommand(() -> { + s_Swerve.onTheFly(() -> s_Eyes.trapPath).schedule(); - /* Operator Buttons */ - - // aim amp + }), + new InstantCommand(), + + () -> s_Eyes.closeToTrap), + + new Blow(s_Blower) + // .until(() -> s_Eyes.atTrap()) + // .andThen(new InstantCommand(() -> driver.setRumble(RumbleType.kBothRumble, 1)))) //TODO Test this, was only running on init earlier, may need to be run command + ) + ).onFalse(s_Swerve.getDefaultCommand()); //TODO let driver know we are in position to trap via rumble - operatorLeftTrigger.whileTrue( - new SequentialCommandGroup( - new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_SAFE_LEVEL)), - s_Elevator.ElevatorAtPosition(), - new ParallelCommandGroup( + //Feed + if (DriverStation.getAlliance().get() == Alliance.Blue) { + driverRStick.whileTrue(new TeleopSwerve( + s_Swerve, + () -> -driver.getRawAxis(leftY), + () -> -driver.getRawAxis(leftX), + () -> 0, + () -> false, + () -> s_Eyes.getFeedRotation(), + () -> false, + rotationSpeed, + true + ).alongWith(new Shooting(s_ShooterPivot,s_Shooter, 65.0, 117.0))//new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false, false, true)) + ).onFalse(new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition))); + } else { + driverRStick.whileTrue(new TeleopSwerve( + s_Swerve, + () -> driver.getRawAxis(leftY), + () -> driver.getRawAxis(leftX), + () -> 0, + () -> false, + () -> s_Eyes.getFeedRotation(), + () -> false, + rotationSpeed, + true + ).alongWith(new Shooting(s_ShooterPivot,s_Shooter, 65.0, 117.0))) + .onFalse(new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition)) + + + ); + } + + //reach amp + driverLB.whileTrue( + new ParallelCommandGroup( + new PrepareFeed(s_Shooter, 25), + new SequentialCommandGroup( new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_HIGH_LEVEL)), - new AmpShooterPivot(s_ShooterPivot) + s_Elevator.ElevatorAtPosition(Constants.ELEVATOR_SAFE_LEVEL), + new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotAmpPosition)) ) ) ).onFalse( new SequentialCommandGroup( - new AmpShooterPivotRetract(s_ShooterPivot), + + new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition)), s_ShooterPivot.ShooterPivotAtPosition(), - new AmpElevatorRetract(s_Elevator) + new InstantCommand(() -> s_Elevator.SetElevatorPosition(0.0)), + s_Elevator.ElevatorAtPosition(0.0), + new WaitCommand(0.5), + new InstantCommand(() -> s_Elevator.resetEncoder()) ) ); - + //source intake + operatorY.whileTrue( + new SequentialCommandGroup( + new InstantCommand(() -> s_Shooter.setShooterVoltage(0,0)), + new InstantCommand(() -> s_Elevator.SetElevatorPosition(8.85)), + s_Elevator.ElevatorAtPosition(), + + new ParallelCommandGroup( + new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(325)), + new RunLoader(s_Shooter).until(() -> !s_Shooter.getBreakBeamOutput()) + .andThen(new ParallelCommandGroup( + new InstantCommand(() -> driver.setRumble(RumbleType.kBothRumble, 1)) + )))) + + ).onFalse( + new ParallelCommandGroup( + new ParallelCommandGroup( + new InstantCommand(() -> driver.setRumble(RumbleType.kBothRumble, 0)) + ), + new SequentialCommandGroup( + new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition)), + s_ShooterPivot.ShooterPivotAtPosition(), + new InstantCommand(() -> s_Elevator.SetElevatorPosition(0)), + s_Elevator.ElevatorAtPosition(0.0), + new WaitCommand(0.5), + new InstantCommand(() -> s_Elevator.resetEncoder()) + ) + ) + ); + operatorRightTrigger.onTrue( new ParallelCommandGroup( new InstantCommand(() -> s_Shooter.setLoaderVoltage(6)), @@ -443,10 +550,18 @@ private void configureButtonBindings() { ) ); + // dummy shoot commands - operatorDpadDown.whileTrue((new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, 1.25))); - operatorDpadLeft.whileTrue((new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, 2.4))); - operatorDpadUp.whileTrue((new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, 3.17))); + operatorDpadDown.whileTrue(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, 1.25)) + .onFalse(new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition))); + operatorDpadLeft.whileTrue(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, 2.4)) + .onFalse(new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition))); + operatorDpadUp.whileTrue(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, 3.17)) + .onFalse(new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition))); + //prepare feed + operatorA.whileTrue(new PrepareFeed(s_Shooter, 65.0)); + // prepare trap + operatorX.whileTrue(new Shooting(s_ShooterPivot, s_Shooter, 45.0, s_ShooterPivot.shooterPivotStowPosition)); } @@ -459,6 +574,7 @@ public void rumbleControllers() { } } + /** * Use this to pass the autonomous command to the main {@link Robot} class. * diff --git a/src/main/java/frc/robot/commands/AimShoot.java b/src/main/java/frc/robot/commands/AimShoot.java index d78c022..4d34f49 100644 --- a/src/main/java/frc/robot/commands/AimShoot.java +++ b/src/main/java/frc/robot/commands/AimShoot.java @@ -19,7 +19,11 @@ import frc.robot.subsystems.Eyes; import frc.robot.subsystems.Shooter; import frc.robot.subsystems.ShooterPivot; + +import java.sql.Driver; + import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.math.interpolation.InterpolatingTreeMap; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.Timer; @@ -41,154 +45,91 @@ public class AimShoot extends Command { // required WPILib class objects private InterpolatingDoubleTreeMap shooterAngleInterpolation; - private InterpolatingDoubleTreeMap shooterLeftSpeedInterpolation; - private InterpolatingDoubleTreeMap shooterRightSpeedInterpolation; - private InterpolatingDoubleTreeMap shooterAngleInterpolationAuto; - private InterpolatingDoubleTreeMap shooterLeftSpeedInterpolationAuto; - private InterpolatingDoubleTreeMap shooterRightSpeedInterpolationAuto; - private InterpolatingDoubleTreeMap shooterAngleInterpolationElevator; - private InterpolatingDoubleTreeMap shooterLeftSpeedInterpolationElevator; - private InterpolatingDoubleTreeMap shooterRightSpeedInterpolationElevator; + private InterpolatingDoubleTreeMap shooterSpeedInterpolation; // local variables private double shooterAngle; - private double leftShooterSpeed; - private double rightShooterSpeed; + private double shooterSpeed; public boolean isElevatorShot = false; + public boolean onMove = false; + public boolean feed = false; // positions //Higher note shot is lower angle!!! - private final double subWooferDistance = 1.25; - private final double subWooferAngle = 115.0; - private final double subWooferLeftShooterSpeed = 90.0; - private final double subWooferRightShooterSpeed = subWooferLeftShooterSpeed; - - private final double d2Distance = 2.15; - private final double d2Angle = 128.0; - private final double d2LeftShooterSpeed = 90.0; - private final double d2RightShooterSpeed = d2LeftShooterSpeed; - - private final double podiumDistance = 3.17; - private final double podiumAngle = 137; - private final double podiumLeftShooterSpeed = 90.0; - private final double podiumRightShooterSpeed = podiumLeftShooterSpeed; - - private final double d3Distance = 4.0; - private final double d3Angle = 137.0; - private final double d3LeftShooterSpeed = 95.0; - private final double d3RightShooterSpeed = d3LeftShooterSpeed; - - - private final double subWooferDistanceAuto = 1.25; - private final double subWooferAngleAuto = 115.0; - private final double subWooferLeftShooterSpeedAuto = 90.0; - private final double subWooferRightShooterSpeedAuto = subWooferLeftShooterSpeed; - - private final double d2DistanceAuto = 2.15; - private final double d2AngleAuto = 128.0; - private final double d2LeftShooterSpeedAuto = 90.0; - private final double d2RightShooterSpeedAuto = d2LeftShooterSpeed; - - private final double xSpotDistance = 2.4; - private final double xSpotAngle = 131.0; - private final double xSpotLeftShooterSpeed = 92.5; - private final double xSpotRightShooterSpeed = xSpotLeftShooterSpeed; - - private final double podiumDistanceAuto = 3.17; - private final double podiumAngleAuto = 138; - private final double podiumLeftShooterSpeedAuto = 90.0; - private final double podiumRightShooterSpeedAuto = podiumLeftShooterSpeed; - - private final double d3DistanceAuto = 5.27; - private final double d3AngleAuto = 136.0; - private final double d3LeftShooterSpeedAuto = 95.0; - private final double d3RightShooterSpeedAuto = d3LeftShooterSpeed; - - private final double xSpotDistanceAuto = 2.4; - private final double xSpotAngleAuto = 130.0; - private final double xSpotLeftShooterSpeedAuto = 90.0; - private final double xSpotRightShooterSpeedAuto = xSpotLeftShooterSpeed; - + private final double subWooferDistance = 1.38; //(1.33 real field) 1.38 at practice field 1.21 at 930, 1.25 at comp, 1.31 at marquette //1.33 at Archimedes (blue) 1.26 (red) + private final double subWooferAngle = 115.5; //115 + private final double subWooferSpeed = 35.0; //50 + + private final double xSpotDistance = 2.57; //2.57 at practice field 2.45 at marquette //2.45 at Archimedes (blue) 2.37 + private final double xSpotAngle = 131.0; //132 + private final double xSpotSpeed = 45.0; + + private final double podiumDistance = 3.08; //3.17 at comp, 3.02 at marquette, 3.08 Archimedes (blue) 3.09 (red) + private final double podiumAngle = 135; //135 + private final double podiumSpeed = 60.0; + + private final double chainDistance = 4.32; //4.33 at marquette (NOT ACCCURATE) //4.32 at Archimedes (blue) 4.22 (red) + private final double chainAngle = 145.0; //145 + private final double chainSpeed = 80.0; + + private final double wingerDistance = 5.44; + private final double wingerAngle = 147; //147 + private final double wingerSpeed = 40.25; + + //private final double midlineAutoShot3distance = 2.22; + //private final double midlineAutoShot3Angle = 125; + //private final double midlineAutoShot3Speed = 42; + + + private final double feedDistance = 2.4; + private final double feedAngle = 117.0; + private final double feedSpeed = 65.0; private final double elevatorShotDistance = 2.65; - private final double elevatorShotAngle = 144; - private final double elevatorShotLeftShooterSpeed = 90; - private final double elevatorShotRightShooterSpeed = elevatorShotLeftShooterSpeed; + private final double elevatorShotAngle = 146; + private final double elevatorShotShooterSpeed = 30.0; // constructor - public AimShoot(Eyes eyes, ShooterPivot shooterPivot, Shooter shooter, boolean isElevatorShot) { + public AimShoot(Eyes eyes, ShooterPivot shooterPivot, Shooter shooter, boolean isElevatorShot, boolean onMove, boolean feed) { this.eyes = eyes; this.shooterPivot = shooterPivot; this.shooter = shooter; this.isElevatorShot = isElevatorShot; + this.onMove = onMove; + this.feed = feed; addRequirements(eyes, shooterPivot, shooter); // instantiate objects shooterAngleInterpolation = new InterpolatingDoubleTreeMap(); - shooterLeftSpeedInterpolation = new InterpolatingDoubleTreeMap(); - shooterRightSpeedInterpolation = new InterpolatingDoubleTreeMap(); - - shooterAngleInterpolationAuto = new InterpolatingDoubleTreeMap(); - shooterLeftSpeedInterpolationAuto = new InterpolatingDoubleTreeMap(); - shooterRightSpeedInterpolationAuto = new InterpolatingDoubleTreeMap(); - shooterAngleInterpolationElevator = new InterpolatingDoubleTreeMap(); - shooterLeftSpeedInterpolationElevator = new InterpolatingDoubleTreeMap(); - shooterRightSpeedInterpolationElevator = new InterpolatingDoubleTreeMap(); + shooterSpeedInterpolation = new InterpolatingDoubleTreeMap(); // create points in angle linear interpolation line // TODO tune these values shooterAngleInterpolation.put(subWooferDistance, subWooferAngle); - //shooterAngleInterpolation.put(d2Distance, d2Angle); shooterAngleInterpolation.put(podiumDistance, podiumAngle); - shooterAngleInterpolation.put(d3Distance, d3Angle); shooterAngleInterpolation.put(xSpotDistance, xSpotAngle); - - // create points in shooter power linear interpolation line - // TODO tune these values - shooterLeftSpeedInterpolation.put(subWooferDistance, subWooferLeftShooterSpeed); - //shooterLeftSpeedInterpolation.put(d2Distance, d2LeftShooterSpeed); - shooterLeftSpeedInterpolation.put(podiumDistance, podiumLeftShooterSpeed); - shooterLeftSpeedInterpolation.put(d3Distance, d3LeftShooterSpeed); - shooterLeftSpeedInterpolation.put(xSpotDistance, xSpotLeftShooterSpeed); - - shooterRightSpeedInterpolation.put(subWooferDistance, subWooferRightShooterSpeed); - //shooterRightSpeedInterpolation.put(d2Distance, d2RightShooterSpeed); - shooterRightSpeedInterpolation.put(podiumDistance, podiumRightShooterSpeed); - shooterRightSpeedInterpolation.put(d3Distance, d3RightShooterSpeed); - shooterRightSpeedInterpolation.put(xSpotDistance, xSpotRightShooterSpeed); - + shooterAngleInterpolation.put(chainDistance, chainAngle); + shooterAngleInterpolation.put(wingerDistance, wingerAngle); + //shooterAngleInterpolation.put(midlineAutoShot3distance, midlineAutoShot3Angle); - shooterAngleInterpolationAuto.put(subWooferDistanceAuto, subWooferAngleAuto); - shooterAngleInterpolationAuto.put(d2DistanceAuto, d2AngleAuto); - shooterAngleInterpolationAuto.put(podiumDistanceAuto, podiumAngleAuto); - shooterAngleInterpolationAuto.put(d3DistanceAuto, d3AngleAuto); - shooterAngleInterpolationAuto.put(xSpotDistanceAuto, xSpotAngleAuto); + shooterAngleInterpolationElevator.put(elevatorShotDistance, elevatorShotAngle); - // create points in shooter power linear interpolation line + // create points in shooter linear interpolation line // TODO tune these values - shooterLeftSpeedInterpolationAuto.put(subWooferDistanceAuto, subWooferLeftShooterSpeedAuto); - shooterLeftSpeedInterpolationAuto.put(d2DistanceAuto, d2LeftShooterSpeedAuto); - shooterLeftSpeedInterpolationAuto.put(podiumDistanceAuto, podiumLeftShooterSpeedAuto); - shooterLeftSpeedInterpolationAuto.put(d3DistanceAuto, d3LeftShooterSpeedAuto); - shooterLeftSpeedInterpolation.put(xSpotDistance, xSpotLeftShooterSpeed); - - shooterRightSpeedInterpolationAuto.put(subWooferDistanceAuto, subWooferRightShooterSpeedAuto); - shooterRightSpeedInterpolationAuto.put(d2DistanceAuto, d2RightShooterSpeedAuto); - shooterRightSpeedInterpolationAuto.put(podiumDistanceAuto, podiumRightShooterSpeedAuto); - shooterRightSpeedInterpolationAuto.put(d3DistanceAuto, d3RightShooterSpeedAuto); - shooterRightSpeedInterpolation.put(xSpotDistance, xSpotRightShooterSpeedAuto); - - shooterAngleInterpolationElevator.put(elevatorShotDistance, elevatorShotAngle); - shooterLeftSpeedInterpolationElevator.put(elevatorShotDistance, elevatorShotLeftShooterSpeed); - shooterRightSpeedInterpolationElevator.put(elevatorShotDistance, elevatorShotRightShooterSpeed); + shooterSpeedInterpolation.put(podiumDistance, podiumSpeed); + shooterSpeedInterpolation.put(chainDistance, chainSpeed); + shooterSpeedInterpolation.put(xSpotDistance, xSpotSpeed); + shooterSpeedInterpolation.put(wingerDistance, wingerSpeed); + shooterSpeedInterpolation.put(subWooferDistance, subWooferSpeed); + //shooterSpeedInterpolation.put(midlineAutoShot3distance, midlineAutoShot3Speed); } @@ -202,65 +143,26 @@ public AimShoot(Eyes eyes, ShooterPivot shooterPivot, Shooter shooter, double ma // instantiate objects shooterAngleInterpolation = new InterpolatingDoubleTreeMap(); - shooterLeftSpeedInterpolation = new InterpolatingDoubleTreeMap(); - shooterRightSpeedInterpolation = new InterpolatingDoubleTreeMap(); - - shooterAngleInterpolationAuto = new InterpolatingDoubleTreeMap(); - shooterLeftSpeedInterpolationAuto = new InterpolatingDoubleTreeMap(); - shooterRightSpeedInterpolationAuto = new InterpolatingDoubleTreeMap(); - shooterAngleInterpolationElevator = new InterpolatingDoubleTreeMap(); - shooterLeftSpeedInterpolationElevator = new InterpolatingDoubleTreeMap(); - shooterRightSpeedInterpolationElevator = new InterpolatingDoubleTreeMap(); + shooterSpeedInterpolation = new InterpolatingDoubleTreeMap(); // create points in angle linear interpolation line // TODO tune these values shooterAngleInterpolation.put(subWooferDistance, subWooferAngle); - shooterAngleInterpolation.put(d2Distance, d2Angle); shooterAngleInterpolation.put(podiumDistance, podiumAngle); - shooterAngleInterpolation.put(d3Distance, d3Angle); shooterAngleInterpolation.put(xSpotDistance, xSpotAngle); + shooterAngleInterpolation.put(chainDistance, chainAngle); + shooterAngleInterpolation.put(wingerDistance, wingerAngle); - // create points in shooter power linear interpolation line - // TODO tune these values - shooterLeftSpeedInterpolation.put(subWooferDistance, subWooferLeftShooterSpeed); - shooterLeftSpeedInterpolation.put(d2Distance, d2LeftShooterSpeed); - shooterLeftSpeedInterpolation.put(podiumDistance, podiumLeftShooterSpeed); - shooterLeftSpeedInterpolation.put(d3Distance, d3LeftShooterSpeed); - shooterLeftSpeedInterpolation.put(xSpotDistance, xSpotLeftShooterSpeed); - - shooterRightSpeedInterpolation.put(subWooferDistance, subWooferRightShooterSpeed); - shooterRightSpeedInterpolation.put(d2Distance, d2RightShooterSpeed); - shooterRightSpeedInterpolation.put(podiumDistance, podiumRightShooterSpeed); - shooterRightSpeedInterpolation.put(d3Distance, d3RightShooterSpeed); - shooterRightSpeedInterpolation.put(xSpotDistance, xSpotRightShooterSpeed); - - - - shooterAngleInterpolationAuto.put(subWooferDistanceAuto, subWooferAngleAuto); - shooterAngleInterpolationAuto.put(d2DistanceAuto, d2AngleAuto); - shooterAngleInterpolationAuto.put(podiumDistanceAuto, podiumAngleAuto); - shooterAngleInterpolationAuto.put(d3DistanceAuto, d3AngleAuto); - shooterAngleInterpolationAuto.put(xSpotDistanceAuto, xSpotAngleAuto); - - // create points in shooter power linear interpolation line + // create points in angle linear interpolation line // TODO tune these values - shooterLeftSpeedInterpolationAuto.put(subWooferDistanceAuto, subWooferLeftShooterSpeedAuto); - shooterLeftSpeedInterpolationAuto.put(d2DistanceAuto, d2LeftShooterSpeedAuto); - shooterLeftSpeedInterpolationAuto.put(podiumDistanceAuto, podiumLeftShooterSpeedAuto); - shooterLeftSpeedInterpolationAuto.put(d3DistanceAuto, d3LeftShooterSpeedAuto); - shooterLeftSpeedInterpolation.put(xSpotDistance, xSpotLeftShooterSpeed); - - shooterRightSpeedInterpolationAuto.put(subWooferDistanceAuto, subWooferRightShooterSpeedAuto); - shooterRightSpeedInterpolationAuto.put(d2DistanceAuto, d2RightShooterSpeedAuto); - shooterRightSpeedInterpolationAuto.put(podiumDistanceAuto, podiumRightShooterSpeedAuto); - shooterRightSpeedInterpolationAuto.put(d3DistanceAuto, d3RightShooterSpeedAuto); - shooterRightSpeedInterpolation.put(xSpotDistance, xSpotRightShooterSpeedAuto); + shooterSpeedInterpolation.put(podiumDistance, podiumSpeed); + shooterSpeedInterpolation.put(chainDistance, chainSpeed); + shooterSpeedInterpolation.put(xSpotDistance, xSpotSpeed); + shooterSpeedInterpolation.put(wingerDistance, wingerSpeed); + shooterSpeedInterpolation.put(subWooferDistance, subWooferSpeed); shooterAngleInterpolationElevator.put(elevatorShotDistance, elevatorShotAngle); - shooterLeftSpeedInterpolationElevator.put(elevatorShotDistance, elevatorShotLeftShooterSpeed); - shooterRightSpeedInterpolationElevator.put(elevatorShotDistance, elevatorShotRightShooterSpeed); - } @Override @@ -268,42 +170,50 @@ public void execute() { // assign target distance as variable - if (isElevatorShot == true) { + if (isElevatorShot) { distance = elevatorShotDistance; shooterAngle = elevatorShotAngle; + } else if(feed) { + distance = feedDistance; + shooterAngle = feedAngle; } else if(manualDistance == 0) { - distance = eyes.getDistanceFromTarget(); - shooterAngle = shooterAngleInterpolation.get(distance); + if (onMove == true) { + distance = eyes.getDistanceFromMovingTarget(); + shooterAngle = shooterAngleInterpolation.get(distance); + } else { + distance = eyes.getDistanceFromTarget(); + shooterAngle = shooterAngleInterpolation.get(distance); + } + } else { distance = manualDistance; shooterAngle = shooterAngleInterpolation.get(distance); } - // get desired shooter angle using the linear interpolation - // x (input) = distance - // y (output) = shooter angle - - + // move shooter to calculated angle + shooterPivot.moveShooterPivot(shooterAngle); + // get desired shooter power using the linear interpolation // x (input) = distance // y (output) = shooter power - leftShooterSpeed = shooterLeftSpeedInterpolation.get(distance); - rightShooterSpeed = shooterRightSpeedInterpolation.get(distance); + if(isElevatorShot) { + shooterSpeed = elevatorShotShooterSpeed; + } else if (feed){ + shooterSpeed = feedSpeed; + } else { + shooterSpeed = shooterSpeedInterpolation.get(distance); + } - - - // move shooter to calculated angle - shooterPivot.moveShooterPivot(shooterAngle); - // set shooter speed power to calculated value - shooter.shootingMotorsSetControl(rightShooterSpeed, leftShooterSpeed); + shooter.shootingMotorsSetControl(shooterSpeed, shooterSpeed); - if(shooterPivot.atPosition() == true && eyes.swerveAtPosition() == true && shooter.isUpToSpeed() == true) { + if(shooterPivot.atPosition() == true && eyes.swerveAtPosition(onMove) == true && shooter.isUpToSpeed() == true) { eyes.controllerRumble = true; } else { eyes.controllerRumble = false; } + } @@ -312,6 +222,9 @@ public void execute() { public void end(boolean interrupted) { shooter.setLoaderVoltage(shooter.stopLoaderVoltage); shooter.setShooterVoltage(shooter.stopShooterVoltage, shooter.stopShooterVoltage); + if (DriverStation.isAutonomous()) { + shooter.shootingMotorsSetControl(subWooferSpeed, subWooferSpeed); + } eyes.controllerRumble = false; } diff --git a/src/main/java/frc/robot/commands/AmpElevator.java b/src/main/java/frc/robot/commands/AmpElevator.java deleted file mode 100644 index 259ec8f..0000000 --- a/src/main/java/frc/robot/commands/AmpElevator.java +++ /dev/null @@ -1,43 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; -import frc.robot.subsystems.Elevator; - -public class AmpElevator extends Command { - - Elevator s_Elevator; - - /** Creates a new AmpScore. */ - public AmpElevator(Elevator s_Elevator) { - // Use addRequirements() here to declare subsystem dependencies. - this.s_Elevator = s_Elevator; - - addRequirements(s_Elevator); - - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - s_Elevator.SetElevatorPosition(Constants.ELEVATOR_HIGH_LEVEL); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return true; - } -} diff --git a/src/main/java/frc/robot/commands/AmpShooterPivotRetract.java b/src/main/java/frc/robot/commands/AmpShooterPivotRetract.java deleted file mode 100644 index 0b59373..0000000 --- a/src/main/java/frc/robot/commands/AmpShooterPivotRetract.java +++ /dev/null @@ -1,42 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.ShooterPivot; - -public class AmpShooterPivotRetract extends Command { - - ShooterPivot s_ShooterPivot; - - /** Creates a new AmpScore. */ - public AmpShooterPivotRetract(ShooterPivot s_ShooterPivot) { - // Use addRequirements() here to declare subsystem dependencies. - this.s_ShooterPivot = s_ShooterPivot; - - addRequirements(s_ShooterPivot); - - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return true; - } -} diff --git a/src/main/java/frc/robot/commands/Blow.java b/src/main/java/frc/robot/commands/Blow.java new file mode 100644 index 0000000..7295669 --- /dev/null +++ b/src/main/java/frc/robot/commands/Blow.java @@ -0,0 +1,50 @@ + +package frc.robot.commands; + +import frc.robot.subsystems.Blower; +import edu.wpi.first.wpilibj2.command.Command; + + +public class Blow extends Command { + + // required subystems + Blower s_Blower; + + // required WPILib class objects + + + // local variables + + + + + // constructor + public Blow(Blower s_Blower) { + + this.s_Blower = s_Blower; + + addRequirements(s_Blower); + + + } + + @Override + public void initialize() { + s_Blower.runBlower(); + } + + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + s_Blower.stopBlower(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/AmpElevatorRetract.java b/src/main/java/frc/robot/commands/ManualElevator.java similarity index 66% rename from src/main/java/frc/robot/commands/AmpElevatorRetract.java rename to src/main/java/frc/robot/commands/ManualElevator.java index a0e3ae2..b78cac8 100644 --- a/src/main/java/frc/robot/commands/AmpElevatorRetract.java +++ b/src/main/java/frc/robot/commands/ManualElevator.java @@ -5,35 +5,38 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; import frc.robot.subsystems.Elevator; -public class AmpElevatorRetract extends Command { - Elevator s_Elevator; +public class ManualElevator extends Command { + private Elevator s_Elevator; + private double speed; + public ManualElevator(Elevator s_Elevator, double speed) { - /** Creates a new AmpScore. */ - public AmpElevatorRetract(Elevator s_Elevator) { // Use addRequirements() here to declare subsystem dependencies. this.s_Elevator = s_Elevator; - + this.speed = speed; addRequirements(s_Elevator); - } // Called when the command is initially scheduled. @Override - public void initialize() { - s_Elevator.SetElevatorPosition(0); - } + public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + s_Elevator.move(speed); + } // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + s_Elevator.move(0.0); + s_Elevator.resetEncoder(); + } // Returns true when the command should end. @Override diff --git a/src/main/java/frc/robot/commands/AmpShooterPivot.java b/src/main/java/frc/robot/commands/PrepareFeed.java similarity index 58% rename from src/main/java/frc/robot/commands/AmpShooterPivot.java rename to src/main/java/frc/robot/commands/PrepareFeed.java index 6e39536..be1635b 100644 --- a/src/main/java/frc/robot/commands/AmpShooterPivot.java +++ b/src/main/java/frc/robot/commands/PrepareFeed.java @@ -5,31 +5,35 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; +import frc.robot.subsystems.Elevator; +import frc.robot.subsystems.Eyes; +import frc.robot.subsystems.Shooter; import frc.robot.subsystems.ShooterPivot; -public class AmpShooterPivot extends Command { - ShooterPivot s_ShooterPivot; - - /** Creates a new AmpScore. */ - public AmpShooterPivot(ShooterPivot s_ShooterPivot) { - // Use addRequirements() here to declare subsystem dependencies. - this.s_ShooterPivot = s_ShooterPivot; +public class PrepareFeed extends Command { + private Shooter shooter; + + private double shooterAngle; + private double shooterSpeed; - addRequirements(s_ShooterPivot); + public PrepareFeed(Shooter shooter, double shooterSpeed) { + // Use addRequirements() here to declare subsystem dependencies. + this.shooter = shooter; + this.shooterSpeed = shooterSpeed; + addRequirements(shooter); } // Called when the command is initially scheduled. @Override - public void initialize() { - s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotAmpPosition); - } + public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - System.out.println("AmpShooterPivot"); + shooter.shootingMotorsSetControl(shooterSpeed, shooterSpeed); } // Called once the command ends or is interrupted. @@ -39,6 +43,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return true; + return false; } } diff --git a/src/main/java/frc/robot/commands/PrepareShot.java b/src/main/java/frc/robot/commands/PrepareShot.java index 68ac102..49be0a9 100644 --- a/src/main/java/frc/robot/commands/PrepareShot.java +++ b/src/main/java/frc/robot/commands/PrepareShot.java @@ -20,15 +20,17 @@ public class PrepareShot extends Command { // local variables boolean shotReady = false; + boolean onMove = false; // constructor - public PrepareShot(ShooterPivot s_ShooterPivot, Shooter s_Shooter, Eyes s_Eyes) { + public PrepareShot(ShooterPivot s_ShooterPivot, Shooter s_Shooter, Eyes s_Eyes, boolean onMove) { this.s_ShooterPivot = s_ShooterPivot; this.s_Shooter = s_Shooter; this.s_Eyes = s_Eyes; + this.onMove = onMove; addRequirements(s_ShooterPivot, s_Shooter, s_Eyes); @@ -37,7 +39,7 @@ public PrepareShot(ShooterPivot s_ShooterPivot, Shooter s_Shooter, Eyes s_Eyes) @Override public void execute() { - if (s_ShooterPivot.atPosition() == true && s_Eyes.swerveAtPosition() == true && s_Shooter.isUpToSpeed() == true) { + if (s_ShooterPivot.atPosition() == true && s_Eyes.swerveAtPosition(onMove) == true && s_Shooter.isUpToSpeed() == true) { shotReady = true; } else { shotReady = false; diff --git a/src/main/java/frc/robot/commands/RunIntake.java b/src/main/java/frc/robot/commands/RunIntake.java index 8efa7eb..01b5c39 100644 --- a/src/main/java/frc/robot/commands/RunIntake.java +++ b/src/main/java/frc/robot/commands/RunIntake.java @@ -6,6 +6,7 @@ import frc.robot.subsystems.Shooter; import frc.robot.subsystems.Eyes; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Elevator; public class RunIntake extends Command { @@ -15,6 +16,7 @@ public class RunIntake extends Command { ShooterPivot s_ShooterPivot; Shooter s_Shooter; Eyes s_Eyes; + Elevator s_Elevator; // required WPILib class objects @@ -25,14 +27,16 @@ public class RunIntake extends Command { // constructor - public RunIntake(Intake s_Intake, ShooterPivot s_ShooterPivot, Shooter s_Shooter, Eyes s_Eyes) { + public RunIntake(Intake s_Intake, ShooterPivot s_ShooterPivot, Shooter s_Shooter, Eyes s_Eyes, + Elevator s_Elevator) { this.s_Intake = s_Intake; this.s_ShooterPivot = s_ShooterPivot; this.s_Shooter = s_Shooter; this.s_Eyes = s_Eyes; + this.s_Elevator = s_Elevator; - addRequirements(s_Intake, s_ShooterPivot, s_Shooter, s_Eyes); + addRequirements(s_Intake, s_ShooterPivot, s_Shooter, s_Eyes, s_Elevator); } @@ -42,9 +46,11 @@ public void execute() { s_Intake.setIntakePivotPosition(s_Intake.intakeGroundPosition); s_Intake.setIntakeVoltage(s_Intake.runIntakeVoltage); + s_Elevator.SetElevatorPosition(0); s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotIntakePosition); s_Shooter.setLoaderVoltage(s_Shooter.runLoaderVoltage); + } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/RunLoader.java b/src/main/java/frc/robot/commands/RunLoader.java new file mode 100644 index 0000000..a0ebc11 --- /dev/null +++ b/src/main/java/frc/robot/commands/RunLoader.java @@ -0,0 +1,51 @@ + +package frc.robot.commands; + +import frc.robot.subsystems.Shooter; +import edu.wpi.first.wpilibj2.command.Command; + + +public class RunLoader extends Command { + + // required subystems + Shooter s_Shooter; + + // required WPILib class objects + + + // local variables + + + + + // constructor + public RunLoader(Shooter s_Shooter) { + + this.s_Shooter = s_Shooter; + + addRequirements(s_Shooter); + + + } + + @Override + public void execute() { + + s_Shooter.setLoaderVoltage(s_Shooter.runLoaderVoltage); + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + + s_Shooter.setLoaderVoltage(s_Shooter.stopLoaderVoltage); + + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ShootPodium.java b/src/main/java/frc/robot/commands/ShootPodium.java deleted file mode 100644 index 1a07bbb..0000000 --- a/src/main/java/frc/robot/commands/ShootPodium.java +++ /dev/null @@ -1,74 +0,0 @@ - -package frc.robot.commands; - -import frc.robot.subsystems.Swerve; -import frc.robot.subsystems.Elevator; -import frc.robot.subsystems.Eyes; -import frc.robot.subsystems.Shooter; -import frc.robot.subsystems.ShooterPivot; -import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; - - -public class ShootPodium extends Command { - - // required subystems - private ShooterPivot shooterPivot; - private Shooter shooter; - private Elevator elevator; - - private double distance; - - // required WPILib class objects - - // local variables - private double shooterAngle; - private double leftShooterSpeed; - private double rightShooterSpeed; - - // positions - - //Higher note shot is lower angle!!! - - private final double podiumAngle = 130; - private final double podiumLeftShooterSpeed = 90.0; - private final double podiumRightShooterSpeed = podiumLeftShooterSpeed; - - - // constructor - public ShootPodium(ShooterPivot shooterPivot, Shooter shooter) { - this.shooterPivot = shooterPivot; - this.shooter = shooter; - - addRequirements(shooterPivot, shooter); - - // instantiate objects - - } - - @Override - public void execute() { - - // move shooter to calculated angle - shooterPivot.moveShooterPivot(podiumAngle); - - // set shooter speed power to calculated value - shooter.shootingMotorsSetControl(podiumRightShooterSpeed, podiumLeftShooterSpeed); - - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - shooter.setLoaderVoltage(shooter.stopLoaderVoltage); - shooter.setShooterVoltage(shooter.stopShooterVoltage, shooter.stopShooterVoltage); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ShootSubwoofer.java b/src/main/java/frc/robot/commands/ShootSubwoofer.java deleted file mode 100644 index bfc6220..0000000 --- a/src/main/java/frc/robot/commands/ShootSubwoofer.java +++ /dev/null @@ -1,76 +0,0 @@ - - -package frc.robot.commands; - -import frc.robot.subsystems.Swerve; -import frc.robot.subsystems.Elevator; -import frc.robot.subsystems.Eyes; -import frc.robot.subsystems.Shooter; -import frc.robot.subsystems.ShooterPivot; -import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; - - -public class ShootSubwoofer extends Command { - - // required subystems - private ShooterPivot shooterPivot; - private Shooter shooter; - private Elevator elevator; - - private double distance; - - // required WPILib class objects - - // local variables - private double shooterAngle; - private double leftShooterSpeed; - private double rightShooterSpeed; - - - // positions - - //Higher note shot is lower angle!!! - - private final double subwooferAngle = 115; - private final double subwooferLeftShooterSpeed = 90.0; - private final double subwooferRightShooterSpeed = subwooferLeftShooterSpeed; - - - // constructor - public ShootSubwoofer(ShooterPivot shooterPivot, Shooter shooter) { - this.shooterPivot = shooterPivot; - this.shooter = shooter; - - addRequirements(shooterPivot, shooter); - - // instantiate objects - - } - - @Override - public void execute() { - - // move shooter to calculated angle - shooterPivot.moveShooterPivot(subwooferAngle); - - // set shooter speed power to calculated value - shooter.shootingMotorsSetControl(subwooferRightShooterSpeed, subwooferLeftShooterSpeed); - - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - shooter.setLoaderVoltage(shooter.stopLoaderVoltage); - shooter.setShooterVoltage(shooter.stopShooterVoltage, shooter.stopShooterVoltage); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ShootXSpot.java b/src/main/java/frc/robot/commands/ShootXSpot.java deleted file mode 100644 index f72f28d..0000000 --- a/src/main/java/frc/robot/commands/ShootXSpot.java +++ /dev/null @@ -1,76 +0,0 @@ - - -package frc.robot.commands; - -import frc.robot.subsystems.Swerve; -import frc.robot.subsystems.Elevator; -import frc.robot.subsystems.Eyes; -import frc.robot.subsystems.Shooter; -import frc.robot.subsystems.ShooterPivot; -import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; - - -public class ShootXSpot extends Command { - - // required subystems - private ShooterPivot shooterPivot; - private Shooter shooter; - private Elevator elevator; - - private double distance; - - // required WPILib class objects - - // local variables - private double shooterAngle; - private double leftShooterSpeed; - private double rightShooterSpeed; - - - // positions - - //Higher note shot is lower angle!!! - - private final double subwooferAngle = 115; - private final double subwooferLeftShooterSpeed = 90.0; - private final double subwooferRightShooterSpeed = subwooferLeftShooterSpeed; - - - // constructor - public ShootXSpot(ShooterPivot shooterPivot, Shooter shooter) { - this.shooterPivot = shooterPivot; - this.shooter = shooter; - - addRequirements(shooterPivot, shooter); - - // instantiate objects - - } - - @Override - public void execute() { - - // move shooter to calculated angle - shooterPivot.moveShooterPivot(subwooferAngle); - - // set shooter speed power to calculated value - shooter.shootingMotorsSetControl(subwooferRightShooterSpeed, subwooferLeftShooterSpeed); - - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - shooter.setLoaderVoltage(shooter.stopLoaderVoltage); - shooter.setShooterVoltage(shooter.stopShooterVoltage, shooter.stopShooterVoltage); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/Shooting.java b/src/main/java/frc/robot/commands/Shooting.java new file mode 100644 index 0000000..46b0bf6 --- /dev/null +++ b/src/main/java/frc/robot/commands/Shooting.java @@ -0,0 +1,52 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; +import frc.robot.subsystems.Elevator; +import frc.robot.subsystems.Eyes; +import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.ShooterPivot; + + +public class Shooting extends Command { + private ShooterPivot shooterPivot; + private Shooter shooter; + + private double shooterAngle; + private double shooterSpeed; + + public Shooting(ShooterPivot shooterPivot, Shooter shooter, double shooterSpeed, double shooterAngle) { + + // Use addRequirements() here to declare subsystem dependencies. + this.shooterPivot = shooterPivot; + this.shooter = shooter; + this.shooterSpeed = shooterSpeed; + this.shooterAngle = shooterAngle; + addRequirements(shooter,shooterPivot); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + shooter.shootingMotorsSetControl(shooterSpeed,shooterSpeed); + shooterPivot.moveShooterPivot(shooterAngle); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/Blower.java b/src/main/java/frc/robot/subsystems/Blower.java new file mode 100644 index 0000000..826135f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Blower.java @@ -0,0 +1,60 @@ +package frc.robot.subsystems; + + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkLowLevel.MotorType; + +public class Blower extends SubsystemBase { + /* Instance Variables + * This section should contain any variables that need to + * be accessed by your entire subsystem/class. This usually includes any phyical motors or sensors that are a part of your subsystem, + * as well as any data that needs to be accessed by your + * entire subsystem/class E.G. the height of the elevator + * or angle of the wrist. + */ + + /* These are variable declarations. Their access modifiers, + * types, and names are specified but they are not given a + * value yet. They should be given a value in the + * constructor. In this example, they is private, which + * means that nothing outside of this class can access it. + */ + private CANSparkMax motor; + private double internalData; + private int blowerID = 19; + + /* Constructor + * The Constructor is a special type of method that gets + * called when this subsystem/class is created, usually + * when the robot turns on. You should give a value to most + * instance variables created above in this method. You can + * also do anything else that you want to happen right away. + */ + public Blower() { + /* These are variable initializations, where a variable + * is given a value. In this case, the `new` keyword is + * used to create a new CANSparkMax object and give it + * to `motor` as it's value. + */ + motor = new CANSparkMax(blowerID, MotorType.kBrushed); + } + + public void runBlower() { + motor.setVoltage(12); + } + + public void stopBlower() { + motor.setVoltage(0); + } + + /* The below method is included in every Subsystem. You can + * think of it as an infinite loop that runs constantly + * while the robot is on. + * + */ + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 762bc61..2fcdcea 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -20,6 +20,7 @@ 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.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -62,7 +63,8 @@ public class Elevator extends SubsystemBase { private double heightlimit = 16; public double elevatorspeed = 0.1; - public double restingposition = 0; + public double restingposition = 0.0; + public double climbingPosition = -2.0; public double shootingPosition = 12.0; private double elevatorP = 1.5; //4.0 @@ -70,6 +72,7 @@ public class Elevator extends SubsystemBase { private double elevatorD = 0.0; private double voltage = 0.0; + public boolean isClimbed = true; /* Constructor @@ -165,6 +168,16 @@ public double getTargetElevatorPosition(){ return targetElevatorPosition; } + public void climb() { + m_elevator1.setSoftLimit(SoftLimitDirection.kReverse,(float)inchesToMotorRotations(climbingPosition)); + m_elevator2.setSoftLimit(SoftLimitDirection.kReverse,(float)inchesToMotorRotations(climbingPosition)); + SetElevatorPosition(climbingPosition); + } + + public void resetElevatorReverseSoftlimit(){ + m_elevator1.setSoftLimit(SoftLimitDirection.kReverse,(float)inchesToMotorRotations(restingposition)); + m_elevator2.setSoftLimit(SoftLimitDirection.kReverse,(float)inchesToMotorRotations(restingposition)); + } private double motorRotationsToInches(double rotations) { return rotations * Constants.ELEVATOR_ROTATIONS_TO_IN; @@ -187,6 +200,19 @@ public boolean atPosition() { } + public boolean atPosition(double setPosition) { + + double error = Math.abs(e_Elevator.getPosition() - setPosition); + + if (Constants.ELEVATOR_TOLERANCE >= error) { + return true; + + } else { + return false; + } + + } + /* The below method is public, which means that it can be * accessed outside of this class. It also takes in a * single double value as a parameter. It then takes that @@ -211,8 +237,15 @@ private void goToShootingPos(){ } } +public void isClimbed(boolean climbState) { + isClimbed = climbState; +} +public void resetEncoder() { + e_Elevator.setPosition(0.0); +} + /* The below method is included in every Subsystem. You can * think of it as an infinite loop that runs constantly @@ -240,6 +273,10 @@ public void periodic() { SmartDashboard.putNumber("Elevator Encoder Value: ", getPosition()); SmartDashboard.putNumber("Current Elevator Position",e_Elevator.getPosition()); SmartDashboard.putNumber("Elevator Voltage", voltage); + SmartDashboard.putNumber("debug/ELEVATOR TARGET POSITION", targetElevatorPosition); + SmartDashboard.putNumber("debug/Elevator Encoder Value: ", getPosition()); + SmartDashboard.putNumber("debug/Current Elevator Position",e_Elevator.getPosition()); + SmartDashboard.putNumber("debug/Elevator Voltage", voltage); // logData(); @@ -257,6 +294,10 @@ public void SetElevatorPosition (double inches){ public Command ElevatorAtPosition(){ return Commands.waitUntil(() -> atPosition()); } + + public Command ElevatorAtPosition(double setPosition){ + return Commands.waitUntil(() -> atPosition(setPosition)); + } } diff --git a/src/main/java/frc/robot/subsystems/Eyes.java b/src/main/java/frc/robot/subsystems/Eyes.java index cbdb9cf..a8939eb 100644 --- a/src/main/java/frc/robot/subsystems/Eyes.java +++ b/src/main/java/frc/robot/subsystems/Eyes.java @@ -12,20 +12,40 @@ package frc.robot.subsystems; +import frc.lib.util.FieldRelativeAccel; +import frc.lib.util.FieldRelativeSpeed; import frc.robot.Constants; import java.util.List; + +import com.pathplanner.lib.commands.FollowPathHolonomic; +import com.pathplanner.lib.commands.PathfindHolonomic; +import com.pathplanner.lib.commands.PathfindingCommand; +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.PIDConstants; +import com.pathplanner.lib.util.ReplanningConfig; + import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.LimelightHelpers; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructPublisher; import frc.robot.subsystems.Swerve; @@ -34,6 +54,7 @@ public class Eyes extends SubsystemBase { // Swerve subsystem for pose estimator Swerve s_Swerve; + Shooter s_Shooter; // create objects and variables public LimelightHelpers limelight; @@ -41,13 +62,25 @@ public class Eyes extends SubsystemBase { public double ty; public double ta; public double tID; - + private double accelerationCompensation = 0.0; //Note this caused a ton of jitter due to inconsistent loop times + private StructPublisher posePublisher; + private StructPublisher closestTrapPose; + private StructPublisher translationPublisher; + private StructPublisher trapPublisher; public boolean controllerRumble = false; + public boolean closeToTrap = false; + public PathPlannerPath trapPath; // constuctor - public Eyes(Swerve swerve) { + public Eyes(Swerve swerve, Shooter shooter) { + posePublisher = NetworkTableInstance.getDefault().getStructTopic("/Moving Goal pose", Pose2d.struct).publish(); + translationPublisher = NetworkTableInstance.getDefault().getStructTopic("/Moving Goal translation", Translation2d.struct).publish(); + trapPublisher = NetworkTableInstance.getDefault().getStructTopic("/Closest Trap", Translation2d.struct).publish(); + closestTrapPose = NetworkTableInstance.getDefault().getStructTopic("/Closest Trap Pose", Pose2d.struct).publish(); s_Swerve = swerve; + s_Shooter = shooter; + trapPath = closestTrapPath(); } @@ -154,6 +187,37 @@ public Pose3d getTargetPose() { } + /* + * This method will return the known pose of the desired target. + * + * parameters: + * none + * + * returns: + * target pose (Pose2d) + */ + public Pose3d getFeedPose() { + + Pose3d pose; + + // if robot is on blue alliance + if(DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue) { + + // get pose of blue speaker + pose = new Pose3d(Constants.Positions.ampBlueX, Constants.Positions.ampBlueY, 0, new Rotation3d(0,0,Constants.Positions.ampBlueR)); + + // if robot is on red alliance + } else { + + // get pose of red speaker + pose = new Pose3d(Constants.Positions.ampRedX, Constants.Positions.ampRedY, 0, new Rotation3d(0,0,Constants.Positions.ampRedR)); + + } + + return pose; + + } + public double getTargetRotation() { Pose2d robotPose = s_Swerve.m_poseEstimator.getEstimatedPosition(); @@ -179,15 +243,69 @@ public double getTargetRotation() { return -angle + 180; } - public boolean swerveAtPosition() { + public double getFeedRotation() { + + Pose2d robotPose = s_Swerve.m_poseEstimator.getEstimatedPosition(); + Pose3d targetPose = getFeedPose(); + + double robotX = robotPose.getX(); + double robotY = robotPose.getY(); + + double targetX = targetPose.getX(); + double targetY = targetPose.getY(); + + double angle = (Math.atan((targetY - robotY) / (targetX - robotX)) * (180 / Math.PI)); + + if (robotX > targetX) { + + angle = angle + 180; + + } + + SmartDashboard.putNumber("angle", angle); + SmartDashboard.putNumber(" inverted angle", -angle); + + return -angle + 180; + } + + public double getMovingTargetRotation() { + Pose2d robotPose = s_Swerve.m_poseEstimator.getEstimatedPosition(); + Pose2d targetPose = getMovingTarget(); + + double robotX = robotPose.getX(); + double robotY = robotPose.getY(); + double targetX = targetPose.getX(); + double targetY = targetPose.getY(); - double error = Math.abs(getTargetRotation() + s_Swerve.m_poseEstimator.getEstimatedPosition().getRotation().getDegrees() % 360); + double angle = (Math.atan((targetY - robotY) / (targetX - robotX)) * (180 / Math.PI)); + + if (robotX > targetX) { + + angle = angle + 180; - SmartDashboard.putNumber("getTargetRotation", getTargetRotation()); + } + + SmartDashboard.putNumber("angle", angle); + SmartDashboard.putNumber(" inverted angle", -angle); + + return -angle + 180; + } + + + public boolean swerveAtPosition(boolean onMove) { + double error; + if(onMove){ + error = Math.abs(getMovingTargetRotation() + s_Swerve.m_poseEstimator.getEstimatedPosition().getRotation().getDegrees() % 360) % 360; + } else { + error = Math.abs(getTargetRotation() + s_Swerve.m_poseEstimator.getEstimatedPosition().getRotation().getDegrees() % 360); + } + + SmartDashboard.putNumber("getMovingTargetRotation", getMovingTargetRotation()); SmartDashboard.putNumber("estimated rotation", s_Swerve.m_poseEstimator.getEstimatedPosition().getRotation().getDegrees() % 360); SmartDashboard.putNumber("rotationError", error); + //SmartDashboard.putBoolean("swerveAtPosition", error <= Constants.Swerve.atPositionTolerance); if (error <= Constants.Swerve.atPositionTolerance) { return true; @@ -196,6 +314,14 @@ public boolean swerveAtPosition() { } } + + + public double getShotTime(double distance) { + + double linearSpeed = ((Math.PI * 4 * s_Shooter.m_leftShooter.getVelocity().getValue() * (2 * Math.PI)) / 1/1.375) * 0.0254; //TODO: change shooter gear ratio + return (distance / linearSpeed) + 0.25; //TODO: tune constant for shot accel/feeding time + } + public double getDistanceFromTarget() { double distance; @@ -218,6 +344,74 @@ public double getDistanceFromTarget() { } + public Pose2d getMovingTarget() { + double shotTime = getShotTime(getDistanceFromTarget()); + + Translation2d movingGoalLocation = new Translation2d(); + + double robotVelX = s_Swerve.fieldRelativeVelocity.vx; + double robotVelY = s_Swerve.fieldRelativeVelocity.vy; + + //TODO calculate accelerations + double robotAccelX = s_Swerve.fieldRelativeAccel.ax; + double robotAccelY = s_Swerve.fieldRelativeAccel.ay; + + double xVelocityCompensation = 1.0; + double yVelocityCompensation = 1.5; + + for(int i=0;i<5;i++){ + + double virtualGoalX; + double virtualGoalY; + + if(DriverStation.getAlliance().get() == Alliance.Blue) { + virtualGoalX = getTargetPose().getX() - shotTime * ((robotVelX * xVelocityCompensation) + robotAccelX * accelerationCompensation); + virtualGoalY = getTargetPose().getY() - shotTime * ((robotVelY * yVelocityCompensation) + robotAccelY * accelerationCompensation); + } else { + virtualGoalX = getTargetPose().getX() + shotTime * ((robotVelX * xVelocityCompensation) + robotAccelX * accelerationCompensation); + virtualGoalY = getTargetPose().getY() + shotTime * ((robotVelY * yVelocityCompensation) + robotAccelY * accelerationCompensation); + } + + Translation2d testGoalLocation = new Translation2d(virtualGoalX, virtualGoalY); + translationPublisher.set(testGoalLocation); + + Translation2d toTestGoal = testGoalLocation.minus(s_Swerve.getEstimatedPose().getTranslation()); + + + + double newShotTime = getShotTime(toTestGoal.getDistance(new Translation2d())); + + + + if(Math.abs(newShotTime-shotTime) <= 0.010){ + i=4; + } + + if(i == 4){ + movingGoalLocation = testGoalLocation; + SmartDashboard.putNumber("NewShotTime", newShotTime); + } + else{ + shotTime = newShotTime; + } + + } + return new Pose2d(movingGoalLocation, getTargetPose().getRotation().toRotation2d()); //TODO: validate rotation--probably not needed since only get x and y + + } + + public double getDistanceFromMovingTarget() { + + double distance; + + double xDistanceToSpeaker = getMovingTarget().getX() - s_Swerve.m_poseEstimator.getEstimatedPosition().getX(); + double yDistanceToSpeaker = getMovingTarget().getY() - s_Swerve.m_poseEstimator.getEstimatedPosition().getY(); + distance = Math.sqrt(Math.pow(xDistanceToSpeaker, 2) + Math.pow(yDistanceToSpeaker, 2)); + + return distance; + + } + public double getDistanceFromTargetBlind() { double distance; @@ -262,30 +456,171 @@ public double getDistanceFromTargetAuto() { } - /*public void updatePoseEstimatorWithVisionBotPose() { + public double getDistanceFromTargetTrap(double trapX, double trapY) { + + double xDistance = trapX - s_Swerve.m_poseEstimator.getEstimatedPosition().getX(); + double yDistance = trapY - s_Swerve.m_poseEstimator.getEstimatedPosition().getY(); + double distance = Math.sqrt(Math.pow(xDistance, 2) + Math.pow(yDistance, 2)); + + return distance; + + } + + public Pose2d getClosestTrap() { + + double trapLeftDistance; + double trapRightDistance; + double trapCenterDistance; + + Pose2d closestTrap = new Pose2d(); + double closestDistance; + + if(DriverStation.getAlliance().get() == Alliance.Blue) { + trapLeftDistance = getDistanceFromTargetTrap(Constants.Positions.blueTrapLeftX, Constants.Positions.blueTrapLeftY); + trapRightDistance = getDistanceFromTargetTrap(Constants.Positions.blueTrapRightX, Constants.Positions.blueTrapRightY); + trapCenterDistance = getDistanceFromTargetTrap(Constants.Positions.blueTrapCenterX, Constants.Positions.blueTrapCenterY); + + closestDistance = Math.min(Math.min(trapLeftDistance, trapRightDistance), trapCenterDistance); + + if (closestDistance == trapLeftDistance) { + closestTrap = new Pose2d(Constants.Positions.blueTrapLeftX, Constants.Positions.blueTrapLeftY, Rotation2d.fromDegrees(Constants.Positions.blueTrapLeftR)); + } else if (closestDistance == trapRightDistance){ + closestTrap = new Pose2d(Constants.Positions.blueTrapRightX, Constants.Positions.blueTrapRightY, Rotation2d.fromDegrees(Constants.Positions.blueTrapRightR)); + } else { + closestTrap = new Pose2d(Constants.Positions.blueTrapCenterX, Constants.Positions.blueTrapCenterY, Rotation2d.fromDegrees(Constants.Positions.blueTrapCenterR)); + } + + } else { + trapLeftDistance = getDistanceFromTargetTrap(Constants.Positions.redTrapLeftX, Constants.Positions.redTrapLeftY); + trapRightDistance = getDistanceFromTargetTrap(Constants.Positions.redTrapRightX, Constants.Positions.redTrapRightY); + trapCenterDistance = getDistanceFromTargetTrap(Constants.Positions.redTrapCenterX, Constants.Positions.redTrapCenterY); + + closestDistance = Math.min(Math.min(trapLeftDistance, trapRightDistance), trapCenterDistance); + + if (closestDistance == trapLeftDistance) { + closestTrap = new Pose2d(Constants.Positions.redTrapLeftX, Constants.Positions.redTrapLeftY, Rotation2d.fromDegrees(Constants.Positions.redTrapLeftR)); + } else if (closestDistance == trapRightDistance){ + closestTrap = new Pose2d(Constants.Positions.redTrapRightX, Constants.Positions.redTrapRightY, Rotation2d.fromDegrees(Constants.Positions.redTrapRightR)); + } else { + closestTrap = new Pose2d(Constants.Positions.redTrapCenterX, Constants.Positions.redTrapCenterY, Rotation2d.fromDegrees(Constants.Positions.redTrapCenterR)); + } + } + + if (closestDistance < Constants.Positions.distanceLimit) { + closeToTrap = true; + } else { + closeToTrap = false; + } + + //Log Target Trap Location + trapPublisher.set(new Translation2d(closestTrap.getX(), closestTrap.getY())); + closestTrapPose.set(closestTrap); + + return closestTrap; + } + + public boolean atTrap() { + + double trapTranslationTolerance = 3.0; + double trapRotationTolerance = 1.0; + + double xError = Math.abs(getRobotPose().getX() - getClosestTrap().getX()); + double yError = Math.abs(getRobotPose().getY() - getClosestTrap().getY()); + double rError = Math.abs(getRobotPose().getRotation().getDegrees() - getClosestTrap().getRotation().getDegrees()); + + if (trapTranslationTolerance >= xError && trapTranslationTolerance >= yError && trapRotationTolerance >= rError) { + return true; + } else { + return false; + } + + } + + + public PathPlannerPath closestTrapPath() { + + Pose2d closestTrap = getClosestTrap(); + Pose2d currentPose = s_Swerve.m_poseEstimator.getEstimatedPosition(); + + //The bezierFromPoses method required that the rotation component of each pose is the direction of travel, not the rotation of a swerve chassis. To set the rotation the path should end with, use the GoalEndState. + + List bezierPoints = PathPlannerPath.bezierFromPoses( + + new Pose2d(currentPose.getX(), currentPose.getY(), closestTrap.minus(currentPose).getRotation()), //TODO Rotation may be wrong + new Pose2d(closestTrap.getX(), closestTrap.getY(), closestTrap.minus(currentPose).getRotation()) //TODO Rotation may be wrong + + ); + + PathPlannerPath path = new PathPlannerPath( + bezierPoints, + new PathConstraints(3.0, 3.0, 2 * Math.PI, 4 * Math.PI), //TODO adjust speeds + new GoalEndState(0.0, closestTrap.getRotation()) + ); + + // Prevent the path from being flipped as all coordinates are blue origin + path.preventFlipping = true; + + return path; + } + + // public Command onTheFly() { + // PathPlannerPath path = closestTrapPath(); + // return new FollowPathHolonomic( + // path, + // () -> s_Swerve.getPose(), // Robot pose supplier + // () -> s_Swerve.getChassisSpeed(), // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + // (speeds) -> s_Swerve.setChassisSpeed(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds + // new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your Constants class + // new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants + // new PIDConstants(5.0, 0.0, 0.0), // Rotation PID constants + // Constants.Swerve.maxSpeed, // Max module speed, in m/s + // 0.4, // Drive base radius in meters. Distance from robot center to furthest module. + // new ReplanningConfig() // Default path replanning config. See the API for the options here + // ), + // () -> { + // // Boolean supplier that controls when the path will be mirrored for the red alliance + // // This will flip the path being followed to the red side of the field. + // // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + // var alliance = DriverStation.getAlliance(); + // if (alliance.isPresent()) { + // return alliance.get() == DriverStation.Alliance.Red; + // } + // return false; + // }, + // s_Swerve // Reference to this subsystem to set requirements + // ); + // } + + + + public void updatePoseEstimatorWithVisionBotPose() { //invalid limelight if (LimelightHelpers.getTV("") == false) { return; } // distance from current pose to vision estimated pose - double poseDifference = s_Swerve.m_poseEstimator.getEstimatedPosition().getTranslation() - .getDistance(getRobotPose().getTranslation()); + LimelightHelpers.PoseEstimate lastResult = LimelightHelpers.getBotPoseEstimate_wpiBlue(""); + double fpgaTimestamp = Timer.getFPGATimestamp(); + + Translation2d translation = s_Swerve.m_poseEstimator.getEstimatedPosition().getTranslation(); + double poseDifference = translation.getDistance(lastResult.pose.getTranslation()); double xyStds; double degStds; // multiple targets detected - if (limelight.getNumberOfTargetsVisible() >= 2) { + if (lastResult.tagCount >= 2) { xyStds = 0.5; degStds = 6; } // 1 target with large area and close to estimated pose - else if (LimelightHelpers.getTA() > 0.8 && poseDifference < 0.5) { + else if (lastResult.avgTagArea > 0.8 && poseDifference < 0.5) { xyStds = 1.0; degStds = 12; } // 1 target farther away and estimated pose is close - else if (limelight.getBestTargetArea() > 0.1 && poseDifference < 0.3) { + else if (lastResult.avgTagArea > 0.1 && poseDifference < 0.3) { xyStds = 2.0; degStds = 30; } @@ -297,15 +632,41 @@ else if (limelight.getBestTargetArea() > 0.1 && poseDifference < 0.3) { s_Swerve.m_poseEstimator.setVisionMeasurementStdDevs( VecBuilder.fill(xyStds, xyStds, Units.degreesToRadians(degStds))); s_Swerve.m_poseEstimator.addVisionMeasurement(getRobotPose(), - Timer.getFPGATimestamp() - (LimelightHelpers.getLatency_Pipeline("")/1000.0) - (LimelightHelpers.getLatency_Capture("")/1000.0)); + fpgaTimestamp - (LimelightHelpers.getLatency_Pipeline("")/1000.0) - (LimelightHelpers.getLatency_Capture("")/1000.0)); } - } -/* */ + + public void updatePoseEstimatorWithVisionBotPoseMegatag2() { + //invalid limelight + if (LimelightHelpers.getTV("") == false) { + return; + } + + boolean doRejectUpdate = false; + LimelightHelpers.SetRobotOrientation("limelight", s_Swerve.m_poseEstimator.getEstimatedPosition().getRotation().getDegrees(), 0, 0, 0, 0, 0); + LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight"); + if(Math.abs(s_Swerve.gyro.getRate()) > 720) // if our angular velocity is greater than 720 degrees per second, ignore vision updates + { + doRejectUpdate = true; + } + if(!doRejectUpdate) + { + s_Swerve.m_poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(.6,.6,9999999)); + s_Swerve.m_poseEstimator.addVisionMeasurement( + mt2.pose, + mt2.timestampSeconds); + } + } + + //public Command onTheFly = new PathfindHolonomic(getClosestTrap(), null, null, null, null, null, null) @Override public void periodic() { s_Swerve.m_poseEstimator.update(s_Swerve.getGyroYaw(), s_Swerve.getModulePositions()); + + //updatePoseEstimatorWithVisionBotPose(); + + if (LimelightHelpers.getTV("") == true) { s_Swerve.m_poseEstimator.addVisionMeasurement( getRobotPose(), @@ -313,6 +674,9 @@ public void periodic() { ); } + trapPath = closestTrapPath(); + trapPath.getPathPoses().get(0); + SmartDashboard.putNumber("Pose estimator rotations", s_Swerve.m_poseEstimator.getEstimatedPosition().getRotation().getDegrees()); SmartDashboard.putNumber("Pose Estimator X", s_Swerve.m_poseEstimator.getEstimatedPosition().getX()); SmartDashboard.putNumber("Pose Estimator Y", s_Swerve.m_poseEstimator.getEstimatedPosition().getY()); @@ -320,5 +684,9 @@ public void periodic() { SmartDashboard.putNumber("target Y", getTargetPose().getY()); SmartDashboard.putNumber("Distance to Target", getDistanceFromTarget()); + + posePublisher.set(getMovingTarget()); + // trapPathCurrentPosePub.set(trapPath.getPathPoses().get(0)); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 0886038..9e92ce2 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -14,7 +14,10 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; public class Intake extends SubsystemBase { @@ -27,8 +30,8 @@ public class Intake extends SubsystemBase { // positions // NOTE: these positions are also used in robotcontainer. - public final double intakeSafePosition = 28.0; - public final double intakeGroundPosition = -74.0; + public final double intakeSafePosition = -35; + public final double intakeGroundPosition = -135; public final double intakeSourcePosition = intakeSafePosition; // PID values @@ -41,7 +44,7 @@ public class Intake extends SubsystemBase { private final double magnetOffSet = 0.0; // Kermit Limits - private final int intakeCurrentLimit = 120; + private final int intakeCurrentLimit = 60; private final int intakePivotCurrentLimit = 60; // local variables @@ -82,7 +85,7 @@ public Intake() { // TODO: Go over this part with Dylan and student that wrote this. Can we simplify this? e_intakePivotIntegrated = m_IntakePivot.getEncoder(); e_intakePivotIntegrated.setPositionConversionFactor(360 / intakePivotMotorGearRatio); - e_intakePivotIntegrated.setPosition(e_intakePivot.getPosition().getValue()); + e_intakePivotIntegrated.setPosition(e_intakePivot.getPosition().getValue() * 360); e_intakePivotIntegrated.setVelocityConversionFactor(360 / intakePivotMotorGearRatio); // create PID loop for intake pivot @@ -136,12 +139,34 @@ public void setIntakeVoltage(double intakeSpeedVoltage) { public double cancoderInDegrees() { return e_intakePivot.getPosition().getValue() * 360; } + + double getTargetIntakePosition() { + return intakeGroundPosition; + } + + + public boolean atPosition() { + + double error = Math.abs(e_intakePivot.getPosition().getValueAsDouble() - getTargetIntakePosition()); + + if (5.0 >= error) { + return true; + + } else { + return false; + } + + } + + public Command IntakeAtPosition(){ + return Commands.waitUntil(() -> atPosition()); + } @Override public void periodic() { // move the intake pivot motor to the current desired position - intakePivotVoltage = pid.calculate(cancoderInDegrees(), m_setPoint)/*+ feedForward) */; + intakePivotVoltage = pid.calculate(e_intakePivotIntegrated.getPosition(), m_setPoint)/*+ feedForward) */; m_IntakePivot.setVoltage(intakePivotVoltage); // log values @@ -150,5 +175,10 @@ public void periodic() { SmartDashboard.putNumber("Intake Pivot Motor Position", e_intakePivotIntegrated.getPosition()); SmartDashboard.putNumber("Intake setpoint", m_setPoint); SmartDashboard.putNumber("Intake Current", m_Intake.getOutputCurrent()); + SmartDashboard.putNumber("debug/Intake Voltage", intakePivotVoltage); + SmartDashboard.putNumber("debug/Intake CANcoder", cancoderInDegrees()); + SmartDashboard.putNumber("debug/Intake Pivot Motor Position", e_intakePivotIntegrated.getPosition()); + SmartDashboard.putNumber("debug/Intake setpoint", m_setPoint); + SmartDashboard.putNumber("debug/Intake Current", m_Intake.getOutputCurrent()); } } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 11e1173..db90ccf 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -26,6 +26,7 @@ public class Shooter extends SubsystemBase { private final int rightShooterMotorID = 14; private final int loaderMotorID = 16; private final int breakBeamID = 0; + // loader speeds public final double runLoaderVoltage = 12.0; @@ -38,26 +39,30 @@ public class Shooter extends SubsystemBase { public final double stopShooterVoltage = 0.0; public final double shooterSpeedToleranceRPS = 100.0/60.0; //100 RPM + public final double shotSpeedRPS = 90; + public final double shooterSpinReduction = 1.0; + private final int currentLimit = 80; + // left shooter motor PID - private final double lShooterMotorPGains = 0.05; + private final double lShooterMotorPGains = 0.2; private final double lShooterMotorIGains = 0.0; private final double lShooterMotorDGains = 0.0; private final double lShooterMotorSGains = 0.0; private final double lShooterMotorVGains = 0.12; // right shooter motor PID - private final double rShooterMotorPGains = 0.05; + private final double rShooterMotorPGains = 0.2; private final double rShooterMotorIGains = 0.0; private final double rShooterMotorDGains = 0.0; - private final int m_CurrentLimit = 40; + private final int m_LoaderCurrentLimit = 40; private final double rShooterMotorSGains = 0.0; private final double rShooterMotorVGains = 0.12; // WPILib class objects - private TalonFX m_leftShooter; - private TalonFX m_rightShooter; + public TalonFX m_leftShooter; + public TalonFX m_rightShooter; private TalonFX m_loader; private Slot0Configs slotConfigsR; @@ -69,7 +74,9 @@ public class Shooter extends SubsystemBase { public DigitalInput breakBeam; private TalonFXConfigurator configF; - private double m_setSpeed = 0.0; + public double m_setSpeed = 0.0; + + public boolean gotNote = false; // constructor public Shooter() { @@ -86,7 +93,7 @@ public Shooter() { configF.apply( new CurrentLimitsConfigs() .withSupplyCurrentLimitEnable(true) - .withSupplyCurrentLimit(m_CurrentLimit) + .withSupplyCurrentLimit(m_LoaderCurrentLimit) ); // right shooter motor configuration @@ -151,6 +158,11 @@ public void setLoaderVoltage(double loaderSpeedVoltage) { */ public boolean getBreakBeamOutput() { return breakBeam.get(); + + } + + public void checkNote() { + gotNote = !getBreakBeamOutput(); } public Trigger getBreakBeamTrigger() { @@ -174,9 +186,19 @@ public void shootingMotorsConfig() { slotConfigsL.kP = SmartDashboard.getNumber("LShooter kP", 0); slotConfigsL.kV = SmartDashboard.getNumber("LShooter kV", 0); + + // set configurations to motors m_rightShooter.getConfigurator().apply(slotConfigsR); m_leftShooter.getConfigurator().apply(slotConfigsL); + + m_rightShooter.getConfigurator().apply(new CurrentLimitsConfigs() + .withSupplyCurrentLimitEnable(true) + .withSupplyCurrentLimit(currentLimit)); + + m_leftShooter.getConfigurator().apply(new CurrentLimitsConfigs() + .withSupplyCurrentLimitEnable(true) + .withSupplyCurrentLimit(currentLimit)); } /* @@ -238,11 +260,23 @@ public void periodic() { // log shooting data SmartDashboard.putNumber("Right Shooter Speed", m_rightShooter.getVelocity().getValueAsDouble()); SmartDashboard.putNumber("Left Shooter Speed", m_leftShooter.getVelocity().getValueAsDouble()); + SmartDashboard.putNumber("Right Shooter Voltage", m_rightShooter.getMotorVoltage().getValue()); + SmartDashboard.putNumber("Left Shooter Voltage", m_leftShooter.getMotorVoltage().getValue()); SmartDashboard.putNumber("Right Shooter Temp", m_rightShooter.getDeviceTemp().getValueAsDouble()); SmartDashboard.putNumber("Left Shooter Temp", m_leftShooter.getDeviceTemp().getValueAsDouble()); SmartDashboard.putBoolean("Break Beam Sensor", getBreakBeamOutput()); SmartDashboard.putBoolean("driver/Break Beam Sensor", getBreakBeamOutput()); SmartDashboard.putNumber("Right Shooter Current", m_rightShooter.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("Left Shooter Current", m_leftShooter.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("debug/Right Shooter Speed", m_rightShooter.getVelocity().getValueAsDouble()); + SmartDashboard.putNumber("debug/Left Shooter Speed", m_leftShooter.getVelocity().getValueAsDouble()); + SmartDashboard.putNumber("debug/Right Shooter Temp", m_rightShooter.getDeviceTemp().getValueAsDouble()); + SmartDashboard.putNumber("debug/Left Shooter Temp", m_leftShooter.getDeviceTemp().getValueAsDouble()); + SmartDashboard.putBoolean("debug/Break Beam Sensor", getBreakBeamOutput()); + SmartDashboard.putNumber("debug/Right Shooter Current", m_rightShooter.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("debug/Left Shooter Current", m_leftShooter.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Right Shooter Stator Current", m_rightShooter.getStatorCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Left Shooter Stator Current", m_leftShooter.getStatorCurrent().getValueAsDouble()); + } } diff --git a/src/main/java/frc/robot/subsystems/ShooterPivot.java b/src/main/java/frc/robot/subsystems/ShooterPivot.java index 4d71c79..6b5e034 100644 --- a/src/main/java/frc/robot/subsystems/ShooterPivot.java +++ b/src/main/java/frc/robot/subsystems/ShooterPivot.java @@ -29,8 +29,8 @@ public class ShooterPivot extends SubsystemBase { // positions public final double shooterPivotStowPosition = 115.0; public final double shooterPivotIntakePosition = 136.75; - public final double shooterPivotAmpPosition = 200; - public final double shooterPivotClimbPosition = 364.30; + public final double shooterPivotAmpPosition = 192.5; + public final double shooterPivotClimbPosition = 309; // pivot motor PID private final double shooterPivotPGains = 0.3; //0.5 @@ -117,6 +117,7 @@ public boolean atPosition() { double error = Math.abs(cancoderInDegrees() - m_setPoint); + //SmartDashboard.putBoolean("at pivot position", pivotTolerance >= error); if (pivotTolerance >= error) { return true; @@ -145,5 +146,8 @@ public void periodic() { SmartDashboard.putNumber("Shooter CANcoder", cancoderInDegrees()); SmartDashboard.putNumber("Shooter Pivot Motor Position", e_ShooterPivotIntegrated.getPosition()); SmartDashboard.putNumber("Shooter setpoint", m_setPoint); + SmartDashboard.putNumber("debug/Shooter Pivot Voltage", m_ShooterPivotVoltage); + SmartDashboard.putNumber("debug/Shooter Pivot Motor Position", e_ShooterPivotIntegrated.getPosition()); + SmartDashboard.putNumber("debug/Shooter setpoint", m_setPoint); } } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 6ee19e4..61b5048 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -6,6 +6,9 @@ import frc.robot.SwerveModule; import frc.robot.LimelightHelpers.LimelightTarget_Detector; +import frc.lib.util.FieldRelativeAccel; +import frc.lib.util.FieldRelativeAccel; +import frc.lib.util.FieldRelativeSpeed; import frc.robot.Constants; import frc.robot.LimelightHelpers; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -13,6 +16,9 @@ import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; +import java.util.List; +import java.util.function.Supplier; + import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.hardware.Pigeon2; @@ -45,6 +51,8 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.FollowPathHolonomic; +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; @@ -69,7 +77,13 @@ public class Swerve extends SubsystemBase { private StructPublisher posePublisher; public StructArrayPublisher swerveKinematicsPublisher; public StructPublisher estimatedRobotPosePublisher; + private StructPublisher trapPathCurrentPosePub; public SwerveDrivePoseEstimator m_poseEstimator; + public FieldRelativeSpeed fieldRelativeVelocity; + public FieldRelativeSpeed lastFieldRelativeVelocity; + public FieldRelativeAccel fieldRelativeAccel; + private double time; + private double lastTime = 0.0; // constructor @@ -78,6 +92,9 @@ public Swerve() { // instantiate objects gyro = new Pigeon2(Constants.Swerve.pigeonID); m_field = new Field2d(); + fieldRelativeVelocity = new FieldRelativeSpeed(); + lastFieldRelativeVelocity = new FieldRelativeSpeed(); + fieldRelativeAccel = new FieldRelativeAccel(); // set gyro gyro.getConfigurator().apply(new Pigeon2Configuration()); @@ -101,6 +118,7 @@ public Swerve() { posePublisher = NetworkTableInstance.getDefault().getStructTopic("/MyPose", Pose2d.struct).publish(); swerveKinematicsPublisher = NetworkTableInstance.getDefault().getStructArrayTopic("/SwerveModuleStates", SwerveModuleState.struct).publish(); estimatedRobotPosePublisher = NetworkTableInstance.getDefault().getStructTopic("/EstimatedRobotPose", Pose2d.struct).publish(); + trapPathCurrentPosePub = NetworkTableInstance.getDefault().getStructTopic("/Trap Path Current Pose", Pose2d.struct).publish(); posePublisher = NetworkTableInstance.getDefault() .getStructTopic("RobotPose", Pose2d.struct).publish(); @@ -284,16 +302,74 @@ public void resetModulesToAbsolute(){ } } + public Command onTheFly(Supplier pathSupplier) { + + PathPlannerPath path = pathSupplier.get(); + + trapPathCurrentPosePub.set(path.getPathPoses().get(0)); + + return new FollowPathHolonomic( + path, + this::getEstimatedPose, // Robot pose supplier + this::getChassisSpeed, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + this::setChassisSpeed, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds + new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your Constants class + new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants + new PIDConstants(5.0, 0.0, 0.0), // Rotation PID constants + Constants.Swerve.maxSpeed, // Max module speed, in m/s + 0.4, // Drive base radius in meters. Distance from robot center to furthest module. + new ReplanningConfig() // Default path replanning config. See the API for the options here + ), + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this // Reference to this subsystem to set requirements + ); + } + + + // Since AutoBuilder is configured, we can use it to build pathfinding commands + Command pathfindingCommand(Pose2d targetPose) { + // Create the constraints to use while pathfinding + PathConstraints constraints = new PathConstraints( + 3.0, 4.0, + Units.degreesToRadians(540), Units.degreesToRadians(720)); + + return AutoBuilder.pathfindToPose( + targetPose, + constraints, + 0.0, // Goal end velocity in meters/sec + 0.0 // Rotation delay distance in meters. This is how far the robot should travel before attempting to rotate. + ); + } @Override public void periodic(){ swerveOdometry.update(getGyroYaw(), getModulePositions()); + time = Timer.getFPGATimestamp(); + fieldRelativeVelocity = new FieldRelativeSpeed(getChassisSpeed(), getGyroYaw()); + fieldRelativeAccel = new FieldRelativeAccel(fieldRelativeVelocity, lastFieldRelativeVelocity, time-lastTime); //time was 0.02 for standard loop time attempting to calculate it to increase accuracy. May crash with /0 error if startup is too quick + lastFieldRelativeVelocity = fieldRelativeVelocity; + lastTime = time; + + SmartDashboard.putNumber("ChassisSpeedX", getChassisSpeed().vxMetersPerSecond); SmartDashboard.putNumber("ChassisSpeedY", getChassisSpeed().vyMetersPerSecond); SmartDashboard.putNumber("ChassisSpeedOmega", getChassisSpeed().omegaRadiansPerSecond); + SmartDashboard.putNumber("debug/ChassisSpeedX", getChassisSpeed().vxMetersPerSecond); + SmartDashboard.putNumber("debug/ChassisSpeedY", getChassisSpeed().vyMetersPerSecond); + SmartDashboard.putNumber("debug/ChassisSpeedOmega", getChassisSpeed().omegaRadiansPerSecond); @@ -303,9 +379,18 @@ public void periodic(){ SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Velocity", mod.getState().speedMetersPerSecond); } + for(SwerveModule mod : mSwerveMods){ + SmartDashboard.putNumber("debug/Mod " + mod.moduleNumber + " CANcoder", mod.getCANcoder().getDegrees()); + SmartDashboard.putNumber("debug/Mod " + mod.moduleNumber + " Angle", mod.getPosition().angle.getDegrees()); + SmartDashboard.putNumber("debug/Mod " + mod.moduleNumber + " Velocity", mod.getState().speedMetersPerSecond); + } + SmartDashboard.putNumber("Robot X", swerveOdometry.getPoseMeters().getX()); SmartDashboard.putNumber("Robot Y", swerveOdometry.getPoseMeters().getY()); SmartDashboard.putNumber("gyro angle", getGyroYaw().getDegrees()); + SmartDashboard.putNumber("debug/Robot X", swerveOdometry.getPoseMeters().getX()); + SmartDashboard.putNumber("debug/Robot Y", swerveOdometry.getPoseMeters().getY()); + SmartDashboard.putNumber("debug/gyro angle", getGyroYaw().getDegrees()); diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index cae1363..6dc648d 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.1.6", + "version": "2024.2.8", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.1.6" + "version": "2024.2.8" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.1.6", + "version": "2024.2.8", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json index 69a4079..2b7d172 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix6.json", "name": "CTRE-Phoenix (v6)", - "version": "24.1.0", + "version": "24.2.0", "frcYear": 2024, "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.1.0" + "version": "24.2.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -39,7 +39,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -52,7 +52,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -65,7 +65,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -78,7 +78,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -91,7 +91,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -104,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -117,7 +117,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -130,7 +130,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -143,7 +143,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -158,7 +158,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -173,7 +173,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -188,7 +188,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -203,7 +203,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -218,7 +218,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -233,7 +233,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -248,7 +248,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -263,7 +263,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -278,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -293,7 +293,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -323,7 +323,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 0f3520e..f85acd4 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2024.2.0", + "version": "2024.2.4", "frcYear": "2024", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.0" + "version": "2024.2.4" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.0", + "version": "2024.2.4", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.0", + "version": "2024.2.4", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.0", + "version": "2024.2.4", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false,