diff --git a/src/main/deploy/example.txt b/src/main/deploy/example.txt deleted file mode 100644 index bb82515..0000000 --- a/src/main/deploy/example.txt +++ /dev/null @@ -1,3 +0,0 @@ -Files placed in this directory will be deployed to the RoboRIO into the -'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function -to get a proper path relative to the deploy directory. \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Center Subwoofer 1S.auto b/src/main/deploy/pathplanner/autos/Center Subwoofer 1S.auto deleted file mode 100644 index 42fd024..0000000 --- a/src/main/deploy/pathplanner/autos/Center Subwoofer 1S.auto +++ /dev/null @@ -1,37 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.335622956146197, - "y": 5.5196408968316515 - }, - "rotation": 179.37978729180955 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Charge Shooter" - } - }, - { - "type": "named", - "data": { - "name": "Speaker Note Shot" - } - }, - { - "type": "path", - "data": { - "pathName": "Center Subwoofer to Center" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Just Backup.auto b/src/main/deploy/pathplanner/autos/Just Backup.auto deleted file mode 100644 index 68a6d46..0000000 --- a/src/main/deploy/pathplanner/autos/Just Backup.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.9847983228729987, - "y": 1.9646179463299098 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Just Backup" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left Subwoofer Center 1S.auto b/src/main/deploy/pathplanner/autos/Left Subwoofer Center 1S.auto deleted file mode 100644 index 4bb49a0..0000000 --- a/src/main/deploy/pathplanner/autos/Left Subwoofer Center 1S.auto +++ /dev/null @@ -1,43 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.4936438362905214, - "y": 6.981410202136645 - }, - "rotation": 180.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Left Subwoofer" - } - }, - { - "type": "named", - "data": { - "name": "Charge Shooter" - } - }, - { - "type": "named", - "data": { - "name": "Speaker Note Shot" - } - }, - { - "type": "path", - "data": { - "pathName": "L Sub to Center" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 1S 2A.auto b/src/main/deploy/pathplanner/autos/Right 1S 2A.auto deleted file mode 100644 index 1f24ce8..0000000 --- a/src/main/deploy/pathplanner/autos/Right 1S 2A.auto +++ /dev/null @@ -1,74 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.1134340217398382, - "y": 6.525338178881486 - }, - "rotation": 42.79740183823424 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Speaker Note Shot" - } - }, - { - "type": "path", - "data": { - "pathName": "Left Preloaded+Pickup" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "named", - "data": { - "name": "Amp Handoff" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "Note 1 to amp" - } - }, - { - "type": "named", - "data": { - "name": "Amp Note Shot" - } - }, - { - "type": "path", - "data": { - "pathName": "Leftmost Center Note" - } - }, - { - "type": "path", - "data": { - "pathName": "Note 1 to amp" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 2 Amp.auto b/src/main/deploy/pathplanner/autos/Right 2 Amp.auto deleted file mode 100644 index dd7668a..0000000 --- a/src/main/deploy/pathplanner/autos/Right 2 Amp.auto +++ /dev/null @@ -1,50 +0,0 @@ -{ - "version": 1.0, - "startingPose": null, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Speaker Note Shot" - } - }, - { - "type": "path", - "data": { - "pathName": "Left Preloaded+Pickup" - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Amp Handoff" - } - }, - { - "type": "path", - "data": { - "pathName": "Note 1 to amp" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Amp Note Shot" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 3 Amp.auto b/src/main/deploy/pathplanner/autos/Right 3 Amp.auto deleted file mode 100644 index c9f7bb5..0000000 --- a/src/main/deploy/pathplanner/autos/Right 3 Amp.auto +++ /dev/null @@ -1,43 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.1134340217398382, - "y": 6.525338178881486 - }, - "rotation": 33.13560954871888 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Left Preloaded+Pickup" - } - }, - { - "type": "path", - "data": { - "pathName": "Note 1 to amp" - } - }, - { - "type": "path", - "data": { - "pathName": "Leftmost Center Note" - } - }, - { - "type": "path", - "data": { - "pathName": "Note 1 to amp" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right Subwoofer 1S.auto b/src/main/deploy/pathplanner/autos/Right Subwoofer 1S.auto deleted file mode 100644 index b7048f2..0000000 --- a/src/main/deploy/pathplanner/autos/Right Subwoofer 1S.auto +++ /dev/null @@ -1,43 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.0578032584636603, - "y": 4.485794114445372 - }, - "rotation": 141.84277341263092 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Charge Shooter" - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "named", - "data": { - "name": "Speaker Note Shot" - } - }, - { - "type": "path", - "data": { - "pathName": "Right Subwoofer to Center" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json deleted file mode 100644 index bab0da9..0000000 --- a/src/main/deploy/pathplanner/navgrid.json +++ /dev/null @@ -1 +0,0 @@ -{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Subwoofer to Center.path b/src/main/deploy/pathplanner/paths/Center Subwoofer to Center.path deleted file mode 100644 index 93629d1..0000000 --- a/src/main/deploy/pathplanner/paths/Center Subwoofer to Center.path +++ /dev/null @@ -1,74 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.335622956146197, - "y": 5.5196408968316515 - }, - "prevControl": null, - "nextControl": { - "x": 2.3356229561461985, - "y": 5.5196408968316515 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.072055095677142, - "y": 5.028486410249174 - }, - "prevControl": { - "x": 3.364392809755599, - "y": 5.618204981850461 - }, - "nextControl": { - "x": 4.843869288878178, - "y": 4.385307915914977 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.299641721790566, - "y": 0.877061583182996 - }, - "prevControl": { - "x": 6.773404771880769, - "y": 4.046177437084219 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.9, - "rotationDegrees": 90.25335933192582, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 179.04515874612784, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Just Backup.path b/src/main/deploy/pathplanner/paths/Just Backup.path deleted file mode 100644 index c507fa0..0000000 --- a/src/main/deploy/pathplanner/paths/Just Backup.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.9847983228729987, - "y": 1.9646179463299098 - }, - "prevControl": null, - "nextControl": { - "x": 1.9847983228729973, - "y": 1.9646179463299098 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.6453682536994707, - "y": 1.9646179463299098 - }, - "prevControl": { - "x": 1.6453682536994707, - "y": 1.9646179463299098 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 1.3971810272964678, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/L Sub to Center.path b/src/main/deploy/pathplanner/paths/L Sub to Center.path deleted file mode 100644 index e4298b8..0000000 --- a/src/main/deploy/pathplanner/paths/L Sub to Center.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.2888463383764373, - "y": 6.642279723305886 - }, - "prevControl": null, - "nextControl": { - "x": 2.2888463383764366, - "y": 6.642279723305886 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.159911049166827, - "y": 6.864468657712245 - }, - "prevControl": { - "x": 2.159911049166827, - "y": 6.864468657712245 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 21.801409486351982, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Preloaded+Pickup.path b/src/main/deploy/pathplanner/paths/Left Preloaded+Pickup.path deleted file mode 100644 index 732c2b5..0000000 --- a/src/main/deploy/pathplanner/paths/Left Preloaded+Pickup.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.1134340217398382, - "y": 6.525338178881486 - }, - "prevControl": null, - "nextControl": { - "x": 1.148516485067158, - "y": 7.191904982100564 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.3179319293111518, - "y": 6.981410202136645 - }, - "prevControl": { - "x": 1.8033891338437944, - "y": 6.981410202136645 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 40.97173633351488, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Subwoofer.path b/src/main/deploy/pathplanner/paths/Left Subwoofer.path deleted file mode 100644 index 78479a7..0000000 --- a/src/main/deploy/pathplanner/paths/Left Subwoofer.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.4936438362905214, - "y": 6.981410202136645 - }, - "prevControl": null, - "nextControl": { - "x": 1.3941696612245824, - "y": 6.981410202136645 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.148516485067158, - "y": 6.560420642208806 - }, - "prevControl": { - "x": 1.5812001994374358, - "y": 6.864468657712245 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -146.30993247402017, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": -178.6677801461302, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Leftmost Center Note.path b/src/main/deploy/pathplanner/paths/Leftmost Center Note.path deleted file mode 100644 index c76a48f..0000000 --- a/src/main/deploy/pathplanner/paths/Leftmost Center Note.path +++ /dev/null @@ -1,102 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 3.347017520241028, - "y": 6.993104356575341 - }, - "prevControl": null, - "nextControl": { - "x": 4.446268037830382, - "y": 7.110045900999739 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.071755505338439, - "y": 7.297152372084174 - }, - "prevControl": { - "x": 5.077752377722138, - "y": 7.262069908751458 - }, - "nextControl": { - "x": 6.725659674531382, - "y": 7.320231342764915 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.63877220062539, - "y": 7.425788070951012 - }, - "prevControl": { - "x": 6.621380764133118, - "y": 7.460870534278332 - }, - "nextControl": { - "x": 8.656163637117663, - "y": 7.390705607623692 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.545818145767345, - "y": 6.642279723305886 - }, - "prevControl": { - "x": 6.323179825848877, - "y": 7.12758713266797 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1, - "rotationDegrees": 172.50414236027015, - "rotateFast": false - } - ], - "constraintZones": [ - { - "name": "New Constraints Zone", - "minWaypointRelativePos": 0.85, - "maxWaypointRelativePos": 1.35, - "constraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - } - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 1.5, - "rotation": -179.23610153906992, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Note 1 to Speaker.path b/src/main/deploy/pathplanner/paths/Note 1 to Speaker.path deleted file mode 100644 index 0fc3f94..0000000 --- a/src/main/deploy/pathplanner/paths/Note 1 to Speaker.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.0, - "y": 7.0 - }, - "prevControl": null, - "nextControl": { - "x": 3.0, - "y": 7.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.2654580294915574, - "y": 6.291455090032687 - }, - "prevControl": { - "x": 1.7098358983042754, - "y": 6.525338178881486 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -145.88552705465875, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 179.55484341368705, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Note 1 to amp.path b/src/main/deploy/pathplanner/paths/Note 1 to amp.path deleted file mode 100644 index caae895..0000000 --- a/src/main/deploy/pathplanner/paths/Note 1 to amp.path +++ /dev/null @@ -1,63 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.353014392638472, - "y": 6.993104356579084 - }, - "prevControl": null, - "nextControl": { - "x": 1.768306670516475, - "y": 7.004798511021523 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.8384715971711143, - "y": 7.624588696470841 - }, - "prevControl": { - "x": 1.7950792079196904, - "y": 7.315863019194643 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [ - { - "name": "New Event Marker", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [] - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 90.0, - "rotateFast": true - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": -178.36342295838335, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Subwoofer to Center.path b/src/main/deploy/pathplanner/paths/Right Subwoofer to Center.path deleted file mode 100644 index 9bd6838..0000000 --- a/src/main/deploy/pathplanner/paths/Right Subwoofer to Center.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.0578032584636603, - "y": 4.485794114445372 - }, - "prevControl": null, - "nextControl": { - "x": 1.5708842846257125, - "y": 2.6240429623716395 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.0095431619434665, - "y": 0.8355890997496291 - }, - "prevControl": { - "x": 6.0095431619434665, - "y": 0.8355890997496291 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 144.68878656036674, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/java/frc/robot/Commands/AmpHandoff.java b/src/main/java/frc/robot/Commands/AmpHandoff.java deleted file mode 100644 index 60da463..0000000 --- a/src/main/java/frc/robot/Commands/AmpHandoff.java +++ /dev/null @@ -1,13 +0,0 @@ -package frc.robot.Commands; - -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import frc.robot.subsystems.Indexer; -import frc.robot.subsystems.Shooter; - -public class AmpHandoff extends ParallelCommandGroup{ - - AmpHandoff(Indexer indexer, Shooter shooter){ - //addCommands(indexer.shootNote(() -> 1), shooter.ampHandoff()); - } - -} diff --git a/src/main/java/frc/robot/Commands/SpeakerShot.java b/src/main/java/frc/robot/Commands/SpeakerShot.java deleted file mode 100644 index 2b0a1e4..0000000 --- a/src/main/java/frc/robot/Commands/SpeakerShot.java +++ /dev/null @@ -1,13 +0,0 @@ -package frc.robot.Commands; - -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import frc.robot.constants.ShooterConstants; -import frc.robot.subsystems.Indexer; -import frc.robot.subsystems.Shooter; - -public class SpeakerShot extends ParallelCommandGroup{ - - SpeakerShot(Indexer indexer, Shooter shooter){ - //addCommands(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterLoadAngle, 0, 0), indexer.shootNote(() -> 1.0)); - } -} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e387401..36f86e2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,134 +4,26 @@ package frc.robot; -import java.io.IOException; -import java.util.Optional; - -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.auto.NamedCommands; - -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.constants.OIConstants; -import frc.robot.constants.PhotonConstants; -import frc.robot.constants.ShooterConstants; -import frc.robot.subsystems.Climber; import frc.robot.subsystems.Drivetrain; -import frc.robot.subsystems.Indexer; -import frc.robot.utilities.PhotonVision; -import frc.robot.subsystems.Intake; -import frc.robot.subsystems.Shooter; public class RobotContainer { - private static final String kAutoTabName = "Autonomous"; - private static final String kTeleopTabName = "Teloperated"; - - //private PhotonVision photonvision; - private Drivetrain drivetrain; - private Intake intake; - private Shooter shooter; - private Climber climber; - private Indexer indexer; private CommandXboxController driver; - private CommandXboxController operator; - - private Trigger isTeleopTrigger; - - private SendableChooser autoChooser; - - private int ampTagID; - - // TODO There's more than one source tag, how do we want to handle this? - private int sourceTagID; - - // TODO There's more than one speaker tag, how do we want to handle this? - private int speakerTag; - - private boolean setAmpAngle; public RobotContainer() { - /*try { - - photonvision = new PhotonVision( - PhotonConstants.kCameraName, - PhotonConstants.kCameraTransform, - PhotonConstants.kCameraHeightMeters, - PhotonConstants.kCameraPitchRadians - ); - } catch (IOException e) { - photonvision = null; - }*/ - - // TODO Need to provide a real initial pose - // TODO Need to provide a real IAprilTagProvider, null means we're not using one at all - // TODO Need to provide a real VisualPoseProvider, null means we're not using one at all - drivetrain = new Drivetrain(new Pose2d(), null, null); - - intake = new Intake(); - - indexer = new Indexer(); - - shooter = new Shooter(indexer::getBeamBreak); - - climber = new Climber(shooter::getShooterAngle); - - NamedCommands.registerCommand("Charge Shooter 2 Sec", shooter.angleSpeedsSetpoints(() -> ShooterConstants.kShooterLoadAngle, 1.0, 1.0).withTimeout(2.0)); - NamedCommands.registerCommand("Speaker Note Shot", Commands.parallel(shooter.angleSpeedsSetpoints(() -> ShooterConstants.kShooterLoadAngle, 1.0, 1.0), indexer.shootNote(() -> 1.0)).withTimeout(2.0)); - - // An example Named Command, doesn't need to remain once we start actually adding real things - // ALL Named Commands need to be defined AFTER subsystem initialization and BEFORE auto/controller configuration - NamedCommands.registerCommand("Set Drivetrain X", drivetrain.setXCommand()); - - // TODO Specify a default auto string once we have one - autoChooser = AutoBuilder.buildAutoChooser(); + drivetrain = new Drivetrain(new Pose2d()); driver = new CommandXboxController(OIConstants.kPrimaryXboxUSB); - operator = new CommandXboxController(OIConstants.kSecondaryXboxUSB); - - isTeleopTrigger = new Trigger(DriverStation::isTeleopEnabled); - - Optional alliance = DriverStation.getAlliance(); - - if (alliance.isEmpty() || alliance.get() == DriverStation.Alliance.Red) { - ampTagID = 5; - sourceTagID = 9; - speakerTag = 4; - } else { - ampTagID = 6; - sourceTagID = 1; - speakerTag = 8; - } - - setAmpAngle = false; configureBindings(); - shuffleboardSetup(); } - // The objective should be to have the subsystems expose methods that return commands - // that can be bound to the triggers provided by the CommandXboxController class. - // This mindset should help keep RobotContainer a little cleaner this year. - // This is not to say you can't trigger or command chain here (see driveCardnial drivetrain example), - // but generally reduce/avoid any situation where the keyword "new" is involved if you're working - // with a subsystem private void configureBindings() { - isTeleopTrigger.onTrue(new InstantCommand(() -> Shuffleboard.selectTab(kTeleopTabName))); - drivetrain.setDefaultCommand( drivetrain.teleopCommand( driver::getLeftY, @@ -141,161 +33,11 @@ public class RobotContainer { ) ); - //intake.setDefaultCommand(intake.intakeUpCommand()); - intake.setDefaultCommand(intake.manualPivot(operator::getLeftY, () -> 0.0)); - - shooter.setDefaultCommand( - shooter.angleSpeedsSetpoints( - () -> { - if (setAmpAngle) { - return ShooterConstants.kShooterAmpAngle; - } else { - return ShooterConstants.kShooterLoadAngle; - } - }, - () -> { - if (driver.getLeftTriggerAxis() > .25) { - return -1.0; - }else if(driver.getRightTriggerAxis() > 0.25){ - return 1.0; - } else { - return 0; - } - } - ) - ); - //shooter.setDefaultCommand(shooter.manualPivot(operator::getRightY, operator::getLeftTriggerAxis)); - - climber.setDefaultCommand(climber.stop()); - - indexer.setDefaultCommand(indexer.shootNote( - () -> { - if (driver.getLeftTriggerAxis() > .25) { - return -1.0; - }else { - return 0.0; - } - } - )); - - driver.povCenter().onFalse( - drivetrain.driveCardinal( - driver::getLeftY, - driver::getLeftX, - driver.getHID()::getPOV, - OIConstants.kTeleopDriveDeadband).until( - () -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0 - ) - ); - - // Lock on to the appropriate Amp AprilTag while still being able to strafe and drive forward and back - driver.leftBumper().onTrue( - drivetrain.driveAprilTagLock( - driver::getLeftY, - driver::getLeftX, - OIConstants.kTeleopDriveDeadband, - ampTagID - ).until( - () -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0 - ) - ); - - // Lock on to the appropriate Source AprilTag while still being able to strafe and drive forward and back - driver.b().onTrue( - drivetrain.driveAprilTagLock( - driver::getLeftY, - driver::getLeftX, - OIConstants.kTeleopDriveDeadband, - sourceTagID - ).until( - () -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0 - ) - ); - - // Lock on to the appropriate Speaker AprilTag while still being able to strafe and drive forward and back - driver.x().onTrue( - drivetrain.driveAprilTagLock( - driver::getLeftY, - driver::getLeftX, - OIConstants.kTeleopDriveDeadband, - speakerTag - ).until( - () -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0 - ) - ); - - // This was originally a run while held, not sure that's really necessary, change it if need be - driver.y().onTrue(drivetrain.zeroHeadingCommand()); - - // This was originally a run while held, not sure that's really necessary, change it if need be - driver.rightBumper().onTrue(drivetrain.setXCommand()); - - /* - * This has been added because interest has been expressed in trying field relative vs - * robot relative control. This should default to field relative, but give the option - * for the person practicing driving to push start on the driver's controller to quickly switch to - * the other style. - * - * If it becomes something that we need to switch prior to the start of the match, a different - * mechanism will need to be devised, this will work for now. - */ - driver.start().onTrue(drivetrain.toggleFieldRelativeControl()); - - operator.b().whileTrue(Commands.parallel(indexer.shootNote(() -> 1.0), shooter.ampHandoff(() -> driver.getRightTriggerAxis(), () -> { - if(operator.getRightTriggerAxis()>0.25){ - return true; - }else{ - return false; - } - }))); + driver.a().onTrue(drivetrain.zeroHeadingCommand()); - - //driver.leftBumper().toggleOnTrue(intake.intakeDownCommand()); - - operator.a().onTrue(new InstantCommand(() -> { setAmpAngle = true; })); - operator.a().onFalse(new InstantCommand(() -> { setAmpAngle = false; })); - - //operator.a().whileTrue(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterAmpAngle, 0, 0)); - - operator.y().whileTrue(Commands.parallel(climber.setSpeedWithSupplier(operator::getRightTriggerAxis), shooter.climbState())); - - driver.a().whileTrue(indexer.shootNote(() -> 1.0)); - - operator.back().toggleOnTrue(shooter.manualPivot( - () -> MathUtil.clamp(-operator.getLeftY(), -0.75, 0.75), - () -> MathUtil.clamp(operator.getRightTriggerAxis()-operator.getLeftTriggerAxis(), -0.75, 0.75) - )); - - driver.povDown().whileTrue(indexer.shootNote(() -> -1.0)); - - operator.start().onTrue(shooter.zeroEncoder()); - - } - - private void shuffleboardSetup() { - ShuffleboardTab autoTab = Shuffleboard.getTab(kAutoTabName); - autoTab.add("Autonomous Selector", autoChooser) - .withPosition(0, 0) - .withSize(2, 1) - .withWidget(BuiltInWidgets.kComboBoxChooser); - - - - //Always select the auto tab on startup - Shuffleboard.selectTab(kAutoTabName); - - ShuffleboardTab teleopTab = Shuffleboard.getTab(kTeleopTabName); - teleopTab.addBoolean("Field Relative Controls Enabled?", drivetrain::isFieldRelativeControl) - .withPosition(0, 0) - .withSize(2, 1) - .withWidget(BuiltInWidgets.kBooleanBox); - teleopTab.addBoolean("indexer beam break", indexer::getBeamBreak); - teleopTab.addBoolean("shooter beam break", shooter::getBeamBreak); - teleopTab.addDouble("shooter angle", shooter::getShooterAngle); - teleopTab.addDouble("intake angle", intake::getIntakeAngle); } public Command getAutonomousCommand() { - return autoChooser.getSelected(); + return null; } } diff --git a/src/main/java/frc/robot/constants/AutoConstants.java b/src/main/java/frc/robot/constants/AutoConstants.java deleted file mode 100644 index 181c727..0000000 --- a/src/main/java/frc/robot/constants/AutoConstants.java +++ /dev/null @@ -1,34 +0,0 @@ -package frc.robot.constants; - -import com.pathplanner.lib.util.HolonomicPathFollowerConfig; -import com.pathplanner.lib.util.PIDConstants; -import com.pathplanner.lib.util.ReplanningConfig; - -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.util.Units; - -public final class AutoConstants { - public static final double kMaxSpeedMetersPerSecond = 3.895; // max capable = 4.6 - public static final double kMaxAccelerationMetersPerSecondSquared = 3; // max capable = 7.4 - public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; - public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; - - public static final double kPXController = 1.05; - public static final double kPYController = 1.05; - public static final double kPThetaController = 0.95; // needs to be separate from heading control - - public static final double kPHeadingController = 0.02; // for heading control NOT PATHING - public static final double kDHeadingController = 0.0025; - - public static final HolonomicPathFollowerConfig kPFConfig = new HolonomicPathFollowerConfig( - new PIDConstants(kPXController), - new PIDConstants(kPThetaController), - kMaxSpeedMetersPerSecond, - Units.inchesToMeters(Math.sqrt(Math.pow(14-17.5, 2)+ Math.pow(14-1.75, 2))), - new ReplanningConfig() - ); - - // Constraint for the motion profiled robot angle controller - public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints( - kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared); -} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/ClimberConstants.java b/src/main/java/frc/robot/constants/ClimberConstants.java deleted file mode 100644 index 0fa8496..0000000 --- a/src/main/java/frc/robot/constants/ClimberConstants.java +++ /dev/null @@ -1,6 +0,0 @@ -package frc.robot.constants; - -public class ClimberConstants { - public static final int kClimberMotor1CANID = 14; - public static final int kClimberMotor2CANID = 15; -} 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 c280aa0..0000000 --- a/src/main/java/frc/robot/constants/IndexerConstants.java +++ /dev/null @@ -1,7 +0,0 @@ -package frc.robot.constants; - -public class IndexerConstants { - public static final int kIndexerID = 13; - - public static final int kIndexerBeamBreakChannel = 2; -} 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 3a0a547..0000000 --- a/src/main/java/frc/robot/constants/IntakeConstants.java +++ /dev/null @@ -1,22 +0,0 @@ -package frc.robot.constants; - -public class IntakeConstants { - public static final int kIntakePivotID = 10; - public static final int kIntakeRollerID = 12; - - public static final double kIntakePivotConversionFactor = 1/(20.0*(28.0/15.0)); - - public static final int kPivotCurrentLimit = 40; - - public static final double kPIntake = 0; - public static final double kIIntake = 0; - public static final double kDIntake = 0; - - public static final double kSFeedForward = 0; - public static final double kGFeedForward = 1.11; - public static final double kVFeedForward = 0.73; - - public static final double kStartingAngle = Math.toRadians(105.0); - public static final double kUpAngle = Math.toRadians(90.0); - public static final double kDownAngle = Math.toRadians(-34.0); -} diff --git a/src/main/java/frc/robot/constants/PhotonConstants.java b/src/main/java/frc/robot/constants/PhotonConstants.java deleted file mode 100644 index bc6cda2..0000000 --- a/src/main/java/frc/robot/constants/PhotonConstants.java +++ /dev/null @@ -1,17 +0,0 @@ -package frc.robot.constants; - -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.util.Units; - -public class PhotonConstants { - public static final String kCameraName = "Camera_Module_v1"; - - // TODO Set this to something real if visual pose estimation is used - public static final Transform3d kCameraTransform = new Transform3d(); - - // TODO The camera will be moved, this is an old value that needs to update - public static final double kCameraHeightMeters = .517525; - - // TODO The camera will be moved, this is an old value that needs to update - public static final double kCameraPitchRadians = Units.degreesToRadians(15); -} diff --git a/src/main/java/frc/robot/constants/ShooterConstants.java b/src/main/java/frc/robot/constants/ShooterConstants.java deleted file mode 100644 index eafe46e..0000000 --- a/src/main/java/frc/robot/constants/ShooterConstants.java +++ /dev/null @@ -1,34 +0,0 @@ -package frc.robot.constants; - -public class ShooterConstants { - - public static final int kTopShooterID = 9; - public static final int kBottomShooterID = 11; - public static final int kShooterPivotID = 16; - - public static final double kShooterP = 0.0; - public static final double kShooterI = 0.0; - public static final double kShooterD = 0.0; - - public static final double kShooterFF = 0.0; - - public static final double kShooterPivotP = 3.0; - public static final double kShooterPivotI = 0.0; - public static final double kShooterPivotD = 0.0; - - public static final double kShooterLoadAngle = Math.toRadians(-20.0); - public static final double kShooterAmpAngle = Math.toRadians(105.0); - - public static final double kPivotConversion = 1/(40.0*(28.0/15.0)); - - public static final double kSShooterPivotFF = 0.0; - public static final double kGShooterPivotFF = 0.33; - public static final double kVShooterPivotFF = 1.44; - - public static final double kMaxPivotSpeed = 0.0; - public static final double kMaxPivotAcceleration = 0.0; - - public static final int kShooterEncoderChannelA = 0; - public static final int kShooterEncoderChannelB = 1; - public static final int kShooterBeamBreakChannel = 3; -} diff --git a/src/main/java/frc/robot/interfaces/IAprilTagProvider.java b/src/main/java/frc/robot/interfaces/IAprilTagProvider.java deleted file mode 100644 index 09a0616..0000000 --- a/src/main/java/frc/robot/interfaces/IAprilTagProvider.java +++ /dev/null @@ -1,34 +0,0 @@ -package frc.robot.interfaces; - -import java.util.OptionalDouble; - -/** - * An interface which ensures a class can provide common AprilTag oriented - * information from various sources in a consistent way. - */ -public interface IAprilTagProvider { - /** - * A method to get the distance from the camera to the AprilTag specified - * - * @param id The ID of the AprilTag to give a distance to - * @param targetHeightMeters The height of the AprilTag off the ground, in meters - * @return The distance, in meters, to the target, or OptionalDouble.empty() if the tag is not present in the camera's view - */ - public OptionalDouble getTagDistanceFromCameraByID(int id, double targetHeightMeters); - - /** - * A method to get the pitch from the center of the image of a particular AprilTag - * - * @param id The ID of the AprilTag to get the pitch of - * @return The pitch, in degrees, of the target, or OptionalDouble.empty() if the tag is not present in the camera's view - */ - public OptionalDouble getTagPitchByID(int id); - - /** - * A method to get the yaw from the center of the image of a particular AprilTag - * - * @param id The ID of the AprilTag to get the yaw of - * @return The yaw, in degrees, of the target, or OptionalDouble.empty() if the tag is not present in the camera's view - */ - public OptionalDouble getTagYawByID(int id); -} diff --git a/src/main/java/frc/robot/interfaces/IVisualPoseProvider.java b/src/main/java/frc/robot/interfaces/IVisualPoseProvider.java deleted file mode 100644 index 1f6cf62..0000000 --- a/src/main/java/frc/robot/interfaces/IVisualPoseProvider.java +++ /dev/null @@ -1,27 +0,0 @@ -package frc.robot.interfaces; - -import java.util.Optional; - -import edu.wpi.first.math.geometry.Pose2d; - -/** - * An interface which ensures a class' ability to provide visual pose information - * in a consistent way - */ -public interface IVisualPoseProvider { - /** - * A record that can contain the two elements necessary for a WPILIB - * pose estimator to use the information from a vision system as part of a full - * robot pose estimation - */ - public record VisualPose(Pose2d visualPose, double timestamp) {} - - /** - * Return a VisualPose or null if an empty Optional if none is available. - * Implementation should provide an empty response if it's unable to provide - * a reliable pose, or any pose at all. - * - * @return An Optional containing a VisualPose, or empty if no VisualPose can reliably be provided - */ - public Optional getVisualPose(); -} diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java deleted file mode 100644 index d60bc30..0000000 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ /dev/null @@ -1,47 +0,0 @@ -package frc.robot.subsystems; - -import java.util.function.DoubleSupplier; - -import com.ctre.phoenix.motorcontrol.VictorSPXControlMode; -import com.ctre.phoenix.motorcontrol.can.VictorSPX; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constants.ClimberConstants; -import frc.robot.constants.ShooterConstants; - -public class Climber extends SubsystemBase { - private VictorSPX motor1; - private VictorSPX motor2; - - private DoubleSupplier shooterAngle; - - public Climber(DoubleSupplier shooterAngle) { - motor1 = new VictorSPX(ClimberConstants.kClimberMotor1CANID); - motor2 = new VictorSPX(ClimberConstants.kClimberMotor2CANID); - - - motor2.follow(motor1); - motor1.setInverted(true); - motor2.setInverted(true); - this.shooterAngle = shooterAngle; - } - - public Command setSpeedWithSupplier(DoubleSupplier speed) { - return run(() -> { - - motor1.set(VictorSPXControlMode.PercentOutput, speed.getAsDouble()); - - }); - } - - public Command setSpeed(double speed) { - return run(() -> { - motor1.set(VictorSPXControlMode.PercentOutput, speed); - }); - } - - public Command stop() { - return setSpeed(0); - } -} diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index ee790f8..c553719 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -1,7 +1,6 @@ package frc.robot.subsystems; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Pose2d; @@ -12,26 +11,15 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.util.WPIUtilJNI; -import java.util.Optional; -import java.util.OptionalDouble; -import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import com.kauailabs.navx.frc.AHRS; -import com.pathplanner.lib.auto.AutoBuilder; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.SPI; -import frc.robot.constants.AutoConstants; import frc.robot.constants.DrivetrainConstants; import frc.robot.utilities.MAXSwerveModule; import frc.robot.utilities.SwerveUtils; -import frc.robot.interfaces.IAprilTagProvider; -import frc.robot.interfaces.IVisualPoseProvider; -import frc.robot.interfaces.IVisualPoseProvider.VisualPose; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.PIDCommand; -import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Drivetrain extends SubsystemBase { @@ -47,15 +35,9 @@ public class Drivetrain extends SubsystemBase { private final SlewRateLimiter m_magLimiter; private final SlewRateLimiter m_rotLimiter; - private final IAprilTagProvider m_aprilTagProvider; - - private final IVisualPoseProvider m_visualPoseProvider; - // Odometry class for tracking robot pose private final SwerveDrivePoseEstimator m_poseEstimator; - private boolean fieldRelativeControl; - // Slew rate filter variables for controlling lateral acceleration private double m_currentRotation; private double m_currentTranslationDir; @@ -63,10 +45,8 @@ public class Drivetrain extends SubsystemBase { private double m_prevTime; - private double gyroOffset; - /** Creates a new DriveSubsystem. */ - public Drivetrain(Pose2d initialPose, IAprilTagProvider aprilTagProvider, IVisualPoseProvider visualPoseProvider) { + public Drivetrain(Pose2d initialPose) { m_frontLeft = new MAXSwerveModule( DrivetrainConstants.kFrontLeftDrivingCanId, DrivetrainConstants.kFrontLeftTurningCanId, @@ -107,36 +87,10 @@ public class Drivetrain extends SubsystemBase { }, initialPose ); - - m_aprilTagProvider = aprilTagProvider; - - m_visualPoseProvider = visualPoseProvider; - - fieldRelativeControl = true; - m_currentRotation = 0.0; m_currentTranslationDir = 0.0; m_currentTranslationMag = 0.0; m_prevTime = WPIUtilJNI.now() * 1e-6; - - AutoBuilder.configureHolonomic( - this::getPose, - this::resetOdometry, - this::getCurrentChassisSpeeds, - this::driveWithChassisSpeeds, - AutoConstants.kPFConfig, - () -> { - Optional alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; - }, - this - ); - - gyroOffset = DrivetrainConstants.kRobotStartOffset; - } @Override @@ -151,32 +105,6 @@ public class Drivetrain extends SubsystemBase { m_rearRight.getPosition() } ); - - if (m_visualPoseProvider != null) { - Optional vPose = m_visualPoseProvider.getVisualPose(); - - if (vPose.isPresent()) { - m_poseEstimator.addVisionMeasurement(vPose.get().visualPose(), vPose.get().timestamp()); - } - } - } - - public double getRobotStartOffset() { - return gyroOffset; - } - - public void setRobotStartOffset(double offset) { - gyroOffset = offset; - } - - public boolean isFieldRelativeControl() { - return fieldRelativeControl; - } - - public Command toggleFieldRelativeControl() { - return runOnce(() -> { - fieldRelativeControl = !fieldRelativeControl; - }); } /** @@ -188,14 +116,6 @@ public class Drivetrain extends SubsystemBase { return m_poseEstimator.getEstimatedPosition(); } - public double getPoseX(){ - return m_poseEstimator.getEstimatedPosition().getX(); - } - - public double getPoseY(){ - return m_poseEstimator.getEstimatedPosition().getY(); - } - /** * Resets the odometry to the specified pose. * @@ -233,7 +153,7 @@ public class Drivetrain extends SubsystemBase { * field. * @param rateLimit Whether to enable rate limiting for smoother control. */ - public void drive(double xSpeed, double ySpeed, double rot, BooleanSupplier fieldRelative, boolean rateLimit) { + public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative, boolean rateLimit) { double xSpeedCommanded; double ySpeedCommanded; @@ -287,8 +207,8 @@ public class Drivetrain extends SubsystemBase { double rotDelivered = m_currentRotation * DrivetrainConstants.kMaxAngularSpeed; var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates( - fieldRelative.getAsBoolean() - ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, m_poseEstimator.getEstimatedPosition().getRotation()) + fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, Rotation2d.fromDegrees(getGyroAngle())) : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered)); SwerveDriveKinematics.desaturateWheelSpeeds( swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond); @@ -313,88 +233,12 @@ public class Drivetrain extends SubsystemBase { -MathUtil.applyDeadband(xSpeed.getAsDouble(), deadband), -MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband), -MathUtil.applyDeadband(rotation.getAsDouble(), deadband), - () -> fieldRelativeControl, + true, true ); }); } - public Command driveCardinal(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier pov, double deadband) { - PIDController controller = new PIDController( - AutoConstants.kPHeadingController, - 0, - AutoConstants.kDHeadingController - ); - controller.enableContinuousInput(-180, 180); - - return new PIDCommand( - controller, - this::getHeading, - pov::getAsDouble, - (output) -> { - this.drive( - -MathUtil.applyDeadband(xSpeed.getAsDouble(), deadband), - -MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband), - output, - () -> fieldRelativeControl, - true - ); - }, - this); - } - - public Command driveAprilTagLock(DoubleSupplier xSpeed, DoubleSupplier ySpeed, double deadband, int tagID) { - if (m_aprilTagProvider == null) { - return new PrintCommand("No AprilTag Provider Available"); - } - - // TODO The process variable is different here than what these constants are used for, may need to use something different - PIDController controller = new PIDController( - AutoConstants.kPHeadingController, - 0, - AutoConstants.kDHeadingController - ); - - return new PIDCommand( - controller, - () -> { - OptionalDouble tagYaw = m_aprilTagProvider.getTagYawByID(tagID); - - if (tagYaw.isEmpty()) { - return 0; - } - - return tagYaw.getAsDouble(); - }, - () -> { return 0; }, - (output) -> { - // TODO Field relative or no? What makes sense here, since the intent is to orient to the tag, not the field, it shouldn't be affected by the drivers mode choice I don't think. - this.drive( - -MathUtil.applyDeadband(xSpeed.getAsDouble(), deadband), - -MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband), - output, - () -> fieldRelativeControl, - true - ); - }, - this - ); - } - - /** - * Sets the wheels into an X formation to prevent movement. - */ - public void setX() { - m_frontLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45))); - m_frontRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45))); - m_rearLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45))); - m_rearRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45))); - } - - public Command setXCommand() { - return runOnce(this::setX); - } - /** * Sets the swerve ModuleStates. * @@ -426,10 +270,6 @@ public class Drivetrain extends SubsystemBase { return runOnce(this::zeroHeading); } - public void offsetZero(Pose2d angle){ - resetOdometry(angle); - } - /** * Returns the heading of the robot. * diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java deleted file mode 100644 index a07eed8..0000000 --- a/src/main/java/frc/robot/subsystems/Indexer.java +++ /dev/null @@ -1,60 +0,0 @@ -package frc.robot.subsystems; - -import com.revrobotics.CANSparkMax; - -import java.util.function.DoubleSupplier; - -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkLowLevel.MotorType; - -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constants.IndexerConstants; - -public class Indexer extends SubsystemBase{ - - private CANSparkMax indexerMotor; - private DigitalInput indexerBeamBreak; - - public Indexer(){ - indexerMotor = new CANSparkMax(IndexerConstants.kIndexerID, MotorType.kBrushed); - - indexerMotor.setSmartCurrentLimit(40); - indexerMotor.setIdleMode(IdleMode.kBrake); - indexerMotor.burnFlash(); - - indexerBeamBreak = new DigitalInput(IndexerConstants.kIndexerBeamBreakChannel); - } - - public Command autoIndexing(){ - return run(() -> { - if(!indexerBeamBreak.get()){ - indexerMotor.set(0.75); - }else if(indexerBeamBreak.get()){ - indexerMotor.set(0.0); - } - - }); - } - - public Command advanceNote(){ - return run(() -> { - if(indexerBeamBreak.get()){ - indexerMotor.set(0.75); - }else{ - indexerMotor.set(0.75); - } - }); - } - - public Command shootNote(DoubleSupplier indexerSpeed){ - return run(() -> { - indexerMotor.set(indexerSpeed.getAsDouble()); - }); - } - - public boolean getBeamBreak(){ - return indexerBeamBreak.get(); - } -} diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java deleted file mode 100644 index 17f2649..0000000 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ /dev/null @@ -1,115 +0,0 @@ -package frc.robot.subsystems; - -import java.util.function.DoubleSupplier; - -import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkLowLevel.MotorType; - -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constants.IntakeConstants; - -public class Intake extends SubsystemBase{ - private PIDController intakeAnglePID; - - private CANSparkMax intakeRoller; - private CANSparkMax intakePivot; - - private RelativeEncoder intakeEncoder; - - private ArmFeedforward intakeFeedForward; - - public Intake(){ - intakeRoller = new CANSparkMax(IntakeConstants.kIntakeRollerID, MotorType.kBrushless); - - intakeRoller.setSmartCurrentLimit(60); - intakeRoller.setIdleMode(IdleMode.kBrake); - intakeRoller.burnFlash(); - - intakePivot = new CANSparkMax(IntakeConstants.kIntakePivotID, MotorType.kBrushless); - - intakePivot.setIdleMode(IdleMode.kBrake); - intakePivot.setSmartCurrentLimit(IntakeConstants.kPivotCurrentLimit); - intakePivot.burnFlash(); - - intakeFeedForward = new ArmFeedforward( - IntakeConstants.kSFeedForward, - IntakeConstants.kGFeedForward, - IntakeConstants.kVFeedForward - ); - - intakeEncoder = intakePivot.getEncoder(); - intakeEncoder.setPosition(IntakeConstants.kStartingAngle); - intakeEncoder.setPositionConversionFactor(IntakeConstants.kIntakePivotConversionFactor); - - intakeAnglePID = new PIDController( - IntakeConstants.kPIntake, - IntakeConstants.kIIntake, - IntakeConstants.kDIntake - ); - - } - - public Command intakeDownCommand() { - return run(() -> { - intakeRoller.set(1.0); - - intakePivot.setVoltage( - intakeAnglePID.calculate( - intakeEncoder.getPosition(), - IntakeConstants.kDownAngle - ) + intakeFeedForward.calculate( - IntakeConstants.kDownAngle, - intakeEncoder.getVelocity() - ) - ); - }); - } - - public Command manualPivot(DoubleSupplier pivotPower, DoubleSupplier rollerSpinny){ - return run(() ->{ - intakePivot.set(pivotPower.getAsDouble()); - intakeRoller.set(rollerSpinny.getAsDouble()); - }); - } - - public Command climbingStateCommand() { - return run(() -> { - intakeRoller.set(0.0); - - intakePivot.setVoltage( - intakeAnglePID.calculate( - intakeEncoder.getPosition(), - IntakeConstants.kDownAngle - ) + intakeFeedForward.calculate( - IntakeConstants.kDownAngle, - intakeEncoder.getVelocity() - ) - ); - }); - } - - public Command intakeUpCommand() { - return run(() -> { - intakeRoller.set(0.0); - - intakePivot.setVoltage( - intakeAnglePID.calculate( - intakeEncoder.getPosition(), - IntakeConstants.kUpAngle - ) + intakeFeedForward.calculate( - IntakeConstants.kUpAngle, - intakeEncoder.getVelocity() - ) - ); - }); - } - - public double getIntakeAngle(){ - return intakeEncoder.getPosition(); - } -} diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java deleted file mode 100644 index 40f73d0..0000000 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ /dev/null @@ -1,151 +0,0 @@ -package frc.robot.subsystems; - -import java.util.function.BooleanSupplier; -import java.util.function.DoubleSupplier; - -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkLowLevel.MotorType; - -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constants.ShooterConstants; - -public class Shooter extends SubsystemBase{ - private CANSparkMax topShooter; - private CANSparkMax bottomShooter; - - private CANSparkMax shooterPivot; - - private Encoder pivotEncoder; - - private PIDController shooterPivotPID; - private ArmFeedforward shooterPivotFF; - - private DigitalInput shooterBeamBreak; - - private boolean indexerBeamBreak; - - public Shooter(BooleanSupplier indexerBeamBreak){ - topShooter = new CANSparkMax(ShooterConstants.kTopShooterID, MotorType.kBrushless); - bottomShooter = new CANSparkMax(ShooterConstants.kBottomShooterID, MotorType.kBrushless); - - shooterPivot = new CANSparkMax(ShooterConstants.kShooterPivotID, MotorType.kBrushless); - shooterPivot.setInverted(true); - - /* - topPID = topShooter.getPIDController(); - topPID.setFeedbackDevice(topEncoder); - bottomPID = bottomShooter.getPIDController(); - bottomPID.setFeedbackDevice(bottomEncoder); - */ - - shooterPivot.setSmartCurrentLimit(50); - topShooter.setSmartCurrentLimit(40); - bottomShooter.setSmartCurrentLimit(40); - - pivotEncoder = new Encoder(0, 1); - pivotEncoder.setDistancePerPulse(ShooterConstants.kPivotConversion); - - shooterBeamBreak = new DigitalInput(ShooterConstants.kShooterBeamBreakChannel); - - topShooter.setIdleMode(IdleMode.kCoast); - bottomShooter.setIdleMode(IdleMode.kCoast); - - bottomShooter.burnFlash(); - shooterPivot.burnFlash(); - topShooter.burnFlash(); - - shooterPivotPID = new PIDController( - ShooterConstants.kShooterPivotP, - ShooterConstants.kShooterPivotI, - ShooterConstants.kShooterPivotD - ); - shooterPivotFF = new ArmFeedforward(ShooterConstants.kSShooterPivotFF, ShooterConstants.kGShooterPivotFF, ShooterConstants.kVShooterPivotFF); - - } - - /* - public Command angleSpeedsSetpoints(double setpointAngle, double topRPM, double bottomRPM){ - return run(()-> { - angleAndSpeedControl(setpointAngle, topRPM, bottomRPM); - }); - }*/ - - public Command angleSpeedsSetpoints(DoubleSupplier setpointAngle, DoubleSupplier speed){ - return run(()-> { - angleAndSpeedControl(setpointAngle.getAsDouble(), speed.getAsDouble(), speed.getAsDouble()); - }); - } - - public Command angleSpeedsSetpoints(DoubleSupplier setpointAngle, double topRPM, double bottomRPM){ - return run(()-> { - angleAndSpeedControl(setpointAngle.getAsDouble(), topRPM, bottomRPM); - }); - } - - private void angleAndSpeedControl(double setpointAngle, double topRPM, double bottomRPM){ - shooterPivot.setIdleMode(IdleMode.kBrake); - - shooterPivot.setVoltage( - shooterPivotPID.calculate(getShooterAngle(), setpointAngle) + - shooterPivotFF.calculate(setpointAngle, 0.0)); - - //topPID.setReference(topRPM, CANSparkMax.ControlType.kVelocity); - //bottomPID.setReference(bottomRPM, CANSparkMax.ControlType.kVelocity); - topShooter.set(topRPM); - bottomShooter.set(bottomRPM); - } - - public Command ampHandoff(DoubleSupplier shootNoteSpeed, BooleanSupplier ampAngleOrNah){ - return run(() -> { - - shooterPivot.setIdleMode(IdleMode.kBrake); - - shooterPivot.setVoltage( - shooterPivotPID.calculate(getShooterAngle(), ShooterConstants.kShooterLoadAngle) + - shooterPivotFF.calculate(ShooterConstants.kShooterLoadAngle, 0.0)); - - if(shooterBeamBreak.get() || indexerBeamBreak){ - angleAndSpeedControl(ShooterConstants.kShooterLoadAngle, 0.25, 0.25); - }else{ - angleAndSpeedControl(ShooterConstants.kShooterLoadAngle, shootNoteSpeed.getAsDouble(), shootNoteSpeed.getAsDouble()); - } - }); - } - - public Command climbState(){ - return run(() -> { - shooterPivot.setIdleMode(IdleMode.kCoast); - shooterPivot.set(0.0); - }); - } - - public double getShooterAngle(){ - return pivotEncoder.getDistance() + ShooterConstants.kShooterLoadAngle; - } - - public Command zeroEncoder(){ - return run(() -> { - pivotEncoder.reset(); - }); - } - - public Boolean getBeamBreak(){ - return shooterBeamBreak.get(); - } - - public Command manualPivot(DoubleSupplier pivotPower, DoubleSupplier spinny){ - return run(() ->{ - shooterPivot.set(pivotPower.getAsDouble()); - topShooter.set(spinny.getAsDouble()); - bottomShooter.set(spinny.getAsDouble()); - }); - } - - -} diff --git a/src/main/java/frc/robot/utilities/PhotonVision.java b/src/main/java/frc/robot/utilities/PhotonVision.java deleted file mode 100644 index 591e219..0000000 --- a/src/main/java/frc/robot/utilities/PhotonVision.java +++ /dev/null @@ -1,135 +0,0 @@ -package frc.robot.utilities; - -import java.io.IOException; -import java.util.List; -import java.util.Optional; -import java.util.OptionalDouble; - -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.PhotonUtils; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.util.Units; -import frc.robot.interfaces.IAprilTagProvider; -import frc.robot.interfaces.IVisualPoseProvider; - -public class PhotonVision implements IAprilTagProvider,IVisualPoseProvider { - - private final PhotonCamera camera; - - private final PhotonPoseEstimator photonPoseEstimator; - - private final double cameraHeightMeters; - private final double cameraPitchRadians; - - public PhotonVision(String cameraName, Transform3d robotToCam, double cameraHeightMeters, double cameraPitchRadians) throws IOException { - camera = new PhotonCamera(cameraName); - - photonPoseEstimator = new PhotonPoseEstimator( - AprilTagFieldLayout.loadFromResource( - AprilTagFields.k2024Crescendo.m_resourceFile - ), - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - camera, - robotToCam - ); - - this.cameraHeightMeters = cameraHeightMeters; - this.cameraPitchRadians = cameraPitchRadians; - } - - @Override - public Optional getVisualPose() { - Optional pose = photonPoseEstimator.update(); - - if (pose.isEmpty()) { - return Optional.empty(); - } - - return Optional.of( - new VisualPose( - pose.get().estimatedPose.toPose2d(), - pose.get().timestampSeconds - ) - ); - } - - @Override - public OptionalDouble getTagDistanceFromCameraByID(int id, double targetHeightMeters) { - PhotonPipelineResult result = camera.getLatestResult(); - - if (!result.hasTargets()) { - return OptionalDouble.empty(); - } - - Optional desiredTarget = getTargetFromList(result.getTargets(), id); - - if (desiredTarget.isEmpty()) { - return OptionalDouble.empty(); - } - - return OptionalDouble.of( - PhotonUtils.calculateDistanceToTargetMeters( - cameraHeightMeters, - targetHeightMeters, - cameraPitchRadians, - Units.degreesToRadians(desiredTarget.get().getPitch())) - ); - } - - @Override - public OptionalDouble getTagPitchByID(int id) { - PhotonPipelineResult result = camera.getLatestResult(); - - if (!result.hasTargets()) { - return OptionalDouble.empty(); - } - - Optional desiredTarget = getTargetFromList(result.getTargets(), id); - - if (desiredTarget.isEmpty()) { - return OptionalDouble.empty(); - } - - return OptionalDouble.of( - desiredTarget.get().getPitch() - ); - } - - @Override - public OptionalDouble getTagYawByID(int id) { - PhotonPipelineResult result = camera.getLatestResult(); - - if (!result.hasTargets()) { - return OptionalDouble.empty(); - } - - Optional desiredTarget = getTargetFromList(result.getTargets(), id); - - if (desiredTarget.isEmpty()) { - return OptionalDouble.empty(); - } - - return OptionalDouble.of( - desiredTarget.get().getYaw() - ); - } - - private Optional getTargetFromList(List targets, int id) { - for (PhotonTrackedTarget target : targets) { - if (target.getFiducialId() == id) { - return Optional.of(target); - } - } - - return Optional.empty(); - } - -}