From: Kyle-Eldridge <113394349+Kyle-Eldridge@users.noreply.github.com> Date: Tue, 22 Apr 2025 23:06:04 +0000 (-0700) Subject: Delete 2025 code X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=d512de79dbbd23f4d09f9b6bb16aa42fc449669f;p=FRC2026.git Delete 2025 code --- diff --git a/src/main/deploy/pathplanner/autos/Align Right Side.auto b/src/main/deploy/pathplanner/autos/Align Right Side.auto deleted file mode 100644 index 4e318b1..0000000 --- a/src/main/deploy/pathplanner/autos/Align Right Side.auto +++ /dev/null @@ -1,88 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "AP1" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "OuttakeCoral" - } - }, - { - "type": "named", - "data": { - "name": "Lower Elevator" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "Station R P2" - } - }, - { - "type": "path", - "data": { - "pathName": "Station R P3" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "OuttakeCoral" - } - }, - { - "type": "named", - "data": { - "name": "Lower Elevator" - } - } - ] - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "OuttakeCoral" - } - }, - { - "type": "named", - "data": { - "name": "Lower Elevator" - } - } - ] - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left Lollipop J, L, A.auto b/src/main/deploy/pathplanner/autos/Left Lollipop J, L, A.auto deleted file mode 100644 index 02ddf6a..0000000 --- a/src/main/deploy/pathplanner/autos/Left Lollipop J, L, A.auto +++ /dev/null @@ -1,118 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "P 1 - Align After" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drive To 11/20 Right" - } - }, - { - "type": "named", - "data": { - "name": "OuttakeCoral" - } - }, - { - "type": "named", - "data": { - "name": "Lower Elevator" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "Official Lollipop 1" - } - }, - { - "type": "path", - "data": { - "pathName": "Lollipop 2" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drive To 6/19 Right" - } - }, - { - "type": "named", - "data": { - "name": "OuttakeCoral" - } - }, - { - "type": "named", - "data": { - "name": "Lower Elevator" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "Lollipop 3" - } - }, - { - "type": "path", - "data": { - "pathName": "Lollipop 4" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drive To 6/19 Right" - } - }, - { - "type": "named", - "data": { - "name": "OuttakeCoral" - } - }, - { - "type": "named", - "data": { - "name": "Lower Elevator" - } - } - ] - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left Side Alt.auto b/src/main/deploy/pathplanner/autos/Left Side Alt.auto deleted file mode 100644 index 8d405d2..0000000 --- a/src/main/deploy/pathplanner/autos/Left Side Alt.auto +++ /dev/null @@ -1,158 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "P 1" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drive To 11/20 Right" - } - }, - { - "type": "named", - "data": { - "name": "OuttakeCoral" - } - }, - { - "type": "named", - "data": { - "name": "Lower Elevator" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "Gronud P2" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drive To 6/19 Left" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "OuttakeCoral" - } - }, - { - "type": "named", - "data": { - "name": "Lower Elevator" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "GroundB #6" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drive To 6/19 Right" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "OuttakeCoral" - } - }, - { - "type": "named", - "data": { - "name": "Lower Elevator" - } - } - ] - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left Side Sweep.auto b/src/main/deploy/pathplanner/autos/Left Side Sweep.auto deleted file mode 100644 index 9054c73..0000000 --- a/src/main/deploy/pathplanner/autos/Left Side Sweep.auto +++ /dev/null @@ -1,99 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "P 1 - Align After" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drive To 11/20 Right" - } - }, - { - "type": "named", - "data": { - "name": "OuttakeCoral" - } - }, - { - "type": "named", - "data": { - "name": "Lower Elevator" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "Sweep P2" - } - }, - { - "type": "named", - "data": { - "name": "Drive To Left Station" - } - }, - { - "type": "path", - "data": { - "pathName": "LSweep P3" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drive To 6/19 Right" - } - }, - { - "type": "named", - "data": { - "name": "OuttakeCoral" - } - }, - { - "type": "named", - "data": { - "name": "Lower Elevator" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "GrondP4" - } - }, - { - "type": "named", - "data": { - "name": "Drive To Left Station" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left Side.auto b/src/main/deploy/pathplanner/autos/Left Side.auto deleted file mode 100644 index 2cf0435..0000000 --- a/src/main/deploy/pathplanner/autos/Left Side.auto +++ /dev/null @@ -1,124 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "P 1" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drive To 11/20 Right" - } - }, - { - "type": "named", - "data": { - "name": "OuttakeCoral" - } - }, - { - "type": "named", - "data": { - "name": "Lower Elevator" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "Gronud P2" - } - }, - { - "type": "path", - "data": { - "pathName": "P 5" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drive To 6/19 Left" - } - }, - { - "type": "named", - "data": { - "name": "OuttakeCoral" - } - }, - { - "type": "named", - "data": { - "name": "Lower Elevator" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "GroundB #6" - } - }, - { - "type": "path", - "data": { - "pathName": "P 3" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drive To 6/19 Right" - } - }, - { - "type": "named", - "data": { - "name": "OuttakeCoral" - } - }, - { - "type": "named", - "data": { - "name": "Lower Elevator" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "3d intake peice" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right Side.auto b/src/main/deploy/pathplanner/autos/Right Side.auto deleted file mode 100644 index 206b69e..0000000 --- a/src/main/deploy/pathplanner/autos/Right Side.auto +++ /dev/null @@ -1,118 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "P 1 Mirrored" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drive To 9/22 Left" - } - }, - { - "type": "named", - "data": { - "name": "OuttakeCoral" - } - }, - { - "type": "named", - "data": { - "name": "Lower Elevator" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "Gronud P2 Mirrored" - } - }, - { - "type": "path", - "data": { - "pathName": "P 5 Mirrored" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drive To 8/17 Right" - } - }, - { - "type": "named", - "data": { - "name": "OuttakeCoral" - } - }, - { - "type": "named", - "data": { - "name": "Lower Elevator" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "GroundB #6 Mirrored" - } - }, - { - "type": "path", - "data": { - "pathName": "P 3 Mirrored" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drive To 8/17 Left" - } - }, - { - "type": "named", - "data": { - "name": "OuttakeCoral" - } - }, - { - "type": "named", - "data": { - "name": "Lower Elevator" - } - } - ] - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Station Left Side - Align After.auto b/src/main/deploy/pathplanner/autos/Station Left Side - Align After.auto deleted file mode 100644 index 2f6ff4b..0000000 --- a/src/main/deploy/pathplanner/autos/Station Left Side - Align After.auto +++ /dev/null @@ -1,144 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "P 1 - Align After" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drive To 11/20 Right" - } - }, - { - "type": "named", - "data": { - "name": "OuttakeCoral" - } - }, - { - "type": "named", - "data": { - "name": "Lower Elevator" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "Station P2" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Station Intake" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "StationB Alt Opt Left - Align After" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drive To 6/19 Left" - } - }, - { - "type": "named", - "data": { - "name": "OuttakeCoral" - } - }, - { - "type": "named", - "data": { - "name": "Lower Elevator" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": " Station P4" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Station Intake" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": " StationB Alt Opt - Align After" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drive To 6/19 Right" - } - }, - { - "type": "named", - "data": { - "name": "OuttakeCoral" - } - }, - { - "type": "named", - "data": { - "name": "Lower Elevator" - } - } - ] - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Station Left Side.auto b/src/main/deploy/pathplanner/autos/Station Left Side.auto deleted file mode 100644 index 65980d3..0000000 --- a/src/main/deploy/pathplanner/autos/Station Left Side.auto +++ /dev/null @@ -1,144 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "P 1" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drive To 11/20 Right" - } - }, - { - "type": "named", - "data": { - "name": "OuttakeCoral" - } - }, - { - "type": "named", - "data": { - "name": "Lower Elevator" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "Station P2" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Station Intake" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "StationP 3" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drive To 6/19 Left" - } - }, - { - "type": "named", - "data": { - "name": "OuttakeCoral" - } - }, - { - "type": "named", - "data": { - "name": "Lower Elevator" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": " Station P4" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Station Intake" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "StationB Alt Opt" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drive To 6/19 Right" - } - }, - { - "type": "named", - "data": { - "name": "OuttakeCoral" - } - }, - { - "type": "named", - "data": { - "name": "Lower Elevator" - } - } - ] - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Station Right Side.auto b/src/main/deploy/pathplanner/autos/Station Right Side.auto deleted file mode 100644 index bfc5082..0000000 --- a/src/main/deploy/pathplanner/autos/Station Right Side.auto +++ /dev/null @@ -1,142 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "AP1" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drive to 9/22 Left" - } - }, - { - "type": "named", - "data": { - "name": "OuttakeCoral" - } - }, - { - "type": "named", - "data": { - "name": "Lower Elevator" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "Station R P2" - } - }, - { - "type": "named", - "data": { - "name": "Drive to Right Station Intake" - } - }, - { - "type": "named", - "data": { - "name": "Station Intake" - } - }, - { - "type": "path", - "data": { - "pathName": "Station R P3" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drive to 8/17 Right" - } - }, - { - "type": "named", - "data": { - "name": "OuttakeCoral" - } - }, - { - "type": "named", - "data": { - "name": "Lower Elevator" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "Station R P4" - } - }, - { - "type": "named", - "data": { - "name": "Drive to Right Station Intake" - } - }, - { - "type": "named", - "data": { - "name": "Station Intake" - } - }, - { - "type": "path", - "data": { - "pathName": "Station R P5" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drive to 8/17 Left" - } - }, - { - "type": "named", - "data": { - "name": "OuttakeCoral" - } - }, - { - "type": "named", - "data": { - "name": "Lower Elevator" - } - } - ] - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3d intake peice.path b/src/main/deploy/pathplanner/paths/3d intake peice.path deleted file mode 100644 index 69fa8f5..0000000 --- a/src/main/deploy/pathplanner/paths/3d intake peice.path +++ /dev/null @@ -1,80 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.66, - "y": 5.12 - }, - "prevControl": null, - "nextControl": { - "x": 3.909326741052695, - "y": 5.101664891709005 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.722, - "y": 7.129 - }, - "prevControl": { - "x": 1.5210513367419152, - "y": 7.294369927869808 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.6049966239027695, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 1.5, - "maxAcceleration": 1.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "IntakeCoral", - "waypointRelativePos": 0.0, - "endWaypointRelativePos": 1.0, - "command": { - "type": "named", - "data": { - "name": "IntakeCoral" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -145.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0.0, - "rotation": -150.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/4th peice.path b/src/main/deploy/pathplanner/paths/4th peice.path deleted file mode 100644 index 90f5af4..0000000 --- a/src/main/deploy/pathplanner/paths/4th peice.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.9300204918032788, - "y": 7.243698770491803 - }, - "prevControl": null, - "nextControl": { - "x": 2.930020491803279, - "y": 7.243698770491803 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.6202868852459016, - "y": 5.589395491803279 - }, - "prevControl": { - "x": 2.6202868852459016, - "y": 5.589395491803279 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -145.66978280449666 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": -141.34019174590986 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/AP1.path b/src/main/deploy/pathplanner/paths/AP1.path deleted file mode 100644 index a693998..0000000 --- a/src/main/deploy/pathplanner/paths/AP1.path +++ /dev/null @@ -1,66 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.18063524590164, - "y": 0.48 - }, - "prevControl": null, - "nextControl": { - "x": 6.483621180258631, - "y": 1.147118232765553 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.81183, - "y": 2.768 - }, - "prevControl": { - "x": 5.174076079601306, - "y": 2.4481272403373495 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "MoveElevator", - "waypointRelativePos": 0.4514601420678732, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "L4" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 29.999999999999996 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center to G.path b/src/main/deploy/pathplanner/paths/Center to G.path deleted file mode 100644 index 056673f..0000000 --- a/src/main/deploy/pathplanner/paths/Center to G.path +++ /dev/null @@ -1,61 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.229, - "y": 3.8608 - }, - "prevControl": null, - "nextControl": { - "x": 6.903565973322253, - "y": 3.8608 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.74, - "y": 3.8608 - }, - "prevControl": { - "x": 6.308787430236012, - "y": 3.8608 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "MoveElevator", - "waypointRelativePos": 0.7829518547750566, - "endWaypointRelativePos": null, - "command": null - } - ], - "globalConstraints": { - "maxVelocity": 0.5, - "maxAcceleration": 0.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 90.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 90.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center to H.path b/src/main/deploy/pathplanner/paths/Center to H.path deleted file mode 100644 index 5734ea0..0000000 --- a/src/main/deploy/pathplanner/paths/Center to H.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.229, - "y": 4.191 - }, - "prevControl": null, - "nextControl": { - "x": 6.903565973322253, - "y": 4.191 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.74, - "y": 4.191 - }, - "prevControl": { - "x": 6.308787430236012, - "y": 4.191 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 0.5, - "maxAcceleration": 0.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 90.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 90.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/GrondP4.path b/src/main/deploy/pathplanner/paths/GrondP4.path deleted file mode 100644 index 2863bb1..0000000 --- a/src/main/deploy/pathplanner/paths/GrondP4.path +++ /dev/null @@ -1,82 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.721, - "y": 5.077 - }, - "prevControl": null, - "nextControl": { - "x": 3.7080822290916022, - "y": 4.827333960669942 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.260710227277713, - "y": 7.240838068181818 - }, - "prevControl": { - "x": 3.769261363646335, - "y": 7.031434659090908 - }, - "nextControl": { - "x": 2.5266509333291625, - "y": 7.543097777451788 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.8076988636413492, - "y": 6.572741477272728 - }, - "prevControl": { - "x": 2.1270340909140755, - "y": 7.342904545454545 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Intake", - "waypointRelativePos": 0.4, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "IntakeCoral" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 1.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -53.616 - }, - "reversed": false, - "folder": "Ground LSide", - "idealStartingState": { - "velocity": 0, - "rotation": -145.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Gronud P2 Mirrored.path b/src/main/deploy/pathplanner/paths/Gronud P2 Mirrored.path deleted file mode 100644 index 9b881a0..0000000 --- a/src/main/deploy/pathplanner/paths/Gronud P2 Mirrored.path +++ /dev/null @@ -1,101 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 5.0, - "y": 2.81 - }, - "prevControl": null, - "nextControl": { - "x": 4.967147352430555, - "y": 2.1109704137731478 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.6396306818181814, - "y": 1.826 - }, - "prevControl": { - "x": 3.976501550755575, - "y": 1.7392387033253078 - }, - "nextControl": { - "x": 2.8861082175925916, - "y": 2.020070167824074 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.722, - "y": 0.921 - }, - "prevControl": { - "x": 2.7759913917824064, - "y": 2.1777043547453694 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.25, - "rotationDegrees": -39.29328640235812 - } - ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 1.6961512491559774, - "maxWaypointRelativePos": 2.0, - "constraints": { - "maxVelocity": 1.5, - "maxAcceleration": 1.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Intake", - "waypointRelativePos": 0.55, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "IntakeCoral" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -36.0 - }, - "reversed": false, - "folder": "Ground LSide", - "idealStartingState": { - "velocity": 0.0, - "rotation": 29.999999999999996 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Gronud P2.path b/src/main/deploy/pathplanner/paths/Gronud P2.path deleted file mode 100644 index 4345b4a..0000000 --- a/src/main/deploy/pathplanner/paths/Gronud P2.path +++ /dev/null @@ -1,101 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 5.0, - "y": 5.24 - }, - "prevControl": null, - "nextControl": { - "x": 4.856164772727273, - "y": 5.775014204545455 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.6396306818181814, - "y": 6.223735795454545 - }, - "prevControl": { - "x": 3.9487500000000004, - "y": 6.383281249999999 - }, - "nextControl": { - "x": 3.061864563544763, - "y": 5.925533927958591 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.722, - "y": 7.129 - }, - "prevControl": { - "x": 2.562698863636364, - "y": 6.044247159090909 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.25, - "rotationDegrees": -145.0 - } - ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 1.6961512491559774, - "maxWaypointRelativePos": 2.0, - "constraints": { - "maxVelocity": 1.5, - "maxAcceleration": 1.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Intake", - "waypointRelativePos": 0.55, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "IntakeCoral" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -144.0 - }, - "reversed": false, - "folder": "Ground LSide", - "idealStartingState": { - "velocity": 0.0, - "rotation": 150.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/GroundB #2.path b/src/main/deploy/pathplanner/paths/GroundB #2.path deleted file mode 100644 index c1fc6d2..0000000 --- a/src/main/deploy/pathplanner/paths/GroundB #2.path +++ /dev/null @@ -1,66 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 4.0073368, - "y": 5.1909832 - }, - "prevControl": null, - "nextControl": { - "x": 3.6414852850066137, - "y": 5.481452904381226 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.885, - "y": 7.011 - }, - "prevControl": { - "x": 2.279280354370602, - "y": 6.697959071416107 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Intake", - "waypointRelativePos": 0.6463314097279476, - "endWaypointRelativePos": 1.0, - "command": { - "type": "named", - "data": { - "name": "IntakeCoral" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 0.5, - "maxAcceleration": 0.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -144.0 - }, - "reversed": false, - "folder": "Ground BSide", - "idealStartingState": { - "velocity": 0, - "rotation": -150.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/GroundB #3.path b/src/main/deploy/pathplanner/paths/GroundB #3.path deleted file mode 100644 index 049b70a..0000000 --- a/src/main/deploy/pathplanner/paths/GroundB #3.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.885, - "y": 7.011 - }, - "prevControl": null, - "nextControl": { - "x": 2.1167904466171583, - "y": 6.862521975540034 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.7213752, - "y": 5.0258832 - }, - "prevControl": { - "x": 3.3903399058878056, - "y": 5.426007692748621 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -150.0 - }, - "reversed": false, - "folder": "Ground BSide", - "idealStartingState": { - "velocity": 0, - "rotation": -144.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/GroundB #5.path b/src/main/deploy/pathplanner/paths/GroundB #5.path deleted file mode 100644 index b81a9be..0000000 --- a/src/main/deploy/pathplanner/paths/GroundB #5.path +++ /dev/null @@ -1,66 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.722, - "y": 7.129 - }, - "prevControl": null, - "nextControl": { - "x": 1.8649319141897391, - "y": 6.267760789902279 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.1273, - "y": 4.191 - }, - "prevControl": { - "x": 2.1273, - "y": 4.191 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "MoveElevator", - "waypointRelativePos": 0.6806256151574805, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "L4" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.5, - "rotation": -90.89200324814249 - }, - "reversed": false, - "folder": "Ground BSide", - "idealStartingState": { - "velocity": 0, - "rotation": -142.86256094570606 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/GroundB #6 Mirrored.path b/src/main/deploy/pathplanner/paths/GroundB #6 Mirrored.path deleted file mode 100644 index 302abdd..0000000 --- a/src/main/deploy/pathplanner/paths/GroundB #6 Mirrored.path +++ /dev/null @@ -1,85 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.98, - "y": 2.81 - }, - "prevControl": null, - "nextControl": { - "x": 3.661208032083275, - "y": 2.7146988994001573 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.722, - "y": 0.921 - }, - "prevControl": { - "x": 1.2995983363740153, - "y": 0.5281644353113872 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.3291032261499149, - "rotationDegrees": -32.65592295916657 - } - ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.5509790681971605, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 1.5, - "maxAcceleration": 1.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Intake", - "waypointRelativePos": 0.11073598919649108, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "IntakeCoral" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -35.0 - }, - "reversed": false, - "folder": "Ground LSide", - "idealStartingState": { - "velocity": 0, - "rotation": -29.999999999999996 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/GroundB #6.path b/src/main/deploy/pathplanner/paths/GroundB #6.path deleted file mode 100644 index 2a7adb3..0000000 --- a/src/main/deploy/pathplanner/paths/GroundB #6.path +++ /dev/null @@ -1,85 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.98, - "y": 5.24 - }, - "prevControl": null, - "nextControl": { - "x": 3.316855850468241, - "y": 5.760793783082248 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.722, - "y": 7.129 - }, - "prevControl": { - "x": 2.338563708774429, - "y": 6.649638232899023 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.3291032261499149, - "rotationDegrees": -136.13455862983048 - } - ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.5509790681971605, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 1.5, - "maxAcceleration": 1.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Intake", - "waypointRelativePos": 0.11073598919649108, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "IntakeCoral" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -142.62124693823438 - }, - "reversed": false, - "folder": "Ground LSide", - "idealStartingState": { - "velocity": 0, - "rotation": -150.82019924107797 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/GrounddB #4.path b/src/main/deploy/pathplanner/paths/GrounddB #4.path deleted file mode 100644 index 0cc0a83..0000000 --- a/src/main/deploy/pathplanner/paths/GrounddB #4.path +++ /dev/null @@ -1,66 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.7213752, - "y": 5.0258832 - }, - "prevControl": null, - "nextControl": { - "x": 3.2108522727272732, - "y": 5.555639204545454 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.722, - "y": 7.129 - }, - "prevControl": { - "x": 2.061030502628888, - "y": 6.716198757492258 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Intake", - "waypointRelativePos": 0.3779527559055112, - "endWaypointRelativePos": 1.0, - "command": { - "type": "named", - "data": { - "name": "IntakeCoral" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 1.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -144.0 - }, - "reversed": false, - "folder": "Ground BSide", - "idealStartingState": { - "velocity": 0, - "rotation": -150.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LSweep P3.path b/src/main/deploy/pathplanner/paths/LSweep P3.path deleted file mode 100644 index 94ffe3f..0000000 --- a/src/main/deploy/pathplanner/paths/LSweep P3.path +++ /dev/null @@ -1,66 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 0.788, - "y": 6.603 - }, - "prevControl": null, - "nextControl": { - "x": 2.9316477272727273, - "y": 6.26396875 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.721, - "y": 5.025 - }, - "prevControl": { - "x": 3.5675633667260422, - "y": 5.222375782631895 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Elevator", - "waypointRelativePos": 0.16647919010123857, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "L4" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 1.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -150.0 - }, - "reversed": false, - "folder": "Ground LSide", - "idealStartingState": { - "velocity": 0, - "rotation": -56.976000000000006 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Lollipop 2.path b/src/main/deploy/pathplanner/paths/Lollipop 2.path deleted file mode 100644 index d258477..0000000 --- a/src/main/deploy/pathplanner/paths/Lollipop 2.path +++ /dev/null @@ -1,66 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.239, - "y": 5.843 - }, - "prevControl": null, - "nextControl": { - "x": 2.189204371946254, - "y": 5.904804878359121 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.6657752058351885, - "y": 5.122 - }, - "prevControl": { - "x": 3.1011126071083357, - "y": 5.555954762962963 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Elevator", - "waypointRelativePos": 0.4830139640748033, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "L4" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.5, - "rotation": -150.68614751980144 - }, - "reversed": false, - "folder": "Lollipop Path", - "idealStartingState": { - "velocity": 0, - "rotation": -89.56435434461116 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Lollipop 3.path b/src/main/deploy/pathplanner/paths/Lollipop 3.path deleted file mode 100644 index f776ef5..0000000 --- a/src/main/deploy/pathplanner/paths/Lollipop 3.path +++ /dev/null @@ -1,80 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6657752058351885, - "y": 5.122185271626888 - }, - "prevControl": null, - "nextControl": { - "x": 3.232096205428022, - "y": 5.337027431134217 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.224, - "y": 4.023 - }, - "prevControl": { - "x": 2.2404897088951454, - "y": 4.642129432338781 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.6121951664719626, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 0.5, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Intake", - "waypointRelativePos": 0.6682840182086613, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "IntakeCoral" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -48.652092538649285 - }, - "reversed": false, - "folder": "Lollipop Path", - "idealStartingState": { - "velocity": 0, - "rotation": -149.20719589802195 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Lollipop 4.path b/src/main/deploy/pathplanner/paths/Lollipop 4.path deleted file mode 100644 index 1774ff5..0000000 --- a/src/main/deploy/pathplanner/paths/Lollipop 4.path +++ /dev/null @@ -1,66 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.224, - "y": 4.023 - }, - "prevControl": null, - "nextControl": { - "x": 1.9751816342630288, - "y": 4.332334397903094 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.923609664351851, - "y": 4.208198061342593 - }, - "prevControl": { - "x": 2.412264463822535, - "y": 4.184931100328749 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Elevator", - "waypointRelativePos": 0.402936988901869, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "L4" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.5, - "rotation": -89.04406269082419 - }, - "reversed": false, - "folder": "Lollipop Path", - "idealStartingState": { - "velocity": 0, - "rotation": -43.54279799705628 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Lollipop 5.path b/src/main/deploy/pathplanner/paths/Lollipop 5.path deleted file mode 100644 index b9155fd..0000000 --- a/src/main/deploy/pathplanner/paths/Lollipop 5.path +++ /dev/null @@ -1,80 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.239, - "y": 4.191 - }, - "prevControl": null, - "nextControl": { - "x": 3.0626564776043614, - "y": 3.9678578908798716 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.197960792824073, - "y": 2.2002983940972216 - }, - "prevControl": { - "x": 0.9319482350521202, - "y": 1.9434278342202775 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.6121951664719626, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 0.5, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Intake", - "waypointRelativePos": 0.6682840182086613, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "IntakeCoral" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -48.652 - }, - "reversed": false, - "folder": "Lollipop Path", - "idealStartingState": { - "velocity": 0, - "rotation": -90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Lollipop 6.path b/src/main/deploy/pathplanner/paths/Lollipop 6.path deleted file mode 100644 index 98904f0..0000000 --- a/src/main/deploy/pathplanner/paths/Lollipop 6.path +++ /dev/null @@ -1,80 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.198, - "y": 2.2 - }, - "prevControl": null, - "nextControl": { - "x": 1.0216564776043615, - "y": 1.976857890879872 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.23, - "y": 4.191 - }, - "prevControl": { - "x": 2.9639874422280474, - "y": 3.9341294401230558 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.6121951664719626, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 0.5, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Intake", - "waypointRelativePos": 0.6682840182086613, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "IntakeCoral" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -90.0 - }, - "reversed": false, - "folder": "Lollipop Path", - "idealStartingState": { - "velocity": 0, - "rotation": -36.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Official Lollipop 1.path b/src/main/deploy/pathplanner/paths/Official Lollipop 1.path deleted file mode 100644 index 81b32a1..0000000 --- a/src/main/deploy/pathplanner/paths/Official Lollipop 1.path +++ /dev/null @@ -1,80 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 5.026909205835189, - "y": 5.287285271626888 - }, - "prevControl": null, - "nextControl": { - "x": 3.9877799051430065, - "y": 5.857346791883403 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.239, - "y": 5.843 - }, - "prevControl": { - "x": 1.9968631781709456, - "y": 5.892619841156129 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.5537930782710281, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 0.5, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Intake", - "waypointRelativePos": 0.40029835137795283, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "IntakeCoral" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -89.24717948106847 - }, - "reversed": false, - "folder": "Lollipop Path", - "idealStartingState": { - "velocity": 0, - "rotation": 150.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/P 1 - Align After.path b/src/main/deploy/pathplanner/paths/P 1 - Align After.path deleted file mode 100644 index bf6da05..0000000 --- a/src/main/deploy/pathplanner/paths/P 1 - Align After.path +++ /dev/null @@ -1,88 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.065, - "y": 7.315 - }, - "prevControl": null, - "nextControl": { - "x": 6.311723981584821, - "y": 6.540661185128348 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.9713092, - "y": 5.1909832 - }, - "prevControl": { - "x": 5.454483001977311, - "y": 5.695001290538291 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "backdrive", - "waypointRelativePos": 0, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "backdrive" - } - } - }, - { - "name": "lower intake", - "waypointRelativePos": 0.14206787687450353, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "lower intake" - } - } - }, - { - "name": "MoveElevator", - "waypointRelativePos": 0.4625098658247798, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "L4" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.5, - "rotation": 150.0 - }, - "reversed": false, - "folder": "Ground LSide", - "idealStartingState": { - "velocity": 0, - "rotation": 150.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/P 1 Mirrored.path b/src/main/deploy/pathplanner/paths/P 1 Mirrored.path deleted file mode 100644 index 772e6f8..0000000 --- a/src/main/deploy/pathplanner/paths/P 1 Mirrored.path +++ /dev/null @@ -1,88 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.065, - "y": 0.735 - }, - "prevControl": null, - "nextControl": { - "x": 6.163789350689934, - "y": 1.5557182436227601 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.094774590163934, - "y": 2.712 - }, - "prevControl": { - "x": 5.687902753152638, - "y": 2.1760455531838283 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "backdrive", - "waypointRelativePos": 0, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "backdrive" - } - } - }, - { - "name": "lower intake", - "waypointRelativePos": 0.14206787687450353, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "lower intake" - } - } - }, - { - "name": "MoveElevator", - "waypointRelativePos": 0.4625098658247798, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "L4" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 29.999999999999996 - }, - "reversed": false, - "folder": "Ground LSide", - "idealStartingState": { - "velocity": 0, - "rotation": 29.999999999999996 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/P 1.path b/src/main/deploy/pathplanner/paths/P 1.path deleted file mode 100644 index 9a24b89..0000000 --- a/src/main/deploy/pathplanner/paths/P 1.path +++ /dev/null @@ -1,88 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.065, - "y": 7.315 - }, - "prevControl": null, - "nextControl": { - "x": 6.407825499545448, - "y": 6.60471106063858 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.094774590163934, - "y": 5.33765368852459 - }, - "prevControl": { - "x": 5.760558198478665, - "y": 6.031726466482178 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "backdrive", - "waypointRelativePos": 0, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "backdrive" - } - } - }, - { - "name": "lower intake", - "waypointRelativePos": 0.14206787687450353, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "lower intake" - } - } - }, - { - "name": "MoveElevator", - "waypointRelativePos": 0.4625098658247798, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "L4" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 150.0 - }, - "reversed": false, - "folder": "Ground LSide", - "idealStartingState": { - "velocity": 0, - "rotation": 150.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/P 3 Mirrored.path b/src/main/deploy/pathplanner/paths/P 3 Mirrored.path deleted file mode 100644 index 023d3dc..0000000 --- a/src/main/deploy/pathplanner/paths/P 3 Mirrored.path +++ /dev/null @@ -1,66 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.722, - "y": 0.921 - }, - "prevControl": null, - "nextControl": { - "x": 1.9852053519873245, - "y": 0.9368665670159784 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.5843237704918027, - "y": 2.796 - }, - "prevControl": { - "x": 3.3462031912644794, - "y": 2.7198515282593014 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Elevator", - "waypointRelativePos": 0.16647919010123857, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "L4" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.0, - "rotation": -29.999999999999996 - }, - "reversed": false, - "folder": "Ground LSide", - "idealStartingState": { - "velocity": 0, - "rotation": -35.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/P 3.path b/src/main/deploy/pathplanner/paths/P 3.path deleted file mode 100644 index 2a95b2f..0000000 --- a/src/main/deploy/pathplanner/paths/P 3.path +++ /dev/null @@ -1,66 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.722, - "y": 7.1288 - }, - "prevControl": null, - "nextControl": { - "x": 2.513128546999364, - "y": 6.385951337740788 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.5843237704918027, - "y": 5.25373975409836 - }, - "prevControl": { - "x": 3.1610940855407414, - "y": 5.679961652038816 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Elevator", - "waypointRelativePos": 0.16647919010123857, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "L4" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.0, - "rotation": -150.0 - }, - "reversed": false, - "folder": "Ground LSide", - "idealStartingState": { - "velocity": 0.0, - "rotation": -145.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/P 5 Mirrored.path b/src/main/deploy/pathplanner/paths/P 5 Mirrored.path deleted file mode 100644 index ac07b82..0000000 --- a/src/main/deploy/pathplanner/paths/P 5 Mirrored.path +++ /dev/null @@ -1,66 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.722, - "y": 0.921 - }, - "prevControl": null, - "nextControl": { - "x": 2.0053161494766774, - "y": 1.1670302077595403 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.8720286885245896, - "y": 2.7 - }, - "prevControl": { - "x": 3.5346593444490164, - "y": 2.4590517448283564 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "MoveElevator", - "waypointRelativePos": 0.39370078740157555, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "L4" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.0, - "rotation": -29.999999999999996 - }, - "reversed": false, - "folder": "Ground LSide", - "idealStartingState": { - "velocity": 0, - "rotation": -35.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/P 5.path b/src/main/deploy/pathplanner/paths/P 5.path deleted file mode 100644 index 8631720..0000000 --- a/src/main/deploy/pathplanner/paths/P 5.path +++ /dev/null @@ -1,66 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.722, - "y": 7.129 - }, - "prevControl": null, - "nextControl": { - "x": 2.58836769386574, - "y": 6.410185185185185 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.8720286885245896, - "y": 5.3496413934426235 - }, - "prevControl": { - "x": 3.2181991167653305, - "y": 5.822776882447252 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "MoveElevator", - "waypointRelativePos": 0.39370078740157555, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "L4" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.0, - "rotation": -151.83 - }, - "reversed": false, - "folder": "Ground LSide", - "idealStartingState": { - "velocity": 0, - "rotation": -145.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Station P2.path b/src/main/deploy/pathplanner/paths/Station P2.path deleted file mode 100644 index 922df85..0000000 --- a/src/main/deploy/pathplanner/paths/Station P2.path +++ /dev/null @@ -1,71 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 4.9713092, - "y": 5.1909832 - }, - "prevControl": null, - "nextControl": { - "x": 4.461440347544805, - "y": 7.189854101642059 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.65, - "y": 7.4 - }, - "prevControl": { - "x": 3.1784221311513603, - "y": 5.374715163937142 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.4991119005328578, - "rotationDegrees": 61.586 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Station", - "waypointRelativePos": 0.6590141796083744, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "Station Setpoint" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 36.0 - }, - "reversed": false, - "folder": "Station Left", - "idealStartingState": { - "velocity": 0, - "rotation": 150.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Station P4.path b/src/main/deploy/pathplanner/paths/Station P4.path deleted file mode 100644 index df31b0a..0000000 --- a/src/main/deploy/pathplanner/paths/Station P4.path +++ /dev/null @@ -1,71 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.9517367941648103, - "y": 5.287285271626888 - }, - "prevControl": null, - "nextControl": { - "x": 3.4418679417096154, - "y": 7.286156173268947 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.356, - "y": 7.331 - }, - "prevControl": { - "x": 2.8844221311513616, - "y": 5.305715163937142 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.8063943161634158, - "rotationDegrees": 61.586 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "MoveElevator", - "waypointRelativePos": 0.6861642294713134, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "Station Setpoint" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 36.0 - }, - "reversed": false, - "folder": "Station Left", - "idealStartingState": { - "velocity": 0, - "rotation": -150.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Station R P2.path b/src/main/deploy/pathplanner/paths/Station R P2.path deleted file mode 100644 index 34fb495..0000000 --- a/src/main/deploy/pathplanner/paths/Station R P2.path +++ /dev/null @@ -1,71 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 4.812, - "y": 2.768 - }, - "prevControl": null, - "nextControl": { - "x": 3.6667827868852463, - "y": 1.1836495901639348 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.525653409090909, - "y": 0.7293892045454539 - }, - "prevControl": { - "x": 3.111195051053366, - "y": 2.9091389109644723 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.6287744227353493, - "rotationDegrees": 131.817128535173 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Station", - "waypointRelativePos": 0.6634844868735067, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "Station Setpoint" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 145.0 - }, - "reversed": false, - "folder": "Station RSide", - "idealStartingState": { - "velocity": 0, - "rotation": 29.999999999999996 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Station R P3.path b/src/main/deploy/pathplanner/paths/Station R P3.path deleted file mode 100644 index ce2a120..0000000 --- a/src/main/deploy/pathplanner/paths/Station R P3.path +++ /dev/null @@ -1,66 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.526, - "y": 0.729 - }, - "prevControl": null, - "nextControl": { - "x": 3.0456919758517573, - "y": 2.2265382848833575 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.007, - "y": 2.861 - }, - "prevControl": { - "x": 3.745218130977533, - "y": 2.6169235964968895 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Elevator", - "waypointRelativePos": 0.16647919010123857, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "L4" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -29.999999999999996 - }, - "reversed": false, - "folder": "Station RSide", - "idealStartingState": { - "velocity": 0, - "rotation": 144.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Station R P4.path b/src/main/deploy/pathplanner/paths/Station R P4.path deleted file mode 100644 index dca9db2..0000000 --- a/src/main/deploy/pathplanner/paths/Station R P4.path +++ /dev/null @@ -1,66 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 4.007, - "y": 2.861 - }, - "prevControl": null, - "nextControl": { - "x": 2.966570514239714, - "y": 1.77435496905575 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.526, - "y": 0.729 - }, - "prevControl": { - "x": 2.4772300707441195, - "y": 1.765652915018721 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Station", - "waypointRelativePos": 0.6587112171837709, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "Station Setpoint" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 144.0 - }, - "reversed": false, - "folder": "Station RSide", - "idealStartingState": { - "velocity": 0, - "rotation": -29.999999999999996 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Station R P5.path b/src/main/deploy/pathplanner/paths/Station R P5.path deleted file mode 100644 index 1c32f18..0000000 --- a/src/main/deploy/pathplanner/paths/Station R P5.path +++ /dev/null @@ -1,66 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.526, - "y": 0.729 - }, - "prevControl": null, - "nextControl": { - "x": 2.9585445192827198, - "y": 1.9643408880242017 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.721, - "y": 3.026 - }, - "prevControl": { - "x": 3.1963255727771056, - "y": 2.590703696539521 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "MoveElevator", - "waypointRelativePos": 0.39370078740157555, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "L4" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -29.999999999999996 - }, - "reversed": false, - "folder": "Station RSide", - "idealStartingState": { - "velocity": 0, - "rotation": 144.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/StationB #5.path b/src/main/deploy/pathplanner/paths/StationB #5.path deleted file mode 100644 index 0100ceb..0000000 --- a/src/main/deploy/pathplanner/paths/StationB #5.path +++ /dev/null @@ -1,66 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.722, - "y": 7.403 - }, - "prevControl": null, - "nextControl": { - "x": 1.8649319141897391, - "y": 6.541760789902279 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.2385, - "y": 4.191 - }, - "prevControl": { - "x": 2.2385, - "y": 4.191 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "MoveElevator", - "waypointRelativePos": 0.6806256151574805, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "L4" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 0.5, - "maxAcceleration": 0.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -90.89200324814249 - }, - "reversed": false, - "folder": "Station Left", - "idealStartingState": { - "velocity": 0, - "rotation": 36.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/StationB Alt Opt - Align After.path b/src/main/deploy/pathplanner/paths/StationB Alt Opt - Align After.path deleted file mode 100644 index da5d433..0000000 --- a/src/main/deploy/pathplanner/paths/StationB Alt Opt - Align After.path +++ /dev/null @@ -1,66 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.356, - "y": 7.331 - }, - "prevControl": null, - "nextControl": { - "x": 2.334068287037036, - "y": 6.521408420138889 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.666, - "y": 5.122185271626888 - }, - "prevControl": { - "x": 2.836426070601852, - "y": 5.99030882486763 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "MoveElevator", - "waypointRelativePos": 0.6806256151574805, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "L4" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.0, - "rotation": -149.28470582855732 - }, - "reversed": false, - "folder": "Station Left", - "idealStartingState": { - "velocity": 0, - "rotation": 36.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/StationB Alt Opt Left - Align After.path b/src/main/deploy/pathplanner/paths/StationB Alt Opt Left - Align After.path deleted file mode 100644 index d148283..0000000 --- a/src/main/deploy/pathplanner/paths/StationB Alt Opt Left - Align After.path +++ /dev/null @@ -1,66 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.356, - "y": 7.331 - }, - "prevControl": null, - "nextControl": { - "x": 2.320208548409597, - "y": 6.4855863560267855 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.9517367941648103, - "y": 5.287285271626888 - }, - "prevControl": { - "x": 3.2517059326171873, - "y": 5.978196498325892 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "MoveElevator", - "waypointRelativePos": 0.6806256151574805, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "L4" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.0, - "rotation": -149.28470582855732 - }, - "reversed": false, - "folder": "Station Left", - "idealStartingState": { - "velocity": 0, - "rotation": 36.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/StationB Alt Opt.path b/src/main/deploy/pathplanner/paths/StationB Alt Opt.path deleted file mode 100644 index c01326c..0000000 --- a/src/main/deploy/pathplanner/paths/StationB Alt Opt.path +++ /dev/null @@ -1,66 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.65, - "y": 7.4 - }, - "prevControl": null, - "nextControl": { - "x": 2.334068287037036, - "y": 6.521408420138889 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.5380135672433033, - "y": 5.395889718191963 - }, - "prevControl": { - "x": 2.708439637845155, - "y": 6.264013271432705 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "MoveElevator", - "waypointRelativePos": 0.6806256151574805, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "L4" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.0, - "rotation": -149.28470582855732 - }, - "reversed": false, - "folder": "Station Left", - "idealStartingState": { - "velocity": 0, - "rotation": 36.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/StationP 3.path b/src/main/deploy/pathplanner/paths/StationP 3.path deleted file mode 100644 index 3b53624..0000000 --- a/src/main/deploy/pathplanner/paths/StationP 3.path +++ /dev/null @@ -1,71 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.65, - "y": 7.4 - }, - "prevControl": null, - "nextControl": { - "x": 2.9294370334836404, - "y": 6.130971138637706 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.8229949951171873, - "y": 5.580079432896205 - }, - "prevControl": { - "x": 3.6027199899253537, - "y": 5.6983119245714535 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.3836589698046169, - "rotationDegrees": 150.75582343932 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "MoveElevator", - "waypointRelativePos": 0.45374746792707615, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "L4" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -151.83 - }, - "reversed": false, - "folder": "Station Left", - "idealStartingState": { - "velocity": 0, - "rotation": 36.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Sweep P2.path b/src/main/deploy/pathplanner/paths/Sweep P2.path deleted file mode 100644 index 996f28a..0000000 --- a/src/main/deploy/pathplanner/paths/Sweep P2.path +++ /dev/null @@ -1,71 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 4.9713092, - "y": 5.191 - }, - "prevControl": null, - "nextControl": { - "x": 4.267840909084701, - "y": 7.908934659084283 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.7777840909028838, - "y": 6.602656249993373 - }, - "prevControl": { - "x": 2.183778409084702, - "y": 7.44026988635701 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5275310834813515, - "rotationDegrees": -77.19714447023723 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Intake", - "waypointRelativePos": 0.3, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "IntakeCoral" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 1.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.0, - "rotation": -56.976000000000006 - }, - "reversed": false, - "folder": "Ground LSide", - "idealStartingState": { - "velocity": 0, - "rotation": 150.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Sweep P2.path b/src/main/deploy/pathplanner/paths/Sweep P2.path deleted file mode 100644 index 1d79a3e..0000000 --- a/src/main/deploy/pathplanner/paths/Sweep P2.path +++ /dev/null @@ -1,87 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 5.157, - "y": 2.996 - }, - "prevControl": null, - "nextControl": { - "x": 4.011782786885246, - "y": 1.411649590163935 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.831931818176833, - "y": 0.5299573863636362 - }, - "prevControl": { - "x": 3.285943759901909, - "y": 0.6018426104701067 - }, - "nextControl": { - "x": 1.6353409090859239, - "y": 0.34049715909090883 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.688, - "y": 1.368 - }, - "prevControl": { - "x": 1.3261818181768328, - "y": 0.709875 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.2984014209591429, - "rotationDegrees": -36.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Intake", - "waypointRelativePos": 0.55, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "IntakeCoral" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 1.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -127.476 - }, - "reversed": false, - "folder": "Sweep Right Side", - "idealStartingState": { - "velocity": 0, - "rotation": 29.999999999999996 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Sweep P3.path b/src/main/deploy/pathplanner/paths/Sweep P3.path deleted file mode 100644 index f55b8e1..0000000 --- a/src/main/deploy/pathplanner/paths/Sweep P3.path +++ /dev/null @@ -1,66 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 0.688, - "y": 1.368 - }, - "prevControl": null, - "nextControl": { - "x": 2.2076919758517572, - "y": 2.8655382848833577 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.0114204545454544, - "y": 2.1852414772727276 - }, - "prevControl": { - "x": 2.749638585522988, - "y": 1.9411650737696169 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Elevator", - "waypointRelativePos": 0.16647919010123857, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "L4" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 1.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -29.999999999999996 - }, - "reversed": false, - "folder": "Sweep Right Side", - "idealStartingState": { - "velocity": 0, - "rotation": -127.476 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Sweep P4.path b/src/main/deploy/pathplanner/paths/Sweep P4.path deleted file mode 100644 index 4dd6a62..0000000 --- a/src/main/deploy/pathplanner/paths/Sweep P4.path +++ /dev/null @@ -1,66 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.721, - "y": 3.0259168 - }, - "prevControl": null, - "nextControl": { - "x": 2.6805705142397143, - "y": 1.93927176905575 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.722, - "y": 0.923 - }, - "prevControl": { - "x": 2.6732300707441192, - "y": 1.959652915018721 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Intake", - "waypointRelativePos": 0.19856459330143417, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "IntakeCoral" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 1.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -36.0 - }, - "reversed": false, - "folder": "Sweep Right Side", - "idealStartingState": { - "velocity": 0, - "rotation": -29.999999999999996 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Sweep RP5.path b/src/main/deploy/pathplanner/paths/Sweep RP5.path deleted file mode 100644 index f528baf..0000000 --- a/src/main/deploy/pathplanner/paths/Sweep RP5.path +++ /dev/null @@ -1,66 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.722, - "y": 0.923 - }, - "prevControl": null, - "nextControl": { - "x": 3.1545445192827195, - "y": 2.1583408880242017 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.00733, - "y": 2.86082 - }, - "prevControl": { - "x": 3.482655572777105, - "y": 2.4255236965395213 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "MoveElevator", - "waypointRelativePos": 0.39370078740157555, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "L4" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 1.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -29.999999999999996 - }, - "reversed": false, - "folder": "Sweep Right Side", - "idealStartingState": { - "velocity": 0, - "rotation": -36.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SweepGroundB #5.path b/src/main/deploy/pathplanner/paths/SweepGroundB #5.path deleted file mode 100644 index a6493ce..0000000 --- a/src/main/deploy/pathplanner/paths/SweepGroundB #5.path +++ /dev/null @@ -1,66 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 0.808, - "y": 6.57 - }, - "prevControl": null, - "nextControl": { - "x": 3.599744318181818, - "y": 6.393252840909091 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.2385, - "y": 4.191 - }, - "prevControl": { - "x": 2.2385, - "y": 4.191 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "MoveElevator", - "waypointRelativePos": 0.6806256151574805, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "L4" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -90.89200324814249 - }, - "reversed": false, - "folder": "Ground BSide", - "idealStartingState": { - "velocity": 0, - "rotation": -53.616 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 06b0fd4..49efa97 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,49 +7,23 @@ import org.json.simple.parser.ParseException; import org.littletonrobotics.junction.Logger; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.commands.DoNothing; import frc.robot.commands.drive_comm.DefaultDriveCommand; -import frc.robot.commands.drive_comm.DriveToPose; -import frc.robot.commands.gpm.IntakeCoral; -import frc.robot.commands.gpm.MoveArm; -import frc.robot.commands.gpm.MoveElevator; -import frc.robot.commands.gpm.OuttakeCoral; -import frc.robot.commands.gpm.StationIntake; -import frc.robot.constants.ArmConstants; import frc.robot.constants.AutoConstants; import frc.robot.constants.Constants; -import frc.robot.constants.ElevatorConstants; -import frc.robot.constants.FieldConstants; -import frc.robot.constants.IntakeConstants; import frc.robot.constants.VisionConstants; import frc.robot.controls.BaseDriverConfig; import frc.robot.controls.Operator; import frc.robot.controls.PS5ControllerDriverConfig; -import frc.robot.subsystems.arm.Arm; -import frc.robot.subsystems.climb.Climb; import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.subsystems.drivetrain.GyroIOPigeon2; -import frc.robot.subsystems.elevator.Elevator; -import frc.robot.subsystems.indexer.Indexer; -import frc.robot.subsystems.intake.Intake; -import frc.robot.subsystems.outtake.Outtake; -import frc.robot.subsystems.outtake.OuttakeAlpha; -import frc.robot.subsystems.outtake.OuttakeComp; import frc.robot.util.PathGroupLoader; import frc.robot.util.Vision.DetectedObject; import frc.robot.util.Vision.Vision; @@ -67,20 +41,10 @@ public class RobotContainer { // The robot's subsystems are defined here... private Drivetrain drive = null; private Vision vision = null; - private Intake intake = null; - private Indexer indexer = null; - private Outtake outtake = null; - private Elevator elevator = null; - private Climb climb = null; - private Arm arm = null; private Command auto = new DoNothing(); - // Dashboard inputs - // private final LoggedDashboardChooser autoChooser; - // Controllers are defined here private BaseDriverConfig driver = null; - @SuppressWarnings("unused") private Operator operator = null; /** @@ -99,56 +63,36 @@ public class RobotContainer { default: case SwerveCompetition: - outtake = new OuttakeComp(); - elevator = new Elevator(); - climb = new Climb(); - arm = new Arm(); - // Arm can move if the elevator is within tolerance of its safe setpoint or higher - arm.setElevatorStowed(() -> elevator.getPosition() < ElevatorConstants.SAFE_SETPOINT - 0.025); - // Elevator can only move down if the arm is in the intake setpoint - elevator.setArmStowed(() -> arm.canMoveElevator()); case BetaBot: - indexer = new Indexer(); - intake = new Intake(); - //SmartDashboard.putData("commadn schedule", CommandScheduler.getInstance()); vision = new Vision(VisionConstants.APRIL_TAG_CAMERAS); // fall-through - // SmartDashboard.putData("RunIntakeAndIndexer", new RunIntakeAndIndexer(intake, indexer)); case Vivace: case Phil: - if (robotId == RobotId.Phil) { - outtake = new OuttakeAlpha(); - } - if (outtake != null) { - //SmartDashboard.putData("OuttakeCoralBasic", new OutakeMotors(intake, outtake)); - // SmartDashboard.putData("OuttakeCoralBasic", new OutakeMotors(intake, outtake)); - // SmartDashboard.putData("l4 outake", new ScoreL4(elevator, outtake)); - } case Vertigo: drive = new Drivetrain(vision, new GyroIOPigeon2()); - driver = new PS5ControllerDriverConfig(drive, elevator, intake, indexer, outtake, climb, arm); - //operator = new Operator(drive, elevator, intake, indexer, outtake, climb); + driver = new PS5ControllerDriverConfig(drive); + operator = new Operator(drive); // Detected objects need access to the drivetrain DetectedObject.setDrive(drive); - //SignalLogger.start(); + // SignalLogger.start(); driver.configureControls(); - //operator.configureControls(); + operator.configureControls(); initializeAutoBuilder(); registerCommands(); PathGroupLoader.loadPathGroups(); // Load the auto command try { - PathPlannerAuto.getPathGroupFromAutoFile("Left Side"); - auto = new PathPlannerAuto("Left Side"); + PathPlannerAuto.getPathGroupFromAutoFile("Command Name"); + auto = new PathPlannerAuto("Path Name"); } catch (IOException | ParseException e) { - e.printStackTrace(); + e.printStackTrace(); } drive.setDefaultCommand(new DefaultDriveCommand(drive, driver)); break; @@ -157,7 +101,6 @@ public class RobotContainer { // This is really annoying so it's disabled DriverStation.silenceJoystickConnectionWarning(true); - //addPaths(); // TODO: verify this claim. // LiveWindow is causing periodic loop overruns LiveWindow.disableAllTelemetry(); @@ -192,177 +135,9 @@ public class RobotContainer { } public void registerCommands() { - if(intake != null && indexer != null && elevator != null && arm != null){ - NamedCommands.registerCommand("IntakeCoral", new IntakeCoral(intake, indexer, elevator, outtake, arm)); - NamedCommands.registerCommand("lower intake", new InstantCommand(() -> intake.setAngle(IntakeConstants.INTAKE_SAFE_POINT))); - } - if(elevator != null && outtake != null && arm != null){ - NamedCommands.registerCommand("OuttakeCoral", new OuttakeCoral(outtake, elevator, arm).withTimeout(1.5)); - NamedCommands.registerCommand("L4", - new ParallelCommandGroup( - new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT), - new MoveArm(arm, ArmConstants.L4_SETPOINT_RIGHT) - ) - ); - NamedCommands.registerCommand("backdrive", new InstantCommand(() -> outtake.setMotor(0.02))); - - NamedCommands.registerCommand("Lower Elevator", new SequentialCommandGroup( - new InstantCommand(()->arm.setSetpoint(ArmConstants.INTAKE_SETPOINT)), - new InstantCommand(()->elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT)) - )); - - NamedCommands.registerCommand("Score L4", new SequentialCommandGroup( - new ParallelCommandGroup( - new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT), - new MoveArm(arm, ArmConstants.L4_SETPOINT_RIGHT) - ), - new OuttakeCoral(outtake, elevator, arm) - )); - - NamedCommands.registerCommand("Score L3", new SequentialCommandGroup( - new ParallelCommandGroup( - new MoveElevator(elevator, ElevatorConstants.L3_SETPOINT), - new MoveArm(arm, ArmConstants.L2_L3_SETPOINT) - ), - new OuttakeCoral(outtake, elevator, arm) - )); - NamedCommands.registerCommand("Score L2", new SequentialCommandGroup( - new ParallelCommandGroup( - new MoveElevator(elevator, ElevatorConstants.L2_SETPOINT), - new MoveArm(arm, ArmConstants.L2_L3_SETPOINT) - ), - new OuttakeCoral(outtake, elevator, arm) - )); - - NamedCommands.registerCommand("L3", - new ParallelCommandGroup( - new MoveElevator(elevator, ElevatorConstants.L3_SETPOINT), - new MoveArm(arm, ArmConstants.L2_L3_SETPOINT) - ) - ); - NamedCommands.registerCommand("L2", - new ParallelCommandGroup( - new MoveElevator(elevator, ElevatorConstants.L2_SETPOINT), - new MoveArm(arm, ArmConstants.L2_L3_SETPOINT) - ) - ); - NamedCommands.registerCommand("Station Setpoint", - new ParallelCommandGroup( - new MoveElevator(elevator, ElevatorConstants.STATION_INTAKE_SETPOINT), - new MoveArm(arm, ArmConstants.STATION_INTAKE_SETPOINT), - new StationIntake(outtake) - ) - ); - - //NamedCommands.registerCommand("L1", new MoveElevator(elevator, ElevatorConstants.L1_SETPOINT)); - - NamedCommands.registerCommand("Station Intake", new StationIntake(outtake)); - - Pose2d blueStationRight = new Pose2d(1.722, 0.923, Rotation2d.fromDegrees(-36)); - Pose2d blueStationLeft = new Pose2d(blueStationRight.getX(), FieldConstants.FIELD_WIDTH-blueStationRight.getY(), Rotation2d.fromDegrees(-144)); - - Pose2d blueStationIntakeLeft = new Pose2d(1.65, 7.4, Rotation2d.fromDegrees(-144-180)); - Pose2d blueStationIntakeRight = new Pose2d(1.526, 0.729, Rotation2d.fromDegrees(-144-180)); - - Pose2d redStationRight = new Pose2d(FieldConstants.FIELD_LENGTH-blueStationRight.getX(), blueStationLeft.getY(), blueStationRight.getRotation().plus(new Rotation2d(Math.PI))); - Pose2d redStationLeft = new Pose2d(FieldConstants.FIELD_LENGTH-blueStationLeft.getX(), blueStationRight.getY(), blueStationLeft.getRotation().plus(new Rotation2d(Math.PI))); - NamedCommands.registerCommand("Drive To Left Station", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? redStationLeft : blueStationLeft)); - NamedCommands.registerCommand("Drive To Right Station Intake", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? redStationRight : blueStationIntakeRight)); - NamedCommands.registerCommand("Drive To Left Station Intake", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? redStationLeft : blueStationIntakeLeft)); - - NamedCommands.registerCommand("Drive To Right Station", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? redStationRight : blueStationRight)); - NamedCommands.registerCommand("Drive To 6/19 Left", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_6_LEFT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_19_LEFT.l4Pose).withTimeout(1)); - NamedCommands.registerCommand("Drive To 6/19 Right", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_6_RIGHT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_19_RIGHT.l4Pose).withTimeout(1)); - NamedCommands.registerCommand("Drive To 7/18 Left", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_7_LEFT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_18_LEFT.l4Pose)); - NamedCommands.registerCommand("Drive To 7/18 Right", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_7_RIGHT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_18_RIGHT.l4Pose)); - NamedCommands.registerCommand("Drive To 10/21 Right", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_9_RIGHT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_22_RIGHT.l4Pose)); - NamedCommands.registerCommand("Drive To 11/20 Left", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_11_LEFT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_20_LEFT.l4Pose)); - NamedCommands.registerCommand("Drive To 11/20 Right", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_11_RIGHT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_20_RIGHT.l4Pose)); - NamedCommands.registerCommand("Drive To 9/22 Left", new DriveToPose(drive, () -> DriverStation.getAlliance().get() - == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_9_LEFT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_22_LEFT.l4Pose)); - - - NamedCommands.registerCommand("Drive To 8/17 Left", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_8_LEFT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_17_LEFT.l4Pose).withTimeout(1)); - NamedCommands.registerCommand("Drive To 8/17 Right", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_8_RIGHT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_17_RIGHT.l4Pose).withTimeout(1)); - } } - // public void addPaths(){ - // try { - // PathPlannerAuto.getPathGroupFromAutoFile("Left Side"); - // } - // catch (IOException | ParseException e) { - // e.printStackTrace(); - // } - // autoChooser.addDefaultOption("Left Side", new PathPlannerAuto("Left Side")); - - // autoChooser.addOption("Station Right Side", new PathPlannerAuto("Station Right Side")); - // autoChooser.addOption("Left Side Lollipop", new PathPlannerAuto("Left Side Lollipop")); - // autoChooser.addOption("Left Side Ground", new PathPlannerAuto("Left Side Ground")); - - // if(intake != null && indexer != null && arm != null && elevator != null){ - // autoChooser.addOption("One peice blue", - // new SequentialCommandGroup( - // new InstantCommand(()->{ - // drive.resetOdometry(new Pose2d(7.229,4.191, Rotation2d.fromDegrees(90.0))); - // intake.setAngle(IntakeConstants.INTAKE_SAFE_POINT); - // }), - // new DriveToPose(drive, () -> VisionConstants.REEF.BLUE_BRANCH_21_RIGHT.pose).withTimeout(8), - // new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT), - // new MoveArm(arm, ArmConstants.L4_SETPOINT), - // new OuttakeCoral(outtake, elevator, arm), - // new SequentialCommandGroup(new WaitCommand(0.1), - // new MoveArm(arm, ArmConstants.INTAKE_SETPOINT), - // new InstantCommand(()->elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT))), - // new InstantCommand(()-> intake.stow()) - // )); - // autoChooser.addOption("One peice red", - // new SequentialCommandGroup( - // new InstantCommand(()->{ - // drive.resetOdometry(new Pose2d(FieldConstants.FIELD_LENGTH-7.229,FieldConstants.FIELD_WIDTH-4.191, Rotation2d.fromDegrees(-90.0))); - // intake.setAngle(IntakeConstants.INTAKE_SAFE_POINT); - // }), - // new DriveToPose(drive, () -> VisionConstants.REEF.RED_BRANCH_10_RIGHT.pose).withTimeout(8), - // new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT), - // new MoveArm(arm, ArmConstants.L4_SETPOINT), - // new OuttakeCoral(outtake, elevator, arm), - // new SequentialCommandGroup(new WaitCommand(0.1), - // new MoveArm(arm, ArmConstants.INTAKE_SETPOINT), - // new InstantCommand(()->elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT))), - // new InstantCommand(()-> intake.stow()) - // )); - // } - // // autoChooser.addOption("#1", new FollowPathCommand("#1", true, drive) - // // .andThen(new MoveElevator(elevator, ElevatorConstants.L3_SETPOINT)) - // // .andThen(new OuttakeCoral(outtake, elevator, arm)) - // // .andThen(new FollowPathCommand("#2", true, drive)) - // // .andThen(new FollowPathCommand("#3", true, drive)) - // // .andThen(new MoveElevator(elevator, ElevatorConstants.L3_SETPOINT)) - // // .andThen(new OuttakeCoral(outtake, elevator, arm)) - // // .andThen(new FollowPathCommand("#4", true, drive)) - // // .andThen(new FollowPathCommand("#5", true, drive)) - // // .andThen(new MoveElevator(elevator, ElevatorConstants.L3_SETPOINT)) - // // .andThen(new OuttakeCoral(outtake, elevator, arm))); - - - // if(elevator != null && outtake != null) { - // autoChooser.addOption("WaitTest", new FollowPathCommand("Tester", true, drive) - // .andThen(new OuttakeCoralBasic(outtake, ()->true, ()->false)) - // .andThen(new WaitCommand(3)) - // .andThen(new FollowPathCommand("Next Tester", true, drive)) - // ); - - // autoChooser.addOption("Center to G", new FollowPathCommand("Center to G", true, drive) - // .andThen(new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT)) - // .andThen(new OuttakeCoral(outtake, elevator, arm))); - - // autoChooser.addOption("Center to H", new FollowPathCommand("Center to H", true, drive) - // .andThen(new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT)) - // .andThen(new OuttakeCoral(outtake, elevator, arm))); - // } - // } - public static BooleanSupplier getAllianceColorBooleanSupplier() { return () -> { // Boolean supplier that controls when the path will be mirrored for the red @@ -391,42 +166,13 @@ public class RobotContainer { return auto; } - // Logged psoitins of subsystems - public double elevatorHeightLogged(){ - return elevator == null ? 0 : elevator.getPosition(); - } - public double armAngleLogged(){ - return arm == null ? 0 : arm.getAngle() + 90; - } - public double intakeAngleLogged(){ - return intake == null ? -90 : -intake.getPivotAngle(); - } - public double climbAngleLogged(){ - return climb == null ? 0 : climb.getEstimatedClimbAngle(); - } - public void logComponents(){ if(!Constants.LOG_MECHANISMS) return; Logger.recordOutput( "ComponentPoses", new Pose3d[] { - //intake - new Pose3d(0,-0.25,0.27, new Rotation3d(Units.degreesToRadians(intakeAngleLogged()), 0.0, 0.0)), - //climb - new Pose3d(0,0,0, new Rotation3d(0.0,climbAngleLogged(), 0.0)), - //arm - new Pose3d(0,0.110, 0.388 + elevatorHeightLogged(), new Rotation3d(Units.degreesToRadians(armAngleLogged()), 0.0, 0.0)), - //elevator 1 - new Pose3d(0,0,0, new Rotation3d(0.0, 0.0, 0.0)), - //elevator 2 - new Pose3d(0,0, elevatorHeightLogged()/3, new Rotation3d(0.0, 0.0, 0.0)), - //elevator 3 - new Pose3d(0,0, elevatorHeightLogged()*2/3, new Rotation3d(0.0, 0.0, 0.0)), - //elevator 4 - new Pose3d(0,0, elevatorHeightLogged(), new Rotation3d(0.0, 0.0, 0.0)), - //indexer - new Pose3d(0,0,0, new Rotation3d(0.0, 0.0, 0.0)), + // Subsystem Pose3ds } ); } diff --git a/src/main/java/frc/robot/RobotId.java b/src/main/java/frc/robot/RobotId.java index 24d04e0..bd26230 100644 --- a/src/main/java/frc/robot/RobotId.java +++ b/src/main/java/frc/robot/RobotId.java @@ -71,9 +71,6 @@ public enum RobotId { } } - // report the RobotId to the SmartDashboard. - //SmartDashboard.putString("RobotID", robotId.name()); - // return the robot identity return robotId; } diff --git a/src/main/java/frc/robot/commands/SupplierCommand.java b/src/main/java/frc/robot/commands/SupplierCommand.java index b173bd1..c8ada52 100644 --- a/src/main/java/frc/robot/commands/SupplierCommand.java +++ b/src/main/java/frc/robot/commands/SupplierCommand.java @@ -8,6 +8,7 @@ import java.util.function.Supplier; /** * Runs the given command when this command is initialized, and ends when it ends. * Useful for commands that are not created yet because the constructor parameters are not available until initialization. + * This is very similar to WPILib's DeferredCommand */ public class SupplierCommand extends Command { diff --git a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java index 0ea7bf8..c289b5d 100644 --- a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java +++ b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java @@ -14,7 +14,7 @@ import frc.robot.util.Vision.DriverAssist; * Default drive command. Drives robot using driver controls. */ public class DefaultDriveCommand extends Command { - private final Drivetrain swerve; + protected final Drivetrain swerve; private final BaseDriverConfig driver; public DefaultDriveCommand( @@ -50,22 +50,29 @@ public class DefaultDriveCommand extends Command { ChassisSpeeds driverInput = new ChassisSpeeds(forwardTranslation, sideTranslation, rotation); ChassisSpeeds corrected = DriverAssist.calculate(swerve, driverInput, swerve.getDesiredPose(), true); + drive(corrected); + } + + /** + * Drives the robot + * @param speeds The ChassisSpeeds to drive at + */ + protected void drive(ChassisSpeeds speeds){ // If the driver is pressing the align button or a command set the drivetrain to // align, then align to speaker if (driver.getIsAlign() || swerve.getIsAlign()) { swerve.driveHeading( - forwardTranslation, - sideTranslation, - swerve.getAlignAngle(), - true); + speeds.vxMetersPerSecond, + speeds.vyMetersPerSecond, + swerve.getAlignAngle(), + true); } else { swerve.drive( - corrected.vxMetersPerSecond, - corrected.vyMetersPerSecond, - corrected.omegaRadiansPerSecond, - true, - false); + speeds.vxMetersPerSecond, + speeds.vyMetersPerSecond, + speeds.omegaRadiansPerSecond, + true, + false); } } } - diff --git a/src/main/java/frc/robot/commands/gpm/IntakeAlgae.java b/src/main/java/frc/robot/commands/gpm/IntakeAlgae.java deleted file mode 100644 index 0b38ce3..0000000 --- a/src/main/java/frc/robot/commands/gpm/IntakeAlgae.java +++ /dev/null @@ -1,26 +0,0 @@ -package frc.robot.commands.gpm; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.IntakeConstants; -import frc.robot.subsystems.intake.Intake; - -public class IntakeAlgae extends Command { - private final Intake intake; - - public IntakeAlgae(Intake intake) { - this.intake = intake; - addRequirements(intake); - } - - @Override - public void initialize() { - intake.setAngle(IntakeConstants.ALGAE_SETPOINT); - intake.setSpeed(IntakeConstants.ALGAE_INTAKE_POWER); - } - - @Override - public void end(boolean interrupted) { - intake.deactivate(); - intake.setSpeed(-0.05); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/gpm/IntakeAlgaeArm.java b/src/main/java/frc/robot/commands/gpm/IntakeAlgaeArm.java deleted file mode 100644 index 5ddef9b..0000000 --- a/src/main/java/frc/robot/commands/gpm/IntakeAlgaeArm.java +++ /dev/null @@ -1,23 +0,0 @@ -package frc.robot.commands.gpm; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.outtake.Outtake; - -public class IntakeAlgaeArm extends Command { - private final Outtake outtake; - - public IntakeAlgaeArm(Outtake outtake) { - this.outtake = outtake; - addRequirements(outtake); - } - - @Override - public void initialize() { - outtake.intakeAlgaeReef(); - } - - @Override - public void end(boolean interrupted) { - outtake.setMotor(-0.1); - } -} diff --git a/src/main/java/frc/robot/commands/gpm/IntakeCoral.java b/src/main/java/frc/robot/commands/gpm/IntakeCoral.java deleted file mode 100644 index 2b02329..0000000 --- a/src/main/java/frc/robot/commands/gpm/IntakeCoral.java +++ /dev/null @@ -1,28 +0,0 @@ -package frc.robot.commands.gpm; - -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.constants.ArmConstants; -import frc.robot.constants.ElevatorConstants; -import frc.robot.subsystems.arm.Arm; -import frc.robot.subsystems.elevator.Elevator; -import frc.robot.subsystems.indexer.Indexer; -import frc.robot.subsystems.intake.Intake; -import frc.robot.subsystems.outtake.Outtake; - -/** - * Intake a coral. - */ -public class IntakeCoral extends SequentialCommandGroup { - public IntakeCoral(Intake intake, Indexer indexer, Elevator elevator, Outtake outtake, Arm arm) { - addCommands( - new ParallelCommandGroup( - new InstantCommand(() -> { - intake.unstow(); - elevator.setSetpoint(ElevatorConstants.INTAKE_SETPOINT); - arm.setSetpoint(ArmConstants.INTAKE_SETPOINT);}), - new IntakeCoralHelper(intake, indexer, outtake, arm, elevator))); - } - -} diff --git a/src/main/java/frc/robot/commands/gpm/IntakeCoralHelper.java b/src/main/java/frc/robot/commands/gpm/IntakeCoralHelper.java deleted file mode 100644 index 3f5159a..0000000 --- a/src/main/java/frc/robot/commands/gpm/IntakeCoralHelper.java +++ /dev/null @@ -1,106 +0,0 @@ -package frc.robot.commands.gpm; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.ArmConstants; -import frc.robot.constants.ElevatorConstants; -import frc.robot.subsystems.arm.Arm; -import frc.robot.subsystems.elevator.Elevator; -import frc.robot.subsystems.indexer.Indexer; -import frc.robot.subsystems.intake.Intake; -import frc.robot.subsystems.outtake.Outtake; - -/** - * All the intake code except the elevator stuff. - * Don't call this directly; use {@link frc.robot.commands.gpm.IntakeCoral} - * instead. - */ -public class IntakeCoralHelper extends Command { - private Intake intake; - private Indexer indexer; - private Outtake outtake; - private Arm arm; - private Elevator elevator; - - private enum Phase { - Acquiring, Intaking, Indexing, Detected, InOuttake, Done - }; - - private Phase phase; - - public IntakeCoralHelper(Intake intake, Indexer indexer, Outtake outtake, Arm arm, Elevator elevator) { - this.intake = intake; - this.indexer = indexer; - this.outtake = outtake; - this.arm = arm; - this.elevator = elevator; - addRequirements(intake, indexer, arm, elevator); - if(outtake != null){ - addRequirements(outtake); - } - } - - @Override - public void initialize() { - intake.activate(); - intake.unstow(); - indexer.run(); - phase = Phase.Acquiring; - if(outtake != null) { - outtake.setMotor(0.7); - } - } - - @Override - public void execute() { - if(outtake != null && outtake.coralLoaded()){ - phase = Phase.InOuttake; - } - switch (phase) { - case Acquiring: - case Intaking: - case Indexing: - if (!indexer.isIndexerClear()) { - phase = Phase.Detected; - intake.setSpeed(0.1); - indexer.slow(); - } - break; - case Detected: - if(indexer.isIndexerClear()){ - phase = Phase.InOuttake; - intake.deactivate(); - intake.stow(); - } - break; - case InOuttake: - if(outtake == null || outtake.coralLoaded()){ - phase = Phase.Done; - elevator.setSetpoint(ElevatorConstants.INTAKE_STOW_SETPOINT); - arm.setSetpoint(ArmConstants.STOW_SETPOINT); - indexer.stop(); - if(outtake != null){ - outtake.setMotor(0.1); - } - } - break; - case Done: - break; - } - } - - @Override - public boolean isFinished() { - return phase == Phase.Done && elevator.getPosition() > ElevatorConstants.SAFE_SETPOINT-0.025; - } - - @Override - public void end(boolean interrupted) { - // in case it's interrupted - intake.deactivate(); - intake.stow(); - indexer.stop(); - if(outtake != null){ - outtake.setMotor(0.02); - } - } -} diff --git a/src/main/java/frc/robot/commands/gpm/MoveArm.java b/src/main/java/frc/robot/commands/gpm/MoveArm.java deleted file mode 100644 index 1a70774..0000000 --- a/src/main/java/frc/robot/commands/gpm/MoveArm.java +++ /dev/null @@ -1,25 +0,0 @@ -package frc.robot.commands.gpm; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.arm.Arm; - -public class MoveArm extends Command { - private Arm arm; - private double setpoint; - - public MoveArm(Arm arm, double setpoint) { - this.arm = arm; - this.setpoint = setpoint; - addRequirements(arm); - } - - @Override - public void initialize() { - arm.setSetpoint(setpoint); - } - - @Override - public boolean isFinished() { - return arm.atSetpoint(); - } -} diff --git a/src/main/java/frc/robot/commands/gpm/MoveElevator.java b/src/main/java/frc/robot/commands/gpm/MoveElevator.java deleted file mode 100644 index 200f6a8..0000000 --- a/src/main/java/frc/robot/commands/gpm/MoveElevator.java +++ /dev/null @@ -1,47 +0,0 @@ -package frc.robot.commands.gpm; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.elevator.Elevator; - -/** - * Moves the elevator to a position - */ -public class MoveElevator extends Command { - private Elevator elevator; - private double setpoint; - - /** - * Creates a command to move the elevator to a position - * - * @param elevator The elevator subsystem - * @param setpoint The setpoint to move to - */ - public MoveElevator(Elevator elevator, double setpoint) { - this.elevator = elevator; - this.setpoint = setpoint; - if(elevator != null){ - addRequirements(elevator); - } - } - - /** - * Sets the elevator setpoint - */ - @Override - public void initialize() { - if(elevator != null){ - elevator.setSetpoint(setpoint); - } - } - - /** - * Returns whether the elevator is at the setpoint - * - * @return True if the elevator is within about 1 inch of the setpoint, false - * otherwise - */ - @Override - public boolean isFinished() { - return elevator == null || elevator.atSetpoint(); - } -} diff --git a/src/main/java/frc/robot/commands/gpm/NetSetpoint.java b/src/main/java/frc/robot/commands/gpm/NetSetpoint.java deleted file mode 100644 index 57e92ac..0000000 --- a/src/main/java/frc/robot/commands/gpm/NetSetpoint.java +++ /dev/null @@ -1,42 +0,0 @@ -package frc.robot.commands.gpm; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.constants.ArmConstants; -import frc.robot.constants.ElevatorConstants; -import frc.robot.constants.FieldConstants; -import frc.robot.subsystems.arm.Arm; -import frc.robot.subsystems.drivetrain.Drivetrain; -import frc.robot.subsystems.elevator.Elevator; - -public class NetSetpoint extends SequentialCommandGroup { - public NetSetpoint(Elevator elevator, Arm arm, Drivetrain drive){ - this(true, elevator, arm, drive); - } - public NetSetpoint(boolean chooseClosestSide, Elevator elevator, Arm arm, Drivetrain drive){ - addCommands( - // new InstantCommand(()->{ - // if(chooseClosestSide){ - // drive.setAlignAngle( - // useSetpoint1(drive, chooseClosestSide) == (drive.getPose().getX() < FieldConstants.FIELD_LENGTH/2) - // ? Math.PI / 2 : -Math.PI / 2 - // ); - // drive.setIsAlign(false); - // } - // }), - new ParallelCommandGroup( - new MoveArm(arm, ArmConstants.ALGAE_STOW_SETPOINT), - new MoveElevator(elevator, ElevatorConstants.NET_SETPOINT)), - new ConditionalCommand( - new MoveArm(arm, ArmConstants.ALGAE_NET_SETPOINT_1), - new MoveArm(arm, ArmConstants.ALGAE_NET_SETPOINT_2), - ()->useSetpoint1(drive, chooseClosestSide)) - ); - } - private static boolean useSetpoint1(Drivetrain drive, boolean chooseClosestSide){ - boolean positive = MathUtil.angleModulus(drive.getYaw().getRadians()) > 0; - return !chooseClosestSide || positive == (drive.getPose().getX() < FieldConstants.FIELD_LENGTH/2); - } -} diff --git a/src/main/java/frc/robot/commands/gpm/OutakeMotors.java b/src/main/java/frc/robot/commands/gpm/OutakeMotors.java deleted file mode 100644 index 29b2953..0000000 --- a/src/main/java/frc/robot/commands/gpm/OutakeMotors.java +++ /dev/null @@ -1,27 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands.gpm; - -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.subsystems.intake.Intake; -import frc.robot.subsystems.outtake.Outtake; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class OutakeMotors extends SequentialCommandGroup { - /** Creates a new OutakeMotors. */ - public OutakeMotors(Intake intake, Outtake outtake) { - // Add your commands in the addCommands() call, e.g. - // addCommands(new FooCommand(), new BarCommand()); - addCommands( - new InstantCommand(()->outtake.outtake(), outtake), - new WaitCommand(0.5), - new InstantCommand(()->outtake.stop(), outtake) - ); - } -} diff --git a/src/main/java/frc/robot/commands/gpm/OuttakeAlgae.java b/src/main/java/frc/robot/commands/gpm/OuttakeAlgae.java deleted file mode 100644 index 1bae55f..0000000 --- a/src/main/java/frc/robot/commands/gpm/OuttakeAlgae.java +++ /dev/null @@ -1,16 +0,0 @@ -package frc.robot.commands.gpm; - -import edu.wpi.first.wpilibj2.command.ConditionalCommand; -import frc.robot.constants.IntakeConstants; -import frc.robot.subsystems.intake.Intake; -import frc.robot.subsystems.outtake.Outtake; - -public class OuttakeAlgae extends ConditionalCommand { - public OuttakeAlgae(Outtake outtake, Intake intake){ - super( - new OuttakeAlgaeArm(outtake), - new OuttakeAlgaeIntake(intake), - ()-> intake.isAtSetpoint(IntakeConstants.STOW_SETPOINT) - ); - } -} diff --git a/src/main/java/frc/robot/commands/gpm/OuttakeAlgaeArm.java b/src/main/java/frc/robot/commands/gpm/OuttakeAlgaeArm.java deleted file mode 100644 index 6a2ec85..0000000 --- a/src/main/java/frc/robot/commands/gpm/OuttakeAlgaeArm.java +++ /dev/null @@ -1,34 +0,0 @@ -package frc.robot.commands.gpm; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.outtake.Outtake; - -public class OuttakeAlgaeArm extends Command{ - private final Outtake outtake; - - private final Timer timer = new Timer(); - private final double EJECTION_TIME = 0.25; - - public OuttakeAlgaeArm(Outtake outtake) { - this.outtake = outtake; - addRequirements(outtake); - } - - @Override - public void initialize() { - outtake.outtakeAlgae(); - timer.restart(); - } - - @Override - public boolean isFinished() { - return timer.hasElapsed(EJECTION_TIME); - } - - @Override - public void end(boolean interrupted) { - outtake.stop(); - timer.stop(); - } -} diff --git a/src/main/java/frc/robot/commands/gpm/OuttakeAlgaeIntake.java b/src/main/java/frc/robot/commands/gpm/OuttakeAlgaeIntake.java deleted file mode 100644 index dba0acf..0000000 --- a/src/main/java/frc/robot/commands/gpm/OuttakeAlgaeIntake.java +++ /dev/null @@ -1,38 +0,0 @@ -package frc.robot.commands.gpm; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.IntakeConstants; -import frc.robot.subsystems.intake.Intake; - -public class OuttakeAlgaeIntake extends Command { - private Intake intake; - - private final Timer timer = new Timer(); - private final double EJECTION_TIME = 1; - - public OuttakeAlgaeIntake(Intake intake) { - this.intake = intake; - addRequirements(intake); - } - - @Override - public void initialize() { - intake.setSpeed(IntakeConstants.ALGAE_OUTTAKE_POWER); - intake.setAngle(50); - timer.restart(); - } - - @Override - public boolean isFinished() { - return timer.hasElapsed(EJECTION_TIME); - } - - @Override - public void end(boolean interrupted) { - intake.deactivate(); - intake.stow(); - timer.stop(); - } - -} diff --git a/src/main/java/frc/robot/commands/gpm/OuttakeCoral.java b/src/main/java/frc/robot/commands/gpm/OuttakeCoral.java deleted file mode 100644 index 1ea8e57..0000000 --- a/src/main/java/frc/robot/commands/gpm/OuttakeCoral.java +++ /dev/null @@ -1,24 +0,0 @@ -package frc.robot.commands.gpm; - -import java.util.function.BooleanSupplier; - -import edu.wpi.first.wpilibj2.command.ConditionalCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.constants.ElevatorConstants; -import frc.robot.subsystems.arm.Arm; -import frc.robot.subsystems.elevator.Elevator; -import frc.robot.subsystems.outtake.Outtake; - -public class OuttakeCoral extends SequentialCommandGroup { - public OuttakeCoral(Outtake outtake, Elevator elevator, Arm arm){ - BooleanSupplier l4Supplier = () -> elevator.getSetpoint() > ElevatorConstants.L3_SETPOINT + 0.001; - BooleanSupplier l1Supplier = () -> elevator.getSetpoint() < ElevatorConstants.L1_SETPOINT + 0.001; - addCommands( - new ConditionalCommand( - new ScoreL4(outtake, arm), - new OuttakeCoralBasic(outtake, l4Supplier, l1Supplier), - l4Supplier) - //new InstantCommand(()->elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT), elevator) - ); - } -} diff --git a/src/main/java/frc/robot/commands/gpm/OuttakeCoralBasic.java b/src/main/java/frc/robot/commands/gpm/OuttakeCoralBasic.java deleted file mode 100644 index 7279dc4..0000000 --- a/src/main/java/frc/robot/commands/gpm/OuttakeCoralBasic.java +++ /dev/null @@ -1,54 +0,0 @@ -package frc.robot.commands.gpm; - - -import java.util.function.BooleanSupplier; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.outtake.Outtake; - - -/** - * Command to eject coral. - * Wants coral to be present. - */ -public class OuttakeCoralBasic extends Command { - public static final double L1_SPEED = -0.2; - public static final double L4_SPEED = -0.8; - public static final double OUTTAKE_SPEED = -0.45; - - private Outtake outtake; - - // counter to measure time in ticks (every 20 milliseconds); - private Timer timer = new Timer(); - - private BooleanSupplier l4Supplier; - private BooleanSupplier l1Supplier; - - public OuttakeCoralBasic(Outtake outtake, BooleanSupplier l4Supplier, BooleanSupplier l1Supplier){ - this.outtake = outtake; - this.l4Supplier = l4Supplier; - this.l1Supplier = l1Supplier; - addRequirements(outtake); - } - - - @Override - public void initialize(){ - timer.restart(); - boolean l4 = l4Supplier.getAsBoolean(); - boolean l1 = !l4 && l1Supplier.getAsBoolean(); - outtake.setMotor(l4 ? L4_SPEED : l1 ? L1_SPEED : OUTTAKE_SPEED); - } - - public boolean isFinished(){ - return timer.hasElapsed(0.25); - } - - - public void end(boolean interrupted){ - outtake.stop(); - } -} - - diff --git a/src/main/java/frc/robot/commands/gpm/RemoveAlgae.java b/src/main/java/frc/robot/commands/gpm/RemoveAlgae.java deleted file mode 100644 index c63db70..0000000 --- a/src/main/java/frc/robot/commands/gpm/RemoveAlgae.java +++ /dev/null @@ -1,22 +0,0 @@ -package frc.robot.commands.gpm; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.outtake.Outtake; - -public class RemoveAlgae extends Command { - private Outtake outtake; - public RemoveAlgae(Outtake outtake){ - this.outtake = outtake; - addRequirements(outtake); - } - - @Override - public void initialize(){ - outtake.removeAlgae(); - } - - @Override - public void end(boolean interrupted){ - outtake.stop(); - } -} diff --git a/src/main/java/frc/robot/commands/gpm/ResetClimb.java b/src/main/java/frc/robot/commands/gpm/ResetClimb.java deleted file mode 100644 index 4479bc8..0000000 --- a/src/main/java/frc/robot/commands/gpm/ResetClimb.java +++ /dev/null @@ -1,23 +0,0 @@ -package frc.robot.commands.gpm; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.climb.Climb; - -public class ResetClimb extends Command { - private Climb climb; - - public ResetClimb(Climb climb){ - this.climb = climb; - addRequirements(climb); - } - - @Override - public void initialize(){ - climb.reset(true); - } - @Override - public void end(boolean interrupted){ - climb.reset(false); - } - -} diff --git a/src/main/java/frc/robot/commands/gpm/ReverseMotors.java b/src/main/java/frc/robot/commands/gpm/ReverseMotors.java deleted file mode 100644 index 254d08d..0000000 --- a/src/main/java/frc/robot/commands/gpm/ReverseMotors.java +++ /dev/null @@ -1,45 +0,0 @@ -package frc.robot.commands.gpm; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.indexer.Indexer; -import frc.robot.subsystems.intake.Intake; -import frc.robot.subsystems.outtake.Outtake; - -public class ReverseMotors extends Command { - private Intake intake; - private Indexer indexer; - private Outtake outtake; - - public ReverseMotors(Intake intake, Indexer indexer, Outtake outtake) { - this.intake = intake; - this.indexer = indexer; - this.outtake = outtake; - addRequirements(intake, indexer); - if(outtake != null){ - addRequirements(outtake); - } - } - - @Override - public void initialize() { - intake.setSpeed(-.5); - indexer.reverse(); - if(outtake != null){ - outtake.reverse(); - } - } - - @Override - public boolean isFinished() { - return false; - } - - @Override - public void end(boolean interrupted) { - intake.deactivate(); - indexer.stop(); - if(outtake != null){ - outtake.stop();; - } - } -} diff --git a/src/main/java/frc/robot/commands/gpm/RunIntakeAndIndexer.java b/src/main/java/frc/robot/commands/gpm/RunIntakeAndIndexer.java deleted file mode 100644 index 4e2cd3c..0000000 --- a/src/main/java/frc/robot/commands/gpm/RunIntakeAndIndexer.java +++ /dev/null @@ -1,29 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands.gpm; - -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.subsystems.indexer.Indexer; -import frc.robot.subsystems.intake.Intake; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class RunIntakeAndIndexer extends SequentialCommandGroup { - /** Creates a new RunIntakeAndIndexer. */ - public RunIntakeAndIndexer(Intake intake, Indexer indexer ) { - // Add your commands in the addCommands() call, e.g. - // addCommands(new FooCommand(), new BarCommand()); - addCommands( - new InstantCommand(() -> intake.activate()), - new InstantCommand(() -> indexer.run()), - new WaitCommand(3), - new InstantCommand(() -> intake.deactivate()), - new InstantCommand(() -> indexer.stop()) - ); - } -} diff --git a/src/main/java/frc/robot/commands/gpm/ScoreL4.java b/src/main/java/frc/robot/commands/gpm/ScoreL4.java deleted file mode 100644 index 7b0cb59..0000000 --- a/src/main/java/frc/robot/commands/gpm/ScoreL4.java +++ /dev/null @@ -1,22 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands.gpm; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.subsystems.arm.Arm; -import frc.robot.subsystems.outtake.Outtake; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class ScoreL4 extends SequentialCommandGroup { - /** Creates a new ScoreL4. */ - public ScoreL4(Outtake outtake, Arm arm) { - addCommands( - new OuttakeCoralBasic(outtake, ()->true, ()->false) - ); - } -} - diff --git a/src/main/java/frc/robot/commands/gpm/StationIntake.java b/src/main/java/frc/robot/commands/gpm/StationIntake.java deleted file mode 100644 index 1224d33..0000000 --- a/src/main/java/frc/robot/commands/gpm/StationIntake.java +++ /dev/null @@ -1,30 +0,0 @@ -package frc.robot.commands.gpm; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.outtake.Outtake; - -public class StationIntake extends Command { - private Outtake outtake; - - public StationIntake(Outtake outtake) { - this.outtake = outtake; - addRequirements(outtake); - } - - @Override - public void initialize() { - outtake.setMotor(0.7); - } - - @Override - public boolean isFinished() { - return outtake.coralLoaded(); - } - - @Override - public void end(boolean interrupted) { - outtake.setMotor(0.02); - } - - -} diff --git a/src/main/java/frc/robot/commands/test_comm/PoseTransform.java b/src/main/java/frc/robot/commands/test_comm/PoseTransform.java deleted file mode 100644 index 94b969b..0000000 --- a/src/main/java/frc/robot/commands/test_comm/PoseTransform.java +++ /dev/null @@ -1,59 +0,0 @@ -package frc.robot.commands.test_comm; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.TestConstants; -import frc.robot.subsystems.drivetrain.Drivetrain; - -/** - * Tests the odometry of the robot by driving a certain distance and calculating the error. - */ -public class PoseTransform extends Command { - - private final Drivetrain drive; - - private double startTime; - private Pose2d finalPose; - private final Transform2d distanceToMove; - private Pose2d error; - - public PoseTransform(Drivetrain drive, Transform2d poseTransform) { - this.drive = drive; - // finalPose is position after robot moves from current position-- startPose-- by the values that are inputted-- distanceToMove - distanceToMove = poseTransform; - - addRequirements(drive); - } - - @Override - public void initialize() { - startTime = Timer.getFPGATimestamp(); - finalPose = drive.getPose().transformBy(distanceToMove); - } - - @Override - public void execute() { - drive.driveWithPID(finalPose.getX(), finalPose.getY(), finalPose.getRotation().getRadians()); - } - - @Override - public boolean isFinished() { - double errorMarginMeters = TestConstants.POSE_TRANSFORM_TRANSLATION_ERROR; - double errorMarginRadians = Units.degreesToRadians(10); - error = drive.getPose().relativeTo(finalPose); - // if robot thinks its precision is < 0.1 to the target we inputted, it will stop, so then we can see how off it is - return Math.abs(error.getX()) < errorMarginMeters && Math.abs(error.getY()) < errorMarginMeters && Math.abs(error.getRotation().getRadians()) < errorMarginRadians; - } - - @Override - public void end(boolean interrupted) { - drive.stop(); - System.out.println(Timer.getFPGATimestamp() - startTime); - System.out.println(error.getX()); - System.out.println(error.getY()); - System.out.println(error.getRotation().getRadians()); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/test_comm/PoseTransformTest.java b/src/main/java/frc/robot/commands/test_comm/PoseTransformTest.java deleted file mode 100644 index 3e1a535..0000000 --- a/src/main/java/frc/robot/commands/test_comm/PoseTransformTest.java +++ /dev/null @@ -1,67 +0,0 @@ -package frc.robot.commands.test_comm; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.TestConstants; -import frc.robot.subsystems.drivetrain.Drivetrain; - -/** - * Tests the odometry of the robot by driving a certain distance and calculating the error. - */ -public class PoseTransformTest extends Command { - - private final Drivetrain drive; - - private double startTime; - private Pose2d finalPose; - private Pose2d error; - private double x; - private double y; - private double rotation; - - public PoseTransformTest(Drivetrain drive, double x, double y, double rotation) { - this.drive = drive; - // finalPose is position after robot moves from current position-- startPose-- by the values that are inputted-- distanceToMove - this.x =x; - this.y =y; - this.rotation = rotation; - addRequirements(drive); - } - - @Override - public void initialize() { - startTime = Timer.getFPGATimestamp(); - finalPose = drive.getPose().transformBy(new Transform2d( - new Translation2d(x, y), - Rotation2d.fromDegrees(rotation) - )); - } - - @Override - public void execute() { - drive.driveWithPID(finalPose.getX(), finalPose.getY(), finalPose.getRotation().getRadians()); - } - - @Override - public boolean isFinished() { - double errorMarginMeters = TestConstants.POSE_TRANSFORM_TRANSLATION_ERROR; - double errorMarginRadians = Units.degreesToRadians(10); - error = drive.getPose().relativeTo(finalPose); - // if robot thinks its precision is < 0.1 to the target we inputted, it will stop, so then we can see how off it is - return Math.abs(error.getX()) < errorMarginMeters && Math.abs(error.getY()) < errorMarginMeters && Math.abs(error.getRotation().getRadians()) < errorMarginRadians; - } - - @Override - public void end(boolean interrupted) { - drive.stop(); - System.out.println(Timer.getFPGATimestamp() - startTime); - System.out.println(error.getX()); - System.out.println(error.getY()); - System.out.println(error.getRotation().getRadians()); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/vision/AcquireGamePiece.java b/src/main/java/frc/robot/commands/vision/AcquireGamePiece.java index ba4f07a..f9aea13 100644 --- a/src/main/java/frc/robot/commands/vision/AcquireGamePiece.java +++ b/src/main/java/frc/robot/commands/vision/AcquireGamePiece.java @@ -3,13 +3,8 @@ package frc.robot.commands.vision; import java.util.function.Supplier; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.commands.gpm.IntakeCoral; -import frc.robot.subsystems.arm.Arm; +import frc.robot.commands.DoNothing; import frc.robot.subsystems.drivetrain.Drivetrain; -import frc.robot.subsystems.elevator.Elevator; -import frc.robot.subsystems.indexer.Indexer; -import frc.robot.subsystems.intake.Intake; -import frc.robot.subsystems.outtake.Outtake; import frc.robot.util.Vision.DetectedObject; public class AcquireGamePiece extends SequentialCommandGroup { @@ -18,11 +13,9 @@ public class AcquireGamePiece extends SequentialCommandGroup { * * @param gamePiece The supplier for the game piece to intake * @param drive The drivetrain - * @param intake The intake - * @param index The indexer - * @param arm The arm */ - public AcquireGamePiece(Supplier gamePiece, Drivetrain drive, Intake intake, Indexer indexer, Elevator elevator, Outtake outtake, Arm arm){ - addCommands(new IntakeCoral(intake, indexer, elevator, outtake, arm).deadlineFor(new DriveToCoral(gamePiece, drive))); + public AcquireGamePiece(Supplier gamePiece, Drivetrain drive){ + // TODO: Replace DoNothing with next year's intake command + addCommands(new DoNothing().deadlineFor(new DriveToGamePiece(gamePiece, drive))); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/vision/AimAtGamePiece.java b/src/main/java/frc/robot/commands/vision/AimAtGamePiece.java new file mode 100644 index 0000000..f3c371c --- /dev/null +++ b/src/main/java/frc/robot/commands/vision/AimAtGamePiece.java @@ -0,0 +1,59 @@ +package frc.robot.commands.vision; + +import java.util.function.Supplier; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import frc.robot.commands.drive_comm.DefaultDriveCommand; +import frc.robot.constants.VisionConstants; +import frc.robot.controls.BaseDriverConfig; +import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.util.Vision.DetectedObject; + +public class AimAtGamePiece extends DefaultDriveCommand { + private Supplier objectSupplier; + private static int ticksSinceLastObject; + private static DetectedObject cachedObject; + + + public AimAtGamePiece(Drivetrain drive, BaseDriverConfig driver, Supplier objectSupplier){ + super(drive, driver); + this.objectSupplier = objectSupplier; + } + + @Override + public void initialize() { + cachedObject = null; + ticksSinceLastObject = 0; + super.initialize(); + } + + @Override + protected void drive(ChassisSpeeds speeds){ + if(!VisionConstants.OBJECT_DETECTION_ENABLED){ + super.drive(speeds); + return; + } + DetectedObject object = objectSupplier.get(); + + if(object == null || !object.isGamePiece()) { + if (ticksSinceLastObject <= VisionConstants.MAX_EMPTY_TICKS && cachedObject != null) { + object = cachedObject; + } else { + super.drive(speeds); + return; + } + ticksSinceLastObject++; + } else { + ticksSinceLastObject = 0; + cachedObject = object; + } + + // System.out.println("objangle " + object.getAngle()); + swerve.driveHeading( + speeds.vxMetersPerSecond, + speeds.vyMetersPerSecond, + MathUtil.angleModulus(object.getAngle()), + true); + } +} diff --git a/src/main/java/frc/robot/commands/vision/DriveToCoral.java b/src/main/java/frc/robot/commands/vision/DriveToCoral.java deleted file mode 100644 index db4c0b4..0000000 --- a/src/main/java/frc/robot/commands/vision/DriveToCoral.java +++ /dev/null @@ -1,59 +0,0 @@ -package frc.robot.commands.vision; - -import java.util.function.Supplier; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import frc.robot.commands.drive_comm.DriveToPose; -import frc.robot.subsystems.drivetrain.Drivetrain; -import frc.robot.util.Vision.DetectedObject; - -/** - * Moves toward the detected object - *

Only works with the front camera - */ -public class DriveToCoral extends DriveToPose { - private static final boolean constantUpdate = false; - - private static Pose2d tempPose; - - private Supplier objectSupplier; - - /** - * Moves toward the detected object - * @param detectedObject The supplier for the detected object to use - * @param drive The drivetrain - */ - public DriveToCoral(Supplier detectedObject, Drivetrain drive) { - super(drive, - constantUpdate - ? () -> getPose(detectedObject) - : () -> tempPose); - objectSupplier = detectedObject; - updateTarget = constantUpdate; - } - - public static Pose2d getPose(Supplier supplier){ - DetectedObject object = supplier.get(); - if(object == null) return null; - return new Pose2d(object.pose.toPose2d().getTranslation(), new Rotation2d(object.getAngle()+Math.PI/2)); - } - - @Override - public void initialize(){ - // Set the static variable so the super class has access to it - if(constantUpdate){ - tempPose = getPose(objectSupplier); - } - super.initialize(); - } - - @Override - public void execute(){ - // Set the static variable so the super class has access to it - if(constantUpdate){ - tempPose = getPose(objectSupplier); - } - super.execute(); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/vision/DriveToGamePiece.java b/src/main/java/frc/robot/commands/vision/DriveToGamePiece.java new file mode 100644 index 0000000..8974053 --- /dev/null +++ b/src/main/java/frc/robot/commands/vision/DriveToGamePiece.java @@ -0,0 +1,61 @@ +package frc.robot.commands.vision; + +import java.util.function.Supplier; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import frc.robot.commands.drive_comm.DriveToPose; +import frc.robot.constants.VisionConstants; +import frc.robot.constants.swerve.DriveConstants; +import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.util.Vision.DetectedObject; + +/** + * Moves toward the detected object + */ +public class DriveToGamePiece extends DriveToPose { + private static boolean constantUpdate = true; + private static int ticksSinceLastObject; + private static DetectedObject cachedObject; + + /** + * Moves toward the detected object + * @param detectedObject The supplier for the detected object to use + * @param drive The drivetrain + */ + public DriveToGamePiece(Supplier detectedObject, Drivetrain drive) { + super(drive, + () -> getPose(detectedObject, drive) + ); + updateTarget = constantUpdate; + } + + @Override + public void initialize() { + cachedObject = null; + ticksSinceLastObject = 0; + super.initialize(); + } + + public static Pose2d getPose(Supplier supplier, Drivetrain drive){ + DetectedObject object = supplier.get(); + if(object == null || !object.isGamePiece()) { + if (ticksSinceLastObject <= VisionConstants.MAX_EMPTY_TICKS && cachedObject != null) { + object = cachedObject; + } else { + return null; + } + ticksSinceLastObject++; + } else { + ticksSinceLastObject = 0; + cachedObject = object; + } + Rotation2d rotation = new Rotation2d(MathUtil.angleModulus(object.getAngle())); + Translation2d objectTranslation = object.pose.toPose2d().getTranslation(); + Translation2d diff = objectTranslation.minus(drive.getPose().getTranslation()); + Translation2d translation = objectTranslation.minus(diff.times(DriveConstants.ROBOT_WIDTH_WITH_BUMPERS/2/diff.getNorm())); + return new Pose2d(translation, rotation); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/vision/LogVision.java b/src/main/java/frc/robot/commands/vision/LogVision.java new file mode 100644 index 0000000..da5e65f --- /dev/null +++ b/src/main/java/frc/robot/commands/vision/LogVision.java @@ -0,0 +1,35 @@ +package frc.robot.commands.vision; + +import java.util.function.Supplier; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.util.Vision.DetectedObject; + +public class LogVision extends Command { + private Supplier objectSupplier; + public LogVision(Supplier objectSupplier){ + this.objectSupplier = objectSupplier; + } + + @Override + public void execute() { + DetectedObject object = this.objectSupplier.get(); + if (object != null) { + Logger.recordOutput("Vision/object_angle", object.getAngle()); + Logger.recordOutput("Vision/object_distance", object.getDistance()); + } + } + + @Override + public boolean runsWhenDisabled() { + return true; + } + + @Override + public boolean isFinished() { + return false; + } + +} diff --git a/src/main/java/frc/robot/constants/ArmConstants.java b/src/main/java/frc/robot/constants/ArmConstants.java deleted file mode 100644 index bc0be45..0000000 --- a/src/main/java/frc/robot/constants/ArmConstants.java +++ /dev/null @@ -1,52 +0,0 @@ -package frc.robot.constants; - -import edu.wpi.first.math.system.plant.DCMotor; - -public class ArmConstants { - // Degrees - public static final double START_ANGLE = -86.5; - public static final double MIN_ANGLE = -86.5; - public static final double MAX_ANGLE = 180; - public static final double OFFSET = 0 - START_ANGLE; - - public static final double GEAR_RATIO = 29.36; - public static final double ENCODER_GEAR_RATIO = 50.0/24.0; - public static final DCMotor MOTOR = DCMotor.getKrakenX60(1); - - public static final double MASS = 2.46; // kilograms - public static final double LENGTH = 0.138*2; // meters - public static final double MOI = 0.0261057394; // kg*m^2 - public static final double CENTER_OF_MASS_LENGTH = 0.138; // meters - - public static final double MAX_VELOCITY = 21; // rad/s - public static final double MAX_ACCELERATION = 120; // rad/s^2 - - public static final double INTAKE_SETPOINT = START_ANGLE; - public static final double STATION_INTAKE_SETPOINT = 75.5; - - public static final double TOLERANCE = 3.0; - - //Dunk L4 = 6.4 - public static final double L1_SETPOINT = 50; - - //4 in offset - // public static final double L4_SETPOINT = 11; - // Original L4: 7.5 degrees - public static final double L4_SETPOINT_RIGHT = 7.5; - public static final double L4_SETPOINT_LEFT = 6; - public static final double L2_L3_SETPOINT = 21.25; - - //touching reef - public static final double L4_SETPOINT_ALT = 4.5; - public static final double L2_L3_SETPOINT_ALT = 12.23; - - public static final double ALGAE_SETPOINT = -19.37; - public static final double ALGAE_NET_SETPOINT_1 = 85.0; - public static final double ALGAE_NET_SETPOINT_2 = 25; - public static final double ALGAE_STOW_SETPOINT = 50; - public static final double ALGAE_LOLI_SETPOINT = -19; - - public static final double PROCESSOR_SETPOINT = -65.0; - - public static final double STOW_SETPOINT = -14.0; -} diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index bc90b34..6b99d88 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -71,7 +71,7 @@ public class Constants { public static final double HEADING_SLEWRATE = 10; //Modes - public static final Mode SIM_MODE = Mode.REPLAY; + public static final Mode SIM_MODE = Mode.SIM; public static final Mode CURRENT_MODE = RobotBase.isReal() ? Mode.REAL : SIM_MODE; // Enables 3D logs of mechanisms diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java deleted file mode 100644 index be34322..0000000 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ /dev/null @@ -1,59 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.constants; - -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.util.Units; - -/** Stores constants for the elevator. */ -public class ElevatorConstants { - public static final double ANGLE = Units.degreesToRadians(3.5); // radians, from vertical - public static final DCMotor MOTOR = DCMotor.getKrakenX60(1); - public static final int NUMBER_OF_STAGES = 3; - public static final double GEARING = 8.333/NUMBER_OF_STAGES; - public static final double MIN_HEIGHT = 0.0; // meters - public static final double MAX_HEIGHT = Units.inchesToMeters(66);//Units.inchesToMeters(48); // meters - public static final double START_HEIGHT = MIN_HEIGHT; // meters - public static final double CARRIAGE_MASS = 3; // kilograms 2.49475803 - public static final double DRUM_RADIUS = Units.inchesToMeters(1.281/2); // meters - public static final double SPRING_FORCE = 0; //Newtons - - public static final double BOTTOM_LIMIT_SWITCH_HEIGHT = 0;//0.015; // meters - public static final double TOP_LIMIT_SWITCH_HEIGHT = MAX_HEIGHT; // meters - public static final double SIM_LIMIT_SWITCH_TRIGGER_DISTANCE = 0.01; // meters - - public static final double STOW_SETPOINT = 0; - public static final double INTAKE_SETPOINT = 0.036; - public static final double SAFE_SETPOINT = 0.225; - public static final double INTAKE_STOW_SETPOINT = 0.58; - - //4 inch offset - public static final double L1_SETPOINT = 0.0; - public static final double L2_SETPOINT = 0.523; - public static final double L3_SETPOINT = 0.9192; - // public static final double L4_SETPOINT = 1.675; - public static final double L4_SETPOINT = 1.675; - - //touching reef - public static final double L1_SETPOINT_ALT = 0.27; - public static final double L2_SETPOINT_ALT = 0.588; - public static final double L3_SETPOINT_ALT = 0.98-0.0254-0.01; - public static final double L4_SETPOINT_ALT = 1.675; - //Dunk L4 = 1.5 - - public static final double BOTTOM_ALGAE_SETPOINT = 0.405; - public static final double TOP_ALGAE_SETPOINT = 0.799; - - public static final double NET_SETPOINT = MAX_HEIGHT; - public static final double STATION_INTAKE_SETPOINT = 0.233; - - - public static final double CENTER_OF_MASS_HEIGHT_STOWED = Units.inchesToMeters(9.44); - public static final double CENTER_OF_MASS_HEIGHT_EXTENDED = Units.inchesToMeters(10+14.767); - - - // The x distance from the center of the robot to the outtake. - public static final double OUTTAKE_X = Units.inchesToMeters(-7.25); -} diff --git a/src/main/java/frc/robot/constants/IdConstants.java b/src/main/java/frc/robot/constants/IdConstants.java index 7302cf0..a3d0bba 100644 --- a/src/main/java/frc/robot/constants/IdConstants.java +++ b/src/main/java/frc/robot/constants/IdConstants.java @@ -1,7 +1,5 @@ package frc.robot.constants; -import edu.wpi.first.wpilibj.I2C; - public class IdConstants { // Drivetrain public static final int DRIVE_FRONT_LEFT_ID = 1; @@ -20,31 +18,4 @@ public class IdConstants { // LEDs public static final int CANDLE_ID = 1; - - // Elevator - public static final int ELEVATOR_RIGHT_MOTOR = 50; - public static final int ELEVATOR_BOTTOM_LIMIT_SWITCH = 29; - public static final int ELEVATOR_TOP_LIMIT_SWITCH = 30; - - // Indexer - public static final int INDEXER_MOTOR = 56; - public static final int INDEXER_SENSOR = 24; - - // Climb - public static final int CLIMB_MOTOR = 31; - - // Intake - public static final int INTAKE_ROLLER = 51; - public static final int INTAKE_PIVOT = 55; //55 - public static final int INTAKE_LASER_CAN = 25; - - // Outtake - public static final int OUTTAKE_MOTOR_ALPHA = 14; - public static final int OUTTAKE_MOTOR_COMP = 30; - public static final int OUTTAKE_DIO_EJECTING = 3; - public static final I2C.Port i2cPort = I2C.Port.kMXP; - - //Arm - public static final int ARM_MOTOR = 29; - public static final int ARM_ABSOLUTE_ENCODER = 5; } diff --git a/src/main/java/frc/robot/constants/IndexerConstants.java b/src/main/java/frc/robot/constants/IndexerConstants.java deleted file mode 100644 index d0fdd1b..0000000 --- a/src/main/java/frc/robot/constants/IndexerConstants.java +++ /dev/null @@ -1,16 +0,0 @@ -package frc.robot.constants; - -public class IndexerConstants { - public static final double SPEED = 1; - - public static final double MOMENT_OF_INERTIA = 0.000326; - public static final double GEAR_RATIO = 1.0; - public static final int MEASUREMENT_THRESHOLD = 15; // in millimeters - - // this stuff is for sim, all in meters - public static final double WHEEL_CIRCUMFERENCE = Math.PI * 0.1; - public static final double START_SIM_POS_AT = -1.0; // start coral here - public static final double START_SIM_SENSOR_POS_AT = -0.5; // activate the sensor here - public static final double END_SIM_SENSOR_POS_AT = 0.5; // deactivate the sensor here -} - diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java deleted file mode 100644 index 1939452..0000000 --- a/src/main/java/frc/robot/constants/IntakeConstants.java +++ /dev/null @@ -1,23 +0,0 @@ -package frc.robot.constants; - -/** - * Storage class for intake constants - */ -public class IntakeConstants { - public static final double PIVOT_GEAR_RATIO = 75; - public static final double MOMENT_OFiNERTIA = 0.600984136; - public static final double CENTER_OF_MASS_DIST = 0.265; - public static final double MASS = 6.45688739; - public static final double ARM_LENGTH = CENTER_OF_MASS_DIST*2; - public static final double DETECT_CORAL_DIST = 0.49; - public static final double INTAKE_MOTOR_POWER = 1; - public static final double INTAKE_SETPOINT = -30.41; - public static final double STOW_SETPOINT = 90; - public static final double STATION_SETPOINT = 70; - public static final double INTAKE_SAFE_POINT = 30; - - public static final double ALGAE_SETPOINT = 25; - public static final double ALGAE_INTAKE_POWER = -0.5; - public static final double ALGAE_OUTTAKE_POWER = 0.5; - -} diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index c599707..4d61900 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -8,17 +8,12 @@ import org.photonvision.PhotonPoseEstimator.PoseStrategy; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Pair; import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; -import frc.robot.constants.swerve.DriveConstants; /** * Container class for vision constants. @@ -135,6 +130,8 @@ public class VisionConstants { */ public static final double HIGHEST_AMBIGUITY = 0.01; + public static final int MAX_EMPTY_TICKS = 10; + /** * The camera poses *

@@ -177,215 +174,4 @@ public class VisionConstants { new Transform3d( new Translation3d(Units.inchesToMeters(10), 0, Units.inchesToMeters(24)), new Rotation3d(0, Units.degreesToRadians(20), 0)))); - - // Poses to potentially align to - public static final Pose2d RED_PROCESSOR_POSE = new Pose2d( - FieldConstants.APRIL_TAGS.get(2).pose.getX(), - FieldConstants.APRIL_TAGS.get(2).pose.getY() - DriveConstants.ROBOT_WIDTH_WITH_BUMPERS / 2, - new Rotation2d(Math.PI / 2)); - - public static final Pose2d BLUE_PROCESSOR_POSE = new Pose2d( - FieldConstants.APRIL_TAGS.get(15).pose.getX(), - FieldConstants.APRIL_TAGS.get(15).pose.getY() + DriveConstants.ROBOT_WIDTH_WITH_BUMPERS / 2, - new Rotation2d(-Math.PI / 2)); - - public static final Pose2d BLUE_CAGE_LEFT_POSE = new Pose2d( - FieldConstants.FIELD_LENGTH / 2, - FieldConstants.FIELD_WIDTH / 2 + Units.inchesToMeters(1019.0 / 8), - new Rotation2d()); - - public static final Pose2d BLUE_CAGE_MIDDLE_POSE = new Pose2d( - FieldConstants.FIELD_LENGTH / 2, - FieldConstants.FIELD_WIDTH / 2 + Units.inchesToMeters(675.0 / 8), - new Rotation2d()); - - public static final Pose2d BLUE_CAGE_RIGHT_POSE = new Pose2d( - FieldConstants.FIELD_LENGTH / 2, - FieldConstants.FIELD_WIDTH / 2 + Units.inchesToMeters(41.5), - new Rotation2d()); - - public static final Pose2d RED_CAGE_RIGHT_POSE = new Pose2d( - FieldConstants.FIELD_LENGTH / 2, - FieldConstants.FIELD_WIDTH / 2 + Units.inchesToMeters(-41.5), - new Rotation2d(Math.PI)); - - public static final Pose2d RED_CAGE_MIDDLE_POSE = new Pose2d( - FieldConstants.FIELD_LENGTH / 2, - FieldConstants.FIELD_WIDTH / 2 - Units.inchesToMeters(-675.0 / 8), - new Rotation2d(Math.PI)); - - public static final Pose2d RED_CAGE_LEFT_POSE = new Pose2d( - FieldConstants.FIELD_LENGTH / 2, - FieldConstants.FIELD_WIDTH / 2 - Units.inchesToMeters(-1019.0 / 8), - new Rotation2d(Math.PI)); - - /** - * The distance between the AprilTag and algae setpoint in the directin parallel to the face of the reef - * Positive is to the left - */ - public static final double ALGAE_X_OFFSET = 0.02; - - /** - * Stores all of the alignment poses for both reefs. - * Each branch is in the format (RED or BLUE)_BRANCH_(tag ID)_(LEFT or RIGHT) - * Each algae setpoint is in the form (RED or BLUE)_ALGAE_(tag ID) - * Left and right refer to the position of the branch when looking directly at the AprilTag - */ - public enum REEF { - RED_BRANCH_6_LEFT(5, 0.1651, .0587), - RED_BRANCH_6_RIGHT(5, -0.1651, .0587), - RED_BRANCH_7_LEFT(6, 0.1651, .0587), - RED_BRANCH_7_RIGHT(6, -0.1651, .0587), - RED_BRANCH_8_LEFT(7, 0.1651, .0587), - RED_BRANCH_8_RIGHT(7, -0.1651, .0587), - RED_BRANCH_9_LEFT(8, 0.1651, .0587), - RED_BRANCH_9_RIGHT(8, -0.1651, .0587), - RED_BRANCH_10_LEFT(9, 0.1651, .0587), - RED_BRANCH_10_RIGHT(9, -0.1651, .0587), - RED_BRANCH_11_LEFT(10, 0.1651, .0587), - RED_BRANCH_11_RIGHT(10, -0.1651, .0587), - - BLUE_BRANCH_17_LEFT(16, 0.1651, .0587), - BLUE_BRANCH_17_RIGHT(16, -0.1651, .0587), - BLUE_BRANCH_18_LEFT(17, 0.1651, .0587), - BLUE_BRANCH_18_RIGHT(17, -0.1651, .0587), - BLUE_BRANCH_19_LEFT(18, 0.1651, .0587), - BLUE_BRANCH_19_RIGHT(18, -0.1651, .0587), - BLUE_BRANCH_20_LEFT(19, 0.1651, .0587), - BLUE_BRANCH_20_RIGHT(19, -0.1651, .0587), - BLUE_BRANCH_21_LEFT(20, 0.1651, .0587), - BLUE_BRANCH_21_RIGHT(20, -0.1651, .0587), - BLUE_BRANCH_22_LEFT(21, 0.1651, .0587), - BLUE_BRANCH_22_RIGHT(21, -0.1651, .0587), - - RED_ALGAE_6(5, ALGAE_X_OFFSET, 0, true), - RED_ALGAE_7(6, ALGAE_X_OFFSET, 0, true), - RED_ALGAE_8(7, ALGAE_X_OFFSET, 0, true), - RED_ALGAE_9(8, ALGAE_X_OFFSET, 0, true), - RED_ALGAE_10(9, ALGAE_X_OFFSET, 0, true), - RED_ALGAE_11(10, ALGAE_X_OFFSET, 0, true), - BLUE_ALGAE_17(16, ALGAE_X_OFFSET, 0, true), - BLUE_ALGAE_18(17, ALGAE_X_OFFSET, 0, true), - BLUE_ALGAE_19(18, ALGAE_X_OFFSET, 0, true), - BLUE_ALGAE_20(19, ALGAE_X_OFFSET, 0, true), - BLUE_ALGAE_21(20, ALGAE_X_OFFSET, 0, true), - BLUE_ALGAE_22(21, ALGAE_X_OFFSET, 0, true); - - /** - * The pose to align to for scoring on this branch or intaking this algae - */ - public final Pose2d pose; - /** - * The pose to align to for scoring on L4, null for algae - */ - public final Pose2d l4Pose; - /** - * The pose to align to for scoring on L1, null for algae - */ - public final Pose2d l1Pose; - /** - * The ID of the AprilTag on the smae side of hte reef ast his branch - */ - public final int aprilTagId; - private final int aprilTagIndex; - /** - * The horizontal (parallel to the closest face of the reef) distance, in meters, between the AprilTag and branch - */ - public final double xOffset; - /** - * The y (normal to the closest face of the reef) distance, in meters, between the AprilTag and branch - */ - public final double yOffset; - - public final boolean isAlgae; - - private REEF(int aprilTagIndex, double xOffset, double yOffset) { - this(aprilTagIndex, xOffset, yOffset, false); - } - private REEF(int aprilTagIndex, double xOffset, double yOffset, boolean isAlgae) { - this.aprilTagIndex = aprilTagIndex; - aprilTagId = aprilTagIndex+1; - this.xOffset = xOffset; - this.yOffset = yOffset; - this.isAlgae = isAlgae; - pose = getPose(); - if(isAlgae){ - l4Pose = null; - l1Pose = null; - }else{ - l4Pose = pose.transformBy(new Transform2d(0, Units.inchesToMeters(2), new Rotation2d())); - l1Pose = getL1Pose(); - } - } - - /** - * Calculates the Pose2d to align to the branch based on the AprilTag's pose and - * offsets. - * - * @return The calculated Pose2d for this reef branch. - */ - private Pose2d getPose() { - Pose3d basePose3d = FieldConstants.APRIL_TAGS.get(aprilTagIndex).pose; - double adjustedYOffset = DriveConstants.ROBOT_WIDTH_WITH_BUMPERS / 2.0 + (isAlgae ? 0 : Units.inchesToMeters(4.5)); - - // Apply both X and Y offsets to calculate the reef branch pose - Transform3d transform = new Transform3d(adjustedYOffset, -xOffset, 0, new Rotation3d(0, 0, 0)); - - Pose3d branchPose3d = basePose3d.plus(transform); - - // Convert the calculated branch Pose3d to Pose2d - return branchPose3d.toPose2d().transformBy(new Transform2d(0, 0, new Rotation2d(Math.PI/2))); - } - /** - * Calculates the Pose2d to align to for scoring on L1 near this branch based on the AprilTag's pose and - * offsets. - * - * @return The calculated Pose2d for this reef branch. - */ - private Pose2d getL1Pose() { - Pose3d basePose3d = FieldConstants.APRIL_TAGS.get(aprilTagIndex).pose; - double adjustedYOffset = DriveConstants.ROBOT_WIDTH_WITH_BUMPERS / 2.0; - - // Apply both X and Y offsets to calculate the reef branch pose - Transform3d transform = new Transform3d(adjustedYOffset, -Math.signum(xOffset)*Units.inchesToMeters(37.04/2-1), 0, new Rotation3d(0, 0, 0)); - - Pose3d branchPose3d = basePose3d.plus(transform); - - // Convert the calculated branch Pose3d to Pose2d - return branchPose3d.toPose2d().transformBy(new Transform2d(0, 0, new Rotation2d(Math.PI/2))); - } - - /** - * Finds the appropriate reef branch based on the AprilTag ID and whether the - * pose is left or right. - * - * @param aprilTagId The ID of the AprilTag. - * @param isLeftPose True if the pose is left, false if it's right. - * @return The matching REEF, or null if no match is found. - */ - public static REEF fromAprilTagIdAndPose(int aprilTagId, boolean isLeftPose) { - for (REEF branch : values()) { - if (!branch.isAlgae && branch.aprilTagId == aprilTagId && - ((isLeftPose && branch.xOffset > 0) || (!isLeftPose && branch.xOffset < 0))) { - return branch; - } - } - return null; - } - - /** - * Finds the appropriate reef algae position based on the AprilTag ID - * - * @param aprilTagId The ID of the AprilTag. - * @return The matching REEF, or null if no match is found. - */ - public static REEF fromAprilTagIdAlgae(int aprilTagId) { - for (REEF branch : values()) { - if (branch.isAlgae && branch.aprilTagId == aprilTagId) { - return branch; - } - } - return null; - } - } } \ No newline at end of file diff --git a/src/main/java/frc/robot/controls/GameControllerDriverConfig.java b/src/main/java/frc/robot/controls/GameControllerDriverConfig.java index 3cf0260..e21c1eb 100644 --- a/src/main/java/frc/robot/controls/GameControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/GameControllerDriverConfig.java @@ -2,283 +2,42 @@ package frc.robot.controls; import java.util.function.BooleanSupplier; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.StartEndCommand; -import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Robot; -import frc.robot.commands.drive_comm.DriveToPose; -import frc.robot.commands.gpm.IntakeAlgae; -import frc.robot.commands.gpm.IntakeAlgaeArm; -import frc.robot.commands.gpm.IntakeCoral; -import frc.robot.commands.gpm.MoveArm; -import frc.robot.commands.gpm.MoveElevator; -import frc.robot.commands.gpm.NetSetpoint; -import frc.robot.commands.gpm.OuttakeAlgae; -import frc.robot.commands.gpm.OuttakeCoral; -import frc.robot.commands.gpm.ResetClimb; -import frc.robot.commands.gpm.ReverseMotors; -import frc.robot.commands.gpm.StationIntake; -import frc.robot.constants.ArmConstants; import frc.robot.constants.Constants; -import frc.robot.constants.ElevatorConstants; -import frc.robot.constants.FieldConstants; -import frc.robot.constants.VisionConstants; -import frc.robot.subsystems.arm.Arm; -import frc.robot.subsystems.climb.Climb; import frc.robot.subsystems.drivetrain.Drivetrain; -import frc.robot.subsystems.elevator.Elevator; -import frc.robot.subsystems.indexer.Indexer; -import frc.robot.subsystems.intake.Intake; -import frc.robot.subsystems.outtake.Outtake; import lib.controllers.GameController; import lib.controllers.GameController.Axis; import lib.controllers.GameController.Button; -import lib.controllers.GameController.DPad; /** * Driver controls for the generic game controller. */ public class GameControllerDriverConfig extends BaseDriverConfig { - public static final boolean singleAlignmentButton = true; - private final GameController driver = new GameController(Constants.DRIVER_JOY); private final BooleanSupplier slowModeSupplier = driver.get(Button.RIGHT_JOY); - private final Elevator elevator; - private final Intake intake; - private final Indexer indexer; - private final Outtake outtake; - private final Climb climb; - private final Arm arm; - private int alignmentDirection = 0; - private Pose2d alignmentPose = null; - public GameControllerDriverConfig(Drivetrain drive, Elevator elevator, Intake intake, Indexer indexer, - Outtake outtake, Climb climb, Arm arm) { + public GameControllerDriverConfig(Drivetrain drive) { super(drive); - this.elevator = elevator; - this.intake = intake; - this.indexer = indexer; - this.outtake = outtake; - this.climb = climb; - this.arm = arm; } @Override public void configureControls() { - Trigger menu = driver.get(Button.LEFT_JOY); - - // Elevator setpoints - if (elevator != null && arm != null) { - driver.get(Button.BACK).and(menu.negate()).onTrue( - new SequentialCommandGroup( - new MoveElevator(elevator, ElevatorConstants.L1_SETPOINT), - new MoveArm(arm, ArmConstants.L1_SETPOINT))); - - driver.get(driver.LEFT_TRIGGER_BUTTON).onTrue( - new SequentialCommandGroup( - new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT), - new MoveArm(arm, ArmConstants.L4_SETPOINT_RIGHT))); - - Command l2Coral = new SequentialCommandGroup( - new MoveElevator(elevator, ElevatorConstants.L2_SETPOINT), - new MoveArm(arm, ArmConstants.L2_L3_SETPOINT)); - Command l3Coral = new SequentialCommandGroup( - new MoveElevator(elevator, ElevatorConstants.L3_SETPOINT), - new MoveArm(arm, ArmConstants.L2_L3_SETPOINT)); - Command l2Algae = new ParallelCommandGroup( - new MoveElevator(elevator, ElevatorConstants.BOTTOM_ALGAE_SETPOINT), - new MoveArm(arm, ArmConstants.ALGAE_SETPOINT)).andThen(new IntakeAlgaeArm(outtake)); - Command l3Algae = new ParallelCommandGroup( - new MoveElevator(elevator, ElevatorConstants.TOP_ALGAE_SETPOINT), - new MoveArm(arm, ArmConstants.ALGAE_SETPOINT)).andThen(new IntakeAlgaeArm(outtake)); - driver.get(Button.RB).whileTrue(new ConditionalCommand(l2Algae, new InstantCommand(l2Coral::schedule), menu)); - driver.get(Button.LB).whileTrue(new ConditionalCommand(l3Algae, new InstantCommand(l3Coral::schedule), menu)); - - // Processor setpoint - driver.get(Button.Y).and(menu.negate()).onTrue( - new ParallelCommandGroup( - new MoveElevator(elevator, ElevatorConstants.SAFE_SETPOINT + 0.001), - new MoveArm(arm, ArmConstants.PROCESSOR_SETPOINT))); - driver.get(DPad.UP).onTrue(new NetSetpoint(elevator, arm, getDrivetrain())); - } - // Intake/outtake - Trigger r3 = driver.get(Button.RIGHT_JOY); - - if (intake != null && indexer != null && elevator != null && outtake != null && arm != null) {// && elevator != null){ - boolean toggle = true; - Command intakeCoral = new IntakeCoral(intake, indexer, elevator, outtake, arm); - Command intakeAlgae = new IntakeAlgae(intake); - driver.get(Button.A).onTrue(new InstantCommand(() -> { - if (r3.getAsBoolean()) - return; - if (menu.getAsBoolean()) { - intakeAlgae.schedule(); - } else { - if (toggle) { - if (intakeCoral.isScheduled()) { - intakeCoral.cancel(); - } else { - intakeCoral.schedule(); - } - } else { - intakeCoral.schedule(); - } - } - })).onFalse(new InstantCommand(() -> { - if (!toggle) { - intakeCoral.cancel(); - } - intakeAlgae.cancel(); - })); - // On true, run the command to start intaking - // On false, run the command to finish intaking if it has a coral - Command startIntake = new StationIntake(outtake); - // Command finishIntake = new FinishStationIntake(intake, indexer, elevator, - // outtake); - driver.get(Button.A).and(r3).and(menu.negate()).onTrue(startIntake) - .onFalse(new InstantCommand(() -> { - if (!startIntake.isScheduled()) { - // finishIntake.schedule(); - } else { - startIntake.cancel(); - } - })); - } - - if (intake != null && outtake != null && arm != null && elevator != null) { - driver.get(DPad.DOWN).and(menu).onTrue(new SequentialCommandGroup( - new OuttakeAlgae(outtake, intake), - new InstantCommand(() -> { - arm.setSetpoint(ArmConstants.START_ANGLE); - elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT); - getDrivetrain().setIsAlign(false); - }, elevator))); - } - - if (outtake != null && elevator != null && arm != null) { - driver.get(DPad.DOWN).and(menu.negate()) - .onTrue(new OuttakeCoral(outtake, elevator, arm) - .alongWith(new InstantCommand(() -> getDrivetrain().setDesiredPose(() -> null))) - .andThen(new SequentialCommandGroup(new MoveArm(arm, ArmConstants.INTAKE_SETPOINT), - new MoveElevator(elevator, ElevatorConstants.STOW_SETPOINT)))); - } - driver.get(DPad.DOWN).and(menu.negate()).onTrue(new InstantCommand(() -> { - }, getDrivetrain())); - if (intake != null && indexer != null && outtake != null) { - driver.get(Button.B).and(menu.negate()).whileTrue(new ReverseMotors(intake, indexer, outtake)); - } - - // Climb - if (climb != null) { - driver.get(Button.X).and(menu.negate()) - .toggleOnTrue(new StartEndCommand(() -> climb.extend(), () -> climb.climb(), climb)); - if (intake != null) { - driver.get(Button.X).and(menu.negate()).onTrue(new InstantCommand(() -> intake.setAngle(65), intake)); - } - driver.get(Button.X).and(menu).whileTrue(new ResetClimb(climb)); - driver.get(driver.RIGHT_TRIGGER_BUTTON).and(menu).onTrue(new InstantCommand(() -> climb.stow(), climb)); - } - - // Alignment - if (singleAlignmentButton) { - driver.get(DPad.LEFT).toggleOnTrue(new InstantCommand(() -> { - setAlignmentDirection(); - setAlignmentPose(false, true); - }).andThen(new DriveToPose(getDrivetrain(), () -> alignmentPose))); - driver.get(DPad.RIGHT).toggleOnTrue(new InstantCommand(() -> { - setAlignmentDirection(); - setAlignmentPose(false, false); - }).andThen(new DriveToPose(getDrivetrain(), () -> alignmentPose))); - } else { - driver.get(DPad.LEFT).onTrue(new InstantCommand(() -> setAlignmentPose(false, true)) - .andThen(new DriveToPose(getDrivetrain(), () -> alignmentPose))); - driver.get(DPad.RIGHT).onTrue(new InstantCommand(() -> setAlignmentPose(false, false)) - .andThen(new DriveToPose(getDrivetrain(), () -> alignmentPose))); - } - // Reset yaw to be away from driver driver.get(Button.START).onTrue(new InstantCommand(() -> super.getDrivetrain().setYaw( new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI)))); // Cancel commands - driver.get(driver.RIGHT_TRIGGER_BUTTON).and(menu.negate()).onTrue(new InstantCommand(() -> { - if (elevator != null) { - if (outtake != null && outtake.coralLoaded()) { - elevator.setSetpoint(ElevatorConstants.INTAKE_STOW_SETPOINT); - } else { - elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT); - } - } - if (outtake != null) { - outtake.stop(); - } - if (intake != null) { - intake.stow(); - intake.deactivate(); - } - if (indexer != null) { - indexer.stop(); - } - if (climb != null) { - climb.stow(); - } - if (arm != null) { - if (outtake != null && outtake.coralLoaded()) { - arm.setSetpoint(ArmConstants.STOW_SETPOINT); - } else { - arm.setSetpoint(ArmConstants.START_ANGLE); - } - } + driver.get(driver.RIGHT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> { getDrivetrain().setIsAlign(false); getDrivetrain().setDesiredPose(() -> null); CommandScheduler.getInstance().cancelAll(); })); } - private void setAlignmentDirection() { - Translation2d drivePose = getDrivetrain().getPose().getTranslation(); - int closestDirection = 0; - double closestDist = 20; - boolean isRed = Robot.getAlliance() == Alliance.Red; - int start = isRed ? 5 : 16; - for (int i = 0; i < 6; i++) { - double dist = FieldConstants.APRIL_TAGS.get(start + i).pose.toPose2d().getTranslation().getDistance(drivePose); - if (dist < closestDist) { - closestDist = dist; - closestDirection = i; - } - } - if (isRed) { - closestDirection = (8 - closestDirection) % 6; - } - alignmentDirection = closestDirection; - } - - /** - * Sets the drivetrain's alignment pose to the selected position - * - * @param isAlgae True for algae, false for branches - * @param isLeft True for left branch, false for right, ignored for algae - */ - private void setAlignmentPose(boolean isAlgae, boolean isLeft) { - int id = Robot.getAlliance() == Alliance.Blue ? alignmentDirection + 17 - : (8 - alignmentDirection) % 6 + 6; - if (isAlgae) { - alignmentPose = VisionConstants.REEF.fromAprilTagIdAlgae(id).pose; - } else { - alignmentPose = VisionConstants.REEF.fromAprilTagIdAndPose(id, isLeft).pose; - } - } - @Override public double getRawForwardTranslation() { return driver.get(Axis.LEFT_Y); diff --git a/src/main/java/frc/robot/controls/Operator.java b/src/main/java/frc/robot/controls/Operator.java index 31ae3d1..2827c05 100644 --- a/src/main/java/frc/robot/controls/Operator.java +++ b/src/main/java/frc/robot/controls/Operator.java @@ -4,36 +4,12 @@ package frc.robot.controls; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Robot; -import frc.robot.commands.DoNothing; -import frc.robot.commands.gpm.IntakeAlgae; -import frc.robot.commands.gpm.IntakeCoral; -import frc.robot.commands.gpm.MoveElevator; -import frc.robot.commands.gpm.OuttakeAlgaeIntake; -import frc.robot.commands.gpm.OuttakeCoral; -import frc.robot.commands.gpm.RemoveAlgae; -import frc.robot.commands.gpm.ReverseMotors; -import frc.robot.commands.gpm.StationIntake; import frc.robot.constants.Constants; -import frc.robot.constants.ElevatorConstants; -import frc.robot.constants.IntakeConstants; -import frc.robot.constants.VisionConstants; -import frc.robot.subsystems.arm.Arm; -import frc.robot.subsystems.climb.Climb; import frc.robot.subsystems.drivetrain.Drivetrain; -import frc.robot.subsystems.elevator.Elevator; -import frc.robot.subsystems.indexer.Indexer; -import frc.robot.subsystems.intake.Intake; -import frc.robot.subsystems.outtake.Outtake; import lib.controllers.GameController; -import lib.controllers.GameController.Button; -import lib.controllers.GameController.DPad; /** * Controls for the operator, which are almost a duplicate of most of the driver's controls @@ -43,117 +19,20 @@ public class Operator { private final GameController driver = new GameController(Constants.OPERATOR_JOY); private final Drivetrain drive; - private final Elevator elevator; - private final Intake intake; - private final Indexer indexer; - private final Outtake outtake; - private final Climb climb; - private final Arm arm; - private int alignmentDirection = 0; - - public Operator(Drivetrain drive, Elevator elevator, Intake intake, Indexer indexer, Outtake outtake, Climb climb, Arm arm) { + + public Operator(Drivetrain drive) { this.drive = drive; - this.elevator = elevator; - this.intake = intake; - this.indexer = indexer; - this.outtake = outtake; - this.climb = climb; - this.arm = arm; } public void configureControls() { - // TODO: Update to match new driver controls, except the ones that move the drivetrain - Trigger menu = driver.get(Button.LEFT_JOY); - - // Elevator setpoints - if(elevator != null && outtake != null) { - driver.get(Button.BACK).onTrue(new MoveElevator(elevator, ElevatorConstants.L1_SETPOINT).deadlineFor(new RemoveAlgae(outtake))); - driver.get(Button.LB).onTrue(new MoveElevator(elevator, ElevatorConstants.L2_SETPOINT).deadlineFor(new RemoveAlgae(outtake))); - driver.get(Button.RB).and(menu.negate()).onTrue(new MoveElevator(elevator, ElevatorConstants.L3_SETPOINT).deadlineFor(new RemoveAlgae(outtake))); - driver.get(driver.LEFT_TRIGGER_BUTTON).onTrue(new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT).deadlineFor(new RemoveAlgae(outtake))); - driver.get(Button.Y).and(menu.negate()).onTrue(new MoveElevator(elevator, ElevatorConstants.STOW_SETPOINT)); - } - - // Intake/outtake - Trigger r3 = driver.get(Button.RIGHT_JOY); - if(intake != null && indexer != null){// && elevator != null){ - driver.get(Button.A).and(menu.negate()).and(r3.negate()).whileTrue(new IntakeCoral(intake, indexer, elevator, outtake, arm)); - // On true, run the command to start intaking - // On false, run the command to finish intaking if it has a coral - Command startIntake = new StationIntake(outtake); - Command finishIntake = new DoNothing(); - driver.get(Button.A).and(r3).and(menu.negate()).onTrue(startIntake) - .onFalse(new InstantCommand(()->{ - if(!startIntake.isScheduled()){ - finishIntake.schedule(); - }else{ - startIntake.cancel(); - } - })); - } - if(intake != null){ - driver.get(Button.A).and(menu).whileTrue(new IntakeAlgae(intake)); - driver.get(DPad.DOWN).and(menu).onTrue(new OuttakeAlgaeIntake(intake)); - } - if(outtake != null && elevator != null){ - driver.get(DPad.DOWN).and(menu.negate()).onTrue(new OuttakeCoral(outtake, elevator, arm)); - } - if(intake != null && indexer != null){ - driver.get(Button.B).and(menu.negate()).whileTrue(new ReverseMotors(intake, indexer, outtake)); - } - - // Climb - if(climb != null){ - driver.get(Button.X).and(menu.negate()).onTrue(new InstantCommand(()->climb.extend(), climb)) - .onFalse(new InstantCommand(()->climb.climb(), climb)); - if(intake != null){ - driver.get(Button.X).and(menu.negate()).onTrue(new InstantCommand(()->intake.setAngle(IntakeConstants.ALGAE_SETPOINT), intake)); - } - } - - // Alignment - driver.get(Button.B).and(menu).onTrue(new InstantCommand(()->alignmentDirection = 0)); - driver.get(Button.Y).and(menu).onTrue(new InstantCommand(()->alignmentDirection = 2)); - driver.get(Button.X).and(menu).onTrue(new InstantCommand(()->alignmentDirection = 3)); - driver.get(Button.RB).onTrue(new InstantCommand(()->alignmentDirection = 4)); - driver.get(DPad.UP).onTrue(new InstantCommand(()->alignmentDirection = 5)); - driver.get(DPad.LEFT).onTrue(new InstantCommand(()->setAlignmentPose(true))); - driver.get(DPad.RIGHT).onTrue(new InstantCommand(()->setAlignmentPose(false))); - - // Cancel commands - driver.get(Button.START).onTrue(new InstantCommand(()->{ - if(elevator != null){ - elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT); - } - if(outtake != null){ - outtake.stop(); - } - if(intake != null){ - intake.stow(); - intake.deactivate(); - } - if(indexer != null){ - indexer.stop(); - } - if(climb != null){ - climb.stow(); - } - drive.setDesiredPose(()->null); + // Cancel commands, could be removed if the operator doesn't need this button + driver.get(driver.RIGHT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> { + drive.setIsAlign(false); + drive.setDesiredPose(() -> null); CommandScheduler.getInstance().cancelAll(); })); } - /** - * Sets the drivetrain's alignmetn pose to the selected position - * @param isLeft True for left branch, false for right - */ - private void setAlignmentPose(boolean isLeft){ - Pose2d pose = VisionConstants.REEF.fromAprilTagIdAndPose( - Robot.getAlliance() == Alliance.Blue ? alignmentDirection + 17 - : (8-alignmentDirection) % 6 + 6, - isLeft).pose; - drive.setDesiredPose(pose); - } public Trigger getRightTrigger(){ return new Trigger(driver.RIGHT_TRIGGER_BUTTON); diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index eaea0f9..6ef2c09 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -2,48 +2,15 @@ package frc.robot.controls; import java.util.function.BooleanSupplier; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.StartEndCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Robot; -import frc.robot.commands.DoNothing; -import frc.robot.commands.drive_comm.DriveToPose; -import frc.robot.commands.gpm.IntakeAlgae; -import frc.robot.commands.gpm.IntakeAlgaeArm; -import frc.robot.commands.gpm.IntakeCoral; -import frc.robot.commands.gpm.MoveArm; -import frc.robot.commands.gpm.MoveElevator; -import frc.robot.commands.gpm.OuttakeAlgae; -import frc.robot.commands.gpm.NetSetpoint; -import frc.robot.commands.gpm.OuttakeCoral; -import frc.robot.commands.gpm.ResetClimb; -import frc.robot.commands.gpm.ReverseMotors; -import frc.robot.commands.gpm.StationIntake; -import frc.robot.constants.ArmConstants; import frc.robot.constants.Constants; -import frc.robot.constants.ElevatorConstants; -import frc.robot.constants.FieldConstants; -import frc.robot.constants.VisionConstants; -import frc.robot.subsystems.arm.Arm; -import frc.robot.subsystems.climb.Climb; import frc.robot.subsystems.drivetrain.Drivetrain; -import frc.robot.subsystems.elevator.Elevator; -import frc.robot.subsystems.indexer.Indexer; -import frc.robot.subsystems.intake.Intake; -import frc.robot.subsystems.outtake.Outtake; import lib.controllers.PS5Controller; -import lib.controllers.PS5Controller.DPad; import lib.controllers.PS5Controller.PS5Axis; import lib.controllers.PS5Controller.PS5Button; @@ -51,395 +18,33 @@ import lib.controllers.PS5Controller.PS5Button; * Driver controls for the PS5 controller */ public class PS5ControllerDriverConfig extends BaseDriverConfig { - private final boolean autoOuttake = true; - private final PS5Controller driver = new PS5Controller(Constants.DRIVER_JOY); - private final Elevator elevator; - private final Intake intake; - private final Indexer indexer; - private final Outtake outtake; - private final Climb climb; - private final Arm arm; private final BooleanSupplier slowModeSupplier = ()->false; - private Pose2d alignmentPose = null; - // 0 == not selected, -1 == left, 1 == right - private byte selectedDirection = 0; - public PS5ControllerDriverConfig(Drivetrain drive, Elevator elevator, Intake intake, Indexer indexer, Outtake outtake, Climb climb, Arm arm) { + public PS5ControllerDriverConfig(Drivetrain drive) { super(drive); - this.elevator = elevator; - this.intake = intake; - this.indexer = indexer; - this.outtake = outtake; - this.climb = climb; - this.arm = arm; } public void configureControls() { - Trigger menu = driver.get(PS5Button.LEFT_JOY); - - // Elevator setpoints - if(elevator != null && arm != null && outtake != null) { - driver.get(PS5Button.OPTIONS).and(menu.negate()).onTrue( - new SequentialCommandGroup( - new InstantCommand(()->setAlignmentPose(false, true)), - new ConditionalCommand( - new ParallelCommandGroup( - new MoveElevator(elevator, ElevatorConstants.L1_SETPOINT), - new MoveArm(arm, ArmConstants.L1_SETPOINT), - new DriveToPose(getDrivetrain(), ()->alignmentPose) - ), - // This is instant so it doesn't requre the drivetrain for more than 1 frame - new InstantCommand(()->{ - elevator.setSetpoint(ElevatorConstants.L1_SETPOINT); - arm.setSetpoint(ArmConstants.L1_SETPOINT); - }), - ()->selectedDirection != 0 - ), - new ConditionalCommand( - new SequentialCommandGroup( - new OuttakeCoral(outtake, elevator, arm), - new MoveElevator(elevator, ElevatorConstants.L2_SETPOINT), - new InstantCommand(()->{ - elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT); - arm.setSetpoint(ArmConstants.INTAKE_SETPOINT); - alignmentPose = null; - selectedDirection = 0; - }, elevator, arm) - ), - new DoNothing(), - () -> selectedDirection != 0 && autoOuttake - ) - ) - ); - - driver.get(PS5Button.LEFT_TRIGGER).onTrue( - new SequentialCommandGroup( - new InstantCommand(()->setAlignmentPose(true)), - new ConditionalCommand( - new ParallelCommandGroup( - new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT), - new ConditionalCommand( - new MoveArm(arm, ArmConstants.L4_SETPOINT_RIGHT), - new MoveArm(arm, ArmConstants.L4_SETPOINT_LEFT), - () -> selectedDirection >= 0 - ), - new DriveToPose(getDrivetrain(), ()->alignmentPose) - ), - // This is instant so it doesn't requre the drivetrain for more than 1 frame - new InstantCommand(()->{ - elevator.setSetpoint(ElevatorConstants.L4_SETPOINT); - arm.setSetpoint(ArmConstants.L4_SETPOINT_RIGHT); - }), - ()->selectedDirection != 0 - ), - new ConditionalCommand( - new WaitCommand(0.5), - new DoNothing(), - () -> selectedDirection < 0 - ), - new ConditionalCommand( - new OuttakeCoral(outtake, elevator, arm) - .andThen(new InstantCommand(()->{ - elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT); - arm.setSetpoint(ArmConstants.INTAKE_SETPOINT); - alignmentPose = null; - selectedDirection = 0; - }, elevator, arm)), - new DoNothing(), - () -> selectedDirection != 0 && autoOuttake - ) - ) - ); - - Command l2Coral = new SequentialCommandGroup( - new SequentialCommandGroup( - new InstantCommand(()->setAlignmentPose(false)), - new ConditionalCommand( - new ParallelCommandGroup( - new MoveElevator(elevator, ElevatorConstants.L2_SETPOINT), - new MoveArm(arm, ArmConstants.L2_L3_SETPOINT), - new DriveToPose(getDrivetrain(), ()->alignmentPose) - ), - // This is instant so it doesn't requre the drivetrain for more than 1 frame - new InstantCommand(()->{ - elevator.setSetpoint(ElevatorConstants.L2_SETPOINT); - arm.setSetpoint(ArmConstants.L2_L3_SETPOINT); - }), - ()->selectedDirection != 0 - ), - new ConditionalCommand( - new OuttakeCoral(outtake, elevator, arm) - .andThen(new InstantCommand(()->{ - elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT); - arm.setSetpoint(ArmConstants.INTAKE_SETPOINT); - alignmentPose = null; - selectedDirection = 0; - }, elevator, arm)), - new DoNothing(), - () -> selectedDirection != 0 && autoOuttake - ) - ) - ); - Command l3Coral = new SequentialCommandGroup( - new SequentialCommandGroup( - new InstantCommand(()->setAlignmentPose(false)), - new ConditionalCommand( - new ParallelCommandGroup( - new MoveElevator(elevator, ElevatorConstants.L3_SETPOINT), - new MoveArm(arm, ArmConstants.L2_L3_SETPOINT), - new DriveToPose(getDrivetrain(), ()->alignmentPose) - ), - // This is instant so it doesn't requre the drivetrain for more than 1 frame - new InstantCommand(()->{ - elevator.setSetpoint(ElevatorConstants.L3_SETPOINT); - arm.setSetpoint(ArmConstants.L2_L3_SETPOINT); - }), - ()->selectedDirection != 0 - ), - new ConditionalCommand( - new OuttakeCoral(outtake, elevator, arm) - .andThen(new InstantCommand(()->{ - elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT); - arm.setSetpoint(ArmConstants.INTAKE_SETPOINT); - alignmentPose = null; - selectedDirection = 0; - }, elevator, arm)), - new DoNothing(), - () -> selectedDirection != 0 && autoOuttake - ) - ) - ); - Command l2Algae = new ParallelCommandGroup( - new MoveElevator(elevator, ElevatorConstants.BOTTOM_ALGAE_SETPOINT), - new MoveArm(arm, ArmConstants.ALGAE_SETPOINT)).andThen(new IntakeAlgaeArm(outtake)); - Command l3Algae = new ParallelCommandGroup( - new MoveElevator(elevator, ElevatorConstants.TOP_ALGAE_SETPOINT), - new MoveArm(arm, ArmConstants.ALGAE_SETPOINT)).andThen(new IntakeAlgaeArm(outtake)); - driver.get(PS5Button.RB).whileTrue(new ConditionalCommand(l2Algae, new InstantCommand(l2Coral::schedule), menu)); - driver.get(PS5Button.LB).whileTrue(new ConditionalCommand(l3Algae, new InstantCommand(l3Coral::schedule), menu)); - - //Processor setpoint - driver.get(PS5Button.TRIANGLE).and(menu.negate()).onTrue( - new ParallelCommandGroup( - new MoveElevator(elevator, ElevatorConstants.SAFE_SETPOINT + 0.001), - new MoveArm(arm, ArmConstants.PROCESSOR_SETPOINT) - ) - ); - driver.get(DPad.UP).onTrue(new NetSetpoint(elevator, arm, getDrivetrain())); - } - - // Intake/outtake - Trigger r3 = driver.get(PS5Button.RIGHT_JOY); - - if(intake != null && indexer != null && elevator != null && outtake != null && arm != null){ - boolean toggle = true; - Command intakeCoral = new IntakeCoral(intake, indexer, elevator, outtake, arm); - Command intakeAlgae = new IntakeAlgae(intake); - driver.get(PS5Button.CROSS).onTrue(new InstantCommand(()->{ - if(r3.getAsBoolean()) return; - if(menu.getAsBoolean()){ - intakeAlgae.schedule(); - }else{ - if(toggle){ - if(intakeCoral.isScheduled()){ - intakeCoral.cancel(); - }else{ - intakeCoral.schedule(); - } - }else{ - intakeCoral.schedule(); - } - } - })).onFalse(new InstantCommand(()->{ - if(!toggle){ - intakeCoral.cancel(); - } - intakeAlgae.cancel(); - })); - // On true, run the command to start intaking - // On false, run the command to finish intaking if it has a coral - Command startIntake = new StationIntake(outtake); - // Command finishIn6take = new FinishStationIntake(intake, indexer, elevator, outtake); - // driver.get(PS5Button.CROSS).and(r3).and(menu.negate()).onTrue(startIntake) - // .onFalse(new InstantCommand(()->{ - // if(!startIntake.isScheduled()){ - // // finishIntake.schedule(); - // }else{ - // startIntake.cancel(); - // } - // })); - driver.get(PS5Button.CROSS).and(r3).onTrue( - new SequentialCommandGroup( - new MoveElevator(elevator, ElevatorConstants.STATION_INTAKE_SETPOINT), - new MoveArm(arm, ArmConstants.STATION_INTAKE_SETPOINT)). - andThen(startIntake)); - } - - if(intake != null && outtake != null && arm != null && elevator != null){ - Command algae = new SequentialCommandGroup( - new OuttakeAlgae(outtake, intake), - // Only move the arm and elevator in sequence when scoring in the net - new ConditionalCommand( - new SequentialCommandGroup( - new MoveArm(arm, ArmConstants.ALGAE_STOW_SETPOINT), - new InstantCommand(()-> elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT)), - new WaitCommand(0.25), - new InstantCommand(()-> arm.setSetpoint(ArmConstants.INTAKE_SETPOINT)) - ), - new InstantCommand(()->{ - elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT); - arm.setSetpoint(ArmConstants.INTAKE_SETPOINT); - }), - // 1 meter is in between L3 and net setpoints - () -> elevator.getSetpoint() > 1 - ) - ); - Command coral = new OuttakeCoral(outtake, elevator, arm).alongWith(new InstantCommand(()->getDrivetrain().setDesiredPose(()->null))) - .andThen( - new ConditionalCommand( - new SequentialCommandGroup(new MoveArm(arm, ArmConstants.INTAKE_SETPOINT), new MoveElevator(elevator, ElevatorConstants.STOW_SETPOINT), new InstantCommand(()->selectedDirection = 0)), - new DoNothing(), - ()->!arm.canMoveElevator() - )); - Command cancelAlign = new InstantCommand(()->{}, getDrivetrain()); - - driver.get(DPad.DOWN).onTrue(new InstantCommand(()->{ - if(menu.getAsBoolean()){ - algae.schedule(); - }else{ - coral.schedule(); - } - cancelAlign.schedule(); - })); - } - if(intake != null && indexer != null && outtake != null){ - driver.get(PS5Button.CIRCLE).and(menu.negate()).whileTrue(new ReverseMotors(intake, indexer, outtake)); - } - - // Climb - if(climb != null){ - driver.get(PS5Button.SQUARE).and(menu.negate()).toggleOnTrue(new StartEndCommand(()->climb.extend(), ()->climb.climb(), climb)); - if(intake != null){ - driver.get(PS5Button.SQUARE).and(menu.negate()).onTrue(new InstantCommand(()->intake.setAngle(65), intake)); - } - driver.get(PS5Button.PS).and(menu).whileTrue(new ResetClimb(climb)); - driver.get(PS5Button.RIGHT_TRIGGER).and(menu).onTrue(new InstantCommand(()->climb.stow(), climb)); - } - - // Alignment - driver.get(DPad.LEFT).toggleOnTrue(new InstantCommand(()->{ - selectedDirection = -1; - })); - driver.get(DPad.RIGHT).toggleOnTrue(new InstantCommand(()->{ - selectedDirection = 1; - })); - driver.get(PS5Button.TOUCHPAD).toggleOnTrue(new InstantCommand(()->{ - setAlignmentPose(true, false, false, false); - }).andThen(new DriveToPose(getDrivetrain(), ()->alignmentPose))); - // Reset the yaw. Mainly useful for testing/driver practice - driver.get(PS5Button.CREATE).and(menu.negate()).onTrue(new InstantCommand(() -> getDrivetrain().setYaw( - new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI) - ))); - driver.get(PS5Button.CREATE).and(menu).onTrue(new InstantCommand(() -> getDrivetrain().setYaw( - new Rotation2d(Robot.getAlliance() == Alliance.Blue ? Math.PI*5/6 : -Math.PI/6) + driver.get(PS5Button.CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw( + new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI) ))); // Cancel commands - driver.get(PS5Button.RIGHT_TRIGGER).and(menu.negate()).onTrue(new InstantCommand(()->{ - if(elevator != null){ - if(outtake != null && outtake.coralLoaded()){ - elevator.setSetpoint(ElevatorConstants.INTAKE_STOW_SETPOINT); - }else{ - elevator.setSetpoint(ElevatorConstants.STOW_SETPOINT); - } - } - if(outtake != null){ - outtake.stop(); - } - if(intake != null){ - intake.stow(); - intake.deactivate(); - } - if(indexer != null){ - indexer.stop(); - } - if(climb != null){ - climb.stow(); - } - if(arm != null){ - if(outtake != null && outtake.coralLoaded()){ - arm.setSetpoint(ArmConstants.STOW_SETPOINT); - }else{ - arm.setSetpoint(ArmConstants.START_ANGLE); - } - } + driver.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(()->{ getDrivetrain().setIsAlign(false); getDrivetrain().setDesiredPose(()->null); - alignmentPose = null; - selectedDirection = 0; CommandScheduler.getInstance().cancelAll(); })); - driver.get(PS5Button.MUTE).and(menu).onTrue(new FunctionalCommand( + // Align wheels + driver.get(PS5Button.MUTE).onTrue(new FunctionalCommand( ()->getDrivetrain().setStateDeadband(false), getDrivetrain()::alignWheels, interrupted->getDrivetrain().setStateDeadband(true), ()->false, getDrivetrain()).withTimeout(2)); } - - /** - * Sets the drivetrain's alignmetn pose to the nearest reef branch or algae location - * @param isAlgae True for algae, false for branches - * @param isLeft True for left branch, false for right, ignored for algae - * @param l4 If the robot should align to the L4 scoring pose - */ - private void setAlignmentPose(boolean isAlgae, boolean isLeft, boolean l4, boolean l1){ - Translation2d drivePose = getDrivetrain().getPose().getTranslation(); - int closestId = 0; - double closestDist = 20; - boolean isRed = Robot.getAlliance() == Alliance.Red; - int start = isRed ? 5 : 16; - for(int i = 0; i < 6; i++){ - double dist = FieldConstants.APRIL_TAGS.get(start+i).pose.toPose2d().getTranslation().getDistance(drivePose); - if(dist < closestDist){ - closestDist = dist; - closestId = start+i+1; - } - } - if(isAlgae){ - alignmentPose = VisionConstants.REEF.fromAprilTagIdAlgae(closestId).pose; - }else{ - if(l4){ - alignmentPose = VisionConstants.REEF.fromAprilTagIdAndPose(closestId, isLeft).l4Pose; - }else if(l1){ - alignmentPose = VisionConstants.REEF.fromAprilTagIdAndPose(closestId, isLeft).l1Pose; - }else{ - alignmentPose = VisionConstants.REEF.fromAprilTagIdAndPose(closestId, isLeft).pose; - } - } - } - - /** - * Sets the drivetrain's alignmetn pose to the nearest reef branch with the selected direction - * @param l4 If the robot should align to the L4 scoring pose - * @param l1 If the robot should align to the L1 scoring pose - */ - private void setAlignmentPose(boolean l4, boolean l1){ - if(selectedDirection == 0){ - alignmentPose = null; - return; - } - setAlignmentPose(false, selectedDirection < 0, l4, l1); - } - /** - * Sets the drivetrain's alignmetn pose to the nearest reef branch with the selected direction - * @param l4 If the robot should align to the L4 scoring pose - */ - private void setAlignmentPose(boolean l4){ - setAlignmentPose(l4, false); - } @Override public double getRawSideTranslation() { diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java deleted file mode 100644 index 0a075db..0000000 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ /dev/null @@ -1,168 +0,0 @@ -package frc.robot.subsystems.arm; - -import java.util.function.BooleanSupplier; - -import org.littletonrobotics.junction.Logger; - -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.sim.TalonFXSimState; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DutyCycleEncoder; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj.util.Color8Bit; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constants.ArmConstants; -import frc.robot.constants.Constants; -import frc.robot.constants.IdConstants; -import frc.robot.util.PhoenixUtil; - -public class Arm extends SubsystemBase implements ArmIO { - //motor - private TalonFX motor = new TalonFX(IdConstants.ARM_MOTOR); - private TalonFXSimState encoderSim; - - // Mechism2d display - private Mechanism2d simulationMechanism; - private MechanismLigament2d simLigament; - private SingleJointedArmSim armSim; - - private final DutyCycleEncoder absoluteEncoder = new DutyCycleEncoder(IdConstants.ARM_ABSOLUTE_ENCODER); - - private double setpoint = ArmConstants.START_ANGLE; - - private MotionMagicVoltage voltageRequest = new MotionMagicVoltage(0); - - private final ArmFeedforward feedforward = new ArmFeedforward(0, ArmConstants.MASS*ArmConstants.CENTER_OF_MASS_LENGTH/ArmConstants.GEAR_RATIO/ArmConstants.MOTOR.KtNMPerAmp*ArmConstants.MOTOR.rOhms, 0); - - private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); - - private BooleanSupplier elevatorStowed; - - public Arm() { - if (RobotBase.isSimulation()) { - encoderSim = motor.getSimState(); - encoderSim.setRawRotorPosition(Units.degreesToRotations(ArmConstants.START_ANGLE)*ArmConstants.GEAR_RATIO); - armSim = new SingleJointedArmSim( - ArmConstants.MOTOR, - ArmConstants.GEAR_RATIO, - ArmConstants.MOI, - ArmConstants.LENGTH, - Units.degreesToRadians(ArmConstants.MIN_ANGLE), //min angle - Units.degreesToRadians(ArmConstants.MAX_ANGLE), //max angle - true, - Units.degreesToRadians(ArmConstants.START_ANGLE)); - simulationMechanism = new Mechanism2d(3, 3); - MechanismRoot2d root = simulationMechanism.getRoot("Arm", 1.5, 1.5); - simLigament = root.append( - new MechanismLigament2d("angle", 1, ArmConstants.START_ANGLE, 4, new Color8Bit(Color.kAliceBlue)) - ); - SmartDashboard.putData("Arm Display", simulationMechanism); - } - - // resetAbsolute(); - motor.setPosition(Units.degreesToRotations(ArmConstants.START_ANGLE)*ArmConstants.GEAR_RATIO); - - var talonFXConfigs = new TalonFXConfiguration(); - - var slot0Configs = talonFXConfigs.Slot0; - slot0Configs.kS = 0.1; // Static friction compensation (should be >0 if friction exists) - slot0Configs.kG = ArmConstants.MASS * 9.81 * ArmConstants.CENTER_OF_MASS_LENGTH / ArmConstants.GEAR_RATIO; // Gravity compensation - slot0Configs.kV = 0.12; // Velocity gain: 1 rps -> 0.12V - slot0Configs.kA = 0; // Acceleration gain: 1 rps² -> 0V (should be tuned if acceleration matters) - slot0Configs.kP = 0.85; // If position error is 2.5 rotations, apply 12V (0.5 * 2.5 * 12V) - slot0Configs.kI = 0.; // Integral term (usually left at 0 for MotionMagic) - slot0Configs.kD = 0; // Derivative term (used to dampen oscillations) - - // set Motion Magic settings - var motionMagicConfigs = talonFXConfigs.MotionMagic; - motionMagicConfigs.MotionMagicCruiseVelocity = ArmConstants.MAX_VELOCITY * ArmConstants.GEAR_RATIO/Math.PI/2; - motionMagicConfigs.MotionMagicAcceleration = ArmConstants.MAX_ACCELERATION * ArmConstants.GEAR_RATIO/Math.PI/2; - talonFXConfigs.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - motor.getConfigurator().apply(talonFXConfigs); - updateInputs(); - PhoenixUtil.tryUntilOk(100, ()->motor.setNeutralMode(NeutralModeValue.Brake)); - - } - - public void setElevatorStowed(BooleanSupplier elevatorStowed){ - this.elevatorStowed = elevatorStowed; - } - - @Override - public void periodic() { - double setpoint2 = setpoint; - if(elevatorStowed == null || elevatorStowed.getAsBoolean() && Math.abs(setpoint2-ArmConstants.L1_SETPOINT) > 0.0001){ - setpoint2 = ArmConstants.START_ANGLE; - } - double setpointRotations = Units.degreesToRotations(setpoint2) * ArmConstants.GEAR_RATIO; - motor.setControl(voltageRequest.withPosition(setpointRotations).withFeedForward(feedforward.calculate(Units.degreesToRadians(getAngle()), 0))); - updateInputs(); - Logger.recordOutput("Arm/Atsetpoint",atSetpoint()); - } - - @Override - public void simulationPeriodic() { - armSim.setInputVoltage(getAppliedVoltage()); - armSim.update(Constants.LOOP_TIME); - - double armRotations = Units.radiansToRotations(armSim.getAngleRads()); - encoderSim.setRawRotorPosition(armRotations * ArmConstants.GEAR_RATIO); - simLigament.setAngle(getAngle()); - } - - public void setSetpoint(double setpoint) { - - this.setpoint = MathUtil.clamp(setpoint, ArmConstants.MIN_ANGLE, ArmConstants.MAX_ANGLE); - } - - public double getAppliedVoltage() { - return motor.getMotorVoltage().getValueAsDouble(); - } - - /** - * Gets the angle of the arm - * @return The angle in degrees - */ - public double getAngle() { - return inputs.measuredAngle; - } - - public void resetAbsolute(){ - if(RobotBase.isSimulation()){ - motor.setPosition(Units.degreesToRotations(ArmConstants.START_ANGLE)*ArmConstants.GEAR_RATIO); - }else{ - double absolutePosition = absoluteEncoder.get() / ArmConstants.ENCODER_GEAR_RATIO; - motor.setPosition(MathUtil.inputModulus(absolutePosition - Units.degreesToRotations(ArmConstants.OFFSET), -0.5, 0.5)*ArmConstants.GEAR_RATIO); - } - } - - public boolean atSetpoint() { - return Math.abs(getAngle() - setpoint) < ArmConstants.TOLERANCE; - } - - public boolean canMoveElevator() { - return Math.abs(getAngle() - ArmConstants.START_ANGLE) < 5 || Math.abs(getAngle() - ArmConstants.L1_SETPOINT) < 5; - } - - @Override - public void updateInputs(){ - inputs.measuredAngle = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / ArmConstants.GEAR_RATIO; - inputs.currentAmps = motor.getStatorCurrent().getValueAsDouble(); - - Logger.processInputs("Arm", inputs); - Logger.recordOutput("Arm/setpointDeg", setpoint); - } -} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java deleted file mode 100644 index 8eab615..0000000 --- a/src/main/java/frc/robot/subsystems/arm/ArmIO.java +++ /dev/null @@ -1,13 +0,0 @@ -package frc.robot.subsystems.arm; - -import org.littletonrobotics.junction.AutoLog; - -public interface ArmIO { - @AutoLog - public static class ArmIOInputs{ - public double measuredAngle = 0.0; - public double currentAmps = 0.0; - } - - public void updateInputs(); -} diff --git a/src/main/java/frc/robot/subsystems/climb/Climb.java b/src/main/java/frc/robot/subsystems/climb/Climb.java deleted file mode 100644 index 9dceaeb..0000000 --- a/src/main/java/frc/robot/subsystems/climb/Climb.java +++ /dev/null @@ -1,185 +0,0 @@ -package frc.robot.subsystems.climb; - -import org.littletonrobotics.junction.Logger; - -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.sim.TalonFXSimState; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj.util.Color8Bit; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constants.Constants; -import frc.robot.constants.IdConstants; -import frc.robot.util.ClimbArmSim; - -public class Climb extends SubsystemBase { - - private static final double startingPosition = 0; - private static final double extendPosition = 2; - private static final double climbPosition = -0.83; - - //Motors - private final PIDController pid = new PIDController(2.5, 0, 0.0); - - private TalonFX motor = new TalonFX(IdConstants.CLIMB_MOTOR, Constants.CANIVORE_CAN); - private final DCMotor climbGearBox = DCMotor.getKrakenX60(1); - private TalonFXSimState encoderSim; - - //Mechism2d display - private final Mechanism2d simulationMechanism = new Mechanism2d(3, 3); - private final MechanismRoot2d mechanismRoot = simulationMechanism.getRoot("Climb", 1.5, 1.5); - private final MechanismLigament2d simLigament = mechanismRoot.append( - new MechanismLigament2d("angle", 1, startingPosition, 4, new Color8Bit(Color.kAntiqueWhite)) - ); - - private final double versaPlanetaryGearRatio = 1.0; - private final double climbGearRatio = 60.0/1.0; - private final double totalGearRatio = versaPlanetaryGearRatio * climbGearRatio; - - private ClimbArmSim climbSim; - - private double power; - - private boolean resetting = false; - - private final ClimbIOInputsAutoLogged inputs = new ClimbIOInputsAutoLogged(); - - public Climb() { - if (RobotBase.isSimulation()) { - encoderSim = motor.getSimState(); - encoderSim.setRawRotorPosition(Units.degreesToRotations(startingPosition)*totalGearRatio); - - climbSim = new ClimbArmSim( - climbGearBox, - totalGearRatio, - 0.1, - 0.127, - 0, //min angle - Units.degreesToRadians(90), //max angle - true, - Units.degreesToRadians(startingPosition), - 60 - ); - - climbSim.setIsClimbing(true); - SmartDashboard.putData("Climb Display", simulationMechanism); - } - - pid.setIZone(1); - - pid.setSetpoint(Units.degreesToRadians(startingPosition)); - - motor.setPosition(Units.degreesToRotations(startingPosition)*totalGearRatio); - motor.setNeutralMode(NeutralModeValue.Brake); - //SmartDashboard.putData("Climb PID", pid); - } - - @Override - public void periodic() { - - inputs.measuredPositionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble() / totalGearRatio); - inputs.currentAmps = motor.getStatorCurrent().getValueAsDouble(); - Logger.processInputs("Climb", inputs); - - double motorPosition = motor.getPosition().getValueAsDouble(); - double currentPosition = Units.rotationsToRadians(motorPosition/totalGearRatio); - power = pid.calculate(currentPosition); - - if(resetting){ - power = -0.1; - } - - Logger.recordOutput("Climb/Motor Power", power); - Logger.recordOutput("Climb/setpointDeg", motorPosition/totalGearRatio); - - motor.set(MathUtil.clamp(power, -1, 1)); - } - - - @Override - public void simulationPeriodic() { - climbSim.setInput(power * Constants.ROBOT_VOLTAGE); - climbSim.update(Constants.LOOP_TIME); - - double climbRotations = Units.radiansToRotations(climbSim.getAngleRads()); - encoderSim.setRawRotorPosition(climbRotations * totalGearRatio); - - simLigament.setAngle(Units.radiansToDegrees(getAngle())); - } - - /** - * Sets the motor to an angle from 0-90 deg - * @param angle in degrees - */ - public void setAngle(double angle) { - pid.reset(); - pid.setSetpoint(Units.degreesToRadians(angle)); - } - - /** - * Gets the current position of the motor in degrees - * @return The angle in degrees - */ - public double getAngle() { - return inputs.measuredPositionDeg; - } - - /** - * Turns the motor to 90 degrees (extended positiion) - */ - public void extend(){ - double extendAngle = Units.rotationsToDegrees(extendPosition); - setAngle(extendAngle); - } - - /** - * Turns the motor to 0 degrees (climb position) - */ - public void climb(){ - setAngle(Units.rotationsToDegrees(climbPosition)); - } - - /** - * Turns the motor to 0 degrees (climb position) - */ - public void stow(){ - setAngle(startingPosition); - } - - public void reset(boolean resetting){ - this.resetting = resetting; - if(!resetting){ - motor.setPosition(climbPosition*totalGearRatio); - pid.setSetpoint(Units.degreesToRadians(startingPosition)); - pid.reset(); - } - } - - public double getCurrent(){ - return motor.getStatorCurrent().getValueAsDouble(); - } - - //not working - public Mechanism2d getMech2d() { - return simulationMechanism; - } - - /** - * Gets the estimated angle of the climb - * This is slightly inaccurate since it assumes the climb rotates exactly 90 degrees and the motor position is proportional to the climb position - * Used only for 3D robot display - */ - public double getEstimatedClimbAngle(){ - return Units.degreesToRotations(getAngle())/(extendPosition-climbPosition)*Math.PI/2; - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbIO.java b/src/main/java/frc/robot/subsystems/climb/ClimbIO.java deleted file mode 100644 index 6de5982..0000000 --- a/src/main/java/frc/robot/subsystems/climb/ClimbIO.java +++ /dev/null @@ -1,11 +0,0 @@ -package frc.robot.subsystems.climb; - -import org.littletonrobotics.junction.AutoLog; - -public interface ClimbIO { - @AutoLog - public static class ClimbIOInputs{ - public double measuredPositionDeg = 0.0; - public double currentAmps = 0.0; - } -} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 0175034..8edbb78 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -197,11 +197,16 @@ public class Drivetrain extends SubsystemBase { double[] sampleTimestamps = gyroInputs.odometryYawTimestamps; // All signals are sampled together int sampleCount = sampleTimestamps.length; + SwerveModulePosition[][] positions = new SwerveModulePosition[4][]; + for(int i = 0; i < modules.length; i++){ + positions[i] = modules[i].getOdometryPositions(); + sampleCount = Math.min(sampleCount, positions[i].length); + } for (int i = 0; i < sampleCount; i++) { // Read wheel positions and deltas from each module SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { - modulePositions[moduleIndex] = modules[moduleIndex].getOdometryPositions()[i]; + modulePositions[moduleIndex] = positions[moduleIndex][i]; } // Use the real gyro angle rawGyroRotation = gyroInputs.odometryYawPositions[i]; @@ -282,7 +287,9 @@ public class Drivetrain extends SubsystemBase { // Even if vision is disabled, it should still update inputs // This prevents it from storing a lot of unread results, and it could be useful for replays - vision.updateInputs(); + if(vision != null){ + vision.updateInputs(); + } if(VisionConstants.ENABLED){ if(vision != null && visionEnabled && visionEnableTimer.hasElapsed(5)){ @@ -685,7 +692,7 @@ public class Drivetrain extends SubsystemBase { } public void alignWheels(){ - SwerveModuleState state = new SwerveModuleState(0, new Rotation2d(1.465)); + SwerveModuleState state = new SwerveModuleState(0, new Rotation2d(0)); setModuleStates(new SwerveModuleState[]{ state, state, state, state }, false); diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java deleted file mode 100644 index 9fc85ae..0000000 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ /dev/null @@ -1,183 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems.elevator; - -import java.util.function.BooleanSupplier; - -import org.littletonrobotics.junction.Logger; - -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constants.Constants; -import frc.robot.constants.ElevatorConstants; -import frc.robot.constants.IdConstants; -import frc.robot.util.AngledElevatorSim; -import frc.robot.util.PhoenixUtil; - -public class Elevator extends SubsystemBase { - private TalonFX rightMotor = new TalonFX(IdConstants.ELEVATOR_RIGHT_MOTOR, Constants.CANIVORE_CAN); - - private double setpoint = ElevatorConstants.INTAKE_SETPOINT; - - private MotionMagicVoltage voltageRequest = new MotionMagicVoltage(0); - - private double maxVelocity = 3.6; // m/s 3.68 - private double maxAcceleration = 14; // m/s 8 - - // Sim variables - private AngledElevatorSim sim; - private Mechanism2d mechanism; - private MechanismLigament2d ligament; - private double voltage; - - private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged(); - - private BooleanSupplier armStowed; - - public Elevator() { - // This increases both the time and memory efficiency of the code when running - // on a real robot; do not remove this if statement - if (RobotBase.isSimulation()) { - sim = new AngledElevatorSim(ElevatorConstants.MOTOR, ElevatorConstants.GEARING, ElevatorConstants.CARRIAGE_MASS, - ElevatorConstants.DRUM_RADIUS, ElevatorConstants.MIN_HEIGHT, ElevatorConstants.MAX_HEIGHT, true, - ElevatorConstants.START_HEIGHT, ElevatorConstants.ANGLE, ElevatorConstants.SPRING_FORCE); - double width = ElevatorConstants.MAX_HEIGHT * Math.sin(ElevatorConstants.ANGLE); - double height = ElevatorConstants.MAX_HEIGHT * Math.cos(ElevatorConstants.ANGLE); - double size = Math.max(width, height); - mechanism = new Mechanism2d(size, size); - ligament = mechanism.getRoot("base", size / 2 - width / 2, size / 2 - height / 2).append(new MechanismLigament2d( - "elevator", ElevatorConstants.START_HEIGHT, 90 - Units.radiansToDegrees(Math.abs(ElevatorConstants.ANGLE)))); - SmartDashboard.putData("elevator", mechanism); - } - Timer.delay(1.0); - - //m_lastProfiledReference = new ExponentialProfile.State(getPosition(),0); - resetEncoder(ElevatorConstants.START_HEIGHT); - - var talonFXConfigs = new TalonFXConfiguration(); - - // set slot 0 gains - var slot0Configs = talonFXConfigs.Slot0; - slot0Configs.kS = 0.15; // Add 0.25 V output to overcome static friction - slot0Configs.kV = 0.12; // A velocity target of 1 rps results in 0.12 V output - slot0Configs.kA = 0; // An acceleration of 1 rps/s requires 0.01 V output - slot0Configs.kP = 0.75; // A position error of 2.5 rotations results in 12 V output - slot0Configs.kI = 0; // no output for integrated error - slot0Configs.kD = 0; // A velocity error of 1 rps results in 0.1 V output - - // set Motion Magic settings - var motionMagicConfigs = talonFXConfigs.MotionMagic; - motionMagicConfigs.MotionMagicCruiseVelocity = ElevatorConstants.GEARING * maxVelocity/ElevatorConstants.DRUM_RADIUS/Math.PI/2; // Target cruise velocity - motionMagicConfigs.MotionMagicAcceleration = ElevatorConstants.GEARING * maxAcceleration/ElevatorConstants.DRUM_RADIUS/Math.PI/2; // Target acceleration - rightMotor.getConfigurator().apply(talonFXConfigs); - rightMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)); - updateInputs(); - PhoenixUtil.tryUntilOk(100, ()-> rightMotor.setNeutralMode(NeutralModeValue.Brake)); - } - - public void setArmStowed(BooleanSupplier armStowed){ - this.armStowed = armStowed; - } - - @Override - public void periodic() { - double setpoint2 = setpoint; - if(setpoint2 < ElevatorConstants.SAFE_SETPOINT && (armStowed == null || !armStowed.getAsBoolean())){ - setpoint2 = ElevatorConstants.SAFE_SETPOINT; - } - double setpointRotations = ElevatorConstants.GEARING * setpoint2 / ElevatorConstants.DRUM_RADIUS/Math.PI/2; - rightMotor.setControl(voltageRequest.withPosition(setpointRotations).withFeedForward(0.4)); - updateInputs(); - Logger.processInputs("Elevator", inputs); - Logger.recordOutput("Elevator/Setpoint", getSetpoint()); - Logger.recordOutput("Elevator/AtSetpoint", atSetpoint()); - } - - @Override - public void simulationPeriodic() { - sim.setInputVoltage(0); - sim.update(Constants.LOOP_TIME); - ligament.setLength(sim.getPositionMeters()); - rightMotor.getSimState().setRawRotorPosition( - sim.getPositionMeters() / (2 * Math.PI * ElevatorConstants.DRUM_RADIUS) * ElevatorConstants.GEARING); - } - - public void resetEncoder(double height) { - // Without the if statement, this causes loop overruns in simulation, and this - // code does nothing anyway on sim (it sets the position to itself) - if (RobotBase.isReal()) { - rightMotor.setPosition(height / (2 * Math.PI * ElevatorConstants.DRUM_RADIUS) * ElevatorConstants.GEARING); - } - } - - public void updateInputs(){ - inputs.measuredPosition = rightMotor.getPosition().getValueAsDouble() / ElevatorConstants.GEARING - * (2 * Math.PI * ElevatorConstants.DRUM_RADIUS); - inputs.velocity = rightMotor.getVelocity().getValueAsDouble()/ ElevatorConstants.GEARING - * (2 * Math.PI * ElevatorConstants.DRUM_RADIUS); - inputs.currentAmps = rightMotor.getStatorCurrent().getValueAsDouble(); - } - - /** - * Get the position of the elevator in meters. - */ - public double getPosition() { - return inputs.measuredPosition; - } - - /** - * Get the velocity of the elevator in m/s. - */ - public double getVelocity(){ - return inputs.velocity; - } - - public double getVoltage(){ - return voltage; - } - - /** - * Method to set the setpoint of the elevator. Clamped between min and max height. - * @param setpoint The setpoint in meters. - */ - public void setSetpoint(double setpoint) { - this.setpoint = MathUtil.clamp(setpoint, ElevatorConstants.MIN_HEIGHT, ElevatorConstants.MAX_HEIGHT); - } - - /** - * Get the velocity of the elevator in meters. - */ - public double getSetpoint() { - return setpoint; - } - - public Mechanism2d getMechanism2d() { - return mechanism; - } - - public boolean atSetpoint(){ - return Math.abs(getPosition() - setpoint) < (0.025+0.0125); - } - - /** - * Get the COM at the current elevater height. - */ - public double getCenterOfMassHeight(){ - return (getPosition()-ElevatorConstants.MIN_HEIGHT)/(ElevatorConstants.MAX_HEIGHT-ElevatorConstants.MIN_HEIGHT)*(ElevatorConstants.CENTER_OF_MASS_HEIGHT_EXTENDED-ElevatorConstants.CENTER_OF_MASS_HEIGHT_STOWED)+ElevatorConstants.CENTER_OF_MASS_HEIGHT_STOWED; - } -} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java deleted file mode 100644 index c0f96a9..0000000 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ /dev/null @@ -1,12 +0,0 @@ -package frc.robot.subsystems.elevator; - -import org.littletonrobotics.junction.AutoLog; - -public interface ElevatorIO { - @AutoLog - public static class ElevatorIOInputs { - public double measuredPosition = 0.0; - public double velocity = 0.0; - public double currentAmps = 0.0; - } -} diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java deleted file mode 100644 index 4a6eaf8..0000000 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ /dev/null @@ -1,142 +0,0 @@ -package frc.robot.subsystems.indexer; - -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; - -import com.ctre.phoenix6.hardware.TalonFX; - -import au.grapplerobotics.ConfigurationFailedException; -import au.grapplerobotics.LaserCan; -import au.grapplerobotics.interfaces.LaserCanInterface; -import au.grapplerobotics.interfaces.LaserCanInterface.RangingMode; -import au.grapplerobotics.interfaces.LaserCanInterface.RegionOfInterest; -import au.grapplerobotics.interfaces.LaserCanInterface.TimingBudget; -import au.grapplerobotics.simulation.MockLaserCan; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.simulation.FlywheelSim; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Robot; -import frc.robot.constants.Constants; -import frc.robot.constants.IdConstants; -import frc.robot.constants.IndexerConstants; - -public class Indexer extends SubsystemBase { - private TalonFX motor; - private MockLaserCan simSensor; - private LaserCanInterface sensor; - - private FlywheelSim flywheelSim; - - // where the coral is for simulation - // in meters - private double simCoralPos; - - private final IndexerIOInputsAutoLogged inputs = new IndexerIOInputsAutoLogged(); - - public Indexer() { - motor = new TalonFX(IdConstants.INDEXER_MOTOR); - if (Robot.isSimulation()) { - flywheelSim = new FlywheelSim(LinearSystemId.createFlywheelSystem(DCMotor.getNEO(1), - IndexerConstants.MOMENT_OF_INERTIA, IndexerConstants.GEAR_RATIO), DCMotor.getNEO(1)); - - // have both interfaces availible - simSensor = new MockLaserCan(); - sensor = simSensor; - } else { - sensor = new LaserCan(IdConstants.INDEXER_SENSOR); - try { - sensor.setRangingMode(RangingMode.SHORT); - sensor.setTimingBudget(TimingBudget.TIMING_BUDGET_20MS); - sensor.setRegionOfInterest(new RegionOfInterest(-4, -4, 8, 8)); - } catch (ConfigurationFailedException e) { - DriverStation.reportError("Indexer LaserCan configuration error", true); - } - } - simCoralPos = IndexerConstants.START_SIM_POS_AT; // initialize it anyway, it's easier - } - - /** Runs the indexer. */ - public void run() { - motor.set(IndexerConstants.SPEED); - simCoralPos = IndexerConstants.START_SIM_POS_AT; - } - - public void slow(){ - motor.set(0.6); - } - - /** Reverses the indexer. */ - public void reverse() { - motor.set(-IndexerConstants.SPEED); - simCoralPos = IndexerConstants.END_SIM_SENSOR_POS_AT; - } - - /** Stops the indexer */ - public void stop() { - motor.stopMotor(); - } - - /** - * @return the motor velocity in rotations per minute - */ - public double getMotor() { - return inputs.velocity; - } - - /** - * Gets the LaserCAN's distance reading. - * If the distance is null, return 314,159 - * - * @return the distance, in millimeters - */ - public int getSensorValue() { - return inputs.sensorDistance; - } - - /** - * Checks whether a coral is in the indexer. - * True means nothing is there, false means something is there - * - * @return the sensor's state - */ - @AutoLogOutput(key = "Intake/isIndexerClear") - public boolean isIndexerClear() { - return true; - } - - @Override - public void periodic() { - //SmartDashboard.putBoolean("Indexer has coral ", isIndexerClear()); - - if (Robot.isReal()) { - inputs.velocity = motor.getVelocity().getValueAsDouble() / IndexerConstants.GEAR_RATIO; - } else { - inputs.velocity = flywheelSim.getAngularVelocityRPM(); - } - var measurement = sensor.getMeasurement(); - inputs.sensorDistance = (measurement == null || measurement.status > 0) ? 314159 : measurement.distance_mm; - Logger.processInputs("Indexer", inputs); - Logger.recordOutput("Indexer/indexer coral",isIndexerClear()); - } - - @Override - public void simulationPeriodic() { - flywheelSim.setInput(motor.get() * Constants.ROBOT_VOLTAGE); - flywheelSim.update(Constants.LOOP_TIME); - - // pretend we have a fake coral - simCoralPos += flywheelSim.getAngularVelocityRPM() / 60. * Constants.LOOP_TIME - * IndexerConstants.WHEEL_CIRCUMFERENCE; - - // toggle the sensor (values are backwards because that's how the sensor works) - simSensor.setMeasurementPartialSim(0, // 0 == valid measurement - (simCoralPos < IndexerConstants.START_SIM_SENSOR_POS_AT - || simCoralPos > IndexerConstants.END_SIM_SENSOR_POS_AT) - ? IndexerConstants.MEASUREMENT_THRESHOLD * 2 - : 0, - 1000 // IDK what this is exactly, but 1000 seems good - ); - } -} diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java deleted file mode 100644 index 3678c55..0000000 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java +++ /dev/null @@ -1,11 +0,0 @@ -package frc.robot.subsystems.indexer; - -import org.littletonrobotics.junction.AutoLog; - -public interface IndexerIO { - @AutoLog - public class IndexerIOInputs{ - public double velocity = 0.0; - public int sensorDistance = 0; - } -} diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java deleted file mode 100644 index 27e0309..0000000 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ /dev/null @@ -1,242 +0,0 @@ -package frc.robot.subsystems.intake; - -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; - -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; - -import au.grapplerobotics.LaserCan; -import au.grapplerobotics.interfaces.LaserCanInterface.Measurement; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constants.Constants; -import frc.robot.constants.IdConstants; -import frc.robot.constants.IntakeConstants; - - -public class Intake extends SubsystemBase { - private final TalonFX rollerMotor = new TalonFX(IdConstants.INTAKE_ROLLER); - private final TalonFX pivotMotor = new TalonFX(IdConstants.INTAKE_PIVOT, Constants.CANIVORE_CAN); - - private SingleJointedArmSim stowArmSim; - private Mechanism2d stowMechanism2d; - private MechanismLigament2d stowWheelLigament; - - private final double positionTolerance = 5; - - private final PIDController stowPID = new PIDController(0.015, 0, 0); - private double power; - - private LaserCan laserCan; - private boolean hasCoral = false; - private boolean isMoving = false; - private Timer laserCanSimTimer; - private DCMotor dcMotor = DCMotor.getKrakenX60(1); - private ArmFeedforward feedforward = new ArmFeedforward(0, - Constants.GRAVITY_ACCELERATION * IntakeConstants.CENTER_OF_MASS_DIST * IntakeConstants.MASS - / IntakeConstants.PIVOT_GEAR_RATIO * dcMotor.rOhms / dcMotor.KtNMPerAmp / Constants.ROBOT_VOLTAGE, - 0); - private double startPosition = 90; - - private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); - - public Intake() { - if (RobotBase.isSimulation()) { - stowMechanism2d = new Mechanism2d(10, 10); - stowWheelLigament = stowMechanism2d.getRoot("Root", 5, 5) - .append(new MechanismLigament2d("Intake", 4, startPosition)); - SmartDashboard.putData("Intake pivot", stowMechanism2d); - stowArmSim = new SingleJointedArmSim( - dcMotor, - IntakeConstants.PIVOT_GEAR_RATIO, - IntakeConstants.MOMENT_OFiNERTIA, - IntakeConstants.ARM_LENGTH, - Math.toRadians(0), - Math.toRadians(90), - true, - Units.degreesToRadians(startPosition)); - laserCanSimTimer = new Timer(); - } else { - // laserCan = new LaserCan(IdConstants.INTAKE_LASER_CAN); - // try { - // laserCan.setRangingMode(RangingMode.SHORT); - // laserCan.setTimingBudget(TimingBudget.TIMING_BUDGET_20MS); - // laserCan.setRegionOfInterest(new RegionOfInterest(-4, -4, 8, 8)); - // } catch (ConfigurationFailedException e) { - // DriverStation.reportError("Intake LaserCan configuration error", true); - // } - } - rollerMotor.getConfigurator().apply(new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake).withInverted(InvertedValue.CounterClockwise_Positive)); - pivotMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive)); - pivotMotor.setPosition(Units.degreesToRotations(startPosition) * IntakeConstants.PIVOT_GEAR_RATIO); - pivotMotor.setNeutralMode(NeutralModeValue.Coast); - stowPID.setTolerance(positionTolerance); - - setAngle(startPosition); - - rollerMotor.getConfigurator().apply(new CurrentLimitsConfigs().withStatorCurrentLimit(40).withStatorCurrentLimitEnable(true)); - } - - /** - * publishes stuff to smartdashboard - */ - @SuppressWarnings("unused") - private void publish() { - SmartDashboard.putNumber("Stow Motor Position", getPivotAngle()); - SmartDashboard.putNumber("Target Angle", stowPID.getSetpoint()); - SmartDashboard.putNumber("Roller Motor Power", power); - - SmartDashboard.putBoolean("Has Coral", hasCoral()); - SmartDashboard.putBoolean("Stow Arm Sim - Is Stowed", isAtSetpoint(90)); - SmartDashboard.putBoolean("Stow Arm Sim - Is Unstowed", isAtSetpoint(0)); - SmartDashboard.putBoolean("Is Roller Active", rollerMotor.get() > 0); - } - - @Override - public void periodic() { - //publish(); - // SmartDashboard.putNumber("angle", getPivotAngle()); - // SmartDashboard.putBoolean("Intake has coral", hasCoral()); - - double position = getPivotAngle(); - power = stowPID.calculate(position) + feedforward.calculate(Units.degreesToRadians(position), 0); - power = MathUtil.clamp(power, -1, 1); - pivotMotor.set(power); - if (laserCan != null) { - Measurement measurement = laserCan.getMeasurement(); - hasCoral = measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT - && measurement.distance_mm <= 1000 * IntakeConstants.DETECT_CORAL_DIST; - } - if(RobotBase.isSimulation()) { - inputs.measuredPivotPosition = Units.radiansToDegrees(stowArmSim.getAngleRads()); - } else { - inputs.measuredPivotPosition = Units.rotationsToDegrees(pivotMotor.getPosition().getValueAsDouble()) / IntakeConstants.PIVOT_GEAR_RATIO; - } - inputs.rollerVelocity = rollerMotor.getVelocity().getValueAsDouble(); - Logger.processInputs("Intake", inputs); - - Logger.recordOutput("Intake/PivotSetpoint", stowPID.getSetpoint()); - } - - @Override - public void simulationPeriodic() { - stowArmSim.setInputVoltage(power * Constants.ROBOT_VOLTAGE); - stowArmSim.update(Constants.LOOP_TIME); - stowWheelLigament.setAngle(Units.radiansToDegrees(stowArmSim.getAngleRads())); - if (!isMoving) { - laserCanSimTimer.reset(); - laserCanSimTimer.start(); - hasCoral = false; - } else { - if (isAtSetpoint()) { - laserCanSimTimer.start(); - } - hasCoral = laserCanSimTimer.hasElapsed(0.5) && !laserCanSimTimer.hasElapsed(1); - } - } - - /** - * Gets the rotation of the intake. - * - * @return the rotation of the intake (in degrees). - */ - public double getPivotAngle() { - return inputs.measuredPivotPosition; - } - - public PIDController getPID() { - return stowPID; - } - - /** - * Checks if intake has coral. - * - * @return Boolean (True if has Coral, False otherwise) - */ - @AutoLogOutput(key = "Intake/HasCoral") - public boolean hasCoral() { - return hasCoral; - } - - /** - * Checks if motor is at current setpoint. - * - * @return Boolean (True if at setpoint, False otherwise) - */ - public boolean isAtSetpoint(double setpoint) { - return Math.abs(getPivotAngle() - setpoint) < positionTolerance; - } - - /** - * Returns whether or not the intake is at its setpoint - * - * @return True if it is at the setpoint, false otherwise - */ - @AutoLogOutput(key = "Intake/AtSetPoint") - public boolean isAtSetpoint() { - return stowPID.atSetpoint(); - } - - /** - * Sets the desired angle of the intake, mostly to stow or unstow. - * - * @param angle desired angle of the intake in degrees - */ - public void setAngle(double angle) { - stowPID.setSetpoint(angle); - } - - /** - * Sets the speed of the roller motor. - * - * @param power The desired speed of the roller, between 0 and 1. - */ - public void setSpeed(double power) { - rollerMotor.set(power); - isMoving = Math.abs(power) < 0.01; - - } - - /** - * Moves the intake up, doesn't stop it. - */ - public void stow() { - stowPID.setSetpoint(IntakeConstants.STOW_SETPOINT); - } - - /** - * Moves the intake down. - */ - public void unstow() { - stowPID.setSetpoint(IntakeConstants.INTAKE_SETPOINT); - } - - /** - * Stops the motor. - */ - public void deactivate() { - rollerMotor.set(0); - } - - /** - * Starts the motor. - */ - public void activate(){ - rollerMotor.set(IntakeConstants.INTAKE_MOTOR_POWER); - } -} - diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java deleted file mode 100644 index e0e9305..0000000 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ /dev/null @@ -1,11 +0,0 @@ -package frc.robot.subsystems.intake; - -import org.littletonrobotics.junction.AutoLog; - -public interface IntakeIO { - @AutoLog - public static class IntakeIOInputs { - public double rollerVelocity = 0.0; - public double measuredPivotPosition = 0.0; - } -} diff --git a/src/main/java/frc/robot/subsystems/outtake/Outtake.java b/src/main/java/frc/robot/subsystems/outtake/Outtake.java deleted file mode 100644 index 09bb2e2..0000000 --- a/src/main/java/frc/robot/subsystems/outtake/Outtake.java +++ /dev/null @@ -1,51 +0,0 @@ -package frc.robot.subsystems.outtake; - -import edu.wpi.first.wpilibj.simulation.DIOSim; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -/** - * Abstract class for the outtake. All commands should use this subsystem - */ -public abstract class Outtake extends SubsystemBase { - /** Coral detected before the rollers */ - protected DIOSim dioInputLoaded; - /** Coral detected after the rollers */ - protected DIOSim dioInputEjecting; - - protected abstract double getMotorSpeed(); - - public void simulationPeriodic(){} - - /** Set the motor power to move the coral */ - public abstract void setMotor(double power); - - /** stop the coral motor */ - public void stop() { - setMotor(0); - } - - /** start spinning the rollers to eject the coral */ - public abstract void outtake(); - - public abstract boolean coralLoaded(); - - /** - * Coral is at the ejecting beam break sensor. - * @return coral is interrupting the beam breaker. - */ - public abstract boolean coralEjecting(); - - public abstract void reverse(); - - public void removeAlgae(){ - setMotor(-0.6); - } - - public void intakeAlgaeReef() { - setMotor(-0.6); - } - - public void outtakeAlgae() { - setMotor(0.9); - } -} diff --git a/src/main/java/frc/robot/subsystems/outtake/OuttakeAlpha.java b/src/main/java/frc/robot/subsystems/outtake/OuttakeAlpha.java deleted file mode 100644 index edd057c..0000000 --- a/src/main/java/frc/robot/subsystems/outtake/OuttakeAlpha.java +++ /dev/null @@ -1,107 +0,0 @@ -package frc.robot.subsystems.outtake; - - -import com.revrobotics.ColorSensorV3; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.SparkFlexConfig; - - -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.simulation.DIOSim; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.constants.IdConstants; - - -public class OuttakeAlpha extends Outtake { - - private SparkFlex motor = new SparkFlex(IdConstants.OUTTAKE_MOTOR_ALPHA, MotorType.kBrushless); - private double power; - - - /** Coral detected before the rollers */ - private final ColorSensorV3 colorSensor = new ColorSensorV3(IdConstants.i2cPort); - /** Coral detected after the rollers */ - private DigitalInput digitalInputEjecting = new DigitalInput(IdConstants.OUTTAKE_DIO_EJECTING); - - - - public OuttakeAlpha(){ - motor.configure(new SparkFlexConfig() - .inverted(true) - .idleMode(IdleMode.kBrake), - ResetMode.kResetSafeParameters, - PersistMode.kNoPersistParameters - ); - if (RobotBase.isSimulation()){ - // object that will control the ejecting sensor - dioInputEjecting = new DIOSim(digitalInputEjecting); - // assume coral is loaded - dioInputLoaded.setValue(false); - // we are not ejecting - dioInputEjecting.setValue(true); - } - } - - @Override - protected double getMotorSpeed(){ - return motor.get(); - } - - @Override - public void periodic(){ - motor.set(power); - // SmartDashboard.putBoolean("Coral loaded", coralLoaded()); - // SmartDashboard.putBoolean("Coral ejected", coralEjecting()); - - } - - /** Set the motor power to move the coral */ - public void setMotor(double power){ - this.power = power; - } - - - /** start spinning the rollers to eject the coral */ - public void outtake(){ - // assumes the coral is present - // if the coral is not present, we should not bother to spin the rollers - setMotor(SmartDashboard.getNumber("wheel speed", 0.2)); - // this starts the motor... what needs to be done later? - } - - /** - * Coral is at the ejecting beam break sensor. - * @return coral is interrupting the beam breaker. - */ - public boolean coralEjecting() { - return !digitalInputEjecting.get(); - } - - - public void reverse(){ - setMotor(-0.2); - } - - - public boolean isSimulation(){ - return RobotBase.isSimulation(); - } - - public int getProximity() { - return colorSensor.getProximity(); // Returns 0 (far) to ~2047 (very close) - } - - // coral detection from color sensor - public boolean coralLoaded() { - //this is about 1/2inch away -- might have to change based on placement - if (getProximity() > 800) { - return true; - } - return false; - } -} diff --git a/src/main/java/frc/robot/subsystems/outtake/OuttakeComp.java b/src/main/java/frc/robot/subsystems/outtake/OuttakeComp.java deleted file mode 100644 index b2daa19..0000000 --- a/src/main/java/frc/robot/subsystems/outtake/OuttakeComp.java +++ /dev/null @@ -1,87 +0,0 @@ -package frc.robot.subsystems.outtake; - -import org.littletonrobotics.junction.Logger; - -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.revrobotics.ColorSensorV3; - -import edu.wpi.first.hal.I2CJNI; -import frc.robot.constants.IdConstants; - -public class OuttakeComp extends Outtake { - - private TalonFX motor = new TalonFX(IdConstants.OUTTAKE_MOTOR_COMP); - private double power; - - /** Coral detected before the rollers */ - private ColorSensorV3 colorSensor = new ColorSensorV3(IdConstants.i2cPort); - - OuttakeIOIntakesAutoLogged inputs = new OuttakeIOIntakesAutoLogged(); - - public OuttakeComp(){ - motor.getConfigurator().apply(new MotorOutputConfigs() - .withInverted(InvertedValue.Clockwise_Positive) - .withNeutralMode(NeutralModeValue.Brake) - ); - } - - @Override - protected double getMotorSpeed() { - return power; - } - - @Override - public void periodic(){ - motor.set(power); - // SmartDashboard.putBoolean("Coral loaded", coralLoaded()); - // SmartDashboard.putBoolean("Coral ejected", coralEjecting()); - - inputs.motorVelocity = motor.getVelocity().getValueAsDouble(); - Logger.processInputs("Outtake", inputs); - //Logger.recordOutput("Outtake/Sensor", getProximity()); - //Logger.recordOutput("Outtake/SensorConnected", colorSensor.isConnected()); - } - - /** Set the motor power to move the coral */ - public void setMotor(double power){ - this.power = power; - } - - /** start spinning the rollers to eject the coral */ - public void outtake(){ - setMotor(-0.3); - } - - /** - * Coral is in the outtake. - * @return The same thing as coralLoaded(), for compatibility with previous code - */ - public boolean coralEjecting() { - return coralLoaded(); - } - - - public void reverse(){ - setMotor(0.2); - } - - public int getProximity() { - inputs.proximity = colorSensor.getProximity(); - if (inputs.proximity > 0){ - return inputs.proximity; - } - else{ - I2CJNI.i2CClose(1); - colorSensor = new ColorSensorV3(IdConstants.i2cPort); - return inputs.proximity = colorSensor.getProximity(); // Returns 0 (far) to ~2047 (very close) - } - } - - // coral detection from color sensor - public boolean coralLoaded() { - return getProximity() > 2000; - } -} diff --git a/src/main/java/frc/robot/subsystems/outtake/OuttakeIO.java b/src/main/java/frc/robot/subsystems/outtake/OuttakeIO.java deleted file mode 100644 index 006d7c8..0000000 --- a/src/main/java/frc/robot/subsystems/outtake/OuttakeIO.java +++ /dev/null @@ -1,11 +0,0 @@ -package frc.robot.subsystems.outtake; - -import org.littletonrobotics.junction.AutoLog; - -public interface OuttakeIO { - @AutoLog - public static class OuttakeIOIntakes { - public double motorVelocity = 0.0; - public int proximity = 0; - } -}