diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index b3758f6..8862b85 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -1,12 +1,12 @@ { - "robotWidth": 0.7, - "robotLength": 0.7, + "robotWidth": 0.8, + "robotLength": 0.8, "holonomicMode": true, "pathFolders": [], "autoFolders": [], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 3.0, + "defaultMaxVel": 4.6, + "defaultMaxAccel": 5.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, - "maxModuleSpeed": 4.5 + "maxModuleSpeed": 4.6 } \ No newline at end of file diff --git a/build.gradle b/build.gradle index d25e5bd..72793ee 100644 --- a/build.gradle +++ b/build.gradle @@ -1,7 +1,7 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1" + id "edu.wpi.first.GradleRIO" version "2024.2.1" id "idea" } diff --git a/src/main/deploy/pathplanner/autos/2NoteMid.auto b/src/main/deploy/pathplanner/autos/2NoteMid.auto new file mode 100644 index 0000000..63c3f4f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/2NoteMid.auto @@ -0,0 +1,74 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3110120765696076, + "y": 5.513288844759509 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "named", + "data": { + "name": "intakeOut" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Mid To Close Mid" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Close Mid to Mid" + } + }, + { + "type": "named", + "data": { + "name": "intakeIn" + } + }, + { + "type": "named", + "data": { + "name": "stopIntaking" + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3NoteMid.Note1.auto b/src/main/deploy/pathplanner/autos/3NoteMid.Note1.auto new file mode 100644 index 0000000..08e2d85 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3NoteMid.Note1.auto @@ -0,0 +1,163 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3110120765696076, + "y": 5.513288844759509 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "named", + "data": { + "name": "intakeOut" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Mid To Close Mid" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeIn" + } + }, + { + "type": "path", + "data": { + "pathName": "Close Mid to Mid" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.6 + } + }, + { + "type": "named", + "data": { + "name": "stopIntaking" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "named", + "data": { + "name": "intakeOut" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Mid To Close Top" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeIn" + } + }, + { + "type": "path", + "data": { + "pathName": "Close Top To Mid" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.6 + } + }, + { + "type": "named", + "data": { + "name": "stopIntaking" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3NoteMid.Note3.auto b/src/main/deploy/pathplanner/autos/3NoteMid.Note3.auto new file mode 100644 index 0000000..36f626b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3NoteMid.Note3.auto @@ -0,0 +1,163 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3110120765696076, + "y": 5.513288844759509 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "named", + "data": { + "name": "intakeOut" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Mid To Close Mid" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeIn" + } + }, + { + "type": "path", + "data": { + "pathName": "Close Mid to Mid" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.75 + } + }, + { + "type": "named", + "data": { + "name": "stopIntaking" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "named", + "data": { + "name": "intakeOut" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SpeakerC.Note3" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeIn" + } + }, + { + "type": "path", + "data": { + "pathName": "Note3.SpeakerC" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.75 + } + }, + { + "type": "named", + "data": { + "name": "stopIntaking" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/4Note.auto b/src/main/deploy/pathplanner/autos/4Note.auto new file mode 100644 index 0000000..1aa5dd0 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/4Note.auto @@ -0,0 +1,226 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3110120765696076, + "y": 5.513288844759509 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "named", + "data": { + "name": "intakeOut" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Mid To Close Mid" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeIn" + } + }, + { + "type": "path", + "data": { + "pathName": "Close Mid to Mid" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.75 + } + }, + { + "type": "named", + "data": { + "name": "stopIntaking" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "named", + "data": { + "name": "intakeOut" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Mid To Close Top" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeIn" + } + }, + { + "type": "path", + "data": { + "pathName": "Close Top To Mid" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.75 + } + }, + { + "type": "named", + "data": { + "name": "stopIntaking" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "named", + "data": { + "name": "intakeOut" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SpeakerC.Note3" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeIn" + } + }, + { + "type": "path", + "data": { + "pathName": "Note3.SpeakerC" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.75 + } + }, + { + "type": "named", + "data": { + "name": "stopIntaking" + } + } + ] + } + } + ] + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Demo1.auto b/src/main/deploy/pathplanner/autos/Amp.auto similarity index 61% rename from src/main/deploy/pathplanner/autos/Demo1.auto rename to src/main/deploy/pathplanner/autos/Amp.auto index 09f1cf9..5d6cd4d 100644 --- a/src/main/deploy/pathplanner/autos/Demo1.auto +++ b/src/main/deploy/pathplanner/autos/Amp.auto @@ -2,31 +2,31 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.3763132980811417, - "y": 5.569261320340823 + "x": 1.82409310273166, + "y": 7.696215392430785 }, - "rotation": 180.0 + "rotation": -90.0 }, "command": { "type": "sequential", "data": { "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "Mid to amp" + "name": "log" } }, { - "type": "wait", + "type": "named", "data": { - "waitTime": 2.0 + "name": "shoot" } }, { "type": "path", "data": { - "pathName": "Amp to mid note" + "pathName": "Amp" } } ] diff --git a/src/main/deploy/pathplanner/autos/Demo2.auto b/src/main/deploy/pathplanner/autos/BotToCentBot.auto similarity index 58% rename from src/main/deploy/pathplanner/autos/Demo2.auto rename to src/main/deploy/pathplanner/autos/BotToCentBot.auto index 7a80bde..ea12cb3 100644 --- a/src/main/deploy/pathplanner/autos/Demo2.auto +++ b/src/main/deploy/pathplanner/autos/BotToCentBot.auto @@ -2,31 +2,31 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.7792735585471171, - "y": 4.440483062784308 + "x": 0.8539869673657066, + "y": 4.508504473180682 }, - "rotation": 120.00492087082401 + "rotation": -58.751279204501785 }, "command": { "type": "sequential", "data": { "commands": [ { - "type": "path", + "type": "named", "data": { - "pathName": "Source side to source note" + "name": "log" } }, { - "type": "wait", + "type": "named", "data": { - "waitTime": 0.5 + "name": "shoot" } }, { "type": "path", "data": { - "pathName": "Source note to source side" + "pathName": "Bot to mid bot" } } ] diff --git a/src/main/deploy/pathplanner/autos/MidToCentTop.auto b/src/main/deploy/pathplanner/autos/MidToCentTop.auto new file mode 100644 index 0000000..6801329 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/MidToCentTop.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.309243119380694, + "y": 5.492445188825977 + }, + "rotation": 0.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "log" + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Mid to center top" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SpeakerS.Note8.auto b/src/main/deploy/pathplanner/autos/SpeakerS.Note8.auto new file mode 100644 index 0000000..c61b9c3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/SpeakerS.Note8.auto @@ -0,0 +1,50 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.6419522005022541, + "y": 4.377819358446314 + }, + "rotation": -59.37446728130534 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "stopShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "SpeakerS.Note8" + } + } + ] + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TopToCentTop.auto b/src/main/deploy/pathplanner/autos/TopToCentTop.auto new file mode 100644 index 0000000..931d7ba --- /dev/null +++ b/src/main/deploy/pathplanner/autos/TopToCentTop.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7606160666866788, + "y": 6.6047521185951465 + }, + "rotation": 59.381394591090626 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "log" + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "top to center top" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/safe.auto b/src/main/deploy/pathplanner/autos/safe.auto new file mode 100644 index 0000000..3b6e6ef --- /dev/null +++ b/src/main/deploy/pathplanner/autos/safe.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.8539869673657066, + "y": 4.508504473180682 + }, + "rotation": -59.34933204294715 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "log" + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Safe Auto" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/test.auto b/src/main/deploy/pathplanner/autos/test.auto new file mode 100644 index 0000000..c59ff46 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/test.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3110120765696076, + "y": 5.513288844759509 + }, + "rotation": 0.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "log" + } + }, + { + "type": "named", + "data": { + "name": "shoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Mid To Close Mid" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Amp to mid note.path b/src/main/deploy/pathplanner/paths/Amp to mid note.path deleted file mode 100644 index 6975456..0000000 --- a/src/main/deploy/pathplanner/paths/Amp to mid note.path +++ /dev/null @@ -1,102 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.8024918101118255, - "y": 7.895182457031582 - }, - "prevControl": null, - "nextControl": { - "x": 1.7651768263889298, - "y": 6.747746707611961 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.9173805620338513, - "y": 5.6065763040617 - }, - "prevControl": { - "x": 1.8167440858156032, - "y": 6.612941065997554 - }, - "nextControl": { - "x": 1.9267093079660895, - "y": 5.513288844762175 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.402475350405246, - "y": 5.6065763040617 - }, - "prevControl": { - "x": 2.589050269009629, - "y": 5.625233795922139 - }, - "nextControl": { - "x": 2.215900431800863, - "y": 5.587918812201261 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.2281684392428616, - "y": 5.6065763040617 - }, - "prevControl": { - "x": 1.1582028447662178, - "y": 5.59724755813148 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.85, - "rotationDegrees": 180.0, - "rotateFast": true - } - ], - "constraintZones": [ - { - "name": "New Constraints Zone", - "minWaypointRelativePos": 1.1500000000000001, - "maxWaypointRelativePos": 1.75, - "constraints": { - "maxVelocity": 0.5, - "maxAcceleration": 1.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - } - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 90.0, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Amp.path b/src/main/deploy/pathplanner/paths/Amp.path new file mode 100644 index 0000000..0359dbd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Amp.path @@ -0,0 +1,74 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.82409310273166, + "y": 7.696215392430785 + }, + "prevControl": null, + "nextControl": { + "x": 2.8968988847068604, + "y": 7.546955457547279 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9155563765672983, + "y": 7.5749416953379365 + }, + "prevControl": { + "x": 2.432429726634338, + "y": 7.638826211031551 + }, + "nextControl": { + "x": 4.044334634123814, + "y": 7.42568176045443 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.572424781213199, + "y": 7.388366776733554 + }, + "prevControl": { + "x": 7.072424781213199, + "y": 7.388366776733554 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0, + "rotationDegrees": -90.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.6, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -90.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bot to mid bot.path b/src/main/deploy/pathplanner/paths/Bot to mid bot.path new file mode 100644 index 0000000..c5bd8f4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bot to mid bot.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.8539869673657066, + "y": 4.508504473180682 + }, + "prevControl": null, + "nextControl": { + "x": 1.8539869673657114, + "y": 4.508504473180682 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.1022699648261556, + "y": 1.4979395969525402 + }, + "prevControl": { + "x": 1.2459786562758262, + "y": 2.0144327671892466 + }, + "nextControl": { + "x": 3.0274679511791946, + "y": 0.939883668676104 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.497794813771446, + "y": 0.7462996744175311 + }, + "prevControl": { + "x": 6.9977948137714465, + "y": 0.7462996744175311 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.6, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -59.2810957359708, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Mid to amp.path b/src/main/deploy/pathplanner/paths/Close Mid to Mid.path similarity index 63% rename from src/main/deploy/pathplanner/paths/Mid to amp.path rename to src/main/deploy/pathplanner/paths/Close Mid to Mid.path index a9e23a7..fb736aa 100644 --- a/src/main/deploy/pathplanner/paths/Mid to amp.path +++ b/src/main/deploy/pathplanner/paths/Close Mid to Mid.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.3763132980811417, - "y": 5.569261320340823 + "x": 2.66, + "y": 5.55 }, "prevControl": null, "nextControl": { - "x": 1.9267093079640694, - "y": 6.306232248828135 + "x": 1.6600000000000001, + "y": 5.55 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.8054356108712217, - "y": 7.649571662779689 + "x": 1.3533001663498863, + "y": 5.55 }, "prevControl": { - "x": 1.8427505945920981, - "y": 6.604752118595146 + "x": 2.295549686080821, + "y": 5.55 }, "nextControl": null, "isLocked": false, @@ -32,21 +32,21 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.6, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 90.0, + "rotation": 0, "rotateFast": false }, "reversed": false, "folder": null, "previewStartingState": { - "rotation": 180.0, + "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Close Top To Mid.path b/src/main/deploy/pathplanner/paths/Close Top To Mid.path new file mode 100644 index 0000000..d3d6f7a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Close Top To Mid.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.4864340637772187, + "y": 6.991800676923469 + }, + "prevControl": null, + "nextControl": { + "x": 2.3864340637772186, + "y": 6.991800676923469 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.123258272103077, + "y": 6.991800676923469 + }, + "prevControl": { + "x": 2.287495365622818, + "y": 7.065463266561544 + }, + "nextControl": { + "x": 1.6616994951342325, + "y": 6.784785233255618 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.323928801703758, + "y": 5.551187918118234 + }, + "prevControl": { + "x": 2.323928801703758, + "y": 5.551187918118234 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.6, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Mid To Close Mid.path b/src/main/deploy/pathplanner/paths/Mid To Close Mid.path new file mode 100644 index 0000000..c9aef80 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Mid To Close Mid.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.28, + "y": 5.55 + }, + "prevControl": null, + "nextControl": { + "x": 1.8124744886418784, + "y": 5.55 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.5868974814872714, + "y": 5.55 + }, + "prevControl": { + "x": 2.2190222956808627, + "y": 5.55 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.6, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0.0, + "velocity": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Mid To Close Top.path b/src/main/deploy/pathplanner/paths/Mid To Close Top.path new file mode 100644 index 0000000..86d6580 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Mid To Close Top.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.289171939085591, + "y": 5.546051033026494 + }, + "prevControl": null, + "nextControl": { + "x": 2.598379014939848, + "y": 5.690535017433672 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0728986001800274, + "y": 6.991800676923469 + }, + "prevControl": { + "x": 1.4545163398986403, + "y": 6.80503451422615 + }, + "nextControl": { + "x": 2.1195423298311233, + "y": 7.0058881935945685 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.5124985608445702, + "y": 6.991800676923469 + }, + "prevControl": { + "x": 2.41249856084457, + "y": 6.991800676923469 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.6, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Mid to center top.path b/src/main/deploy/pathplanner/paths/Mid to center top.path new file mode 100644 index 0000000..6a3276c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Mid to center top.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.309243119380694, + "y": 5.492445188825977 + }, + "prevControl": null, + "nextControl": { + "x": 1.852613365334066, + "y": 6.35890044588676 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.909982492594682, + "y": 6.256100669625311 + }, + "prevControl": { + "x": 2.293183835025989, + "y": 6.226729304979181 + }, + "nextControl": { + "x": 4.294247534437606, + "y": 6.322018052570215 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.609739764934075, + "y": 7.360380538942897 + }, + "prevControl": { + "x": 7.109739764934075, + "y": 7.360380538942897 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.6, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.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/Note3.SpeakerC.path b/src/main/deploy/pathplanner/paths/Note3.SpeakerC.path new file mode 100644 index 0000000..a35c8a3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Note3.SpeakerC.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.5, + "y": 4.11 + }, + "prevControl": null, + "nextControl": { + "x": 2.3384574944462946, + "y": 3.8603434005079107 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0288415532108353, + "y": 4.097305368134887 + }, + "prevControl": { + "x": 2.0367375362376348, + "y": 3.9729436354627934 + }, + "nextControl": { + "x": 1.9700988239185793, + "y": 5.0225033544879265 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.34, + "y": 5.55 + }, + "prevControl": { + "x": 2.0744556721993943, + "y": 5.55 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.6, + "maxAcceleration": 5.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/Safe Auto.path b/src/main/deploy/pathplanner/paths/Safe Auto.path new file mode 100644 index 0000000..77df419 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Safe Auto.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.8539869673657066, + "y": 4.508504473180682 + }, + "prevControl": null, + "nextControl": { + "x": 0.7979310504075553, + "y": 1.9776941372064571 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0013392754058237, + "y": 1.4179693813933087 + }, + "prevControl": { + "x": 1.1450479668554943, + "y": 1.9344625516300151 + }, + "nextControl": { + "x": 2.9265372617588623, + "y": 0.8599134531168725 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.2029233149375385, + "y": 1.1754219872076113 + }, + "prevControl": { + "x": 4.702923314937539, + "y": 1.1754219872076113 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.6, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -59.2810957359708, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Source note to source side.path b/src/main/deploy/pathplanner/paths/Source note to source side.path deleted file mode 100644 index c5cd6a3..0000000 --- a/src/main/deploy/pathplanner/paths/Source note to source side.path +++ /dev/null @@ -1,64 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.551735285288752, - "y": 3.6568684046459006 - }, - "prevControl": null, - "nextControl": { - "x": 1.7774493730805638, - "y": 3.544923453483271 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.8818897637795275, - "y": 4.41249682499365 - }, - "prevControl": { - "x": 1.4229570277322372, - "y": 3.684854642436558 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "New Constraints Zone", - "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - } - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 123.94358701975182, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": -136.68468431789634, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Source side to source note.path b/src/main/deploy/pathplanner/paths/Source side to source note.path deleted file mode 100644 index 48408b6..0000000 --- a/src/main/deploy/pathplanner/paths/Source side to source note.path +++ /dev/null @@ -1,64 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.7792735585471171, - "y": 4.440483062784308 - }, - "prevControl": null, - "nextControl": { - "x": 1.779273558547119, - "y": 3.940483062784308 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.533077793428314, - "y": 3.694183388366777 - }, - "prevControl": { - "x": 1.7774493730805647, - "y": 3.0878149029025335 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "New Constraints Zone", - "minWaypointRelativePos": 0.8, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 1.0, - "maxAcceleration": 1.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - } - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -139.93921554212616, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 120.57922687248902, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SpeakerC.Note3.path b/src/main/deploy/pathplanner/paths/SpeakerC.Note3.path new file mode 100644 index 0000000..fb7bf26 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SpeakerC.Note3.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.309243119380694, + "y": 5.55 + }, + "prevControl": null, + "nextControl": { + "x": 2.654351490521163, + "y": 5.503960098829289 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.9360380538942894, + "y": 3.9833745122035706 + }, + "prevControl": { + "x": 1.6561756759877155, + "y": 4.285871337197221 + }, + "nextControl": { + "x": 1.9616383756039566, + "y": 3.95570371166969 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.533077793428314, + "y": 3.9833745122035706 + }, + "prevControl": { + "x": 2.194222362098976, + "y": 3.9780227214999897 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.6, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SpeakerS.Note8.path b/src/main/deploy/pathplanner/paths/SpeakerS.Note8.path new file mode 100644 index 0000000..1d735f2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SpeakerS.Note8.path @@ -0,0 +1,79 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.6419522005022541, + "y": 4.377819358446314 + }, + "prevControl": null, + "nextControl": { + "x": 1.6047408109658412, + "y": 2.318976008690591 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.630250978090726, + "y": 1.3524488816112226 + }, + "prevControl": { + "x": 2.9206051269403472, + "y": 1.5130233538187157 + }, + "nextControl": { + "x": 5.866350427994003, + "y": 1.2363511432225178 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.986352139508573, + "y": 1.3524488816112226 + }, + "prevControl": { + "x": 5.886838264180244, + "y": 1.2295218644937702 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Intake Down", + "waypointRelativePos": 0.9, + "command": { + "type": "parallel", + "data": { + "commands": [] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 50.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -55.58163552094371, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -60.01348815657413, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/top to center top.path b/src/main/deploy/pathplanner/paths/top to center top.path new file mode 100644 index 0000000..556d1aa --- /dev/null +++ b/src/main/deploy/pathplanner/paths/top to center top.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7606160666866788, + "y": 6.6047521185951465 + }, + "prevControl": null, + "nextControl": { + "x": 1.3039863126400504, + "y": 7.471207375655929 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.8782413928464217, + "y": 6.259588519177039 + }, + "prevControl": { + "x": 2.261442735277729, + "y": 6.2302171545309095 + }, + "nextControl": { + "x": 4.262506434689346, + "y": 6.325505902121943 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.441822338190131, + "y": 7.201791858129171 + }, + "prevControl": { + "x": 6.941822338190131, + "y": 7.201791858129171 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.6, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 59.23728046576105, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/swerve/neo/controllerproperties.json b/src/main/deploy/swerve/neo/controllerproperties.json index c5ab644..972cbd7 100644 --- a/src/main/deploy/swerve/neo/controllerproperties.json +++ b/src/main/deploy/swerve/neo/controllerproperties.json @@ -1,7 +1,7 @@ { "angleJoystickRadiusDeadband": 0.5, "heading": { - "p": 0.4, + "p": 0.3, "i": 0, "d": 0.01 } diff --git a/src/main/deploy/swerve/neo/modules/backleft.json b/src/main/deploy/swerve/neo/modules/backleft.json index 2707b64..efc25ce 100644 --- a/src/main/deploy/swerve/neo/modules/backleft.json +++ b/src/main/deploy/swerve/neo/modules/backleft.json @@ -15,10 +15,10 @@ "canbus": null }, "inverted": { - "drive": false, + "drive": true, "angle": true }, - "absoluteEncoderOffset": 271.142578125, + "absoluteEncoderOffset": 331.171875, "location": { "front": -13, "left": 13 diff --git a/src/main/deploy/swerve/neo/modules/backright.json b/src/main/deploy/swerve/neo/modules/backright.json index 03efd3e..d266fed 100644 --- a/src/main/deploy/swerve/neo/modules/backright.json +++ b/src/main/deploy/swerve/neo/modules/backright.json @@ -15,10 +15,10 @@ "canbus": null }, "inverted": { - "drive": false, + "drive": true, "angle": true }, - "absoluteEncoderOffset": 316.845703125, + "absoluteEncoderOffset": 146.513671875, "location": { "front": -13, "left": -13 diff --git a/src/main/deploy/swerve/neo/modules/frontleft.json b/src/main/deploy/swerve/neo/modules/frontleft.json index c895ee0..fe951de 100644 --- a/src/main/deploy/swerve/neo/modules/frontleft.json +++ b/src/main/deploy/swerve/neo/modules/frontleft.json @@ -15,10 +15,10 @@ "canbus": null }, "inverted": { - "drive": false, + "drive": true, "angle": true }, - "absoluteEncoderOffset": 339.873046875, + "absoluteEncoderOffset": 58.095703125, "location": { "front": 13, "left": 13 diff --git a/src/main/deploy/swerve/neo/modules/frontright.json b/src/main/deploy/swerve/neo/modules/frontright.json index 2fee389..32736e8 100644 --- a/src/main/deploy/swerve/neo/modules/frontright.json +++ b/src/main/deploy/swerve/neo/modules/frontright.json @@ -15,10 +15,10 @@ "canbus": null }, "inverted": { - "drive": false, + "drive": true, "angle": true }, - "absoluteEncoderOffset": 33.837890625, + "absoluteEncoderOffset": 178.59375, "location": { "front": 13, "left": -13 diff --git a/src/main/deploy/swerve/neo/modules/pidfproperties.json b/src/main/deploy/swerve/neo/modules/pidfproperties.json index 09a5bc5..a3456b5 100644 --- a/src/main/deploy/swerve/neo/modules/pidfproperties.json +++ b/src/main/deploy/swerve/neo/modules/pidfproperties.json @@ -7,9 +7,9 @@ "iz": 0 }, "angle": { - "p": 0.0225, + "p": 0.01, "i": 0, - "d": 0.05, + "d": 0.0, "f": 0, "iz": 0 } diff --git a/src/main/java/frc/team7520/robot/Constants.java b/src/main/java/frc/team7520/robot/Constants.java index 9880d61..3bb9642 100644 --- a/src/main/java/frc/team7520/robot/Constants.java +++ b/src/main/java/frc/team7520/robot/Constants.java @@ -4,6 +4,7 @@ package frc.team7520.robot; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import swervelib.math.Matter; @@ -20,7 +21,7 @@ */ public final class Constants { - public static final double ROBOT_MASS = (148 - 20.3) * 0.453592; // 32lbs * kg per pound + public static final double ROBOT_MASS = 49.8952; // Mass in kilos public static final Matter CHASSIS = new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS); public static final double LOOP_TIME = 0.13; //s, 20ms + 110ms sprk max velocity lag @@ -41,6 +42,7 @@ public static final class Drivebase { public static class OperatorConstants { // Joystick Ports public static final int DRIVER_CONTROLLER_PORT = 0; + public static final int OPERATOR_CONTROLLER_PORT = 1; // Joystick Deadband public static final double LEFT_X_DEADBAND = 0.01; @@ -59,4 +61,90 @@ public static class Telemetry { // change at comp to low public static final SwerveDriveTelemetry.TelemetryVerbosity SWERVE_VERBOSITY = SwerveDriveTelemetry.TelemetryVerbosity.HIGH; } + + public static class IntakeConstants { + public enum Position { + SHOOT(new Rotation2d(0), 1), + INTAKE(new Rotation2d(Units.degreesToRadians(212.152317734808d)), -0.3), + AMP(new Rotation2d(Units.degreesToRadians(90)), 0.525); + + private final Rotation2d position; + private final double speed; + + Position(Rotation2d position, double speed) { + this.position = position; + this.speed = speed; + } + + public Rotation2d getPosition() { + return position; + } + + public double getSpeed() { + return speed; + } + } + + public static class PivotConstants { + public static final int CAN_ID = 23; + + public static final double GearRatio = 100; + public static final double degreeConversionFactor = 360/GearRatio; + public static final double rotationConversionFactor = 1/GearRatio; + + public static final double Intake = 211.374d; + public static final double Amp = 23.809415817260742 * degreeConversionFactor; + public static final double Shoot = 0; + + public static final double kP = 0.00022; + public static final double kI = 0; + public static final double kD = 0; + public static final double kFF = 0.000156; + + public static final double OUTPUT_MAX = 1; + public static final double OUTPUT_MIN = -1; + + public static final double SmartMaxVel = 600000; + public static final double SmartMinVel = 0; + public static final double SmartAccel = 10000; + public static final double SmartErr = 2; + public static final int SlotID = 0; + } + public static class WheelConstants { + public static final int CAN_ID = 22; + + public static final double kP = 0.0020645; + public static final double kI = 0; + public static final double kD = 0; + public static final double kFF = 0; + + public static final double MAX_RPM = 5676; + } + } + + public static class ShooterConstants { + public static final int shooterLeftID = 20; + public static final int shooterRightID = 21; + + public static final double kP = 0.002; + public static final double kI = 0.0; + public static final double kD = 0.0; + public static final double kFF = 0.000156; + + public static final double MAX_RPM = 5676; + + + } + + public static class ClimberConstants { + public static final int climberLeftID = 30; + public static final int climberRightID = 31; + public static final int maxPosition = 520; + + public static final double kP = 0.004; + public static final double kI = 0.0; + public static final double kD = 0.0; + public static final double kFF = 0.0006; + + } } diff --git a/src/main/java/frc/team7520/robot/RobotContainer.java b/src/main/java/frc/team7520/robot/RobotContainer.java index 5d48b55..9587b06 100644 --- a/src/main/java/frc/team7520/robot/RobotContainer.java +++ b/src/main/java/frc/team7520/robot/RobotContainer.java @@ -7,23 +7,37 @@ import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; +import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RepeatCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.*; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.team7520.robot.Constants.IntakeConstants; +import frc.team7520.robot.Constants.IntakeConstants.Position; import frc.team7520.robot.Constants.OperatorConstants; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.team7520.robot.auto.AutoIntake; +import frc.team7520.robot.auto.AutoShoot; +import frc.team7520.robot.auto.ShootSequence; import frc.team7520.robot.commands.AbsoluteDrive; +import frc.team7520.robot.commands.Climber; +import frc.team7520.robot.commands.Intake; +import frc.team7520.robot.commands.Shooter; import frc.team7520.robot.commands.TeleopDrive; +import frc.team7520.robot.subsystems.climber.ClimberSubsystem; +import frc.team7520.robot.subsystems.LED; +import frc.team7520.robot.subsystems.intake.IntakeSubsystem; +import frc.team7520.robot.subsystems.shooter.ShooterSubsystem; import frc.team7520.robot.subsystems.swerve.SwerveSubsystem; import java.io.File; +import static frc.team7520.robot.subsystems.LED.candle; + /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -37,14 +51,44 @@ public class RobotContainer private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve/neo")); + private final ShooterSubsystem shooterSubsystem = ShooterSubsystem.getInstance(); + + private final IntakeSubsystem intakeSubsystem = IntakeSubsystem.getInstance(); + + private final ClimberSubsystem climberSubsystem = ClimberSubsystem.getInstance(); + private final LED LEDSubsystem = LED.getInstance(); + + public final SendableChooser autoChooser = new SendableChooser<>(); + // Replace with CommandPS4Controller or CommandJoystick if needed private final XboxController driverController = new XboxController(OperatorConstants.DRIVER_CONTROLLER_PORT); + private final XboxController operatorController = + new XboxController(OperatorConstants.OPERATOR_CONTROLLER_PORT); + + private final Intake intake = new Intake(intakeSubsystem, + operatorController::getRightBumper, + operatorController::getYButton, + operatorController::getAButton, + operatorController::getBButton, + operatorController::getXButton, + intakeSubsystem::getSwitchVal + ); + + private Shooter shooter; + + + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + + registerAutos(); + + CameraServer.startAutomaticCapture(); + // Configure the trigger bindings configureBindings(); @@ -57,11 +101,35 @@ public RobotContainer() OperatorConstants.LEFT_Y_DEADBAND), () -> MathUtil.applyDeadband(-driverController.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), - () -> -driverController.getRightX(), - () -> -driverController.getRightY(), + () -> driverController.getRightX(), + () -> driverController.getRightY(), driverController::getRightBumper, - driverController::getLeftBumper + driverController::getLeftBumper, + () -> false + ); + + shooter = new Shooter(shooterSubsystem, + operatorController::getLeftTriggerAxis, + operatorController::getLeftBumper + ); + + + Climber climber = new Climber(climberSubsystem, + () -> false, + () -> false, + operatorController::getStartButton, + operatorController::getRightY, + operatorController::getLeftY, + operatorController::getBackButton ); + // Intake intake = new Intake(intakeSubsystem, + // operatorController::getRightBumper, + // operatorController::getYButton, + // operatorController::getAButton, + // operatorController::getBButton, + // operatorController::getXButton, + // intakeSubsystem::getSwitchVal + // ); // Old drive method // like in video games @@ -75,6 +143,35 @@ public RobotContainer() () -> driverController.getRawAxis(2), () -> true); drivebase.setDefaultCommand(closedAbsoluteDrive); + shooterSubsystem.setDefaultCommand(shooter); + intakeSubsystem.setDefaultCommand(intake); + climberSubsystem.setDefaultCommand(climber); + LEDSubsystem.setDefaultCommand(LEDSubsystem.idle()); + + candle.animate(LEDSubsystem.idleAnimation); + } + + private void registerAutos(){ + + registerNamedCommands(); + + autoChooser.setDefaultOption("Safe auto", drivebase.getPPAutoCommand("safe", true)); + autoChooser.addOption("Amp", drivebase.getPPAutoCommand("Amp", true)); + autoChooser.addOption("Test", drivebase.getPPAutoCommand("test", true)); + autoChooser.addOption("BotToCentBot", drivebase.getPPAutoCommand("BotToCentBot", true)); + autoChooser.addOption("MidToCentTop", drivebase.getPPAutoCommand("MidToCentTop", true)); + autoChooser.addOption("TopToCentTop", drivebase.getPPAutoCommand("TopToCentTop", true)); + autoChooser.addOption("2Note", drivebase.getPPAutoCommand("2NoteMid", true)); + autoChooser.addOption("3NoteMid.Note1", drivebase.getPPAutoCommand("3NoteMid.Note1", true)); + autoChooser.addOption("3NoteMid.Note3", drivebase.getPPAutoCommand("3NoteMid.Note3", true)); + autoChooser.addOption("4Note", drivebase.getPPAutoCommand("4Note", true)); + + // 1note shoot and Speaker Source side to note8 parking + autoChooser.addOption("SpeakerS.Note8", drivebase.getPPAutoCommand("SpeakerS.Note8", true)); + + + + SmartDashboard.putData(autoChooser); } /** @@ -84,9 +181,21 @@ public RobotContainer() private void registerNamedCommands() { // Example - NamedCommands.registerCommand("Shoot", new WaitCommand(1)); + NamedCommands.registerCommand("shoot", new ShootSequence()); + NamedCommands.registerCommand("log", new InstantCommand(() -> System.out.println("eeeeeeeeeeeeeeeeeeeeeeeee"))); + NamedCommands.registerCommand("intakeOut", new AutoIntake(Position.INTAKE)); + NamedCommands.registerCommand("intake", new InstantCommand(() -> intakeSubsystem.setSpeed(Position.INTAKE.getSpeed()))); + NamedCommands.registerCommand("stopIntaking", new InstantCommand(() -> intakeSubsystem.setSpeed(0))); + NamedCommands.registerCommand("intakeIn", new AutoIntake(Position.SHOOT)); + + // Overriding default command to always shoot: used to stop shooter in race group + NamedCommands.registerCommand("stopShoot", new AutoShoot(0, false)); + + } + + /** * Use this method to define your trigger->command mappings. Triggers can be created via the * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary @@ -104,8 +213,21 @@ private void configureBindings() // X/Lock wheels new JoystickButton(driverController, XboxController.Button.kX.value) .whileTrue(new RepeatCommand(new InstantCommand(drivebase::lock))); - } + new Trigger(() -> intake.currPosition == Position.INTAKE) + .and(new JoystickButton(operatorController, XboxController.Button.kRightBumper.value)) + .onTrue(new RepeatCommand(LEDSubsystem.intaking())) + .onFalse(LEDSubsystem.idle()); + + new Trigger(() -> intake.currPosition == Position.INTAKE) + .and(new JoystickButton(operatorController, XboxController.Button.kX.value)) + .whileTrue(new RepeatCommand(LEDSubsystem.intaking())) + .onFalse(LEDSubsystem.idle()); + + new Trigger(intakeSubsystem::getSwitchVal) + .whileFalse(new RepeatCommand(LEDSubsystem.noteIn())) + .onTrue(LEDSubsystem.idle()); + } /** * Use this to pass the autonomous command to the main {@link Robot} class. @@ -114,6 +236,12 @@ private void configureBindings() */ public Command getAutonomousCommand() { - return drivebase.getPPAutoCommand("Demo1", true); + return new SequentialCommandGroup( + new ParallelCommandGroup( + new InstantCommand(() -> shooterSubsystem.setDefaultCommand(new AutoShoot(0.7, false))), + autoChooser.getSelected() + ), + new InstantCommand(() -> shooterSubsystem.setDefaultCommand(shooter)) + ); } } diff --git a/src/main/java/frc/team7520/robot/auto/AutoIntake.java b/src/main/java/frc/team7520/robot/auto/AutoIntake.java new file mode 100644 index 0000000..e2965af --- /dev/null +++ b/src/main/java/frc/team7520/robot/auto/AutoIntake.java @@ -0,0 +1,46 @@ +package frc.team7520.robot.auto; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.team7520.robot.Constants; +import frc.team7520.robot.subsystems.intake.IntakeSubsystem; + + +public class AutoIntake extends Command { +// private final ShooterSubsystem shooterSubsystem = ShooterSubsystem.getInstance(); + private final IntakeSubsystem intakeSubsystem = IntakeSubsystem.getInstance(); + + private final Constants.IntakeConstants.Position desiredPos; + + public AutoIntake(Constants.IntakeConstants.Position desiredPos) { + // each subsystem used by the command must be passed into the + // addRequirements() method (which takes a vararg of Subsystem) + addRequirements( +// this.shooterSubsystem, + this.intakeSubsystem + ); + + this.desiredPos = desiredPos; + } + + @Override + public void initialize() { + intakeSubsystem.setPosition(desiredPos); +// shooterSubsystem.setSpeed(1, false); + } + + @Override + public void execute() { + + } + + @Override + public boolean isFinished() { + // TODO: Make this return true when this Command no longer needs to run execute() + return intakeSubsystem.atPosition(); + } + + @Override + public void end(boolean interrupted) { + + } +} diff --git a/src/main/java/frc/team7520/robot/auto/AutoShoot.java b/src/main/java/frc/team7520/robot/auto/AutoShoot.java new file mode 100644 index 0000000..0d0823b --- /dev/null +++ b/src/main/java/frc/team7520/robot/auto/AutoShoot.java @@ -0,0 +1,44 @@ +package frc.team7520.robot.auto; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.team7520.robot.subsystems.shooter.ShooterSubsystem; + + +public class AutoShoot extends Command { + private final ShooterSubsystem shooterSubsystem = ShooterSubsystem.getInstance(); + private final double power; + + private final boolean closedLoop; + public AutoShoot(double power, boolean closedLoop) { + // each subsystem used by the command must be passed into the + // addRequirements() method (which takes a vararg of Subsystem) + addRequirements(this.shooterSubsystem); + + this.power = power; + this.closedLoop = closedLoop; + + } + + @Override + public void initialize() { + + } + + @Override + public void execute() { + + shooterSubsystem.setSpeed(power, closedLoop); + + } + + @Override + public boolean isFinished() { + // TODO: Make this return true when this Command no longer needs to run execute() + return false; + } + + @Override + public void end(boolean interrupted) { + + } +} diff --git a/src/main/java/frc/team7520/robot/auto/ShootSequence.java b/src/main/java/frc/team7520/robot/auto/ShootSequence.java new file mode 100644 index 0000000..a09e5a3 --- /dev/null +++ b/src/main/java/frc/team7520/robot/auto/ShootSequence.java @@ -0,0 +1,29 @@ +package frc.team7520.robot.auto; + + +import edu.wpi.first.wpilibj2.command.*; +import frc.team7520.robot.Constants; +import frc.team7520.robot.subsystems.intake.IntakeSubsystem; +import frc.team7520.robot.subsystems.shooter.ShooterSubsystem; + +public class ShootSequence extends SequentialCommandGroup { + + public ShootSequence() { + + // TODO: Add your sequential commands in the super() call, e.g. + // super(new OpenClawCommand(), new MoveArmCommand()); + super( + new ParallelCommandGroup( + new AutoIntake(Constants.IntakeConstants.Position.SHOOT), + new ParallelRaceGroup( + new AutoShoot(1, false), + new WaitCommand(0.25) + ) + ), + new InstantCommand(() -> IntakeSubsystem.getInstance().setSpeed(1)), + new WaitCommand(0.3), + new InstantCommand(() -> IntakeSubsystem.getInstance().setSpeed(0)) +// new InstantCommand(() -> ShooterSubsystem.getInstance().setSpeed(0, false)) + ); + } +} diff --git a/src/main/java/frc/team7520/robot/commands/AbsoluteDrive.java b/src/main/java/frc/team7520/robot/commands/AbsoluteDrive.java index e97fa46..3483d96 100644 --- a/src/main/java/frc/team7520/robot/commands/AbsoluteDrive.java +++ b/src/main/java/frc/team7520/robot/commands/AbsoluteDrive.java @@ -26,7 +26,7 @@ public class AbsoluteDrive extends Command { private final SwerveSubsystem swerve; private final DoubleSupplier vX, vY; private final DoubleSupplier headingHorizontal, headingVertical; - private final BooleanSupplier CCWSpin, CWSpin; + private final BooleanSupplier CCWSpin, CWSpin, speedCutoffSup; private boolean initRotation = false; /** @@ -50,7 +50,7 @@ public class AbsoluteDrive extends Command { * with no deadband. Positive is away from the alliance wall. */ public AbsoluteDrive(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier headingHorizontal, - DoubleSupplier headingVertical) { + DoubleSupplier headingVertical, BooleanSupplier speedCutoffSup) { this.swerve = swerve; this.vX = vX; this.vY = vY; @@ -58,12 +58,13 @@ public AbsoluteDrive(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier v this.headingVertical = headingVertical; this.CCWSpin = () -> false; this.CWSpin = () -> false; + this.speedCutoffSup = speedCutoffSup; addRequirements(swerve); } public AbsoluteDrive(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier headingHorizontal, - DoubleSupplier headingVertical, BooleanSupplier CWSpin, BooleanSupplier CCWSpin) { + DoubleSupplier headingVertical, BooleanSupplier CWSpin, BooleanSupplier CCWSpin, BooleanSupplier speedCutoffSup) { this.swerve = swerve; this.vX = vX; this.vY = vY; @@ -71,6 +72,7 @@ public AbsoluteDrive(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier v this.headingVertical = headingVertical; this.CCWSpin = CCWSpin; this.CWSpin = CWSpin; + this.speedCutoffSup = speedCutoffSup; addRequirements(swerve); } @@ -79,21 +81,27 @@ public AbsoluteDrive(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier v @Override public void initialize() { initRotation = true; + SmartDashboard.putBoolean("initRotation", initRotation); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + Boolean speedCutoff = speedCutoffSup.getAsBoolean(); + + double vXspeed = vX.getAsDouble() * (speedCutoffSup.getAsBoolean() ? 1 : 1); + double vYspeed = vY.getAsDouble() * (speedCutoffSup.getAsBoolean() ? 1 : 1); + ChassisSpeeds desiredSpeeds; if (CWSpin.getAsBoolean()) { - desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), swerve.getHeading().minus(Rotation2d.fromDegrees(90))); + desiredSpeeds = swerve.getTargetSpeeds(vXspeed, vYspeed, swerve.getHeading().minus(Rotation2d.fromDegrees(20))); } else if (CCWSpin.getAsBoolean()) { - desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(),swerve.getHeading().plus(Rotation2d.fromDegrees(90))); + desiredSpeeds = swerve.getTargetSpeeds(vXspeed, vYspeed, swerve.getHeading().plus(Rotation2d.fromDegrees(20))); } else { // Get the desired chassis speeds based on a 2 joystick module. - desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), + desiredSpeeds = swerve.getTargetSpeeds(vXspeed, vYspeed, headingHorizontal.getAsDouble(), headingVertical.getAsDouble()); } @@ -106,6 +114,8 @@ public void execute() { // Set the Current Heading to the desired Heading desiredSpeeds = swerve.getTargetSpeeds(0, 0, firstLoopHeading.getSin(), firstLoopHeading.getCos()); + + SmartDashboard.putBoolean("initRotation", initRotation); } //Dont Init Rotation Again initRotation = false; @@ -122,6 +132,7 @@ public void execute() { // Make the robot move swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); + } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/team7520/robot/commands/Climber.java b/src/main/java/frc/team7520/robot/commands/Climber.java new file mode 100644 index 0000000..7c7eca3 --- /dev/null +++ b/src/main/java/frc/team7520/robot/commands/Climber.java @@ -0,0 +1,147 @@ +// 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.team7520.robot.commands; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.team7520.robot.Constants; +import frc.team7520.robot.Constants.ClimberConstants; +import frc.team7520.robot.subsystems.climber.ClimberSubsystem; +import frc.team7520.robot.subsystems.climber.ClimberSubsystem; +import edu.wpi.first.wpilibj2.command.Command; + +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + +import com.fasterxml.jackson.databind.ser.std.NumberSerializers.DoubleSerializer; +import com.revrobotics.CANSparkMax; + +/** An example command that uses an example subsystem. */ +public class Climber extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final ClimberSubsystem subsystem; + private final BooleanSupplier bExtent; + private final BooleanSupplier bRetract; + private final BooleanSupplier bStop; + private final DoubleSupplier bRightManual; + private final DoubleSupplier bLeftManual; + private final BooleanSupplier bShift; + + private static final double motorSpeed = 0.9; + double leftMotorSpeed = 0; + double rightMotorSpeed = 0; + double pValue = 0.00001; + + enum State { + NOTHING, + EXTEND, + RETRACT + } + + State state = State.NOTHING; + + //public Position currPosition = Position.SHOOT; + + + + /** + * Creates a new ExampleCommand. + * + * @param climberSubsystem The subsystem used by this command. + */ + public Climber(ClimberSubsystem climberSubsystem, BooleanSupplier retract, BooleanSupplier extent, BooleanSupplier stop, DoubleSupplier rightManual, DoubleSupplier leftManual, BooleanSupplier shift) { + this.subsystem = climberSubsystem; + this.bRetract = retract; + this.bExtent = extent; + this.bStop = stop; + this.bRightManual = rightManual; + this.bLeftManual = leftManual; + this.bShift = shift; + subsystem.setLeftPosition(ClimberConstants.maxPosition); + subsystem.setRightPosition(ClimberConstants.maxPosition); + + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(climberSubsystem); + } + + + + + // Called when the command is initially scheduled. + @Override + public void initialize() { + + } + + public String convertState(State state) { + if (state == State.NOTHING) { + return "NOTHING"; + } else if (state == State.EXTEND) { + return "EXTEND"; + } else if (state == State.RETRACT) { + return "RETRACT"; + } + return ""; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + SmartDashboard.putString("State", convertState(this.state)); + double leftPosition = subsystem.getLeftPosition(); + double rightPosition = subsystem.getRightPosition(); + + /* + m_SparkMaxPIDController.setP(pValue); + m_SparkMaxPIDController.setI(0); + m_SparkMaxPIDController.setD(0); + m_SparkMaxPIDController.setOutputRange(-0.3, 0.3); + m_OtherSparkMaxPIDController.setP(pValue); + m_OtherSparkMaxPIDController.setI(0); + m_OtherSparkMaxPIDController.setD(0); + m_OtherSparkMaxPIDController.setOutputRange(-0.3, 0.3); + */ + + + + if (bExtent.getAsBoolean()) { + this.state = State.EXTEND; + subsystem.setRightArmReference(0); + subsystem.setLeftArmReference(0); + } else if (bRetract.getAsBoolean()) { + subsystem.setRightArmReference(ClimberConstants.maxPosition); + subsystem.setLeftArmReference(ClimberConstants.maxPosition); + this.state = State.RETRACT; + } else if (bStop.getAsBoolean()) { + if (bShift.getAsBoolean()) { + subsystem.setZeroPos(); + this.state = State.NOTHING; + } else { + subsystem.stop(); + this.state = State.NOTHING; + } + } + + if (this.state == State.NOTHING) { + subsystem.setRightSpeed(bRightManual.getAsDouble()); + subsystem.setLeftSpeed(bLeftManual.getAsDouble()); + } + } + + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + //subsystem.setLeftSpeed(0); + //subsystem.setRightSpeed(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/team7520/robot/commands/Intake.java b/src/main/java/frc/team7520/robot/commands/Intake.java new file mode 100644 index 0000000..e79dadf --- /dev/null +++ b/src/main/java/frc/team7520/robot/commands/Intake.java @@ -0,0 +1,116 @@ +// 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.team7520.robot.commands; + +import frc.team7520.robot.Constants; +import frc.team7520.robot.subsystems.intake.IntakeSubsystem; +import edu.wpi.first.wpilibj2.command.Command; + +import java.util.function.BooleanSupplier; + +/** An example command that uses an example subsystem. */ +public class Intake extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final IntakeSubsystem intakeSubsystem; + private final BooleanSupplier shootSup; + private final BooleanSupplier shootPosSup; + private final BooleanSupplier intakePosSup; + private final BooleanSupplier ampPosSup; + private final BooleanSupplier reverseSup; + private final BooleanSupplier switchSup; + + public Constants.IntakeConstants.Position currPosition = Constants.IntakeConstants.Position.SHOOT; + + + + /** + * Creates a new ExampleCommand. + * + * @param intakeSubsystem The subsystem used by this command. + */ + public Intake(IntakeSubsystem intakeSubsystem, BooleanSupplier shootSup, BooleanSupplier shootPosSup, + BooleanSupplier intakePosSup, BooleanSupplier ampPosSup, BooleanSupplier reverseSup, BooleanSupplier switchSup) { + this.intakeSubsystem = intakeSubsystem; + this.shootSup = shootSup; + this.shootPosSup = shootPosSup; + this.intakePosSup = intakePosSup; + this.ampPosSup = ampPosSup; + this.reverseSup = reverseSup; + this.switchSup = switchSup; + + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(intakeSubsystem); + } + + public void handleWheels() { + if (shootSup.getAsBoolean() && currPosition == Constants.IntakeConstants.Position.SHOOT) { + intakeSubsystem.setSpeed(0.35, false); + return; + } + if (shootSup.getAsBoolean() && currPosition == Constants.IntakeConstants.Position.AMP) { + intakeSubsystem.setSpeed(0.525, false); + return; + } + if (shootSup.getAsBoolean() && currPosition == Constants.IntakeConstants.Position.INTAKE) { + if (switchSup.getAsBoolean()) { + intakeSubsystem.setSpeed(-0.35, false); + } + else { + intakeSubsystem.setSpeed(0, false); + } + return; + } + if(reverseSup.getAsBoolean()) { + intakeSubsystem.setSpeed(-0.35); + return; + } + intakeSubsystem.stop(); + } + + public void handlePosition() { + if (shootPosSup.getAsBoolean()) { + intakeSubsystem.setPosition(Constants.IntakeConstants.Position.SHOOT); + currPosition = Constants.IntakeConstants.Position.SHOOT; + return; + } + if (intakePosSup.getAsBoolean()) { + intakeSubsystem.setPosition(Constants.IntakeConstants.Position.INTAKE); + currPosition = Constants.IntakeConstants.Position.INTAKE; + return; + } + if (ampPosSup.getAsBoolean()) { + intakeSubsystem.setPosition(Constants.IntakeConstants.Position.AMP); + currPosition = Constants.IntakeConstants.Position.AMP; + return; + } + + } + + // 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() { + + handleWheels(); + handlePosition(); + + } + + + // 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/team7520/robot/commands/Shooter.java b/src/main/java/frc/team7520/robot/commands/Shooter.java new file mode 100644 index 0000000..157b1c9 --- /dev/null +++ b/src/main/java/frc/team7520/robot/commands/Shooter.java @@ -0,0 +1,48 @@ +package frc.team7520.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.team7520.robot.subsystems.shooter.ShooterSubsystem; + +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + + +public class Shooter extends Command { + private final ShooterSubsystem shooterSubsystem; + private final DoubleSupplier throttleSup; + private final BooleanSupplier invertSup; + + public Shooter(ShooterSubsystem shooterSubsystem, DoubleSupplier throttleSup, BooleanSupplier invertSup) { + this.shooterSubsystem = shooterSubsystem; + this.throttleSup = throttleSup; + this.invertSup = invertSup; + + + // each subsystem used by the command must be passed into the + // addRequirements() method (which takes a vararg of Subsystem) + addRequirements(this.shooterSubsystem); + } + + @Override + public void initialize() { + + } + + @Override + public void execute() { + double throttle = throttleSup.getAsDouble() * (invertSup.getAsBoolean() ? -1 : 1) * 1; + + shooterSubsystem.setSpeed(throttle, false); + } + + @Override + public boolean isFinished() { + // TODO: Make this return true when this Command no longer needs to run execute() + return false; + } + + @Override + public void end(boolean interrupted) { + + } +} diff --git a/src/main/java/frc/team7520/robot/subsystems/LED.java b/src/main/java/frc/team7520/robot/subsystems/LED.java new file mode 100644 index 0000000..8e4cb0b --- /dev/null +++ b/src/main/java/frc/team7520/robot/subsystems/LED.java @@ -0,0 +1,81 @@ +package frc.team7520.robot.subsystems; + + +import com.ctre.phoenix.led.*; +import com.ctre.phoenix.led.ColorFlowAnimation.Direction; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class LED extends SubsystemBase { + + // With eager singleton initialization, any static variables/fields used in the + // constructor must appear before the "INSTANCE" variable so that they are initialized + // before the constructor is called when the "INSTANCE" variable initializes. + + /** + * The Singleton instance of this LED. Code should use + * the {@link #getInstance()} method to get the single instance (rather + * than trying to construct an instance of this class.) + */ + private final static LED INSTANCE = new LED(); + + public final static CANdle candle = new CANdle(17); + + public final Animation idleAnimation = new RainbowAnimation(255, 0.75, 100); + private final Animation intakingAnimation = new ColorFlowAnimation(255, 165, 0, 0, 0.75, 100, Direction.Forward); + private final Animation noteIn = new ColorFlowAnimation(0, 255, 0, 0, 0.75, 100, Direction.Forward); + + /** + * Returns the Singleton instance of this LED. This static method + * should be used, rather than the constructor, to get the single instance + * of this class. For example: {@code LED.getInstance();} + */ + @SuppressWarnings("WeakerAccess") + public static LED getInstance() { + return INSTANCE; + } + + /** + * Creates a new instance of this LED. This constructor + * is private since this class is a Singleton. Code should use + * the {@link #getInstance()} method to get the singleton instance. + */ + private LED() { + + } + + public Command idle() { + return run( + () -> { + candle.animate(idleAnimation); + } + ); + } + + public InstantCommand intaking() { + return new InstantCommand( + () -> { + candle.animate(intakingAnimation); + } + ); + } + + public InstantCommand noteIn() { + return new InstantCommand( + () -> { + candle.animate(noteIn); + } + ); + } + + public InstantCommand clear() { + return new InstantCommand( + () -> { + candle.clearAnimation(0); + } + ); + } +} + diff --git a/src/main/java/frc/team7520/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/team7520/robot/subsystems/climber/ClimberSubsystem.java new file mode 100644 index 0000000..a4eb999 --- /dev/null +++ b/src/main/java/frc/team7520/robot/subsystems/climber/ClimberSubsystem.java @@ -0,0 +1,128 @@ +// 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.team7520.robot.subsystems.climber; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; + +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.team7520.robot.Constants; +import frc.team7520.robot.Constants.ClimberConstants; + + +public class ClimberSubsystem extends SubsystemBase { + + private CANSparkMax leftClimberMotor; + private CANSparkMax rightClimberMotor; + + private SparkPIDController leftClimberPID; + private SparkPIDController rightClimberPID; + + private RelativeEncoder leftClimberEncoder; + private RelativeEncoder rightClimberEncoder; + + private SlewRateLimiter mSpeedLimiter = new SlewRateLimiter(1); + + + private final static ClimberSubsystem INSTANCE = new ClimberSubsystem(); + + @SuppressWarnings("WeakerAccess") + public static ClimberSubsystem getInstance() { + return INSTANCE; + } + + /** Creates a new ExampleSubsystem. */ + private ClimberSubsystem() { + leftClimberMotor = new CANSparkMax(Constants.ClimberConstants.climberLeftID, CANSparkMax.MotorType.kBrushless); + rightClimberMotor = new CANSparkMax(Constants.ClimberConstants.climberRightID, CANSparkMax.MotorType.kBrushless); + + leftClimberMotor.restoreFactoryDefaults(); + rightClimberMotor.restoreFactoryDefaults(); + + leftClimberPID = leftClimberMotor.getPIDController(); + leftClimberPID.setP(Constants.ClimberConstants.kP); + leftClimberPID.setI(Constants.ClimberConstants.kI); + leftClimberPID.setD(Constants.ClimberConstants.kD); + //leftClimberPID.setFF(Constants.ClimberConstants.kFF); + + rightClimberPID = rightClimberMotor.getPIDController(); + rightClimberPID.setP(Constants.ClimberConstants.kP); + rightClimberPID.setI(Constants.ClimberConstants.kI); + rightClimberPID.setD(Constants.ClimberConstants.kD); + //rightClimberPID.setFF(Constants.ClimberConstants.kFF); + + leftClimberEncoder = leftClimberMotor.getEncoder(); + rightClimberEncoder = rightClimberMotor.getEncoder(); + + leftClimberMotor.setIdleMode(CANSparkMax.IdleMode.kBrake); + rightClimberMotor.setIdleMode(CANSparkMax.IdleMode.kBrake); + + leftClimberMotor.setInverted(false); + rightClimberMotor.setInverted(false); + } + + public void setPosition(Rotation2d position){ + //DesiredPosition = position; + //pivotPID.setReference(DesiredPosition.getRotations(), ControlType.kSmartMotion); + } + + public double getLeftPosition(){ + return leftClimberEncoder.getPosition(); + } + + public double getRightPosition(){ + return rightClimberEncoder.getPosition(); + } + + public void setLeftPosition(double pos){ + leftClimberEncoder.setPosition(pos); + } + + public void setRightPosition(double pos){ + rightClimberEncoder.setPosition(pos); + } + + public void setRightArmReference(double pos) { + rightClimberPID.setReference(pos, CANSparkMax.ControlType.kPosition); + } + + public void setLeftArmReference(double pos) { + leftClimberPID.setReference(pos, CANSparkMax.ControlType.kPosition); + } + + public void setLeftSpeed(double speed){ + leftClimberMotor.set(speed); + } + + public void setRightSpeed(double speed){ + rightClimberMotor.set(speed); + } + + public void setZeroPos() { + leftClimberEncoder.setPosition(ClimberConstants.maxPosition); + rightClimberEncoder.setPosition(ClimberConstants.maxPosition); + leftClimberPID.setReference(ClimberConstants.maxPosition, CANSparkMax.ControlType.kPosition); + rightClimberPID.setReference(ClimberConstants.maxPosition, CANSparkMax.ControlType.kPosition); + } + + + + public void stop() { + leftClimberMotor.set(0); + rightClimberMotor.set(0); + leftClimberPID.setReference(leftClimberEncoder.getPosition(), CANSparkMax.ControlType.kPosition); + rightClimberPID.setReference(rightClimberEncoder.getPosition(), CANSparkMax.ControlType.kPosition); + } + + @Override + public void periodic() { + SmartDashboard.putNumber("ClimberL", leftClimberEncoder.getPosition()); + SmartDashboard.putNumber("ClimberR", rightClimberEncoder.getPosition()); + } +} diff --git a/src/main/java/frc/team7520/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/team7520/robot/subsystems/intake/IntakeSubsystem.java new file mode 100644 index 0000000..01b4131 --- /dev/null +++ b/src/main/java/frc/team7520/robot/subsystems/intake/IntakeSubsystem.java @@ -0,0 +1,179 @@ +// 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.team7520.robot.subsystems.intake; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; +import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DutyCycleEncoder; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.team7520.robot.Constants; +import frc.team7520.robot.Constants.IntakeConstants; + +public class IntakeSubsystem extends SubsystemBase { + + public CANSparkMax pivot = new CANSparkMax(IntakeConstants.PivotConstants.CAN_ID, MotorType.kBrushless); + public CANSparkMax wheels = new CANSparkMax(IntakeConstants.WheelConstants.CAN_ID, MotorType.kBrushless); + + private RelativeEncoder pivotEncoder; + private final SparkPIDController pivotPID = pivot.getPIDController(); + private final SparkPIDController wheelsPID = wheels.getPIDController(); + + private final DigitalInput input = new DigitalInput(0); + + private final SlewRateLimiter slewRateLimiter = new SlewRateLimiter(5); + + public Rotation2d desiredPosition = Rotation2d.fromDegrees(IntakeConstants.PivotConstants.Shoot); + + // rev through bore encoders + public DutyCycleEncoder wheelAbsEncoder = new DutyCycleEncoder(2); + public DutyCycleEncoder pivotAbsEncoder = new DutyCycleEncoder(1); + +// public DiffEncoder diffedEncoder = new DiffEncoder(pivotAbsEncoder, pivotAbsEncoder); + + private final static IntakeSubsystem INSTANCE = new IntakeSubsystem(); + + @SuppressWarnings("WeakerAccess") + public static IntakeSubsystem getInstance() { + return INSTANCE; + } + + /** Creates a new ExampleSubsystem. */ + private IntakeSubsystem() { + this.pivotEncoder = pivot.getEncoder(); + pivotEncoder.setPosition(0); + pivotEncoder.setPositionConversionFactor(IntakeConstants.PivotConstants.degreeConversionFactor); + + pivotPID.setP(IntakeConstants.PivotConstants.kP); + pivotPID.setI(IntakeConstants.PivotConstants.kI); + pivotPID.setD(IntakeConstants.PivotConstants.kD); + pivotPID.setFF(IntakeConstants.PivotConstants.kFF); + + pivotPID.setSmartMotionMaxVelocity(IntakeConstants.PivotConstants.SmartMaxVel, IntakeConstants.PivotConstants.SlotID); + pivotPID.setSmartMotionMinOutputVelocity(IntakeConstants.PivotConstants.SmartMinVel, IntakeConstants.PivotConstants.SlotID); + pivotPID.setSmartMotionMaxAccel(IntakeConstants.PivotConstants.SmartAccel, IntakeConstants.PivotConstants.SlotID); + pivotPID.setSmartMotionAllowedClosedLoopError(IntakeConstants.PivotConstants.SmartErr, IntakeConstants.PivotConstants.SlotID); + + + pivot.setIdleMode(CANSparkMax.IdleMode.kCoast); + +// pivotAbsEncoder.setDistancePerRotation(360); + pivotAbsEncoder.setPositionOffset(0.159); + + // Wheels + wheels.setIdleMode(CANSparkMax.IdleMode.kBrake); +// wheels.setInverted(true); + + wheelsPID.setP(IntakeConstants.WheelConstants.kP); + wheelsPID.setI(IntakeConstants.WheelConstants.kI); + wheelsPID.setD(IntakeConstants.WheelConstants.kD); + wheelsPID.setFF(IntakeConstants.WheelConstants.kFF); + +// wheelAbsEncoder.setDistancePerRotation(360); + + + } + + public void setPosition(IntakeConstants.Position position){ + desiredPosition = position.getPosition(); + pivotPID.setReference(desiredPosition.getDegrees(), ControlType.kSmartMotion); + } + + public boolean atPosition() { + double currPos = pivotEncoder.getPosition(); + double targetPos = desiredPosition.getDegrees(); + double error = Math.abs(targetPos - currPos); + + return error < IntakeConstants.PivotConstants.SmartErr; + } + +// public Command Shoot() { +// return runOnce( +// () -> { +// setRotation(Rotation2d.fromDegrees(IntakeConstants.PivotConstants.Shoot)); +// }).andThen(MoveIntake()); +// } +// +// public Command Amp() { +// return runOnce( +// () -> { +// setRotation(Rotation2d.fromDegrees(IntakeConstants.PivotConstants.Amp)); +// }).andThen(MoveIntake()); +// } +// +// public Command Intake() { +// return runOnce( +// () -> { +// setRotation(Rotation2d.fromDegrees(IntakeConstants.PivotConstants.Floor)); +// }).andThen(MoveIntake()); +// } +// +// public Command Manual(DoubleSupplier pivot) { +// return runOnce( +// () -> { +// double pivotVal = pivot.getAsDouble(); +// +// if(Math.abs(pivotVal) > 0.05) { +// DesiredPosition = DesiredPosition.plus(Rotation2d.fromDegrees(1 * pivotVal)); +// } +// }).andThen(MoveIntake()); +// } + +// public void moveIntake() { +// pivotPID.setReference(DesiredPosition.getRotations(), ControlType.kSmartMotion); +// } + + public void stop() { + + wheels.set(0); + + } + + public void setSpeed(double speed) { + setSpeed(speed, false); + } + + public void setSpeed(double speed, boolean closedLoop) { + speed = slewRateLimiter.calculate(speed); + + if(closedLoop) { + speed *= IntakeConstants.WheelConstants.MAX_RPM; + + if (speed == 0) { + wheels.set(0); + } else { + wheelsPID.setReference(speed, ControlType.kVelocity); + } + } else { + wheels.set(speed); + } + } + + public boolean getSwitchVal() { + return input.get(); + } + + public double getDiffedEncoder(){ + return pivotAbsEncoder.get() - wheelAbsEncoder.get(); + } + + @Override + public void periodic() { + SmartDashboard.putNumber("pivotEncoder", pivotEncoder.getPosition()); + SmartDashboard.putNumber("DesiredDeg", desiredPosition.getDegrees()); + SmartDashboard.putNumber("DesiredRot", desiredPosition.getRotations()); + SmartDashboard.putNumber("diffedEncoder", getDiffedEncoder()); + SmartDashboard.putNumber("PivotAbsEncoder", pivotAbsEncoder.get()); + SmartDashboard.putNumber("wheelsAbsEncoder", wheelAbsEncoder.get()); + } + +} diff --git a/src/main/java/frc/team7520/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/team7520/robot/subsystems/shooter/ShooterSubsystem.java new file mode 100644 index 0000000..73b2127 --- /dev/null +++ b/src/main/java/frc/team7520/robot/subsystems/shooter/ShooterSubsystem.java @@ -0,0 +1,125 @@ +package frc.team7520.robot.subsystems.shooter; + + +import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.team7520.robot.Constants; + +public class ShooterSubsystem extends SubsystemBase { + + // With eager singleton initialization, any static variables/fields used in the + // constructor must appear before the "INSTANCE" variable so that they are initialized + // before the constructor is called when the "INSTANCE" variable initializes. + + private CANSparkMax leftShooterMotor; + private CANSparkMax rightShooterMotor; + + private SparkPIDController leftShooterPID; + private SparkPIDController rightShooterPID; + + private RelativeEncoder leftShooterEncoder; + private RelativeEncoder rightShooterEncoder; + + private SlewRateLimiter mSpeedLimiter = new SlewRateLimiter(100); + + /** + * The Singleton instance of this shooterSubsystem. Code should use + * the {@link #getInstance()} method to get the single instance (rather + * than trying to construct an instance of this class.) + */ + private final static ShooterSubsystem INSTANCE = new ShooterSubsystem(); + + /** + * Returns the Singleton instance of this shooterSubsystem. This static method + * should be used, rather than the constructor, to get the single instance + * of this class. For example: {@code shooterSubsystem.getInstance();} + */ + @SuppressWarnings("WeakerAccess") + public static ShooterSubsystem getInstance() { + return INSTANCE; + } + + /** + * Creates a new instance of this shooterSubsystem. This constructor + * is private since this class is a Singleton. Code should use + * the {@link #getInstance()} method to get the singleton instance. + */ + private ShooterSubsystem() { + /* leftShooterMotor = new CANSparkMax(Constants.kShooterLeftMotorId, MotorType.kBrushless); + rightShooterMotor = new CANSparkMax(Constants.kShooterRightMotorId, MotorType.kBrushless); + leftShooterMotor.restoreFactoryDefaults(); + rightShooterMotor.restoreFactoryDefaults(); + + leftShooterPID = leftShooterMotor.getPIDController(); + leftShooterPID.setP(Constants.kShooterP); + leftShooterPID.setI(Constants.kShooterI); + leftShooterPID.setD(Constants.kShooterD); + leftShooterPID.setOutputRange(Constants.kShooterMinOutput, Constants.kShooterMaxOutput); + + rightShooterPID = rightShooterMotor.getPIDController(); + rightShooterPID.setP(Constants.kShooterP); + rightShooterPID.setI(Constants.kShooterI); + rightShooterPID.setD(Constants.kShooterD); + rightShooterPID.setOutputRange(Constants.kShooterMinOutput, Constants.kShooterMaxOutput); + + leftShooterEncoder = leftShooterMotor.getEncoder(); + rightShooterEncoder = rightShooterMotor.getEncoder(); + + leftShooterMotor.setIdleMode(CANSparkMax.IdleMode.kBrake); + rightShooterMotor.setIdleMode(CANSparkMax.IdleMode.kBrake); + + leftShooterMotor.setInverted(false); + rightShooterMotor.setInverted(true);*/ + + leftShooterMotor = new CANSparkMax(Constants.ShooterConstants.shooterLeftID, CANSparkMax.MotorType.kBrushless); + rightShooterMotor = new CANSparkMax(Constants.ShooterConstants.shooterRightID, CANSparkMax.MotorType.kBrushless); + + leftShooterMotor.restoreFactoryDefaults(); + rightShooterMotor.restoreFactoryDefaults(); + + leftShooterPID = leftShooterMotor.getPIDController(); + leftShooterPID.setP(Constants.ShooterConstants.kP); + leftShooterPID.setI(Constants.ShooterConstants.kI); + leftShooterPID.setD(Constants.ShooterConstants.kD); + leftShooterPID.setFF(Constants.ShooterConstants.kFF); + + rightShooterPID = rightShooterMotor.getPIDController(); + rightShooterPID.setP(Constants.ShooterConstants.kP); + rightShooterPID.setI(Constants.ShooterConstants.kI); + rightShooterPID.setD(Constants.ShooterConstants.kD); + rightShooterPID.setFF(Constants.ShooterConstants.kFF); + + leftShooterEncoder = leftShooterMotor.getEncoder(); + rightShooterEncoder = rightShooterMotor.getEncoder(); + + leftShooterMotor.setIdleMode(CANSparkMax.IdleMode.kCoast); + rightShooterMotor.setIdleMode(CANSparkMax.IdleMode.kCoast); + + leftShooterMotor.setInverted(false); + rightShooterMotor.setInverted(true); + } + + public void setSpeed(double speed, boolean closedLoop) { + speed = mSpeedLimiter.calculate(speed); + + if (closedLoop) { + speed *= Constants.ShooterConstants.MAX_RPM; + + leftShooterPID.setReference(speed, CANSparkBase.ControlType.kVelocity); + rightShooterPID.setReference(speed, CANSparkBase.ControlType.kVelocity); + } else { + leftShooterMotor.set(speed); + rightShooterMotor.set(speed); + } + } + + public void stop() { + leftShooterMotor.stopMotor(); + rightShooterMotor.stopMotor(); + } +} + diff --git a/src/main/java/frc/team7520/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/team7520/robot/subsystems/swerve/SwerveSubsystem.java index fe19188..f120c91 100644 --- a/src/main/java/frc/team7520/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/team7520/robot/subsystems/swerve/SwerveSubsystem.java @@ -12,6 +12,7 @@ import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; @@ -19,6 +20,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; 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.team7520.robot.Constants; @@ -142,7 +144,8 @@ public Command getPathCommand(String pathName, boolean setOdomToStart) { */ public Command getPPAutoCommand(String autoName, boolean setOdomToStart) { if (setOdomToStart) { - resetOdometry(PathPlannerAuto.getStaringPoseFromAutoFile(autoName)); + SmartDashboard.putNumber("HeadingFromFile", -1); +// resetOdometry(PathPlannerAuto.getStaringPoseFromAutoFile(autoName)); } return new PathPlannerAuto(autoName); } @@ -223,6 +226,10 @@ public SwerveDriveKinematics getKinematics() { * @param initialHolonomicPose The pose to set the odometry to */ public void resetOdometry(Pose2d initialHolonomicPose) { +// SmartDashboard.putNumber("ResetHeading", initialHolonomicPose.getRotation().getDegrees()); + + swerveDrive.setGyro(new Rotation3d(0, 0, initialHolonomicPose.getRotation().getRadians())); + swerveDrive.resetOdometry(initialHolonomicPose); } @@ -322,6 +329,8 @@ public void setTargetHeading(Rotation2d angle){ public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d angle) { xInput = Math.pow(xInput, 3); yInput = Math.pow(yInput, 3); + + swerveDrive.swerveController.lastAngleScalar = angle.getRadians(); return swerveDrive.swerveController.getTargetSpeeds(xInput, yInput, angle.getRadians(), diff --git a/src/main/java/frc/team7520/robot/util/DiffEncoder.java b/src/main/java/frc/team7520/robot/util/DiffEncoder.java new file mode 100644 index 0000000..b83af41 --- /dev/null +++ b/src/main/java/frc/team7520/robot/util/DiffEncoder.java @@ -0,0 +1,40 @@ +package frc.team7520.robot.util; + +import edu.wpi.first.wpilibj.DutyCycleEncoder; + +public class DiffEncoder{ + + private DutyCycleEncoder encoder1; + private DutyCycleEncoder encoder2; + + public DiffEncoder(DutyCycleEncoder encoder1, DutyCycleEncoder encoder2){ + this.encoder1 = encoder1; + this.encoder2 = encoder2; + } + + public double get(){ + return encoder1.get() - encoder2.get(); + } + + public double getRate(){ + return encoder1.getFrequency() - encoder2.getFrequency(); + } + + public void reset(){ + encoder1.reset(); + encoder2.reset(); + } + + public void setDistancePerRotation(double distance){ + encoder1.setDistancePerRotation(distance); + encoder2.setDistancePerRotation(distance); + } + + public double getDistancePerRotation(){ + return encoder1.getDistancePerRotation(); + } + + public double getDistance(){ + return encoder1.getDistance() - encoder2.getDistance(); + } +}