diff --git a/simgui.json b/simgui.json index 159d4d1..721b509 100644 --- a/simgui.json +++ b/simgui.json @@ -192,7 +192,7 @@ 0.0, 0.8500000238418579 ], - "height": 338, + "height": 0, "series": [ { "color": [ diff --git a/src/main/deploy/pathplanner/autos/!everything black 2.0.auto b/src/main/deploy/pathplanner/autos/!everything black 2.0.auto new file mode 100644 index 0000000..1ed2251 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/!everything black 2.0.auto @@ -0,0 +1,44 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7636103146671326, + "y": 4.403274541213847 + }, + "rotation": -60.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "stage.sweep2" + } + }, + { + "type": "named", + "data": { + "name": "stow" + } + } + ] + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/stage.full.auto b/src/main/deploy/pathplanner/autos/amp.2+1.auto similarity index 64% rename from src/main/deploy/pathplanner/autos/stage.full.auto rename to src/main/deploy/pathplanner/autos/amp.2+1.auto index 57476a3..6f7aa3a 100644 --- a/src/main/deploy/pathplanner/autos/stage.full.auto +++ b/src/main/deploy/pathplanner/autos/amp.2+1.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.73, - "y": 4.39 + "x": 0.8, + "y": 6.63 }, - "rotation": -60.0 + "rotation": 60.0 }, "command": { "type": "sequential", @@ -14,57 +14,51 @@ { "type": "named", "data": { - "name": "home" + "name": "shoot" } }, { "type": "named", "data": { - "name": "readyShoot" + "name": "intake" } }, { - "type": "named", + "type": "path", "data": { - "name": "shootNote" + "pathName": "amp.toNearest" } }, { "type": "path", "data": { - "pathName": "stage.toNearest" + "pathName": "amp.toSubwoofer" } }, { - "type": "race", + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "parallel", "data": { "commands": [ { "type": "named", "data": { - "name": "intakeNote" + "name": "intake" } }, { - "type": "wait", + "type": "path", "data": { - "waitTime": 2.0 + "pathName": "amp.race" } } ] } - }, - { - "type": "path", - "data": { - "pathName": "stage.toSubwoofer" - } - }, - { - "type": "named", - "data": { - "name": "shootNote" - } } ] } diff --git a/src/main/deploy/pathplanner/autos/amp.shootIntakeShootLeave.auto b/src/main/deploy/pathplanner/autos/amp.shootIntakeShootLeave.auto new file mode 100644 index 0000000..6c8b161 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/amp.shootIntakeShootLeave.auto @@ -0,0 +1,61 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.8, + "y": 6.63 + }, + "rotation": 60.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "path", + "data": { + "pathName": "amp.toNearest" + } + }, + { + "type": "path", + "data": { + "pathName": "amp.toSubwoofer" + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "named", + "data": { + "name": "stow" + } + }, + { + "type": "path", + "data": { + "pathName": "amp.leave" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/amp.shootLeave.auto b/src/main/deploy/pathplanner/autos/amp.shootLeave.auto index 7374646..f1f5697 100644 --- a/src/main/deploy/pathplanner/autos/amp.shootLeave.auto +++ b/src/main/deploy/pathplanner/autos/amp.shootLeave.auto @@ -12,28 +12,15 @@ "data": { "commands": [ { - "type": "named", + "type": "wait", "data": { - "name": "home" + "waitTime": 2.0 } }, { - "type": "deadline", + "type": "named", "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 3.0 - } - }, - { - "type": "named", - "data": { - "name": "shootNote" - } - } - ] + "name": "shoot" } }, { diff --git a/src/main/deploy/pathplanner/autos/!!!AMP!!!.auto b/src/main/deploy/pathplanner/autos/amp.shootLeaveNoMove.auto similarity index 85% rename from src/main/deploy/pathplanner/autos/!!!AMP!!!.auto rename to src/main/deploy/pathplanner/autos/amp.shootLeaveNoMove.auto index 185323b..df093e7 100644 --- a/src/main/deploy/pathplanner/autos/!!!AMP!!!.auto +++ b/src/main/deploy/pathplanner/autos/amp.shootLeaveNoMove.auto @@ -12,15 +12,15 @@ "data": { "commands": [ { - "type": "named", + "type": "wait", "data": { - "name": "home" + "waitTime": 2.0 } }, { "type": "named", "data": { - "name": "shootNote" + "name": "shoot" } }, { diff --git a/src/main/deploy/pathplanner/autos/amp.leaveOnly.auto b/src/main/deploy/pathplanner/autos/amp.shootWaitLeave.auto similarity index 56% rename from src/main/deploy/pathplanner/autos/amp.leaveOnly.auto rename to src/main/deploy/pathplanner/autos/amp.shootWaitLeave.auto index 295a313..d082bf9 100644 --- a/src/main/deploy/pathplanner/autos/amp.leaveOnly.auto +++ b/src/main/deploy/pathplanner/autos/amp.shootWaitLeave.auto @@ -11,10 +11,28 @@ "type": "sequential", "data": { "commands": [ + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, { "type": "named", "data": { - "name": "home" + "name": "shoot" + } + }, + { + "type": "named", + "data": { + "name": "stow" + } + }, + { + "type": "wait", + "data": { + "waitTime": 8.0 } }, { diff --git a/src/main/deploy/pathplanner/autos/center.3.auto b/src/main/deploy/pathplanner/autos/center.3.auto new file mode 100644 index 0000000..2247eb0 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/center.3.auto @@ -0,0 +1,101 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3633965729469937, + "y": 5.555088802485298 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "center.toNearest" + } + }, + { + "type": "path", + "data": { + "pathName": "center.toSubwoofer" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "center.toStage" + } + }, + { + "type": "path", + "data": { + "pathName": "center.stageToSub" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/center.home.auto b/src/main/deploy/pathplanner/autos/center.home.auto deleted file mode 100644 index aeb8e4b..0000000 --- a/src/main/deploy/pathplanner/autos/center.home.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.36, - "y": 5.56 - }, - "rotation": 0.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "home" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/center.full.auto b/src/main/deploy/pathplanner/autos/center.shootIntakeShootLeave.auto similarity index 76% rename from src/main/deploy/pathplanner/autos/center.full.auto rename to src/main/deploy/pathplanner/autos/center.shootIntakeShootLeave.auto index 836de3c..97ba570 100644 --- a/src/main/deploy/pathplanner/autos/center.full.auto +++ b/src/main/deploy/pathplanner/autos/center.shootIntakeShootLeave.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.36, - "y": 5.56 + "x": 1.3633965729469937, + "y": 5.555088802485298 }, - "rotation": 0.0 + "rotation": 0 }, "command": { "type": "sequential", @@ -14,43 +14,43 @@ { "type": "named", "data": { - "name": "home" + "name": "shoot" } }, { "type": "named", "data": { - "name": "readyShoot" + "name": "intake" } }, { - "type": "named", + "type": "path", "data": { - "name": "shootNote" + "pathName": "center.toNearest" } }, { "type": "path", "data": { - "pathName": "center.toNearest" + "pathName": "center.toSubwoofer" } }, { "type": "named", "data": { - "name": "intakeNote" + "name": "shoot" } }, { - "type": "path", + "type": "named", "data": { - "pathName": "center.toSubwoofer" + "name": "stow" } }, { - "type": "named", + "type": "path", "data": { - "name": "shootNote" + "pathName": "center.leave" } } ] diff --git a/src/main/deploy/pathplanner/autos/center.shootLeave.auto b/src/main/deploy/pathplanner/autos/center.shootLeave.auto new file mode 100644 index 0000000..50c3a8d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/center.shootLeave.auto @@ -0,0 +1,43 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3633965729469937, + "y": 5.555088802485298 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "named", + "data": { + "name": "stow" + } + }, + { + "type": "path", + "data": { + "pathName": "center.leave" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/center.leaveOnly.auto b/src/main/deploy/pathplanner/autos/center.shootLeaveNoMove.auto similarity index 55% rename from src/main/deploy/pathplanner/autos/center.leaveOnly.auto rename to src/main/deploy/pathplanner/autos/center.shootLeaveNoMove.auto index a4a6808..07ca891 100644 --- a/src/main/deploy/pathplanner/autos/center.leaveOnly.auto +++ b/src/main/deploy/pathplanner/autos/center.shootLeaveNoMove.auto @@ -2,25 +2,31 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.36, - "y": 5.56 + "x": 1.3633965729469937, + "y": 5.555088802485298 }, - "rotation": 0.0 + "rotation": 0 }, "command": { "type": "sequential", "data": { "commands": [ + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, { "type": "named", "data": { - "name": "home" + "name": "shoot" } }, { - "type": "path", + "type": "named", "data": { - "pathName": "center.leave" + "name": "stow" } } ] diff --git a/src/main/deploy/pathplanner/autos/everything black.auto b/src/main/deploy/pathplanner/autos/everything black.auto new file mode 100644 index 0000000..6543d17 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/everything black.auto @@ -0,0 +1,44 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7636103146671326, + "y": 4.403274541213847 + }, + "rotation": -60.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "stage.sweep" + } + }, + { + "type": "named", + "data": { + "name": "stow" + } + } + ] + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/stage.home.auto b/src/main/deploy/pathplanner/autos/stage.home.auto deleted file mode 100644 index 254117d..0000000 --- a/src/main/deploy/pathplanner/autos/stage.home.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.8108477755417065, - "y": 4.311854008323402 - }, - "rotation": -57.20046872738075 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "home" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/source.auto b/src/main/deploy/pathplanner/autos/stage.leave.auto similarity index 94% rename from src/main/deploy/pathplanner/autos/source.auto rename to src/main/deploy/pathplanner/autos/stage.leave.auto index 9cc6469..9c07f9a 100644 --- a/src/main/deploy/pathplanner/autos/source.auto +++ b/src/main/deploy/pathplanner/autos/stage.leave.auto @@ -14,7 +14,7 @@ { "type": "named", "data": { - "name": "home" + "name": "stow" } }, { diff --git a/src/main/deploy/pathplanner/autos/stage.shootIntakeShootLeave.auto b/src/main/deploy/pathplanner/autos/stage.shootIntakeShootLeave.auto new file mode 100644 index 0000000..cb5d1ef --- /dev/null +++ b/src/main/deploy/pathplanner/autos/stage.shootIntakeShootLeave.auto @@ -0,0 +1,61 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7636103146671326, + "y": 4.403274541213847 + }, + "rotation": -60.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "path", + "data": { + "pathName": "stage.toNearest" + } + }, + { + "type": "path", + "data": { + "pathName": "stage.toSubwoofer" + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "named", + "data": { + "name": "stow" + } + }, + { + "type": "path", + "data": { + "pathName": "stage.leave" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/stage.shootLeave.auto b/src/main/deploy/pathplanner/autos/stage.shootLeave.auto new file mode 100644 index 0000000..9f22aaa --- /dev/null +++ b/src/main/deploy/pathplanner/autos/stage.shootLeave.auto @@ -0,0 +1,43 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7636103146671326, + "y": 4.403274541213847 + }, + "rotation": -60.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "named", + "data": { + "name": "stow" + } + }, + { + "type": "path", + "data": { + "pathName": "stage.leave" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/stage.leaveOnly.auto b/src/main/deploy/pathplanner/autos/stage.shootLeaveNoMove.auto similarity index 59% rename from src/main/deploy/pathplanner/autos/stage.leaveOnly.auto rename to src/main/deploy/pathplanner/autos/stage.shootLeaveNoMove.auto index 3148f23..64e06b1 100644 --- a/src/main/deploy/pathplanner/autos/stage.leaveOnly.auto +++ b/src/main/deploy/pathplanner/autos/stage.shootLeaveNoMove.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.73, - "y": 4.39 + "x": 0.7636103146671326, + "y": 4.403274541213847 }, "rotation": -60.0 }, @@ -11,16 +11,22 @@ "type": "sequential", "data": { "commands": [ + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, { "type": "named", "data": { - "name": "home" + "name": "shoot" } }, { - "type": "path", + "type": "named", "data": { - "pathName": "stage.leave" + "name": "stow" } } ] diff --git a/src/main/deploy/pathplanner/autos/amp.home.auto b/src/main/deploy/pathplanner/autos/swiffer sweeper upper.auto similarity index 63% rename from src/main/deploy/pathplanner/autos/amp.home.auto rename to src/main/deploy/pathplanner/autos/swiffer sweeper upper.auto index d903830..a05209a 100644 --- a/src/main/deploy/pathplanner/autos/amp.home.auto +++ b/src/main/deploy/pathplanner/autos/swiffer sweeper upper.auto @@ -2,19 +2,19 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.8, - "y": 6.63 + "x": 0.7636103146671326, + "y": 4.403274541213847 }, - "rotation": 60.0 + "rotation": -60.0 }, "command": { "type": "sequential", "data": { "commands": [ { - "type": "named", + "type": "path", "data": { - "name": "home" + "pathName": "stage.sweep" } } ] diff --git a/src/main/deploy/pathplanner/paths/amp.race.path b/src/main/deploy/pathplanner/paths/amp.race.path new file mode 100644 index 0000000..cf38c55 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/amp.race.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.8009808327308976, + "y": 6.630585568863446 + }, + "prevControl": null, + "nextControl": { + "x": 1.0648639126144392, + "y": 7.087644470479496 + }, + "isLocked": false, + "linkedName": "amp.subwoofer" + }, + { + "anchor": { + "x": 7.813533915227601, + "y": 7.448092043117039 + }, + "prevControl": { + "x": 7.40790774850547, + "y": 7.448092043117039 + }, + "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.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 60.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/amp.toNearest.path b/src/main/deploy/pathplanner/paths/amp.toNearest.path index 5956295..957f632 100644 --- a/src/main/deploy/pathplanner/paths/amp.toNearest.path +++ b/src/main/deploy/pathplanner/paths/amp.toNearest.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 3.188780992946603, - "y": 7.025263281295794 + "x": 2.672476671629519, + "y": 7.003080254377342 }, "prevControl": { - "x": 2.7831548262244725, - "y": 7.025263281295794 + "x": 2.2668505049073886, + "y": 7.003080254377342 }, "nextControl": null, "isLocked": false, @@ -35,48 +35,13 @@ "rotateFast": false }, { - "waypointRelativePos": 0.7, + "waypointRelativePos": 0.4, "rotationDegrees": 0, "rotateFast": true } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "New Event Marker", - "waypointRelativePos": 0.1, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "readyIntake" - } - } - ] - } - } - }, - { - "name": "New Event Marker", - "waypointRelativePos": 0.5, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "intakeNote" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/amp.toSubwoofer.path b/src/main/deploy/pathplanner/paths/amp.toSubwoofer.path index d1f8562..8e890bb 100644 --- a/src/main/deploy/pathplanner/paths/amp.toSubwoofer.path +++ b/src/main/deploy/pathplanner/paths/amp.toSubwoofer.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.188780992946603, - "y": 7.025263281295794 + "x": 2.672476671629519, + "y": 7.003080254377342 }, "prevControl": null, "nextControl": { - "x": 2.6665891639475623, - "y": 7.139329047888865 + "x": 2.1502848426304784, + "y": 7.117146020970414 }, "isLocked": false, "linkedName": "amp.nearestNote" @@ -36,25 +36,7 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "New Event Marker", - "waypointRelativePos": 0.2, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "readyShoot" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/center.stageToSub.path b/src/main/deploy/pathplanner/paths/center.stageToSub.path new file mode 100644 index 0000000..985fb48 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/center.stageToSub.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.5553683061717036, + "y": 4.356431195030721 + }, + "prevControl": null, + "nextControl": { + "x": 2.6607658350837373, + "y": 5.668044888158249 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3374413054104268, + "y": 5.738309907432939 + }, + "prevControl": { + "x": 2.6841875081753006, + "y": 5.480671503425745 + }, + "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.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -36.458444461947124, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/center.toNearest.path b/src/main/deploy/pathplanner/paths/center.toNearest.path index ccad971..055be37 100644 --- a/src/main/deploy/pathplanner/paths/center.toNearest.path +++ b/src/main/deploy/pathplanner/paths/center.toNearest.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 2.409292510892716, + "x": 2.7151677380277848, "y": 5.555088802485298 }, "prevControl": { - "x": 1.409292510892716, + "x": 1.7151677380277848, "y": 5.555088802485298 }, "nextControl": null, @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "New Event Marker", - "waypointRelativePos": 0.8, - "command": { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "intakeNote" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/center.toStage.path b/src/main/deploy/pathplanner/paths/center.toStage.path new file mode 100644 index 0000000..61c31db --- /dev/null +++ b/src/main/deploy/pathplanner/paths/center.toStage.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3633965729469937, + "y": 5.555088802485298 + }, + "prevControl": null, + "nextControl": { + "x": 1.8058747672416873, + "y": 4.883418839590889 + }, + "isLocked": false, + "linkedName": "center.subwoofer" + }, + { + "anchor": { + "x": 2.55, + "y": 4.403274541213847 + }, + "prevControl": { + "x": 1.6716872590663883, + "y": 5.059081387777612 + }, + "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": -42.58049078334365, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/center.toSubwoofer.path b/src/main/deploy/pathplanner/paths/center.toSubwoofer.path index 6a9d2c6..2b7afbf 100644 --- a/src/main/deploy/pathplanner/paths/center.toSubwoofer.path +++ b/src/main/deploy/pathplanner/paths/center.toSubwoofer.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 2.409292510892716, + "x": 2.7151677380277848, "y": 5.555088802485298 }, "prevControl": null, "nextControl": { - "x": 3.409292510892716, + "x": 3.7151677380277848, "y": 5.555088802485298 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/stage.leave.path b/src/main/deploy/pathplanner/paths/stage.leave.path index 0ec2c59..c7745fb 100644 --- a/src/main/deploy/pathplanner/paths/stage.leave.path +++ b/src/main/deploy/pathplanner/paths/stage.leave.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 0.8820136283968368, - "y": 4.2848712274841425 + "x": 0.8473341007203647, + "y": 4.2582606899676225 }, "isLocked": false, "linkedName": "stage.subwoofer" diff --git a/src/main/deploy/pathplanner/paths/stage.sweep.path b/src/main/deploy/pathplanner/paths/stage.sweep.path new file mode 100644 index 0000000..6848b86 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/stage.sweep.path @@ -0,0 +1,84 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7636103146671326, + "y": 4.403274541213847 + }, + "prevControl": null, + "nextControl": { + "x": 0.3175006350012716, + "y": 0.7104198823782273 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.64958220358666, + "y": 1.7332038087756627 + }, + "prevControl": { + "x": 7.5483714599448986, + "y": 0.6658905121898175 + }, + "nextControl": { + "x": 7.778401405590257, + "y": 3.0916608480863177 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.321678780304778, + "y": 4.11050362756931 + }, + "prevControl": { + "x": 7.26312459757587, + "y": 3.560094309917578 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.7, + "rotationDegrees": -125.0, + "rotateFast": false + }, + { + "waypointRelativePos": 0.85, + "rotationDegrees": 180.0, + "rotateFast": true + }, + { + "waypointRelativePos": 1.1, + "rotationDegrees": -143.53044435548432, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -148.29857033049424, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -59.88626684901758, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/stage.sweep2.path b/src/main/deploy/pathplanner/paths/stage.sweep2.path new file mode 100644 index 0000000..10d073c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/stage.sweep2.path @@ -0,0 +1,84 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7636103146671326, + "y": 4.403274541213847 + }, + "prevControl": null, + "nextControl": { + "x": 0.9275620263080739, + "y": 1.5692520971347208 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.836955588319164, + "y": 1.7449146453214444 + }, + "prevControl": { + "x": 7.735744844677402, + "y": 0.6776013487355992 + }, + "nextControl": { + "x": 7.965774790322761, + "y": 3.1033716846320996 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.520763001583063, + "y": 4.403274541213847 + }, + "prevControl": { + "x": 7.4622088188541555, + "y": 3.852865223562115 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.7, + "rotationDegrees": -125.0, + "rotateFast": false + }, + { + "waypointRelativePos": 0.85, + "rotationDegrees": 180.0, + "rotateFast": true + }, + { + "waypointRelativePos": 1.1, + "rotationDegrees": -143.53044435548432, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -148.29857033049424, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -59.88626684901758, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/stage.toNearest.path b/src/main/deploy/pathplanner/paths/stage.toNearest.path index b7933bb..51f692c 100644 --- a/src/main/deploy/pathplanner/paths/stage.toNearest.path +++ b/src/main/deploy/pathplanner/paths/stage.toNearest.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.290889197163011, - "y": 4.124382094918037 + "x": 2.6263652527305066, + "y": 4.272386237080167 }, "prevControl": { - "x": 1.2908891971630112, - "y": 4.124382094918037 + "x": 1.6263652527305066, + "y": 4.272386237080167 }, "nextControl": null, "isLocked": false, @@ -30,48 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "New Event Marker", - "waypointRelativePos": 0.1, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "readyIntake" - } - } - ] - } - } - }, - { - "name": "New Event Marker", - "waypointRelativePos": 0.6, - "command": { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "intakeNote" - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/stage.toSubwoofer.path b/src/main/deploy/pathplanner/paths/stage.toSubwoofer.path index bd29c30..7e32b33 100644 --- a/src/main/deploy/pathplanner/paths/stage.toSubwoofer.path +++ b/src/main/deploy/pathplanner/paths/stage.toSubwoofer.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.290889197163011, - "y": 4.124382094918037 + "x": 2.6263652527305066, + "y": 4.272386237080167 }, "prevControl": null, "nextControl": { - "x": 1.7356097479669974, - "y": 3.8879977331994615 + "x": 2.071085803534493, + "y": 4.036001875361592 }, "isLocked": false, "linkedName": "stage.nearestNote" @@ -30,25 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "New Event Marker", - "waypointRelativePos": 0.1, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "readyShoot" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/java/frc/robot/RobotConstants.java b/src/main/java/frc/robot/RobotConstants.java index 538d9c6..7f64a20 100644 --- a/src/main/java/frc/robot/RobotConstants.java +++ b/src/main/java/frc/robot/RobotConstants.java @@ -51,5 +51,5 @@ public enum Subsystem { Subsystem.SHOOTER, Subsystem.SWERVE); - public static final Set REAL_SUBSYSTEMS = EnumSet.of(Subsystem.ARM, Subsystem.INTAKE, Subsystem.SHOOTER); + public static final Set REAL_SUBSYSTEMS = ALL_SUBSYSTEMS; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e44a5da..2a6c728 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,11 +1,12 @@ package frc.robot; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.lib.Telemetry; import frc.robot.arm.Arm; +import frc.robot.arm.ManualCommand; import frc.robot.auto.Auto; import frc.robot.intake.Intake; import frc.robot.odometry.Odometry; @@ -76,18 +77,17 @@ private void configureDefaultCommands() { private void configureBindings() { driverController.y().onTrue(odometry.tare()); - operatorController.leftBumper().onTrue(superstructure.stow()); + operatorController.leftBumper().onTrue(superstructure.eject()); operatorController.leftTrigger().onTrue(superstructure.intake()); - operatorController.rightBumper().onTrue(superstructure.idle()); + operatorController.rightBumper().onTrue(superstructure.pass()); operatorController.rightTrigger().onTrue(superstructure.shoot()); - operatorController.a().onTrue(superstructure.ampPosition()); - operatorController.b().onTrue(superstructure.ampShoot()); + // operatorController.a().onTrue(superstructure.ampPosition()); + // operatorController.b().onTrue(superstructure.ampShoot()); operatorController.x().onTrue(superstructure.stow()); - operatorController.y().whileTrue(arm.setVoltage(() -> 3 * MathUtil.applyDeadband(-operatorController.getLeftY(), 0.1))); - operatorController.povUp().onTrue(superstructure.eject()); + operatorController.y().whileTrue(superstructure.manualControl().andThen(new ManualCommand(operatorController::getLeftY))).onFalse(Commands.runOnce(() -> superstructure.setSetpoint(superstructure.getState()))); } /** diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java index 2b6f295..1894a1f 100644 --- a/src/main/java/frc/robot/arm/Arm.java +++ b/src/main/java/frc/robot/arm/Arm.java @@ -5,12 +5,9 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import frc.lib.Subsystem; import frc.lib.Telemetry; import frc.robot.arm.ShoulderMotorIO.ShoulderMotorIOValues; -import java.util.function.DoubleSupplier; /** Subsystem class for the arm subsystem. */ public class Arm extends Subsystem { @@ -83,7 +80,7 @@ public void setSetpoint(double positionRotations, double velocityRotationsPerSec shoulderMotor.setSetpoint(positionRotations, velocityRotationsPerSecond); } - public Command setVoltage(DoubleSupplier volts) { - return Commands.run(() -> shoulderMotor.setVoltage(volts.getAsDouble())); + public void setVoltage(double volts) { + shoulderMotor.setVoltage(volts); } } diff --git a/src/main/java/frc/robot/arm/ManualCommand.java b/src/main/java/frc/robot/arm/ManualCommand.java new file mode 100644 index 0000000..b5d3461 --- /dev/null +++ b/src/main/java/frc/robot/arm/ManualCommand.java @@ -0,0 +1,51 @@ +package frc.robot.arm; + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.superstructure.SuperstructureConstants.ShoulderAngleConstants; + +public class ManualCommand extends Command { + + private final Arm arm; + + private final DoubleSupplier joystick; + + public ManualCommand(DoubleSupplier joystick) { + arm = Arm.getInstance(); + + this.joystick = joystick; + + addRequirements(arm); + } + + @Override + public void execute() { + double voltageScalar = 12.0; + + double percent = -joystick.getAsDouble(); + + double volts = percent * voltageScalar; + + double positionRotations = arm.getMeasuredShoulderState().position; + + if (positionRotations > ShoulderAngleConstants.AMP.getRotations() && volts > 0) { + volts = 0; + } + + if (positionRotations < ShoulderAngleConstants.STOW.getRotations() && volts < 0) { + volts = 0; + } + + volts = MathUtil.clamp(volts, -12.0, 6.0); + + arm.setVoltage(volts); + } + + @Override + public void end(boolean interrupted) { + arm.setVoltage(0); + } + +} diff --git a/src/main/java/frc/robot/arm/ShoulderMotorIOSim.java b/src/main/java/frc/robot/arm/ShoulderMotorIOSim.java index f513b91..5a05447 100644 --- a/src/main/java/frc/robot/arm/ShoulderMotorIOSim.java +++ b/src/main/java/frc/robot/arm/ShoulderMotorIOSim.java @@ -46,5 +46,6 @@ public void setSetpoint(double positionRotations, double velocityRotationsPerSec @Override public void setVoltage(double volts) { this.inputVoltage = volts; + this.positionRotations += volts / 1000; } } diff --git a/src/main/java/frc/robot/arm/ShoulderMotorIOTalonFX.java b/src/main/java/frc/robot/arm/ShoulderMotorIOTalonFX.java index ffdeeae..8ef544d 100644 --- a/src/main/java/frc/robot/arm/ShoulderMotorIOTalonFX.java +++ b/src/main/java/frc/robot/arm/ShoulderMotorIOTalonFX.java @@ -7,7 +7,6 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; - import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.util.Units; @@ -33,9 +32,10 @@ public ShoulderMotorIOTalonFX() { cancoder = new CANcoder(52); - feedback = new PIDController(0, 0, 0); + feedback = new PIDController(20, 0, 0); - feedforward = new ArmFeedforward(0.14, 1.29327, 0); + feedforward = new ArmFeedforward(0.14, 0.45, 4.0); + // feedforward = new ArmFeedforward(0.14, 0, 0); } @Override @@ -59,7 +59,7 @@ public void configure() { cancoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive; - cancoderConfig.MagnetSensor.MagnetOffset = Units.degreesToRotations(-114.12509); + cancoderConfig.MagnetSensor.MagnetOffset = Units.degreesToRotations(-146.77) + Units.degreesToRotations(-27.07); Configurator.configureCANcoder(cancoder.getConfigurator(), cancoderConfig); } diff --git a/src/main/java/frc/robot/auto/Auto.java b/src/main/java/frc/robot/auto/Auto.java index 78210c1..18cef07 100644 --- a/src/main/java/frc/robot/auto/Auto.java +++ b/src/main/java/frc/robot/auto/Auto.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import frc.lib.AllianceFlipHelper; import frc.lib.Subsystem; import frc.lib.Telemetry; @@ -70,12 +71,9 @@ private Auto() { AllianceFlipHelper::shouldFlip, swerve); - NamedCommands.registerCommand("home", superstructure.stow()); NamedCommands.registerCommand("stow", superstructure.stow()); - NamedCommands.registerCommand("readyIntake", superstructure.intake()); - NamedCommands.registerCommand("intakeNote", superstructure.intake()); - NamedCommands.registerCommand("readyShoot", superstructure.shoot()); - NamedCommands.registerCommand("shootNote", superstructure.shoot()); + NamedCommands.registerCommand("shoot", Commands.deadline(Commands.waitSeconds(3.0), superstructure.shoot())); + NamedCommands.registerCommand("intake", superstructure.intake()); autoChooser = AutoBuilder.buildAutoChooser(); } diff --git a/src/main/java/frc/robot/intake/RollerMotorIOTalonFX.java b/src/main/java/frc/robot/intake/RollerMotorIOTalonFX.java index eacbaa7..dfba08e 100644 --- a/src/main/java/frc/robot/intake/RollerMotorIOTalonFX.java +++ b/src/main/java/frc/robot/intake/RollerMotorIOTalonFX.java @@ -27,10 +27,15 @@ public void configure() { config.Feedback.SensorToMechanismRatio = 24.0 / 16.0; - config.MotorOutput.NeutralMode = NeutralModeValue.Coast; + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + config.CurrentLimits.SupplyCurrentLimit = 30; + config.CurrentLimits.SupplyCurrentThreshold = 30; + config.CurrentLimits.SupplyTimeThreshold = 0.1; + config.CurrentLimits.SupplyCurrentLimitEnable = true; + config.CurrentLimits.StatorCurrentLimit = 40; config.CurrentLimits.StatorCurrentLimitEnable = true; diff --git a/src/main/java/frc/robot/shooter/FlywheelMotorIOTalonFX.java b/src/main/java/frc/robot/shooter/FlywheelMotorIOTalonFX.java index e9b1368..db42d83 100644 --- a/src/main/java/frc/robot/shooter/FlywheelMotorIOTalonFX.java +++ b/src/main/java/frc/robot/shooter/FlywheelMotorIOTalonFX.java @@ -32,7 +32,7 @@ public void configure() { config.Feedback.SensorToMechanismRatio = 36.0 / 16.0; - config.MotorOutput.NeutralMode = NeutralModeValue.Coast; + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; diff --git a/src/main/java/frc/robot/shooter/SerializerMotorIOTalonFX.java b/src/main/java/frc/robot/shooter/SerializerMotorIOTalonFX.java index addbe26..15ff9a8 100644 --- a/src/main/java/frc/robot/shooter/SerializerMotorIOTalonFX.java +++ b/src/main/java/frc/robot/shooter/SerializerMotorIOTalonFX.java @@ -32,7 +32,7 @@ public void configure() { config.Feedback.SensorToMechanismRatio = 36.0 / 16.0; - config.MotorOutput.NeutralMode = NeutralModeValue.Coast; + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; diff --git a/src/main/java/frc/robot/shooter/ShooterConstants.java b/src/main/java/frc/robot/shooter/ShooterConstants.java index 5554366..e42042e 100644 --- a/src/main/java/frc/robot/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/shooter/ShooterConstants.java @@ -1,6 +1,7 @@ package frc.robot.shooter; import edu.wpi.first.math.filter.SlewRateLimiter; +import frc.lib.MotionProfileCalculator; /** Constants for the shooter subsystem. */ public class ShooterConstants { @@ -11,7 +12,7 @@ public static class SerializerConstants { public static final double INTAKE_VELOCITY = 34; /** Velocity to apply while pulling in rotations per second. */ - public static final double PULL_VELOCITY = -5; + public static final double PULL_VELOCITY = -20; /** Velocity to apply while serializing in rotations per second. */ public static final double SERIALIZE_VELOCITY = 20; @@ -28,16 +29,22 @@ public static class FlywheelConstants { /** Velocity to apply while shooting into the speaker in rotations per second. */ public static final double SPEAKER_VELOCITY = 44; + /** Velocity to apply while passing in rotations per second. */ + public static final double PASS_VELOCITY = 12; + /** Velocity to apply while shooting into the amp in rotations per second. */ public static final double AMP_VELOCITY = 20; /** Maximum speed in rotations per second. */ public static final double MAXIMUM_SPEED = 46.711; - /** Speed tolerance in rotations per second. */ - public static final double SPEED_TOLERANCE = 1.0; + /** Maximum acceleration in rotations per second per second. */ + public static final double MAXIMUM_ACCELERATION = MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.25); /** Acceleration limiter. */ - public static final SlewRateLimiter ACCELERATION_LIMITER = new SlewRateLimiter(22); + public static final SlewRateLimiter ACCELERATION_LIMITER = new SlewRateLimiter(MAXIMUM_ACCELERATION); + + /** Speed tolerance in rotations per second. */ + public static final double SPEED_TOLERANCE = 2.5; } } diff --git a/src/main/java/frc/robot/superstructure/Superstructure.java b/src/main/java/frc/robot/superstructure/Superstructure.java index 94399bf..0ea5cd0 100644 --- a/src/main/java/frc/robot/superstructure/Superstructure.java +++ b/src/main/java/frc/robot/superstructure/Superstructure.java @@ -35,10 +35,10 @@ private Superstructure() { intake = Intake.getInstance(); shooter = Shooter.getInstance(); - setPosition(SuperstructureState.STOW); + setPosition(SuperstructureState.INITIAL); - setpoint = SuperstructureState.STOW; - goal = SuperstructureState.STOW; + setpoint = SuperstructureState.INITIAL; + goal = SuperstructureState.INITIAL; } /** @@ -126,6 +126,7 @@ private void updateMeasurement() { measurement = new SuperstructureState( measuredShoulderState, + false, measuredIntakeRollerVelocity, measuredShooterFlywheelVelocity, false, @@ -140,11 +141,17 @@ public SuperstructureState getState() { return measurement; } + public void setSetpoint(SuperstructureState setpoint) { + this.setpoint = setpoint; + } + private void updateSetpoint() { setpoint = SuperstructureState.nextSetpoint(setpoint, goal); - arm.setSetpoint( - setpoint.shoulderAngleRotations().position, setpoint.shoulderAngleRotations().velocity); + if (setpoint.shoulderManual() == false) { + arm.setSetpoint( + setpoint.shoulderAngleRotations().position, setpoint.shoulderAngleRotations().velocity); + } shooter.setSetpoint( setpoint.flywheelVelocityRotationsPerSecond(), @@ -168,7 +175,7 @@ public boolean at(SuperstructureState goal) { } private Command to(SuperstructureState goal) { - return new ToGoal(goal); + return run(() -> setGoal(goal)).until(() -> at(goal)).raceWith(Commands.waitSeconds(1.0)); } public Command stow() { @@ -176,7 +183,7 @@ public Command stow() { } public Command intake() { - return to(SuperstructureState.INTAKE_POSITION).andThen(to(SuperstructureState.INTAKE)); + return to(SuperstructureState.INTAKE); } public Command idle() { @@ -191,6 +198,10 @@ public Command shoot() { return pull().andThen(to(SuperstructureState.SPEAKER_SPIN).andThen(to(SuperstructureState.SPEAKER_SHOOT))); } + public Command pass() { + return pull().andThen(to(SuperstructureState.PASS_SPIN).andThen(to(SuperstructureState.PASS_SHOOT))); + } + public Command ampPosition() { return to(SuperstructureState.AMP_POSITION); } @@ -200,6 +211,10 @@ public Command ampShoot() { } public Command eject() { - return to(SuperstructureState.EJECT); + return to(SuperstructureState.EJECT_POSITION).andThen(to(SuperstructureState.EJECT)); + } + + public Command manualControl() { + return to(SuperstructureState.MANUAL); } } diff --git a/src/main/java/frc/robot/superstructure/SuperstructureConstants.java b/src/main/java/frc/robot/superstructure/SuperstructureConstants.java index 33d2e00..d8a9506 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureConstants.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureConstants.java @@ -2,22 +2,27 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; import frc.lib.MotionProfileCalculator; public class SuperstructureConstants { public static class ShoulderAngleConstants { - public static final Rotation2d STOW = Rotation2d.fromDegrees(-19.379); + public static final Rotation2d INITIAL = Rotation2d.fromDegrees(-26.45); + + public static final Rotation2d STOW = Rotation2d.fromDegrees(-26.45); + + public static final Rotation2d SHOOT = Rotation2d.fromDegrees(-15); public static final Rotation2d EJECT = Rotation2d.fromDegrees(0); public static final Rotation2d AMP = Rotation2d.fromDegrees(90); /** Tolerance of the shoulder joint. */ - public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(5.0); + public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(2.0); /** Maximum speed of the shoulder joint in rotations per second. */ - public static final double MAXIMUM_SPEED = 0.5; + public static final double MAXIMUM_SPEED = Units.degreesToRotations(60.0); /** Maximum acceleration of the shoulder joint in rotations per second per second. */ public static final double MAXIMUM_ACCELERATION = diff --git a/src/main/java/frc/robot/superstructure/SuperstructureState.java b/src/main/java/frc/robot/superstructure/SuperstructureState.java index ca340cc..354a7e0 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureState.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureState.java @@ -13,60 +13,85 @@ /** Represents the state of the superstructure. */ public record SuperstructureState( State shoulderAngleRotations, + boolean shoulderManual, double rollerVelocityRotationsPerSecond, double flywheelVelocityRotationsPerSecond, boolean rampFlywheelVelocity, double serializerVelocityRotationsPerSecond) { + public static final SuperstructureState INITIAL = + new SuperstructureState(ShoulderAngleConstants.INITIAL, false, 0, 0, false, 0); + public static final SuperstructureState STOW = - new SuperstructureState(ShoulderAngleConstants.STOW, 0, 0, false, 0); + new SuperstructureState(ShoulderAngleConstants.STOW, false, 0, 0, false, 0); public static final SuperstructureState INTAKE_POSITION = - new SuperstructureState(ShoulderAngleConstants.STOW, 0, 0, false, 0); + new SuperstructureState(ShoulderAngleConstants.STOW, false, 0, 0, false, 0); public static final SuperstructureState INTAKE = new SuperstructureState( ShoulderAngleConstants.STOW, + false, RollerConstants.INTAKE_VELOCITY, 0, false, SerializerConstants.INTAKE_VELOCITY); - public static final SuperstructureState PULL = new SuperstructureState(ShoulderAngleConstants.STOW, 0, 0, false, SerializerConstants.PULL_VELOCITY); + public static final SuperstructureState PULL = new SuperstructureState(ShoulderAngleConstants.STOW, false, 0, 0, false, SerializerConstants.PULL_VELOCITY); - public static final SuperstructureState EJECT = new SuperstructureState(ShoulderAngleConstants.EJECT, 0, 0, false, SerializerConstants.PULL_VELOCITY); + public static final SuperstructureState EJECT_POSITION = new SuperstructureState(ShoulderAngleConstants.EJECT, false, 0, 0, false, 0); + + public static final SuperstructureState EJECT = new SuperstructureState(ShoulderAngleConstants.EJECT, false, 0, 0, false, SerializerConstants.PULL_VELOCITY); public static final SuperstructureState SPEAKER_SPIN = new SuperstructureState( - ShoulderAngleConstants.STOW, 0, FlywheelConstants.SPEAKER_VELOCITY, true, 0); + ShoulderAngleConstants.SHOOT, false, 0, FlywheelConstants.SPEAKER_VELOCITY, true, 0); public static final SuperstructureState SPEAKER_SHOOT = new SuperstructureState( - ShoulderAngleConstants.STOW, + ShoulderAngleConstants.SHOOT, + false, 0, FlywheelConstants.SPEAKER_VELOCITY, false, SerializerConstants.SERIALIZE_VELOCITY); + public static final SuperstructureState PASS_SPIN = + new SuperstructureState( + ShoulderAngleConstants.STOW, false, 0, FlywheelConstants.PASS_VELOCITY, false, 0); + + public static final SuperstructureState PASS_SHOOT = + new SuperstructureState( + ShoulderAngleConstants.STOW, + false, + 0, + FlywheelConstants.PASS_VELOCITY, + false, + SerializerConstants.SERIALIZE_VELOCITY); + public static final SuperstructureState AMP_POSITION = - new SuperstructureState(ShoulderAngleConstants.AMP, 0, 0, false, 0); + new SuperstructureState(ShoulderAngleConstants.AMP, false, 0, 0, false, 0); public static final SuperstructureState AMP_SPIN = new SuperstructureState( - ShoulderAngleConstants.AMP, 0, FlywheelConstants.AMP_VELOCITY, false, 0); + ShoulderAngleConstants.AMP, false, 0, FlywheelConstants.AMP_VELOCITY, false, 0); public static final SuperstructureState AMP_SHOOT = new SuperstructureState( ShoulderAngleConstants.AMP, + false, 0, FlywheelConstants.AMP_VELOCITY, false, SerializerConstants.SERIALIZE_VELOCITY); + public static final SuperstructureState MANUAL = new SuperstructureState(ShoulderAngleConstants.STOW, true, 0, 0, false, 0); + /** * Creates a new superstructure state. * * @param shoulderAngleRotations + * @param shoulderManual * @param rollerVelocityRotationsPerSecond * @param flywheelVelocityRotationsPerSecond * @param rampFlywheelVelocity @@ -74,6 +99,7 @@ public record SuperstructureState( */ public SuperstructureState { Objects.requireNonNull(shoulderAngleRotations); + Objects.requireNonNull(shoulderManual); Objects.requireNonNull(rollerVelocityRotationsPerSecond); Objects.requireNonNull(flywheelVelocityRotationsPerSecond); Objects.requireNonNull(rampFlywheelVelocity); @@ -84,6 +110,7 @@ public record SuperstructureState( * Creates a new superstructure state. * * @param shoulderAngle + * @param shoulderManual * @param rollerVelocityRotationsPerSecond * @param flywheelVelocityRotationsPerSecond * @param rampFlywheelVelocity @@ -91,12 +118,14 @@ public record SuperstructureState( */ public SuperstructureState( Rotation2d shoulderAngle, + boolean shoulderManual, double rollerVelocityRotationsPerSecond, double flywheelVelocityRotationsPerSecond, boolean rampFlywheelVelocity, double serializerVelocityRotationsPerSecond) { this( new State(shoulderAngle.getRotations(), 0), + shoulderManual, rollerVelocityRotationsPerSecond, flywheelVelocityRotationsPerSecond, rampFlywheelVelocity, @@ -193,6 +222,7 @@ public static SuperstructureState nextSetpoint( return new SuperstructureState( nextShoulderSetpoint, + goal.shoulderManual(), goal.rollerVelocityRotationsPerSecond(), nextFlywheelVelocitySetpoint, goal.rampFlywheelVelocity(), diff --git a/src/main/java/frc/robot/superstructure/ToGoal.java b/src/main/java/frc/robot/superstructure/ToGoal.java deleted file mode 100644 index 3f42158..0000000 --- a/src/main/java/frc/robot/superstructure/ToGoal.java +++ /dev/null @@ -1,46 +0,0 @@ -package frc.robot.superstructure; - -import edu.wpi.first.wpilibj2.command.Command; -import java.util.LinkedList; -import java.util.Queue; - -public class ToGoal extends Command { - - private final Superstructure superstructure; - - private final SuperstructureState goal; - - private SuperstructureGoals goals; - - public ToGoal(SuperstructureState goal) { - this.superstructure = Superstructure.getInstance(); - - this.goal = goal; - - Queue empty = new LinkedList(); - goals = new SuperstructureGoals(empty); - - addRequirements(this.superstructure); - } - - @Override - public void initialize() { - goals = new SuperstructureGoals(superstructure.getState(), this.goal); - } - - @Override - public void execute() { - SuperstructureState goal = goals.get(); - - if (superstructure.at(goal)) { - goal = goals.next(); - } - - superstructure.setGoal(goal); - } - - @Override - public boolean isFinished() { - return superstructure.at(goal); - } -} diff --git a/src/main/java/frc/robot/swerve/DriveRequest.java b/src/main/java/frc/robot/swerve/DriveRequest.java index 3eff277..bdea0e4 100644 --- a/src/main/java/frc/robot/swerve/DriveRequest.java +++ b/src/main/java/frc/robot/swerve/DriveRequest.java @@ -98,7 +98,7 @@ public boolean isDrifting() { public Rotation2d omega() { if (Math.abs(this.rotation().getY()) < 0.1) return new Rotation2d(); - return SwerveConstants.MAXIMUM_ROTATION_SPEED.times(this.rotation().getY() * 0.75); + return SwerveConstants.MAXIMUM_ROTATION_SPEED.times(this.rotation().getY()); } public Rotation2d driverHeading() { diff --git a/src/main/java/frc/robot/swerve/SteerMotorIOTalonFXPIDF.java b/src/main/java/frc/robot/swerve/SteerMotorIOTalonFXPIDF.java index 99e2ed2..feae7da 100644 --- a/src/main/java/frc/robot/swerve/SteerMotorIOTalonFXPIDF.java +++ b/src/main/java/frc/robot/swerve/SteerMotorIOTalonFXPIDF.java @@ -43,7 +43,7 @@ public void configure() { talonFXPIDFConfig.ClosedLoopGeneral.ContinuousWrap = true; talonFXPIDFConfig.CurrentLimits = - new MotorCurrentLimits(40.0, 20.0, 30.0, 0.1).asCurrentLimitsConfigs(); + new MotorCurrentLimits(35.0, 15.0, 25.0, 0.1).asCurrentLimitsConfigs(); talonFXPIDFConfig.Feedback.SensorToMechanismRatio = MK4iConstants.STEER_GEARING; diff --git a/src/main/java/frc/robot/swerve/SwerveConstants.java b/src/main/java/frc/robot/swerve/SwerveConstants.java index c2266be..50c6f89 100644 --- a/src/main/java/frc/robot/swerve/SwerveConstants.java +++ b/src/main/java/frc/robot/swerve/SwerveConstants.java @@ -67,28 +67,28 @@ public static class MK4iConstants { new SwerveModuleConfig( new SwerveModuleCAN(16, 8, 24, SWERVE_BUS), new Translation2d(X_OFFSET, Y_OFFSET), - Rotation2d.fromRotations(-0.385742)); + Rotation2d.fromRotations(-0.060303)); /** Module configuration for the north east swerve module. */ public static final SwerveModuleConfig NORTH_EAST_MODULE_CONFIG = new SwerveModuleConfig( new SwerveModuleCAN(18, 16, 30, SWERVE_BUS), new Translation2d(X_OFFSET, -Y_OFFSET), - Rotation2d.fromRotations(-0.229004)); + Rotation2d.fromRotations(0.180908)); /** Module configuration for the south east swerve module. */ public static final SwerveModuleConfig SOUTH_EAST_MODULE_CONFIG = new SwerveModuleConfig( new SwerveModuleCAN(22,12, 26, SWERVE_BUS), new Translation2d(-X_OFFSET, -Y_OFFSET), - Rotation2d.fromRotations(-0.096436)); + Rotation2d.fromRotations(0.273438)); /** Module configuration for the south west swerve module. */ public static final SwerveModuleConfig SOUTH_WEST_MODULE_CONFIG = new SwerveModuleConfig( new SwerveModuleCAN(20, 10, 28, SWERVE_BUS), new Translation2d(-X_OFFSET, Y_OFFSET), - Rotation2d.fromRotations(-0.448242)); + Rotation2d.fromRotations(0.214111)); /** * Calculates the maximum attainable open loop speed in meters per second.