diff --git a/.vscode/settings.json b/.vscode/settings.json index 612cdd0..1745ba0 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -56,5 +56,6 @@ "edu.wpi.first.math.proto.*", "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", - ] + ], + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" } diff --git a/Updates/1-30-2025.txt b/Updates/1-30-2025.txt deleted file mode 100644 index f446164..0000000 --- a/Updates/1-30-2025.txt +++ /dev/null @@ -1,6 +0,0 @@ -We got the drive train working! Here's how we got there: -1. We installed the NavX2 to the RoboRIO, so we have a working gyro. -2. We updated CAN values in our code so that it communicates properly. -3. We got many errors when trying to run, so we changed the gear ratios from 0 to a random number. -4. We also commented out the path planner methods since they were not initialized. -5. Basically, a lot of bug fixing did the trick. \ No newline at end of file diff --git a/build.gradle b/build.gradle index 1945af5..81c0f3b 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.2.1" + id "edu.wpi.first.GradleRIO" version "2025.1.1" } java { diff --git a/settings.gradle b/settings.gradle index c493958..969c7b0 100644 --- a/settings.gradle +++ b/settings.gradle @@ -20,8 +20,8 @@ pluginManagement { } def frcHomeMaven = new File(frcHome, 'maven') maven { - name = 'frcHome' - url = frcHomeMaven + name 'frcHome' + url frcHomeMaven } } } diff --git a/src/main/deploy/pathplanner/autos/JUST SHOOT!!.auto b/src/main/deploy/pathplanner/autos/JUST SHOOT!!.auto deleted file mode 100644 index 8190ebe..0000000 --- a/src/main/deploy/pathplanner/autos/JUST SHOOT!!.auto +++ /dev/null @@ -1,38 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 2, - "y": 2 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShootNote" - } - }, - { - "type": "wait", - "data": { - "waitTime": 5.0 - } - } - ] - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/One Note Amp Side.auto b/src/main/deploy/pathplanner/autos/One Note Amp Side.auto deleted file mode 100644 index 4f2757c..0000000 --- a/src/main/deploy/pathplanner/autos/One Note Amp Side.auto +++ /dev/null @@ -1,44 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.7275269251393202, - "y": 6.770915422172725 - }, - "rotation": 56.9832471271642 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShootNote" - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "Left to left note path" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/One Note Source Side.auto b/src/main/deploy/pathplanner/autos/One Note Source Side.auto deleted file mode 100644 index 7911c05..0000000 --- a/src/main/deploy/pathplanner/autos/One Note Source Side.auto +++ /dev/null @@ -1,44 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.7392210795817603, - "y": 4.350225452587657 - }, - "rotation": -58.5704343851614 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShootNote" - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "Right to right note path" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Two Note Center Speaker.auto b/src/main/deploy/pathplanner/autos/Two Note Center Speaker.auto deleted file mode 100644 index 0852e8c..0000000 --- a/src/main/deploy/pathplanner/autos/Two Note Center Speaker.auto +++ /dev/null @@ -1,57 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.44, - "y": 5.5196408968316515 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShootNote" - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - } - ] - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Center to center note path" - } - }, - { - "type": "named", - "data": { - "name": "IntakeNote" - } - } - ] - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Two Note Center auto.auto b/src/main/deploy/pathplanner/autos/Two Note Center auto.auto deleted file mode 100644 index 1f50ddd..0000000 --- a/src/main/deploy/pathplanner/autos/Two Note Center auto.auto +++ /dev/null @@ -1,50 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.44, - "y": 5.5196408968316515 - }, - "rotation": 0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShootNote" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Center to center note path" - } - }, - { - "type": "named", - "data": { - "name": "IntakeNote" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ShootNote" - } - } - ] - } - }, - "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/Amp to Note.path b/src/main/deploy/pathplanner/paths/Amp to Note.path deleted file mode 100644 index 5766b35..0000000 --- a/src/main/deploy/pathplanner/paths/Amp to Note.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.82, - "y": 7.53 - }, - "prevControl": null, - "nextControl": { - "x": 1.87, - "y": 7.53 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.6217429011434596, - "y": 6.998240302786913 - }, - "prevControl": { - "x": 2.57174290114346, - "y": 6.998240302786913 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": "Double Amp Auto", - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center to center note path.path b/src/main/deploy/pathplanner/paths/Center to center note path.path deleted file mode 100644 index f0dcef2..0000000 --- a/src/main/deploy/pathplanner/paths/Center to center note path.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.44, - "y": 5.5196408968316515 - }, - "prevControl": null, - "nextControl": { - "x": 2.0283806616115356, - "y": 5.527514849608872 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.60221165235483, - "y": 5.5196408968316515 - }, - "prevControl": { - "x": 2.7022116523548307, - "y": 5.5196408968316515 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left to left note path.path b/src/main/deploy/pathplanner/paths/Left to left note path.path deleted file mode 100644 index 64c488c..0000000 --- a/src/main/deploy/pathplanner/paths/Left to left note path.path +++ /dev/null @@ -1,74 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.7275269251393202, - "y": 6.770915422172725 - }, - "prevControl": null, - "nextControl": { - "x": 1.1134340217398382, - "y": 6.595503105536126 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.6279768172071953, - "y": 7.1217400554459225 - }, - "prevControl": { - "x": 1.5572286622646418, - "y": 7.1217400554459225 - }, - "nextControl": { - "x": 1.698724972149747, - "y": 7.1217400554459225 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.5868974814872705, - "y": 7.016492665463964 - }, - "prevControl": { - "x": 2.142519612674553, - "y": 6.946327738809324 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1, - "rotationDegrees": 0, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 60.57254359681032, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Middle to Speaker.path b/src/main/deploy/pathplanner/paths/Middle to Speaker.path deleted file mode 100644 index 8410cf9..0000000 --- a/src/main/deploy/pathplanner/paths/Middle to Speaker.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 8.0, - "y": 0.77 - }, - "prevControl": null, - "nextControl": { - "x": 8.0, - "y": 0.27000000000000013 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.73, - "y": 4.372744601345059 - }, - "prevControl": { - "x": 0.6799999999999999, - "y": 4.372744601345059 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -60.0, - "rotateFast": false - }, - "reversed": false, - "folder": "Triple Right Speaker Auto", - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Note to Amp.path b/src/main/deploy/pathplanner/paths/Note to Amp.path deleted file mode 100644 index 0be438d..0000000 --- a/src/main/deploy/pathplanner/paths/Note to Amp.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.62, - "y": 7.0 - }, - "prevControl": null, - "nextControl": { - "x": 2.67, - "y": 7.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.82, - "y": 7.53 - }, - "prevControl": { - "x": 1.7700000000000002, - "y": 7.53 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -90.0, - "rotateFast": false - }, - "reversed": false, - "folder": "Double Amp Auto", - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Note to Speaker.path b/src/main/deploy/pathplanner/paths/Right Note to Speaker.path deleted file mode 100644 index cdb4e09..0000000 --- a/src/main/deploy/pathplanner/paths/Right Note to Speaker.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.58, - "y": 4.1 - }, - "prevControl": null, - "nextControl": { - "x": 2.6310000000000002, - "y": 4.1 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.7265787504547981, - "y": 4.372744601345059 - }, - "prevControl": { - "x": 0.6765787504547981, - "y": 4.372744601345059 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -60.0, - "rotateFast": false - }, - "reversed": false, - "folder": "Triple Right Speaker Auto", - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right to right note path.path b/src/main/deploy/pathplanner/paths/Right to right note path.path deleted file mode 100644 index 8bc8345..0000000 --- a/src/main/deploy/pathplanner/paths/Right to right note path.path +++ /dev/null @@ -1,58 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.7392210795817603, - "y": 4.350225452587657 - }, - "prevControl": null, - "nextControl": { - "x": 1.7392210795817582, - "y": 4.350225452587657 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.976958421819838, - "y": 1.8737338473250418 - }, - "prevControl": { - "x": 1.976958421819838, - "y": 1.8737338473250418 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": -65.73217158836104, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -52.88313931672987, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": -60.75117366345301, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Speaker to Middle .path b/src/main/deploy/pathplanner/paths/Speaker to Middle .path deleted file mode 100644 index bf9b367..0000000 --- a/src/main/deploy/pathplanner/paths/Speaker to Middle .path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.8652492980661636, - "y": 4.215584647385511 - }, - "prevControl": null, - "nextControl": { - "x": 0.8652492980661636, - "y": 3.215584647385511 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.0, - "y": 0.7673103634495558 - }, - "prevControl": { - "x": 7.95, - "y": 0.7673103634495558 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": "Triple Right Speaker Auto", - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Speaker to Right Note.path b/src/main/deploy/pathplanner/paths/Speaker to Right Note.path deleted file mode 100644 index d4dcb21..0000000 --- a/src/main/deploy/pathplanner/paths/Speaker to Right Note.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.87, - "y": 4.22 - }, - "prevControl": null, - "nextControl": { - "x": 0.92, - "y": 4.22 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.5755193852730045, - "y": 4.095403506122328 - }, - "prevControl": { - "x": 2.5255193852730047, - "y": 4.095403506122328 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": "Triple Right Speaker Auto", - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Start to Amp.path b/src/main/deploy/pathplanner/paths/Start to Amp.path deleted file mode 100644 index 541b5ae..0000000 --- a/src/main/deploy/pathplanner/paths/Start to Amp.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.4291761916857166, - "y": 7.090687334527822 - }, - "prevControl": null, - "nextControl": { - "x": 1.4291761916857166, - "y": 7.090687334527822 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.8174537249975398, - "y": 7.525188383710101 - }, - "prevControl": { - "x": 1.7674537249975397, - "y": 7.525188383710101 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -90.0, - "rotateFast": false - }, - "reversed": false, - "folder": "Double Amp Auto", - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Start to Speaker.path b/src/main/deploy/pathplanner/paths/Start to Speaker.path deleted file mode 100644 index e052798..0000000 --- a/src/main/deploy/pathplanner/paths/Start to Speaker.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.43, - "y": 4.1 - }, - "prevControl": null, - "nextControl": { - "x": 1.48, - "y": 4.1 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.73, - "y": 4.37 - }, - "prevControl": { - "x": 0.6799999999999999, - "y": 4.37 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -60.0, - "rotateFast": false - }, - "reversed": false, - "folder": "Triple Right Speaker Auto", - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/swerve/controllerproperties.json b/src/main/deploy/swerve/controllerproperties.json deleted file mode 100644 index d6d2856..0000000 --- a/src/main/deploy/swerve/controllerproperties.json +++ /dev/null @@ -1,8 +0,0 @@ -{ - "angleJoystickRadiusDeadband": 0.5, - "heading": { - "p": 0.4, - "i": 0, - "d": 0.01 - } - } \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/backleft.json b/src/main/deploy/swerve/modules/backleft.json deleted file mode 100644 index 0ae525e..0000000 --- a/src/main/deploy/swerve/modules/backleft.json +++ /dev/null @@ -1,26 +0,0 @@ -{ - "drive": { - "type": "sparkmax_neo", - "id": 12, - "canbus": null - }, - "angle": { - "type": "sparkmax_neo", - "id": 15, - "canbus": null - }, - "encoder": { - "type": "cancoder", - "id": 20, - "canbus": null - }, - "inverted": { - "drive": false, - "angle": false - }, - "absoluteEncoderOffset": -18.281, - "location": { - "front": -12, - "left": -12 - } - } \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/backright.json b/src/main/deploy/swerve/modules/backright.json deleted file mode 100644 index 7166249..0000000 --- a/src/main/deploy/swerve/modules/backright.json +++ /dev/null @@ -1,26 +0,0 @@ -{ - "drive": { - "type": "sparkmax_neo", - "id": 13, - "canbus": null - }, - "angle": { - "type": "sparkmax_neo", - "id": 14, - "canbus": null - }, - "encoder": { - "type": "cancoder", - "id": 18, - "canbus": null - }, - "inverted": { - "drive": false, - "angle": false - }, - "absoluteEncoderOffset": -18.281, - "location": { - "front": -12, - "left": -12 - } - } \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/frontleft.json b/src/main/deploy/swerve/modules/frontleft.json deleted file mode 100644 index 9b72abf..0000000 --- a/src/main/deploy/swerve/modules/frontleft.json +++ /dev/null @@ -1,26 +0,0 @@ -{ - "drive": { - "type": "sparkmax_neo", - "id": 16, - "canbus": null - }, - "angle": { - "type": "sparkmax_neo", - "id": 11, - "canbus": null - }, - "encoder": { - "type": "cancoder", - "id": 19, - "canbus": null - }, - "inverted": { - "drive": false, - "angle": false - }, - "absoluteEncoderOffset": -18.281, - "location": { - "front": -12, - "left": -12 - } - } \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/frontright.json b/src/main/deploy/swerve/modules/frontright.json deleted file mode 100644 index 057452b..0000000 --- a/src/main/deploy/swerve/modules/frontright.json +++ /dev/null @@ -1,26 +0,0 @@ -{ - "drive": { - "type": "sparkmax_neo", - "id": 10, - "canbus": null - }, - "angle": { - "type": "sparkmax_neo", - "id": 17, - "canbus": null - }, - "encoder": { - "type": "cancoder", - "id": 21, - "canbus": null - }, - "inverted": { - "drive": false, - "angle": false - }, - "absoluteEncoderOffset": -18.281, - "location": { - "front": -12, - "left": -12 - } - } \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/physicalproperties.json b/src/main/deploy/swerve/modules/physicalproperties.json deleted file mode 100644 index 2e39c75..0000000 --- a/src/main/deploy/swerve/modules/physicalproperties.json +++ /dev/null @@ -1,24 +0,0 @@ -{ - "optimalVoltage": 12, - "robotMass": 110.2311, - "wheelGripCoefficientOfFriction": 1.19, - "currentLimit": { - "drive": 40, - "angle": 20 - }, - "conversionFactors": { - "angle": { - "gearRatio": 21.43, - "factor": 16.8 - }, - "drive": { - "diameter": 4, - "gearRatio": 6.75, - "factor": 0.047 - } - }, - "rampRate": { - "drive": 0.25, - "angle": 0.25 - } - } \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/pidfproperties.json b/src/main/deploy/swerve/modules/pidfproperties.json deleted file mode 100644 index 1e00204..0000000 --- a/src/main/deploy/swerve/modules/pidfproperties.json +++ /dev/null @@ -1,16 +0,0 @@ -{ - "drive": { - "p": 0.0020645, - "i": 0, - "d": 0, - "f": 0, - "iz": 0 - }, - "angle": { - "p": 0.01, - "i": 0, - "d": 0, - "f": 0, - "iz": 0 - } - } \ No newline at end of file diff --git a/src/main/deploy/swerve/swervedrive.json b/src/main/deploy/swerve/swervedrive.json deleted file mode 100644 index 8d9cae0..0000000 --- a/src/main/deploy/swerve/swervedrive.json +++ /dev/null @@ -1,14 +0,0 @@ -{ - "imu": { - "type": "navx", - "id": 0, - "canbus": null - }, - "invertedIMU": true, - "modules": [ - "frontleft.json", - "frontright.json", - "backleft.json", - "backright.json" - ] - } \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b5b7e09..7d7960b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,48 +4,152 @@ package frc.robot; -import edu.wpi.first.math.geometry.Translation3d; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.config.PIDConstants; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.util.Units; -import swervelib.math.Matter; /** - * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean constants. This - * class should not be used for any other purpose. All constants should be declared globally (i.e. public static). Do - * not put anything functional in this class. + * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean + * constants. This class should not be used for any other purpose. All constants should be declared + * globally (i.e. public static). Do not put anything functional in this class. * *

It is advised to statically import this class (or one of its inner classes) wherever the * constants are needed, to reduce verbosity. */ -public final class Constants -{ - - public static final double ROBOT_MASS = (148 - 20.3) * 0.453592; // 32lbs * kg per pound - public static final Matter CHASSIS = new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS); - public static final double LOOP_TIME = 0.13; //s, 20ms + 110ms sprk max velocity lag - public static final double MAX_SPEED = Units.feetToMeters(14.5); - // Maximum speed of the robot in meters per second, used to limit acceleration. - -// public static final class AutonConstants -// { -// -// public static final PIDConstants TRANSLATION_PID = new PIDConstants(0.7, 0, 0); -// public static final PIDConstants ANGLE_PID = new PIDConstants(0.4, 0, 0.01); -// } - - public static final class DrivebaseConstants +public final class Constants { + public static class OperatorConstants { + public static final int kDriverControllerPort = 0; + public static final double kDriverDeadband = 0.05; + public static final double kDriverSensativity = 0.8; + + public static final int kOperatorControllerPort = 1; + } + + public static class DrivetrainConstants + { + //CAN Id's for the driving and turning motors + public static final int kFrontLeftDrivingCANId = 16; + public static final int kFrontRightDrivingCANId = 17; + public static final int kBackLeftDrivingCANId = 13; + public static final int kBackRightDrivingCANId = 15; + + public static final int kFrontLeftTurningCANId = 11; + public static final int kFrontRightTurningCANId = 10; + public static final int kBackLeftTurningCANId = 14; + public static final int kBackRightTurningCANId = 12; + + //Not the maxium capable speed of the robot + //but an allowed max speed of the robot + public static final double kMaxSpeedMetersPerSecond = 8; + public static final double kMaxAngularSpeed = 2*Math.PI; //radians per second + + public static final double kTrackWidth = Units.inchesToMeters(24.5); + public static final double kWheelBase = Units.inchesToMeters(24.5); + + public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( + new Translation2d(kWheelBase/2, kTrackWidth/2), + new Translation2d(kWheelBase/2, -kTrackWidth/2), + new Translation2d(-kWheelBase/2, kTrackWidth/2), + new Translation2d(-kWheelBase/2, -kTrackWidth/2)); + + //May be removed + //Use REV hardware client to remove offset in the wheels by zeroing them + //if that doesn't work record offset of the wheels when lined up + //Angular offsets of the modules relative to the chassis in radians + public static final double kFrontLeftChassisAngularOffset = -Math.PI/2; + public static final double kFrontRightChassisAngularOffset = 0; + public static final double kBackLeftChassisAngularOffset = Math.PI; + public static final double kBackRightChassisAngularOffset = Math.PI/2; + + public static final PPHolonomicDriveController kPathFollowerConfig = new PPHolonomicDriveController( + new PIDConstants(5, 0, 0), + new PIDConstants(5,0,0), + kMaxSpeedMetersPerSecond); + } + + public static class SwerveModuleConstants + { + //may need to change to 12T or 14T + public static final double kDrivingMotorPinionTeeth = 14; + + //turn motor is inverted + public static final boolean kTurningEncoderInverted = true; + + public static final double kWheelDiameterMeters = Units.inchesToMeters(2); + public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI; + + //RPM of the NEO brushless motors + public static final double kFreeSpeedRPM = 5676; //rotations per min + public static final double kDrivingMotorFreeSpeedRPS = kFreeSpeedRPM / 60; //rotations per sec + //gear ratios of driving motor + //45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 teeth on the bevel pinion + public static final double kDrivingMotorReduction = (45.0 * 22) / (kDrivingMotorPinionTeeth * 15); + public static final double kDrivingWheelFreeSpeedRPS = (kDrivingMotorFreeSpeedRPS * kWheelCircumferenceMeters) / kDrivingMotorReduction; + + //conversion factors that may be needed + public static final double kDrivingEncoderPositionFactor = kWheelCircumferenceMeters / kDrivingMotorReduction; //meters + public static final double kDrivingEncoderVelocityFactor = kDrivingEncoderPositionFactor / 60.0; //meters per second + + public static final double kTurningEncoderPositionFactor = (2 * Math.PI); //radians + public static final double kTurningEncoderVelocityFactor = kTurningEncoderPositionFactor / 60.0; //radians per second + + //constants for the PIDs + public static final double kDrivingP = 0.006; + public static final double kDrivingI = 0; + public static final double kDrivingD = 0; + public static final double kDrivingFF = 1/kDrivingWheelFreeSpeedRPS; + public static final double kDrivingMinOutput = -1; + public static final double kDrivingMaxOutput = 1; + + //0.22 + public static final double kTurningP = 0.4; + public static final double kTurningI = 0; + public static final double kTurningD = 0; + public static final double kTurningMinOutput = -1; + public static final double kTurningMaxOutput = 1; + public static final boolean kEnablePIDWrapping = true; + public static final double kTurningEncoderPositionPIDMinInput = 0; + public static final double kTurningEncoderPositionPIDMaxInput = (2 * Math.PI); + + public static final IdleMode kDrivingMotorIdleMode = IdleMode.kBrake; + public static final IdleMode kTurningMotorIdleMode = IdleMode.kBrake; + + public static final int kDrivingMotorCurrentLimit = 50; // amps + public static final int kTurningMotorCurrentLimit = 20; // amps + } + + public static class EndEffectorConstants { + //CAN Id's for the End Effector + public static final int kEndEffectorMotorCANId = 30; + public static final int kEndEffectorTiltCANId = 31; + public static final int kEndEffectorFingerCANId = 24; + public static final int kEndEffectorAlgaeCANId = 23; + } - // Hold time on motor brakes when disabled - public static final double WHEEL_LOCK_TIME = 10; // seconds + public static class ElevatorConstants + { + //CAN Id's for the End Effector + public static final int kElevator1CANId = 25; + public static final int kElevator2CANId = 29; } - public static class OperatorConstants + public static class AlgaeIntakeConstants { + //CAN Id's for the End Effector + public static final int kAlgaeRollerCANId = 27; + public static final int kAlgaeTiltCANId = 22; + } - // Joystick Deadband - public static final double DEADBAND = 0.1; - public static final double LEFT_Y_DEADBAND = 0.1; - public static final double RIGHT_X_DEADBAND = 0.1; - public static final double TURN_CONSTANT = 6; + public static class ClimberConstants + { + //CAN Id's for the End Effector + public static final int kClimberWinchCANId = 26; + public static final int kClimberGrabCANId = 32; } } diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index 4ca4b2f..8776e5d 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -7,23 +7,19 @@ import edu.wpi.first.wpilibj.RobotBase; /** - * Do NOT add any static variables to this class, or any initialization at all. Unless you know what you are doing, do - * not modify this file except to change the parameter class to the startRobot call. + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. */ -public final class Main -{ - - private Main() - { - } +public final class Main { + private Main() {} /** * Main initialization function. Do not perform any initialization here. * *

If you change your main robot class, change the parameter type. */ - public static void main(String... args) - { + public static void main(String... args) { RobotBase.startRobot(Robot::new); } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2b44325..47d3d5a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,67 +4,39 @@ package frc.robot; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; /** - * The VM is configured to automatically run this class, and to call the functions corresponding to each mode, as - * described in the TimedRobot documentation. If you change the name of this class or the package after creating this - * project, you must also update the build.gradle file in the project. + * The methods in this class are called automatically corresponding to each mode, as described in + * the TimedRobot documentation. If you change the name of this class or the package after creating + * this project, you must also update the Main.java file in the project. */ -public class Robot extends TimedRobot -{ +public class Robot extends TimedRobot { + private Command m_autonomousCommand; - private static Robot instance; - private Command m_autonomousCommand; - - private RobotContainer m_robotContainer; - - private Timer disabledTimer; - - public Robot() - { - instance = this; - } - - public static Robot getInstance() - { - return instance; - } + private final RobotContainer m_robotContainer; /** - * This function is run when the robot is first started up and should be used for any initialization code. + * This function is run when the robot is first started up and should be used for any + * initialization code. */ - @Override - public void robotInit() - { + public Robot() { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); - - // Create a timer to disable motor brake a few seconds after disable. This will let the robot stop - // immediately when disabled, but then also let it be pushed more - disabledTimer = new Timer(); - - if (isSimulation()) - { - DriverStation.silenceJoystickConnectionWarning(true); - } } /** - * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics that you want ran - * during disabled, autonomous, teleoperated and test. + * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics + * that you want ran during disabled, autonomous, teleoperated and test. * *

This runs after the mode specific periodic functions, but before LiveWindow and * SmartDashboard integrated updating. */ @Override - public void robotPeriodic() - { + public void robotPeriodic() { // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled // commands, running already-scheduled commands, removing finished or interrupted commands, // and running subsystem periodic() methods. This must be called from the robot's periodic @@ -72,105 +44,58 @@ public void robotPeriodic() CommandScheduler.getInstance().run(); } - /** - * This function is called once each time the robot enters Disabled mode. - */ + /** This function is called once each time the robot enters Disabled mode. */ @Override - public void disabledInit() - { - m_robotContainer.setMotorBrake(true); - disabledTimer.reset(); - disabledTimer.start(); - } + public void disabledInit() {} @Override - public void disabledPeriodic() - { - if (disabledTimer.hasElapsed(Constants.DrivebaseConstants.WHEEL_LOCK_TIME)) - { - m_robotContainer.setMotorBrake(false); - disabledTimer.stop(); - disabledTimer.reset(); - } - } + public void disabledPeriodic() {} - /** - * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. - */ + /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override - public void autonomousInit() - { - // m_robotContainer.setMotorBrake(true); - // m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - - // // schedule the autonomous command (example) - // if (m_autonomousCommand != null) - // { - // m_autonomousCommand.schedule(); - // } - return; + public void autonomousInit() { + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + + // schedule the autonomous command (example) + if (m_autonomousCommand != null) { + m_autonomousCommand.schedule(); + } } - /** - * This function is called periodically during autonomous. - */ + /** This function is called periodically during autonomous. */ @Override - public void autonomousPeriodic() - { - } + public void autonomousPeriodic() {} @Override - public void teleopInit() - { + public void teleopInit() { // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove // this line or comment it out. - if (m_autonomousCommand != null) - { + if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); - } else - { - CommandScheduler.getInstance().cancelAll(); } } - /** - * This function is called periodically during operator control. - */ + /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() - { - } + public void teleopPeriodic() {} @Override - public void testInit() - { + public void testInit() { // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); } - /** - * This function is called periodically during test mode. - */ + /** This function is called periodically during test mode. */ @Override - public void testPeriodic() - { - } + public void testPeriodic() {} - /** - * This function is called once when the robot is first started up. - */ + /** This function is called once when the robot is first started up. */ @Override - public void simulationInit() - { - } + public void simulationInit() {} - /** - * This function is called periodically whilst in simulation. - */ + /** This function is called periodically whilst in simulation. */ @Override - public void simulationPeriodic() - { - } + public void simulationPeriodic() {} } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e355cd8..cf7f8a2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,154 +4,107 @@ package frc.robot; -import com.pathplanner.lib.auto.NamedCommands; -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; -import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.wpilibj.RobotBase; +import frc.robot.Constants.OperatorConstants; +import frc.robot.commands.AlgaeIntakeCommands.DeployArmCommand; +import frc.robot.commands.AlgaeIntakeCommands.IntakeRollerBar; +import frc.robot.commands.AlgaeIntakeCommands.OuttakeRollerBar; +import frc.robot.commands.AlgaeIntakeCommands.RetractArmCommand; +import frc.robot.commands.ClimberCommands.ExtendWinchCommand; +import frc.robot.commands.ClimberCommands.GrabCageCommand; +import frc.robot.commands.ClimberCommands.RetractWinchCommand; +import frc.robot.commands.ElevatorCommands.ElevatorDownCommand; +import frc.robot.commands.ElevatorCommands.ElevatorUpCommand; +import frc.robot.commands.EndEffectorCommands.IntakeCommand; +import frc.robot.commands.EndEffectorCommands.ShootCommand; +import frc.robot.commands.EndEffectorCommands.TiltCommand; +import frc.robot.subsystems.AlgaeIntakeSubsystem; +import frc.robot.subsystems.ClimberSubsystem; +import frc.robot.subsystems.DriveSubsystem; +import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.EndEffectorSubsystem; + +import com.pathplanner.lib.auto.AutoBuilder; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.PowerDistribution; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; 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.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Constants.OperatorConstants; -import frc.robot.subsystems.swervedrive.SwerveSubsystem; -import java.io.File; -import swervelib.SwerveInputStream; /** - * This class is where the bulk of the robot should be declared. Since Command-based is a "declarative" paradigm, very - * little robot logic should actually be handled in the {@link Robot} periodic methods (other than the scheduler calls). - * Instead, the structure of the robot (including subsystems, commands, and trigger mappings) should be declared here. + * This class is where the bulk of the robot should be declared. Since Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} + * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including + * subsystems, commands, and trigger mappings) should be declared here. */ -public class RobotContainer -{ +public class RobotContainer { - // Replace with CommandPS4Controller or CommandJoystick if needed - final CommandXboxController driverXbox = new CommandXboxController(0); - // The robot's subsystems and commands are defined here... - private final SwerveSubsystem drivebase = new SwerveSubsystem(); + public static PowerDistribution pdh; - /** - * Converts driver input into a field-relative ChassisSpeeds that is controlled by angular velocity. - */ - SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(), - () -> driverXbox.getLeftY() * -1, - () -> driverXbox.getLeftX() * -1) - .withControllerRotationAxis(driverXbox::getRightX) - .deadband(OperatorConstants.DEADBAND) - .scaleTranslation(0.8) - .allianceRelativeControl(true); + public static DriveSubsystem driveTrain; + public static ClimberSubsystem climber; + public static AlgaeIntakeSubsystem algaeIntake; + public static ElevatorSubsystem elevator; + public static EndEffectorSubsystem endEffector; - /** - * Clone's the angular velocity input stream and converts it to a fieldRelative input stream. - */ - SwerveInputStream driveDirectAngle = driveAngularVelocity.copy().withControllerHeadingAxis(driverXbox::getRightX, - driverXbox::getRightY) - .headingWhile(true); + public static CommandXboxController driverController; + public static CommandXboxController operatorController; - /** - * Clone's the angular velocity input stream and converts it to a robotRelative input stream. - */ - SwerveInputStream driveRobotOriented = driveAngularVelocity.copy().robotRelative(true) - .allianceRelativeControl(false); - - SwerveInputStream driveAngularVelocityKeyboard = SwerveInputStream.of(drivebase.getSwerveDrive(), - () -> -driverXbox.getLeftY(), - () -> -driverXbox.getLeftX()) - .withControllerRotationAxis(() -> driverXbox.getRawAxis( - 2)) - .deadband(OperatorConstants.DEADBAND) - .scaleTranslation(0.8) - .allianceRelativeControl(true); - // Derive the heading axis with math! - SwerveInputStream driveDirectAngleKeyboard = driveAngularVelocityKeyboard.copy() - .withControllerHeadingAxis(() -> - Math.sin( - driverXbox.getRawAxis( - 2) * - Math.PI) * - (Math.PI * - 2), - () -> - Math.cos( - driverXbox.getRawAxis( - 2) * - Math.PI) * - (Math.PI * - 2)) - .headingWhile(true); + private final SendableChooser autoChooser; - /** - * The container for the robot. Contains subsystems, OI devices, and commands. - */ - public RobotContainer() - { + /** The container for the robot. Contains subsystems, OI devices, and commands. */ + public RobotContainer() { // Configure the trigger bindings configureBindings(); - DriverStation.silenceJoystickConnectionWarning(true); - NamedCommands.registerCommand("test", Commands.print("I EXIST")); + pdh = new PowerDistribution(); + + driveTrain = new DriveSubsystem(); + climber = new ClimberSubsystem(); + algaeIntake = new AlgaeIntakeSubsystem(); + elevator = new ElevatorSubsystem(); + endEffector = new EndEffectorSubsystem(); + + driverController = new CommandXboxController(OperatorConstants.kDriverControllerPort); + operatorController = new CommandXboxController(OperatorConstants.kOperatorControllerPort); + + autoChooser = AutoBuilder.buildAutoChooser(); + autoChooser.setDefaultOption("NOTHING!!!", new InstantCommand()); + + driveTrain.setDefaultCommand(new RunCommand( + //left joystick controls translation + //right joystick controls rotation of the robot + () -> driveTrain.drive( + -MathUtil.applyDeadband(driverController.getLeftY(), OperatorConstants.kDriverDeadband), + -MathUtil.applyDeadband(driverController.getLeftX(), OperatorConstants.kDriverDeadband), + -MathUtil.applyDeadband(driverController.getRightX(), OperatorConstants.kDriverDeadband), + true), + driveTrain)); } - /** - * Use this method to define your trigger->command mappings. Triggers can be created via the - * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary predicate, or via the - * named factories in {@link edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for - * {@link CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller PS4} - * controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight joysticks}. - */ - private void configureBindings() + private void configureBindings() { - - Command driveFieldOrientedDirectAngle = drivebase.driveFieldOriented(driveDirectAngle); - Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity); - Command driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented); - Command driveSetpointGen = drivebase.driveWithSetpointGeneratorFieldRelative( - driveDirectAngle); - Command driveFieldOrientedDirectAngleKeyboard = drivebase.driveFieldOriented(driveDirectAngleKeyboard); - Command driveFieldOrientedAnglularVelocityKeyboard = drivebase.driveFieldOriented(driveAngularVelocityKeyboard); - Command driveSetpointGenKeyboard = drivebase.driveWithSetpointGeneratorFieldRelative( - driveDirectAngleKeyboard); - - if (RobotBase.isSimulation()) - { - drivebase.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard); - } else - { - drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); - } - - if (Robot.isSimulation()) - { - driverXbox.start().onTrue(Commands.runOnce(() -> drivebase.resetOdometry(new Pose2d(3, 3, new Rotation2d())))); - driverXbox.button(1).whileTrue(drivebase.sysIdDriveMotorCommand()); - - } - if (DriverStation.isTest()) - { - drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); // Overrides drive command above! - - driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly()); - driverXbox.y().whileTrue(drivebase.driveToDistanceCommand(1.0, 0.2)); - driverXbox.start().onTrue((Commands.runOnce(drivebase::zeroGyro))); - driverXbox.back().whileTrue(drivebase.centerModulesCommand()); - driverXbox.leftBumper().onTrue(Commands.none()); - driverXbox.rightBumper().onTrue(Commands.none()); - } else - { - driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro))); - driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading)); - // driverXbox.b().whileTrue( - // drivebase.driveToPose( - // new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0))) - // ); - driverXbox.start().whileTrue(Commands.none()); - driverXbox.back().whileTrue(Commands.none()); - driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly()); - driverXbox.rightBumper().onTrue(Commands.none()); - } - + // Algae Intake Commands + operatorController.a().whileTrue(new DeployArmCommand(0.1)); // TODO change speed and button + operatorController.b().whileTrue(new RetractArmCommand(0.1)); + operatorController.leftBumper().whileTrue(new IntakeRollerBar(0.1)); + operatorController.rightBumper().whileTrue(new OuttakeRollerBar(0.1)); + + // Climber + operatorController.leftTrigger().whileTrue(new ExtendWinchCommand(0.1)); + operatorController.rightTrigger().whileTrue(new RetractWinchCommand(0.1)); + operatorController.povDown().whileTrue(new GrabCageCommand(0.1)); + + // Elevator + operatorController.povUp().whileTrue(new ElevatorUpCommand(0.1)); + operatorController.povDown().whileTrue(new ElevatorDownCommand(0.1)); + + // End Effector + operatorController.povLeft().whileTrue(new IntakeCommand(0.1)); + operatorController.povRight().whileTrue(new ShootCommand(0.1)); + operatorController.povCenter().whileTrue(new TiltCommand(0.1)); } /** @@ -159,14 +112,8 @@ private void configureBindings() * * @return the command to run in autonomous */ - // public Command getAutonomousCommand() - // { - // // An example command will be run in autonomous - // return drivebase.getAutonomousCommand("New Auto"); - // } - - public void setMotorBrake(boolean brake) - { - drivebase.setMotorBrake(brake); + public Command getAutonomousCommand() { + // An example command will be run in autonomous + return autoChooser.getSelected(); } } diff --git a/src/main/java/frc/robot/commands/AlgaeIntakeCommands/DeployArmCommand.java b/src/main/java/frc/robot/commands/AlgaeIntakeCommands/DeployArmCommand.java new file mode 100644 index 0000000..74b2f52 --- /dev/null +++ b/src/main/java/frc/robot/commands/AlgaeIntakeCommands/DeployArmCommand.java @@ -0,0 +1,31 @@ +package frc.robot.commands.AlgaeIntakeCommands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; + +public class DeployArmCommand extends Command +{ + private final double speed; + + public DeployArmCommand(Double _speed) + { + addRequirements(RobotContainer.algaeIntake); + speed = _speed; + } + + @Override + public void initialize() {} + + @Override + public void execute() { + RobotContainer.algaeIntake.setSpeedArmTilt(speed); + } + + @Override + public void end(boolean interrupted) {} + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/AlgaeIntakeCommands/IntakeRollerBar.java b/src/main/java/frc/robot/commands/AlgaeIntakeCommands/IntakeRollerBar.java new file mode 100644 index 0000000..324ad32 --- /dev/null +++ b/src/main/java/frc/robot/commands/AlgaeIntakeCommands/IntakeRollerBar.java @@ -0,0 +1,31 @@ +package frc.robot.commands.AlgaeIntakeCommands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; + +public class IntakeRollerBar extends Command +{ + private final double speed; + + public IntakeRollerBar(Double _speed) + { + addRequirements(RobotContainer.algaeIntake); + speed = _speed; + } + + @Override + public void initialize() {} + + @Override + public void execute() { + RobotContainer.algaeIntake.setSpeedRoller(speed); + } + + @Override + public void end(boolean interrupted) {} + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/AlgaeIntakeCommands/OuttakeRollerBar.java b/src/main/java/frc/robot/commands/AlgaeIntakeCommands/OuttakeRollerBar.java new file mode 100644 index 0000000..1a53897 --- /dev/null +++ b/src/main/java/frc/robot/commands/AlgaeIntakeCommands/OuttakeRollerBar.java @@ -0,0 +1,31 @@ +package frc.robot.commands.AlgaeIntakeCommands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; + +public class OuttakeRollerBar extends Command +{ + private final double speed; + + public OuttakeRollerBar(Double _speed) + { + addRequirements(RobotContainer.algaeIntake); + speed = _speed; + } + + @Override + public void initialize() {} + + @Override + public void execute() { + RobotContainer.algaeIntake.setSpeedRoller(-speed); + } + + @Override + public void end(boolean interrupted) {} + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/AlgaeIntakeCommands/RetractArmCommand.java b/src/main/java/frc/robot/commands/AlgaeIntakeCommands/RetractArmCommand.java new file mode 100644 index 0000000..1e72f8e --- /dev/null +++ b/src/main/java/frc/robot/commands/AlgaeIntakeCommands/RetractArmCommand.java @@ -0,0 +1,31 @@ +package frc.robot.commands.AlgaeIntakeCommands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; + +public class RetractArmCommand extends Command +{ + private final double speed; + + public RetractArmCommand(Double _speed) + { + addRequirements(RobotContainer.algaeIntake); + speed = _speed; + } + + @Override + public void initialize() {} + + @Override + public void execute() { + RobotContainer.algaeIntake.setSpeedArmTilt(-speed); + } + + @Override + public void end(boolean interrupted) {} + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ClimberCommands/ExtendWinchCommand.java b/src/main/java/frc/robot/commands/ClimberCommands/ExtendWinchCommand.java new file mode 100644 index 0000000..e17213c --- /dev/null +++ b/src/main/java/frc/robot/commands/ClimberCommands/ExtendWinchCommand.java @@ -0,0 +1,31 @@ +package frc.robot.commands.ClimberCommands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; + +public class ExtendWinchCommand extends Command +{ + private final double speed; + + public ExtendWinchCommand(Double _speed) + { + addRequirements(RobotContainer.climber); + speed = _speed; + } + + @Override + public void initialize() {} + + @Override + public void execute() { + RobotContainer.climber.Winch(speed); + } + + @Override + public void end(boolean interrupted) {} + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ClimberCommands/GrabCageCommand.java b/src/main/java/frc/robot/commands/ClimberCommands/GrabCageCommand.java new file mode 100644 index 0000000..ce9c42b --- /dev/null +++ b/src/main/java/frc/robot/commands/ClimberCommands/GrabCageCommand.java @@ -0,0 +1,31 @@ +package frc.robot.commands.ClimberCommands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; + +public class GrabCageCommand extends Command +{ + private final double speed; + + public GrabCageCommand(Double _speed) + { + addRequirements(RobotContainer.climber); + speed = _speed; + } + + @Override + public void initialize() {} + + @Override + public void execute() { + RobotContainer.climber.Grab(speed); + } + + @Override + public void end(boolean interrupted) {} + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ClimberCommands/RetractWinchCommand.java b/src/main/java/frc/robot/commands/ClimberCommands/RetractWinchCommand.java new file mode 100644 index 0000000..3ae8a9d --- /dev/null +++ b/src/main/java/frc/robot/commands/ClimberCommands/RetractWinchCommand.java @@ -0,0 +1,31 @@ +package frc.robot.commands.ClimberCommands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; + +public class RetractWinchCommand extends Command +{ + private final double speed; + + public RetractWinchCommand(Double _speed) + { + addRequirements(RobotContainer.climber); + speed = _speed; + } + + @Override + public void initialize() {} + + @Override + public void execute() { + RobotContainer.climber.Winch(-speed); + } + + @Override + public void end(boolean interrupted) {} + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ElevatorCommands/ElevatorDownCommand.java b/src/main/java/frc/robot/commands/ElevatorCommands/ElevatorDownCommand.java new file mode 100644 index 0000000..5172684 --- /dev/null +++ b/src/main/java/frc/robot/commands/ElevatorCommands/ElevatorDownCommand.java @@ -0,0 +1,33 @@ +package frc.robot.commands.ElevatorCommands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; + +public class ElevatorDownCommand extends Command +{ + private final double speed; + + public ElevatorDownCommand(Double _speed) + { + addRequirements(RobotContainer.elevator); + speed = _speed; + } + + @Override + public void initialize() {} + + @Override + public void execute() + { + RobotContainer.elevator.setSpeedElevator1(-speed); + RobotContainer.elevator.setSpeedElevator2(-speed); + } + + @Override + public void end(boolean interrupted) {} + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ElevatorCommands/ElevatorUpCommand.java b/src/main/java/frc/robot/commands/ElevatorCommands/ElevatorUpCommand.java new file mode 100644 index 0000000..86d3dc3 --- /dev/null +++ b/src/main/java/frc/robot/commands/ElevatorCommands/ElevatorUpCommand.java @@ -0,0 +1,33 @@ +package frc.robot.commands.ElevatorCommands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; + +public class ElevatorUpCommand extends Command +{ + private final double speed; + + public ElevatorUpCommand(Double _speed) + { + addRequirements(RobotContainer.elevator); + speed = _speed; + } + + @Override + public void initialize() {} + + @Override + public void execute() + { + RobotContainer.elevator.setSpeedElevator1(speed); + RobotContainer.elevator.setSpeedElevator2(speed); + } + + @Override + public void end(boolean interrupted) {} + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/EndEffectorCommands/IntakeCommand.java b/src/main/java/frc/robot/commands/EndEffectorCommands/IntakeCommand.java new file mode 100644 index 0000000..5df3142 --- /dev/null +++ b/src/main/java/frc/robot/commands/EndEffectorCommands/IntakeCommand.java @@ -0,0 +1,33 @@ +package frc.robot.commands.EndEffectorCommands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; + +public class IntakeCommand extends Command +{ + private final double speed; + + public IntakeCommand(Double _speed) + { + addRequirements(RobotContainer.endEffector); + speed = _speed; + } + + @Override + public void initialize() {} + + @Override + public void execute() + { + RobotContainer.endEffector.setSpeedEndEffectorAlgae(speed); + RobotContainer.endEffector.setSpeedEndEffectorFinger(speed); + } + + @Override + public void end(boolean interrupted) {} + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/EndEffectorCommands/ShootCommand.java b/src/main/java/frc/robot/commands/EndEffectorCommands/ShootCommand.java new file mode 100644 index 0000000..a8f74b9 --- /dev/null +++ b/src/main/java/frc/robot/commands/EndEffectorCommands/ShootCommand.java @@ -0,0 +1,32 @@ +package frc.robot.commands.EndEffectorCommands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; + +public class ShootCommand extends Command +{ + private final double speed; + + public ShootCommand(Double _speed) + { + addRequirements(RobotContainer.endEffector); + speed = _speed; + } + + @Override + public void initialize() {} + + @Override + public void execute() + { + RobotContainer.endEffector.setSpeedEndEffectorMotor(speed); + } + + @Override + public void end(boolean interrupted) {} + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/EndEffectorCommands/TiltCommand.java b/src/main/java/frc/robot/commands/EndEffectorCommands/TiltCommand.java new file mode 100644 index 0000000..b822c95 --- /dev/null +++ b/src/main/java/frc/robot/commands/EndEffectorCommands/TiltCommand.java @@ -0,0 +1,32 @@ +package frc.robot.commands.EndEffectorCommands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; + +public class TiltCommand extends Command +{ + private final double speed; + + public TiltCommand(Double _speed) + { + addRequirements(RobotContainer.endEffector); + speed = _speed; + } + + @Override + public void initialize() {} + + @Override + public void execute() + { + RobotContainer.endEffector.setSpeedEndEffectorTilt(speed); + } + + @Override + public void end(boolean interrupted) {} + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java b/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java deleted file mode 100644 index 6919e45..0000000 --- a/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java +++ /dev/null @@ -1,86 +0,0 @@ -package frc.robot.commands.swervedrive.auto; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.swervedrive.SwerveSubsystem; - - -/** - * Auto Balance command using a simple PID controller. Created by Team 3512 - * ... - */ -public class AutoBalanceCommand extends Command -{ - - private final SwerveSubsystem swerveSubsystem; - private final PIDController controller; - - public AutoBalanceCommand(SwerveSubsystem swerveSubsystem) - { - this.swerveSubsystem = swerveSubsystem; - controller = new PIDController(1.0, 0.0, 0.0); - controller.setTolerance(1); - controller.setSetpoint(0.0); - // each subsystem used by the command must be passed into the - // addRequirements() method (which takes a vararg of Subsystem) - addRequirements(this.swerveSubsystem); - } - - /** - * The initial subroutine of a command. Called once when the command is initially scheduled. - */ - @Override - public void initialize() - { - - } - - /** - * The main body of a command. Called repeatedly while the command is scheduled. (That is, it is called repeatedly - * until {@link #isFinished()}) returns true.) - */ - @Override - public void execute() - { - SmartDashboard.putBoolean("At Tolerance", controller.atSetpoint()); - - double translationVal = MathUtil.clamp(controller.calculate(swerveSubsystem.getPitch().getDegrees(), 0.0), -0.5, - 0.5); - swerveSubsystem.drive(new Translation2d(translationVal, 0.0), 0.0, true); - } - - /** - *

- * Returns whether this command has finished. Once a command finishes -- indicated by this method returning true -- - * the scheduler will call its {@link #end(boolean)} method. - *

- * Returning false will result in the command never ending automatically. It may still be cancelled manually or - * interrupted by another command. Hard coding this command to always return true will result in the command executing - * once and finishing immediately. It is recommended to use * - * {@link edu.wpi.first.wpilibj2.command.InstantCommand InstantCommand} for such an operation. - *

- * - * @return whether this command has finished. - */ - @Override - public boolean isFinished() - { - return controller.atSetpoint(); - } - - /** - * The action to take when the command ends. Called when either the command finishes normally -- that is it is called - * when {@link #isFinished()} returns true -- or when it is interrupted/canceled. This is where you may want to wrap - * up loose ends, like shutting off a motor that was being used in the command. - * - * @param interrupted whether the command was interrupted/canceled - */ - @Override - public void end(boolean interrupted) - { - swerveSubsystem.lock(); - } -} diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java deleted file mode 100644 index 9ab90b7..0000000 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java +++ /dev/null @@ -1,120 +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.swervedrive.drivebase; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; -import frc.robot.subsystems.swervedrive.SwerveSubsystem; -import java.util.List; -import java.util.function.DoubleSupplier; -import swervelib.SwerveController; -import swervelib.math.SwerveMath; - -/** - * An example command that uses an example subsystem. - */ -public class AbsoluteDrive extends Command -{ - - private final SwerveSubsystem swerve; - private final DoubleSupplier vX, vY; - private final DoubleSupplier headingHorizontal, headingVertical; - private boolean initRotation = false; - - /** - * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, where x is - * torwards/away from alliance wall and y is left/right. headingHorzontal and headingVertical are the Cartesian - * coordinates from which the robot's angle will be derived— they will be converted to a polar angle, which the robot - * will rotate to. - * - * @param swerve The swerve drivebase subsystem. - * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the range -1 - * to 1 with deadband already accounted for. Positive X is away from the alliance wall. - * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the range -1 - * to 1 with deadband already accounted for. Positive Y is towards the left wall when - * looking through the driver station glass. - * @param headingHorizontal DoubleSupplier that supplies the horizontal component of the robot's heading angle. In the - * robot coordinate system, this is along the same axis as vY. Should range from -1 to 1 with - * no deadband. Positive is towards the left wall when looking through the driver station - * glass. - * @param headingVertical DoubleSupplier that supplies the vertical component of the robot's heading angle. In the - * robot coordinate system, this is along the same axis as vX. Should range from -1 to 1 - * with no deadband. Positive is away from the alliance wall. - */ - public AbsoluteDrive(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier headingHorizontal, - DoubleSupplier headingVertical) - { - this.swerve = swerve; - this.vX = vX; - this.vY = vY; - this.headingHorizontal = headingHorizontal; - this.headingVertical = headingVertical; - - addRequirements(swerve); - } - - @Override - public void initialize() - { - initRotation = true; - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() - { - - // Get the desired chassis speeds based on a 2 joystick module. - ChassisSpeeds desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), - headingHorizontal.getAsDouble(), - headingVertical.getAsDouble()); - - // Prevent Movement After Auto - if (initRotation) - { - if (headingHorizontal.getAsDouble() == 0 && headingVertical.getAsDouble() == 0) - { - // Get the curretHeading - Rotation2d firstLoopHeading = swerve.getHeading(); - - // Set the Current Heading to the desired Heading - desiredSpeeds = swerve.getTargetSpeeds(0, 0, firstLoopHeading.getSin(), firstLoopHeading.getCos()); - } - //Dont Init Rotation Again - initRotation = false; - } - - // Limit velocity to prevent tippy - Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); - translation = SwerveMath.limitVelocity(translation, swerve.getFieldVelocity(), swerve.getPose(), - Constants.LOOP_TIME, Constants.ROBOT_MASS, List.of(Constants.CHASSIS), - swerve.getSwerveDriveConfiguration()); - SmartDashboard.putNumber("LimitedTranslation", translation.getX()); - SmartDashboard.putString("Translation", translation.toString()); - - // Make the robot move - swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); - - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) - { - } - - // Returns true when the command should end. - @Override - public boolean isFinished() - { - return false; - } - - -} diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java deleted file mode 100644 index a92904f..0000000 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java +++ /dev/null @@ -1,154 +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.swervedrive.drivebase; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; -import frc.robot.subsystems.swervedrive.SwerveSubsystem; -import java.util.List; -import java.util.function.BooleanSupplier; -import java.util.function.DoubleSupplier; -import swervelib.SwerveController; -import swervelib.math.SwerveMath; - -/** - * A more advanced Swerve Control System that has 4 buttons for which direction to face - */ -public class AbsoluteDriveAdv extends Command -{ - - private final SwerveSubsystem swerve; - private final DoubleSupplier vX, vY; - private final DoubleSupplier headingAdjust; - private final BooleanSupplier lookAway, lookTowards, lookLeft, lookRight; - private boolean resetHeading = false; - - /** - * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, where x is - * torwards/away from alliance wall and y is left/right. Heading Adjust changes the current heading after being - * multipied by a constant. The look booleans are shortcuts to get the robot to face a certian direction. Based off of - * ideas in https://www.chiefdelphi.com/t/experiments-with-a-swerve-steering-knob/446172 - * - * @param swerve The swerve drivebase subsystem. - * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the range -1 to 1 - * with deadband already accounted for. Positive X is away from the alliance wall. - * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the range -1 to 1 - * with deadband already accounted for. Positive Y is towards the left wall when looking through - * the driver station glass. - * @param headingAdjust DoubleSupplier that supplies the component of the robot's heading angle that should be - * adjusted. Should range from -1 to 1 with deadband already accounted for. - * @param lookAway Face the robot towards the opposing alliance's wall in the same direction the driver is - * facing - * @param lookTowards Face the robot towards the driver - * @param lookLeft Face the robot left - * @param lookRight Face the robot right - */ - public AbsoluteDriveAdv(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier headingAdjust, - BooleanSupplier lookAway, BooleanSupplier lookTowards, BooleanSupplier lookLeft, - BooleanSupplier lookRight) - { - this.swerve = swerve; - this.vX = vX; - this.vY = vY; - this.headingAdjust = headingAdjust; - this.lookAway = lookAway; - this.lookTowards = lookTowards; - this.lookLeft = lookLeft; - this.lookRight = lookRight; - - addRequirements(swerve); - } - - @Override - public void initialize() - { - resetHeading = true; - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() - { - double headingX = 0; - double headingY = 0; - - // These are written to allow combinations for 45 angles - // Face Away from Drivers - if (lookAway.getAsBoolean()) - { - headingY = -1; - } - // Face Right - if (lookRight.getAsBoolean()) - { - headingX = 1; - } - // Face Left - if (lookLeft.getAsBoolean()) - { - headingX = -1; - } - // Face Towards the Drivers - if (lookTowards.getAsBoolean()) - { - headingY = 1; - } - - // Prevent Movement After Auto - if (resetHeading) - { - if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) == 0) - { - // Get the curret Heading - Rotation2d currentHeading = swerve.getHeading(); - - // Set the Current Heading to the desired Heading - headingX = currentHeading.getSin(); - headingY = currentHeading.getCos(); - } - //Dont reset Heading Again - resetHeading = false; - } - - ChassisSpeeds desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), headingX, headingY); - - // Limit velocity to prevent tippy - Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); - translation = SwerveMath.limitVelocity(translation, swerve.getFieldVelocity(), swerve.getPose(), - Constants.LOOP_TIME, Constants.ROBOT_MASS, List.of(Constants.CHASSIS), - swerve.getSwerveDriveConfiguration()); - SmartDashboard.putNumber("LimitedTranslation", translation.getX()); - SmartDashboard.putString("Translation", translation.toString()); - - // Make the robot move - if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0) - { - resetHeading = true; - swerve.drive(translation, (Constants.OperatorConstants.TURN_CONSTANT * -headingAdjust.getAsDouble()), true); - } else - { - swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) - { - } - - // Returns true when the command should end. - @Override - public boolean isFinished() - { - return false; - } - - -} diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteFieldDrive.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteFieldDrive.java deleted file mode 100644 index b91489f..0000000 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteFieldDrive.java +++ /dev/null @@ -1,95 +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.swervedrive.drivebase; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; -import frc.robot.subsystems.swervedrive.SwerveSubsystem; -import java.util.List; -import java.util.function.DoubleSupplier; -import swervelib.SwerveController; -import swervelib.math.SwerveMath; - -/** - * An example command that uses an example subsystem. - */ -public class AbsoluteFieldDrive extends Command -{ - - private final SwerveSubsystem swerve; - private final DoubleSupplier vX, vY, heading; - - /** - * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, where x is - * torwards/away from alliance wall and y is left/right. headingHorzontal and headingVertical are the Cartesian - * coordinates from which the robot's angle will be derived— they will be converted to a polar angle, which the robot - * will rotate to. - * - * @param swerve The swerve drivebase subsystem. - * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the range -1 to 1 with - * deadband already accounted for. Positive X is away from the alliance wall. - * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the range -1 to 1 with - * deadband already accounted for. Positive Y is towards the left wall when looking through the driver - * station glass. - * @param heading DoubleSupplier that supplies the robot's heading angle. - */ - public AbsoluteFieldDrive(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier vY, - DoubleSupplier heading) - { - this.swerve = swerve; - this.vX = vX; - this.vY = vY; - this.heading = heading; - - addRequirements(swerve); - } - - @Override - public void initialize() - { - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() - { - - // Get the desired chassis speeds based on a 2 joystick module. - - ChassisSpeeds desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), - new Rotation2d(heading.getAsDouble() * Math.PI)); - - // Limit velocity to prevent tippy - Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); - translation = SwerveMath.limitVelocity(translation, swerve.getFieldVelocity(), swerve.getPose(), - Constants.LOOP_TIME, Constants.ROBOT_MASS, List.of(Constants.CHASSIS), - swerve.getSwerveDriveConfiguration()); - SmartDashboard.putNumber("LimitedTranslation", translation.getX()); - SmartDashboard.putString("Translation", translation.toString()); - - // Make the robot move - swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); - - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) - { - } - - // Returns true when the command should end. - @Override - public boolean isFinished() - { - return false; - } - - -} diff --git a/src/main/java/frc/robot/subsystems/AlgaeIntakeSubsystem.java b/src/main/java/frc/robot/subsystems/AlgaeIntakeSubsystem.java new file mode 100644 index 0000000..5c14229 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/AlgaeIntakeSubsystem.java @@ -0,0 +1,35 @@ +package frc.robot.subsystems; + +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import frc.robot.Constants.AlgaeIntakeConstants; + +public class AlgaeIntakeSubsystem extends SubsystemBase +{ + private final SparkMax algaeRoller; + private final SparkMax algaeTilt; + + public AlgaeIntakeSubsystem() + { + algaeRoller = new SparkMax(AlgaeIntakeConstants.kAlgaeRollerCANId, MotorType.kBrushless); + algaeTilt = new SparkMax(AlgaeIntakeConstants.kAlgaeTiltCANId, MotorType.kBrushless); + } + + public void setSpeedRoller(double speed) + { + algaeRoller.set(speed); + } + + public void setSpeedArmTilt(double speed) + { + algaeTilt.set(speed); + } + + @Override + public void periodic() + { + + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java new file mode 100644 index 0000000..9710cc5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -0,0 +1,37 @@ +package frc.robot.subsystems; + +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj.PowerDistribution; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import frc.robot.Constants.ClimberConstants; + +public class ClimberSubsystem extends SubsystemBase +{ + private final SparkMax climberWinch; + private final SparkMax climberGrab; + + public ClimberSubsystem() + { + climberWinch = new SparkMax(ClimberConstants.kClimberWinchCANId, MotorType.kBrushless); + climberGrab = new SparkMax(ClimberConstants.kClimberGrabCANId, MotorType.kBrushless); + } + + public void Winch(double speed) + { + climberWinch.set(speed); + } + + public void Grab(double speed) + { + climberGrab.set(speed); + } + + @Override + public void periodic() + { + + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java new file mode 100644 index 0000000..9538dcb --- /dev/null +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -0,0 +1,219 @@ +package frc.robot.subsystems; + +import com.kauailabs.navx.frc.AHRS; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.SPI; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.DrivetrainConstants; + +public class DriveSubsystem extends SubsystemBase +{ + private final SwerveModule frontLeft = new SwerveModule( + DrivetrainConstants.kFrontLeftDrivingCANId, + DrivetrainConstants.kFrontLeftTurningCANId, + DrivetrainConstants.kFrontLeftChassisAngularOffset); + + private final SwerveModule frontRight = new SwerveModule( + DrivetrainConstants.kFrontRightDrivingCANId, + DrivetrainConstants.kFrontRightTurningCANId, + DrivetrainConstants.kFrontRightChassisAngularOffset); + + private final SwerveModule backLeft = new SwerveModule( + DrivetrainConstants.kBackLeftDrivingCANId, + DrivetrainConstants.kBackLeftTurningCANId, + DrivetrainConstants.kBackLeftChassisAngularOffset); + + private final SwerveModule backRight = new SwerveModule( + DrivetrainConstants.kBackRightDrivingCANId, + DrivetrainConstants.kBackRightTurningCANId, + DrivetrainConstants.kBackRightChassisAngularOffset); + + private AHRS imu = new AHRS(SPI.Port.kMXP); + + private SwerveDriveOdometry odometry = new SwerveDriveOdometry( + DrivetrainConstants.kDriveKinematics, getHeading(), getModulePositions()); + + private Field2d field = new Field2d(); + + private final StructArrayPublisher measuredSwerveStatePublisher; + private final StructArrayPublisher setpointSwerveStatePublisher; + + public DriveSubsystem() + { + /* + AutoBuilder.configureHolonomic( + this::getPose, + this::resetPose, + this::getSpeeds, + this::driveWithChassisSpeeds, + DrivetrainConstants.kPathFollowerConfig, + () -> { + var alliance = DriverStation.getAlliance(); + + if(alliance.isPresent()) + { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this); + + PathPlannerLogging.setLogActivePathCallback((poses) -> field.getObject("path").setPoses(poses)); + + SmartDashboard.putData("Field", field);*/ + + measuredSwerveStatePublisher = NetworkTableInstance.getDefault() + .getStructArrayTopic("/SwerveModuleState/Measured", SwerveModuleState.struct).publish(); + + setpointSwerveStatePublisher = NetworkTableInstance.getDefault() + .getStructArrayTopic("/SwerveModuleState/Setpoint", SwerveModuleState.struct).publish(); + } + + /** + * Method to drive drivetrain with joysticks + * + * @param xSpeed speed of robot in x direction (forward) + * @param ySpeed speed of robot in y direction (sideways) + * @param rotSpeed angular rate of the robot + * @param fieldRelative set if x and y speeds are relative to the field + */ + public void drive(double xSpeed, double ySpeed, double rotSpeed, boolean fieldRelative) + { + xSpeed *= DrivetrainConstants.kMaxSpeedMetersPerSecond; + ySpeed *= DrivetrainConstants.kMaxSpeedMetersPerSecond; + rotSpeed *= DrivetrainConstants.kMaxAngularSpeed; + + + var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates( + fieldRelative ? //if fieldRelative + ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rotSpeed, getHeading()) + :new ChassisSpeeds(xSpeed, ySpeed, rotSpeed)); + + + SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond); + + frontLeft.setDesiredState(swerveModuleStates[0]); + frontRight.setDesiredState(swerveModuleStates[1]); + backLeft.setDesiredState(swerveModuleStates[2]); + backRight.setDesiredState(swerveModuleStates[3]); + } + + + public SwerveDriveKinematics getKinematics() + { + return DrivetrainConstants.kDriveKinematics; + } + + /** + * A temperary method for testing the swerve modules. This will be used to tune PIDs + * + * @param speed velocity of the wheels in meters per second + * @param angle angle of the wheels in radians + */ + public void testSwerve(double speed, double angle) + { + SwerveModuleState testState = new SwerveModuleState(speed, new Rotation2d(angle)); + + frontLeft.setDesiredState(testState); + frontRight.setDesiredState(testState); + backLeft.setDesiredState(testState); + backRight.setDesiredState(testState); + } + + public void setTurningMotors(double percent) + { + frontLeft.setTurnMotor(percent); + frontRight.setTurnMotor(percent); + backLeft.setTurnMotor(percent); + backRight.setTurnMotor(percent); + } + + public void setDrivingMotors(double percent) + { + frontLeft.setDriveMotor(percent); + frontRight.setDriveMotor(percent); + backLeft.setDriveMotor(percent); + backRight.setDriveMotor(percent); + } + + public void driveWithChassisSpeeds(ChassisSpeeds chassisSpeeds) + { + ChassisSpeeds targetSpeeds = ChassisSpeeds.discretize(chassisSpeeds, 0.02); + + SwerveModuleState[] targetStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(targetSpeeds); + + SwerveDriveKinematics.desaturateWheelSpeeds(targetStates, DrivetrainConstants.kMaxSpeedMetersPerSecond); + + frontLeft.setDesiredState(targetStates[0]); + frontRight.setDesiredState(targetStates[1]); + backLeft.setDesiredState(targetStates[2]); + backRight.setDesiredState(targetStates[3]); + } + + public Pose2d getPose() + { + return odometry.getPoseMeters(); + } + + public void resetPose(Pose2d pose) + { + odometry.resetPosition( + getHeading(), getModulePositions(), pose); + } + + public ChassisSpeeds getSpeeds() + { + return DrivetrainConstants.kDriveKinematics.toChassisSpeeds(getModuleStates()); + } + + public Rotation2d getHeading() + { + return Rotation2d.fromDegrees(-imu.getAngle()); + } + + public SwerveModuleState[] getModuleStates() + { + return new SwerveModuleState[]{ + frontLeft.getModuleState(), + frontRight.getModuleState(), + backLeft.getModuleState(), + backRight.getModuleState()}; + } + + public SwerveModulePosition[] getModulePositions() + { + return new SwerveModulePosition[]{ + frontLeft.getPosition(), frontRight.getPosition(), + backLeft.getPosition(), backRight.getPosition()}; + } + + @Override + public void periodic() + { + //update odometry + odometry.update(getHeading(), getModulePositions()); + + //publish to networktable for advantagescope + measuredSwerveStatePublisher.set(getModuleStates()); + + field.setRobotPose(getPose()); + + setpointSwerveStatePublisher.set(new SwerveModuleState[]{ + frontLeft.getDesiredState(), + frontRight.getDesiredState(), + backLeft.getDesiredState(), + backRight.getDesiredState() + }); + } +} diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java new file mode 100644 index 0000000..f7d8e72 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -0,0 +1,35 @@ +package frc.robot.subsystems; + +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import frc.robot.Constants.ElevatorConstants; + +public class ElevatorSubsystem extends SubsystemBase +{ + private final SparkMax elevator1; + private final SparkMax elevator2; + + public ElevatorSubsystem() + { + elevator1 = new SparkMax(ElevatorConstants.kElevator1CANId, MotorType.kBrushless); + elevator2 = new SparkMax(ElevatorConstants.kElevator2CANId, MotorType.kBrushless); + } + + public void setSpeedElevator1(double speed) + { + elevator1.set(speed); + } + + public void setSpeedElevator2(double speed) + { + elevator2.set(speed); + } + + @Override + public void periodic() + { + + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/EndEffectorSubsystem.java b/src/main/java/frc/robot/subsystems/EndEffectorSubsystem.java new file mode 100644 index 0000000..4dd8d41 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/EndEffectorSubsystem.java @@ -0,0 +1,49 @@ +package frc.robot.subsystems; + +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import frc.robot.Constants.EndEffectorConstants; + +public class EndEffectorSubsystem extends SubsystemBase +{ + private final SparkMax endEffectorAlgae; + private final SparkMax endEffectorFinger; + private final SparkMax endEffectorMotor; + private final SparkMax endEffectorTilt; + + public EndEffectorSubsystem() + { + endEffectorAlgae = new SparkMax(EndEffectorConstants.kEndEffectorAlgaeCANId, MotorType.kBrushless); + endEffectorFinger = new SparkMax(EndEffectorConstants.kEndEffectorFingerCANId, MotorType.kBrushless); + endEffectorMotor = new SparkMax(EndEffectorConstants.kEndEffectorMotorCANId, MotorType.kBrushless); + endEffectorTilt = new SparkMax(EndEffectorConstants.kEndEffectorTiltCANId, MotorType.kBrushless); + } + + public void setSpeedEndEffectorAlgae(double speed) + { + endEffectorAlgae.set(speed); + } + + public void setSpeedEndEffectorFinger(double speed) + { + endEffectorFinger.set(speed); + } + + public void setSpeedEndEffectorMotor(double speed) + { + endEffectorMotor.set(speed); + } + + public void setSpeedEndEffectorTilt(double speed) + { + endEffectorTilt.set(speed); + } + + @Override + public void periodic() + { + + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java new file mode 100644 index 0000000..19fcf4a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -0,0 +1,185 @@ +package frc.robot.subsystems; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; + +import frc.robot.Constants.SwerveModuleConstants; + +public class SwerveModule +{ + private final SparkMax drivingMotor; + private final SparkMax turningMotor; + + private SparkMaxConfig motorConfig; + + private final RelativeEncoder drivingEncoder; + private final AbsoluteEncoder turningEncoder; + + private final SparkClosedLoopController drivingPIDController; + private final SparkClosedLoopController turningPIDController; + + private final double chassisAngularOffset; + private SwerveModuleState desiredState = new SwerveModuleState(0.0, new Rotation2d()); + private SwerveModuleState optimizedState = new SwerveModuleState(0.0,new Rotation2d()); + + public SwerveModule(int driveMotorId, int turnMotorId, double p_chassisAngularOffset) + { + drivingMotor = new SparkMax(driveMotorId, MotorType.kBrushless); + turningMotor = new SparkMax(turnMotorId, MotorType.kBrushless); + + //factory reset motor controllers to get them to a know state + //drivingMotor.restoreFactoryDefaults(); // Deprecated + //turningMotor.restoreFactoryDefaults(); + + drivingEncoder = drivingMotor.getEncoder(); + turningEncoder = turningMotor.getAbsoluteEncoder(); + + motorConfig = new SparkMaxConfig(); + + motorConfig.encoder + .positionConversionFactor(1) + .velocityConversionFactor(1); + + //Apply conversion factors to encoders to be used with the wpilib api + //drivingEncoder.setPositionConversionFactor(SwerveModuleConstants.kDrivingEncoderPositionFactor); // Deprecated + //drivingEncoder.setVelocityConversionFactor(SwerveModuleConstants.kDrivingEncoderVelocityFactor); + //turningEncoder.setPositionConversionFactor(SwerveModuleConstants.kTurningEncoderPositionFactor); + //turningEncoder.setVelocityConversionFactor(SwerveModuleConstants.kTurningEncoderVelocityFactor); + + //invert turning encoder since shaft rotates opposite direction of turning motor + //turningEncoder.setInverted(SwerveModuleConstants.kTurningEncoderInverted); // Deprecated + + //PID controller setup + drivingPIDController = drivingMotor.getClosedLoopController(); + turningPIDController = turningMotor.getClosedLoopController(); + //drivingPIDController.setFeedbackDevice(drivingEncoder); // Deprecated + //turningPIDController.setFeedbackDevice(turningEncoder); + + //set PID constants and min/max output + motorConfig.closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + // Set PID values for position control. We don't need to pass a closed loop + // slot, as it will default to slot 0. + .p(SwerveModuleConstants.kTurningP) + .i(SwerveModuleConstants.kTurningI) + .d(SwerveModuleConstants.kTurningD) + .outputRange(SwerveModuleConstants.kTurningMinOutput, SwerveModuleConstants.kTurningMaxOutput) + // Set PID values for velocity control in slot 1 + .p(SwerveModuleConstants.kDrivingP, ClosedLoopSlot.kSlot1) + .i(SwerveModuleConstants.kDrivingI, ClosedLoopSlot.kSlot1) + .d(SwerveModuleConstants.kDrivingD, ClosedLoopSlot.kSlot1) + .velocityFF(SwerveModuleConstants.kDrivingFF, ClosedLoopSlot.kSlot1) + .outputRange(SwerveModuleConstants.kDrivingMinOutput, SwerveModuleConstants.kDrivingMaxOutput, ClosedLoopSlot.kSlot1); + + //drivingPIDController.setP(SwerveModuleConstants.kDrivingP); // Deprecated + //drivingPIDController.setI(SwerveModuleConstants.kDrivingI); + //drivingPIDController.setD(SwerveModuleConstants.kDrivingD); + //drivingPIDController.setFF(SwerveModuleConstants.kDrivingFF); + //drivingPIDController.setOutputRange(SwerveModuleConstants.kDrivingMinOutput, SwerveModuleConstants.kDrivingMaxOutput); + + //turningPIDController.setP(SwerveModuleConstants.kTurningP); // Deprecated + //turningPIDController.setI(SwerveModuleConstants.kTurningI); + //turningPIDController.setD(SwerveModuleConstants.kTurningD); + //turningPIDController.setOutputRange(SwerveModuleConstants.kTurningMinOutput, SwerveModuleConstants.kTurningMaxOutput); + + drivingMotor.configure(motorConfig, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); + turningMotor.configure(motorConfig, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); + + //enable PID wrapping for turning encoder + //turningPIDController.setPositionPIDWrappingEnabled(SwerveModuleConstants.kEnablePIDWrapping); // Deprecated + //turningPIDController.setPositionPIDWrappingMinInput(SwerveModuleConstants.kTurningEncoderPositionPIDMinInput); + //turningPIDController.setPositionPIDWrappingMaxInput(SwerveModuleConstants.kTurningEncoderPositionPIDMaxInput); + + //drivingMotor.setIdleMode(SwerveModuleConstants.kDrivingMotorIdleMode); // Deprecated + //turningMotor.setIdleMode(SwerveModuleConstants.kTurningMotorIdleMode); + //drivingMotor.setSmartCurrentLimit(SwerveModuleConstants.kDrivingMotorCurrentLimit); + //turningMotor.setSmartCurrentLimit(SwerveModuleConstants.kTurningMotorCurrentLimit); + + //save motor settings + //drivingMotor.burnFlash(); + //turningMotor.burnFlash(); + + drivingPIDController.setReference(SwerveModuleConstants.kDrivingMotorCurrentLimit, ControlType.kCurrent, ClosedLoopSlot.kSlot1); + turningPIDController.setReference(SwerveModuleConstants.kTurningMotorCurrentLimit, ControlType.kCurrent, ClosedLoopSlot.kSlot0); + + chassisAngularOffset = p_chassisAngularOffset; + desiredState.angle = new Rotation2d(turningEncoder.getPosition()); + optimizedState.angle = new Rotation2d(turningEncoder.getPosition()); + drivingEncoder.setPosition(0); + } + + public void setDesiredState(SwerveModuleState p_desiredState) + { + //make new desired state with angular offset + SwerveModuleState correctDesiredState = new SwerveModuleState(desiredState.speedMetersPerSecond, + desiredState.angle.plus(Rotation2d.fromRadians(chassisAngularOffset))); + + //make optimize desired state to make sure the wheel never turns more than 90 degrees + SwerveModuleState optimizedDesiredState = SwerveModuleState.optimize(correctDesiredState, + new Rotation2d(turningEncoder.getPosition())); + + //Command driving and turning motors to their respective setpoints + drivingPIDController.setReference(optimizedDesiredState.speedMetersPerSecond, SparkMax.ControlType.kVelocity); + turningPIDController.setReference(optimizedDesiredState.angle.getRadians(), SparkMax.ControlType.kPosition); + + desiredState = p_desiredState; + optimizedState = optimizedDesiredState; + } + + public void setTurnMotor(double percent) + { + turningMotor.set(percent); + } + + public void setDriveMotor(double percent) + { + drivingMotor.set(percent); + } + + public SwerveModuleState getDesiredState() + { + return desiredState; + } + + public SwerveModuleState getOptimizedState() + { + return optimizedState; + } + + public SwerveModuleState getModuleState() + { + return new SwerveModuleState(drivingEncoder.getVelocity(), + new Rotation2d(turningEncoder.getPosition() - chassisAngularOffset)); + } + + public SparkClosedLoopController getPIDController() + { + return drivingPIDController; + } + + public SwerveModulePosition getPosition() + { + return new SwerveModulePosition( + drivingEncoder.getPosition(), + new Rotation2d(turningEncoder.getPosition() - chassisAngularOffset) + ); + } + + public void resetEncoder() + { + drivingEncoder.setPosition(0); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java deleted file mode 100644 index 6fa86b1..0000000 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ /dev/null @@ -1,736 +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.swervedrive; - -import static edu.wpi.first.units.Units.Meter; - -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.commands.PathPlannerAuto; -import com.pathplanner.lib.commands.PathfindingCommand; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.path.PathPlannerPath; -import com.pathplanner.lib.util.DriveFeedforwards; -import com.pathplanner.lib.util.swerve.SwerveSetpoint; -import com.pathplanner.lib.util.swerve.SwerveSetpointGenerator; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -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.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; -import frc.robot.Constants; -import frc.robot.subsystems.swervedrive.Vision.Cameras; -import java.io.File; -import java.io.IOException; -import java.util.Arrays; -import java.util.Optional; -import java.util.concurrent.atomic.AtomicReference; -import java.util.function.DoubleSupplier; -import java.util.function.Supplier; -import org.json.simple.parser.ParseException; -import org.photonvision.targeting.PhotonPipelineResult; -import swervelib.SwerveController; -import swervelib.SwerveDrive; -import swervelib.SwerveDriveTest; -import swervelib.math.SwerveMath; -import swervelib.parser.SwerveControllerConfiguration; -import swervelib.parser.SwerveDriveConfiguration; -import swervelib.parser.SwerveParser; -import swervelib.telemetry.SwerveDriveTelemetry; -import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; - -public class SwerveSubsystem extends SubsystemBase -{ - - /** - * Swerve drive object. - */ - private final SwerveDrive swerveDrive; - /** - * AprilTag field layout. - */ - private final AprilTagFieldLayout aprilTagFieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo); - /** - * Enable vision odometry updates while driving. - */ - private final boolean visionDriveTest = false; - /** - * PhotonVision class to keep an accurate odometry. - */ - private Vision vision; - File directory = new File (Filesystem.getDeployDirectory(),"swerve"); - /** - * Initialize {@link SwerveDrive} with the directory provided. - * - * @param directory Directory of swerve drive config files. - */ - public SwerveSubsystem() - { - // Configure the Telemetry before creating the SwerveDrive to avoid unnecessary objects being created. - SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; - try - { - swerveDrive = new SwerveParser(directory).createSwerveDrive(Constants.MAX_SPEED, - new Pose2d(new Translation2d(Meter.of(1), - Meter.of(4)), - Rotation2d.fromDegrees(0))); - // Alternative method if you don't want to supply the conversion factor via JSON files. - // swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumSpeed, angleConversionFactor, driveConversionFactor); - } catch (Exception e) - { - throw new RuntimeException(e); - } - swerveDrive.setHeadingCorrection(false); // Heading correction should only be used while controlling the robot via angle. - swerveDrive.setCosineCompensator(false);//!SwerveDriveTelemetry.isSimulation); // Disables cosine compensation for simulations since it causes discrepancies not seen in real life. - swerveDrive.setAngularVelocityCompensation(true, - true, - 0.1); //Correct for skew that gets worse as angular velocity increases. Start with a coefficient of 0.1. - swerveDrive.setModuleEncoderAutoSynchronize(false, - 1); // Enable if you want to resynchronize your absolute encoders and motor encoders periodically when they are not moving. -// swerveDrive.pushOffsetsToEncoders(); // Set the absolute encoder to be used over the internal encoder and push the offsets onto it. Throws warning if not possible - if (visionDriveTest) - { - setupPhotonVision(); - // Stop the odometry thread if we are using vision that way we can synchronize updates better. - swerveDrive.stopOdometryThread(); - } - setupPathPlanner(); - } - - /** - * Construct the swerve drive. - * - * @param driveCfg SwerveDriveConfiguration for the swerve. - * @param controllerCfg Swerve Controller. - */ - public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg) - { - swerveDrive = new SwerveDrive(driveCfg, - controllerCfg, - Constants.MAX_SPEED, - new Pose2d(new Translation2d(Meter.of(2), Meter.of(0)), - Rotation2d.fromDegrees(0))); - } - - /** - * Setup the photon vision class. - */ - public void setupPhotonVision() - { - vision = new Vision(swerveDrive::getPose, swerveDrive.field); - } - - @Override - public void periodic() - { - // When vision is enabled we must manually update odometry in SwerveDrive - if (visionDriveTest) - { - swerveDrive.updateOdometry(); - vision.updatePoseEstimation(swerveDrive); - } - } - - @Override - public void simulationPeriodic() - { - } - - /** - * Setup AutoBuilder for PathPlanner. - */ - public void setupPathPlanner() - { - // Load the RobotConfig from the GUI settings. You should probably - // store this in your Constants file - RobotConfig config; - try - { - config = RobotConfig.fromGUISettings(); - - final boolean enableFeedforward = true; - // Configure AutoBuilder last - AutoBuilder.configure( - this::getPose, - // Robot pose supplier - this::resetOdometry, - // Method to reset odometry (will be called if your auto has a starting pose) - this::getRobotVelocity, - // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - (speedsRobotRelative, moduleFeedForwards) -> { - if (enableFeedforward) - { - swerveDrive.drive( - speedsRobotRelative, - swerveDrive.kinematics.toSwerveModuleStates(speedsRobotRelative), - moduleFeedForwards.linearForces() - ); - } else - { - swerveDrive.setChassisSpeeds(speedsRobotRelative); - } - }, - // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards - new PPHolonomicDriveController( - // PPHolonomicController is the built in path following controller for holonomic drive trains - new PIDConstants(5.0, 0.0, 0.0), - // Translation PID constants - new PIDConstants(5.0, 0.0, 0.0) - // Rotation PID constants - ), - config, - // The robot configuration - () -> { - // Boolean supplier that controls when the path will be mirrored for the red alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) - { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; - }, - this - // Reference to this subsystem to set requirements - ); - - } catch (Exception e) - { - // Handle exception as needed - e.printStackTrace(); - } - - //Preload PathPlanner Path finding - // IF USING CUSTOM PATHFINDER ADD BEFORE THIS LINE - PathfindingCommand.warmupCommand().schedule(); - } - - /** - * Aim the robot at the target returned by PhotonVision. - * - * @return A {@link Command} which will run the alignment. - */ - public Command aimAtTarget(Cameras camera) - { - - return run(() -> { - Optional resultO = camera.getBestResult(); - if (resultO.isPresent()) - { - var result = resultO.get(); - if (result.hasTargets()) - { - drive(getTargetSpeeds(0, - 0, - Rotation2d.fromDegrees(result.getBestTarget() - .getYaw()))); // Not sure if this will work, more math may be required. - } - } - }); - } - - /** - * Get the path follower with events. - * - * @param pathName PathPlanner path name. - * @return {@link AutoBuilder#followPath(PathPlannerPath)} path command. - */ - // public Command getAutonomousCommand(String pathName) - { - // Create a path following command using AutoBuilder. This will also trigger event markers. - //return new PathPlannerAuto(pathName); - } - - /** - * Use PathPlanner Path finding to go to a point on the field. - * - * @param pose Target {@link Pose2d} to go to. - * @return PathFinding command - */ - //public Command driveToPose(Pose2d pose) - { -// Create the constraints to use while pathfinding - // PathConstraints constraints = new PathConstraints( - //swerveDrive.getMaximumChassisVelocity(), 4.0, - //swerveDrive.getMaximumChassisAngularVelocity(), Units.degreesToRadians(720)); - -// Since AutoBuilder is configured, we can use it to build pathfinding commands - // return AutoBuilder.pathfindToPose( - // pose, - // constraints, - // edu.wpi.first.units.Units.MetersPerSecond.of(0) // Goal end velocity in meters/sec - // ); - } - - /** - * Drive with {@link SwerveSetpointGenerator} from 254, implemented by PathPlanner. - * - * @param robotRelativeChassisSpeed Robot relative {@link ChassisSpeeds} to achieve. - * @return {@link Command} to run. - * @throws IOException If the PathPlanner GUI settings is invalid - * @throws ParseException If PathPlanner GUI settings is nonexistent. - */ - private Command driveWithSetpointGenerator(Supplier robotRelativeChassisSpeed) - throws IOException, ParseException - { - SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator(RobotConfig.fromGUISettings(), - swerveDrive.getMaximumChassisAngularVelocity()); - AtomicReference prevSetpoint - = new AtomicReference<>(new SwerveSetpoint(swerveDrive.getRobotVelocity(), - swerveDrive.getStates(), - DriveFeedforwards.zeros(swerveDrive.getModules().length))); - AtomicReference previousTime = new AtomicReference<>(); - - return startRun(() -> previousTime.set(Timer.getFPGATimestamp()), - () -> { - double newTime = Timer.getFPGATimestamp(); - SwerveSetpoint newSetpoint = setpointGenerator.generateSetpoint(prevSetpoint.get(), - robotRelativeChassisSpeed.get(), - newTime - previousTime.get()); - swerveDrive.drive(newSetpoint.robotRelativeSpeeds(), - newSetpoint.moduleStates(), - newSetpoint.feedforwards().linearForces()); - prevSetpoint.set(newSetpoint); - previousTime.set(newTime); - - }); - } - - /** - * Drive with 254's Setpoint generator; port written by PathPlanner. - * - * @param fieldRelativeSpeeds Field-Relative {@link ChassisSpeeds} - * @return Command to drive the robot using the setpoint generator. - */ - public Command driveWithSetpointGeneratorFieldRelative(Supplier fieldRelativeSpeeds) - { - try - { - return driveWithSetpointGenerator(() -> { - return ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds.get(), getHeading()); - - }); - } catch (Exception e) - { - DriverStation.reportError(e.toString(), true); - } - return Commands.none(); - - } - - - /** - * Command to characterize the robot drive motors using SysId - * - * @return SysId Drive Command - */ - public Command sysIdDriveMotorCommand() - { - return SwerveDriveTest.generateSysIdCommand( - SwerveDriveTest.setDriveSysIdRoutine( - new Config(), - this, swerveDrive, 12, true), - 3.0, 5.0, 3.0); - } - - /** - * Command to characterize the robot angle motors using SysId - * - * @return SysId Angle Command - */ - public Command sysIdAngleMotorCommand() - { - return SwerveDriveTest.generateSysIdCommand( - SwerveDriveTest.setAngleSysIdRoutine( - new Config(), - this, swerveDrive), - 3.0, 5.0, 3.0); - } - - /** - * Returns a Command that centers the modules of the SwerveDrive subsystem. - * - * @return a Command that centers the modules of the SwerveDrive subsystem - */ - public Command centerModulesCommand() - { - return run(() -> Arrays.asList(swerveDrive.getModules()) - .forEach(it -> it.setAngle(0.0))); - } - - /** - * Returns a Command that drives the swerve drive to a specific distance at a given speed. - * - * @param distanceInMeters the distance to drive in meters - * @param speedInMetersPerSecond the speed at which to drive in meters per second - * @return a Command that drives the swerve drive to a specific distance at a given speed - */ - public Command driveToDistanceCommand(double distanceInMeters, double speedInMetersPerSecond) - { - return run(() -> drive(new ChassisSpeeds(speedInMetersPerSecond, 0, 0))) - .until(() -> swerveDrive.getPose().getTranslation().getDistance(new Translation2d(0, 0)) > - distanceInMeters); - } - - /** - * Replaces the swerve module feedforward with a new SimpleMotorFeedforward object. - * - * @param kS the static gain of the feedforward - * @param kV the velocity gain of the feedforward - * @param kA the acceleration gain of the feedforward - */ - public void replaceSwerveModuleFeedforward(double kS, double kV, double kA) - { - swerveDrive.replaceSwerveModuleFeedforward(new SimpleMotorFeedforward(kS, kV, kA)); - } - - /** - * Command to drive the robot using translative values and heading as angular velocity. - * - * @param translationX Translation in the X direction. Cubed for smoother controls. - * @param translationY Translation in the Y direction. Cubed for smoother controls. - * @param angularRotationX Angular velocity of the robot to set. Cubed for smoother controls. - * @return Drive command. - */ - public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier angularRotationX) - { - return run(() -> { - // Make the robot move - swerveDrive.drive(SwerveMath.scaleTranslation(new Translation2d( - translationX.getAsDouble() * swerveDrive.getMaximumChassisVelocity(), - translationY.getAsDouble() * swerveDrive.getMaximumChassisVelocity()), 0.8), - Math.pow(angularRotationX.getAsDouble(), 3) * swerveDrive.getMaximumChassisAngularVelocity(), - true, - false); - }); - } - - /** - * Command to drive the robot using translative values and heading as a setpoint. - * - * @param translationX Translation in the X direction. Cubed for smoother controls. - * @param translationY Translation in the Y direction. Cubed for smoother controls. - * @param headingX Heading X to calculate angle of the joystick. - * @param headingY Heading Y to calculate angle of the joystick. - * @return Drive command. - */ - public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier headingX, - DoubleSupplier headingY) - { - // swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for this kind of control. - return run(() -> { - - Translation2d scaledInputs = SwerveMath.scaleTranslation(new Translation2d(translationX.getAsDouble(), - translationY.getAsDouble()), 0.8); - - // Make the robot move - driveFieldOriented(swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(), scaledInputs.getY(), - headingX.getAsDouble(), - headingY.getAsDouble(), - swerveDrive.getOdometryHeading().getRadians(), - swerveDrive.getMaximumChassisVelocity())); - }); - } - - /** - * The primary method for controlling the drivebase. Takes a {@link Translation2d} and a rotation rate, and - * calculates and commands module states accordingly. Can use either open-loop or closed-loop velocity control for - * the wheel velocities. Also has field- and robot-relative modes, which affect how the translation vector is used. - * - * @param translation {@link Translation2d} that is the commanded linear velocity of the robot, in meters per - * second. In robot-relative mode, positive x is torwards the bow (front) and positive y is - * torwards port (left). In field-relative mode, positive x is away from the alliance wall - * (field North) and positive y is torwards the left wall when looking through the driver station - * glass (field West). - * @param rotation Robot angular rate, in radians per second. CCW positive. Unaffected by field/robot - * relativity. - * @param fieldRelative Drive mode. True for field-relative, false for robot-relative. - */ - public void drive(Translation2d translation, double rotation, boolean fieldRelative) - { - swerveDrive.drive(translation, - rotation, - fieldRelative, - false); // Open loop is disabled since it shouldn't be used most of the time. - } - - /** - * Drive the robot given a chassis field oriented velocity. - * - * @param velocity Velocity according to the field. - */ - public void driveFieldOriented(ChassisSpeeds velocity) - { - swerveDrive.driveFieldOriented(velocity); - } - - /** - * Drive the robot given a chassis field oriented velocity. - * - * @param velocity Velocity according to the field. - */ - public Command driveFieldOriented(Supplier velocity) - { - return run(() -> { - swerveDrive.driveFieldOriented(velocity.get()); - }); - } - - /** - * Drive according to the chassis robot oriented velocity. - * - * @param velocity Robot oriented {@link ChassisSpeeds} - */ - public void drive(ChassisSpeeds velocity) - { - swerveDrive.drive(velocity); - } - - - /** - * Get the swerve drive kinematics object. - * - * @return {@link SwerveDriveKinematics} of the swerve drive. - */ - public SwerveDriveKinematics getKinematics() - { - return swerveDrive.kinematics; - } - - /** - * Resets odometry to the given pose. Gyro angle and module positions do not need to be reset when calling this - * method. However, if either gyro angle or module position is reset, this must be called in order for odometry to - * keep working. - * - * @param initialHolonomicPose The pose to set the odometry to - */ - public void resetOdometry(Pose2d initialHolonomicPose) - { - swerveDrive.resetOdometry(initialHolonomicPose); - } - - /** - * Gets the current pose (position and rotation) of the robot, as reported by odometry. - * - * @return The robot's pose - */ - public Pose2d getPose() - { - return swerveDrive.getPose(); - } - - /** - * Set chassis speeds with closed-loop velocity control. - * - * @param chassisSpeeds Chassis Speeds to set. - */ - public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) - { - swerveDrive.setChassisSpeeds(chassisSpeeds); - } - - /** - * Post the trajectory to the field. - * - * @param trajectory The trajectory to post. - */ - public void postTrajectory(Trajectory trajectory) - { - swerveDrive.postTrajectory(trajectory); - } - - /** - * Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0. - */ - public void zeroGyro() - { - swerveDrive.zeroGyro(); - } - - /** - * Checks if the alliance is red, defaults to false if alliance isn't available. - * - * @return true if the red alliance, false if blue. Defaults to false if none is available. - */ - private boolean isRedAlliance() - { - var alliance = DriverStation.getAlliance(); - return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false; - } - - /** - * This will zero (calibrate) the robot to assume the current position is facing forward - *

- * If red alliance rotate the robot 180 after the drviebase zero command - */ - public void zeroGyroWithAlliance() - { - if (isRedAlliance()) - { - zeroGyro(); - //Set the pose 180 degrees - resetOdometry(new Pose2d(getPose().getTranslation(), Rotation2d.fromDegrees(180))); - } else - { - zeroGyro(); - } - } - - /** - * Sets the drive motors to brake/coast mode. - * - * @param brake True to set motors to brake mode, false for coast. - */ - public void setMotorBrake(boolean brake) - { - swerveDrive.setMotorIdleMode(brake); - } - - /** - * Gets the current yaw angle of the robot, as reported by the swerve pose estimator in the underlying drivebase. - * Note, this is not the raw gyro reading, this may be corrected from calls to resetOdometry(). - * - * @return The yaw angle - */ - public Rotation2d getHeading() - { - return getPose().getRotation(); - } - - /** - * Get the chassis speeds based on controller input of 2 joysticks. One for speeds in which direction. The other for - * the angle of the robot. - * - * @param xInput X joystick input for the robot to move in the X direction. - * @param yInput Y joystick input for the robot to move in the Y direction. - * @param headingX X joystick which controls the angle of the robot. - * @param headingY Y joystick which controls the angle of the robot. - * @return {@link ChassisSpeeds} which can be sent to the Swerve Drive. - */ - public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headingX, double headingY) - { - Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput)); - return swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(), - scaledInputs.getY(), - headingX, - headingY, - getHeading().getRadians(), - Constants.MAX_SPEED); - } - - /** - * Get the chassis speeds based on controller input of 1 joystick and one angle. Control the robot at an offset of - * 90deg. - * - * @param xInput X joystick input for the robot to move in the X direction. - * @param yInput Y joystick input for the robot to move in the Y direction. - * @param angle The angle in as a {@link Rotation2d}. - * @return {@link ChassisSpeeds} which can be sent to the Swerve Drive. - */ - public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d angle) - { - Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput)); - - return swerveDrive.swerveController.getTargetSpeeds(scaledInputs.getX(), - scaledInputs.getY(), - angle.getRadians(), - getHeading().getRadians(), - Constants.MAX_SPEED); - } - - /** - * Gets the current field-relative velocity (x, y and omega) of the robot - * - * @return A ChassisSpeeds object of the current field-relative velocity - */ - public ChassisSpeeds getFieldVelocity() - { - return swerveDrive.getFieldVelocity(); - } - - /** - * Gets the current velocity (x, y and omega) of the robot - * - * @return A {@link ChassisSpeeds} object of the current velocity - */ - public ChassisSpeeds getRobotVelocity() - { - return swerveDrive.getRobotVelocity(); - } - - /** - * Get the {@link SwerveController} in the swerve drive. - * - * @return {@link SwerveController} from the {@link SwerveDrive}. - */ - public SwerveController getSwerveController() - { - return swerveDrive.swerveController; - } - - /** - * Get the {@link SwerveDriveConfiguration} object. - * - * @return The {@link SwerveDriveConfiguration} fpr the current drive. - */ - public SwerveDriveConfiguration getSwerveDriveConfiguration() - { - return swerveDrive.swerveDriveConfiguration; - } - - /** - * Lock the swerve drive to prevent it from moving. - */ - public void lock() - { - swerveDrive.lockPose(); - } - - /** - * Gets the current pitch angle of the robot, as reported by the imu. - * - * @return The heading as a {@link Rotation2d} angle - */ - public Rotation2d getPitch() - { - return swerveDrive.getPitch(); - } - - /** - * Add a fake vision reading for testing purposes. - */ - public void addFakeVisionReading() - { - swerveDrive.addVisionMeasurement(new Pose2d(3, 3, Rotation2d.fromDegrees(65)), Timer.getFPGATimestamp()); - } - - /** - * Gets the swerve drive object. - * - * @return {@link SwerveDrive} - */ - public SwerveDrive getSwerveDrive() - { - return swerveDrive; - } -} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java deleted file mode 100644 index 496ea70..0000000 --- a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java +++ /dev/null @@ -1,636 +0,0 @@ -package frc.robot.subsystems.swervedrive; - -import static edu.wpi.first.units.Units.Microseconds; -import static edu.wpi.first.units.Units.Milliseconds; -import static edu.wpi.first.units.Units.Seconds; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.Matrix; -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 edu.wpi.first.networktables.NetworkTablesJNI; -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.Alert.AlertType; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import frc.robot.Robot; -import java.awt.Desktop; -import java.util.ArrayList; -import java.util.List; -import java.util.Optional; -import java.util.function.Supplier; -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.PhotonUtils; -import org.photonvision.simulation.PhotonCameraSim; -import org.photonvision.simulation.SimCameraProperties; -import org.photonvision.simulation.VisionSystemSim; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; -import swervelib.SwerveDrive; -import swervelib.telemetry.SwerveDriveTelemetry; - - -/** - * Example PhotonVision class to aid in the pursuit of accurate odometry. Taken from - * https://gitlab.com/ironclad_code/ironclad-2024/-/blob/master/src/main/java/frc/robot/vision/Vision.java?ref_type=heads - */ -public class Vision -{ - - /** - * April Tag Field Layout of the year. - */ - public static final AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField( - AprilTagFields.k2025Reefscape); - /** - * Ambiguity defined as a value between (0,1). Used in {@link Vision#filterPose}. - */ - private final double maximumAmbiguity = 0.25; - /** - * Photon Vision Simulation - */ - public VisionSystemSim visionSim; - /** - * Count of times that the odom thinks we're more than 10meters away from the april tag. - */ - private double longDistangePoseEstimationCount = 0; - /** - * Current pose from the pose estimator using wheel odometry. - */ - private Supplier currentPose; - /** - * Field from {@link swervelib.SwerveDrive#field} - */ - private Field2d field2d; - - - /** - * Constructor for the Vision class. - * - * @param currentPose Current pose supplier, should reference {@link SwerveDrive#getPose()} - * @param field Current field, should be {@link SwerveDrive#field} - */ - public Vision(Supplier currentPose, Field2d field) - { - this.currentPose = currentPose; - this.field2d = field; - - if (Robot.isSimulation()) - { - visionSim = new VisionSystemSim("Vision"); - visionSim.addAprilTags(fieldLayout); - - for (Cameras c : Cameras.values()) - { - c.addToVisionSim(visionSim); - } - - openSimCameraViews(); - } - } - - /** - * Calculates a target pose relative to an AprilTag on the field. - * - * @param aprilTag The ID of the AprilTag. - * @param robotOffset The offset {@link Transform2d} of the robot to apply to the pose for the robot to position - * itself correctly. - * @return The target pose of the AprilTag. - */ - public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset) - { - Optional aprilTagPose3d = fieldLayout.getTagPose(aprilTag); - if (aprilTagPose3d.isPresent()) - { - return aprilTagPose3d.get().toPose2d().transformBy(robotOffset); - } else - { - throw new RuntimeException("Cannot get AprilTag " + aprilTag + " from field " + fieldLayout.toString()); - } - - } - - /** - * Update the pose estimation inside of {@link SwerveDrive} with all of the given poses. - * - * @param swerveDrive {@link SwerveDrive} instance. - */ - public void updatePoseEstimation(SwerveDrive swerveDrive) - { - if (SwerveDriveTelemetry.isSimulation && swerveDrive.getSimulationDriveTrainPose().isPresent()) - { - /* - * In the maple-sim, odometry is simulated using encoder values, accounting for factors like skidding and drifting. - * As a result, the odometry may not always be 100% accurate. - * However, the vision system should be able to provide a reasonably accurate pose estimation, even when odometry is incorrect. - * (This is why teams implement vision system to correct odometry.) - * Therefore, we must ensure that the actual robot pose is provided in the simulator when updating the vision simulation during the simulation. - */ - visionSim.update(swerveDrive.getSimulationDriveTrainPose().get()); - } - for (Cameras camera : Cameras.values()) - { - Optional poseEst = getEstimatedGlobalPose(camera); - if (poseEst.isPresent()) - { - var pose = poseEst.get(); - swerveDrive.addVisionMeasurement(pose.estimatedPose.toPose2d(), - pose.timestampSeconds, - camera.curStdDevs); - } - } - - } - - /** - * Generates the estimated robot pose. Returns empty if: - *

    - *
  • No Pose Estimates could be generated
    • - *
  • The generated pose estimate was considered not accurate
    • - *
- * - * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to create the estimate - */ - public Optional getEstimatedGlobalPose(Cameras camera) - { - Optional poseEst = camera.getEstimatedGlobalPose(); - if (Robot.isSimulation()) - { - Field2d debugField = visionSim.getDebugField(); - // Uncomment to enable outputting of vision targets in sim. - poseEst.ifPresentOrElse( - est -> - debugField - .getObject("VisionEstimation") - .setPose(est.estimatedPose.toPose2d()), - () -> { - debugField.getObject("VisionEstimation").setPoses(); - }); - } - return poseEst; - } - - - /** - * Filter pose via the ambiguity and find best estimate between all of the camera's throwing out distances more than - * 10m for a short amount of time. - * - * @param pose Estimated robot pose. - * @return Could be empty if there isn't a good reading. - */ - @Deprecated(since = "2024", forRemoval = true) - private Optional filterPose(Optional pose) - { - if (pose.isPresent()) - { - double bestTargetAmbiguity = 1; // 1 is max ambiguity - for (PhotonTrackedTarget target : pose.get().targetsUsed) - { - double ambiguity = target.getPoseAmbiguity(); - if (ambiguity != -1 && ambiguity < bestTargetAmbiguity) - { - bestTargetAmbiguity = ambiguity; - } - } - //ambiguity to high dont use estimate - if (bestTargetAmbiguity > maximumAmbiguity) - { - return Optional.empty(); - } - - //est pose is very far from recorded robot pose - if (PhotonUtils.getDistanceToPose(currentPose.get(), pose.get().estimatedPose.toPose2d()) > 1) - { - longDistangePoseEstimationCount++; - - //if it calculates that were 10 meter away for more than 10 times in a row its probably right - if (longDistangePoseEstimationCount < 10) - { - return Optional.empty(); - } - } else - { - longDistangePoseEstimationCount = 0; - } - return pose; - } - return Optional.empty(); - } - - - /** - * Get distance of the robot from the AprilTag pose. - * - * @param id AprilTag ID - * @return Distance - */ - public double getDistanceFromAprilTag(int id) - { - Optional tag = fieldLayout.getTagPose(id); - return tag.map(pose3d -> PhotonUtils.getDistanceToPose(currentPose.get(), pose3d.toPose2d())).orElse(-1.0); - } - - /** - * Get tracked target from a camera of AprilTagID - * - * @param id AprilTag ID - * @param camera Camera to check. - * @return Tracked target. - */ - public PhotonTrackedTarget getTargetFromId(int id, Cameras camera) - { - PhotonTrackedTarget target = null; - for (PhotonPipelineResult result : camera.resultsList) - { - if (result.hasTargets()) - { - for (PhotonTrackedTarget i : result.getTargets()) - { - if (i.getFiducialId() == id) - { - return i; - } - } - } - } - return target; - - } - - /** - * Vision simulation. - * - * @return Vision Simulation - */ - public VisionSystemSim getVisionSim() - { - return visionSim; - } - - /** - * Open up the photon vision camera streams on the localhost, assumes running photon vision on localhost. - */ - private void openSimCameraViews() - { - if (Desktop.isDesktopSupported() && Desktop.getDesktop().isSupported(Desktop.Action.BROWSE)) - { -// try -// { -// Desktop.getDesktop().browse(new URI("http://localhost:1182/")); -// Desktop.getDesktop().browse(new URI("http://localhost:1184/")); -// Desktop.getDesktop().browse(new URI("http://localhost:1186/")); -// } catch (IOException | URISyntaxException e) -// { -// e.printStackTrace(); -// } - } - } - - /** - * Update the {@link Field2d} to include tracked targets/ - */ - public void updateVisionField() - { - - List targets = new ArrayList(); - for (Cameras c : Cameras.values()) - { - if (!c.resultsList.isEmpty()) - { - PhotonPipelineResult latest = c.resultsList.get(0); - if (latest.hasTargets()) - { - targets.addAll(latest.targets); - } - } - } - - List poses = new ArrayList<>(); - for (PhotonTrackedTarget target : targets) - { - if (fieldLayout.getTagPose(target.getFiducialId()).isPresent()) - { - Pose2d targetPose = fieldLayout.getTagPose(target.getFiducialId()).get().toPose2d(); - poses.add(targetPose); - } - } - - field2d.getObject("tracked targets").setPoses(poses); - } - - /** - * Camera Enum to select each camera - */ - enum Cameras - { - /** - * Left Camera - */ - LEFT_CAM("left", - new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(30)), - new Translation3d(Units.inchesToMeters(12.056), - Units.inchesToMeters(10.981), - Units.inchesToMeters(8.44)), - VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)), - /** - * Right Camera - */ - RIGHT_CAM("right", - new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(-30)), - new Translation3d(Units.inchesToMeters(12.056), - Units.inchesToMeters(-10.981), - Units.inchesToMeters(8.44)), - VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)), - /** - * Center Camera - */ - CENTER_CAM("center", - new Rotation3d(0, Units.degreesToRadians(18), 0), - new Translation3d(Units.inchesToMeters(-4.628), - Units.inchesToMeters(-10.687), - Units.inchesToMeters(16.129)), - VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)); - - /** - * Latency alert to use when high latency is detected. - */ - public final Alert latencyAlert; - /** - * Camera instance for comms. - */ - public final PhotonCamera camera; - /** - * Pose estimator for camera. - */ - public final PhotonPoseEstimator poseEstimator; - /** - * Standard Deviation for single tag readings for pose estimation. - */ - private final Matrix singleTagStdDevs; - /** - * Standard deviation for multi-tag readings for pose estimation. - */ - private final Matrix multiTagStdDevs; - /** - * Transform of the camera rotation and translation relative to the center of the robot - */ - private final Transform3d robotToCamTransform; - /** - * Current standard deviations used. - */ - public Matrix curStdDevs; - /** - * Estimated robot pose. - */ - public Optional estimatedRobotPose; - /** - * Simulated camera instance which only exists during simulations. - */ - public PhotonCameraSim cameraSim; - /** - * Results list to be updated periodically and cached to avoid unnecessary queries. - */ - public List resultsList = new ArrayList<>(); - /** - * Last read from the camera timestamp to prevent lag due to slow data fetches. - */ - private double lastReadTimestamp = Microseconds.of(NetworkTablesJNI.now()).in(Seconds); - - /** - * Construct a Photon Camera class with help. Standard deviations are fake values, experiment and determine - * estimation noise on an actual robot. - * - * @param name Name of the PhotonVision camera found in the PV UI. - * @param robotToCamRotation {@link Rotation3d} of the camera. - * @param robotToCamTranslation {@link Translation3d} relative to the center of the robot. - * @param singleTagStdDevs Single AprilTag standard deviations of estimated poses from the camera. - * @param multiTagStdDevsMatrix Multi AprilTag standard deviations of estimated poses from the camera. - */ - Cameras(String name, Rotation3d robotToCamRotation, Translation3d robotToCamTranslation, - Matrix singleTagStdDevs, Matrix multiTagStdDevsMatrix) - { - latencyAlert = new Alert("'" + name + "' Camera is experiencing high latency.", AlertType.kWarning); - - camera = new PhotonCamera(name); - - // https://docs.wpilib.org/en/stable/docs/software/basic-programming/coordinate-system.html - robotToCamTransform = new Transform3d(robotToCamTranslation, robotToCamRotation); - - poseEstimator = new PhotonPoseEstimator(Vision.fieldLayout, - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - robotToCamTransform); - poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); - - this.singleTagStdDevs = singleTagStdDevs; - this.multiTagStdDevs = multiTagStdDevsMatrix; - - if (Robot.isSimulation()) - { - SimCameraProperties cameraProp = new SimCameraProperties(); - // A 640 x 480 camera with a 100 degree diagonal FOV. - cameraProp.setCalibration(960, 720, Rotation2d.fromDegrees(100)); - // Approximate detection noise with average and standard deviation error in pixels. - cameraProp.setCalibError(0.25, 0.08); - // Set the camera image capture framerate (Note: this is limited by robot loop rate). - cameraProp.setFPS(30); - // The average and standard deviation in milliseconds of image data latency. - cameraProp.setAvgLatencyMs(35); - cameraProp.setLatencyStdDevMs(5); - - cameraSim = new PhotonCameraSim(camera, cameraProp); - cameraSim.enableDrawWireframe(true); - } - } - - /** - * Add camera to {@link VisionSystemSim} for simulated photon vision. - * - * @param systemSim {@link VisionSystemSim} to use. - */ - public void addToVisionSim(VisionSystemSim systemSim) - { - if (Robot.isSimulation()) - { - systemSim.addCamera(cameraSim, robotToCamTransform); - } - } - - /** - * Get the result with the least ambiguity from the best tracked target within the Cache. This may not be the most - * recent result! - * - * @return The result in the cache with the least ambiguous best tracked target. This is not the most recent result! - */ - public Optional getBestResult() - { - if (resultsList.isEmpty()) - { - return Optional.empty(); - } - - PhotonPipelineResult bestResult = resultsList.get(0); - double amiguity = bestResult.getBestTarget().getPoseAmbiguity(); - double currentAmbiguity = 0; - for (PhotonPipelineResult result : resultsList) - { - currentAmbiguity = result.getBestTarget().getPoseAmbiguity(); - if (currentAmbiguity < amiguity && currentAmbiguity > 0) - { - bestResult = result; - amiguity = currentAmbiguity; - } - } - return Optional.of(bestResult); - } - - /** - * Get the latest result from the current cache. - * - * @return Empty optional if nothing is found. Latest result if something is there. - */ - public Optional getLatestResult() - { - return resultsList.isEmpty() ? Optional.empty() : Optional.of(resultsList.get(0)); - } - - /** - * Get the estimated robot pose. Updates the current robot pose estimation, standard deviations, and flushes the - * cache of results. - * - * @return Estimated pose. - */ - public Optional getEstimatedGlobalPose() - { - updateUnreadResults(); - return estimatedRobotPose; - } - - /** - * Update the latest results, cached with a maximum refresh rate of 1req/15ms. Sorts the list by timestamp. - */ - private void updateUnreadResults() - { - double mostRecentTimestamp = resultsList.isEmpty() ? 0.0 : resultsList.get(0).getTimestampSeconds(); - double currentTimestamp = Microseconds.of(NetworkTablesJNI.now()).in(Seconds); - double debounceTime = Milliseconds.of(15).in(Seconds); - for (PhotonPipelineResult result : resultsList) - { - mostRecentTimestamp = Math.max(mostRecentTimestamp, result.getTimestampSeconds()); - } - if ((resultsList.isEmpty() || (currentTimestamp - mostRecentTimestamp >= debounceTime)) && - (currentTimestamp - lastReadTimestamp) >= debounceTime) - { - resultsList = Robot.isReal() ? camera.getAllUnreadResults() : cameraSim.getCamera().getAllUnreadResults(); - lastReadTimestamp = currentTimestamp; - resultsList.sort((PhotonPipelineResult a, PhotonPipelineResult b) -> { - return a.getTimestampSeconds() >= b.getTimestampSeconds() ? 1 : -1; - }); - if (!resultsList.isEmpty()) - { - updateEstimatedGlobalPose(); - } - } - } - - /** - * The latest estimated robot pose on the field from vision data. This may be empty. This should only be called once - * per loop. - * - *

Also includes updates for the standard deviations, which can (optionally) be retrieved with - * {@link Cameras#updateEstimationStdDevs} - * - * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets used for - * estimation. - */ - private void updateEstimatedGlobalPose() - { - Optional visionEst = Optional.empty(); - for (var change : resultsList) - { - visionEst = poseEstimator.update(change); - updateEstimationStdDevs(visionEst, change.getTargets()); - } - estimatedRobotPose = visionEst; - } - - /** - * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard deviations based - * on number of tags, estimation strategy, and distance from the tags. - * - * @param estimatedPose The estimated pose to guess standard deviations for. - * @param targets All targets in this camera frame - */ - private void updateEstimationStdDevs( - Optional estimatedPose, List targets) - { - if (estimatedPose.isEmpty()) - { - // No pose input. Default to single-tag std devs - curStdDevs = singleTagStdDevs; - - } else - { - // Pose present. Start running Heuristic - var estStdDevs = singleTagStdDevs; - int numTags = 0; - double avgDist = 0; - - // Precalculation - see how many tags we found, and calculate an average-distance metric - for (var tgt : targets) - { - var tagPose = poseEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); - if (tagPose.isEmpty()) - { - continue; - } - numTags++; - avgDist += - tagPose - .get() - .toPose2d() - .getTranslation() - .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); - } - - if (numTags == 0) - { - // No tags visible. Default to single-tag std devs - curStdDevs = singleTagStdDevs; - } else - { - // One or more tags visible, run the full heuristic. - avgDist /= numTags; - // Decrease std devs if multiple targets are visible - if (numTags > 1) - { - estStdDevs = multiTagStdDevs; - } - // Increase std devs based on (average) distance - if (numTags == 1 && avgDist > 4) - { - estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); - } else - { - estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); - } - curStdDevs = estStdDevs; - } - } - } - - - } - -} diff --git a/src/main/java/frc/utils/JoystickUtils.java b/src/main/java/frc/utils/JoystickUtils.java deleted file mode 100644 index 8399d8e..0000000 --- a/src/main/java/frc/utils/JoystickUtils.java +++ /dev/null @@ -1,26 +0,0 @@ -package frc.utils; - -public class JoystickUtils -{ - public JoystickUtils(){ } - - /**Method that will dampen the joystick inputs - * - * @param value the joystick input - * @param deadband Range around zero. - * @param sensitivity how much sensativity will be applied. This is a number between [0,1] - * @return returns new value of joystick input - */ - public static double applySensitivity(double value, double deadband, double sensitivity) - { - //if joystick value is below the deadband return 0 - if(Math.abs(value) <= deadband) - { - return 0; - } - double newValue = sensitivity * Math.pow(value, 3); - newValue += (1-sensitivity) * value; - - return newValue; - } -} diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json new file mode 100644 index 0000000..c86a80d --- /dev/null +++ b/vendordeps/NavX.json @@ -0,0 +1,39 @@ +{ + "fileName": "NavX.json", + "name": "NavX", + "version": "2025.1.1-beta-1", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "frcYear": "2025", + "mavenUrls": [ + "https://dev.studica.com/maven/release/2025/" + ], + "jsonUrl": "https://dev.studica.com/releases/2025/NavX.json", + "javaDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx_frc-java", + "version": "2025.1.1-beta-1" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx_frc-cpp", + "version": "2025.1.1-beta-1", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": false, + "libName": "navx_frc", + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/PathplannerLib-2025.2.2.json b/vendordeps/PathplannerLib-2025.2.3.json similarity index 87% rename from vendordeps/PathplannerLib-2025.2.2.json rename to vendordeps/PathplannerLib-2025.2.3.json index a5bf9ee..9151ce4 100644 --- a/vendordeps/PathplannerLib-2025.2.2.json +++ b/vendordeps/PathplannerLib-2025.2.3.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.2.2.json", + "fileName": "PathplannerLib-2025.2.3.json", "name": "PathplannerLib", - "version": "2025.2.2", + "version": "2025.2.3", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.2.2" + "version": "2025.2.3" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.2.2", + "version": "2025.2.3", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix5-5.35.1.json b/vendordeps/Phoenix5-5.35.1.json deleted file mode 100644 index 69df8b5..0000000 --- a/vendordeps/Phoenix5-5.35.1.json +++ /dev/null @@ -1,171 +0,0 @@ -{ - "fileName": "Phoenix5-5.35.1.json", - "name": "CTRE-Phoenix (v5)", - "version": "5.35.1", - "frcYear": "2025", - "uuid": "ab676553-b602-441f-a38d-f1296eff6537", - "mavenUrls": [ - "https://maven.ctr-electronics.com/release/" - ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2025-latest.json", - "requires": [ - { - "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", - "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", - "offlineFileName": "Phoenix6-frc2025-latest.json", - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json" - } - ], - "conflictsWith": [ - { - "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", - "errorMessage": "Users must use the Phoenix 5 replay vendordep when using the Phoenix 6 replay vendordep.", - "offlineFileName": "Phoenix6-replay-frc2025-latest.json" - }, - { - "uuid": "fbc886a4-2cec-40c0-9835-71086a8cc3df", - "errorMessage": "Users cannot have both the replay and regular Phoenix 5 vendordeps in their robot program.", - "offlineFileName": "Phoenix5-replay-frc2025-latest.json" - } - ], - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-java", - "version": "5.35.1" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-java", - "version": "5.35.1" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.35.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.35.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-cpp", - "version": "5.35.1", - "libName": "CTRE_Phoenix_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-cpp", - "version": "5.35.1", - "libName": "CTRE_Phoenix", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.35.1", - "libName": "CTRE_PhoenixCCI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "5.35.1", - "libName": "CTRE_Phoenix_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "api-cpp-sim", - "version": "5.35.1", - "libName": "CTRE_PhoenixSim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.35.1", - "libName": "CTRE_PhoenixCCISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - } - ] -} \ No newline at end of file diff --git a/vendordeps/Phoenix6-25.2.1.json b/vendordeps/Phoenix6-25.2.1.json deleted file mode 100644 index 1397da1..0000000 --- a/vendordeps/Phoenix6-25.2.1.json +++ /dev/null @@ -1,419 +0,0 @@ -{ - "fileName": "Phoenix6-25.2.1.json", - "name": "CTRE-Phoenix (v6)", - "version": "25.2.1", - "frcYear": "2025", - "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", - "mavenUrls": [ - "https://maven.ctr-electronics.com/release/" - ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json", - "conflictsWith": [ - { - "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", - "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", - "offlineFileName": "Phoenix6-replay-frc2025-latest.json" - } - ], - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-java", - "version": "25.2.1" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "api-cpp", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "api-cpp-sim", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFXS", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANrange", - "version": "25.2.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-cpp", - "version": "25.2.1", - "libName": "CTRE_Phoenix6_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "25.2.1", - "libName": "CTRE_PhoenixTools", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "25.2.1", - "libName": "CTRE_Phoenix6_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "25.2.1", - "libName": "CTRE_PhoenixTools_Sim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "25.2.1", - "libName": "CTRE_SimTalonSRX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "25.2.1", - "libName": "CTRE_SimVictorSPX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "25.2.1", - "libName": "CTRE_SimPigeonIMU", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simCANCoder", - "version": "25.2.1", - "libName": "CTRE_SimCANCoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "25.2.1", - "libName": "CTRE_SimProTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFXS", - "version": "25.2.1", - "libName": "CTRE_SimProTalonFXS", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "25.2.1", - "libName": "CTRE_SimProCANcoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "25.2.1", - "libName": "CTRE_SimProPigeon2", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANrange", - "version": "25.2.1", - "libName": "CTRE_SimProCANrange", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - } - ] -} \ No newline at end of file diff --git a/vendordeps/REVLib-2025.0.2.json b/vendordeps/REVLib.json similarity index 98% rename from vendordeps/REVLib-2025.0.2.json rename to vendordeps/REVLib.json index c29aefa..7194603 100644 --- a/vendordeps/REVLib-2025.0.2.json +++ b/vendordeps/REVLib.json @@ -1,5 +1,5 @@ { - "fileName": "REVLib-2025.0.2.json", + "fileName": "REVLib.json", "name": "REVLib", "version": "2025.0.2", "frcYear": "2025", diff --git a/vendordeps/ReduxLib-2025.0.1.json b/vendordeps/ReduxLib-2025.0.1.json deleted file mode 100644 index 6cc750e..0000000 --- a/vendordeps/ReduxLib-2025.0.1.json +++ /dev/null @@ -1,72 +0,0 @@ -{ - "fileName": "ReduxLib-2025.0.1.json", - "name": "ReduxLib", - "version": "2025.0.1", - "frcYear": "2025", - "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", - "mavenUrls": [ - "https://maven.reduxrobotics.com/" - ], - "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2025.json", - "javaDependencies": [ - { - "groupId": "com.reduxrobotics.frc", - "artifactId": "ReduxLib-java", - "version": "2025.0.1" - } - ], - "jniDependencies": [ - { - "groupId": "com.reduxrobotics.frc", - "artifactId": "ReduxLib-driver", - "version": "2025.0.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxathena", - "linuxx86-64", - "linuxarm32", - "linuxarm64", - "osxuniversal", - "windowsx86-64" - ] - } - ], - "cppDependencies": [ - { - "groupId": "com.reduxrobotics.frc", - "artifactId": "ReduxLib-cpp", - "version": "2025.0.1", - "libName": "ReduxLib", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxx86-64", - "linuxarm32", - "linuxarm64", - "osxuniversal", - "windowsx86-64" - ] - }, - { - "groupId": "com.reduxrobotics.frc", - "artifactId": "ReduxLib-driver", - "version": "2025.0.1", - "libName": "ReduxCore", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxx86-64", - "linuxarm32", - "linuxarm64", - "osxuniversal", - "windowsx86-64" - ] - } - ] -} \ No newline at end of file diff --git a/vendordeps/Studica-2025.0.1.json b/vendordeps/Studica-2025.0.1.json deleted file mode 100644 index 5010be0..0000000 --- a/vendordeps/Studica-2025.0.1.json +++ /dev/null @@ -1,71 +0,0 @@ -{ - "fileName": "Studica-2025.0.1.json", - "name": "Studica", - "version": "2025.0.1", - "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", - "frcYear": "2025", - "mavenUrls": [ - "https://dev.studica.com/maven/release/2025/" - ], - "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.1.json", - "cppDependencies": [ - { - "artifactId": "Studica-cpp", - "binaryPlatforms": [ - "linuxathena", - "linuxarm32", - "linuxarm64", - "linuxx86-64", - "osxuniversal", - "windowsx86-64" - ], - "groupId": "com.studica.frc", - "headerClassifier": "headers", - "libName": "Studica", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "version": "2025.0.1" - }, - { - "artifactId": "Studica-driver", - "binaryPlatforms": [ - "linuxathena", - "linuxarm32", - "linuxarm64", - "linuxx86-64", - "osxuniversal", - "windowsx86-64" - ], - "groupId": "com.studica.frc", - "headerClassifier": "headers", - "libName": "StudicaDriver", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "version": "2025.0.1" - } - ], - "javaDependencies": [ - { - "artifactId": "Studica-java", - "groupId": "com.studica.frc", - "version": "2025.0.1" - } - ], - "jniDependencies": [ - { - "artifactId": "Studica-driver", - "groupId": "com.studica.frc", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxathena", - "linuxarm32", - "linuxarm64", - "linuxx86-64", - "osxuniversal", - "windowsx86-64" - ], - "version": "2025.0.1" - } - ] -} \ No newline at end of file diff --git a/vendordeps/ThriftyLib.json b/vendordeps/ThriftyLib.json deleted file mode 100644 index 00d3f73..0000000 --- a/vendordeps/ThriftyLib.json +++ /dev/null @@ -1,20 +0,0 @@ -{ - "fileName": "ThriftyLib.json", - "name": "ThriftyLib", - "version": "2025.0.2", - "frcYear": "2025", - "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0", - "mavenUrls": [ - "https://docs.home.thethriftybot.com" - ], - "jsonUrl": "https://docs.home.thethriftybot.com/ThriftyLib.json", - "javaDependencies": [ - { - "groupId": "com.thethriftybot.frc", - "artifactId": "ThriftyLib-java", - "version": "2025.0.2" - } - ], - "jniDependencies": [], - "cppDependencies": [] -} \ No newline at end of file diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json deleted file mode 100644 index 467a45b..0000000 --- a/vendordeps/maple-sim.json +++ /dev/null @@ -1,26 +0,0 @@ -{ - "fileName": "maple-sim.json", - "name": "maplesim", - "version": "0.3.5", - "frcYear": "2025", - "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", - "mavenUrls": [ - "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/repos/releases", - "https://repo1.maven.org/maven2" - ], - "jsonUrl": "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/maple-sim.json", - "javaDependencies": [ - { - "groupId": "org.ironmaple", - "artifactId": "maplesim-java", - "version": "0.3.5" - }, - { - "groupId": "org.dyn4j", - "artifactId": "dyn4j", - "version": "5.0.2" - } - ], - "jniDependencies": [], - "cppDependencies": [] -} \ No newline at end of file diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json deleted file mode 100644 index 6af3d3e..0000000 --- a/vendordeps/photonlib.json +++ /dev/null @@ -1,71 +0,0 @@ -{ - "fileName": "photonlib.json", - "name": "photonlib", - "version": "v2025.1.1", - "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", - "frcYear": "2025", - "mavenUrls": [ - "https://maven.photonvision.org/repository/internal", - "https://maven.photonvision.org/repository/snapshots" - ], - "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", - "jniDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2025.1.1", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "cppDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-cpp", - "version": "v2025.1.1", - "libName": "photonlib", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2025.1.1", - "libName": "photontargeting", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "javaDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-java", - "version": "v2025.1.1" - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-java", - "version": "v2025.1.1" - } - ] -} \ No newline at end of file diff --git a/vendordeps/yagsl-2025.3.0-beta2.json b/vendordeps/yagsl-2025.3.0-beta2.json deleted file mode 100644 index bfd01b7..0000000 --- a/vendordeps/yagsl-2025.3.0-beta2.json +++ /dev/null @@ -1,64 +0,0 @@ -{ - "fileName": "yagsl-2025.3.0-beta2.json", - "name": "YAGSL", - "version": "2025.3.0-beta2", - "frcYear": "2025", - "uuid": "1ccce5a4-acd2-4d18-bca3-4b8047188400", - "mavenUrls": [ - "https://broncbotz3481.github.io/YAGSL-Lib/yagsl/repos" - ], - "jsonUrl": "https://broncbotz3481.github.io/YAGSL-Lib/yagsl/yagsl.json", - "javaDependencies": [ - { - "groupId": "swervelib", - "artifactId": "YAGSL-java", - "version": "2025.3.0-beta2" - } - ], - "requires": [ - { - "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", - "errorMessage": "REVLib is required!", - "offlineFileName": "REVLib-2025.0.0.json", - "onlineUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json" - }, - { - "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", - "errorMessage": "ReduxLib is required!", - "offlineFileName": "ReduxLib-2025.0.0.json", - "onlineUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2025.json" - }, - { - "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", - "errorMessage": "Phoenix6 is required!", - "offlineFileName": "Phoenix6-25.1.0.json", - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json" - }, - { - "uuid": "ab676553-b602-441f-a38d-f1296eff6537", - "errorMessage": "Phoenix5 is required!", - "offlineFileName": "Phoenix5-5.35.0.json", - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2025-latest.json" - }, - { - "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", - "errorMessage": "Studica is required!", - "offlineFileName": "Studica-2025.0.0.json", - "onlineUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.0.json" - }, - { - "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0", - "errorMessage": "ThriftyLib is required!", - "offlineFileName": "ThriftyLib.json", - "onlineUrl": "https://docs.home.thethriftybot.com/ThriftyLib.json" - }, - { - "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", - "errorMessage": "maple-sim is required for simulation", - "offlineFileName": "maple-sim.json", - "onlineUrl": "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/maple-sim.json" - } - ], - "jniDependencies": [], - "cppDependencies": [] -} \ No newline at end of file