diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 2aaab01..58c4307 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -1,9 +1,16 @@ { - "robotWidth": 0.9, - "robotLength": 0.9, + "robotWidth": 0.558, + "robotLength": 0.558, "holonomicMode": true, - "pathFolders": [], - "autoFolders": [], + "pathFolders": [ + "TEST", + "MID", + "BTM", + "TOP" + ], + "autoFolders": [ + "TEST" + ], "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, diff --git a/README.md b/README.md index 5dc92e3..bf15ec5 100644 --- a/README.md +++ b/README.md @@ -7,3 +7,21 @@ our base code is derived from [this example swerve code](https://github.com/dirt ## Play Structure Our robot is designed to be a fast cycle, floor pickup, and speaker shooting bot. We are (planning) on using a 4 bar extension with 2 pairs of bars with 2 inch compliant wheels as our intake. The intake will directly feed to our launcher, which will have 2 pairs of wheels connected with a belt. The launcher itself will be variable angle, allowing us to be able to shoot from up close, and afar. We will have a camera on the front of the launcher to see the april tags on the speaker, and will use auto alignment to shoot. + +| Color | Code | Name | Function | +| --- | --- | --- | --- | +| 🟪 Purple | D | Default | Default Mode. Intake in, no note, normal operation. | +| Intake Modes | +| 🟥 Red Blink | IO | Intake Out | Intake out and running, waiting for note pickup, reduced movemnt and rotation speed.| +| 🟥 Red | IL | Intake Loaded | Intake out with note in, ready to retract, slightly reduced movement speed.| +| 🟧 Orange | R | Ready | Intake in with note, searching for speaker april tags | +| Scoring Modes | +| 🟧 Orange Blink | R | Ready | Intake in with note, speaker april tags found, ready to lock | +| 🟨 Yellow | KP | Lock Pending| Locking on speaker, speed greatly reduced, no rotation control | +| 🟩 Green | K | Locked | locked, preparing to shoot. | +| 🟦 Blue | L | Launching | Cycling launcher | +| Endgame Modes | +| 🟨🟥 Yellow Red | CE | Climber Extending | Extending climber decreased speed | +| 🟨🟧 Yellow Orange | CR | Climber Ready | Climber ready to climb, greatly decreased speed | +| 🟨🟦 Yellow Blue | C | Climbing | Climbing chain, movement disabled | +| 🟨🟩 Yellow Green | C | Climber Locked | Climbing commplete, locked | diff --git a/src/main/deploy/pathplanner/autos/AnadaAuto.auto b/src/main/deploy/pathplanner/autos/AnadaAuto.auto deleted file mode 100644 index 40dd28b..0000000 --- a/src/main/deploy/pathplanner/autos/AnadaAuto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 2, - "y": 2 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Swaws" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Btm4.auto b/src/main/deploy/pathplanner/autos/Btm4.auto new file mode 100644 index 0000000..c00f446 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Btm4.auto @@ -0,0 +1,79 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.78, + "y": 4.65 + }, + "rotation": -61.189206257026925 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "BTM-TO-3" + } + }, + { + "type": "path", + "data": { + "pathName": "BTM-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "BTM-TO-M5" + } + }, + { + "type": "path", + "data": { + "pathName": "MB-TO-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "BTM-TO-M4" + } + }, + { + "type": "path", + "data": { + "pathName": "MB-TO-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/BtmMain.auto b/src/main/deploy/pathplanner/autos/BtmMain.auto new file mode 100644 index 0000000..2f261e7 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/BtmMain.auto @@ -0,0 +1,67 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.78, + "y": 4.65 + }, + "rotation": -61.189206257026925 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "BTM-TO-3" + } + }, + { + "type": "path", + "data": { + "pathName": "BTM-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "BTM-TO-M5" + } + }, + { + "type": "path", + "data": { + "pathName": "MB-TO-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "BTM-TAXI" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Btmalt.auto b/src/main/deploy/pathplanner/autos/Btmalt.auto new file mode 100644 index 0000000..302dad8 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Btmalt.auto @@ -0,0 +1,61 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7275269251393202, + "y": 4.630885159206216 + }, + "rotation": -60.10109816138542 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "BTM-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "BTM-TO-M4" + } + }, + { + "type": "path", + "data": { + "pathName": "MB-TO-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "BTM-TO-M5" + } + }, + { + "type": "path", + "data": { + "pathName": "MB-TO-SHOOT" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Mid4.auto b/src/main/deploy/pathplanner/autos/Mid4.auto new file mode 100644 index 0000000..e188d68 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Mid4.auto @@ -0,0 +1,85 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.171904793952038, + "y": 5.531335051274091 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "M-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "M-TO-2" + } + }, + { + "type": "path", + "data": { + "pathName": "M-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "M-TO-3" + } + }, + { + "type": "path", + "data": { + "pathName": "M-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "M-TO-1" + } + }, + { + "type": "path", + "data": { + "pathName": "M-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/MidMain.auto b/src/main/deploy/pathplanner/autos/MidMain.auto new file mode 100644 index 0000000..f637a4a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/MidMain.auto @@ -0,0 +1,61 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "M-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "M-TO-2" + } + }, + { + "type": "path", + "data": { + "pathName": "M-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "M-TO-3" + } + }, + { + "type": "path", + "data": { + "pathName": "M-SHOOT" + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TestAuto.auto b/src/main/deploy/pathplanner/autos/TestAuto.auto deleted file mode 100644 index ed5313f..0000000 --- a/src/main/deploy/pathplanner/autos/TestAuto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.8912450873334795, - "y": 3.508246332731982 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Swaws" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TopMain.auto b/src/main/deploy/pathplanner/autos/TopMain.auto new file mode 100644 index 0000000..55266dd --- /dev/null +++ b/src/main/deploy/pathplanner/autos/TopMain.auto @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.8795509328910394, + "y": 6.689056341075646 + }, + "rotation": 56.907309424713056 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.0 + } + }, + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + }, + { + "type": "path", + "data": { + "pathName": "T-SHOOT" + } + }, + { + "type": "path", + "data": { + "pathName": "T-TO-2" + } + }, + { + "type": "path", + "data": { + "pathName": "T-SHOOT" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/bigboyauto.auto b/src/main/deploy/pathplanner/autos/bigboyauto.auto deleted file mode 100644 index e44c808..0000000 --- a/src/main/deploy/pathplanner/autos/bigboyauto.auto +++ /dev/null @@ -1,37 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.522729427225236, - "y": 5.531335051274091 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Oval" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "TheBoxer" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/test.auto similarity index 65% rename from src/main/deploy/pathplanner/autos/New Auto.auto rename to src/main/deploy/pathplanner/autos/test.auto index e143544..9a003af 100644 --- a/src/main/deploy/pathplanner/autos/New Auto.auto +++ b/src/main/deploy/pathplanner/autos/test.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.6513651260920754, - "y": 5.566417514601411 + "x": 1.183598948394478, + "y": 5.5430292057165325 }, "rotation": 0 }, @@ -14,18 +14,18 @@ { "type": "path", "data": { - "pathName": "TestPath" + "pathName": "M-SHOOT" } }, { - "type": "path", + "type": "named", "data": { - "pathName": "TheBoxer" + "name": "LaunchASAP" } } ] } }, - "folder": null, + "folder": "TEST", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BTM-SHOOT.path b/src/main/deploy/pathplanner/paths/BTM-SHOOT.path new file mode 100644 index 0000000..6d42fb5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BTM-SHOOT.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.4993411183403562, + "y": 4.034483282641779 + }, + "prevControl": null, + "nextControl": { + "x": 1.2537638750491178, + "y": 4.104648209296418 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.9029392417759192, + "y": 4.256672217048139 + }, + "prevControl": { + "x": 1.1952931028369176, + "y": 4.139730672623739 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "BTM SHOOT" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -58.39249775375112, + "rotateFast": false + }, + "reversed": false, + "folder": "BTM", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BTM-TAXI.path b/src/main/deploy/pathplanner/paths/BTM-TAXI.path new file mode 100644 index 0000000..6ac425c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BTM-TAXI.path @@ -0,0 +1,64 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.943718987153074, + "y": 2.6311847495489866 + }, + "prevControl": null, + "nextControl": { + "x": 2.9437189871530745, + "y": 2.6311847495489866 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.890346316305168, + "y": 0.7835083476434771 + }, + "prevControl": { + "x": 5.890346316305168, + "y": 0.7835083476434771 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 5.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": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "BTM", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BTM-TO-2.path b/src/main/deploy/pathplanner/paths/BTM-TO-2.path new file mode 100644 index 0000000..642b0a9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BTM-TO-2.path @@ -0,0 +1,63 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.9029392417759192, + "y": 4.256672217048139 + }, + "prevControl": null, + "nextControl": { + "x": 1.9029392417759186, + "y": 4.256672217048139 + }, + "isLocked": false, + "linkedName": "BTM SHOOT" + }, + { + "anchor": { + "x": 2.4699559370628714, + "y": 5.601499977928731 + }, + "prevControl": { + "x": 1.4699559370628714, + "y": 5.601499977928731 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "BTM", + "previewStartingState": { + "rotation": -57.17145820858741, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BTM-TO-3.path b/src/main/deploy/pathplanner/paths/BTM-TO-3.path new file mode 100644 index 0000000..72d8c43 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BTM-TO-3.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.9029392417759192, + "y": 4.256672217048139 + }, + "prevControl": null, + "nextControl": { + "x": 1.1602106395095981, + "y": 4.256672217048139 + }, + "isLocked": false, + "linkedName": "BTM SHOOT" + }, + { + "anchor": { + "x": 2.4699559370628714, + "y": 4.128036518181299 + }, + "prevControl": { + "x": 1.6981417438618356, + "y": 4.139730672623739 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.15, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RunIntake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "BTM", + "previewStartingState": { + "rotation": -57.17145820858741, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BTM-TO-M4.path b/src/main/deploy/pathplanner/paths/BTM-TO-M4.path new file mode 100644 index 0000000..2ba3da8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BTM-TO-M4.path @@ -0,0 +1,88 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.9029392417759192, + "y": 4.256672217048139 + }, + "prevControl": null, + "nextControl": { + "x": 1.9029392417759181, + "y": 4.256672217048139 + }, + "isLocked": false, + "linkedName": "BTM SHOOT" + }, + { + "anchor": { + "x": 7.942820216124763, + "y": 2.4440782784699477 + }, + "prevControl": { + "x": 5.416882856557737, + "y": 0.2338830888487989 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.75, + "rotationDegrees": 7.329212122874697, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.8, + "constraints": { + "maxVelocity": 5.5, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + } + ], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.55, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RunIntake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "BTM", + "previewStartingState": { + "rotation": -57.17145820858741, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BTM-TO-M5.path b/src/main/deploy/pathplanner/paths/BTM-TO-M5.path new file mode 100644 index 0000000..5c38c08 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BTM-TO-M5.path @@ -0,0 +1,82 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.9029392417759192, + "y": 4.256672217048139 + }, + "prevControl": null, + "nextControl": { + "x": 1.3940937283583967, + "y": 2.1049477996391905 + }, + "isLocked": false, + "linkedName": "BTM SHOOT" + }, + { + "anchor": { + "x": 7.884349443912563, + "y": 0.7835083476434771 + }, + "prevControl": { + "x": 5.440271165442615, + "y": 0.573013567679558 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.85, + "constraints": { + "maxVelocity": 5.5, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + } + ], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.45, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RunIntake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "BTM", + "previewStartingState": { + "rotation": -57.17145820858741, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/M-SHOOT.path b/src/main/deploy/pathplanner/paths/M-SHOOT.path new file mode 100644 index 0000000..4c9740b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/M-SHOOT.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.183598948394478, + "y": 5.5430292057165325 + }, + "prevControl": null, + "nextControl": { + "x": 1.2435065695868026, + "y": 5.546357406893884 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.5928943538798759, + "y": 5.5430292057165325 + }, + "prevControl": { + "x": 1.5333576812770562, + "y": 5.535587121641179 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "MID SHOOT" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "MID", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/M-TO-1.path b/src/main/deploy/pathplanner/paths/M-TO-1.path new file mode 100644 index 0000000..bb9a10b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/M-TO-1.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.5928943538798759, + "y": 5.5430292057165325 + }, + "prevControl": null, + "nextControl": { + "x": 1.5812001994374358, + "y": 6.303149244475128 + }, + "isLocked": false, + "linkedName": "MID SHOOT" + }, + { + "anchor": { + "x": 2.5752033270448305, + "y": 6.841080348827365 + }, + "prevControl": { + "x": 1.5578118905525564, + "y": 6.794303731057606 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.05, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RunIntake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 22.47943439710317, + "rotateFast": false + }, + "reversed": false, + "folder": "MID", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/M-TO-2.path b/src/main/deploy/pathplanner/paths/M-TO-2.path new file mode 100644 index 0000000..0004efc --- /dev/null +++ b/src/main/deploy/pathplanner/paths/M-TO-2.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.5928943538798759, + "y": 5.5430292057165325 + }, + "prevControl": null, + "nextControl": { + "x": 1.791694979401355, + "y": 5.5884625760165525 + }, + "isLocked": false, + "linkedName": "MID SHOOT" + }, + { + "anchor": { + "x": 2.5284267092750707, + "y": 5.556066607628711 + }, + "prevControl": { + "x": 2.388096855965792, + "y": 5.5430292057165325 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.05, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RunIntake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 6.159545400201877, + "rotateFast": false + }, + "reversed": false, + "folder": "MID", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/M-TO-3.path b/src/main/deploy/pathplanner/paths/M-TO-3.path new file mode 100644 index 0000000..75e82f4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/M-TO-3.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.5928943538798759, + "y": 5.5430292057165325 + }, + "prevControl": null, + "nextControl": { + "x": 1.3005404928188773, + "y": 4.420390379242298 + }, + "isLocked": false, + "linkedName": "MID SHOOT" + }, + { + "anchor": { + "x": 2.61028579037215, + "y": 4.326837143702777 + }, + "prevControl": { + "x": 2.493344245947751, + "y": 4.736132549188175 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.05, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RunIntake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -44.999999999999964, + "rotateFast": false + }, + "reversed": false, + "folder": "MID", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/MB-TO-SHOOT.path b/src/main/deploy/pathplanner/paths/MB-TO-SHOOT.path new file mode 100644 index 0000000..7000475 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/MB-TO-SHOOT.path @@ -0,0 +1,64 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.182700177366167, + "y": 1.567016695286953 + }, + "prevControl": null, + "nextControl": { + "x": 5.919731497582651, + "y": 1.4734634597474332 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.9029392417759192, + "y": 4.256672217048139 + }, + "prevControl": { + "x": 1.0315749406427586, + "y": 2.432384124027509 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "BTM SHOOT" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.75, + "constraints": { + "maxVelocity": 5.0, + "maxAcceleration": 5.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": -59.264512298079936, + "rotateFast": false + }, + "reversed": false, + "folder": "BTM", + "previewStartingState": { + "rotation": 1.5481576989780002, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path index 82cb879..fde62e2 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -59,7 +59,7 @@ "rotateFast": false }, "reversed": false, - "folder": null, + "folder": "TEST", "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Oval.path b/src/main/deploy/pathplanner/paths/Oval.path index 3a0f608..6a9b60c 100644 --- a/src/main/deploy/pathplanner/paths/Oval.path +++ b/src/main/deploy/pathplanner/paths/Oval.path @@ -59,7 +59,7 @@ "rotateFast": false }, "reversed": false, - "folder": null, + "folder": "TEST", "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SmartLaunchMid.path b/src/main/deploy/pathplanner/paths/SmartLaunchMid.path new file mode 100644 index 0000000..803b271 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SmartLaunchMid.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.160210639509598, + "y": 5.531335051274091 + }, + "prevControl": null, + "nextControl": { + "x": 1.5812001994374358, + "y": 5.55472336015897 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.5928943538798759, + "y": 5.5430292057165325 + }, + "prevControl": { + "x": 1.3005404928188773, + "y": 5.5430292057165325 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "MID SHOOT" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 1.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "LaunchASAP" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Swaws.path b/src/main/deploy/pathplanner/paths/Swaws.path index 99c1403..5127a38 100644 --- a/src/main/deploy/pathplanner/paths/Swaws.path +++ b/src/main/deploy/pathplanner/paths/Swaws.path @@ -59,7 +59,7 @@ "rotateFast": false }, "reversed": false, - "folder": null, + "folder": "TEST", "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/T-SHOOT.path b/src/main/deploy/pathplanner/paths/T-SHOOT.path new file mode 100644 index 0000000..a6efb21 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/T-SHOOT.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.6396709716496354, + "y": 6.689056341075646 + }, + "prevControl": null, + "nextControl": { + "x": 1.008186631757879, + "y": 6.607197259978566 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.8795509328910394, + "y": 6.689056341075646 + }, + "prevControl": { + "x": 1.2420697206066778, + "y": 6.747527113287845 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "TOP SHOOT" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 58.51656810879132, + "rotateFast": false + }, + "reversed": false, + "folder": "TOP", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/T-TO-1.path b/src/main/deploy/pathplanner/paths/T-TO-1.path new file mode 100644 index 0000000..7fbf13c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/T-TO-1.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.8795509328910394, + "y": 6.689056341075646 + }, + "prevControl": null, + "nextControl": { + "x": 1.3707054194735169, + "y": 6.817692039942485 + }, + "isLocked": false, + "linkedName": "TOP SHOOT" + }, + { + "anchor": { + "x": 2.5518150181599504, + "y": 7.004798511021523 + }, + "prevControl": { + "x": 2.025578068250153, + "y": 6.993104356579082 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RunIntake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 5.662290558173469, + "rotateFast": false + }, + "reversed": false, + "folder": "TOP", + "previewStartingState": { + "rotation": 16.50838708204789, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/T-TO-2.path b/src/main/deploy/pathplanner/paths/T-TO-2.path new file mode 100644 index 0000000..960ca4a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/T-TO-2.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.8795509328910394, + "y": 6.689056341075646 + }, + "prevControl": null, + "nextControl": { + "x": 1.6162826627647553, + "y": 6.303149244475128 + }, + "isLocked": false, + "linkedName": "TOP SHOOT" + }, + { + "anchor": { + "x": 2.69214487146923, + "y": 5.84707722121997 + }, + "prevControl": { + "x": 1.9194615181407153, + "y": 6.091153187927382 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RunIntake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -55.00797980144137, + "rotateFast": false + }, + "reversed": false, + "folder": "TOP", + "previewStartingState": { + "rotation": 14.281095735970798, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/T-TO-3.path b/src/main/deploy/pathplanner/paths/T-TO-3.path new file mode 100644 index 0000000..8463bc3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/T-TO-3.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.8795509328910394, + "y": 6.689056341075646 + }, + "prevControl": null, + "nextControl": { + "x": 2.844168879220949, + "y": 6.221290163378049 + }, + "isLocked": false, + "linkedName": "TOP SHOOT" + }, + { + "anchor": { + "x": 2.5167325548326307, + "y": 4.081259900411539 + }, + "prevControl": { + "x": 1.1251281761822782, + "y": 4.057871591526659 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.3, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RunIntake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "TOP", + "previewStartingState": { + "rotation": 2.045408488887245, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/T-TO-M1.path b/src/main/deploy/pathplanner/paths/T-TO-M1.path new file mode 100644 index 0000000..6001ca6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/T-TO-M1.path @@ -0,0 +1,97 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.8795509328910394, + "y": 6.689056341075646 + }, + "prevControl": null, + "nextControl": { + "x": 1.9889153137819637, + "y": 6.199087072848821 + }, + "isLocked": false, + "linkedName": "TOP SHOOT" + }, + { + "anchor": { + "x": 2.917573402714373, + "y": 6.286398158381903 + }, + "prevControl": { + "x": 2.5200511662284586, + "y": 6.304887564730085 + }, + "nextControl": { + "x": 3.2793314510542197, + "y": 6.269572202645167 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.9545143705672015, + "y": 7.226987445427883 + }, + "prevControl": { + "x": 7.483034508688559, + "y": 6.690794661330605 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4, + "rotationDegrees": 0, + "rotateFast": false + }, + { + "waypointRelativePos": 1.0, + "rotationDegrees": 0.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 1.2, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RunIntake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 27.050597007086274, + "rotateFast": false + }, + "reversed": false, + "folder": "TOP", + "previewStartingState": { + "rotation": 25.559965171823848, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/T-TO-M2.path b/src/main/deploy/pathplanner/paths/T-TO-M2.path new file mode 100644 index 0000000..5dfa5e0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/T-TO-M2.path @@ -0,0 +1,97 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.8795509328910394, + "y": 6.689056341075646 + }, + "prevControl": null, + "nextControl": { + "x": 1.9889153137819633, + "y": 6.199087072848821 + }, + "isLocked": false, + "linkedName": "TOP SHOOT" + }, + { + "anchor": { + "x": 2.917573402714373, + "y": 6.286398158381903 + }, + "prevControl": { + "x": 2.5200511662284586, + "y": 6.304887564730085 + }, + "nextControl": { + "x": 3.2793314510542197, + "y": 6.269572202645167 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.001290988336962, + "y": 5.88215968454729 + }, + "prevControl": { + "x": 7.104554780450131, + "y": 6.0393196385068375 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4, + "rotationDegrees": 0, + "rotateFast": false + }, + { + "waypointRelativePos": 1.0, + "rotationDegrees": 0.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 1.15, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RunIntake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -14.62087398863165, + "rotateFast": false + }, + "reversed": false, + "folder": "TOP", + "previewStartingState": { + "rotation": 26.872647952447924, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/T-TO-M3.path b/src/main/deploy/pathplanner/paths/T-TO-M3.path new file mode 100644 index 0000000..dd71b42 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/T-TO-M3.path @@ -0,0 +1,95 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.8795509328910394, + "y": 6.689056341075646 + }, + "prevControl": null, + "nextControl": { + "x": 1.9889153137819628, + "y": 6.199087072848821 + }, + "isLocked": false, + "linkedName": "TOP SHOOT" + }, + { + "anchor": { + "x": 2.917573402714373, + "y": 6.286398158381903 + }, + "prevControl": { + "x": 2.5200511662284586, + "y": 6.304887564730085 + }, + "nextControl": { + "x": 3.2793314510542197, + "y": 6.269572202645167 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.162464216820325, + "y": 6.286398158381903 + }, + "prevControl": { + "x": 5.260329639621656, + "y": 6.706974187170742 + }, + "nextControl": { + "x": 7.064598794018995, + "y": 5.865822129593065 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.012985142779401, + "y": 4.280060525933018 + }, + "prevControl": { + "x": 7.11624893489257, + "y": 4.437220479892566 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4, + "rotationDegrees": 0, + "rotateFast": false + }, + { + "waypointRelativePos": 1.0, + "rotationDegrees": 0.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -25.08359400619089, + "rotateFast": false + }, + "reversed": false, + "folder": "TOP", + "previewStartingState": { + "rotation": 26.003345844511543, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TestPath.path b/src/main/deploy/pathplanner/paths/TestPath.path index d38e095..13991ca 100644 --- a/src/main/deploy/pathplanner/paths/TestPath.path +++ b/src/main/deploy/pathplanner/paths/TestPath.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.6513651260920754, - "y": 5.566417514601411 + "x": 1.7434960996048117, + "y": 2.14477113638912 }, "prevControl": null, "nextControl": { - "x": 2.7856981070087494, - "y": 1.3448277608805934 + "x": 2.8778290805214857, + "y": -2.0768186173316976 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.101140686611857, - "y": 5.85877137566241 + "x": 4.9976316168848545, + "y": 2.634740404615945 }, "prevControl": { - "x": 5.101140686611857, - "y": 6.95877137566241 + "x": 4.9976316168848545, + "y": 3.734740404615947 }, "nextControl": null, "isLocked": false, @@ -59,7 +59,7 @@ "rotateFast": false }, "reversed": false, - "folder": null, + "folder": "TEST", "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TheBoxer.path b/src/main/deploy/pathplanner/paths/TheBoxer.path index f92965a..1b7c25c 100644 --- a/src/main/deploy/pathplanner/paths/TheBoxer.path +++ b/src/main/deploy/pathplanner/paths/TheBoxer.path @@ -91,7 +91,7 @@ "rotateFast": false }, "reversed": false, - "folder": null, + "folder": "TEST", "previewStartingState": null, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 339347b..df70e97 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -9,6 +9,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.util.Color; import frc.lib.util.COTSTalonFXSwerveConstants; import frc.lib.util.SwerveModuleConstants; @@ -22,8 +23,8 @@ public static final class Swerve { COTSTalonFXSwerveConstants.SDS.MK4i.Falcon500(COTSTalonFXSwerveConstants.SDS.MK4i.driveRatios.L2); /* Drivetrain Constants */ - public static final double trackWidth = Units.inchesToMeters(24.5); //TODOx: This must be tuned to specific robot - public static final double wheelBase = Units.inchesToMeters(24.5); //TODOx: This must be tuned to specific robot + public static final double trackWidth = Units.inchesToMeters(16.75); //TODO: This must be tuned to specific robot + public static final double wheelBase = Units.inchesToMeters(16.75); //TODO: This must be tuned to specific robot public static final double wheelCircumference = chosenModule.wheelCircumference; /* Swerve Kinematics @@ -129,9 +130,97 @@ public static final class Mod3 { //TODOx: This must be tuned to specific robot D } } + public static final class Intake { + public static final int RollerCAN1 = 9; + public static final int RollerCAN2 = 10; + public static final int ActuatorCAN = 11; + + public static final int forwardLim = 60; + public static final int forwardAmpLim = 30; + public static final int reversLim = 0; + + public static final double noteDetectionDistance = 30; + public static final double barP = 5e-5; + public static final double barI = 1e-6; + public static final double barD = 0; + + public static final double barMaxOut = 1; + public static final double barMinOut = -1; + + public static final double barMaxRPM = 570; + public static final double barMaxVel = 200; + public static final double barMaxAcc = 150; + + public static final double barOutPoint = 1000; + public static final double barInPoint = -1000; + + public static final double intakeVel = 0.8; + public static final double intakeNudge = 0.2; + + public static final double acuateVel = 0.8; + public static final double acuateManual = 0.2; + + public static final double OverRun = 0.75; + } + public static final class Launcher { + public static final int RightCAN = 13; + public static final int LeftCAN = 12; + + public static final double LaunchV = 100; + public static final double LaunchP = 1; + + public static final double LaunchASAPTime = 1; + public static final double LaunchStopTime = 1; + + public static final double P = 0.08; + public static final double I = 0.05; + public static final double D = 0.00; + + public static final double FF = 0.000156; + + public static final double MaxOut = 1; + public static final double MinOut = -1; + + public static final double MaxRPM = 2000; + public static final double MaxVel = 1500; + public static final double MaxAcc = 500; + + public static final double OutPoint = 1000; + public static final double InPoint = -1000; + } + + // holds constants for mode system + public static final class Mode { + public static final double blinkTime = 5.0; + // Mode modifiers, in percentages, controlor input is multiplied by these + // max forward speed (1) * 40% speed (.40) = .40 output + public static final class Modifiers { + public static final class Intake { + public static final double driveSpeed = 0.4; + public static final double rotSpeed = 0.4; + } + public static final class Lock { + public static final double driveSpeed = 0.3; + public static final double rotSpeed = 0.0; + } + } + + public static final class Colors { + public static final Color D = Color.kBlue; // Default Color + public static final Color IO = Color.kRed; // Intake Out Color + public static final Color IF = Color.kPurple; // Intake Fix Color + public static final Color IR = Color.kOrange; // Intake Retracting Color + public static final Color IA = Color.kPink; // Intake Amp Color + public static final Color N = Color.kYellow; // Note Color + public static final Color LS = Color.kGreen; // Launcher Spinup Color + public static final Color L = Color.kAqua; // Color Color + public static final Color DIS = Color.kPurple; //Disable Color + } + } + public static final class AutoConstants { //TODO: The below constants are used in the example auto, and must be tuned to specific robot - public static final double kMaxSpeedMetersPerSecond = 2; - public static final double kMaxAccelerationMetersPerSecondSquared = 2; + public static final double kMaxSpeedMetersPerSecond = 1; + public static final double kMaxAccelerationMetersPerSecondSquared = 0.5; public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; @@ -144,4 +233,5 @@ public static final class AutoConstants { //TODO: The below constants are used i new TrapezoidProfile.Constraints( kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared); } + } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d5fec09..cef14e5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,11 +4,6 @@ package frc.robot; -import com.ctre.phoenix6.Orchestra; -import com.ctre.phoenix6.hardware.TalonFX; - -import edu.wpi.first.wpilibj.AddressableLED; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -51,11 +46,14 @@ public void robotPeriodic() { // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); + m_robotContainer.updateInfo(); } /** This function is called once each time the robot enters Disabled mode. */ @Override - public void disabledInit() {} + public void disabledInit() { + m_robotContainer.disabledInit(); + } @Override public void disabledPeriodic() {} @@ -87,6 +85,7 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } + m_robotContainer.teleopInit(); } /** This function is called periodically during operator control. */ @@ -107,4 +106,5 @@ public void testInit() { public void testPeriodic() { m_robotContainer.testPeriodic(); } + } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4f901f3..c792693 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,10 +1,29 @@ package frc.robot; +import org.opencv.core.Mat; +import org.opencv.core.Point; +import org.opencv.core.MatOfPoint; +import org.opencv.core.Scalar; +import org.opencv.imgproc.Imgproc; + import com.ctre.phoenix6.Orchestra; import com.pathplanner.lib.auto.AutoBuilder; - +import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.path.PathPlannerPath; +import com.revrobotics.CANSparkMax; + +import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.cscore.CvSink; +import edu.wpi.first.cscore.CvSource; +import edu.wpi.first.cscore.UsbCamera; +import edu.wpi.first.networktables.DoubleTopic; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -12,6 +31,9 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import java.util.ArrayList; +import java.util.List; + import frc.robot.autos.*; import frc.robot.commands.*; import frc.robot.subsystems.*; @@ -28,15 +50,17 @@ public class RobotContainer { // This needs to be cleaned up to a sendable choser for the smart dashboard to use // also once perephials are added, will need to add 2nd note manipulation controler - // private final Joystick driver = new Joystick(1); private final Joystick driver = new Joystick(0); + private final XboxController secondary = new XboxController(1); + private final PowerDistribution PDP = new PowerDistribution(); // /* XBOX CONTROLOR */ - // /* Drive Controls */ - // private final int translationAxis = XboxController.Axis.kLeftY.value; - // private final int strafeAxis = XboxController.Axis.kLeftX.value; - // private final int rotationAxis = XboxController.Axis.kRightX.value; + /* Drive Controls */ + private final int sLy = XboxController.Axis.kLeftY.value; + private final int sLX = XboxController.Axis.kLeftX.value; + private final int sRX = XboxController.Axis.kRightX.value; + private final int sRY = XboxController.Axis.kRightY.value; // /* Driver Buttons */ // private final JoystickButton zeroGyro = new JoystickButton(driver, XboxController.Button.kY.value); @@ -47,22 +71,41 @@ public class RobotContainer { private final int translationAxis = Joystick.AxisType.kY.value; private final int strafeAxis = Joystick.AxisType.kX.value; private final int rotationAxis = Joystick.AxisType.kTwist.value; + private final int throttleAxis = 2; /* Driver Buttons */ - private final JoystickButton zeroGyro = new JoystickButton(driver, Joystick.ButtonType.kTrigger.value); - private final JoystickButton robotCentric = new JoystickButton(driver, Joystick.ButtonType.kTop.value); + private final JoystickButton zeroGyro = new JoystickButton(driver, 2); + private final JoystickButton robotCentric = new JoystickButton(driver, 7); + private final JoystickButton Test = new JoystickButton(driver, 3); + + // private final NamedCommands NamedCommands = new NamedCommands(); + + private final NetworkTableInstance tableInstance = NetworkTableInstance.getDefault(); /* Subsystems */ - private final LEDS theLEDs = new LEDS(9,60); + private final LEDS theLEDs = new LEDS(0,60); private final Band theBand = new Band(); private final Swerve s_Swerve = new Swerve(theLEDs); + public Intake intake = new Intake(Constants.Intake.ActuatorCAN, Constants.Intake.RollerCAN1, Constants.Intake.RollerCAN2,secondary); + + public Launcher launcher = new Launcher(Constants.Launcher.LeftCAN,Constants.Launcher.RightCAN); + + public DigitalInput LIM = new DigitalInput(0); + + // private final Intake s_Intake = new Intake(#, #, #, #); + + // private final Launcher s_Launcher = new Launcher(#, #, #); + // dashboard selector for autos and music private final SendableChooser autoChooser; private final SendableChooser musiChooser; + + private Thread m_visionThread; + /** The container for the robot. Contains subsystems, OI devices, and commands. * this is the main class most things stem from. only thing above this is the robot.java that * connects this to the driverstation @@ -77,23 +120,78 @@ public RobotContainer() { () -> -driver.getRawAxis(translationAxis), () -> -driver.getRawAxis(strafeAxis), () -> -driver.getRawAxis(rotationAxis), - () -> robotCentric.getAsBoolean() + () -> -driver.getRawAxis(throttleAxis), + () -> robotCentric.getAsBoolean(), + theLEDs ) ); + + intake.setDefaultCommand(new IntakeDefault(intake,LIM)); + + NamedCommands.registerCommand("RunIntake", new IntakeRun(intake, LIM, new JoystickButton(secondary, 3), theLEDs)); + NamedCommands.registerCommand("EndIntake", new IntakeDefault(intake, LIM)); + NamedCommands.registerCommand("LaunchASAP", new LaunchASAP(intake,launcher,theLEDs)); + // Configure the button bindings configureButtonBindings(); //Build the auto chooser - autoChooser = AutoBuilder.buildAutoChooser(); - // autoChooser = AutoBuilder.buildAutoChooser("My Default Auto"); + // autoChooser = AutoBuilder.buildAutoChooser(); + autoChooser = AutoBuilder.buildAutoChooser("Btm4"); SmartDashboard.putData("Auto Chooser", autoChooser); musiChooser = theBand.Buildchoser(); SmartDashboard.putData("Music Choser",musiChooser); - theLEDs.SetFull(255, 60, 0); - + secondary.getPOV(); + + // theLEDs.SetFull(255, 60, 0); + theLEDs.setMode("D"); + + + m_visionThread = + new Thread( + () -> { + // Get the UsbCamera from CameraServer + UsbCamera camera = CameraServer.startAutomaticCapture(); + // Set the resolution + camera.setResolution(640/2, 480/2); + + // Get a CvSink. This will capture Mats from the camera + CvSink cvSink = CameraServer.getVideo(); + // Setup a CvSource. This will send images back to the Dashboard + CvSource outputStream = CameraServer.putVideo("Note", 640/2, 480/2); + + // Mats are very memory expensive. Lets reuse this Mat. + Mat mat = new Mat(); + + // This cannot be 'true'. The program will never exit if it is. This + // lets the robot stop this thread when restarting robot code or + // deploying. + while (!Thread.interrupted()) { + // Tell the CvSink to grab a frame from the camera and put it + // in the source mat. If there is an error notify the output. + if (cvSink.grabFrame(mat) == 0) { + // Send the output the error. + outputStream.notifyError(cvSink.getError()); + // skip the rest of the current iteration + continue; + } + // Imgproc.rectangle( + // mat, new Point(100, 100), new Point(400, 400), new Scalar(255, 255, 255), 5); + // List list = new ArrayList(); + // list.add( new MatOfPoint ( + // new Point(208, 71), new Point(421, 161), + // new Point(226, 232), new Point(332, 52), + // new Point(363, 250))); + // Imgproc.polylines(mat, list, false, new Scalar(255,255,255), 2); + // // Give the output stream a new image to display + outputStream.putFrame(mat); + } + }); + m_visionThread.setDaemon(true); + m_visionThread.start(); } /** @@ -105,9 +203,49 @@ public RobotContainer() { private void configureButtonBindings() { /** Driver Buttons */ zeroGyro.onTrue(new InstantCommand(() -> s_Swerve.zeroHeading())); + // new JoystickButton(secondary, 4).onTrue(new LaunchASAP(intake,launcher,theLEDs)); + new JoystickButton(secondary, 4).onTrue(new LaunchControled(intake,launcher,theLEDs,new JoystickButton(secondary, 4),new JoystickButton(secondary, 6))); + new JoystickButton(secondary, 2).onTrue(new IntakeRun(intake, LIM, new JoystickButton(secondary, 2),theLEDs)); + new JoystickButton(secondary, 3).onTrue(new IntakeFix(intake, LIM, new JoystickButton(secondary, 3),new JoystickButton(secondary, 6),theLEDs)); + new JoystickButton(secondary, 1).onTrue(new IntakeAmp(intake, LIM, new JoystickButton(secondary, 1), new JoystickButton(secondary, 6),theLEDs)); + new JoystickButton(secondary, 8).onTrue(smartLaunch()); } - + public void updateInfo() { + SmartDashboard.putBoolean("IntakeDeployed", intake.getMode()); + SmartDashboard.putBoolean("Note", LIM.get()); + SmartDashboard.putBoolean("Launcher Spin", launcher.spin); + + // {"D","IO","IR","L","LS"}; + SmartDashboard.putBoolean("D", theLEDs.getMode() == "D"); + SmartDashboard.putBoolean("IO", theLEDs.getMode() == "IO"); + SmartDashboard.putBoolean("IR", theLEDs.getMode() == "IR"); + SmartDashboard.putBoolean("IF", theLEDs.getMode() == "IF"); + SmartDashboard.putBoolean("IA", theLEDs.getMode() == "IA"); + SmartDashboard.putBoolean("N", theLEDs.getMode() == "N"); + SmartDashboard.putBoolean("LS", theLEDs.getMode() == "LS"); + SmartDashboard.putBoolean("L", theLEDs.getMode() == "L"); + + SmartDashboard.putData("intake", intake); + SmartDashboard.putData("Launcher", launcher); + SmartDashboard.putData("Gyro",s_Swerve.gyro); + SmartDashboard.putBoolean("ControlorA", new JoystickButton(secondary, 1).getAsBoolean()); + SmartDashboard.putBoolean("ControlorB", new JoystickButton(secondary, 2).getAsBoolean()); + SmartDashboard.putBoolean("ControlorX", new JoystickButton(secondary, 3).getAsBoolean()); + + // System.out.println("Radio"); + // System.out.println(PDP.getCurrent(15)); + // System.out.println("RIO"); + // System.out.println(PDP.getCurrent(20)); + // System.out.println("Total"); + // System.out.println(PDP.getTotalCurrent()); + + intake.updateData(); + + double throtleVal = (driver.getRawAxis(throttleAxis)+1)/2; + SmartDashboard.putNumber("Throttle", throtleVal); + + } /** * Use this to pass the autonomous command to the main {@link Robot} class. @@ -128,14 +266,29 @@ public Command getAutonomousCommand() { return autoChooser.getSelected(); } + public Command smartLaunch() { + // Load the path you want to follow using its name in the GUI + PathPlannerPath path = PathPlannerPath.fromPathFile("Example Path"); + + // Create a path following command using AutoBuilder. This will also trigger event markers. + return AutoBuilder.followPath(path); + } + + public void teleopInit() { + theLEDs.SetFull(0, 0, 255); + theLEDs.setMode("D"); + theLEDs.updateMode(); + } + public void teleopPeriodic() { // theLEDs.rainbow(); + theLEDs.updateMode(); } public void autonomousPeriodic() { - theLEDs.rainbow(); + // theLEDs.rainbow(); } - + public void testInit() { theBand.play(musiChooser.getSelected()); } @@ -146,6 +299,7 @@ public void testPeriodic() { public void disabledInit () { theLEDs.SetFull(255, 60, 0); + theBand.orchestra.stop(); } // public void PlayMusic(int sel) { diff --git a/src/main/java/frc/robot/commands/IntakeAmp.java b/src/main/java/frc/robot/commands/IntakeAmp.java new file mode 100644 index 0000000..7595510 --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeAmp.java @@ -0,0 +1,96 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import java.util.function.BooleanSupplier; + +import com.revrobotics.CANSparkMax; + +import frc.robot.Constants; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.LEDS; + +public class IntakeAmp extends Command { + + private Intake s_Intake; + private DigitalInput LIM; + private JoystickButton overrideButton; + private JoystickButton PushButton; + // private Boolean end; + private Boolean bHold; + private Boolean end; + private Timer timer; + private LEDS theLEDs; + + /** + * Run intake untill note is obtained or manualy disabled + * + * @param s_Intake Intake object + */ + public IntakeAmp(Intake s_Intake, DigitalInput LIM, JoystickButton overrideButton, JoystickButton PushButton, LEDS theLEDs) { + this.s_Intake = s_Intake; + this.LIM = LIM; + this.overrideButton = overrideButton; + this.PushButton = PushButton; + this.theLEDs = theLEDs; + // this.end = false; + this.bHold = true; + this.end = false; + addRequirements(s_Intake); + } + + @Override + public void initialize() { + // tell the intake to extend + s_Intake.m_barMotor.setSoftLimit(CANSparkMax.SoftLimitDirection.kForward, Constants.Intake.forwardAmpLim); + System.out.println("Start IntakeAmp"); + s_Intake.setDeploy(true); + this.bHold = true; + this.end = false; + timer = new Timer(); + theLEDs.setMode("IA"); + // timer.start(); + } + + @Override + public void execute() { + if (bHold) bHold = overrideButton.getAsBoolean(); + if (PushButton.getAsBoolean() && !end) { + timer.start(); + end = true; + System.out.println("End1"); + s_Intake.pushIntake(false); + // theLEDs.setMode("IR"); + // System.out.println("set IR"); + + } + System.out.println(timer.get()); + System.out.println(timer.get() >= Constants.Intake.OverRun); + } + + @Override + public void end(boolean interrupted) { + System.out.println("End IntakeAmp"); + s_Intake.m_barMotor.setSoftLimit(CANSparkMax.SoftLimitDirection.kForward, Constants.Intake.forwardLim); + s_Intake.pushIntake(true); + if (end) { + theLEDs.setMode("N"); + System.out.println("set N"); + } else { + theLEDs.setMode("D"); + System.out.println("set D"); + } + } + + @Override + public boolean isFinished() { + // this should trigger when a note is detected, or manualy triggerd + // return(s_Intake.checkForNote() || switchButton.getAsBoolean()); + if (!LIM.get()) System.out.println("LIM"); + if (overrideButton.getAsBoolean()) System.out.println("OVER"); + return timer.get() >= Constants.Intake.OverRun || (overrideButton.getAsBoolean() && !bHold); + } + +} diff --git a/src/main/java/frc/robot/commands/IntakeDefault.java b/src/main/java/frc/robot/commands/IntakeDefault.java new file mode 100644 index 0000000..a870301 --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeDefault.java @@ -0,0 +1,55 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import java.util.function.BooleanSupplier; +import frc.robot.subsystems.Intake; + +public class IntakeDefault extends Command { + + private Intake s_Intake; + private DigitalInput LIM; + // private Boolean end; + private Boolean first; + + /** + * Run intake untill note is obtained or manualy disabled + * + * @param s_Intake Intake object + */ + public IntakeDefault(Intake s_Intake, DigitalInput LIM) { + this.s_Intake = s_Intake; + this.LIM = LIM; + + // this.end = false; + this.first = false; + addRequirements(s_Intake); + } + + @Override + public void initialize() { + // tell the intake to extend + System.out.println("Start IntakeDefault"); + s_Intake.setDeploy(false); + s_Intake.endIntake(); + } + + @Override + public void execute() { + s_Intake.nudge(); + } + + @Override + public void end(boolean interrupted) { + System.out.println("End IntakeDefault"); + } + + @Override + public boolean isFinished() { + // this should trigger when a note is detected, or manualy triggerd + // return(s_Intake.checkForNote() || switchButton.getAsBoolean()); + return false; + } + +} diff --git a/src/main/java/frc/robot/commands/IntakeFix.java b/src/main/java/frc/robot/commands/IntakeFix.java new file mode 100644 index 0000000..e411dbc --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeFix.java @@ -0,0 +1,100 @@ +package frc.robot.commands; + +import edu.wpi.first.networktables.PubSub; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import java.util.function.BooleanSupplier; + +import frc.robot.Constants; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.LEDS; + +public class IntakeFix extends Command { + + private Intake s_Intake; + private DigitalInput LIM; + private JoystickButton overrideButton; + private JoystickButton pushButton; + // private Boolean end; + private Boolean bHold; + private Boolean end; + private Boolean push; + private Timer timer; + private LEDS theLEDs; + + /** + * Run intake untill note is obtained or manualy disabled + * + * @param s_Intake Intake object + */ + public IntakeFix(Intake s_Intake, DigitalInput LIM, JoystickButton overrideButton, JoystickButton pushButton, LEDS theLEDs) { + this.s_Intake = s_Intake; + this.LIM = LIM; + this.overrideButton = overrideButton; + this.theLEDs = theLEDs; + this.pushButton = pushButton; + // this.end = false; + this.bHold = true; + this.push = false; + this.end = false; + addRequirements(s_Intake); + } + + @Override + public void initialize() { + // tell the intake to extend + System.out.println("Start IntakeFix"); + s_Intake.setDeploy(true); + // s_Intake.startIntake(); + this.bHold = true; + this.end = false; + this.push = false; + timer = new Timer(); + theLEDs.setMode("IF"); + // timer.start(); + } + + @Override + public void execute() { + if (bHold) bHold = overrideButton.getAsBoolean(); + if (!LIM.get() && !end) { + timer.start(); + // s_Intake.endIntake(); + end = true; + System.out.println("End1"); + theLEDs.setMode("IR"); + System.out.println("set IR"); + s_Intake.lightPull(false); + } + + s_Intake.nudge(); + + System.out.println(timer.get()); + System.out.println(timer.get() >= Constants.Intake.OverRun); + } + + @Override + public void end(boolean interrupted) { + System.out.println("End IntakeRun"); + s_Intake.lightPull(true); + if (end) { + theLEDs.setMode("N"); + System.out.println("set N"); + } else { + theLEDs.setMode("D"); + System.out.println("set D"); + } + } + + @Override + public boolean isFinished() { + // this should trigger when a note is detected, or manualy triggerd + // return(s_Intake.checkForNote() || switchButton.getAsBoolean()); + if (!LIM.get()) System.out.println("LIM"); + if (overrideButton.getAsBoolean()) System.out.println("OVER"); + return timer.get() >= Constants.Intake.OverRun || (overrideButton.getAsBoolean() && !bHold); + } + +} diff --git a/src/main/java/frc/robot/commands/IntakeRun.java b/src/main/java/frc/robot/commands/IntakeRun.java new file mode 100644 index 0000000..da481b4 --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeRun.java @@ -0,0 +1,92 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import java.util.function.BooleanSupplier; + +import frc.robot.Constants; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.LEDS; + +public class IntakeRun extends Command { + + private Intake s_Intake; + private DigitalInput LIM; + private JoystickButton overrideButton; + // private Boolean end; + private Boolean bHold; + private Boolean end; + private Timer timer; + private LEDS theLEDs; + + /** + * Run intake untill note is obtained or manualy disabled + * + * @param s_Intake Intake object + */ + public IntakeRun(Intake s_Intake, DigitalInput LIM, JoystickButton overrideButton, LEDS theLEDs) { + this.s_Intake = s_Intake; + this.LIM = LIM; + this.overrideButton = overrideButton; + this.theLEDs = theLEDs; + // this.end = false; + this.bHold = true; + this.end = false; + addRequirements(s_Intake); + } + + @Override + public void initialize() { + // tell the intake to extend + System.out.println("Start IntakeRun"); + s_Intake.setDeploy(true); + s_Intake.startIntake(); + this.bHold = true; + this.end = false; + timer = new Timer(); + theLEDs.setMode("IO"); + // timer.start(); + } + + @Override + public void execute() { + if (bHold) bHold = overrideButton.getAsBoolean(); + if (!LIM.get() && !end) { + timer.start(); + s_Intake.endIntake(); + end = true; + System.out.println("End1"); + theLEDs.setMode("IR"); + System.out.println("set IR"); + s_Intake.lightPull(false); + + } + System.out.println(timer.get()); + System.out.println(timer.get() >= Constants.Intake.OverRun); + } + + @Override + public void end(boolean interrupted) { + System.out.println("End IntakeRun"); + s_Intake.lightPull(true); + if (end) { + theLEDs.setMode("N"); + System.out.println("set N"); + } else { + theLEDs.setMode("D"); + System.out.println("set D"); + } + } + + @Override + public boolean isFinished() { + // this should trigger when a note is detected, or manualy triggerd + // return(s_Intake.checkForNote() || switchButton.getAsBoolean()); + if (!LIM.get()) System.out.println("LIM"); + if (overrideButton.getAsBoolean()) System.out.println("OVER"); + return timer.get() >= Constants.Intake.OverRun || (overrideButton.getAsBoolean() && !bHold); + } + +} diff --git a/src/main/java/frc/robot/commands/LaunchASAP.java b/src/main/java/frc/robot/commands/LaunchASAP.java new file mode 100644 index 0000000..1cdddda --- /dev/null +++ b/src/main/java/frc/robot/commands/LaunchASAP.java @@ -0,0 +1,70 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.hal.simulation.ConstBufferCallback; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.Constants; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.LEDS; +import frc.robot.subsystems.Launcher; + +public class LaunchASAP extends Command{ + private Intake s_Intake; + private Launcher s_Launcher; + private Timer timer; + private boolean launch; + private LEDS theLEDs; + + + public LaunchASAP(Intake s_Intake, Launcher s_Launcher, LEDS theLEDs) { + this.s_Intake = s_Intake; + this.s_Launcher = s_Launcher; + this.timer = new Timer(); + this.launch = false; + this.theLEDs = theLEDs; + + addRequirements(s_Intake); + addRequirements(s_Launcher); + } + + @Override + public void initialize() { + timer = new Timer(); + timer.start(); + launch = false; + s_Launcher.startLaunch(); + s_Intake.lightPull(false); + System.out.println("Start LaunchASAP"); + theLEDs.setMode("LS"); + System.out.println("set LS"); + + } + + @Override + public void execute() { + + // System.out.println(timer.get()); + if(timer.get() >= Constants.Launcher.LaunchASAPTime && !launch) { + launch = true; + theLEDs.setMode("L"); + System.out.println("set L"); + s_Intake.pushIntake(false); + System.out.println("Push"); + } + } + + @Override + public void end(boolean interrupted) { + s_Launcher.endLaunch(); + s_Intake.pushIntake(true); + System.out.println("End LaunchASAP"); + theLEDs.setMode("D"); + System.out.println("set D"); + } + + @Override + public boolean isFinished() { + return (timer.get() >= (Constants.Launcher.LaunchASAPTime+Constants.Launcher.LaunchStopTime) && launch); + } +} diff --git a/src/main/java/frc/robot/commands/LaunchControled.java b/src/main/java/frc/robot/commands/LaunchControled.java new file mode 100644 index 0000000..e7c19e5 --- /dev/null +++ b/src/main/java/frc/robot/commands/LaunchControled.java @@ -0,0 +1,74 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.hal.simulation.ConstBufferCallback; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.Constants; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.LEDS; +import frc.robot.subsystems.Launcher; + +public class LaunchControled extends Command{ + private Intake s_Intake; + private Launcher s_Launcher; + private Timer timer; + private boolean launch; + private LEDS theLEDs; + private JoystickButton overrideButton; + private JoystickButton launchButton; + + + public LaunchControled(Intake s_Intake, Launcher s_Launcher, LEDS theLEDs, JoystickButton overrideButton, JoystickButton launchButton) { + this.s_Intake = s_Intake; + this.s_Launcher = s_Launcher; + this.timer = new Timer(); + this.launch = false; + this.theLEDs = theLEDs; + this.overrideButton = overrideButton; + this.launchButton = launchButton; + + addRequirements(s_Intake); + addRequirements(s_Launcher); + } + + @Override + public void initialize() { + timer = new Timer(); + // timer.start(); + launch = false; + s_Intake.lightPull(false); + s_Launcher.startLaunch(); + System.out.println("Start LaunchASAP"); + theLEDs.setMode("LS"); + System.out.println("set LS"); + + } + + @Override + public void execute() { + + if (launchButton.getAsBoolean()) { + timer.start(); + launch = true; + theLEDs.setMode("L"); + System.out.println("set L"); + s_Intake.pushIntake(false); + System.out.println("Push"); + } + } + + @Override + public void end(boolean interrupted) { + s_Launcher.endLaunch(); + s_Intake.pushIntake(true); + System.out.println("End LaunchControled"); + theLEDs.setMode("D"); + System.out.println("set D"); + } + + @Override + public boolean isFinished() { + return (timer.get() >= (Constants.Launcher.LaunchStopTime) && launch); + } +} diff --git a/src/main/java/frc/robot/commands/SmartShoot.java b/src/main/java/frc/robot/commands/SmartShoot.java new file mode 100644 index 0000000..ef16016 --- /dev/null +++ b/src/main/java/frc/robot/commands/SmartShoot.java @@ -0,0 +1,45 @@ +package frc.robot.commands; + +import edu.wpi.first.networktables.PubSub; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import java.util.function.BooleanSupplier; + +import frc.robot.Constants; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.LEDS; + +public class SmartShoot extends Command { + + + /** + * Run intake untill note is obtained or manualy disabled + * + * @param s_Intake Intake object + */ + public SmartShoot() { + } + + @Override + public void initialize() { + + } + + @Override + public void execute() { + + } + + @Override + public void end(boolean interrupted) { + + } + + @Override + public boolean isFinished() { + return false; + } + +} diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java index c4fa75e..0eb7e54 100644 --- a/src/main/java/frc/robot/commands/TeleopSwerve.java +++ b/src/main/java/frc/robot/commands/TeleopSwerve.java @@ -1,6 +1,7 @@ package frc.robot.commands; import frc.robot.Constants; +import frc.robot.subsystems.LEDS; import frc.robot.subsystems.Swerve; import java.util.function.BooleanSupplier; @@ -8,6 +9,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -16,9 +18,20 @@ public class TeleopSwerve extends Command { private DoubleSupplier translationSup; private DoubleSupplier strafeSup; private DoubleSupplier rotationSup; + private DoubleSupplier throtleSupplier; private BooleanSupplier robotCentricSup; - - public TeleopSwerve(Swerve s_Swerve, DoubleSupplier translationSup, DoubleSupplier strafeSup, DoubleSupplier rotationSup, BooleanSupplier robotCentricSup) { + private LEDS led; + + /** + * Default Teleop mode, takes the swerve and conrolor + * + * @param s_Swerve swerve base + * @param translationSup forward and back suplier, typicaly a controlor axis + * @param strafeSup left and right suplier, typicaly a controlor axis + * @param rotationSup Rotation suplier, typicaly a controlor axis + * @param robotCentricSup wether to drive relative to the robot. + */ + public TeleopSwerve(Swerve s_Swerve, DoubleSupplier translationSup, DoubleSupplier strafeSup, DoubleSupplier rotationSup, DoubleSupplier throtleSupplier, BooleanSupplier robotCentricSup, LEDS led) { this.s_Swerve = s_Swerve; addRequirements(s_Swerve); @@ -26,6 +39,8 @@ public TeleopSwerve(Swerve s_Swerve, DoubleSupplier translationSup, DoubleSuppli this.strafeSup = strafeSup; this.rotationSup = rotationSup; this.robotCentricSup = robotCentricSup; + this.throtleSupplier = throtleSupplier; + this.led = led; } @Override @@ -34,11 +49,13 @@ public void execute() { double translationVal = MathUtil.applyDeadband(translationSup.getAsDouble(), Constants.stickDeadband); double strafeVal = MathUtil.applyDeadband(strafeSup.getAsDouble(), Constants.stickDeadband); double rotationVal = MathUtil.applyDeadband(rotationSup.getAsDouble(), Constants.stickDeadband); + double throtleVal = 1-((throtleSupplier.getAsDouble()+1)/2); + // SmartDashboard.putNumber("Throttle", throtleVal); /* Drive */ s_Swerve.drive( - new Translation2d(translationVal, strafeVal).times(Constants.Swerve.maxSpeed), - rotationVal * Constants.Swerve.maxAngularVelocity, + new Translation2d(translationVal*throtleVal, strafeVal*throtleVal).times(Constants.Swerve.maxSpeed), + (rotationVal * throtleVal) * Constants.Swerve.maxAngularVelocity, !robotCentricSup.getAsBoolean(), true ); diff --git a/src/main/java/frc/robot/subsystems/Band.java b/src/main/java/frc/robot/subsystems/Band.java index 7583ea5..6eb2dd6 100644 --- a/src/main/java/frc/robot/subsystems/Band.java +++ b/src/main/java/frc/robot/subsystems/Band.java @@ -13,7 +13,7 @@ public class Band { - private Orchestra _orchestra; + public Orchestra orchestra; private List songs; TalonFX[] _fxes = { @@ -33,10 +33,10 @@ public class Band { public Band() { songs = new ArrayList(); - _orchestra = new Orchestra(); + orchestra = new Orchestra(); for (TalonFX motor : _fxes) { - _orchestra.addInstrument(motor); + orchestra.addInstrument(motor); } File[] musicFiles = new File(Filesystem.getDeployDirectory(), "music").listFiles(); @@ -64,7 +64,7 @@ public SendableChooser Buildchoser(){ * @param songName song file name with .chrp extention. */ public void play(String songName){ - _orchestra.loadMusic("music/"+songName); - _orchestra.play(); + orchestra.loadMusic("music/"+songName); + orchestra.play(); } } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java new file mode 100644 index 0000000..8833ac2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -0,0 +1,215 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +import java.util.function.IntSupplier; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.VictorSPXControlMode; +import com.ctre.phoenix.motorcontrol.can.VictorSPX; +import com.revrobotics.CANSparkMax; +// import com.revrobotics.CANSparkMax.SoftLimitDirection; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; +import com.revrobotics.CANSparkBase.IdleMode; + +public class Intake extends SubsystemBase{ + + public VictorSPX m_intakeMotor1; + public VictorSPX m_intakeMotor2; + public CANSparkMax m_barMotor; + public RelativeEncoder barEncoder; + // public SparkPIDController barPID; + private boolean deployed; + + private XboxController secondaryController; + + public double kP, kI, kD, kMaxOut, kMinOut, maxRPM, maxVel, maxAcc; + + + public Intake(int m_barMotorId, int m_intakeMotor1ID, int m_intakeMotor2ID, XboxController secondaryControllerIn) { + // SmartDashboard.putNumber("test",3); + m_barMotor = new CANSparkMax(m_barMotorId, com.revrobotics.CANSparkLowLevel.MotorType.kBrushless); + m_intakeMotor1 = new VictorSPX(m_intakeMotor1ID); + m_intakeMotor2 = new VictorSPX(m_intakeMotor2ID); + + m_barMotor.restoreFactoryDefaults(); + m_barMotor.setIdleMode(IdleMode.kBrake); + + secondaryController = secondaryControllerIn; + + barEncoder = m_barMotor.getEncoder(); + // barPID = m_barMotor.getPIDController(); + + // kP = Constants.Intake.barP; + // kI = Constants.Intake.barI; + // kD = Constants.Intake.barD; + + // kMaxOut = Constants.Intake.barMaxOut; + // kMinOut = Constants.Intake.barMinOut; + + // maxRPM = Constants.Intake.barMaxRPM; + // maxVel = Constants.Intake.barMaxVel; + // maxAcc = Constants.Intake.barMaxAcc; + + // set PID coefficients + // barPID.setP(kP); + // barPID.setI(kI); + // barPID.setD(kD); + // barPID.setIZone(0); + // barPID.setFF(0); + // barPID.setOutputRange(kMinOut, kMaxOut); + + + m_barMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kForward, true); + m_barMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, true); + + m_barMotor.setSoftLimit(CANSparkMax.SoftLimitDirection.kForward, Constants.Intake.forwardLim); + m_barMotor.setSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, Constants.Intake.reversLim); + + // barPID.setReference(-0.3, CANSparkMax.ControlType.kDutyCycle); + } + + public void startIntake() { + m_intakeMotor1.set(VictorSPXControlMode.PercentOutput, Constants.Intake.intakeVel); + m_intakeMotor2.set(VictorSPXControlMode.PercentOutput, Constants.Intake.intakeVel); + // barPID.setReference(0.3, CANSparkMax.ControlType.kDutyCycle); + m_barMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kForward, true); + m_barMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, false); + m_barMotor.set(Constants.Intake.acuateVel); + + System.out.println("Start"); + } + + public void endIntake() { + m_intakeMotor1.set(VictorSPXControlMode.PercentOutput, 0); + m_intakeMotor2.set(VictorSPXControlMode.PercentOutput, 0); + // barPID.setReference(-0.3, CANSparkMax.ControlType.kDutyCycle); + m_barMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kForward, false); + m_barMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, true); + m_barMotor.set(-Constants.Intake.acuateVel); + System.out.println("End"); + } + + public void runIntake() { + // m_barMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kForward, SmartDashboard.getBoolean("Forward Soft Limit Enabled", true)); + // m_barMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, SmartDashboard.getBoolean("Reverse Soft Limit Enabled", true)); + m_barMotor.setSoftLimit(CANSparkMax.SoftLimitDirection.kForward, (float)SmartDashboard.getNumber("Forward Soft Limit", 15)); + m_barMotor.setSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, (float)SmartDashboard.getNumber("Reverse Soft Limit", 0)); + } + + public void breakIntake(){ + m_intakeMotor1.setNeutralMode(NeutralMode.Brake); + m_intakeMotor2.setNeutralMode(NeutralMode.Brake); + } + + public boolean checkForNote() { + return false; + } + + public void pushIntake(boolean stop) { + if(!stop) { + m_intakeMotor1.set(VictorSPXControlMode.PercentOutput, -Constants.Intake.intakeVel); + m_intakeMotor2.set(VictorSPXControlMode.PercentOutput, -Constants.Intake.intakeVel*0.5); + } else { + m_intakeMotor1.set(VictorSPXControlMode.PercentOutput, 0); + m_intakeMotor2.set(VictorSPXControlMode.PercentOutput, 0); + } + } + + public void toggleDeploy() { + if(!deployed) { + deployed = true; + startIntake(); + } else { + deployed = false; + endIntake(); + } + } + + public void lightPull(boolean stop) { + if(!stop) { + m_intakeMotor1.set(VictorSPXControlMode.PercentOutput, Constants.Intake.intakeNudge); + m_intakeMotor2.set(VictorSPXControlMode.PercentOutput, Constants.Intake.intakeNudge); + } else { + m_intakeMotor1.set(VictorSPXControlMode.PercentOutput, 0); + m_intakeMotor2.set(VictorSPXControlMode.PercentOutput, 0); + } + + } + + public void lightPush(boolean stop) { + if(!stop) { + m_intakeMotor1.set(VictorSPXControlMode.PercentOutput, Constants.Intake.intakeNudge); + m_intakeMotor2.set(VictorSPXControlMode.PercentOutput, Constants.Intake.intakeNudge); + } else { + m_intakeMotor1.set(VictorSPXControlMode.PercentOutput, 0); + m_intakeMotor2.set(VictorSPXControlMode.PercentOutput, 0); + } + + } + + public void nudge() { + if (secondaryController.getPOV() != -1) { + System.out.println(secondaryController.getPOV()); + } + if (secondaryController.getPOV() == 0) { + m_intakeMotor1.set(VictorSPXControlMode.PercentOutput, -Constants.Intake.intakeNudge); + m_intakeMotor2.set(VictorSPXControlMode.PercentOutput, -Constants.Intake.intakeNudge); + } else if (secondaryController.getPOV() == 180){ + m_intakeMotor1.set(VictorSPXControlMode.PercentOutput, Constants.Intake.intakeNudge); + m_intakeMotor2.set(VictorSPXControlMode.PercentOutput, Constants.Intake.intakeNudge); + } else { + m_intakeMotor1.set(VictorSPXControlMode.PercentOutput, 0); + m_intakeMotor2.set(VictorSPXControlMode.PercentOutput, 0); + } + } + + public void manualAcuation() { + if (secondaryController.getPOV() != -1) { + System.out.println(secondaryController.getPOV()); + } + if (secondaryController.getPOV() == 90) { + m_barMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kForward, false); + m_barMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, true); + m_barMotor.set(-Constants.Intake.acuateManual); + } else if (secondaryController.getPOV() == 270){ + m_barMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kForward, true); + m_barMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, false); + m_barMotor.set(Constants.Intake.acuateManual); + } else { + m_barMotor.set(0); + } + } + + public void setDeploy(boolean mode) { + if(mode && !deployed) { + deployed = true; + } else if(!mode && deployed) { + deployed = false; + } + } + + public boolean getMode() { + return deployed; + } + + public void updateData() { + SmartDashboard.putBoolean("Forward Soft Limit Enabled", + m_barMotor.isSoftLimitEnabled(CANSparkMax.SoftLimitDirection.kForward)); + SmartDashboard.putBoolean("Reverse Soft Limit Enabled", + m_barMotor.isSoftLimitEnabled(CANSparkMax.SoftLimitDirection.kReverse)); + SmartDashboard.putNumber("Forward Soft Limit", + m_barMotor.getSoftLimit(CANSparkMax.SoftLimitDirection.kForward)); + SmartDashboard.putNumber("Reverse Soft Limit", + m_barMotor.getSoftLimit(CANSparkMax.SoftLimitDirection.kReverse)); + } + + + +} diff --git a/src/main/java/frc/robot/subsystems/LEDS.java b/src/main/java/frc/robot/subsystems/LEDS.java index 53c0044..a68592c 100644 --- a/src/main/java/frc/robot/subsystems/LEDS.java +++ b/src/main/java/frc/robot/subsystems/LEDS.java @@ -1,13 +1,26 @@ package frc.robot.subsystems; +import java.util.Arrays; +import java.util.logging.LogManager; + import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + -public class LEDS { +public class LEDS{ private AddressableLED m_led; private AddressableLEDBuffer m_ledBuffer; private int rainbowValue = 0; + private Timer timer; + private String mode; + private String lastMode; + private double nextBlink; + private boolean blink; /** * Master LED Controler. manages the ligts for ease of use. use WS2812B individualy addressable LED strips @@ -17,13 +30,19 @@ public class LEDS { */ public LEDS(int PWM, int length) { - m_led = new AddressableLED(9); + m_led = new AddressableLED(PWM); - m_ledBuffer = new AddressableLEDBuffer(60); + m_ledBuffer = new AddressableLEDBuffer(length); m_led.setLength(m_ledBuffer.getLength()); m_led.setData(m_ledBuffer); m_led.start(); + + timer = new Timer(); + nextBlink = timer.get() + Constants.Mode.blinkTime; + + mode = "D"; + lastMode = "N"; } // public int[] HEXtoRGB(String hex){ @@ -48,6 +67,20 @@ public void SetFull(int R, int G, int B) { m_led.setData(m_ledBuffer); } + /** + * Set the full strip using a {@link Color} + * + * @param clr {@link Color} to set to + */ + public void SetFull(Color clr) { + // For every Pixel + for (var i = 0; i < m_ledBuffer.getLength(); i++) { + // Sets the specified LED to the Color provided + m_ledBuffer.setLED(i, clr); + } + m_led.setData(m_ledBuffer); + } + /** * Set the entire strip to one volor using HSV * @@ -64,6 +97,73 @@ public void SetFullHSV(int H, int S, int V) { m_led.setData(m_ledBuffer); } + public void updateMode() { + if (mode != lastMode) { + lastMode = mode; + /* + * LED modes + * + * D BLUE, Default Drive Mode + * IO RED, Intake out, running intake + * IR ORANGE Intake Stowing + * IF PURPLE Intake Fixing + * IA PINK Intake Amp + * N YELLOW Note Ready + * LS GREEN Launcher Spin Up + * L AQUA Launching + * + */ + switch (mode) { + case "D": + SetFull(Constants.Mode.Colors.D); + System.out.println("D"); + break; + case "IO": + SetFull(Constants.Mode.Colors.IO); + System.out.println("IO"); + break; + case "IR": + SetFull(Constants.Mode.Colors.IR); + System.out.println("IR"); + break; + case "IF": + SetFull(Constants.Mode.Colors.IF); + System.out.println("IF"); + break; + case "IA": + SetFull(Constants.Mode.Colors.IA); + System.out.println("IA"); + break; + case "N": + SetFull(Constants.Mode.Colors.N); + System.out.println("N"); + break; + case "LS": + SetFull(Constants.Mode.Colors.LS); + System.out.println("LS"); + break; + case "L": + SetFull(Constants.Mode.Colors.L); + System.out.println("L"); + break; + default: + break; + } + } + } + + public void setMode(String modeID) { + String[] vModes = {"D","IO","IR","IF","IA","N","LS","L"}; + if (Arrays.asList(vModes).contains(modeID)) { + mode = modeID; + } + this.updateMode(); + } + + public String getMode() { + return mode; + } + /* * Rainbow function, called perodicly, shifts a rainbow through the strip */ diff --git a/src/main/java/frc/robot/subsystems/Launcher.java b/src/main/java/frc/robot/subsystems/Launcher.java new file mode 100644 index 0000000..ec4e725 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Launcher.java @@ -0,0 +1,50 @@ +package frc.robot.subsystems; +import frc.robot.Constants; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkPIDController; +import com.revrobotics.CANSparkBase; + + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.VictorSPXControlMode; +import com.ctre.phoenix.motorcontrol.can.VictorSPX; + +import edu.wpi.first.networktables.NetworkTableInstance.NetworkMode; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Launcher extends SubsystemBase { + + public CANSparkMax m_launcherRight; + public SparkPIDController m_RPID; + public CANSparkMax m_launcherLeft; + public SparkPIDController m_LPID; + public boolean spin; + + public Intake s_Intake; + + public Launcher(int lMotorId, int rMotorId) { + m_launcherLeft = new CANSparkMax(lMotorId, com.revrobotics.CANSparkLowLevel.MotorType.kBrushless); + m_launcherRight = new CANSparkMax(rMotorId, com.revrobotics.CANSparkLowLevel.MotorType.kBrushless); + + m_launcherLeft.restoreFactoryDefaults(); + m_launcherRight.restoreFactoryDefaults(); + spin = false; + } + + + public void startLaunch() { + System.out.println("start launch"); + m_launcherLeft.set(-Constants.Launcher.LaunchP); + m_launcherRight.set(Constants.Launcher.LaunchP); + spin = true; + } + + public void endLaunch() { + m_launcherLeft.set(0); + m_launcherRight.set(0); + spin = false; + } + + +} diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index f99cc7c..7997519 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -4,6 +4,7 @@ import frc.robot.Constants; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.Odometry; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -56,10 +57,10 @@ public Swerve(LEDS iled) { this::getChasisHeading, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE this::driveRobotRelative, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your Constants class - new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants - new PIDConstants(5.0, 0.0, 0.0), // Rotation PID constants - 3.5, // Max module speed, in m/s - 0.4, // Drive base radius in meters. Distance from robot center to furthest module. + new PIDConstants(5.0, 1.0, 0.0), // Translation PID constants + new PIDConstants(5.0, 1.0, 0.0), // Rotation PID constants + 4.5, // Max module speed, in m/s + 0.3, // Drive base radius in meters. Distance from robot center to furthest module. new ReplanningConfig() // Default path replanning config. See the API for the options here ), () -> { @@ -165,7 +166,11 @@ public Rotation2d getHeading(){ } public void setHeading(Rotation2d heading){ - swerveOdometry.resetPosition(getGyroYaw(), getModulePositions(), new Pose2d(getPose().getTranslation(), heading)); + swerveOdometry.resetPosition( + getGyroYaw(), + getModulePositions(), + new Pose2d(getPose().getTranslation(), heading) + ); } public void zeroHeading(){ @@ -183,19 +188,13 @@ public void resetModulesToAbsolute(){ } public ChassisSpeeds getChasisHeading(){ - Pose2d now = getPose(); - return ChassisSpeeds.fromFieldRelativeSpeeds( - now.getX(), - now.getY(), - now.getRotation().getRadians(), - this.getHeading() - ); + return Constants.Swerve.swerveKinematics.toChassisSpeeds(getModuleStates()); } @Override public void periodic(){ + //set Module dashboard values swerveOdometry.update(getGyroYaw(), getModulePositions()); - for(SwerveModule mod : mSwerveMods){ SmartDashboard.putNumber("Mod " + mod.moduleNumber + " CANcoder", mod.getCANcoder().getDegrees()); SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Angle", mod.getPosition().angle.getDegrees()); diff --git a/src/main/java/frc/robot/subsystems/camera.java b/src/main/java/frc/robot/subsystems/camera.java new file mode 100644 index 0000000..2669261 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/camera.java @@ -0,0 +1,15 @@ +package frc.robot.subsystems; + +import edu.wpi.first.networktables.DoubleTopic; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; + +public class camera { + + NetworkTableInstance inst = NetworkTableInstance.getDefault(); + NetworkTable table = inst.getTable("datatable"); + DoubleTopic dblTopic = inst.getDoubleTopic("/datatable/X"); + + + +} diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json new file mode 100644 index 0000000..88a68dd --- /dev/null +++ b/vendordeps/Phoenix5.json @@ -0,0 +1,151 @@ +{ + "fileName": "Phoenix5.json", + "name": "CTRE-Phoenix (v5)", + "version": "5.33.0", + "frcYear": 2024, + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json", + "requires": [ + { + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", + "offlineFileName": "Phoenix6.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.33.0" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.33.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.33.0", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.33.0", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.0", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.33.0", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.33.0", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.0", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..0f3520e --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,74 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2024.2.0", + "frcYear": "2024", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2024.2.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2024.2.0", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.0", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file