diff --git a/.vscode/settings.json b/.vscode/settings.json index 1745ba0..612cdd0 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -56,6 +56,5 @@ "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/build.gradle b/build.gradle index dcbfde6..0ab3b81 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.3.1" + id "edu.wpi.first.GradleRIO" version "2025.1.1" } java { @@ -33,7 +33,7 @@ deploy { frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { files = project.fileTree('src/main/deploy') directory = '/home/lvuser/deploy' - deleteOldFiles = true // Change to true to delete files on roboRIO that no + deleteOldFiles = false // Change to true to delete files on roboRIO that no // longer exist in deploy directory of this project } } @@ -47,7 +47,7 @@ def deployArtifact = deploy.targets.roborio.artifacts.frcJava wpi.java.debugJni = false // Set this to true to enable desktop support. -def includeDesktopSupport = true +def includeDesktopSupport = false // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 5. diff --git a/gradlew b/gradlew old mode 100755 new mode 100644 diff --git a/simgui-ds.json b/simgui-ds.json index 4a63cc1..73cc713 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,9 +1,4 @@ { - "System Joysticks": { - "window": { - "enabled": false - } - }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/src/main/deploy/pathplanner/autos/3Piece-A.auto b/src/main/deploy/pathplanner/autos/3Piece-A.auto new file mode 100644 index 0000000..cc12487 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3Piece-A.auto @@ -0,0 +1,73 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "A_R1" + } + }, + { + "type": "named", + "data": { + "name": "L1" + } + }, + { + "type": "path", + "data": { + "pathName": "R1_HP1" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + }, + { + "type": "path", + "data": { + "pathName": "HP1_R2" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + }, + { + "type": "path", + "data": { + "pathName": "R2_HP1" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + }, + { + "type": "path", + "data": { + "pathName": "HP1_R3" + } + }, + { + "type": "named", + "data": { + "name": "L1" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/A_R3.auto b/src/main/deploy/pathplanner/autos/A_R3.auto deleted file mode 100644 index a2d8abf..0000000 --- a/src/main/deploy/pathplanner/autos/A_R3.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "A_R2" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/HP2_R6.auto b/src/main/deploy/pathplanner/autos/HP2_R6.auto deleted file mode 100644 index c9b3428..0000000 --- a/src/main/deploy/pathplanner/autos/HP2_R6.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "HP2_R6" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Leave A.auto b/src/main/deploy/pathplanner/autos/Leave A.auto deleted file mode 100644 index d8b7a51..0000000 --- a/src/main/deploy/pathplanner/autos/Leave A.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "LeaveA" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TestAuto.auto b/src/main/deploy/pathplanner/autos/TestAuto.auto deleted file mode 100644 index ba05359..0000000 --- a/src/main/deploy/pathplanner/autos/TestAuto.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "TestPath" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A_R1.path b/src/main/deploy/pathplanner/paths/A_R1.path index b13a00e..c2b9654 100644 --- a/src/main/deploy/pathplanner/paths/A_R1.path +++ b/src/main/deploy/pathplanner/paths/A_R1.path @@ -54,7 +54,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/A_R2.path b/src/main/deploy/pathplanner/paths/A_R2.path index 138c08e..62b3951 100644 --- a/src/main/deploy/pathplanner/paths/A_R2.path +++ b/src/main/deploy/pathplanner/paths/A_R2.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/A_R3.path b/src/main/deploy/pathplanner/paths/A_R3.path index 3e052d8..93317dd 100644 --- a/src/main/deploy/pathplanner/paths/A_R3.path +++ b/src/main/deploy/pathplanner/paths/A_R3.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/A_R4.path b/src/main/deploy/pathplanner/paths/A_R4.path index 37e0d52..53feb30 100644 --- a/src/main/deploy/pathplanner/paths/A_R4.path +++ b/src/main/deploy/pathplanner/paths/A_R4.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/B_R2.path b/src/main/deploy/pathplanner/paths/B_R2.path index 1a9d6ff..a0ac52b 100644 --- a/src/main/deploy/pathplanner/paths/B_R2.path +++ b/src/main/deploy/pathplanner/paths/B_R2.path @@ -38,7 +38,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/B_R3.path b/src/main/deploy/pathplanner/paths/B_R3.path index 71b925e..2eb1129 100644 --- a/src/main/deploy/pathplanner/paths/B_R3.path +++ b/src/main/deploy/pathplanner/paths/B_R3.path @@ -38,7 +38,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/B_R4.path b/src/main/deploy/pathplanner/paths/B_R4.path index 4510ba8..b7abbf9 100644 --- a/src/main/deploy/pathplanner/paths/B_R4.path +++ b/src/main/deploy/pathplanner/paths/B_R4.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/B_R5.path b/src/main/deploy/pathplanner/paths/B_R5.path index 95defe6..dd7a13f 100644 --- a/src/main/deploy/pathplanner/paths/B_R5.path +++ b/src/main/deploy/pathplanner/paths/B_R5.path @@ -38,7 +38,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/C_R3.path b/src/main/deploy/pathplanner/paths/C_R3.path index 4e34e79..4786955 100644 --- a/src/main/deploy/pathplanner/paths/C_R3.path +++ b/src/main/deploy/pathplanner/paths/C_R3.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/C_R4.path b/src/main/deploy/pathplanner/paths/C_R4.path index 01c8c00..69bb49c 100644 --- a/src/main/deploy/pathplanner/paths/C_R4.path +++ b/src/main/deploy/pathplanner/paths/C_R4.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/C_R5.path b/src/main/deploy/pathplanner/paths/C_R5.path index 0f821b9..5a6d9cc 100644 --- a/src/main/deploy/pathplanner/paths/C_R5.path +++ b/src/main/deploy/pathplanner/paths/C_R5.path @@ -38,7 +38,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/C_R6.path b/src/main/deploy/pathplanner/paths/C_R6.path index 3152363..cbd1143 100644 --- a/src/main/deploy/pathplanner/paths/C_R6.path +++ b/src/main/deploy/pathplanner/paths/C_R6.path @@ -54,7 +54,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/HP1_R1.path b/src/main/deploy/pathplanner/paths/HP1_R1.path index d9f60e1..f362011 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R1.path +++ b/src/main/deploy/pathplanner/paths/HP1_R1.path @@ -54,7 +54,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/HP1_R2.path b/src/main/deploy/pathplanner/paths/HP1_R2.path index 813b803..147af5b 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R2.path +++ b/src/main/deploy/pathplanner/paths/HP1_R2.path @@ -49,7 +49,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/HP1_R3.path b/src/main/deploy/pathplanner/paths/HP1_R3.path index cf08bd7..a08014b 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R3.path +++ b/src/main/deploy/pathplanner/paths/HP1_R3.path @@ -54,7 +54,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/HP1_R4.path b/src/main/deploy/pathplanner/paths/HP1_R4.path index 2ecfc54..860a822 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R4.path +++ b/src/main/deploy/pathplanner/paths/HP1_R4.path @@ -54,7 +54,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/HP1_R5.path b/src/main/deploy/pathplanner/paths/HP1_R5.path index 8c0f456..97afd75 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R5.path +++ b/src/main/deploy/pathplanner/paths/HP1_R5.path @@ -70,7 +70,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/HP1_R6.path b/src/main/deploy/pathplanner/paths/HP1_R6.path index 6dc5c85..3522805 100644 --- a/src/main/deploy/pathplanner/paths/HP1_R6.path +++ b/src/main/deploy/pathplanner/paths/HP1_R6.path @@ -54,7 +54,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/HP2_R1.path b/src/main/deploy/pathplanner/paths/HP2_R1.path index b6c7542..14932d3 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R1.path +++ b/src/main/deploy/pathplanner/paths/HP2_R1.path @@ -38,7 +38,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/HP2_R2.path b/src/main/deploy/pathplanner/paths/HP2_R2.path index f06c78a..2c2baee 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R2.path +++ b/src/main/deploy/pathplanner/paths/HP2_R2.path @@ -54,7 +54,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/HP2_R3.path b/src/main/deploy/pathplanner/paths/HP2_R3.path index 228d14e..488b3a0 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R3.path +++ b/src/main/deploy/pathplanner/paths/HP2_R3.path @@ -70,7 +70,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/HP2_R4.path b/src/main/deploy/pathplanner/paths/HP2_R4.path index 5fa6fb1..23f58bd 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R4.path +++ b/src/main/deploy/pathplanner/paths/HP2_R4.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 3.2040620895951974, - "y": 0.2784113500195273 + "y": 0.27841135001952744 }, "isLocked": false, "linkedName": "HP2" @@ -54,7 +54,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/HP2_R5.path b/src/main/deploy/pathplanner/paths/HP2_R5.path index 09ca8d4..6305e1b 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R5.path +++ b/src/main/deploy/pathplanner/paths/HP2_R5.path @@ -38,7 +38,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/HP2_R6.path b/src/main/deploy/pathplanner/paths/HP2_R6.path index 9c5d000..c0ad666 100644 --- a/src/main/deploy/pathplanner/paths/HP2_R6.path +++ b/src/main/deploy/pathplanner/paths/HP2_R6.path @@ -38,7 +38,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/LeaveA.path b/src/main/deploy/pathplanner/paths/LeaveA.path index 99efd9f..635629b 100644 --- a/src/main/deploy/pathplanner/paths/LeaveA.path +++ b/src/main/deploy/pathplanner/paths/LeaveA.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/LeaveB.path b/src/main/deploy/pathplanner/paths/LeaveB.path index eed1340..e958cd2 100644 --- a/src/main/deploy/pathplanner/paths/LeaveB.path +++ b/src/main/deploy/pathplanner/paths/LeaveB.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/LeaveC.path b/src/main/deploy/pathplanner/paths/LeaveC.path index 17b95ca..13521e5 100644 --- a/src/main/deploy/pathplanner/paths/LeaveC.path +++ b/src/main/deploy/pathplanner/paths/LeaveC.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/R1_HP1.path b/src/main/deploy/pathplanner/paths/R1_HP1.path index aa38231..243f4d7 100644 --- a/src/main/deploy/pathplanner/paths/R1_HP1.path +++ b/src/main/deploy/pathplanner/paths/R1_HP1.path @@ -54,7 +54,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/R1_HP2.path b/src/main/deploy/pathplanner/paths/R1_HP2.path index 69151dd..56f97de 100644 --- a/src/main/deploy/pathplanner/paths/R1_HP2.path +++ b/src/main/deploy/pathplanner/paths/R1_HP2.path @@ -54,7 +54,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/R2_HP1.path b/src/main/deploy/pathplanner/paths/R2_HP1.path index 411551d..295b6b3 100644 --- a/src/main/deploy/pathplanner/paths/R2_HP1.path +++ b/src/main/deploy/pathplanner/paths/R2_HP1.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/R2_HP2.path b/src/main/deploy/pathplanner/paths/R2_HP2.path index 9226a7d..5906654 100644 --- a/src/main/deploy/pathplanner/paths/R2_HP2.path +++ b/src/main/deploy/pathplanner/paths/R2_HP2.path @@ -54,7 +54,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/R3_HP1.path b/src/main/deploy/pathplanner/paths/R3_HP1.path index 3e64fc9..53a657b 100644 --- a/src/main/deploy/pathplanner/paths/R3_HP1.path +++ b/src/main/deploy/pathplanner/paths/R3_HP1.path @@ -54,7 +54,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/R3_HP2.path b/src/main/deploy/pathplanner/paths/R3_HP2.path index 7c49a68..71ea67a 100644 --- a/src/main/deploy/pathplanner/paths/R3_HP2.path +++ b/src/main/deploy/pathplanner/paths/R3_HP2.path @@ -20,8 +20,8 @@ "y": 5.576493502516883 }, "prevControl": { - "x": 3.161967272343525, - "y": 5.822659919233287 + "x": 3.1619672723435244, + "y": 5.822659919233285 }, "nextControl": { "x": 2.589902912627433, @@ -70,7 +70,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/R4_HP1.path b/src/main/deploy/pathplanner/paths/R4_HP1.path index 29982f6..0add02c 100644 --- a/src/main/deploy/pathplanner/paths/R4_HP1.path +++ b/src/main/deploy/pathplanner/paths/R4_HP1.path @@ -70,7 +70,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/R4_HP2.path b/src/main/deploy/pathplanner/paths/R4_HP2.path index d6df8ec..a9ce710 100644 --- a/src/main/deploy/pathplanner/paths/R4_HP2.path +++ b/src/main/deploy/pathplanner/paths/R4_HP2.path @@ -54,7 +54,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/R5_HP1.path b/src/main/deploy/pathplanner/paths/R5_HP1.path index 8fd7314..ad4ec15 100644 --- a/src/main/deploy/pathplanner/paths/R5_HP1.path +++ b/src/main/deploy/pathplanner/paths/R5_HP1.path @@ -70,7 +70,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/R5_HP2.path b/src/main/deploy/pathplanner/paths/R5_HP2.path index c3d055a..58dcf8c 100644 --- a/src/main/deploy/pathplanner/paths/R5_HP2.path +++ b/src/main/deploy/pathplanner/paths/R5_HP2.path @@ -38,7 +38,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/R6_HP1.path b/src/main/deploy/pathplanner/paths/R6_HP1.path index ecb0299..338a258 100644 --- a/src/main/deploy/pathplanner/paths/R6_HP1.path +++ b/src/main/deploy/pathplanner/paths/R6_HP1.path @@ -54,7 +54,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/R6_HP2.path b/src/main/deploy/pathplanner/paths/R6_HP2.path index e1140cc..11ee9c4 100644 --- a/src/main/deploy/pathplanner/paths/R6_HP2.path +++ b/src/main/deploy/pathplanner/paths/R6_HP2.path @@ -54,7 +54,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/TestPath.path b/src/main/deploy/pathplanner/paths/TestPath.path deleted file mode 100644 index 42d6faf..0000000 --- a/src/main/deploy/pathplanner/paths/TestPath.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.0, - "y": 7.0 - }, - "prevControl": null, - "nextControl": { - "x": 3.0, - "y": 7.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.0, - "y": 6.0 - }, - "prevControl": { - "x": 3.0, - "y": 6.0 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "Test", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 566711c..46d6cb0 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -5,22 +5,21 @@ "pathFolders": [ "HP to Reef", "Leave Start", - "Test", "Reef to HP", "Start to Reef" ], "autoFolders": [], - "defaultMaxVel": 5.0, + "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 50.0, - "robotMOI": 5.1, + "robotMass": 74.088, + "robotMOI": 6.883, "robotTrackwidth": 0.546, - "driveWheelRadius": 0.049, - "driveGearing": 6.122, - "maxDriveSpeed": 5.0, + "driveWheelRadius": 0.048, + "driveGearing": 5.143, + "maxDriveSpeed": 5.45, "driveMotorType": "krakenX60", "driveCurrentLimit": 60.0, "wheelCOF": 1.2, diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java deleted file mode 100644 index 3c0df25..0000000 --- a/src/main/java/frc/robot/FieldConstants.java +++ /dev/null @@ -1,241 +0,0 @@ -// Copyright (c) 2025 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file at -// the root directory of this project. - -package frc.robot; - -import edu.wpi.first.math.geometry.*; -import edu.wpi.first.math.util.Units; -import java.util.ArrayList; -import java.util.HashMap; -import java.util.List; -import java.util.Map; - -/** - * Contains various field dimensions and useful reference points. All units are in meters and poses - * have a blue alliance origin. - */ -public class FieldConstants { - public static final double fieldLength = Units.inchesToMeters(690.876); - public static final double fieldWidth = Units.inchesToMeters(317); - public static final Translation2d fieldCenter = - new Translation2d(fieldLength / 2, fieldWidth / 2); - public static final double startingLineX = - Units.inchesToMeters(299.438); // Measured from the inside of starting - // line - - public static class Processor { - public static final Pose2d centerFace = - new Pose2d(Units.inchesToMeters(235.726), 0, Rotation2d.fromDegrees(90)); - } - - public static class Barge { - public static final Translation2d farCage = - new Translation2d(Units.inchesToMeters(345.428), Units.inchesToMeters(286.779)); - public static final Translation2d middleCage = - new Translation2d(Units.inchesToMeters(345.428), Units.inchesToMeters(242.855)); - public static final Translation2d closeCage = - new Translation2d(Units.inchesToMeters(345.428), Units.inchesToMeters(199.947)); - - // Measured from floor to bottom of cage - public static final double deepHeight = Units.inchesToMeters(3.125); - public static final double shallowHeight = Units.inchesToMeters(30.125); - } - - public static class CoralStation { - public static final Pose2d leftCenterFace = - new Pose2d( - Units.inchesToMeters(33.526), - Units.inchesToMeters(291.176), - Rotation2d.fromDegrees(90 - 144.011)); - public static final Pose2d rightCenterFace = - new Pose2d( - Units.inchesToMeters(33.526), - Units.inchesToMeters(25.824), - Rotation2d.fromDegrees(144.011 - 90)); - } - - public static class Reef { - public static final Translation2d centerOfReef = - new Translation2d(Units.inchesToMeters(176.746), Units.inchesToMeters(158.501)); - public static final double faceToZoneLine = - Units.inchesToMeters(12); // Side of the reef to the inside of the - // reef zone line - - public static final Pose2d[] centerFaces = - new Pose2d[12]; // Starting facing the driver station in clockwise - // order - public static final List> branchPositions = - new ArrayList<>(); // Starting at the right - // branch facing the - // driver station in - // clockwise - - static { - // Initialize faces - centerFaces[0] = - new Pose2d( - Units.inchesToMeters(144.003), - Units.inchesToMeters(158.500), - Rotation2d.fromDegrees(180)); - centerFaces[1] = - new Pose2d( - Units.inchesToMeters(160.373), - Units.inchesToMeters(186.857), - Rotation2d.fromDegrees(120)); - centerFaces[2] = - new Pose2d( - Units.inchesToMeters(193.116), - Units.inchesToMeters(186.858), - Rotation2d.fromDegrees(60)); - centerFaces[3] = - new Pose2d( - Units.inchesToMeters(209.489), - Units.inchesToMeters(158.502), - Rotation2d.fromDegrees(0)); - centerFaces[4] = - new Pose2d( - Units.inchesToMeters(193.118), - Units.inchesToMeters(130.145), - Rotation2d.fromDegrees(-60)); - centerFaces[5] = - new Pose2d( - Units.inchesToMeters(160.375), - Units.inchesToMeters(130.144), - Rotation2d.fromDegrees(-120)); - - centerFaces[6] = centerFaces[0].rotateAround(fieldCenter, Rotation2d.k180deg); - centerFaces[7] = centerFaces[1].rotateAround(fieldCenter, Rotation2d.k180deg); - centerFaces[8] = centerFaces[2].rotateAround(fieldCenter, Rotation2d.k180deg); - centerFaces[9] = centerFaces[3].rotateAround(fieldCenter, Rotation2d.k180deg); - centerFaces[10] = centerFaces[4].rotateAround(fieldCenter, Rotation2d.k180deg); - centerFaces[11] = centerFaces[5].rotateAround(fieldCenter, Rotation2d.k180deg); - - // Initialize branch positions - for (int face = 0; face < centerFaces.length; face++) { - Map fillRight = new HashMap<>(); - Map fillLeft = new HashMap<>(); - for (var level : ReefHeight.values()) { - Pose2d poseDirection = new Pose2d(); - if (face < 6) { - poseDirection = - new Pose2d(centerOfReef, centerFaces[face].getRotation()); - } else { - poseDirection = - new Pose2d(centerOfReef.rotateAround(fieldCenter, Rotation2d.k180deg), - centerFaces[face].getRotation()); - } - - double adjustX = Units.inchesToMeters(30.738); // Depth of branch from reef face - double adjustY = Units.inchesToMeters(6.469); // Offset from reef face - // centerline to branch - - fillRight.put( - level, - new Pose3d( - new Translation3d( - poseDirection - .transformBy( - new Transform2d(adjustX, adjustY, new Rotation2d())) - .getX(), - poseDirection - .transformBy( - new Transform2d(adjustX, adjustY, new Rotation2d())) - .getY(), - level.height), - new Rotation3d( - 0, - Units.degreesToRadians(level.pitch), - poseDirection.getRotation().getRadians()))); - fillLeft.put( - level, - new Pose3d( - new Translation3d( - poseDirection - .transformBy( - new Transform2d(adjustX, -adjustY, new Rotation2d())) - .getX(), - poseDirection - .transformBy( - new Transform2d(adjustX, -adjustY, new Rotation2d())) - .getY(), - level.height), - new Rotation3d( - 0, - Units.degreesToRadians(level.pitch), - poseDirection.getRotation().getRadians()))); - } - branchPositions.add(fillRight); - branchPositions.add(fillLeft); - } - - - } - } - - public static class StagingPositions { - // Measured from the center of the ice cream - public static final Pose2d leftIceCream = - new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(230.5), new Rotation2d()); - public static final Pose2d middleIceCream = - new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(158.5), new Rotation2d()); - public static final Pose2d rightIceCream = - new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(86.5), new Rotation2d()); - } - - public enum ReefHeight { - L4(Units.inchesToMeters(72), -90), - L3(Units.inchesToMeters(47.625), -35), - L2(Units.inchesToMeters(31.875), -35), - L1(Units.inchesToMeters(18), 0); - - ReefHeight(double height, double pitch) - { - this.height = height; - this.pitch = pitch; // in degrees - } - - public final double height; - public final double pitch; - } - - public static Pose2d getNearestReefFace(Pose2d currentPose) - { - return currentPose.nearest(List.of(FieldConstants.Reef.centerFaces)); - } - - public enum ReefSide { - LEFT, - RIGHT - } - - public static Pose2d getNearestReefBranch(Pose2d currentPose, ReefSide side) - { - return FieldConstants.Reef.branchPositions - .get(List.of(FieldConstants.Reef.centerFaces).indexOf(getNearestReefFace(currentPose)) - * 2 + (side == ReefSide.LEFT ? 1 : 0)) - .get(FieldConstants.ReefHeight.L1).toPose2d(); - } - - public static Pose2d getNearestCoralStation(Pose2d currentPose) - { - if (currentPose.getTranslation().getX() > FieldConstants.fieldLength / 2) { - if (currentPose.getTranslation().getY() > FieldConstants.fieldWidth / 2) { - return FieldConstants.CoralStation.rightCenterFace - .rotateAround(FieldConstants.fieldCenter, Rotation2d.k180deg); - } else { - return FieldConstants.CoralStation.leftCenterFace - .rotateAround(FieldConstants.fieldCenter, Rotation2d.k180deg); - } - } else { - if (currentPose.getTranslation().getY() > FieldConstants.fieldWidth / 2) { - return FieldConstants.CoralStation.leftCenterFace; - } else { - return FieldConstants.CoralStation.rightCenterFace; - } - } - } -} diff --git a/src/main/java/frc/robot/HardwareConstants.java b/src/main/java/frc/robot/HardwareConstants.java index 8a37a03..73ac298 100644 --- a/src/main/java/frc/robot/HardwareConstants.java +++ b/src/main/java/frc/robot/HardwareConstants.java @@ -24,18 +24,18 @@ public class CAN { public static final int ARM_MTR_ID = 14; public static final int INTAKE_MTR_ID = 15; + public static final int CLIMBER_MTR_ID = 1; public static final int PRIMARY_ELEVATOR_ID = 16; public static final int SECONDARY_ELEVATOR_ID = 17; public static final int CANDLE_ID = 50; + + public static final int FEEDER_MTR_ID = 20; } public class DIO { // Add digitial I/O ports used here public static final int LIGHT_SENSOR_CHANNEL = 0; } - - // Use LoggedTunableNumbers - public static final boolean tuningMode = true; } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 44c6726..d02faca 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,10 +4,6 @@ package frc.robot; -import java.io.IOException; -import java.util.ArrayList; -import java.util.List; - import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; @@ -15,129 +11,49 @@ import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.commands.PathPlannerAuto; -import com.pathplanner.lib.path.PathPlannerPath; - -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.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.HardwareConstants.CAN; -import java.io.IOException; -import org.json.simple.parser.ParseException; - public class Robot extends LoggedRobot { private Command m_autonomousCommand; - private Command m_lastAutonomousCommand; - private List m_pathsToShow = new ArrayList(); - private Field2d m_autoTraj = new Field2d(); - public static final double fieldLength = Units.inchesToMeters(690.876); - public static final double fieldWidth = Units.inchesToMeters(317); - public static final Translation2d fieldCenter = - new Translation2d(fieldLength / 2, fieldWidth / 2); private final RobotContainer m_robotContainer; public Robot() { + Logger.recordMetadata("ProjectName", "ReefCode"); // Set a metadata value - switch (HardwareConstants.currentMode) { - case REAL: - Logger.addDataReceiver(new WPILOGWriter()); - Logger.addDataReceiver(new NT4Publisher()); - new PowerDistribution(CAN.PDH_ID, ModuleType.kRev); - break; - - case SIM: - System.out.println("SIM!!!"); - Logger.addDataReceiver(new NT4Publisher()); - break; - - case REPLAY: - setUseTiming(false); // Run as fast as possible - String logPath = LogFileUtil.findReplayLog(); - Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log - Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new log - break; + if (isReal()) { + Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs") + Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables + new PowerDistribution(CAN.PDH_ID, ModuleType.kRev); // Enables power distribution logging + } else { + setUseTiming(false); // Run as fast as possible + String logPath = LogFileUtil.findReplayLog(); // Pull the replay log from AdvantageScope (or prompt the user) + Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log + Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new log } - Logger.recordMetadata("ProjectName", "ReefCode"); // Set a metadata value - Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may // be added. - + m_robotContainer = new RobotContainer(); } @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); - } @Override public void disabledInit() { + m_robotContainer.startIdleAnimations(); } @Override public void disabledPeriodic() { - { - var m_alliance = DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red; - // Get currently selected command - - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - // Check if is the same as the last one - if (m_autonomousCommand != m_lastAutonomousCommand && m_autonomousCommand != null) { - // Check if its contained in the list of our autos - if (AutoBuilder.getAllAutoNames().contains(m_autonomousCommand.getName())) { - // Clear the current path - m_pathsToShow.clear(); - // Grabs all paths from the auto - try { - for (PathPlannerPath path : PathPlannerAuto - .getPathGroupFromAutoFile(m_autonomousCommand.getName())) { - // Adds all poses to master list - m_pathsToShow.addAll(path.getPathPoses()); - } - } catch (IOException | ParseException e) { - // TODO Auto-generated catch block - e.printStackTrace(); - } - // Check to see which alliance we are on Red Alliance - if (m_alliance) { - for (int i = 0; i < m_pathsToShow.size(); i++) { - m_pathsToShow.set(i, - m_pathsToShow.get(i).rotateAround(fieldCenter, Rotation2d.k180deg)); - } - } - // Displays all poses on Field2d widget - m_autoTraj.getObject("traj").setPoses(m_pathsToShow); - } - } - m_lastAutonomousCommand = m_autonomousCommand; - - if (!m_pathsToShow.isEmpty()) { - var firstPose = m_pathsToShow.get(0); - Logger.recordOutput("Alignment/StartPose", firstPose); - SmartDashboard.putBoolean("Alignment/Translation", - firstPose.getTranslation().getDistance( - m_robotContainer.getDrive().getPose().getTranslation()) <= Units - .inchesToMeters(1.5)); - SmartDashboard.putBoolean("Alignment/Rotation", - firstPose.getRotation().minus(m_robotContainer.getDrive().getPose().getRotation()) - .getDegrees() < 1); - } - } } @Override @@ -147,18 +63,18 @@ public void disabledExit() { @Override public void autonomousInit() { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - System.out.println(m_autonomousCommand); - - System.out.println("!!!!!!!!!!Autonomous init!!!!!!!!!!!"); if (m_autonomousCommand != null) { - System.out.println("FOUND A COMMAND"); m_autonomousCommand.schedule(); } + + m_robotContainer.calibrateAndStartPIDs(); + m_robotContainer.startEnabledLEDs(); } @Override - public void autonomousPeriodic() {} + public void autonomousPeriodic() { + } @Override public void autonomousExit() { @@ -169,6 +85,8 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } + m_robotContainer.calibrateAndStartPIDs(); + m_robotContainer.startEnabledLEDs(); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6a470a5..c6cf064 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,22 +6,12 @@ import static edu.wpi.first.units.Units.*; -import java.lang.invoke.VarHandle.AccessMode; -import java.util.function.Supplier; - -import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; - -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.ctre.phoenix6.swerve.SwerveRequest; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -29,11 +19,15 @@ import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.arm.ArmCommands; import frc.robot.subsystems.arm.Arm.ArmPosition; -import frc.robot.subsystems.arm.io.RealArmIO; +import frc.robot.subsystems.climber.Climber; +import frc.robot.subsystems.climber.ClimberCommands; +import frc.robot.subsystems.climber.RealClimberIO; +import frc.robot.subsystems.arm.RealArmIO; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveCommands; import frc.robot.subsystems.drive.Telemetry; import frc.robot.subsystems.drive.TunerConstants; +import frc.robot.subsystems.elevator.CmdElevatorCalibrate; import frc.robot.subsystems.drive.io.GyroIO; import frc.robot.subsystems.drive.io.GyroIOPigeon2; import frc.robot.subsystems.drive.io.ModuleIO; @@ -41,28 +35,25 @@ import frc.robot.subsystems.drive.io.ModuleIOTalonFX; import frc.robot.subsystems.elevator.Elevator; import frc.robot.subsystems.elevator.ElevatorCommands; +import frc.robot.subsystems.elevator.RealElevatorIO; +import frc.robot.subsystems.leds.LEDs; +import frc.robot.subsystems.leds.LEDsCommands; import frc.robot.subsystems.elevator.Elevator.ElevatorPosition; -import frc.robot.subsystems.elevator.io.RealElevatorIO; import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands; import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands.OverallPosition; import frc.robot.subsystems.presets.Preset; -import frc.robot.subsystems.vision.Vision; -import frc.robot.subsystems.vision.VisionIO; -import frc.robot.subsystems.vision.VisionIOPhotonVision; -import frc.robot.subsystems.vision.VisionIOPhotonVisionSim; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.FieldConstants.ReefSide; - -import static frc.robot.subsystems.vision.VisionConstants.*; public class RobotContainer { private final Drive drive; private final Elevator elevatorSubsystem = new Elevator(new RealElevatorIO()); private final Arm armSubsystem = new Arm(new RealArmIO()); + private final LEDs LEDSubsystem = new LEDs(); private final ElevatorCommands elevatorCommands = new ElevatorCommands(elevatorSubsystem); private final ArmCommands armCommands = new ArmCommands(armSubsystem); - private final Vision vision; - private final CommandXboxController joystick = new CommandXboxController(0); + private final LEDsCommands LEDCommands = new LEDsCommands(LEDSubsystem); + + private final Climber climber = new Climber(new RealClimberIO()); + private final ClimberCommands ClimberCommands = new ClimberCommands(climber); private final MultiSubsystemCommands multiSubsystemCommands = new MultiSubsystemCommands(elevatorSubsystem, armSubsystem, elevatorCommands, armCommands); @@ -75,7 +66,7 @@ public class RobotContainer { private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max // angular velocity - + /* Setting up bindings for necessary control of the swerve drive platform */ // private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() // .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband @@ -83,6 +74,9 @@ public class RobotContainer { private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); + private final CommandXboxController joystick = new CommandXboxController(0); + private final CommandXboxController climberstick = new CommandXboxController(1); + private final GenericHID buttonbox1 = new GenericHID(1); private final JoystickButton L1Button = new JoystickButton(buttonbox1, 1); private final JoystickButton L2Button = new JoystickButton(buttonbox1, 2); @@ -90,17 +84,16 @@ public class RobotContainer { private final JoystickButton L3Button = new JoystickButton(buttonbox1, 4); private final JoystickButton L4Button = new JoystickButton(buttonbox1, 5); private final JoystickButton LoadingButton = new JoystickButton(buttonbox1, 6); - private final JoystickButton button7 = new JoystickButton(buttonbox1, 7); + private final JoystickButton intakeButton = new JoystickButton(buttonbox1, 7); private final JoystickButton L4_scoreButton = new JoystickButton(buttonbox1, 8); - private final JoystickButton button9 = new JoystickButton(buttonbox1, 9); + private final JoystickButton spitButton = new JoystickButton(buttonbox1, 9); private final GenericHID buttonbox2 = new GenericHID(2); private final JoystickButton presetButton = new JoystickButton(buttonbox2, 2); - - private final LoggedDashboardChooser autoChooser; + private final JoystickButton incrementButton = new JoystickButton(buttonbox2, 4); + private final JoystickButton decrementButton = new JoystickButton(buttonbox2, 7); // public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); - private double speedMultiplier = 0.9; private final Telemetry logger = new Telemetry(MaxSpeed); @@ -117,14 +110,12 @@ public RobotContainer() { new ModuleIOTalonFX(TunerConstants.BackLeft), new ModuleIOTalonFX(TunerConstants.BackRight)); - vision = - new Vision( - drive::addVisionMeasurement, - new VisionIOPhotonVision(camera5Name, robotToCamera5), - new VisionIOPhotonVision(camera6Name, robotToCamera6), - new VisionIOPhotonVision(camera7Name, robotToCamera7), - new VisionIOPhotonVision(camera8Name, robotToCamera8) - ); + // vision = + // new Vision( + // drive::addVisionMeasurement, + // new VisionIOPhotonVision(camera0Name, robotToCamera0), + // new VisionIOPhotonVision(camera1Name, robotToCamera1)); + break; case SIM: @@ -137,22 +128,11 @@ public RobotContainer() { new ModuleIOSim(TunerConstants.BackLeft), new ModuleIOSim(TunerConstants.BackRight)); - /* We should be using this VisionIOPhotonVisionSim here but it's running too slow and - causing the a loop overrun. - vision = - new Vision( - drive::addVisionMeasurement, - new VisionIOPhotonVisionSim(camera7Name, robotToCamera7, drive::getPose), - new VisionIOPhotonVisionSim(camera8Name, robotToCamera8, drive::getPose) - ); */ - vision = - new Vision( - drive::addVisionMeasurement, - new VisionIO() {}, - new VisionIO() {}, - new VisionIO() {}, - new VisionIO() {}); - + // vision = + // new Vision( + // drive::addVisionMeasurement, + // new VisionIOPhotonVisionSim(camera0Name, robotToCamera0, drive::getPose), + // new VisionIOPhotonVisionSim(camera1Name, robotToCamera1, drive::getPose)); break; default: @@ -165,15 +145,10 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}); - vision = - new Vision( - drive::addVisionMeasurement, - new VisionIO() {}, - new VisionIO() {}, - new VisionIO() {}, - new VisionIO() {}); + // vision = new Vision(drive::addVisionMeasurement, new VisionIO() {}, new VisionIO() {}); break; } + // All AutoAligns for reef will align to Left position //TODO: Add AutoAligns to all the commands. @@ -196,36 +171,34 @@ public RobotContainer() { // AutoAlign to Intake + Intake NamedCommands.registerCommand("Intake", - multiSubsystemCommands.intake()); - + multiSubsystemCommands.loadGamepiece()); - autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); - - // hide the joystick missing warnings - DriverStation.silenceJoystickConnectionWarning(true); configureBindings(); } - private Command joystickApproach(Supplier approachPose) - { - return DriveCommands.joystickApproach( - drive, - () -> -joystick.getLeftY() * speedMultiplier, - approachPose); - } - private void configureBindings() { // Note that X is defined as forward according to WPILib convention, // and Y is defined as to the left according to WPILib convention. - drive.setDefaultCommand( DriveCommands.joystickDrive( drive, () -> -joystick.getLeftY(), () -> -joystick.getLeftX(), - () -> -joystick.getRightX())); + () -> -joystick.getRightX())); + + climber.setDefaultCommand( + ClimberCommands.runClimber( + () -> -climberstick.getLeftY())); + + // drivetrain.setDefaultCommand( + // // Drivetrain will execute this command periodically + // drivetrain.applyRequest(() -> drive.withVelocityX(-joystick.getLeftY() * MaxSpeed) // Drive forward with + // // negative Y (forward) + // .withVelocityY(-joystick.getLeftX() * MaxSpeed) // Drive left with negative X (left) + // .withRotationalRate(-joystick.getRightX() * MaxAngularRate) // Drive counterclockwise with negative X (left) + // )); // joystick.a().whileTrue(drivetrain.applyRequest(() -> brake)); // joystick.b().whileTrue(drivetrain @@ -238,46 +211,15 @@ private void configureBindings() { joystick.start().and(joystick.y()).whileTrue(drive.sysIdQuasistatic(Direction.kForward)); joystick.start().and(joystick.x()).whileTrue(drive.sysIdQuasistatic(Direction.kReverse)); - // Driver Right Bumper: Approach Nearest Right-Side Reef Branch - joystick.rightBumper() - .whileTrue( - joystickApproach( - () -> FieldConstants.getNearestReefBranch(drive.getPose(), ReefSide.RIGHT))); + //climberstick.start().and(climberstick.y()).onTrue(getAutonomousCommand()) - // Driver Left Bumper: Approach Nearest Left-Side Reef Branch - joystick.leftBumper() - .whileTrue( - joystickApproach( - () -> FieldConstants.getNearestReefBranch(drive.getPose(), ReefSide.LEFT))); - - - - /* - // Driver Left Bumper and Algae mode: Approach Nearest Reef Face - joystick.rightBumper() - .whileTrue( - joystickApproach(() -> FieldConstants.getNearestReefFace(drive.getPose()))); - - */ - - // Reset gyro to 0° when Y button is pressed - joystick.y() - .onTrue( - Commands.runOnce( - () -> - drive.setPose( - new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), - drive) - .ignoringDisable(true)); - - // // reset the field-centric heading on left bumper press + // reset the field-centric heading on left bumper press // joystick.leftBumper().onTrue(drive.runOnce(() -> drive.seedFieldCentric())); // drive.registerTelemetry(logger::telemeterize); - // Elevator sys id routines - button7.whileTrue(elevatorSubsystem.sysIdDynamic(Direction.kForward)); - button9.whileTrue(elevatorSubsystem.sysIdDynamic(Direction.kReverse)); + intakeButton.onTrue(multiSubsystemCommands.loadGamepiece().raceWith(LEDCommands.intaking()).andThen(LEDCommands.hasPiece()).andThen(LEDCommands.elevatorOrArmIsMoving())); + spitButton.onTrue(armCommands.spit()); L1Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L1)); L2Button.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L2)); @@ -287,30 +229,62 @@ private void configureBindings() { LoadingButton.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.Loading)); L4_scoreButton.onTrue(multiSubsystemCommands.setOverallSetpoint(OverallPosition.L4_Score)); - // Runs the preset to score unless the preset is invalid. - /* joystick.rightBumper().onTrue( - multiSubsystemCommands.scoreGamepieceAtPosition(preset.getLevel()).unless(() -> !preset.isPresetValid())); - */ + multiSubsystemCommands.scoreGamepieceAtPosition(() -> preset.getLevel()).unless(() + -> !preset.isPresetValid())); // Resets the preset when we don't have a piece. - armSubsystem._hasPiece.onFalse(preset.resetPreset()); + armSubsystem._hasPiece.onFalse(preset.resetPreset().andThen(LEDCommands.pickingUpCoral())); // Sets the level preset presetButton.and(L1Button).onTrue(preset.setPresetLevelCommand(OverallPosition.L1)); presetButton.and(L2Button).onTrue(preset.setPresetLevelCommand(OverallPosition.L2)); presetButton.and(L3Button).onTrue(preset.setPresetLevelCommand(OverallPosition.L3)); presetButton.and(L4Button).onTrue(preset.setPresetLevelCommand(OverallPosition.L4)); - } + incrementButton.onTrue(elevatorCommands.incrementElevatorPosition()); + decrementButton.onTrue(elevatorCommands.decrementElevatorPosition()); + } public Command getAutonomousCommand() { - return autoChooser.get(); + return Commands.print("No autonomous command configured"); + } + + public void calibrateAndStartPIDs() { + // PID commands: we only want one of them so start/stop works correctly + Command elevatorPIDCommand = elevatorCommands.runElevatorPID(); + Command armPIDCommand = armCommands.runArmPID(); + // Start elevator pid + if (elevatorSubsystem.isCalibrated()) { + elevatorCommands.runElevatorPID(); + if (!CommandScheduler.getInstance().isScheduled(elevatorPIDCommand)) { + CommandScheduler.getInstance().schedule(elevatorPIDCommand); + } + } else { + Command calibCommand = new CmdElevatorCalibrate(elevatorSubsystem).andThen(elevatorPIDCommand); + CommandScheduler.getInstance().schedule(calibCommand); + } + + // Start arm pid + if (!CommandScheduler.getInstance().isScheduled(armPIDCommand)) { + CommandScheduler.getInstance().schedule(armPIDCommand); + } + + // Set initial positions + CommandScheduler.getInstance().schedule(elevatorCommands.setElevatorSetpoint(ElevatorPosition.Stow)); + CommandScheduler.getInstance().schedule(armCommands.setArmPosition(ArmPosition.Stow)); + } + + public void startIdleAnimations() { + Command disabled1 = LEDCommands.disabledAnimation1(); + if (!CommandScheduler.getInstance().isScheduled(disabled1)) + CommandScheduler.getInstance().schedule(disabled1); } - public Drive getDrive() { - return drive; + public void startEnabledLEDs() { + Command initialLEDs = LEDCommands.pickingUpCoral(); + if (!CommandScheduler.getInstance().isScheduled(initialLEDs)) + CommandScheduler.getInstance().schedule(initialLEDs); } - } diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java index 869cb97..8f59c7e 100644 --- a/src/main/java/frc/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/robot/subsystems/arm/Arm.java @@ -3,32 +3,32 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.RPM; import static edu.wpi.first.units.Units.Volt; -import java.util.function.BooleanSupplier; +import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.subsystems.arm.io.ArmIO; -import frc.robot.subsystems.arm.io.ArmIO.ArmIOInputs; +import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands; +import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands.GamepieceMode; public class Arm extends SubsystemBase { public enum ArmPosition { - Stow(40), - Loading(30), - L4_Score(45); + Stow(80), + Loading_Coral(120), + Loading_Algae(50), + Loading(120), + L4_Score(45), + Algae_Score(60); double angle; @@ -40,36 +40,69 @@ public enum ArmPosition { public final Trigger _hasPiece = new Trigger(() -> hasPiece()); private ArmIO _io; - private ArmIOInputs _inputs; + private ArmIOInputsAutoLogged _inputs; private boolean _prevLightSensorVal; + private boolean _hasGamepiece; + private int _intakeSpikeCounter; + private ArmPosition _currentPos; + private ArmPosition _desiredPos; + private MultiSubsystemCommands.GamepieceMode _currentMode; private ProfiledPIDController _armPidController; - private static final double KP = 0; - private static final double KI = 0; + private static final double KP = 0.09; + private static final double KI = 0.01; private static final double KD = 0; - private static final double PROFILE_VEL = 0; - private static final double PROFILE_ACC = 0; + private static final double PROFILE_VEL = 160; + private static final double PROFILE_ACC = 145; + + private static final double HAS_ALGAE_CURRENT = 2; + + private static final double ARM_WEIGHT_N = 3.5 * 9.81; + private static final double ARM_STALL_TORQUE_NM = 3.6; + private static final double ARM_STALL_CURRENT = 211; + private static final double ARM_KT = ARM_STALL_TORQUE_NM / ARM_STALL_CURRENT; + private static final double ARM_RESISTANCE = 0.057; + private static final double ARM_MOMENT_METERS = 0.3928; + private static final double ARM_GEARING = 17; + private static final double ARM_FEEDFORWARD_COEFF = 0.53; SysIdRoutine routine = new SysIdRoutine(new Config(), new SysIdRoutine.Mechanism(this::setArmVoltage, this::populateLog, this)); public Arm(ArmIO io) { _io = io; - _inputs = new ArmIOInputs(); + _inputs = new ArmIOInputsAutoLogged(); _armPidController = new ProfiledPIDController(KP, KI, KD, new Constraints(PROFILE_VEL, PROFILE_ACC)); - + _armPidController.setTolerance(7); } public void setArmSetpoint(ArmPosition setpoint) { + if (setpoint == ArmPosition.Loading) + setpoint = (_currentMode == GamepieceMode.ALGAE) ? ArmPosition.Loading_Algae : ArmPosition.Loading_Coral; + + _armPidController.reset(_inputs._armEncoderPositionDegrees); _armPidController.setGoal(setpoint.angle); + _desiredPos = setpoint; } public void setIntakeSpeed(double speed) { _io.setIntakeMotorSpeed(speed); } + public void setFeederSpeed(double speed) { + _io.setFeederMotorSpeed(speed); + } + + public void spit() { + setIntakeSpeed(0.5); + } + + public void clearHasGamepiece() { + _hasGamepiece = false; + } + public void setArmVoltage(Voltage voltage) { _io.setArmMotorVoltage(voltage); } @@ -79,22 +112,56 @@ public void resetIntakeEncoders() { } public boolean intakeAtDesiredRotations() { - return _inputs._intakeMotorPositionRotations >= 2; + return _inputs._intakeMotorPositionRotations <= -2; } public boolean hasPiece() { - boolean current = _inputs._lightSensorState; - boolean hasPiece = _prevLightSensorVal && !current; - _prevLightSensorVal = current; + boolean hasPiece; + if (_currentMode == GamepieceMode.CORAL) { + boolean currentState = _inputs._lightSensorState; + hasPiece = _prevLightSensorVal && !currentState; + _prevLightSensorVal = currentState; + } else { + if (_inputs._intakeMotorCurrent >= HAS_ALGAE_CURRENT) { + _intakeSpikeCounter++; + } + hasPiece = _intakeSpikeCounter >= 5; + } + return hasPiece; } + public void resetIntakeSpikeCounter() { + _intakeSpikeCounter = 0; + } + + public boolean lightSensorSeesGamepiece() { + return _inputs._lightSensorState; + } + public boolean armAtSetpoint() { - return _armPidController.atGoal(); + boolean atSetpoint = _armPidController.atGoal(); + if (atSetpoint) + _currentPos = _desiredPos; + return atSetpoint; + } + + public ArmPosition getCurrentPos() { + return _currentPos; } public void runArmPID() { - _io.setArmMotorSpeed(_armPidController.calculate(_inputs._armEncoderPositionDegrees)); + double out = (_armPidController.calculate(_inputs._armEncoderPositionDegrees) + + ARM_FEEDFORWARD_COEFF * Math.cos(Units.degreesToRadians(_inputs._armEncoderPositionDegrees))); + _io.setArmMotorVoltage(Voltage.ofBaseUnits(out, Volt)); + } + + public GamepieceMode getCurrentMode() { + return _currentMode; + } + + public void setCurrentMode(GamepieceMode mode) { + _currentMode = mode; } public void populateLog(SysIdRoutineLog log) { @@ -116,5 +183,12 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { @Override public void periodic() { _io.updateInputs(_inputs); + Logger.processInputs("Arm", _inputs); + + Logger.recordOutput("Arm/desiredPos", _armPidController.getSetpoint().position); + Logger.recordOutput("Arm/hasPiece", hasPiece()); + Logger.recordOutput("Arm/atSetpoint", armAtSetpoint()); + Logger.recordOutput("Arm/currentPosEnum", _currentPos); + Logger.recordOutput("Arm/desiredPosEnum", _desiredPos); } } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java index a8242b1..9dd4363 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmCommands.java @@ -17,43 +17,58 @@ public ArmCommands(Arm arm) { public Command spit() { return new StartEndCommand( () -> { - _arm.setIntakeSpeed(0.5); + if (_arm.getCurrentPos() == ArmPosition.L4_Score) + _arm.setArmSetpoint(ArmPosition.Stow); + _arm.spit(); }, () -> { _arm.setIntakeSpeed(0); + _arm.clearHasGamepiece(); }, - _arm).withTimeout(1); + _arm).withTimeout(0.5); + } public Command setArmPosition(ArmPosition setpoint) { - - return new InstantCommand(); - /* if (setpoint == ArmPosition.L4_Score) { return new InstantCommand( () -> { _arm.setArmSetpoint(setpoint); }, - _arm).alongWith(intakeForNumberOfRotations()); + _arm).andThen(intakeForNumberOfRotations()); } return new InstantCommand( () -> { _arm.setArmSetpoint(setpoint); }, - _arm); */ + _arm); } public Command intake() { return new StartEndCommand( () -> { - _arm.setIntakeSpeed(0.5); + _arm.resetIntakeSpikeCounter(); + _arm.setIntakeSpeed(0.45); + _arm.setFeederSpeed(0.45); }, () -> { _arm.setIntakeSpeed(0); + _arm.setFeederSpeed(0); }, _arm).until(() -> _arm.hasPiece()); } + public Command moveGamepieceToLightSensor() { + return new StartEndCommand( + () -> { + _arm.setIntakeSpeed(-0.3); + }, + () -> { + _arm.setIntakeSpeed(0); + }, + _arm).until(() -> _arm.lightSensorSeesGamepiece()); + } + public Command runArmPID() { return Commands.run(() -> { _arm.runArmPID(); diff --git a/src/main/java/frc/robot/subsystems/arm/io/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java similarity index 80% rename from src/main/java/frc/robot/subsystems/arm/io/ArmIO.java rename to src/main/java/frc/robot/subsystems/arm/ArmIO.java index 7f4633b..9f4ca5d 100644 --- a/src/main/java/frc/robot/subsystems/arm/io/ArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.arm.io; +package frc.robot.subsystems.arm; import org.littletonrobotics.junction.AutoLog; @@ -11,6 +11,10 @@ public static class ArmIOInputs { public double _armMotorVoltage = 0.0; public double _armMotorSpeed = 0.0; + public double _feederMotorCurrent = 0.0; + public double _feederMotorVoltage = 0.0; + public double _feederMotorSpeed = 0.0; + public double _armEncoderPositionDegrees = 0.0; public double _armEncoderVelocity = 0.0; @@ -32,4 +36,5 @@ public default void resetIntakeEncoders() {} public default void setArmMotorVoltage(Voltage voltage) {} + public default void setFeederMotorSpeed(double speed) {} } diff --git a/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java b/src/main/java/frc/robot/subsystems/arm/RealArmIO.java similarity index 54% rename from src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java rename to src/main/java/frc/robot/subsystems/arm/RealArmIO.java index b0e01c3..f5faa65 100644 --- a/src/main/java/frc/robot/subsystems/arm/io/RealArmIO.java +++ b/src/main/java/frc/robot/subsystems/arm/RealArmIO.java @@ -1,9 +1,13 @@ -package frc.robot.subsystems.arm.io; +package frc.robot.subsystems.arm; import com.revrobotics.spark.SparkAbsoluteEncoder; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkFlexConfig; import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.DigitalInput; @@ -12,22 +16,29 @@ public class RealArmIO implements ArmIO { - public double POS_AT_90 = 0.0; - public double POS_AT_0 = 0.0; - public double ENCODER_CONVERSION = (POS_AT_90 - POS_AT_0) * 90; + private static final double POS_AT_90 = 0.447; + private static final double POS_AT_0 = 0.701; + private static final double ENCODER_CONVERSION = (POS_AT_90 - POS_AT_0) / 90.0; private double INTAKE_ROTATION_CONVERSION = 1; private SparkFlex _armMotor; private DigitalInput _lightSensor; - private SparkMax _intakeMotor; + private SparkFlex _intakeMotor; private SparkAbsoluteEncoder _armEncoder; + private SparkMax _feederMotor; public RealArmIO() { _armMotor = new SparkFlex(CAN.ARM_MTR_ID, MotorType.kBrushless); - _intakeMotor = new SparkMax(CAN.INTAKE_MTR_ID, MotorType.kBrushless); + _intakeMotor = new SparkFlex(CAN.INTAKE_MTR_ID, MotorType.kBrushless); _lightSensor = new DigitalInput(DIO.LIGHT_SENSOR_CHANNEL); - _armEncoder = _intakeMotor.getAbsoluteEncoder(); + _armEncoder = _armMotor.getAbsoluteEncoder(); + + SparkFlexConfig armConfig = new SparkFlexConfig(); + armConfig.idleMode(IdleMode.kBrake); + armConfig.inverted(true); + armConfig.voltageCompensation(12); + _armMotor.configure(armConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } public void updateInputs(ArmIOInputs inputs) { @@ -35,13 +46,17 @@ public void updateInputs(ArmIOInputs inputs) { inputs._armMotorCurrent = _armMotor.getOutputCurrent(); inputs._armMotorVoltage = _armMotor.getAppliedOutput() * _armMotor.getBusVoltage(); + inputs._feederMotorSpeed = _feederMotor.get(); + inputs._feederMotorCurrent = _feederMotor.getOutputCurrent(); + inputs._feederMotorVoltage = _feederMotor.getAppliedOutput() * _feederMotor.getBusVoltage(); + inputs._lightSensorState = !_lightSensor.get(); inputs._intakeMotorVelocityRotationsPerMin = _intakeMotor.get(); inputs._intakeMotorCurrent = _intakeMotor.getOutputCurrent(); inputs._intakeMotorVoltage = _intakeMotor.getAppliedOutput() * _armMotor.getBusVoltage(); inputs._intakeMotorPositionRotations = _intakeMotor.getEncoder().getPosition() * INTAKE_ROTATION_CONVERSION; - inputs._armEncoderPositionDegrees = _armEncoder.getPosition() * ENCODER_CONVERSION; + inputs._armEncoderPositionDegrees = (_armEncoder.getPosition() - POS_AT_0) / ENCODER_CONVERSION; inputs._armEncoderVelocity = _armEncoder.getVelocity(); } @@ -49,10 +64,14 @@ public void setArmMotorSpeed(double speed) { _armMotor.set(speed); } - public void setIntakeSpeed(double speed) { + public void setIntakeMotorSpeed(double speed) { _intakeMotor.set(speed); } + public void setFeederMotorSpeed(double speed){ + _feederMotor.set(speed); + + } public void resetIntakeEncoders() { _intakeMotor.getEncoder().setPosition(0); } @@ -60,4 +79,8 @@ public void resetIntakeEncoders() { public void setArmMotorVoltage(Voltage voltage) { _armMotor.setVoltage(voltage); } + + public void setFeederMotorVoltage(Voltage voltage) { + _feederMotor.setVoltage(voltage); + } } diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java new file mode 100644 index 0000000..464f269 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -0,0 +1,26 @@ +package frc.robot.subsystems.climber; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Climber extends SubsystemBase { + + private ClimberIO _io; + private ClimberIOInputsAutoLogged _inputs; + + public Climber(ClimberIO io) { + _io = io; + _inputs = new ClimberIOInputsAutoLogged(); + } + + public void setClimberSpeed(double speed) { + _io.setClimberMotorSpeed(speed); + } + + @Override + public void periodic() { + _io.updateInputs(_inputs); + Logger.processInputs("Climber", _inputs); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberCommands.java b/src/main/java/frc/robot/subsystems/climber/ClimberCommands.java new file mode 100644 index 0000000..5474c0f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberCommands.java @@ -0,0 +1,22 @@ +package frc.robot.subsystems.climber; + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; + +public class ClimberCommands { + private Climber _climber; + + public ClimberCommands(Climber climber) { + _climber = climber; + } + + public Command runClimber(DoubleSupplier ySupplier) { + return Commands.run( + () -> { + _climber.setClimberSpeed(ySupplier.getAsDouble()); + }, + _climber); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java new file mode 100644 index 0000000..90bcb9f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -0,0 +1,16 @@ +package frc.robot.subsystems.climber; + +import org.littletonrobotics.junction.AutoLog; + +import frc.robot.subsystems.arm.ArmIO.ArmIOInputs; + +public interface ClimberIO { + @AutoLog + public static class ClimberIOInputs { + public double _climberMotorSpeed = 0.0; + } + + public default void updateInputs(ClimberIOInputs inputs) {} + + public default void setClimberMotorSpeed(double speed) {} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/climber/RealClimberIO.java b/src/main/java/frc/robot/subsystems/climber/RealClimberIO.java new file mode 100644 index 0000000..16b4b90 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/RealClimberIO.java @@ -0,0 +1,30 @@ +package frc.robot.subsystems.climber; + +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import frc.robot.HardwareConstants.CAN; + +public class RealClimberIO implements ClimberIO{ + private SparkFlex _climberMotor; + + public RealClimberIO() { + _climberMotor = new SparkFlex(CAN.CLIMBER_MTR_ID, MotorType.kBrushless); + + SparkFlexConfig climberConfig = new SparkFlexConfig(); + climberConfig.idleMode(IdleMode.kBrake); + _climberMotor.configure(climberConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + public void updateInputs(ClimberIOInputs inputs) { + inputs._climberMotorSpeed = _climberMotor.get(); + } + + public void setClimberMotorSpeed(double speed) { + _climberMotor.set(speed); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java.old b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java similarity index 100% rename from src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java.old rename to src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 9f7b52d..5270b06 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -74,12 +74,23 @@ public class Drive extends SubsystemBase { Math.hypot(TunerConstants.BackLeft.LocationX, TunerConstants.BackLeft.LocationY), Math.hypot(TunerConstants.BackRight.LocationX, TunerConstants.BackRight.LocationY))); - // PathPlanner config constants to use if we can't read in the path planner json file - private static final double ROBOT_MASS_KG = 50.0; - private static final double ROBOT_MOI = 5.10; - private static final double WHEEL_COF = 1.13; - - + // PathPlanner config constants + private static final double ROBOT_MASS_KG = 74.088; + private static final double ROBOT_MOI = 6.883; + private static final double WHEEL_COF = 1.2; + private static final RobotConfig PP_CONFIG = + new RobotConfig( + ROBOT_MASS_KG, + ROBOT_MOI, + new ModuleConfig( + TunerConstants.FrontLeft.WheelRadius, + TunerConstants.kSpeedAt12Volts.in(MetersPerSecond), + WHEEL_COF, + DCMotor.getKrakenX60Foc(1) + .withReduction(TunerConstants.FrontLeft.DriveMotorGearRatio), + TunerConstants.FrontLeft.SlipCurrent, + 1), + getModuleTranslations()); static final Lock odometryLock = new ReentrantLock(); private final GyroIO gyroIO; @@ -119,53 +130,6 @@ public Drive( // Start odometry thread PhoenixOdometryThread.getInstance().start(); - - RobotConfig config; - try{ - config = RobotConfig.fromGUISettings(); - } catch (Exception e) { - // Handle exception as needed - e.printStackTrace(); - config = new RobotConfig( - ROBOT_MASS_KG, - ROBOT_MOI, - new ModuleConfig( - TunerConstants.FrontLeft.WheelRadius, - TunerConstants.kSpeedAt12Volts.in(MetersPerSecond), - WHEEL_COF, - DCMotor.getKrakenX60Foc(1) - .withReduction(TunerConstants.FrontLeft.DriveMotorGearRatio), - TunerConstants.FrontLeft.SlipCurrent, - 1), - getModuleTranslations()); - } - - // Configure AutoBuilder last - AutoBuilder.configure( - this::getPose, // Robot pose supplier - this::setPose, // Method to reset odometry (will be called if your auto has a starting pose) - this::getChassisSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - this::runVelocity, // 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 - ); - - /* // Configure AutoBuilder for PathPlanner AutoBuilder.configure( this::getPose, @@ -173,19 +137,16 @@ public Drive( this::getChassisSpeeds, this::runVelocity, new PPHolonomicDriveController( - new PIDConstants(3, 0.0, 0.0), new PIDConstants(5.5, 0.0, 0.0)), - PP_CONFIG, + new PIDConstants(5.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0)), + PP_CONFIG, () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, - this); */ - + this); Pathfinding.setPathfinder(new LocalADStarAK()); - PathPlannerLogging.setLogActivePathCallback( (activePath) -> { Logger.recordOutput( "Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); }); - PathPlannerLogging.setLogTargetPoseCallback( (targetPose) -> { Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); @@ -268,7 +229,6 @@ public void periodic() { * @param speeds Speeds in meters/sec */ public void runVelocity(ChassisSpeeds speeds) { - // Calculate module setpoints ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveCommands.java b/src/main/java/frc/robot/subsystems/drive/DriveCommands.java index 098cd7a..fb3ecca 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveCommands.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveCommands.java @@ -26,7 +26,6 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.drive.Drive; @@ -36,8 +35,6 @@ import java.util.List; import java.util.function.DoubleSupplier; import java.util.function.Supplier; -import frc.robot.util.TuneableProfiledPID; -import org.littletonrobotics.junction.Logger; public class DriveCommands { private static final double DEADBAND = 0.1; @@ -50,14 +47,6 @@ public class DriveCommands { private static final double WHEEL_RADIUS_MAX_VELOCITY = 0.25; // Rad/Sec private static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2 - public enum DriveMode { - dmJoystick, - dmAngle, - dmApproach -} - -private static DriveMode currentDriveMode = DriveMode.dmJoystick; - private DriveCommands() {} private static Translation2d getLinearVelocityFromJoysticks(double x, double y) { @@ -100,8 +89,9 @@ public static Command joystickDrive( linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), omega * drive.getMaxAngularSpeedRadPerSec()); - boolean isFlipped = DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red; + boolean isFlipped = + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; drive.runVelocity( ChassisSpeeds.fromFieldRelativeSpeeds( speeds, @@ -166,101 +156,6 @@ public static Command joystickDriveAtAngle( .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians())); } - /** - * Robot relative drive command using joystick for linear control towards the approach target, - * PID for aligning with the target laterally, and PID for angular control. Used for approaching - * a known target, usually from a short distance. The approachSupplier must supply a Pose2d with - * a rotation facing away from the target - */ - public static Command joystickApproach( - Drive drive, - DoubleSupplier ySupplier, - Supplier approachSupplier) - { - - // Create PID controller - TuneableProfiledPID angleController = - new TuneableProfiledPID( - "angleController", - ANGLE_KP, - 0.0, - ANGLE_KD, - ANGLE_MAX_VELOCITY, - ANGLE_MAX_ACCELERATION); - angleController.enableContinuousInput(-Math.PI, Math.PI); - - TuneableProfiledPID alignController = - new TuneableProfiledPID( - "alignController", - 0.3, - 0.0, - 0, - 20, - 8); - alignController.setGoal(0); - - // Construct command - return Commands.run( - () -> { - - //System.out.println("JOYSTICK APPROACH!!!"); - currentDriveMode = DriveMode.dmApproach; - // Name constants - Translation2d currentTranslation = drive.getPose().getTranslation(); - Translation2d approachTranslation = approachSupplier.get().getTranslation(); - double distanceToApproach = currentTranslation.getDistance(approachTranslation); - - Rotation2d alignmentDirection = approachSupplier.get().getRotation(); - - // Find lateral distance from goal - Translation2d goalTranslation = new Translation2d( - alignmentDirection.getCos() * distanceToApproach + approachTranslation.getX(), - alignmentDirection.getSin() * distanceToApproach + approachTranslation.getY()); - - Translation2d robotToGoal = currentTranslation.minus(goalTranslation); - double distanceToGoal = - Math.hypot(robotToGoal.getX(), robotToGoal.getY()); - - // Calculate lateral linear velocity - Translation2d offsetVector = - new Translation2d(alignController.calculate(distanceToGoal), 0) - .rotateBy(robotToGoal.getAngle()); - - Logger.recordOutput("AlignDebug/Current", distanceToGoal); - - // Calculate total linear velocity - Translation2d linearVelocity = - getLinearVelocityFromJoysticks(0, - ySupplier.getAsDouble()).rotateBy( - approachSupplier.get().getRotation()).rotateBy(Rotation2d.kCCW_90deg) - .plus(offsetVector); - - SmartDashboard.putData(alignController); // TODO: Calibrate PID - Logger.recordOutput("AlignDebug/approachTarget", approachTranslation); - - // Calculate angular speed - double omega = - angleController.calculate( - drive.getRotation().getRadians(), approachSupplier.get().getRotation() - .rotateBy(Rotation2d.k180deg).getRadians()); - - // Convert to field relative speeds & send command - ChassisSpeeds speeds = - new ChassisSpeeds( - linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), - linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), - omega); - drive.runVelocity( - ChassisSpeeds.fromFieldRelativeSpeeds( - speeds, - drive.getRotation())); - }, - drive) - - // Reset PID controller when command starts - .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians())); - } - /** * Measures the velocity feedforward constants for the drive motors. * diff --git a/src/main/java/frc/robot/subsystems/drive/TunerConstants.java b/src/main/java/frc/robot/subsystems/drive/TunerConstants.java index 5538d02..f6cdc10 100644 --- a/src/main/java/frc/robot/subsystems/drive/TunerConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/TunerConstants.java @@ -22,13 +22,13 @@ public class TunerConstants { // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(107).withKI(0).withKD(0.7) + .withKP(100).withKI(0).withKD(0.5) .withKS(0.1).withKV(2.66).withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(2.3).withKI(0).withKD(0) + .withKP(0.1).withKI(0).withKD(0) .withKS(0).withKV(0.124); // The closed-loop output type to use for the steer motors; @@ -72,8 +72,8 @@ public class TunerConstants { // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.21); - + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.21*0.7); + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot private static final double kCoupleRatio = 3.5714285714285716; @@ -127,7 +127,7 @@ public class TunerConstants { private static final int kFrontLeftDriveMotorId = 1; private static final int kFrontLeftSteerMotorId = 2; private static final int kFrontLeftEncoderId = 3; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.3748); + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.378662109375); private static final boolean kFrontLeftSteerMotorInverted = true; private static final boolean kFrontLeftEncoderInverted = false; @@ -138,7 +138,7 @@ public class TunerConstants { private static final int kFrontRightDriveMotorId = 4; private static final int kFrontRightSteerMotorId = 5; private static final int kFrontRightEncoderId = 6; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.170); + private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.221435546875); private static final boolean kFrontRightSteerMotorInverted = true; private static final boolean kFrontRightEncoderInverted = false; @@ -149,7 +149,7 @@ public class TunerConstants { private static final int kBackLeftDriveMotorId = 7; private static final int kBackLeftSteerMotorId = 8; private static final int kBackLeftEncoderId = 9; - private static final Angle kBackLeftEncoderOffset = Rotations.of(0.2615); + private static final Angle kBackLeftEncoderOffset = Rotations.of(0.2783203125); private static final boolean kBackLeftSteerMotorInverted = true; private static final boolean kBackLeftEncoderInverted = false; @@ -160,7 +160,7 @@ public class TunerConstants { private static final int kBackRightDriveMotorId = 10; private static final int kBackRightSteerMotorId = 11; private static final int kBackRightEncoderId = 12; - private static final Angle kBackRightEncoderOffset = Rotations.of(-0.1426); + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.142333984375); private static final boolean kBackRightSteerMotorInverted = true; private static final boolean kBackRightEncoderInverted = false; @@ -193,12 +193,11 @@ public class TunerConstants { * Creates a CommandSwerveDrivetrain instance. * This should only be called once in your robot program,. */ - /* public static CommandSwerveDrivetrain createDrivetrain() { return new CommandSwerveDrivetrain( DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight ); - } */ + } /** diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index facbe08..d5cb940 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -7,49 +7,33 @@ import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Rotation; import static edu.wpi.first.units.Units.Volt; import org.littletonrobotics.junction.Logger; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkFlexConfig; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; - import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.VelocityUnit; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.Velocity; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; -import frc.robot.HardwareConstants.CAN; + import frc.robot.subsystems.arm.Arm.ArmPosition; -import frc.robot.subsystems.arm.io.ArmIO.ArmIOInputs; -import frc.robot.subsystems.elevator.io.ElevatorIO; -import frc.robot.subsystems.elevator.io.ElevatorIO.ElevatorIOInputs; +import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands; +import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands.GamepieceMode; public class Elevator extends SubsystemBase { public enum ElevatorPosition { - L1(1), - L2(2), - L3(3), - L4(4), - Stow(5); + L1(5), + L2(9), + L3(25.5), + L4(48), + Stow(0.5); public final double setpoint; @@ -59,65 +43,65 @@ private ElevatorPosition(double setpoint) { } private static final double CALIBRATION_SPEED = -0.1; - private static final double HARD_STOP_CURRENT_LIMIT = 30; + private static final double HARD_STOP_CURRENT_LIMIT = 50; private static final double INCREMENT_CONSTANT = 1; private static final double DECREMENT_CONSTANT = 1; - private static final double ELEVATOR_MOTOR_KP = 0.21; // 0.08 - private static final double ELEVATOR_MOTOR_KI = 0; // 0.001 - private static final double ELEVATOR_MOTOR_KD = 0; // 0.002 - private static final double ELEVATOR_PID_VEL = 120; - private static final double ELEVATOR_PID_ACC = 125; + private static final double ELEVATOR_MOTOR_KP = 0.75; + private static final double ELEVATOR_MOTOR_KI = 0.15; + private static final double ELEVATOR_MOTOR_KD = 0; + private static final double ELEVATOR_PID_VEL = 220; + private static final double ELEVATOR_PID_ACC = 215; - private static final double ELEVATOR_MOTOR_TOLERANCE = 0.0; - - private static final double ELEVATOR_MAX_INCHES = 14.0; - private static final double ELEVATOR_MAX_ROTATIONS = 28.67; + private static final double ELEVATOR_MAX_INCHES = 48; + private static final double ELEVATOR_MAX_ROTATIONS = 36.4; private static final double MOTOR_CONVERSION = ELEVATOR_MAX_INCHES / ELEVATOR_MAX_ROTATIONS; - // In pounds - private static final double ELEVATOR_STAGE1_WEIGHT = 3.75; - private static final double ELEVATOR_STAGE2_WEIGHT = 0; - - // In inches - private static final double SPOOL_DIAMETER = 1.6; + // In newtons + private static final double ELEVATOR_STAGE1_WEIGHT_N = 2.053 * 9.81; + private static final double ELEVATOR_STAGE2_WEIGHT_N = 7.554 * 9.81; + private static final double ELEVATOR_TOTAL_WEIGHT_N = ELEVATOR_STAGE1_WEIGHT_N + (ELEVATOR_STAGE2_WEIGHT_N); - // In pound - inches - private static final double STALL_TORQUE = 62.75; - - // In amps - private static final double STALL_CURRENT = 366; - - // In Ohms - private static final double RESISTANCE = 0.0185; + // In m + private static final double SPOOL_DIAMETER = 0.0527; + private static final double ELEVATOR_STALL_TORQUE_LB_IN = 3.6; + private static final double ELEVATOR_STALL_CURRENT = 211; + private static final double ELEVATOR_KT = ELEVATOR_STALL_TORQUE_LB_IN / ELEVATOR_STALL_CURRENT; private static final double GEAR_RATIO = 7.75; private static final double NUMBER_OF_MOTORS = 2; + private static final double EFFECTIVE_KT = ELEVATOR_KT * NUMBER_OF_MOTORS * GEAR_RATIO; + private static final double ELEVATOR_RESISTANCE = 0.057; - private static final double FEEDFORWARD_CONSTANT = ((ELEVATOR_STAGE1_WEIGHT + (2 * ELEVATOR_STAGE2_WEIGHT)) - * SPOOL_DIAMETER * STALL_CURRENT * RESISTANCE) / (NUMBER_OF_MOTORS * GEAR_RATIO * STALL_TORQUE); + private static final double FEEDFORWARD_CONSTANT = ((ELEVATOR_TOTAL_WEIGHT_N * SPOOL_DIAMETER * ELEVATOR_RESISTANCE) / (EFFECTIVE_KT)) - 0.65; private boolean _isCalibrated; private ElevatorPosition _currentPos; + private ElevatorPosition _desiredPos; + private MultiSubsystemCommands.GamepieceMode _currentMode; private ProfiledPIDController _elevatorMotorPID; private ElevatorIO _io; - private ElevatorIOInputs _inputs; + private ElevatorIOInputsAutoLogged _inputs; + + SysIdRoutine routine = new SysIdRoutine(new Config(), new SysIdRoutine.Mechanism(this::setElevatorVoltage, this::populateLog, this)); public Elevator(ElevatorIO io) { _io = io; - _inputs = new ElevatorIOInputs(); + _inputs = new ElevatorIOInputsAutoLogged(); _elevatorMotorPID = new ProfiledPIDController(ELEVATOR_MOTOR_KP, ELEVATOR_MOTOR_KI, ELEVATOR_MOTOR_KD, new Constraints(ELEVATOR_PID_VEL, ELEVATOR_PID_ACC)); - _elevatorMotorPID.setTolerance(ELEVATOR_MOTOR_TOLERANCE); - _elevatorMotorPID.setGoal(0.25); + _elevatorMotorPID.setTolerance(0.5); + + _currentPos = ElevatorPosition.Stow; + _desiredPos = ElevatorPosition.Stow; } // Runs the motors down at the calibration speed @@ -135,20 +119,23 @@ public void setElevatorVoltage(Voltage voltage) { public void setSetpoint(ElevatorPosition setpoint) { setSetpoint(setpoint.setpoint); - _currentPos = setpoint; + _desiredPos = setpoint; } // Sets the setpoint of the PID public void setSetpoint(double setpoint) { if (setpoint < 0.25 || setpoint > ELEVATOR_MAX_INCHES) return; - _elevatorMotorPID.setGoal(setpoint); _elevatorMotorPID.reset(getCurrentPosInches()); + _elevatorMotorPID.setGoal(setpoint); } // Checks if it is at the setpoint public boolean isAtSetpoint() { - return _elevatorMotorPID.atSetpoint(); + boolean atSetpoint = Math.abs(_elevatorMotorPID.getGoal().position - getCurrentPosInches()) <= 0.5; + if (atSetpoint) + _currentPos = _desiredPos; + return atSetpoint; } // Increases elevator position @@ -174,7 +161,8 @@ public void resetEncoders() { // Runs motors with PID public void runMotorsWithPID() { - _io.setElevatorSpeed(_elevatorMotorPID.calculate(getCurrentPosInches())); + // _io.setElevatorSpeed(); + _io.setElevatorVoltage(Voltage.ofBaseUnits(_elevatorMotorPID.calculate(getCurrentPosInches()) + FEEDFORWARD_CONSTANT, Volt)); } public boolean isCalibrated() { @@ -187,26 +175,23 @@ public void setIsCalibrated(boolean isCalibrated) { // Converts the motors position from weird units to normal people inches public double getCurrentPosInches() { - return _inputs._elevatorPosition * MOTOR_CONVERSION; + return (_inputs._elevatorPosition * MOTOR_CONVERSION) - 0.25; } public ElevatorPosition getCurrentPos() { return _currentPos; } - public boolean canMoveToPos(ElevatorPosition currentElevator, ArmPosition desiredArm) { - switch (currentElevator) { - case L1: - case L2: - case L3: - return (desiredArm != ArmPosition.L4_Score) || (desiredArm != ArmPosition.Loading); - case L4: - return desiredArm != ArmPosition.Loading; - case Stow: - return desiredArm != ArmPosition.L4_Score; - default: - return false; - } + public GamepieceMode getCurrentMode() { + return _currentMode; + } + + public void setCurrentMode(GamepieceMode mode) { + _currentMode = mode; + } + + public double getElevatorMaxHeight() { + return ELEVATOR_MAX_INCHES; } public void populateLog(SysIdRoutineLog log) { @@ -228,6 +213,7 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { @Override public void periodic() { _io.updateInputs(_inputs); + Logger.processInputs("Elevator", _inputs); // This method will be called once per scheduler run Logger.recordOutput("Elevator/currentPos", getCurrentPosInches()); @@ -236,8 +222,9 @@ public void periodic() { Logger.recordOutput("Elevator/Current", _inputs._elevatorMotorCurrent); Logger.recordOutput("Elevator/motorSpeed", _inputs._elevatorSpeed); Logger.recordOutput("Elevator/CurrentSetpoint", _elevatorMotorPID.getSetpoint().position); - - // Logger.recordOutput("Elevator/motorSpeed", _primaryMotor.get()); + Logger.recordOutput("Elevator/atSetpoint", isAtSetpoint()); + Logger.recordOutput("Elevator/currentPosEnum", _currentPos); + Logger.recordOutput("Elevator/desiredPosEnum", _desiredPos); } } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java index 02025b2..38f5634 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorCommands.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.robot.subsystems.elevator.Elevator.ElevatorPosition; +import frc.robot.subsystems.multisubsystemcommands.MultiSubsystemCommands.GamepieceMode; public final class ElevatorCommands { private Elevator _elevator; diff --git a/src/main/java/frc/robot/subsystems/elevator/io/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java similarity index 94% rename from src/main/java/frc/robot/subsystems/elevator/io/ElevatorIO.java rename to src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index e354123..a681447 100644 --- a/src/main/java/frc/robot/subsystems/elevator/io/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.elevator.io; +package frc.robot.subsystems.elevator; import org.littletonrobotics.junction.AutoLog; diff --git a/src/main/java/frc/robot/subsystems/elevator/io/RealElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/RealElevatorIO.java similarity index 75% rename from src/main/java/frc/robot/subsystems/elevator/io/RealElevatorIO.java rename to src/main/java/frc/robot/subsystems/elevator/RealElevatorIO.java index e8275a1..5986afc 100644 --- a/src/main/java/frc/robot/subsystems/elevator/io/RealElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/RealElevatorIO.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.elevator.io; +package frc.robot.subsystems.elevator; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; @@ -11,8 +11,8 @@ import com.revrobotics.spark.config.SparkFlexConfig; import frc.robot.HardwareConstants.CAN; -import frc.robot.subsystems.arm.io.ArmIO; -import frc.robot.subsystems.elevator.io.ElevatorIO.ElevatorIOInputs; +import frc.robot.subsystems.arm.ArmIO; +import frc.robot.subsystems.elevator.ElevatorIO.ElevatorIOInputs; public class RealElevatorIO implements ElevatorIO { private SparkFlex _primaryMotor; @@ -24,13 +24,14 @@ public RealElevatorIO() { _secondaryMotor = new SparkFlex(CAN.SECONDARY_ELEVATOR_ID, MotorType.kBrushless); SparkFlexConfig primaryConfig = new SparkFlexConfig(); - primaryConfig.inverted(true); - primaryConfig.idleMode(IdleMode.kCoast); - _primaryMotor.configure(primaryConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + primaryConfig.inverted(false); + primaryConfig.idleMode(IdleMode.kBrake); + _primaryMotor.configure(primaryConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); SparkFlexConfig secondaryConfig = new SparkFlexConfig(); secondaryConfig.follow(CAN.PRIMARY_ELEVATOR_ID); - _secondaryMotor.configure(secondaryConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + secondaryConfig.idleMode(IdleMode.kBrake); + _secondaryMotor.configure(secondaryConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } public void updateInputs(ElevatorIOInputs inputs) { @@ -42,7 +43,7 @@ public void updateInputs(ElevatorIOInputs inputs) { } public void setElevatorSpeed(double speed) { - _primaryMotor.set(speed); + _primaryMotor.set(speed); } public void setElevatorVoltage(Voltage voltage) { diff --git a/src/main/java/frc/robot/subsystems/leds/LEDs.java b/src/main/java/frc/robot/subsystems/leds/LEDs.java index a2614b3..11da210 100644 --- a/src/main/java/frc/robot/subsystems/leds/LEDs.java +++ b/src/main/java/frc/robot/subsystems/leds/LEDs.java @@ -6,9 +6,11 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.HardwareConstants; import frc.robot.HardwareConstants.CAN; +import frc.robot.subsystems.elevator.Elevator; import com.ctre.phoenix.led.RainbowAnimation; import com.ctre.phoenix.led.StrobeAnimation; @@ -20,10 +22,12 @@ public class LEDs extends SubsystemBase { CANdle _candle = new CANdle(CAN.CANDLE_ID); - static int _numLEDs = 8; + static int _numLEDs = 60; //leds/height * currentheight boolean _alreadyRunning = false; LEDAnimation _currentAnimation = LEDAnimation.None; + Elevator _elevator; + public enum LEDAnimation { None(null, null, 0), @@ -43,7 +47,9 @@ public enum LEDAnimation { SolidTeal(new LEDColor(0, 225, 174), null, 0), - SolidCoral(new LEDColor(255, 80, 15), null, 0); + SolidCoral(new LEDColor(255, 80, 15), null, 0), + + SolidRed(new LEDColor(255, 0, 0), null, 0); LEDColor _color; Animation _animation; @@ -81,6 +87,11 @@ public void runAnimation(LEDAnimation animation) { _candle.clearAnimation(0); _candle.setLEDs(0, 0, 0); _candle.animate(animation.getAnimation()); + } else if (animation == LEDAnimation.SolidRed) { + _candle.clearAnimation(0); + LEDColor color = animation.getColor(); + _candle.setLEDs(color.getR(), color.getG(), color.getB(),0, 0, + (int) Math.round(_numLEDs/_elevator.getElevatorMaxHeight() * _elevator.getCurrentPosInches())); } else if (animation.getAnimation() == null) { LEDColor color = animation.getColor(); _candle.clearAnimation(0); @@ -119,7 +130,7 @@ public void pickingUpCoral() { public void elevatorOrArmIsMoving() { if (!_alreadyRunning) { - runAnimation(LEDAnimation.BlinkDarkBlue); + runAnimation(LEDAnimation.SolidDarkBlue); _alreadyRunning = false; } } @@ -145,6 +156,13 @@ public void robotHasClimbed() { } } + public void disabledAnimation1() { + if (!_alreadyRunning) { + runAnimation(LEDAnimation.PartyMode); + _alreadyRunning = false; + } + } + public void reset() { _candle.clearAnimation(0); _alreadyRunning = false; @@ -152,6 +170,7 @@ public void reset() { @Override public void periodic() { + Logger.recordOutput("LEDs/alreadyRunning", _alreadyRunning); Logger.recordOutput("LEDs/CurrentAnimation", _currentAnimation.toString()); } } diff --git a/src/main/java/frc/robot/subsystems/leds/LEDsCommands.java b/src/main/java/frc/robot/subsystems/leds/LEDsCommands.java index a664aca..1649a9d 100644 --- a/src/main/java/frc/robot/subsystems/leds/LEDsCommands.java +++ b/src/main/java/frc/robot/subsystems/leds/LEDsCommands.java @@ -14,6 +14,7 @@ public LEDsCommands(LEDs leds) { _leds = leds; } + //Runs when intake button is pushed public Command intaking() { return new StartEndCommand( () -> { @@ -25,6 +26,7 @@ public Command intaking() { _leds); } + //Runs when we have just acquired a piece public Command hasPiece() { return new StartEndCommand( () -> { @@ -33,9 +35,10 @@ public Command hasPiece() { () -> { _leds.reset(); }, - _leds); + _leds).withTimeout(3); } + // Runs when we have no gamepiece and we are toggled to pick up coral public Command pickingUpCoral() { return new StartEndCommand( () -> { @@ -47,6 +50,7 @@ public Command pickingUpCoral() { _leds); } + // Runs when we have no gamepiece and we are toggled to pick up coral public Command pickingUpAlgae() { return new StartEndCommand( () -> { @@ -58,6 +62,7 @@ public Command pickingUpAlgae() { _leds); } + //We have a piece and hasPiece animation has already run public Command elevatorOrArmIsMoving() { return new StartEndCommand( () -> { @@ -69,6 +74,7 @@ public Command elevatorOrArmIsMoving() { _leds); } + //Runs when robot has finished climbing public Command robotHasClimbed() { return new StartEndCommand( () -> { @@ -79,15 +85,21 @@ public Command robotHasClimbed() { }, _leds); } - - public Command elevatorAndArmAtSetpoints() { + + public Command disabledAnimation1() { return new StartEndCommand( () -> { - _leds.elevatorAndArmAtSetpoints(); - }, + _leds.disabledAnimation1(); + }, () -> { _leds.reset(); }, - _leds); + _leds) { + @Override + public boolean runsWhenDisabled() { + return true; + } + }; } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java index 4fcb7fd..e679f13 100644 --- a/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java +++ b/src/main/java/frc/robot/subsystems/multisubsystemcommands/MultiSubsystemCommands.java @@ -1,6 +1,9 @@ package frc.robot.subsystems.multisubsystemcommands; +import java.util.function.Supplier; + import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.subsystems.arm.Arm; import frc.robot.subsystems.arm.Arm.ArmPosition; @@ -12,7 +15,7 @@ public class MultiSubsystemCommands { public enum OverallPosition { Stow(ElevatorPosition.Stow, ArmPosition.Stow), - Loading(ElevatorPosition.Stow, ArmPosition.Loading), + Loading(ElevatorPosition.Stow, ArmPosition.Loading_Coral), L1(ElevatorPosition.L1, ArmPosition.Stow), L2(ElevatorPosition.L2, ArmPosition.Stow), L3(ElevatorPosition.L3, ArmPosition.Stow), @@ -36,6 +39,11 @@ ArmPosition getArmPosition() { } } + public enum GamepieceMode { + ALGAE, + CORAL; + } + private Elevator _elevator; private Arm _arm; private ElevatorCommands _elevatorCommands; @@ -52,7 +60,16 @@ public MultiSubsystemCommands(Elevator elevator, Arm arm, ElevatorCommands eleva public Command setOverallSetpoint(OverallPosition setpoint) { return _elevatorCommands.setElevatorSetpoint(setpoint.getElevatorPosition()) .alongWith(_armCommands.setArmPosition(setpoint.getArmPosition())) - .unless(() -> !_elevator.canMoveToPos(_elevator.getCurrentPos(), setpoint.getArmPosition())); + .unless(() -> !canMoveToPos(_elevator.getCurrentPos(), setpoint.getElevatorPosition(), + _arm.getCurrentPos(), setpoint.getArmPosition())); + } + + public Command setGamepieceMode(GamepieceMode mode) { + return new InstantCommand( + () -> { + _elevator.setCurrentMode(mode); + _arm.setCurrentMode(mode); + }, _elevator, _arm); } public Command waitForOverallMechanism() { @@ -72,11 +89,11 @@ private Command score(OverallPosition setpoint) { } } - public Command scoreGamepieceAtPosition(OverallPosition setpoint) { - - return new InstantCommand(); + public Command scoreGamepieceAtPosition(Supplier setpoint) { + return scoreGamepieceAtPosition(setpoint.get()); + } - /* + public Command scoreGamepieceAtPosition(OverallPosition setpoint) { if (setpoint == OverallPosition.Stow || setpoint == OverallPosition.Loading || setpoint == OverallPosition.L4_Score) { throw new RuntimeException("scoreGamepieceAtPosition cannot run to stow,loading,or L4_score"); @@ -84,14 +101,110 @@ public Command scoreGamepieceAtPosition(OverallPosition setpoint) { return setOverallSetpoint(setpoint) .andThen(waitForOverallMechanism()) .andThen(score(setpoint)) - .andThen(setOverallSetpoint(OverallPosition.Stow)); */ + .andThen(setOverallSetpoint(OverallPosition.Stow)); + } + + public Command loadGamepiece() { + return Commands.either(loadAlgae(), loadCoral(), () -> _arm.getCurrentMode() == GamepieceMode.ALGAE); } - public Command intake() { + public Command loadCoral() { return setOverallSetpoint(OverallPosition.Loading) .andThen(waitForOverallMechanism()) .andThen(_armCommands.intake()) - .andThen(setOverallSetpoint(OverallPosition.Stow)); + .andThen(setOverallSetpoint(OverallPosition.Stow)) + .andThen(_armCommands.moveGamepieceToLightSensor()) + .unless(() -> !canMoveToPos(_elevator.getCurrentPos(), ElevatorPosition.Stow, + _arm.getCurrentPos(), ArmPosition.Loading_Coral)); + } -} - \ No newline at end of file + + public Command loadAlgae() { + return setOverallSetpoint(OverallPosition.Loading) + .andThen(waitForOverallMechanism()) + .andThen(_armCommands.intake()) + .andThen(setOverallSetpoint(OverallPosition.Stow)) + .unless(() -> !canMoveToPos(_elevator.getCurrentPos(), ElevatorPosition.L2, + _arm.getCurrentPos(), ArmPosition.Loading_Algae)); + } + + public boolean canMoveToPos(ElevatorPosition currentElevator, ElevatorPosition desiredElevator, + ArmPosition currentArm, ArmPosition desiredArm) { + boolean canMoveArm = false; + boolean canMoveElevator = false; + + if (_arm.getCurrentMode() == GamepieceMode.CORAL) { + switch (currentElevator) { + case L1: + case L2: + case L3: + canMoveArm = (desiredArm != ArmPosition.L4_Score) && (desiredArm != ArmPosition.Loading_Coral); + break; + case L4: + canMoveArm = (desiredArm != ArmPosition.Loading_Coral); + break; + case Stow: + canMoveArm = (desiredArm != ArmPosition.L4_Score); + break; + default: + canMoveArm = false; + break; + } + + switch (desiredElevator) { + case L1: + case L2: + case L3: + canMoveElevator = (currentArm != ArmPosition.L4_Score) && (currentArm != ArmPosition.Loading_Coral); + break; + case L4: + canMoveElevator = (currentArm != ArmPosition.Loading_Coral); + break; + case Stow: + canMoveElevator = (currentArm != ArmPosition.L4_Score); + break; + default: + canMoveElevator = false; + break; + } + } else { + switch (currentElevator) { + case L1: + case L4: + canMoveArm = false; + break; + case L2: + case L3: + canMoveArm = desiredArm != ArmPosition.Algae_Score; + break; + case Stow: + canMoveArm = desiredArm != ArmPosition.Loading_Algae; + break; + default: + canMoveArm = false; + break; + } + + switch (desiredElevator) { + case L1: + case L4: + canMoveElevator = false; + break; + case L2: + case L3: + canMoveElevator = currentArm != ArmPosition.Algae_Score; + break; + case Stow: + canMoveElevator = currentArm != ArmPosition.Loading_Algae; + break; + default: + canMoveElevator = false; + break; + } + } + System.out.println("Arm: " + canMoveArm + " Elevator: " + canMoveElevator); + + return canMoveArm && canMoveElevator; + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/presets/Preset.java b/src/main/java/frc/robot/subsystems/presets/Preset.java index 9b4dac4..cf0a89f 100644 --- a/src/main/java/frc/robot/subsystems/presets/Preset.java +++ b/src/main/java/frc/robot/subsystems/presets/Preset.java @@ -10,7 +10,7 @@ public enum ReefSide { Right; } - private OverallPosition _level = OverallPosition.Stow; + private OverallPosition _level = OverallPosition.L1; private ReefSide _side = ReefSide.Right; private boolean _isLevelValid = false; private boolean _isSideValid = false; diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java deleted file mode 100644 index 07e3d99..0000000 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ /dev/null @@ -1,192 +0,0 @@ -// Code modified from AdvantageKit Vision Template (v4.0.1): -// https://github.com/Mechanical-Advantage/AdvantageKit/releases -// -// Copyright 2021-2025 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package frc.robot.subsystems.vision; - -import static frc.robot.subsystems.vision.VisionConstants.*; - -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.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.Alert.AlertType; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.subsystems.vision.VisionIO.PoseObservationType; -import java.util.LinkedList; -import java.util.List; -import org.littletonrobotics.junction.Logger; - - -public class Vision extends SubsystemBase { - private final VisionConsumer consumer; - private final VisionIO[] io; - private final VisionIOInputsAutoLogged[] inputs; - private final Alert[] disconnectedAlerts; - - public Vision(VisionConsumer consumer, VisionIO... io) { - this.consumer = consumer; - this.io = io; - - // Initialize inputs - this.inputs = new VisionIOInputsAutoLogged[io.length]; - for (int i = 0; i < inputs.length; i++) { - inputs[i] = new VisionIOInputsAutoLogged(); - } - - // Initialize disconnected alerts - this.disconnectedAlerts = new Alert[io.length]; - for (int i = 0; i < inputs.length; i++) { - disconnectedAlerts[i] = - new Alert( - "Vision camera " + Integer.toString(i) + " is disconnected.", AlertType.kWarning); - } - } - - /** - * Returns the X angle to the best target, which can be used for simple servoing with vision. - * - * @param cameraIndex The index of the camera to use. - */ - public Rotation2d getTargetX(int cameraIndex) { - return inputs[cameraIndex].latestTargetObservation.tx(); - } - - @Override - public void periodic() { - for (int i = 0; i < io.length; i++) { - io[i].updateInputs(inputs[i]); - Logger.processInputs("Vision/Camera" + Integer.toString(i), inputs[i]); - } - - // Initialize logging values - List allTagPoses = new LinkedList<>(); - List allRobotPoses = new LinkedList<>(); - List allRobotPosesAccepted = new LinkedList<>(); - List allRobotPosesRejected = new LinkedList<>(); - - // Loop over cameras - for (int cameraIndex = 0; cameraIndex < io.length; cameraIndex++) { - // Update disconnected alert - disconnectedAlerts[cameraIndex].set(!inputs[cameraIndex].connected); - - // Initialize logging values - List tagPoses = new LinkedList<>(); - List robotPoses = new LinkedList<>(); - List robotPosesAccepted = new LinkedList<>(); - List robotPosesRejected = new LinkedList<>(); - - // Add tag poses - for (int tagId : inputs[cameraIndex].tagIds) { - var tagPose = aprilTagLayout.getTagPose(tagId); - if (tagPose.isPresent()) { - tagPoses.add(tagPose.get()); - } - } - - // Loop over pose observations - for (var observation : inputs[cameraIndex].poseObservations) { - // Check whether to reject pose - boolean rejectPose = - observation.tagCount() == 0 // Must have at least one tag - || (observation.tagCount() == 1 - && observation.ambiguity() > maxAmbiguity) // Cannot be high ambiguity - || Math.abs(observation.pose().getZ()) - > maxZError // Must have realistic Z coordinate - - // Must be within the field boundaries - || observation.pose().getX() < 0.0 - || observation.pose().getX() > aprilTagLayout.getFieldLength() - || observation.pose().getY() < 0.0 - || observation.pose().getY() > aprilTagLayout.getFieldWidth(); - - // Add pose to log - robotPoses.add(observation.pose()); - if (rejectPose) { - robotPosesRejected.add(observation.pose()); - } else { - robotPosesAccepted.add(observation.pose()); - } - - // Skip if rejected - if (rejectPose) { - continue; - } - - // Calculate standard deviations - double stdDevFactor = - Math.pow(observation.averageTagDistance(), 2.0) / observation.tagCount(); - double linearStdDev = linearStdDevBaseline * stdDevFactor; - double angularStdDev = angularStdDevBaseline * stdDevFactor; - if (observation.type() == PoseObservationType.MEGATAG_2) { - linearStdDev *= linearStdDevMegatag2Factor; - angularStdDev *= angularStdDevMegatag2Factor; - } - if (cameraIndex < cameraStdDevFactors.length) { - linearStdDev *= cameraStdDevFactors[cameraIndex]; - angularStdDev *= cameraStdDevFactors[cameraIndex]; - } - - // Send vision observation - consumer.accept( - observation.pose().toPose2d(), - observation.timestamp(), - VecBuilder.fill(linearStdDev, linearStdDev, angularStdDev)); - } - - // Log camera datadata - Logger.recordOutput( - "Vision/Camera" + Integer.toString(cameraIndex) + "/TagPoses", - tagPoses.toArray(new Pose3d[tagPoses.size()])); - Logger.recordOutput( - "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPoses", - robotPoses.toArray(new Pose3d[robotPoses.size()])); - Logger.recordOutput( - "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPosesAccepted", - robotPosesAccepted.toArray(new Pose3d[robotPosesAccepted.size()])); - Logger.recordOutput( - "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPosesRejected", - robotPosesRejected.toArray(new Pose3d[robotPosesRejected.size()])); - allTagPoses.addAll(tagPoses); - allRobotPoses.addAll(robotPoses); - allRobotPosesAccepted.addAll(robotPosesAccepted); - allRobotPosesRejected.addAll(robotPosesRejected); - } - - // Log summary data - Logger.recordOutput( - "Vision/Summary/TagPoses", allTagPoses.toArray(new Pose3d[allTagPoses.size()])); - Logger.recordOutput( - "Vision/Summary/RobotPoses", allRobotPoses.toArray(new Pose3d[allRobotPoses.size()])); - Logger.recordOutput( - "Vision/Summary/RobotPosesAccepted", - allRobotPosesAccepted.toArray(new Pose3d[allRobotPosesAccepted.size()])); - Logger.recordOutput( - "Vision/Summary/RobotPosesRejected", - allRobotPosesRejected.toArray(new Pose3d[allRobotPosesRejected.size()])); - } - - @FunctionalInterface - public static interface VisionConsumer { - public void accept( - Pose2d visionRobotPoseMeters, - double timestampSeconds, - Matrix visionMeasurementStdDevs); - } -} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java deleted file mode 100644 index e4ce08f..0000000 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ /dev/null @@ -1,89 +0,0 @@ -// Code modified from AdvantageKit Vision Template (v4.0.1): -// https://github.com/Mechanical-Advantage/AdvantageKit/releases -// -// Copyright 2021-2025 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package frc.robot.subsystems.vision; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.util.Units; - -public class VisionConstants { - // AprilTag layout - public static AprilTagFieldLayout aprilTagLayout = - AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeAndyMark); - - // Camera names, must match names configured on coprocessor - - // New cameras for 2025 season are 5,6,7,8 - - public static String camera5Name = "photoncamera_5"; - public static String camera6Name = "photoncamera_6"; - public static String camera7Name = "photoncamera_7"; - public static String camera8Name = "photoncamera_8"; - - // Robot to camera transforms - // (Not used by Limelight, configure in web UI instead) - - // Note: 0.0254 multiplier converts inches to meters - - // Camera 5 - public static Transform3d robotToCamera5 = - new Transform3d(Units.inchesToMeters(-13.75), Units.inchesToMeters(-11.25), Units.inchesToMeters(9.25), - new Rotation3d(0.0, -Math.PI/4, Math.PI)); - // Camera 6 - public static Transform3d robotToCamera6 = - new Transform3d(Units.inchesToMeters(-13.75), Units.inchesToMeters(11.25), Units.inchesToMeters(9.25), - new Rotation3d(0.0, 0, Math.PI)); - - - // Camera 7 - // Front right - public static Transform3d robotToCamera7 = - new Transform3d(Units.inchesToMeters(14.125), Units.inchesToMeters(-8.5), Units.inchesToMeters(7.75), - new Rotation3d(0.0, 0, 0)); - - // Front Left - // Camera 8 - public static Transform3d robotToCamera8 = - new Transform3d(Units.inchesToMeters(14.125), Units.inchesToMeters(11.5), Units.inchesToMeters(8.25), - new Rotation3d(0, Units.degreesToRadians(-10), 0)); - - // Basic filtering thresholds - public static double maxAmbiguity = 0.3; - public static double maxZError = 0.75; - - // Standard deviation baselines, for 1 meter distance and 1 tag - // (Adjusted automatically based on distance and # of tags) - public static double linearStdDevBaseline = 0.02; // Meters - public static double angularStdDevBaseline = 0.06; // Radians - - // Standard deviation multipliers for each camera - // (Adjust to trust some cameras more than others) - public static double[] cameraStdDevFactors = - new double[] { - 1.0, // Camera 5 - 1.0, // Camera 6 - 1.0, // Camera 7 - 1.0 // Camera 8 - }; - - // Multipliers to apply for MegaTag 2 observations - public static double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve - public static double angularStdDevMegatag2Factor = - Double.POSITIVE_INFINITY; // Ignore rotation data -} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java deleted file mode 100644 index b1e70f6..0000000 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ /dev/null @@ -1,52 +0,0 @@ -// Code modified from AdvantageKit Vision Template (v4.0.1): -// https://github.com/Mechanical-Advantage/AdvantageKit/releases -// -// Copyright 2021-2025 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package frc.robot.subsystems.vision; - -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import org.littletonrobotics.junction.AutoLog; - -public interface VisionIO { - @AutoLog - public static class VisionIOInputs { - public boolean connected = false; - public TargetObservation latestTargetObservation = - new TargetObservation(new Rotation2d(), new Rotation2d()); - public PoseObservation[] poseObservations = new PoseObservation[0]; - public int[] tagIds = new int[0]; - } - - /** Represents the angle to a simple target, not used for pose estimation. */ - public static record TargetObservation(Rotation2d tx, Rotation2d ty) {} - - /** Represents a robot pose sample used for pose estimation. */ - public static record PoseObservation( - double timestamp, - Pose3d pose, - double ambiguity, - int tagCount, - double averageTagDistance, - PoseObservationType type) {} - - public static enum PoseObservationType { - MEGATAG_1, - MEGATAG_2, - PHOTONVISION - } - - public default void updateInputs(VisionIOInputs inputs) {} -} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java deleted file mode 100644 index a09d102..0000000 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java +++ /dev/null @@ -1,159 +0,0 @@ -// Code modified from AdvantageKit Vision Template (v4.0.1): -// https://github.com/Mechanical-Advantage/AdvantageKit/releases -// -// Copyright 2021-2025 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package frc.robot.subsystems.vision; - -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.networktables.DoubleArrayPublisher; -import edu.wpi.first.networktables.DoubleArraySubscriber; -import edu.wpi.first.networktables.DoubleSubscriber; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.RobotController; -import java.util.HashSet; -import java.util.LinkedList; -import java.util.List; -import java.util.Set; -import java.util.function.Supplier; - -/** IO implementation for real Limelight hardware. */ -public class VisionIOLimelight implements VisionIO { - private final Supplier rotationSupplier; - private final DoubleArrayPublisher orientationPublisher; - - private final DoubleSubscriber latencySubscriber; - private final DoubleSubscriber txSubscriber; - private final DoubleSubscriber tySubscriber; - private final DoubleArraySubscriber megatag1Subscriber; - private final DoubleArraySubscriber megatag2Subscriber; - - /** - * Creates a new VisionIOLimelight. - * - * @param name The configured name of the Limelight. - * @param rotationSupplier Supplier for the current estimated rotation, used for MegaTag 2. - */ - public VisionIOLimelight(String name, Supplier rotationSupplier) { - var table = NetworkTableInstance.getDefault().getTable(name); - this.rotationSupplier = rotationSupplier; - orientationPublisher = table.getDoubleArrayTopic("robot_orientation_set").publish(); - latencySubscriber = table.getDoubleTopic("tl").subscribe(0.0); - txSubscriber = table.getDoubleTopic("tx").subscribe(0.0); - tySubscriber = table.getDoubleTopic("ty").subscribe(0.0); - megatag1Subscriber = table.getDoubleArrayTopic("botpose_wpiblue").subscribe(new double[] {}); - megatag2Subscriber = - table.getDoubleArrayTopic("botpose_orb_wpiblue").subscribe(new double[] {}); - } - - @Override - public void updateInputs(VisionIOInputs inputs) { - // Update connection status based on whether an update has been seen in the last 250ms - inputs.connected = - ((RobotController.getFPGATime() - latencySubscriber.getLastChange()) / 1000) < 250; - - // Update target observation - inputs.latestTargetObservation = - new TargetObservation( - Rotation2d.fromDegrees(txSubscriber.get()), Rotation2d.fromDegrees(tySubscriber.get())); - - // Update orientation for MegaTag 2 - orientationPublisher.accept( - new double[] {rotationSupplier.get().getDegrees(), 0.0, 0.0, 0.0, 0.0, 0.0}); - NetworkTableInstance.getDefault() - .flush(); // Increases network traffic but recommended by Limelight - - // Read new pose observations from NetworkTables - Set tagIds = new HashSet<>(); - List poseObservations = new LinkedList<>(); - for (var rawSample : megatag1Subscriber.readQueue()) { - if (rawSample.value.length == 0) continue; - for (int i = 11; i < rawSample.value.length; i += 7) { - tagIds.add((int) rawSample.value[i]); - } - poseObservations.add( - new PoseObservation( - // Timestamp, based on server timestamp of publish and latency - rawSample.timestamp * 1.0e-6 - rawSample.value[6] * 1.0e-3, - - // 3D pose estimate - parsePose(rawSample.value), - - // Ambiguity, using only the first tag because ambiguity isn't applicable for multitag - rawSample.value.length >= 18 ? rawSample.value[17] : 0.0, - - // Tag count - (int) rawSample.value[7], - - // Average tag distance - rawSample.value[9], - - // Observation type - PoseObservationType.MEGATAG_1)); - } - for (var rawSample : megatag2Subscriber.readQueue()) { - if (rawSample.value.length == 0) continue; - for (int i = 11; i < rawSample.value.length; i += 7) { - tagIds.add((int) rawSample.value[i]); - } - poseObservations.add( - new PoseObservation( - // Timestamp, based on server timestamp of publish and latency - rawSample.timestamp * 1.0e-6 - rawSample.value[6] * 1.0e-3, - - // 3D pose estimate - parsePose(rawSample.value), - - // Ambiguity, zeroed because the pose is already disambiguated - 0.0, - - // Tag count - (int) rawSample.value[7], - - // Average tag distance - rawSample.value[9], - - // Observation type - PoseObservationType.MEGATAG_2)); - } - - // Save pose observations to inputs object - inputs.poseObservations = new PoseObservation[poseObservations.size()]; - for (int i = 0; i < poseObservations.size(); i++) { - inputs.poseObservations[i] = poseObservations.get(i); - } - - // Save tag IDs to inputs objects - inputs.tagIds = new int[tagIds.size()]; - int i = 0; - for (int id : tagIds) { - inputs.tagIds[i++] = id; - } - } - - /** Parses the 3D pose from a Limelight botpose array. */ - private static Pose3d parsePose(double[] rawLLArray) { - return new Pose3d( - rawLLArray[0], - rawLLArray[1], - rawLLArray[2], - new Rotation3d( - Units.degreesToRadians(rawLLArray[3]), - Units.degreesToRadians(rawLLArray[4]), - Units.degreesToRadians(rawLLArray[5]))); - } -} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java deleted file mode 100644 index 94a8d53..0000000 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ /dev/null @@ -1,149 +0,0 @@ -// Code modified from AdvantageKit Vision Template (v4.0.1): -// https://github.com/Mechanical-Advantage/AdvantageKit/releases -// -// Copyright 2021-2025 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package frc.robot.subsystems.vision; - -import static frc.robot.subsystems.vision.VisionConstants.*; - -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform3d; -import java.util.HashSet; -import java.util.LinkedList; -import java.util.List; -import java.util.Set; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; - -/** IO implementation for real PhotonVision hardware. */ -public class VisionIOPhotonVision implements VisionIO { - protected final PhotonCamera camera; - protected final Transform3d robotToCamera; - - /** - * Creates a new VisionIOPhotonVision. - * - * @param name The configured name of the camera. - * @param rotationSupplier The 3D position of the camera relative to the robot. - */ - public VisionIOPhotonVision(String name, Transform3d robotToCamera) { - camera = new PhotonCamera(name); - this.robotToCamera = robotToCamera; - } - - // NEW COMMENT - // SECTION 2 - @Override - public void updateInputs(VisionIOInputs inputs) { - - inputs.connected = camera.isConnected(); - - // Read new camera observations - Set tagIds = new HashSet<>(); - List poseObservations = new LinkedList<>(); - - for (var result : camera.getAllUnreadResults()) { - // Update latest target observation - if (result.hasTargets()) { - - inputs.latestTargetObservation = - new TargetObservation( - Rotation2d.fromDegrees(result.getBestTarget().getYaw()), - Rotation2d.fromDegrees(result.getBestTarget().getPitch())); - } else { - inputs.latestTargetObservation = new TargetObservation(new Rotation2d(), new Rotation2d()); - } - - // Add pose observation - if (result.multitagResult.isPresent()) { // Multitag result - var multitagResult = result.multitagResult.get(); - - // Calculate robot pose - Transform3d fieldToCamera = multitagResult.estimatedPose.best; - Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse()); - Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation()); - - - // Calculate average tag distance - double totalTagDistance = 0.0; - for (var target : result.targets) { - totalTagDistance += target.bestCameraToTarget.getTranslation().getNorm(); - } - - // Add tag IDs - tagIds.addAll(multitagResult.fiducialIDsUsed); - - // Add observation - poseObservations.add( - new PoseObservation( - result.getTimestampSeconds(), // Timestamp - robotPose, // 3D pose estimate - multitagResult.estimatedPose.ambiguity, // Ambiguity - multitagResult.fiducialIDsUsed.size(), // Tag count - totalTagDistance / result.targets.size(), // Average tag distance - PoseObservationType.PHOTONVISION)); // Observation type - - } else if (!result.targets.isEmpty()) { // Single tag result - var target = result.targets.get(0); - - // Calculate robot pose - var tagPose = aprilTagLayout.getTagPose(target.fiducialId); - //System.out.println("****** " + camera.getName() + " *********************************"); - //System.out.println("tagPose:" + tagPose); - if (tagPose.isPresent()) { - Transform3d fieldToTarget = - new Transform3d(tagPose.get().getTranslation(), tagPose.get().getRotation()); - //System.out.println("fieldToTarget:" + fieldToTarget); - Transform3d cameraToTarget = target.bestCameraToTarget; - //System.out.println("cameraToTarget:" + cameraToTarget); - Transform3d fieldToCamera = fieldToTarget.plus(cameraToTarget.inverse()); - //System.out.println("fieldToCamera:" + fieldToCamera); - Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse()); - //System.out.println("fieldToRobot:" + fieldToRobot); - Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation()); - //System.out.println("robotPose:" + robotPose); - - // Add tag ID - tagIds.add((short) target.fiducialId); - - // Add observation - poseObservations.add( - new PoseObservation( - result.getTimestampSeconds(), // Timestamp - robotPose, // 3D pose estimate - target.poseAmbiguity, // Ambiguity - 1, // Tag count - cameraToTarget.getTranslation().getNorm(), // Average tag distance - PoseObservationType.PHOTONVISION)); // Observation type - } - } - } - - // Save pose observations to inputs object - inputs.poseObservations = new PoseObservation[poseObservations.size()]; - for (int i = 0; i < poseObservations.size(); i++) { - inputs.poseObservations[i] = poseObservations.get(i); - } - - // Save tag IDs to inputs objects - inputs.tagIds = new int[tagIds.size()]; - int i = 0; - for (int id : tagIds) { - inputs.tagIds[i++] = id; - } - } -} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java deleted file mode 100644 index a0fd43c..0000000 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java +++ /dev/null @@ -1,63 +0,0 @@ -// Code modified from AdvantageKit Vision Template (v4.0.1): -// https://github.com/Mechanical-Advantage/AdvantageKit/releases -// -// Copyright 2021-2025 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package frc.robot.subsystems.vision; - -import static frc.robot.subsystems.vision.VisionConstants.aprilTagLayout; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Transform3d; -import java.util.function.Supplier; -import org.photonvision.simulation.PhotonCameraSim; -import org.photonvision.simulation.SimCameraProperties; -import org.photonvision.simulation.VisionSystemSim; - -/** IO implementation for physics sim using PhotonVision simulator. */ -public class VisionIOPhotonVisionSim extends VisionIOPhotonVision { - private static VisionSystemSim visionSim; - - private final Supplier poseSupplier; - private final PhotonCameraSim cameraSim; - - /** - * Creates a new VisionIOPhotonVisionSim. - * - * @param name The name of the camera. - * @param poseSupplier Supplier for the robot pose to use in simulation. - */ - public VisionIOPhotonVisionSim( - String name, Transform3d robotToCamera, Supplier poseSupplier) { - super(name, robotToCamera); - this.poseSupplier = poseSupplier; - - // Initialize vision sim - if (visionSim == null) { - visionSim = new VisionSystemSim("main"); - visionSim.addAprilTags(aprilTagLayout); - } - - // Add sim camera - var cameraProperties = new SimCameraProperties(); - cameraSim = new PhotonCameraSim(camera, cameraProperties); - visionSim.addCamera(cameraSim, robotToCamera); - } - - @Override - public void updateInputs(VisionIOInputs inputs) { - visionSim.update(poseSupplier.get()); - super.updateInputs(inputs); - } -} diff --git a/src/main/java/frc/robot/util/LoggedTunableNumber.java b/src/main/java/frc/robot/util/LoggedTunableNumber.java deleted file mode 100644 index 5574778..0000000 --- a/src/main/java/frc/robot/util/LoggedTunableNumber.java +++ /dev/null @@ -1,133 +0,0 @@ -// Copyright (c) 2023 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file at -// the root directory of this project. - -package frc.robot.util; - -import frc.robot.HardwareConstants; -import java.util.Arrays; -import java.util.HashMap; -import java.util.Map; -import java.util.function.Consumer; -import java.util.function.DoubleSupplier; -import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; - -/** - * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or - * value not in dashboard. - */ -@SuppressWarnings("UnusedVariable") -public class LoggedTunableNumber implements DoubleSupplier { - private static final String tableKey = "TunableNumbers"; - - private final String key; - private boolean hasDefault = false; - private double defaultValue; - private LoggedNetworkNumber dashboardNumber; - private Map lastHasChangedValues = new HashMap<>(); - - /** - * Create a new LoggedTunableNumber - * - * @param dashboardKey Key on dashboard - */ - public LoggedTunableNumber(String dashboardKey) - { - this.key = tableKey + "/" + dashboardKey; - } - - /** - * Create a new LoggedTunableNumber with the default value - * - * @param dashboardKey Key on dashboard - * @param defaultValue Default value - */ - public LoggedTunableNumber(String dashboardKey, double defaultValue) - { - this(dashboardKey); - initDefault(defaultValue); - } - - /** - * Set the default value of the number. The default value can only be set once. - * - * @param defaultValue The default value - */ - public void initDefault(double defaultValue) - { - if (!hasDefault) { - hasDefault = true; - this.defaultValue = defaultValue; - if (HardwareConstants.tuningMode) { - dashboardNumber = new LoggedNetworkNumber(key, defaultValue); - } - } - } - - /** - * Get the current value, from dashboard if available and in tuning mode. - * - * @return The current value - */ - public double get() - { - if (!hasDefault) { - return 0.0; - } else { - return HardwareConstants.tuningMode ? dashboardNumber.get() : defaultValue; - } - } - - /** - * Checks whether the number has changed since our last check - * - * @param id Unique identifier for the caller to avoid conflicts when shared between multiple - * objects. Recommended approach is to pass the result of "hashCode()" - * @return True if the number has changed since the last time this method was called, false - * otherwise. - */ - public boolean hasChanged(int id) - { - double currentValue = get(); - Double lastValue = lastHasChangedValues.get(id); - if (lastValue == null || currentValue != lastValue) { - lastHasChangedValues.put(id, currentValue); - return true; - } - - return false; - } - - /** - * Runs action if any of the tunableNumbers have changed - * - * @param id Unique identifier for the caller to avoid conflicts when shared between multiple * - * objects. Recommended approach is to pass the result of "hashCode()" - * @param action Callback to run when any of the tunable numbers have changed. Access tunable - * numbers in order inputted in method - * @param tunableNumbers All tunable numbers to check - */ - public static void ifChanged( - int id, Consumer action, LoggedTunableNumber... tunableNumbers) - { - if (Arrays.stream(tunableNumbers).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) { - action.accept( - Arrays.stream(tunableNumbers).mapToDouble(LoggedTunableNumber::get).toArray()); - } - } - - /** Runs action if any of the tunableNumbers have changed */ - public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) - { - ifChanged(id, values -> action.run(), tunableNumbers); - } - - @Override - public double getAsDouble() - { - return get(); - } -} diff --git a/src/main/java/frc/robot/util/TuneableProfiledPID.java b/src/main/java/frc/robot/util/TuneableProfiledPID.java deleted file mode 100644 index 1972576..0000000 --- a/src/main/java/frc/robot/util/TuneableProfiledPID.java +++ /dev/null @@ -1,51 +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.util; - -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.trajectory.TrapezoidProfile; - -/** Add your docs here. */ -public class TuneableProfiledPID extends ProfiledPIDController { - - // Instance name for tagging log values - String m_name; - - // Tunable numbers - private LoggedTunableNumber m_kP, m_kI, m_kD, m_maxV, m_maxA; - - public TuneableProfiledPID(String name, double kP, double kI, - double kD, double maxV, double maxA) - { - super(kP, kI, kD, new TrapezoidProfile.Constraints(maxV, maxA)); - - m_name = name; - - // Tunable numbers for PID and motion gain constants - m_kP = new LoggedTunableNumber(m_name + "/kP", kP); - m_kI = new LoggedTunableNumber(m_name + "/kI", kI); - m_kD = new LoggedTunableNumber(m_name + "/kD", kD); - - m_maxV = new LoggedTunableNumber(m_name + "/maxV", maxV); - m_maxA = new LoggedTunableNumber(m_name + "/maxA", maxA); - - } - - public void updatePID() - { - // If changed, update controller constants from Tuneable Numbers - if (m_kP.hasChanged(hashCode()) - || m_kI.hasChanged(hashCode()) - || m_kD.hasChanged(hashCode())) { - this.setPID(m_kP.get(), m_kI.get(), m_kD.get()); - } - - if (m_maxV.hasChanged(hashCode()) - || m_maxA.hasChanged(hashCode())) { - this.setConstraints(new TrapezoidProfile.Constraints(m_maxV.get(), m_maxA.get())); - } - } - -} diff --git a/vendordeps/PathplannerLib-2025.2.5.json b/vendordeps/PathplannerLib-2025.2.1.json similarity index 87% rename from vendordeps/PathplannerLib-2025.2.5.json rename to vendordeps/PathplannerLib-2025.2.1.json index b2a7265..71e25f3 100644 --- a/vendordeps/PathplannerLib-2025.2.5.json +++ b/vendordeps/PathplannerLib-2025.2.1.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.2.5.json", + "fileName": "PathplannerLib-2025.2.1.json", "name": "PathplannerLib", - "version": "2025.2.5", + "version": "2025.2.1", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.2.5" + "version": "2025.2.1" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.2.5", + "version": "2025.2.1", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6-25.3.1.json b/vendordeps/Phoenix6-25.2.1.json similarity index 86% rename from vendordeps/Phoenix6-25.3.1.json rename to vendordeps/Phoenix6-25.2.1.json index a216d97..1397da1 100644 --- a/vendordeps/Phoenix6-25.3.1.json +++ b/vendordeps/Phoenix6-25.2.1.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-25.3.1.json", + "fileName": "Phoenix6-25.2.1.json", "name": "CTRE-Phoenix (v6)", - "version": "25.3.1", + "version": "25.2.1", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.3.1" + "version": "25.2.1" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.3.1", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.3.1", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.3.1", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.3.1", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.3.1", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.3.1", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.3.1", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.3.1", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.3.1", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.3.1", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.3.1", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.3.1", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,21 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.3.1", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANdi", - "version": "25.3.1", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -224,7 +210,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.3.1", + "version": "25.2.1", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,7 +226,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.3.1", + "version": "25.2.1", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,7 +242,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.3.1", + "version": "25.2.1", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +258,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.3.1", + "version": "25.2.1", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +274,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.3.1", + "version": "25.2.1", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +290,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.3.1", + "version": "25.2.1", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +306,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.3.1", + "version": "25.2.1", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +322,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.3.1", + "version": "25.2.1", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +338,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.3.1", + "version": "25.2.1", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +354,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.3.1", + "version": "25.2.1", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +370,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.3.1", + "version": "25.2.1", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +386,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.3.1", + "version": "25.2.1", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +402,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.3.1", + "version": "25.2.1", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -428,22 +414,6 @@ "osxuniversal" ], "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANdi", - "version": "25.3.1", - "libName": "CTRE_SimProCANdi", - "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.json b/vendordeps/REVLib-2025.0.1.json similarity index 90% rename from vendordeps/REVLib.json rename to vendordeps/REVLib-2025.0.1.json index ac62be8..c998054 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib-2025.0.1.json @@ -1,7 +1,7 @@ { - "fileName": "REVLib.json", + "fileName": "REVLib-2025.0.1.json", "name": "REVLib", - "version": "2025.0.3", + "version": "2025.0.1", "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2025.0.3" + "version": "2025.0.1" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.3", + "version": "2025.0.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -36,7 +36,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2025.0.3", + "version": "2025.0.1", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -53,7 +53,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.3", + "version": "2025.0.1", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json deleted file mode 100644 index 1219919..0000000 --- a/vendordeps/photonlib.json +++ /dev/null @@ -1,71 +0,0 @@ -{ - "fileName": "photonlib.json", - "name": "photonlib", - "version": "v2025.2.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.2.1", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "cppDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-cpp", - "version": "v2025.2.1", - "libName": "photonlib", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2025.2.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.2.1" - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-java", - "version": "v2025.2.1" - } - ] -} \ No newline at end of file