diff --git a/.Glass/glass.json b/.Glass/glass.json new file mode 100644 index 0000000..0d5ce82 --- /dev/null +++ b/.Glass/glass.json @@ -0,0 +1,45 @@ +{ + "NetworkTables": { + "Retained Values": { + "open": false + }, + "transitory": { + "AdvantageKit": { + "NetworkInputs": { + "Shooter": { + "open": true + }, + "Tuning": { + "Shooter": { + "open": true + }, + "open": true + } + }, + "open": true + }, + "Tuning": { + "Angler": { + "open": true + }, + "open": true + } + }, + "types": { + "/AdvantageKit/RealOutputs/Alerts": "Alerts", + "/AdvantageKit/RealOutputs/PathPlanner": "Alerts", + "/FMSInfo": "FMSInfo", + "/SmartDashboard/Alerts": "Alerts", + "/SmartDashboard/Auto Choices": "String Chooser", + "/SmartDashboard/PathPlanner": "Alerts", + "/SmartDashboard/Scheduler": "Scheduler" + } + }, + "NetworkTables Info": { + "visible": true + }, + "NetworkTables Settings": { + "mode": "Client (NT4)", + "serverTeam": "5431" + } +} diff --git a/.OutlineViewer/outlineviewer.json b/.OutlineViewer/outlineviewer.json new file mode 100644 index 0000000..0967ef4 --- /dev/null +++ b/.OutlineViewer/outlineviewer.json @@ -0,0 +1 @@ +{} diff --git a/elastic-layout.json b/elastic-layout.json new file mode 100644 index 0000000..544b7b4 --- /dev/null +++ b/elastic-layout.json @@ -0,0 +1,3 @@ +{ + +} \ No newline at end of file diff --git a/src/main/deploy/apriltags/andymark/2026-hub.json b/src/main/deploy/apriltags/andymark/2026-hub.json new file mode 100644 index 0000000..1e7c79c --- /dev/null +++ b/src/main/deploy/apriltags/andymark/2026-hub.json @@ -0,0 +1,296 @@ +{ + "tags": [ + { + "ID": 2, + "pose": { + "translation": { + "x": 11.9013986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 3, + "pose": { + "translation": { + "x": 11.2978438, + "y": 4.3769534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 11.2978438, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 11.9013986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": 12.2569986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 9, + "pose": { + "translation": { + "x": 12.5051566, + "y": 3.6657534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 10, + "pose": { + "translation": { + "x": 12.5051566, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 11, + "pose": { + "translation": { + "x": 12.2569986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 18, + "pose": { + "translation": { + "x": 4.6115986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 19, + "pose": { + "translation": { + "x": 5.2151534, + "y": 3.6657534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 20, + "pose": { + "translation": { + "x": 5.2151534, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 21, + "pose": { + "translation": { + "x": 4.6115986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 24, + "pose": { + "translation": { + "x": 4.2559986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 25, + "pose": { + "translation": { + "x": 4.007866, + "y": 4.3769534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 26, + "pose": { + "translation": { + "x": 4.007866, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 27, + "pose": { + "translation": { + "x": 4.2559986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + } + ], + "field": { + "length": 16.518, + "width": 8.043 + } +} \ No newline at end of file diff --git a/src/main/deploy/apriltags/andymark/2026-none.json b/src/main/deploy/apriltags/andymark/2026-none.json new file mode 100644 index 0000000..a558e3b --- /dev/null +++ b/src/main/deploy/apriltags/andymark/2026-none.json @@ -0,0 +1,7 @@ +{ + "tags": [], + "field": { + "length": 16.518, + "width": 8.043 + } +} \ No newline at end of file diff --git a/src/main/deploy/apriltags/andymark/2026-official.json b/src/main/deploy/apriltags/andymark/2026-official.json new file mode 100644 index 0000000..2b732e2 --- /dev/null +++ b/src/main/deploy/apriltags/andymark/2026-official.json @@ -0,0 +1,584 @@ +{ + "tags": [ + { + "ID": 1, + "pose": { + "translation": { + "x": 11.863959, + "y": 7.411491399999999, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 2, + "pose": { + "translation": { + "x": 11.9013986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 3, + "pose": { + "translation": { + "x": 11.2978438, + "y": 4.3769534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 11.2978438, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 11.9013986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 11.863959, + "y": 0.6312154, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": 11.9388636, + "y": 0.6312154, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": 12.2569986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 9, + "pose": { + "translation": { + "x": 12.5051566, + "y": 3.6657534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 10, + "pose": { + "translation": { + "x": 12.5051566, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 11, + "pose": { + "translation": { + "x": 12.2569986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 12, + "pose": { + "translation": { + "x": 11.9388636, + "y": 7.411491399999999, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 13, + "pose": { + "translation": { + "x": 16.499332, + "y": 7.391907999999999, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 14, + "pose": { + "translation": { + "x": 16.499332, + "y": 6.960107999999999, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 15, + "pose": { + "translation": { + "x": 16.4989764, + "y": 4.3124882, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 16, + "pose": { + "translation": { + "x": 16.4989764, + "y": 3.8806881999999994, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 17, + "pose": { + "translation": { + "x": 4.6490636, + "y": 0.6312154, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 18, + "pose": { + "translation": { + "x": 4.6115986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 19, + "pose": { + "translation": { + "x": 5.2151534, + "y": 3.6657534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 20, + "pose": { + "translation": { + "x": 5.2151534, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 21, + "pose": { + "translation": { + "x": 4.6115986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 22, + "pose": { + "translation": { + "x": 4.6490636, + "y": 7.411491399999999, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 23, + "pose": { + "translation": { + "x": 4.574159, + "y": 7.411491399999999, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 24, + "pose": { + "translation": { + "x": 4.2559986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 25, + "pose": { + "translation": { + "x": 4.007866, + "y": 4.3769534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 26, + "pose": { + "translation": { + "x": 4.007866, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 27, + "pose": { + "translation": { + "x": 4.2559986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 28, + "pose": { + "translation": { + "x": 4.574159, + "y": 0.6312154, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 29, + "pose": { + "translation": { + "x": 0.0136906, + "y": 0.6507734, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 30, + "pose": { + "translation": { + "x": 0.0136906, + "y": 1.0825734, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 31, + "pose": { + "translation": { + "x": 0.0140462, + "y": 3.7301932, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 32, + "pose": { + "translation": { + "x": 0.0140462, + "y": 4.1619931999999995, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + } + ], + "field": { + "length": 16.518, + "width": 8.043 + } +} \ No newline at end of file diff --git a/src/main/deploy/apriltags/andymark/2026-outpost.json b/src/main/deploy/apriltags/andymark/2026-outpost.json new file mode 100644 index 0000000..e69de29 diff --git a/src/main/deploy/apriltags/andymark/2026-tower.json b/src/main/deploy/apriltags/andymark/2026-tower.json new file mode 100644 index 0000000..37c7f24 --- /dev/null +++ b/src/main/deploy/apriltags/andymark/2026-tower.json @@ -0,0 +1,80 @@ +{ + "tags": [ + { + "ID": 15, + "pose": { + "translation": { + "x": 16.4989764, + "y": 4.3124882, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 16, + "pose": { + "translation": { + "x": 16.4989764, + "y": 3.8806881999999994, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 31, + "pose": { + "translation": { + "x": 0.0140462, + "y": 3.7301932, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 32, + "pose": { + "translation": { + "x": 0.0140462, + "y": 4.1619931999999995, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + } + ], + "field": { + "length": 16.518, + "width": 8.043 + } +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Example Auto.auto b/src/main/deploy/pathplanner/autos/Example Auto.auto deleted file mode 100644 index 70b7ab2..0000000 --- a/src/main/deploy/pathplanner/autos/Example Auto.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Example Path" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/MidDepot.auto b/src/main/deploy/pathplanner/autos/MidDepot.auto new file mode 100644 index 0000000..9aaa1dd --- /dev/null +++ b/src/main/deploy/pathplanner/autos/MidDepot.auto @@ -0,0 +1,50 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "MidToDepotToTopMidShoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 5.0 + } + }, + { + "type": "named", + "data": { + "name": "AutoAlign" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AutoShoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": "AA", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/MidSimple.auto b/src/main/deploy/pathplanner/autos/MidSimple.auto new file mode 100644 index 0000000..2701938 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/MidSimple.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShootClose" + } + }, + { + "type": "path", + "data": { + "pathName": "MidToTower" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] + } + }, + "resetOdom": true, + "folder": "AA", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/NearOutpostClimb.auto b/src/main/deploy/pathplanner/autos/NearOutpostClimb.auto new file mode 100644 index 0000000..41a206e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/NearOutpostClimb.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "NearToOutpost" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "path", + "data": { + "pathName": "OutpostToMid" + } + }, + { + "type": "named", + "data": { + "name": "ShootClose" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "path", + "data": { + "pathName": "MidToClimbFar" + } + }, + { + "type": "named", + "data": { + "name": "ClimbL1" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/NearOutpostShot.auto b/src/main/deploy/pathplanner/autos/NearOutpostShot.auto new file mode 100644 index 0000000..9a5c8b4 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/NearOutpostShot.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "NearToOutpost" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "path", + "data": { + "pathName": "OutpostToMid" + } + }, + { + "type": "named", + "data": { + "name": "ShootClose" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/StepBackShoot.auto b/src/main/deploy/pathplanner/autos/StepBackShoot.auto new file mode 100644 index 0000000..3e74f4b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/StepBackShoot.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TakingStepBack" + } + }, + { + "type": "named", + "data": { + "name": "deployIntake" + } + }, + { + "type": "named", + "data": { + "name": "ShootClose" + } + } + ] + } + }, + "resetOdom": true, + "folder": "AA", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TopDepot.auto b/src/main/deploy/pathplanner/autos/TopDepot.auto new file mode 100644 index 0000000..b7755c4 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/TopDepot.auto @@ -0,0 +1,50 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TopTrenchToDepotToTopMidShoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 5.0 + } + }, + { + "type": "named", + "data": { + "name": "AutoAlign" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AutoShoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": "AA", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/MidToClimbFar.path similarity index 57% rename from src/main/deploy/pathplanner/paths/Example Path.path rename to src/main/deploy/pathplanner/paths/MidToClimbFar.path index 303dbb2..dfa4fdc 100644 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/MidToClimbFar.path @@ -3,41 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.0, - "y": 7.0 + "x": 2.3453777777777782, + "y": 4.054488888888889 }, "prevControl": null, "nextControl": { - "x": 3.013282910175676, - "y": 6.5274984191074985 + "x": 2.5986948697796577, + "y": 4.073707657710675 }, "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.166769560390973, - "y": 5.0964860911203305 - }, - "prevControl": { - "x": 4.166769560390973, - "y": 6.0964860911203305 - }, - "nextControl": { - "x": 6.166769560390973, - "y": 4.0964860911203305 - }, - "isLocked": false, - "linkedName": null + "linkedName": "Mid" }, { "anchor": { - "x": 7.0, - "y": 1.0 + "x": 1.2929777777753984, + "y": 4.298099999996573 }, "prevControl": { - "x": 6.75, - "y": 2.5 + "x": 1.0456042819636244, + "y": 4.347729686023186 }, "nextControl": null, "isLocked": false, @@ -49,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 1.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -58,7 +42,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": -0.30476191052866086 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/MidToDepotToTopMidShoot.path b/src/main/deploy/pathplanner/paths/MidToDepotToTopMidShoot.path new file mode 100644 index 0000000..0d56d34 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/MidToDepotToTopMidShoot.path @@ -0,0 +1,106 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5473894436519258, + "y": 4.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.2923395149786017, + "y": 5.775249643366619 + }, + "isLocked": false, + "linkedName": "MidStart" + }, + { + "anchor": { + "x": 0.533, + "y": 5.051 + }, + "prevControl": { + "x": 0.533, + "y": 4.768482149020954 + }, + "nextControl": { + "x": 0.533, + "y": 5.333517850979046 + }, + "isLocked": false, + "linkedName": "DepotBottom" + }, + { + "anchor": { + "x": 0.533, + "y": 6.5 + }, + "prevControl": { + "x": 0.533, + "y": 5.599291374986944 + }, + "nextControl": { + "x": 0.533, + "y": 7.400708625013055 + }, + "isLocked": false, + "linkedName": "DepotTop" + }, + { + "anchor": { + "x": 2.3958487874465044, + "y": 5.684679029957205 + }, + "prevControl": { + "x": 2.2077046137697844, + "y": 5.849305181924334 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "TopMidShoot" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4, + "rotationDegrees": -16.06402391526286 + }, + { + "waypointRelativePos": 1.0, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "deployIntake", + "waypointRelativePos": 0.16377171215882175, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 270.0, + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 91.23197740263973 + }, + "reversed": false, + "folder": "AA", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/MidToTower.path b/src/main/deploy/pathplanner/paths/MidToTower.path new file mode 100644 index 0000000..741f28a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/MidToTower.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5473894436519258, + "y": 4.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0139148641319236, + "y": 4.0 + }, + "isLocked": false, + "linkedName": "MidStart" + }, + { + "anchor": { + "x": 1.8912410841654772, + "y": 3.795634807417975 + }, + "prevControl": { + "x": 2.3379528555486515, + "y": 3.795634807417975 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "TowerShoot" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "deployIntake", + "waypointRelativePos": 0.3349088453747421, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": null + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 270.0, + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "AA", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/NearToOutpost.path b/src/main/deploy/pathplanner/paths/NearToOutpost.path new file mode 100644 index 0000000..2e61868 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/NearToOutpost.path @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.767346647646219, + "y": 0.6903566333808839 + }, + "prevControl": null, + "nextControl": { + "x": 3.162229180205553, + "y": 0.690356633380884 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.4291726105563479, + "y": 0.6903566333808839 + }, + "prevControl": { + "x": 1.2172227385084622, + "y": 0.6903566333808838 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Outpost" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "shoot", + "waypointRelativePos": 0.5, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/OutpostToMid.path b/src/main/deploy/pathplanner/paths/OutpostToMid.path new file mode 100644 index 0000000..7fa2eab --- /dev/null +++ b/src/main/deploy/pathplanner/paths/OutpostToMid.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.4291726105563479, + "y": 0.6903566333808839 + }, + "prevControl": null, + "nextControl": { + "x": 1.429172610556348, + "y": 0.6903566333808842 + }, + "isLocked": false, + "linkedName": "Outpost" + }, + { + "anchor": { + "x": 2.3453777777777782, + "y": 4.054488888888889 + }, + "prevControl": { + "x": 1.3453777777777782, + "y": 4.054488888888889 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Mid" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 270.0, + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TakingStepBack.path b/src/main/deploy/pathplanner/paths/TakingStepBack.path new file mode 100644 index 0000000..891c3c8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TakingStepBack.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.1, + "y": 4.015511111111111 + }, + "prevControl": null, + "nextControl": { + "x": 4.596725420737161, + "y": 4.0100318002396955 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6028673323823104, + "y": 4.028530670470756 + }, + "prevControl": { + "x": 2.3528831255653007, + "y": 4.031340713543461 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 270.0, + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "AA", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TopTrenchToDepotToTopMidShoot.path b/src/main/deploy/pathplanner/paths/TopTrenchToDepotToTopMidShoot.path new file mode 100644 index 0000000..bdeb460 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TopTrenchToDepotToTopMidShoot.path @@ -0,0 +1,102 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.4075222222222226, + "y": 5.545388888888888 + }, + "prevControl": null, + "nextControl": { + "x": 2.939788888888889, + "y": 4.814555555555555 + }, + "isLocked": false, + "linkedName": "TopTrenchStart" + }, + { + "anchor": { + "x": 0.533, + "y": 5.051 + }, + "prevControl": { + "x": 0.533, + "y": 4.762340810322713 + }, + "nextControl": { + "x": 0.533, + "y": 5.339659189677287 + }, + "isLocked": false, + "linkedName": "DepotBottom" + }, + { + "anchor": { + "x": 0.533, + "y": 6.5 + }, + "prevControl": { + "x": 0.533, + "y": 6.096986380542398 + }, + "nextControl": { + "x": 0.533, + "y": 6.903013619457602 + }, + "isLocked": false, + "linkedName": "DepotTop" + }, + { + "anchor": { + "x": 2.3958487874465044, + "y": 5.684679029957205 + }, + "prevControl": { + "x": 1.3958487874465044, + "y": 5.684679029957205 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "TopMidShoot" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.6597510373443868, + "rotationDegrees": -20.0 + }, + { + "waypointRelativePos": 1, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "deployIntake", + "waypointRelativePos": 0.36029776674937175, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 270.0, + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 91.23197740263973 + }, + "reversed": false, + "folder": "AA", + "idealStartingState": { + "velocity": 0, + "rotation": -51.170175095029535 + }, + "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 f981b63..810f1e7 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -2,20 +2,38 @@ "robotWidth": 0.9, "robotLength": 0.9, "holonomicMode": true, - "pathFolders": [], - "autoFolders": [], + "pathFolders": [ + "AA" + ], + "autoFolders": [ + "AA" + ], "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, - "defaultMaxAngVel": 540.0, - "defaultMaxAngAccel": 720.0, - "robotMass": 74.088, + "defaultMaxAngVel": 270.0, + "defaultMaxAngAccel": 360.0, + "defaultNominalVoltage": 12.0, + "robotMass": 60.0, "robotMOI": 6.883, - "robotWheelbase": 0.546, "robotTrackwidth": 0.546, "driveWheelRadius": 0.048, "driveGearing": 5.143, "maxDriveSpeed": 5.45, "driveMotorType": "krakenX60FOC", "driveCurrentLimit": 60.0, - "wheelCOF": 1.2 -} + "wheelCOF": 1.2, + "flModuleX": 0.273, + "flModuleY": 0.273, + "frModuleX": 0.273, + "frModuleY": -0.273, + "blModuleX": -0.273, + "blModuleY": 0.273, + "brModuleX": -0.273, + "brModuleY": -0.273, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [ + "{\"name\":\"Rectangle\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.6,\"y\":0.0},\"size\":{\"width\":0.9,\"length\":0.3},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}", + "{\"name\":\"Line\",\"type\":\"line\",\"data\":{\"start\":{\"x\":-0.3,\"y\":0.206375},\"end\":{\"x\":-0.15,\"y\":0.206},\"strokeWidth\":0.05}}" + ] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 16a2bd6..9a4b404 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -20,7 +20,8 @@ public final class Constants { public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode; // CAN bus that the devices are located on; - public static final CANBus CANBUS = new CANBus("canivore", "./logs/example.hoot"); + public static final CANBus CANIVORE_CANBUS = new CANBus("Canivore", "./logs/example.hoot"); + public static final CANBus RIO_CANBUS = new CANBus(); public static enum Mode { /** Running on a real robot. */ diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java new file mode 100644 index 0000000..b5fa919 --- /dev/null +++ b/src/main/java/frc/robot/FieldConstants.java @@ -0,0 +1,384 @@ +// Copyright (c) 2025-2026 Littleton Robotics +// 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 com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Filesystem; +import java.io.IOException; +import java.nio.file.Path; +import lombok.Getter; +import lombok.RequiredArgsConstructor; + +/** + * Contains information for location of field element and other useful reference points. + * + *

NOTE: All constants are defined relative to the field coordinate system, and from the + * perspective of the blue alliance station + */ +public class FieldConstants { + public static final FieldType fieldType = FieldType.ANDYMARK; + + // AprilTag related constants + public static final int aprilTagCount = AprilTagLayoutType.OFFICIAL.getLayout().getTags().size(); + public static final double aprilTagWidth = Units.inchesToMeters(6.5); + public static final AprilTagLayoutType defaultAprilTagType = AprilTagLayoutType.OFFICIAL; + + // Field dimensions + public static final double fieldLength = AprilTagLayoutType.OFFICIAL.getLayout().getFieldLength(); + public static final double fieldWidth = AprilTagLayoutType.OFFICIAL.getLayout().getFieldWidth(); + + // Fuel dimensions + public static final double fuelDiameter = Units.inchesToMeters(5.91); + + /** + * Officially defined and relevant vertical lines found on the field (defined by X-axis offset) + */ + public static class LinesVertical { + public static final double center = fieldLength / 2.0; + public static final double starting = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX(); + public static final double allianceZone = starting; + public static final double hubCenter = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX() + Hub.width / 2.0; + public static final double neutralZoneNear = center - Units.inchesToMeters(120); + public static final double neutralZoneFar = center + Units.inchesToMeters(120); + public static final double oppHubCenter = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(4).get().getX() + Hub.width / 2.0; + public static final double oppAllianceZone = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(10).get().getX(); + } + + /** + * Officially defined and relevant horizontal lines found on the field (defined by Y-axis offset) + * + *

NOTE: The field element start and end are always left to right from the perspective of the + * alliance station + */ + public static class LinesHorizontal { + + public static final double center = fieldWidth / 2.0; + + // Right of hub + public static final double rightBumpStart = Hub.nearRightCorner.getY(); + public static final double rightBumpEnd = rightBumpStart - RightBump.width; + public static final double rightBumpMiddle = (rightBumpStart + rightBumpEnd) / 2.0; + public static final double rightTrenchOpenStart = rightBumpEnd - Units.inchesToMeters(12.0); + public static final double rightTrenchOpenEnd = 0; + + // Left of hub + public static final double leftBumpEnd = Hub.nearLeftCorner.getY(); + public static final double leftBumpStart = leftBumpEnd + LeftBump.width; + public static final double leftBumpMiddle = (leftBumpStart + leftBumpEnd) / 2.0; + public static final double leftTrenchOpenEnd = leftBumpStart + Units.inchesToMeters(12.0); + public static final double leftTrenchOpenStart = fieldWidth; + } + + /** Hub related constants */ + public static class Hub { + + // Dimensions + public static final double width = Units.inchesToMeters(47.0); + public static final double height = + Units.inchesToMeters(72.0); // includes the catcher at the top + public static final double innerWidth = Units.inchesToMeters(41.7); + public static final double innerHeight = Units.inchesToMeters(56.5); + + // Relevant reference points on alliance side + public static final Translation3d topCenterPoint = + new Translation3d( + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX() + width / 2.0, + fieldWidth / 2.0, + height); + public static final Translation3d innerCenterPoint = + new Translation3d( + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX() + width / 2.0, + fieldWidth / 2.0, + innerHeight); + + public static final Translation2d nearLeftCorner = + new Translation2d(topCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d nearRightCorner = + new Translation2d(topCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 - width / 2.0); + public static final Translation2d farLeftCorner = + new Translation2d(topCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d farRightCorner = + new Translation2d(topCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 - width / 2.0); + + // Relevant reference points on the opposite side + public static final Translation3d oppTopCenterPoint = + new Translation3d( + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(4).get().getX() + width / 2.0, + fieldWidth / 2.0, + height); + public static final Translation2d oppNearLeftCorner = + new Translation2d(oppTopCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d oppNearRightCorner = + new Translation2d(oppTopCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 - width / 2.0); + public static final Translation2d oppFarLeftCorner = + new Translation2d(oppTopCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d oppFarRightCorner = + new Translation2d(oppTopCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 - width / 2.0); + + // Hub faces + public static final Pose2d nearFace = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().toPose2d(); + public static final Pose2d farFace = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(20).get().toPose2d(); + public static final Pose2d rightFace = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(18).get().toPose2d(); + public static final Pose2d leftFace = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(21).get().toPose2d(); + } + + /** Left Bump related constants */ + public static class LeftBump { + + // Dimensions + public static final double width = Units.inchesToMeters(73.0); + public static final double height = Units.inchesToMeters(6.513); + public static final double depth = Units.inchesToMeters(44.4); + + // Relevant reference points on alliance side + public static final Translation2d nearLeftCorner = + new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); + public static final Translation2d nearRightCorner = Hub.nearLeftCorner; + public static final Translation2d farLeftCorner = + new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); + public static final Translation2d farRightCorner = Hub.farLeftCorner; + + // Relevant reference points on opposing side + public static final Translation2d oppNearLeftCorner = + new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); + public static final Translation2d oppNearRightCorner = Hub.oppNearLeftCorner; + public static final Translation2d oppFarLeftCorner = + new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); + public static final Translation2d oppFarRightCorner = Hub.oppFarLeftCorner; + } + + /** Right Bump related constants */ + public static class RightBump { + // Dimensions + public static final double width = Units.inchesToMeters(73.0); + public static final double height = Units.inchesToMeters(6.513); + public static final double depth = Units.inchesToMeters(44.4); + + // Relevant reference points on alliance side + public static final Translation2d nearLeftCorner = + new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); + public static final Translation2d nearRightCorner = Hub.nearLeftCorner; + public static final Translation2d farLeftCorner = + new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); + public static final Translation2d farRightCorner = Hub.farLeftCorner; + + // Relevant reference points on opposing side + public static final Translation2d oppNearLeftCorner = + new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); + public static final Translation2d oppNearRightCorner = Hub.oppNearLeftCorner; + public static final Translation2d oppFarLeftCorner = + new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); + public static final Translation2d oppFarRightCorner = Hub.oppFarLeftCorner; + } + + /** Left Trench related constants */ + public static class LeftTrench { + // Dimensions + public static final double width = Units.inchesToMeters(65.65); + public static final double depth = Units.inchesToMeters(47.0); + public static final double height = Units.inchesToMeters(40.25); + public static final double openingWidth = Units.inchesToMeters(50.34); + public static final double openingHeight = Units.inchesToMeters(22.25); + + // Relevant reference points on alliance side + public static final Translation3d openingTopLeft = + new Translation3d(LinesVertical.hubCenter, fieldWidth, openingHeight); + public static final Translation3d openingTopRight = + new Translation3d(LinesVertical.hubCenter, fieldWidth - openingWidth, openingHeight); + + // Relevant reference points on opposing side + public static final Translation3d oppOpeningTopLeft = + new Translation3d(LinesVertical.oppHubCenter, fieldWidth, openingHeight); + public static final Translation3d oppOpeningTopRight = + new Translation3d(LinesVertical.oppHubCenter, fieldWidth - openingWidth, openingHeight); + } + + public static class RightTrench { + + // Dimensions + public static final double width = Units.inchesToMeters(65.65); + public static final double depth = Units.inchesToMeters(47.0); + public static final double height = Units.inchesToMeters(40.25); + public static final double openingWidth = Units.inchesToMeters(50.34); + public static final double openingHeight = Units.inchesToMeters(22.25); + + // Relevant reference points on alliance side + public static final Translation3d openingTopLeft = + new Translation3d(LinesVertical.hubCenter, openingWidth, openingHeight); + public static final Translation3d openingTopRight = + new Translation3d(LinesVertical.hubCenter, 0, openingHeight); + + // Relevant reference points on opposing side + public static final Translation3d oppOpeningTopLeft = + new Translation3d(LinesVertical.oppHubCenter, openingWidth, openingHeight); + public static final Translation3d oppOpeningTopRight = + new Translation3d(LinesVertical.oppHubCenter, 0, openingHeight); + } + + /** Tower related constants */ + public static class Tower { + // Dimensions + public static final double width = Units.inchesToMeters(49.25); + public static final double depth = Units.inchesToMeters(45.0); + public static final double height = Units.inchesToMeters(78.25); + public static final double innerOpeningWidth = Units.inchesToMeters(32.250); + public static final double frontFaceX = Units.inchesToMeters(43.51); + + public static final double uprightHeight = Units.inchesToMeters(72.1); + + // Rung heights from the floor + public static final double lowRungHeight = Units.inchesToMeters(27.0); + public static final double midRungHeight = Units.inchesToMeters(45.0); + public static final double highRungHeight = Units.inchesToMeters(63.0); + + // Relevant reference points on alliance side + public static final Translation2d centerPoint = + new Translation2d( + frontFaceX, AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(31).get().getY()); + public static final Translation2d leftUpright = + new Translation2d( + frontFaceX, + (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(31).get().getY()) + + innerOpeningWidth / 2 + + Units.inchesToMeters(0.75)); + public static final Translation2d rightUpright = + new Translation2d( + frontFaceX, + (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(31).get().getY()) + - innerOpeningWidth / 2 + - Units.inchesToMeters(0.75)); + + // Relevant reference points on opposing side + public static final Translation2d oppCenterPoint = + new Translation2d( + fieldLength - frontFaceX, + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(15).get().getY()); + public static final Translation2d oppLeftUpright = + new Translation2d( + fieldLength - frontFaceX, + (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(15).get().getY()) + + innerOpeningWidth / 2 + + Units.inchesToMeters(0.75)); + public static final Translation2d oppRightUpright = + new Translation2d( + fieldLength - frontFaceX, + (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(15).get().getY()) + - innerOpeningWidth / 2 + - Units.inchesToMeters(0.75)); + } + + public static class Depot { + // Dimensions + public static final double width = Units.inchesToMeters(42.0); + public static final double depth = Units.inchesToMeters(27.0); + public static final double height = Units.inchesToMeters(1.125); + public static final double distanceFromCenterY = Units.inchesToMeters(75.93); + + // Relevant reference points on alliance side + public static final Translation3d depotCenter = + new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY, height); + public static final Translation3d leftCorner = + new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY + (width / 2), height); + public static final Translation3d rightCorner = + new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY - (width / 2), height); + } + + public static class Outpost { + // Dimensions + public static final double width = Units.inchesToMeters(31.8); + public static final double openingDistanceFromFloor = Units.inchesToMeters(28.1); + public static final double height = Units.inchesToMeters(7.0); + + // Relevant reference points on alliance side + public static final Translation2d centerPoint = + new Translation2d(0, AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(29).get().getY()); + } + + public static class FuelPool { + // Dimensions + public static final double width = Units.inchesToMeters(181.9); + public static final double depth = Units.inchesToMeters(71.9); + + // Relevant reference points on alliance side + public static final Translation2d nearLeftCorner = + new Translation2d(fieldLength / 2.0 - depth / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d nearRightCorner = + new Translation2d(fieldLength / 2.0 - depth / 2.0, fieldWidth / 2.0 - width / 2.0); + public static final Translation2d leftCenter = + new Translation2d(fieldLength / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d rightCenter = + new Translation2d(fieldLength / 2.0, fieldWidth / 2.0 - width / 2.0); + } + + @RequiredArgsConstructor + public enum FieldType { + HQ("welded"), + ANDYMARK("andymark"), + WELDED("welded"); + + @Getter private final String jsonFolder; + } + + public enum AprilTagLayoutType { + OFFICIAL("2026-official"), + NONE("2026-none"), + HUB("2026-hub"), + OUTPOST("2026-outpost"), + TOWER("2026-tower"); + + private final String name; + private volatile AprilTagFieldLayout layout; + private volatile String layoutString; + + AprilTagLayoutType(String name) { + this.name = name; + } + + public AprilTagFieldLayout getLayout() { + if (layout == null) { + synchronized (this) { + if (layout == null) { + try { + Path p = + Path.of( + Filesystem.getDeployDirectory().getPath(), + "apriltags", + fieldType.getJsonFolder(), + name + ".json"); + layout = new AprilTagFieldLayout(p); + layoutString = new ObjectMapper().writeValueAsString(layout); + } catch (IOException e) { + throw new RuntimeException(e); + } + } + } + } + return layout; + } + + public String getLayoutString() { + if (layoutString == null) { + getLayout(); + } + return layoutString; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 935ea9d..802cf41 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -9,6 +9,8 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Commands; + import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; @@ -104,7 +106,11 @@ public void autonomousInit() { // schedule the autonomous command (example) if (autonomousCommand != null) { - CommandScheduler.getInstance().schedule(autonomousCommand); + CommandScheduler.getInstance().schedule( + Commands.sequence( + robotContainer.getHomingCommand(), + autonomousCommand.asProxy() + )); } } @@ -122,6 +128,8 @@ public void teleopInit() { if (autonomousCommand != null) { autonomousCommand.cancel(); } + + robotContainer.teleopInit(); } /** This function is called periodically during operator control. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index de14205..2467ced 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,31 +8,47 @@ package frc.robot; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 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.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.commands.AutoShootCommand; import frc.robot.commands.DriveCommands; +import frc.robot.commands.InhaleCommand; +import frc.robot.commands.ShootFuelCommand; +import frc.robot.commands.ShootFuelCommandAuto; +import frc.robot.commands.UnjamCommand; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.climber.Climber; import frc.robot.subsystems.climber.ClimberIO; import frc.robot.subsystems.climber.ClimberIOSim; -import frc.robot.subsystems.climber.ClimberIOSparkFlex; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.GyroIO; import frc.robot.subsystems.drive.GyroIOPigeon2; import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; +import frc.robot.subsystems.feeder.Feeder; +import frc.robot.subsystems.feeder.FeederIO; +import frc.robot.subsystems.feeder.FeederIOSim; +import frc.robot.subsystems.feeder.FeederIOSparkFlex; +import frc.robot.subsystems.feeder.FeederConstants.FeederModes; import frc.robot.subsystems.hopper.Carpet; import frc.robot.subsystems.hopper.CarpetIO; import frc.robot.subsystems.hopper.CarpetIOSim; import frc.robot.subsystems.hopper.CarpetIOSparkFlex; +import frc.robot.subsystems.hopper.CarpetConstants.CarpetModes; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeConstants.IntakeMode; import frc.robot.subsystems.intake.pivot.PivotIO; @@ -42,14 +58,24 @@ import frc.robot.subsystems.intake.roller.RollerIOSim; import frc.robot.subsystems.intake.roller.RollerIOSparkFlex; import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.ShooterConstants.ShooterModes; import frc.robot.subsystems.shooter.angler.AnglerIO; import frc.robot.subsystems.shooter.angler.AnglerIOSim; import frc.robot.subsystems.shooter.angler.AnglerIOTalonFX; import frc.robot.subsystems.shooter.flywheel.FlywheelIO; import frc.robot.subsystems.shooter.flywheel.FlywheelIOSim; import frc.robot.subsystems.shooter.flywheel.FlywheelIOTalonFX; +import frc.robot.subsystems.vision.Vision; +import frc.robot.subsystems.vision.VisionIO; +import frc.robot.subsystems.vision.VisionIOLimelight; +import frc.robot.util.AllianceFlipUtil; import frc.team5431.titan.core.joysticks.CommandXboxController; +import static edu.wpi.first.units.Units.Inches; + +import java.util.Set; + +import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** @@ -66,14 +92,17 @@ public class RobotContainer { private final Shooter shooter; private final Carpet carpet; private final Climber climber; + private final Feeder feeder; + private final Vision vision; // Controller - private final CommandXboxController driver = new CommandXboxController(0); - private final CommandXboxController operator = new CommandXboxController(1); + private final CommandXboxController controller = new CommandXboxController(0); // Dashboard inputs private final LoggedDashboardChooser autoChooser; + + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { switch (Constants.currentMode) { @@ -96,10 +125,12 @@ public RobotContainer() { // new VisionIOLimelight(camera0Name, drive::getRotation), // new VisionIOLimelight(camera1Name, drive::getRotation)); - intake = new Intake(new RollerIOSim(), new PivotIOSim()); - shooter = new Shooter(new AnglerIOSim(), new FlywheelIOSim()); - carpet = new Carpet(new CarpetIOSim()); + intake = new Intake(new RollerIOSparkFlex(), new PivotIOSparkFlex()); + shooter = new Shooter(new AnglerIOSim(), new FlywheelIOTalonFX()); + carpet = new Carpet(new CarpetIOSparkFlex()); climber = new Climber(new ClimberIOSim()); + feeder = new Feeder(new FeederIOSparkFlex()); + vision = new Vision(drive::addVisionMeasurement, new VisionIOLimelight("limelight", drive::getRotation)); // vision = // new Vision( // demoDrive::addVisionMeasurement, @@ -148,6 +179,8 @@ public RobotContainer() { new Carpet(new CarpetIOSim()); climber = new Climber(new ClimberIOSim()); + feeder = new Feeder(new FeederIOSim()); + vision = new Vision(drive::addVisionMeasurement, new VisionIO() {}); break; default: // Replayed robot, disable IO implementations @@ -164,9 +197,12 @@ public RobotContainer() { shooter = new Shooter(new AnglerIO() {}, new FlywheelIO() {}); carpet = new Carpet(new CarpetIO() {}); climber = new Climber(new ClimberIO() {}); + feeder = new Feeder(new FeederIO() {}); + vision = new Vision(drive::addVisionMeasurement, new VisionIO() {}); break; } + registerCommands(); // Set up auto routines autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); @@ -188,11 +224,9 @@ public RobotContainer() { "Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)); configureDriverBindings(); - configureOperatorBindings(); - SmartDashboard.putData("Scheduler", CommandScheduler.getInstance()); - + RobotController.setBrownoutVoltage(6); } /** @@ -202,37 +236,92 @@ public RobotContainer() { * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureDriverBindings() { - // Default command, normal field-relative drive + // Default Commands drive.setDefaultCommand( DriveCommands.joystickDrive( drive, - () -> -driver.getLeftY(), - () -> -driver.getLeftX(), - () -> -driver.getRightX())); + () -> -controller.getLeftY() * 0.7, + () -> -controller.getLeftX() * 0.7, + () -> -controller.getRightX())); + + intake.setDefaultCommand(intake.stop()); + carpet.setDefaultCommand(carpet.runCarpetCommand(CarpetModes.IDLE)); + feeder.setDefaultCommand(feeder.runFeederCommand(FeederModes.IDLE)); + shooter.setDefaultCommand(shooter.stop()); + // shooter.setDefaultCommand(); // // Lock to 0° when A button is held - // driver + // controller // .a() // .whileTrue( // DriveCommands.joystickDriveAtAngle( // drive, - // () -> -driver.getLeftY(), - // () -> -driver.getLeftX(), + + // () -> -controller.getLeftY(), + // () -> -controller.getLeftX(), // () -> Rotation2d.kZero)); // // Switch to X pattern when X button is pressed // driver.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); // Reset gyro to 0° when B button is pressed - driver - .b() - .onTrue( - Commands.runOnce( - () -> - drive.setPose( - new Pose2d(drive.getPose().getTranslation(), Rotation2d.kZero)), - drive) - .ignoringDisable(true)); + // controller + // .y() + // .onTrue( + // Commands.runOnce( + // () -> + // drive.setPose( + // new Pose2d(drive.getPose().getTranslation(), Rotation2d.kZero)), + // drive) + // .ignoringDisable(true)); + // controller.y().whileTrue( + // DriveCommands.joystickDriveAtAngle(drive, () -> -controller.getLeftY(), () -> -controller.getLeftX(), this::getAngleToGameElement) + // ); + //TODO: ready to test + controller.y().whileTrue( + new ParallelCommandGroup( + DriveCommands.joystickDriveAtAngle(drive, () -> -controller.getLeftY(), () -> -controller.getLeftX(), () -> getTranslationToGameElement().getAngle()), + new ShootFuelCommandAuto(feeder, shooter, () -> getTranslationToGameElement().getNorm()) + ) + ); + + controller.x().whileTrue(new ShootFuelCommand(feeder, shooter, ShooterModes.SHOOT_CLOSE)); + controller.a().whileTrue(new ShootFuelCommandAuto(feeder, shooter, () -> getTranslationToGameElement().getNorm())); + + // controller.a().whileTrue(Commands.defer(() -> { + // return new AutoShootCommand(intake, carpet, feeder, shooter, drive); + // }, Set.of(intake, carpet, feeder, shooter))); + // controller.b().onTrue(shooter.runAngler(ShooterModes.SHOOT_FAR)); + // controller.y().onTrue(shooter.runAngler(ShooterModes.IDLE)); + controller.rightTrigger().whileTrue(new InhaleCommand(intake, carpet, true)); // TODO: run magic carpet, also when pivot is out, doesn't run if pivot is in + controller.leftTrigger().whileTrue(new InhaleCommand(intake, carpet, false)); + // controller.rightBumper().whileTrue(intake.runIntakeCommand(IntakeMode.INTAKE)); + controller.leftBumper().whileTrue(intake.runIntakeCommand(IntakeMode.OUTTAKE)); + + // controller.x().whileTrue(new ShootFuelCommand(intake, carpet, feeder, shooter)); + // controller.b().whileTrue(shooter.runAngler(ShooterModes.SHOOT_FAR)); + // controller.b().whileTrue(intake.runPivotVoltageCommand(-1)); + // controller.povUp().whileTrue(intake.runPivotVoltageCommand(-5) + // controller.x().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE)); + // controller.a().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_FAR)); + + + controller.povUp().whileTrue(intake.runPivotVoltageCommand(-3)); + controller.povDown().whileTrue(intake.runPivotVoltageCommand(3)); //positive means down + + + // controller.povRight().onTrue(shooter.runAngler(ShooterModes.SHOOT_FAR)); + // controller.povLeft().onTrue(shooter.runAngler(ShooterModes.IDLE)); + controller.povRight().whileTrue(shooter.tune()); + controller.rightBumper().whileTrue(feeder.runFeederCommand(FeederModes.FEEDER)); + + controller.start().whileTrue(new UnjamCommand(feeder, shooter)); + + // controller.x().whileTrue(shooter.tune()); + + + // upclimb is left dpad, downclimb is right dpad + // // Auto aim command example; code from AKit template. // @SuppressWarnings("resource") @@ -251,14 +340,30 @@ private void configureDriverBindings() { // drive)); } - private void configureOperatorBindings() { - // Default Commands - intake.setDefaultCommand(intake.runIntakeeCommand(IntakeMode.OUT_IDLE)); + private void registerCommands() { + NamedCommands.registerCommand("ShootClose", Commands.parallel( + new InhaleCommand(intake, carpet, true), + new ShootFuelCommand(feeder, shooter, ShooterModes.SHOOT_CLOSE) + ).withTimeout(10)); + // NamedCommands.registerCommand("DeployIntake"); + // NamedCommands.registerCommand("ShootCloseJer", ); + NamedCommands.registerCommand("ShootFar", Commands.parallel( + new InhaleCommand(intake, carpet, true), + shooter.runShooterCommand(ShooterModes.SHOOT_FAR) + ).withTimeout(10)); + + + // NamedCommands.registerCommand("AutoShoot", ); + NamedCommands.registerCommand("AutoShoot", Commands.defer( + () -> {return new AutoShootCommand(intake, carpet, feeder, shooter, drive);}, Set.of(intake, carpet, feeder, shooter))); + + NamedCommands.registerCommand("AutoAlign", DriveCommands.joystickDriveAtAngle(drive, () -> 0, () -> 0, () -> getTranslationToGameElement().getAngle())); + NamedCommands.registerCommand("Intake", intake.runIntakeCommand(IntakeMode.INTAKE)); + NamedCommands.registerCommand("deployIntake", intake.runPivotVoltageCommand(3).withTimeout(0.5)); - operator.a().whileTrue(intake.runIntakeeCommand(IntakeMode.INTAKE)); - operator.b().whileTrue(intake.runIntakeeCommand(IntakeMode.OUTTAKE)); + NamedCommands.registerCommand("RevShooterClose", shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE).withTimeout(0.5)); } /** * Use this to pass the autonomous command to the main {@link Robot} class. @@ -268,4 +373,28 @@ private void configureOperatorBindings() { public Command getAutonomousCommand() { return autoChooser.get(); } + + public void teleopInit() { + if (!shooter.isZeroed()) { + CommandScheduler.getInstance().schedule(shooter.homing()); + } + } + + public Translation2d getTranslationToGameElement() { + Translation2d hubPose = FieldConstants.Hub.innerCenterPoint.toTranslation2d(); + Translation2d hubPoseAdj = AllianceFlipUtil.apply(hubPose); + + Pose2d robotPose = drive.getPose(); + Transform2d shooterTransform = new Transform2d(Inches.of(-10.824), Inches.of(8.125), Rotation2d.kZero); + Pose2d shooterPose = robotPose.transformBy(shooterTransform); + + Translation2d diff = hubPoseAdj.minus(shooterPose.getTranslation()); + Logger.recordOutput("/Measurements/DistToHUb", diff.getNorm()); + Logger.recordOutput("/Measurements/AngleToHub", diff.getAngle()); + return diff; + } + + public Command getHomingCommand() { + return shooter.homing(); + } } diff --git a/src/main/java/frc/robot/commands/AutoShootCommand.java b/src/main/java/frc/robot/commands/AutoShootCommand.java new file mode 100644 index 0000000..acf52ae --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoShootCommand.java @@ -0,0 +1,70 @@ +package frc.robot.commands; + +import java.util.function.Supplier; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.feeder.Feeder; +import frc.robot.subsystems.feeder.FeederConstants.FeederModes; +import frc.robot.subsystems.hopper.Carpet; +import frc.robot.subsystems.hopper.CarpetConstants.CarpetModes; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.intake.IntakeConstants.IntakeMode; +import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.ShooterMath; +import frc.robot.subsystems.shooter.ShooterConstants.ShooterAnglerConstants; + +public class AutoShootCommand extends Command { + + private final Intake intake; + private final Carpet carpet; + private final Feeder feeder; + private final Shooter shooter; + private final Supplier distToHub; + private double cyclesAtTarget; + private boolean speedAchieved; + + public AutoShootCommand(Intake intake, Carpet carpet, Feeder feeder, Shooter shooter, Drive drive) { + // addCommands( + // // new ParallelRaceGroup( + // // feeder.runFeederCommand(FeederModes.REVERSE), + // // new WaitCommand(0.5)), + // new ParallelCommandGroup( + // new InhaleCommand(intake, carpet, feeder,true, true).withName("ShootFuelCommand.Inhale")), + // shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE).withName("ShootFuelCommand.Shoot") + // ); + + addRequirements(intake, carpet, feeder, shooter); + + this.intake = intake; + this.carpet = carpet; + this.feeder = feeder; + this.shooter = shooter; + distToHub = drive::distFromHub; + } + + @Override + public void execute() { + double desiredRPM = ShooterMath.calculateSpeed(distToHub.get()); + double desiredPos = ShooterMath.calculateHoodPosition(distToHub.get()); + + shooter.runShooterCustom(desiredRPM, desiredPos); + + if (!MathUtil.isNear(desiredPos, shooter.getPosition(), ShooterAnglerConstants.tolerance)) { + cyclesAtTarget = 0; + } else { + cyclesAtTarget++; + } + + if (MathUtil.isNear(desiredRPM, shooter.getSpeed(), desiredRPM * 0.05)) { + speedAchieved = true; + } + + if (cyclesAtTarget > 5 && speedAchieved) { + intake.runIntakeEnum(IntakeMode.INTAKE); + carpet.runRollerEnum(CarpetModes.INTAKE); + feeder.runFeederEnum(FeederModes.FEEDER); + } + } +} diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index a749f60..1611ab6 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -30,11 +30,13 @@ import java.util.function.DoubleSupplier; import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; + public class DriveCommands { private static final double DEADBAND = 0.1; private static final double ANGLE_KP = 5.0; - private static final double ANGLE_KD = 0.4; - private static final double ANGLE_MAX_VELOCITY = 8.0; + private static final double ANGLE_KD = 0.5; + private static final double ANGLE_MAX_VELOCITY = 15.0; private static final double ANGLE_MAX_ACCELERATION = 20.0; private static final double FF_START_DELAY = 2.0; // Secs private static final double FF_RAMP_RATE = 0.1; // Volts/Sec @@ -128,6 +130,9 @@ public static Command joystickDriveAtAngle( angleController.calculate( drive.getRotation().getRadians(), rotationSupplier.get().getRadians()); + Logger.recordOutput("RotationTuning/curr", drive.getRotation().getRadians()); + Logger.recordOutput("RotationTuning/desired", rotationSupplier.get().getRadians()); + // Convert to field relative speeds & send command ChassisSpeeds speeds = new ChassisSpeeds( diff --git a/src/main/java/frc/robot/commands/InhaleCommand.java b/src/main/java/frc/robot/commands/InhaleCommand.java new file mode 100644 index 0000000..b7a7551 --- /dev/null +++ b/src/main/java/frc/robot/commands/InhaleCommand.java @@ -0,0 +1,32 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import frc.robot.subsystems.hopper.Carpet; +import frc.robot.subsystems.hopper.CarpetConstants.CarpetModes; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.intake.IntakeConstants.IntakeMode; + + +public class InhaleCommand extends ParallelCommandGroup { + + + public InhaleCommand(Intake intake, Carpet carpet, boolean isInhaling) { + + + if (isInhaling) { + addCommands( + intake.runIntakeCommand(IntakeMode.INTAKE), + carpet.runCarpetCommand(CarpetModes.INTAKE) + ); + } + + else { + addCommands( + intake.runIntakeCommand(IntakeMode.OUTTAKE), + carpet.runCarpetCommand(CarpetModes.OUTTAKE) + ); + } + addRequirements(intake, carpet); + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ShootFuelCommand.java b/src/main/java/frc/robot/commands/ShootFuelCommand.java new file mode 100644 index 0000000..84a9fd5 --- /dev/null +++ b/src/main/java/frc/robot/commands/ShootFuelCommand.java @@ -0,0 +1,27 @@ +package frc.robot.commands; + + + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.subsystems.feeder.Feeder; +import frc.robot.subsystems.feeder.FeederConstants.FeederModes; +import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.ShooterConstants.ShooterModes; + +public class ShootFuelCommand extends SequentialCommandGroup { + public ShootFuelCommand(Feeder feeder, Shooter shooter, ShooterModes shooterModes) { + addCommands( + shooter.runShooterCommand(shooterModes).withName("ShootFuelCommand.Shoot"). + until(() -> MathUtil.isNear(shooterModes.speed.magnitude(), shooter.getSpeed(), shooterModes.speed.magnitude() * 0.10)).withTimeout(2), + new ParallelCommandGroup( + feeder.runFeederCommand(FeederModes.FEEDER).withName("FeederCommand.Feed"), + shooter.runShooterCommand(shooterModes).withName("ShootFuelCommand.Shoot") + )); + + addRequirements(feeder, shooter); + } + + //TODO: once shooter hits rpm run feeder +} diff --git a/src/main/java/frc/robot/commands/ShootFuelCommandAuto.java b/src/main/java/frc/robot/commands/ShootFuelCommandAuto.java new file mode 100644 index 0000000..ae7aba1 --- /dev/null +++ b/src/main/java/frc/robot/commands/ShootFuelCommandAuto.java @@ -0,0 +1,27 @@ +package frc.robot.commands; + + + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.subsystems.feeder.Feeder; +import frc.robot.subsystems.feeder.FeederConstants.FeederModes; +import frc.robot.subsystems.shooter.Shooter; + +public class ShootFuelCommandAuto extends SequentialCommandGroup { + public ShootFuelCommandAuto(Feeder feeder, Shooter shooter, DoubleSupplier dist) { + addCommands( + shooter.runShootAuto(dist).withName("ShootFuelCommand.ShootAuto"). + until(shooter.atSetpoint()).withTimeout(2), + new ParallelCommandGroup( + feeder.runFeederCommand(FeederModes.FEEDER).withName("FeederCommand.Feed"), + shooter.runShootAuto(dist).withName("ShootFuelCommand.ShootAuto") + )); + + addRequirements(feeder, shooter); + } + + //TODO: once shooter hits rpm run feeder +} diff --git a/src/main/java/frc/robot/commands/UnjamCommand.java b/src/main/java/frc/robot/commands/UnjamCommand.java new file mode 100644 index 0000000..7188640 --- /dev/null +++ b/src/main/java/frc/robot/commands/UnjamCommand.java @@ -0,0 +1,16 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import frc.robot.subsystems.feeder.Feeder; +import frc.robot.subsystems.feeder.FeederConstants.FeederModes; +import frc.robot.subsystems.shooter.Shooter; + +public class UnjamCommand extends ParallelCommandGroup { + public UnjamCommand(Feeder feeder, Shooter shooter){ + addCommands( + feeder.runFeederCommand(FeederModes.REVERSE), + shooter.runShooterVoltageCommand(-4) + ); + addRequirements(feeder, shooter); + } +} diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index bd1daa8..c6a6879 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -14,6 +14,7 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.*; + // Generated by the 2026 Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class TunerConstants { @@ -28,8 +29,8 @@ public class TunerConstants { // 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(0.1).withKI(0).withKD(0) - .withKS(0).withKV(0.124); + .withKP(1).withKI(0).withKD(0) + .withKS(0).withKV(0.65); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors @@ -53,7 +54,12 @@ public class TunerConstants { // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withSupplyCurrentLimit(Amps.of(65)) + .withStatorCurrentLimit(Amps.of(90)) + ); private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() @@ -85,7 +91,7 @@ public class TunerConstants { private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; - private static final int kPigeonId = 49; + private static final int kPigeonId = 2; // These are only used for simulation private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); @@ -124,45 +130,45 @@ public class TunerConstants { // Front Left - private static final int kFrontLeftDriveMotorId = 1; - private static final int kFrontLeftSteerMotorId = 2; - private static final int kFrontLeftEncoderId = 9; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.301513671875); - private static final boolean kFrontLeftSteerMotorInverted = false; - private static final boolean kFrontLeftEncoderInverted = true; + private static final int kFrontLeftDriveMotorId = 11; + private static final int kFrontLeftSteerMotorId = 12; + private static final int kFrontLeftEncoderId = 13; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.30029296875); + private static final boolean kFrontLeftSteerMotorInverted = true; + private static final boolean kFrontLeftEncoderInverted = false; private static final Distance kFrontLeftXPos = Inches.of(11); private static final Distance kFrontLeftYPos = Inches.of(11.5); // Front Right - private static final int kFrontRightDriveMotorId = 4; - private static final int kFrontRightSteerMotorId = 3; - private static final int kFrontRightEncoderId = 10; - private static final Angle kFrontRightEncoderOffset = Rotations.of(0.318603515625); - private static final boolean kFrontRightSteerMotorInverted = false; - private static final boolean kFrontRightEncoderInverted = true; + private static final int kFrontRightDriveMotorId = 41; + private static final int kFrontRightSteerMotorId = 42; + private static final int kFrontRightEncoderId = 43; + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.320068359375); + private static final boolean kFrontRightSteerMotorInverted = true; + private static final boolean kFrontRightEncoderInverted = false; private static final Distance kFrontRightXPos = Inches.of(11); private static final Distance kFrontRightYPos = Inches.of(-11.5); // Back Left - private static final int kBackLeftDriveMotorId = 7; - private static final int kBackLeftSteerMotorId = 8; - private static final int kBackLeftEncoderId = 12; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.181396484375); - private static final boolean kBackLeftSteerMotorInverted = false; - private static final boolean kBackLeftEncoderInverted = true; + private static final int kBackLeftDriveMotorId = 21; + private static final int kBackLeftSteerMotorId = 22; + private static final int kBackLeftEncoderId = 23; + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.181884765625); + private static final boolean kBackLeftSteerMotorInverted = true; + private static final boolean kBackLeftEncoderInverted = false; private static final Distance kBackLeftXPos = Inches.of(-11); private static final Distance kBackLeftYPos = Inches.of(11.5); // Back Right - private static final int kBackRightDriveMotorId = 5; - private static final int kBackRightSteerMotorId = 6; - private static final int kBackRightEncoderId = 11; - private static final Angle kBackRightEncoderOffset = Rotations.of(-0.458984375); - private static final boolean kBackRightSteerMotorInverted = false; - private static final boolean kBackRightEncoderInverted = true; + private static final int kBackRightDriveMotorId = 31; + private static final int kBackRightSteerMotorId = 32; + private static final int kBackRightEncoderId = 33; + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.4580078125); + private static final boolean kBackRightSteerMotorInverted = true; + private static final boolean kBackRightEncoderInverted = false; private static final Distance kBackRightXPos = Inches.of(-11); private static final Distance kBackRightYPos = Inches.of(-11.5); @@ -189,10 +195,10 @@ public class TunerConstants { kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted ); - // /** - // * Creates a CommandSwerveDrivetrain instance. - // * This should only be called once in your robot program,. - // */ + /** + * 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 @@ -260,10 +266,10 @@ public TunerSwerveDrivetrain( * unspecified or set to 0 Hz, this is 250 Hz on * CAN FD, and 100 Hz on CAN 2.0. * @param odometryStandardDeviation The standard deviation for odometry calculation - * in the form [x, y, theta]áµ€, with units in meters + * in the form [x, y, theta]ᵀ, with units in meters * and radians * @param visionStandardDeviation The standard deviation for vision calculation - * in the form [x, y, theta]áµ€, with units in meters + * in the form [x, y, theta]ᵀ, with units in meters * and radians * @param modules Constants for each specific module */ diff --git a/src/main/java/frc/robot/generated/TunerConstantsTrash.java b/src/main/java/frc/robot/generated/TunerConstantsTrash.java new file mode 100644 index 0000000..12f568d --- /dev/null +++ b/src/main/java/frc/robot/generated/TunerConstantsTrash.java @@ -0,0 +1,570 @@ +// package frc.robot.generated; + +// import static edu.wpi.first.units.Units.*; + +// import com.ctre.phoenix6.CANBus; +// import com.ctre.phoenix6.configs.*; +// import com.ctre.phoenix6.hardware.*; +// import com.ctre.phoenix6.signals.*; +// import com.ctre.phoenix6.swerve.*; +// import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; + +// import edu.wpi.first.math.Matrix; +// import edu.wpi.first.math.numbers.N1; +// import edu.wpi.first.math.numbers.N3; +// import edu.wpi.first.units.measure.*; + +// // Generated by the 2026 Tuner X Swerve Project Generator +// // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html +// public class TunerConstantsTrash { +// // Both sets of gains need to be tuned to your individual robot. + +// // 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(100).withKI(0).withKD(0.5) +// .withKS(0.1).withKV(2.33).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(0.1).withKI(0).withKD(0) +// .withKS(0).withKV(0.124); + +// // The closed-loop output type to use for the steer motors; +// // This affects the PID/FF gains for the steer motors +// private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; +// // The closed-loop output type to use for the drive motors; +// // This affects the PID/FF gains for the drive motors +// private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + +// // The type of motor used for the drive motor +// private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; +// // The type of motor used for the drive motor +// private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; + +// // The remote sensor feedback type to use for the steer motors; +// // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* +// private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + +// // The stator current at which the wheels start to slip; +// // This needs to be tuned to your individual robot +// private static final Current kSlipCurrent = Amps.of(120); + +// // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. +// // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. +// private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); +// private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() +// .withCurrentLimits( +// new CurrentLimitsConfigs() +// // Swerve azimuth does not require much torque output, so we can set a relatively low +// // stator current limit to help avoid brownouts without impacting performance. +// .withStatorCurrentLimit(Amps.of(60)) +// .withStatorCurrentLimitEnable(true) +// ); +// private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); +// // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs +// private static final Pigeon2Configuration pigeonConfigs = null; + +// // CAN bus that the devices are located on; +// // All swerve devices must share the same CAN bus +// public static final CANBus kCANBus = new CANBus("Canivore", "./logs/example.hoot"); + +// // 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.23); + +// // 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.125; + +// private static final double kDriveGearRatio = 5.902777777777778; +// private static final double kSteerGearRatio = 18.75; +// private static final Distance kWheelRadius = Inches.of(2); + +// private static final boolean kInvertLeftSide = false; +// private static final boolean kInvertRightSide = true; + +// private static final int kPigeonId = 30; + +// // These are only used for simulation +// private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); +// private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); +// // Simulated voltage necessary to overcome friction +// private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); +// private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + +// public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() +// .withCANBusName(kCANBus.getName()) +// .withPigeon2Id(kPigeonId) +// .withPigeon2Configs(pigeonConfigs); + +// private static final SwerveModuleConstantsFactory ConstantCreator = +// new SwerveModuleConstantsFactory() +// .withDriveMotorGearRatio(kDriveGearRatio) +// .withSteerMotorGearRatio(kSteerGearRatio) +// .withCouplingGearRatio(kCoupleRatio) +// .withWheelRadius(kWheelRadius) +// .withSteerMotorGains(steerGains) +// .withDriveMotorGains(driveGains) +// .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) +// .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) +// .withSlipCurrent(kSlipCurrent) +// .withSpeedAt12Volts(kSpeedAt12Volts) +// .withDriveMotorType(kDriveMotorType) +// .withSteerMotorType(kSteerMotorType) +// .withFeedbackSource(kSteerFeedbackType) +// .withDriveMotorInitialConfigs(driveInitialConfigs) +// .withSteerMotorInitialConfigs(steerInitialConfigs) +// .withEncoderInitialConfigs(encoderInitialConfigs) +// .withSteerInertia(kSteerInertia) +// .withDriveInertia(kDriveInertia) +// .withSteerFrictionVoltage(kSteerFrictionVoltage) +// .withDriveFrictionVoltage(kDriveFrictionVoltage); + + +// // Front Left +// private static final int kFrontLeftDriveMotorId = 1; +// private static final int kFrontLeftSteerMotorId = 2; +// private static final int kFrontLeftEncoderId = 9; +// private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.298095703125); +// private static final boolean kFrontLeftSteerMotorInverted = false; +// private static final boolean kFrontLeftEncoderInverted = true; + +// private static final Distance kFrontLeftXPos = Inches.of(11); +// private static final Distance kFrontLeftYPos = Inches.of(11.5); + +// // Front Right +// private static final int kFrontRightDriveMotorId = 4; +// private static final int kFrontRightSteerMotorId = 3; +// private static final int kFrontRightEncoderId = 10; +// private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.320556640625); +// private static final boolean kFrontRightSteerMotorInverted = false; +// private static final boolean kFrontRightEncoderInverted = true; + +// private static final Distance kFrontRightXPos = Inches.of(11); +// private static final Distance kFrontRightYPos = Inches.of(-11.5); + +// // Back Left +// private static final int kBackLeftDriveMotorId = 7; +// private static final int kBackLeftSteerMotorId = 8; +// private static final int kBackLeftEncoderId = 12; +// private static final Angle kBackLeftEncoderOffset = Rotations.of(0.18017578125); +// private static final boolean kBackLeftSteerMotorInverted = false; +// private static final boolean kBackLeftEncoderInverted = true; + +// private static final Distance kBackLeftXPos = Inches.of(-11); +// private static final Distance kBackLeftYPos = Inches.of(11.5); + +// // Back Right +// private static final int kBackRightDriveMotorId = 5; +// private static final int kBackRightSteerMotorId = 6; +// private static final int kBackRightEncoderId = 11; +// private static final Angle kBackRightEncoderOffset = Rotations.of(0.4580078125); +// private static final boolean kBackRightSteerMotorInverted = false; +// private static final boolean kBackRightEncoderInverted = true; + +// private static final Distance kBackRightXPos = Inches.of(-11); +// private static final Distance kBackRightYPos = Inches.of(-11.5); + + +// public static final SwerveModuleConstants FrontLeft = +// ConstantCreator.createModuleConstants( +// kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, +// kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted +// ); +// public static final SwerveModuleConstants FrontRight = +// ConstantCreator.createModuleConstants( +// kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, +// kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted +// ); +// public static final SwerveModuleConstants BackLeft = +// ConstantCreator.createModuleConstants( +// kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, +// kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted +// ); +// public static final SwerveModuleConstants BackRight = +// ConstantCreator.createModuleConstants( +// kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, +// kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted +// ); + +// // /** +// // * 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 +// // ); +// // } + + +// /** +// * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. +// */ +// public static class TunerSwerveDrivetrain extends SwerveDrivetrain { +// /** +// * Constructs a CTRE SwerveDrivetrain using the specified constants. +// *

+// * This constructs the underlying hardware devices, so users should not construct +// * the devices themselves. If they need the devices, they can access them through +// * getters in the classes. +// * +// * @param drivetrainConstants Drivetrain-wide constants for the swerve drive +// * @param modules Constants for each specific module +// */ +// public TunerSwerveDrivetrain( +// SwerveDrivetrainConstants drivetrainConstants, +// SwerveModuleConstants... modules +// ) { +// super( +// TalonFX::new, TalonFX::new, CANcoder::new, +// drivetrainConstants, modules +// ); +// } + +// /** +// * Constructs a CTRE SwerveDrivetrain using the specified constants. +// *

+// * This constructs the underlying hardware devices, so users should not construct +// * the devices themselves. If they need the devices, they can access them through +// * getters in the classes. +// * +// * @param drivetrainConstants Drivetrain-wide constants for the swerve drive +// * @param odometryUpdateFrequency The frequency to run the odometry loop. If +// * unspecified or set to 0 Hz, this is 250 Hz on +// * CAN FD, and 100 Hz on CAN 2.0. +// * @param modules Constants for each specific module +// */ +// public TunerSwerveDrivetrain( +// SwerveDrivetrainConstants drivetrainConstants, +// double odometryUpdateFrequency, +// SwerveModuleConstants... modules +// ) { +// super( +// TalonFX::new, TalonFX::new, CANcoder::new, +// drivetrainConstants, odometryUpdateFrequency, modules +// ); +// } + +// /** +// * Constructs a CTRE SwerveDrivetrain using the specified constants. +// *

+// * This constructs the underlying hardware devices, so users should not construct +// * the devices themselves. If they need the devices, they can access them through +// * getters in the classes. +// * +// * @param drivetrainConstants Drivetrain-wide constants for the swerve drive +// * @param odometryUpdateFrequency The frequency to run the odometry loop. If +// * unspecified or set to 0 Hz, this is 250 Hz on +// * CAN FD, and 100 Hz on CAN 2.0. +// * @param odometryStandardDeviation The standard deviation for odometry calculation +// * in the form [x, y, theta]áµ€, with units in meters +// * and radians +// * @param visionStandardDeviation The standard deviation for vision calculation +// * in the form [x, y, theta]áµ€, with units in meters +// * and radians +// * @param modules Constants for each specific module +// */ +// public TunerSwerveDrivetrain( +// SwerveDrivetrainConstants drivetrainConstants, +// double odometryUpdateFrequency, +// Matrix odometryStandardDeviation, +// Matrix visionStandardDeviation, +// SwerveModuleConstants... modules +// ) { +// super( +// TalonFX::new, TalonFX::new, CANcoder::new, +// drivetrainConstants, odometryUpdateFrequency, +// odometryStandardDeviation, visionStandardDeviation, modules +// ); +// } +// } +// } + +// // package frc.robot.generated; + +// // import static edu.wpi.first.units.Units.*; + +// // import com.ctre.phoenix6.CANBus; +// // import com.ctre.phoenix6.configs.*; +// // import com.ctre.phoenix6.hardware.*; +// // import com.ctre.phoenix6.signals.*; +// // import com.ctre.phoenix6.swerve.*; +// // import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; + +// // import edu.wpi.first.math.Matrix; +// // import edu.wpi.first.math.numbers.N1; +// // import edu.wpi.first.math.numbers.N3; +// // import edu.wpi.first.units.measure.*; + +// // // Generated by the 2026 Tuner X Swerve Project Generator +// // // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html +// // public class TunerConstants { +// // // Both sets of gains need to be tuned to your individual robot. + +// // // 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(100).withKI(0).withKD(0.5) +// // .withKS(0.1).withKV(2.33).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(0.1).withKI(0).withKD(0) +// // .withKS(0).withKV(0.124); + +// // // The closed-loop output type to use for the steer motors; +// // // This affects the PID/FF gains for the steer motors +// // private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; +// // // The closed-loop output type to use for the drive motors; +// // // This affects the PID/FF gains for the drive motors +// // private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + +// // // The type of motor used for the drive motor +// // private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; +// // // The type of motor used for the drive motor +// // private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; + +// // // The remote sensor feedback type to use for the steer motors; +// // // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* +// // private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + +// // // The stator current at which the wheels start to slip; +// // // This needs to be tuned to your individual robot +// // private static final Current kSlipCurrent = Amps.of(120); + +// // // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. +// // // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. +// // private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); +// // private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() +// // .withCurrentLimits( +// // new CurrentLimitsConfigs() +// // // Swerve azimuth does not require much torque output, so we can set a relatively low +// // // stator current limit to help avoid brownouts without impacting performance. +// // .withStatorCurrentLimit(Amps.of(60)) +// // .withStatorCurrentLimitEnable(true) +// // ); +// // private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); +// // // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs +// // private static final Pigeon2Configuration pigeonConfigs = null; + +// // // CAN bus that the devices are located on; +// // // All swerve devices must share the same CAN bus +// // public static final CANBus kCANBus = new CANBus("Canivore", "./logs/example.hoot"); + +// // // 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.23); + +// // // 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.125; + +// // private static final double kDriveGearRatio = 5.902777777777778; +// // private static final double kSteerGearRatio = 18.75; +// // private static final Distance kWheelRadius = Inches.of(2); + +// // private static final boolean kInvertLeftSide = false; +// // private static final boolean kInvertRightSide = true; + +// // private static final int kPigeonId = 49; + +// // // These are only used for simulation +// // private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); +// // private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); +// // // Simulated voltage necessary to overcome friction +// // private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); +// // private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + +// // public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() +// // .withCANBusName(kCANBus.getName()) +// // .withPigeon2Id(kPigeonId) +// // .withPigeon2Configs(pigeonConfigs); + +// // private static final SwerveModuleConstantsFactory ConstantCreator = +// // new SwerveModuleConstantsFactory() +// // .withDriveMotorGearRatio(kDriveGearRatio) +// // .withSteerMotorGearRatio(kSteerGearRatio) +// // .withCouplingGearRatio(kCoupleRatio) +// // .withWheelRadius(kWheelRadius) +// // .withSteerMotorGains(steerGains) +// // .withDriveMotorGains(driveGains) +// // .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) +// // .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) +// // .withSlipCurrent(kSlipCurrent) +// // .withSpeedAt12Volts(kSpeedAt12Volts) +// // .withDriveMotorType(kDriveMotorType) +// // .withSteerMotorType(kSteerMotorType) +// // .withFeedbackSource(kSteerFeedbackType) +// // .withDriveMotorInitialConfigs(driveInitialConfigs) +// // .withSteerMotorInitialConfigs(steerInitialConfigs) +// // .withEncoderInitialConfigs(encoderInitialConfigs) +// // .withSteerInertia(kSteerInertia) +// // .withDriveInertia(kDriveInertia) +// // .withSteerFrictionVoltage(kSteerFrictionVoltage) +// // .withDriveFrictionVoltage(kDriveFrictionVoltage); + + +// // // Front Left +// // private static final int kFrontLeftDriveMotorId = 1; +// // private static final int kFrontLeftSteerMotorId = 2; +// // private static final int kFrontLeftEncoderId = 9; +// // private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.304199); +// // private static final boolean kFrontLeftSteerMotorInverted = false; +// // private static final boolean kFrontLeftEncoderInverted = true; + +// // private static final Distance kFrontLeftXPos = Inches.of(11); +// // private static final Distance kFrontLeftYPos = Inches.of(11.5); + +// // // Front Right +// // private static final int kFrontRightDriveMotorId = 4; +// // private static final int kFrontRightSteerMotorId = 3; +// // private static final int kFrontRightEncoderId = 10; +// // private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.365234); +// // private static final boolean kFrontRightSteerMotorInverted = false; +// // private static final boolean kFrontRightEncoderInverted = true; + +// // private static final Distance kFrontRightXPos = Inches.of(11); +// // private static final Distance kFrontRightYPos = Inches.of(-11.5); + +// // // Back Left +// // private static final int kBackLeftDriveMotorId = 7; +// // private static final int kBackLeftSteerMotorId = 8; +// // private static final int kBackLeftEncoderId = 12; +// // private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.363037); +// // private static final boolean kBackLeftSteerMotorInverted = false; +// // private static final boolean kBackLeftEncoderInverted = true; + +// // private static final Distance kBackLeftXPos = Inches.of(-11); +// // private static final Distance kBackLeftYPos = Inches.of(11.5); + +// // // Back Right +// // private static final int kBackRightDriveMotorId = 5; +// // private static final int kBackRightSteerMotorId = 6; +// // private static final int kBackRightEncoderId = 11; +// // private static final Angle kBackRightEncoderOffset = Rotations.of(-0.082275); +// // private static final boolean kBackRightSteerMotorInverted = false; +// // private static final boolean kBackRightEncoderInverted = true; + +// // private static final Distance kBackRightXPos = Inches.of(-11); +// // private static final Distance kBackRightYPos = Inches.of(-11.5); + + +// // public static final SwerveModuleConstants FrontLeft = +// // ConstantCreator.createModuleConstants( +// // kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, +// // kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted +// // ); +// // public static final SwerveModuleConstants FrontRight = +// // ConstantCreator.createModuleConstants( +// // kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, +// // kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted +// // ); +// // public static final SwerveModuleConstants BackLeft = +// // ConstantCreator.createModuleConstants( +// // kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, +// // kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted +// // ); +// // public static final SwerveModuleConstants BackRight = +// // ConstantCreator.createModuleConstants( +// // kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, +// // kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted +// // ); + +// // // /** +// // // * 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 +// // // ); +// // // } + + +// // /** +// // * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. +// // */ +// // public static class TunerSwerveDrivetrain extends SwerveDrivetrain { +// // /** +// // * Constructs a CTRE SwerveDrivetrain using the specified constants. +// // *

+// // * This constructs the underlying hardware devices, so users should not construct +// // * the devices themselves. If they need the devices, they can access them through +// // * getters in the classes. +// // * +// // * @param drivetrainConstants Drivetrain-wide constants for the swerve drive +// // * @param modules Constants for each specific module +// // */ +// // public TunerSwerveDrivetrain( +// // SwerveDrivetrainConstants drivetrainConstants, +// // SwerveModuleConstants... modules +// // ) { +// // super( +// // TalonFX::new, TalonFX::new, CANcoder::new, +// // drivetrainConstants, modules +// // ); +// // } + +// // /** +// // * Constructs a CTRE SwerveDrivetrain using the specified constants. +// // *

+// // * This constructs the underlying hardware devices, so users should not construct +// // * the devices themselves. If they need the devices, they can access them through +// // * getters in the classes. +// // * +// // * @param drivetrainConstants Drivetrain-wide constants for the swerve drive +// // * @param odometryUpdateFrequency The frequency to run the odometry loop. If +// // * unspecified or set to 0 Hz, this is 250 Hz on +// // * CAN FD, and 100 Hz on CAN 2.0. +// // * @param modules Constants for each specific module +// // */ +// // public TunerSwerveDrivetrain( +// // SwerveDrivetrainConstants drivetrainConstants, +// // double odometryUpdateFrequency, +// // SwerveModuleConstants... modules +// // ) { +// // super( +// // TalonFX::new, TalonFX::new, CANcoder::new, +// // drivetrainConstants, odometryUpdateFrequency, modules +// // ); +// // } + +// // /** +// // * Constructs a CTRE SwerveDrivetrain using the specified constants. +// // *

+// // * This constructs the underlying hardware devices, so users should not construct +// // * the devices themselves. If they need the devices, they can access them through +// // * getters in the classes. +// // * +// // * @param drivetrainConstants Drivetrain-wide constants for the swerve drive +// // * @param odometryUpdateFrequency The frequency to run the odometry loop. If +// // * unspecified or set to 0 Hz, this is 250 Hz on +// // * CAN FD, and 100 Hz on CAN 2.0. +// // * @param odometryStandardDeviation The standard deviation for odometry calculation +// // * in the form [x, y, theta]áµ€, with units in meters +// // * and radians +// // * @param visionStandardDeviation The standard deviation for vision calculation +// // * in the form [x, y, theta]áµ€, with units in meters +// // * and radians +// // * @param modules Constants for each specific module +// // */ +// // public TunerSwerveDrivetrain( +// // SwerveDrivetrainConstants drivetrainConstants, +// // double odometryUpdateFrequency, +// // Matrix odometryStandardDeviation, +// // Matrix visionStandardDeviation, +// // SwerveModuleConstants... modules +// // ) { +// // super( +// // TalonFX::new, TalonFX::new, CANcoder::new, +// // drivetrainConstants, odometryUpdateFrequency, +// // odometryStandardDeviation, visionStandardDeviation, modules +// // ); +// // } +// // } +// // } + diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index ea6267c..7a5c411 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -1,3 +1,4 @@ + package frc.robot.subsystems.climber; import org.littletonrobotics.junction.Logger; @@ -28,7 +29,7 @@ public void periodic() { public void runClimberEnum(ClimberModes climberMode) { this.mode = climberMode; - climberIO.setClimberPosition(mode.positionAngle.baseUnitMagnitude()); + climberIO.setClimberPosition(mode.positionAngle.baseUnitMagnitude()); } public Command runCarpetCommand(ClimberModes climberModes) { diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java index 266e503..cee4500 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java @@ -3,3 +3,4 @@ public class ClimberIOSim implements ClimberIO { } + diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkFlex.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkFlex.java index 2de40a0..230fe6f 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkFlex.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkFlex.java @@ -5,6 +5,7 @@ import com.revrobotics.ResetMode; import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkFlex; import static frc.robot.util.SparkUtil.*; @@ -12,7 +13,7 @@ import frc.team5431.titan.core.subsystem.REVMechanism; public class ClimberIOSparkFlex implements ClimberIO { - private final SparkFlex sparkFlex = new SparkFlex(ClimberConstants.id, null); + private final SparkFlex sparkFlex = new SparkFlex(ClimberConstants.id, MotorType.kBrushless); private final RelativeEncoder encoder = sparkFlex.getEncoder(); public static class PivotSparkFlexConfig extends REVMechanism.Config { diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java index 1dd4b97..a58dd88 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java @@ -15,11 +15,11 @@ import frc.team5431.titan.core.subsystem.CTREMechanism; public class ClimberIOTalonFX implements ClimberIO { - private final TalonFX talon = new TalonFX(ClimberConstants.id, Constants.CANBUS); + private final TalonFX talon = new TalonFX(ClimberConstants.id, Constants.CANIVORE_CANBUS); public static class ClimberTalonFXConfig extends CTREMechanism.Config { public ClimberTalonFXConfig() { - super("ClimberTalonFX", Constants.CANBUS); + super("ClimberTalonFX", Constants.CANIVORE_CANBUS); configPIDGains(ClimberConstants.p, ClimberConstants.i, ClimberConstants.d); configNeutralBrakeMode(ClimberConstants.breakType); configFeedbackSensorSource(ClimberConstants.feedbackSensorCTRE); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index ec515c8..e99feb4 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -41,7 +41,9 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; import frc.robot.Constants.Mode; +import frc.robot.FieldConstants; import frc.robot.generated.TunerConstants; +import frc.robot.util.AllianceFlipUtil; import frc.robot.util.LocalADStarAK; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; @@ -50,7 +52,7 @@ public class Drive extends SubsystemBase { // TunerConstants doesn't include these constants, so they are declared locally - static final double ODOMETRY_FREQUENCY = Constants.CANBUS.isNetworkFD() ? 250.0 : 100.0; + static final double ODOMETRY_FREQUENCY = Constants.CANIVORE_CANBUS.isNetworkFD() ? 250.0 : 100.0; public static final double DRIVE_BASE_RADIUS = Math.max( Math.max( @@ -61,7 +63,7 @@ public class Drive extends SubsystemBase { Math.hypot(TunerConstants.BackRight.LocationX, TunerConstants.BackRight.LocationY))); // PathPlanner config constants - private static final double ROBOT_MASS_KG = 74.088; + private static final double ROBOT_MASS_KG = 60; private static final double ROBOT_MOI = 6.883; private static final double WHEEL_COF = 1.2; private static final RobotConfig PP_CONFIG = @@ -154,6 +156,7 @@ public void periodic() { odometryLock.lock(); // Prevents odometry updates while reading data gyroIO.updateInputs(gyroInputs); Logger.processInputs("Drive/Gyro", gyroInputs); + Logger.recordOutput("Drive/HubDistance", distFromHub()); for (var module : modules) { module.periodic(); } @@ -356,4 +359,12 @@ public static Translation2d[] getModuleTranslations() { new Translation2d(TunerConstants.BackRight.LocationX, TunerConstants.BackRight.LocationY) }; } + + public double distFromHub() { + Translation2d hub = FieldConstants.Hub.topCenterPoint.toTranslation2d().minus(this.getPose().getTranslation()); + Translation2d hubAdjusted = AllianceFlipUtil.apply(hub); + Translation2d translateDiff = hubAdjusted.minus(this.getPose().getTranslation()); + return translateDiff.getNorm(); + } + } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index e3d3d20..160a02b 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -23,7 +23,7 @@ /** IO implementation for Pigeon 2. */ public class GyroIOPigeon2 implements GyroIO { private final Pigeon2 pigeon = - new Pigeon2(TunerConstants.DrivetrainConstants.Pigeon2Id, Constants.CANBUS); + new Pigeon2(TunerConstants.DrivetrainConstants.Pigeon2Id, Constants.CANIVORE_CANBUS); private final StatusSignal yaw = pigeon.getYaw(); private final Queue yawPositionQueue; private final Queue yawTimestampQueue; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 8cf20bb..7afb295 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -96,9 +96,9 @@ public ModuleIOTalonFX( SwerveModuleConstants constants) { this.constants = constants; - driveTalon = new TalonFX(constants.DriveMotorId, Constants.CANBUS); - turnTalon = new TalonFX(constants.SteerMotorId, Constants.CANBUS); - cancoder = new CANcoder(constants.EncoderId, Constants.CANBUS); + driveTalon = new TalonFX(constants.DriveMotorId, Constants.CANIVORE_CANBUS); + turnTalon = new TalonFX(constants.SteerMotorId, Constants.CANIVORE_CANBUS); + cancoder = new CANcoder(constants.EncoderId, Constants.CANIVORE_CANBUS); // Configure drive motor var driveConfig = constants.DriveMotorInitialConfigs; diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index 5cc1fae..70cc71a 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -37,7 +37,7 @@ public class PhoenixOdometryThread extends Thread { private final List> genericQueues = new ArrayList<>(); private final List> timestampQueues = new ArrayList<>(); - private static boolean isCANFD = Constants.CANBUS.isNetworkFD(); + private static boolean isCANFD = Constants.CANIVORE_CANBUS.isNetworkFD(); private static PhoenixOdometryThread instance = null; public static PhoenixOdometryThread getInstance() { diff --git a/src/main/java/frc/robot/subsystems/feeder/Feeder.java b/src/main/java/frc/robot/subsystems/feeder/Feeder.java index 43d6c7f..eaf34a5 100644 --- a/src/main/java/frc/robot/subsystems/feeder/Feeder.java +++ b/src/main/java/frc/robot/subsystems/feeder/Feeder.java @@ -2,64 +2,38 @@ import org.littletonrobotics.junction.Logger; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.feeder.FeederConstants.FeederModes; -import frc.robot.subsystems.feeder.FeederConstants.FeederState; -import frc.robot.subsystems.feeder.FeederIO.FeederIOInputs; import lombok.Getter; import lombok.Setter; public class Feeder extends SubsystemBase { private final FeederIO feederIO; - private final FeederIOInputs feederInputs = new FeederIOInputs(); + private final FeederIOInputsAutoLogged feederInputs = new FeederIOInputsAutoLogged(); @Getter @Setter private FeederModes feederMode; - @Getter @Setter private FeederState feederState; public Feeder(FeederIO feederIO) { this.feederIO = feederIO; this.feederMode = FeederModes.IDLE; - this.feederState = FeederState.IDLE; } @Override public void periodic() { feederIO.updateInputs(feederInputs); - // TODO: Add Logger.processInputs once auto-logged classes are generated - // Logger.processInputs("Feeder", feederInputs); - - SmartDashboard.putString("Feeder Mode", feederMode.toString()); - SmartDashboard.putBoolean("Feeder Connected", feederInputs.feederConnected); - SmartDashboard.putNumber("Feeder Voltage", feederInputs.appliedVoltage); - SmartDashboard.putNumber("Feeder RPM", feederInputs.RPM); - SmartDashboard.putNumber("Feeder Current", feederInputs.currentAmps); - + Logger.processInputs("Feeder", feederInputs); Logger.recordOutput("Feeder/Mode", feederMode); - Logger.recordOutput("Feeder/State", feederState); - - switch (this.feederMode) { - case IDLE: - this.feederState = FeederState.IDLE; - break; - case FEEDER: - this.feederState = FeederState.FEEDER; - break; - case REVERSE: - this.feederState = FeederState.REVERSE; - break; - } } public void runFeederEnum(FeederModes feederMode) { this.feederMode = feederMode; - feederIO.setPercentOutput(feederMode.output); + feederIO.setFeederVoltage(feederMode.output); } public Command runFeederCommand(FeederModes feederMode) { - return new RunCommand(() -> runFeederEnum(feederMode), this).withName("Feeder.runEnum"); + return new RunCommand(() -> runFeederEnum(feederMode), this).withName("Feeder.runEnum" + feederMode.toString()); } } diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java index 7b8d5e6..a9ad82e 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java @@ -1,7 +1,5 @@ package frc.robot.subsystems.feeder; -import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; - import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Current; @@ -15,7 +13,7 @@ public enum FeederState { public static final boolean attached = true; - public static final int id = -1; + public static final int id = 24; public static final double p = 1; public static final double i = 0; @@ -25,24 +23,25 @@ public enum FeederState { public static final boolean breakType = false; public static final double gearRatio = 1 / 1; - public static final Current stallLimit = Units.Amps.of(60); + // stall is current at 0 rpm, supply is current at runnign speed. Stator is current throguh "motor widning" whatever that means. + public static final Current stallLimit = Units.Amps.of(90); public static final Current supplyLimit = Units.Amps.of(80); - public static final double maxForwardOutput = 0.5; - public static final double maxReverseOutput = -0.5; + public static final double maxForwardOutput = 1; + public static final double maxReverseOutput = -1; - public static final double FeederSpeed = 0.5; - public static final double reverseSpeed = -0.5; - public static final double idleSpeed = 0.0; + // public static final double FeederSpeed = 0.5; + // public static final double reverseSpeed = -0.5; + // public static final double idleSpeed = 0.0; public enum FeederModes { - FEEDER(FeederSpeed), - REVERSE(reverseSpeed), - IDLE(idleSpeed); + FEEDER(12), + REVERSE(-5), + IDLE(0); public double output; - FeederModes(double output) { - this.output = output; + FeederModes(double voltage) { + this.output = voltage; } } } diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOSparkFlex.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOSparkFlex.java index c9d6dac..841ab4b 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIOSparkFlex.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOSparkFlex.java @@ -19,7 +19,6 @@ public FeederSparkFlexConfig() { super("PivotSparkFlex", FeederConstants.id); configPIDGains(FeederConstants.p, FeederConstants.i, FeederConstants.d); configSmartCurrentLimit(FeederConstants.stallLimit, FeederConstants.supplyLimit); - configSmartStallCurrentLimit(FeederConstants.stallLimit); } } diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java index dc90de4..0b3a682 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java @@ -14,11 +14,11 @@ import frc.team5431.titan.core.subsystem.CTREMechanism; public class FeederIOTalonFX implements FeederIO { - private final TalonFX talon = new TalonFX(FeederConstants.id, Constants.CANBUS); + private final TalonFX talon = new TalonFX(FeederConstants.id, Constants.RIO_CANBUS); public static class FeederIOTalonFXConfig extends CTREMechanism.Config { public FeederIOTalonFXConfig() { - super("Feeder", Constants.CANBUS); + super("Feeder", Constants.RIO_CANBUS); configNeutralBrakeMode(FeederConstants.breakType); configStatorCurrentLimit(FeederConstants.stallLimit); diff --git a/src/main/java/frc/robot/subsystems/hopper/carpet.java b/src/main/java/frc/robot/subsystems/hopper/Carpet.java similarity index 90% rename from src/main/java/frc/robot/subsystems/hopper/carpet.java rename to src/main/java/frc/robot/subsystems/hopper/Carpet.java index f147957..775c27f 100644 --- a/src/main/java/frc/robot/subsystems/hopper/carpet.java +++ b/src/main/java/frc/robot/subsystems/hopper/Carpet.java @@ -6,7 +6,6 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.hopper.CarpetConstants.CarpetModes; -import frc.robot.subsystems.hopper.CarpetIO.CarpetIOInputs; public class Carpet extends SubsystemBase { private final CarpetIO carpetIO; @@ -35,7 +34,7 @@ public void runRollerEnum(CarpetModes carpetMode) { public Command runCarpetCommand(CarpetModes carpetMode) { return new RunCommand(() -> { this.runRollerEnum(carpetMode); - }, this).withName("Carpet.runCarpetEnum"); + }, this).withName("Carpet.runCarpetEnum" + carpetMode.toString()); } } diff --git a/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java b/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java index e8b6f20..fb7e640 100644 --- a/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java +++ b/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java @@ -12,7 +12,8 @@ public class CarpetConstants { public enum CarpetModes { IDLE(Units.Volts.of(0.0)), - INTAKE(Units.Volts.of(8.4)); + INTAKE(Units.Volts.of(5.4)), //5.4 + OUTTAKE(Units.Volts.of(-4.8)); public Voltage voltage; @@ -24,7 +25,7 @@ public enum CarpetModes { public static final class CarpetRollerConstants { public static final boolean attached = true; - public static final int id = -1; + public static final int id = 44; PIDConstants pidConstants = new PIDConstants(1, 0, 0); public static final double p = 1; diff --git a/src/main/java/frc/robot/subsystems/hopper/CarpetIOSparkFlex.java b/src/main/java/frc/robot/subsystems/hopper/CarpetIOSparkFlex.java index a6265a1..d264649 100644 --- a/src/main/java/frc/robot/subsystems/hopper/CarpetIOSparkFlex.java +++ b/src/main/java/frc/robot/subsystems/hopper/CarpetIOSparkFlex.java @@ -10,8 +10,9 @@ import frc.robot.subsystems.hopper.CarpetConstants.CarpetRollerConstants; import frc.team5431.titan.core.subsystem.REVMechanism; -public class CarpetIOSparkFlex extends CarpetIOTalonFX { - private final SparkFlex sparkFlex = new SparkFlex(CarpetRollerConstants.id, null); +public class CarpetIOSparkFlex implements CarpetIO { + // Neo Vortex + private final SparkFlex sparkFlex = new SparkFlex(CarpetRollerConstants.id, com.revrobotics.spark.SparkLowLevel.MotorType.kBrushless); private final RelativeEncoder encoder = sparkFlex.getEncoder(); public static class CarpetIOSparkFlexConfig extends REVMechanism.Config { diff --git a/src/main/java/frc/robot/subsystems/hopper/CarpetIOTalonFX.java b/src/main/java/frc/robot/subsystems/hopper/CarpetIOTalonFX.java index 563ba28..95594db 100644 --- a/src/main/java/frc/robot/subsystems/hopper/CarpetIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/hopper/CarpetIOTalonFX.java @@ -15,11 +15,11 @@ import frc.team5431.titan.core.subsystem.CTREMechanism; public class CarpetIOTalonFX implements CarpetIO { - private final TalonFX talon = new TalonFX(CarpetRollerConstants.id, Constants.CANBUS); + private final TalonFX talon = new TalonFX(CarpetRollerConstants.id, Constants.RIO_CANBUS); public static class CarpetIOTalonFXConfig extends CTREMechanism.Config { public CarpetIOTalonFXConfig() { - super("RollerTalonFX",Constants.CANBUS); + super("RollerTalonFX",Constants.RIO_CANBUS); configPIDGains(CarpetRollerConstants.p, CarpetRollerConstants.i, CarpetRollerConstants.d); configNeutralBrakeMode(CarpetRollerConstants.breakType); configFeedbackSensorSource(CarpetRollerConstants.feedbackSensorCTRE); diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index e3ce7ce..b4414d1 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.StartEndCommand; import frc.robot.subsystems.intake.IntakeConstants.IntakeMode; import frc.robot.subsystems.intake.pivot.PivotIO; import frc.robot.subsystems.intake.pivot.PivotIOInputsAutoLogged; @@ -45,21 +46,26 @@ public void runIntakeEnum(IntakeMode intakeMode) { pivotIO.setPosition(intakeMode.position.magnitude()); } - public Command runIntakeeCommand(IntakeMode intakeMode) { + public Command runIntakeCommand(IntakeMode intakeMode) { return new RunCommand(() -> { this.runIntakeEnum(intakeMode); - }, this).withName("Intake.runIntakeeCommand" + intakeMode.toString()); + }, this).withName("Intake.runIntakeCommand" + intakeMode.toString()); } - // public Command runPivotCommand(IntakeMode intakeMode) { - // return new RunCommand(() -> { - // this.runPivotEnum(intakeMode); - // }, this).withName("Intake.runPivotCommand" + intakeMode.toString()); - // } + public Command runPivotVoltageCommand(double voltage){ + return new StartEndCommand(()-> pivotIO.setPivotVoltage(voltage), ()-> pivotIO.setPivotVoltage(0), this); + } + + public Command stop() { + // return new RunCommand(() -> { + // this.pivotIO.setPivotVoltage(0); + // this.pivotIO.setPivotVoltage(0); + // }, this).withName("Intake.stopAll"); + + return run(() -> { + rollerIO.setRollerVoltage(0); + pivotIO.setPivotVoltage(0); + }).withName("Intake.Stop"); - // public Command runRollerCommand(IntakeMode rollerMode) { - // return new RunCommand(() -> { - // this.runRollerEnum(rollerMode); - // }, this).withName("Intake.runRollerCommand" + rollerMode.toString()); - // } + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index a33f971..f1b6a75 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.intake; +import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; + import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.pathplanner.lib.config.PIDConstants; import com.revrobotics.spark.FeedbackSensor; @@ -15,8 +17,8 @@ public enum IntakeMode { STOW(Units.Volts.of(0.0), Units.Degrees.of(0.0)), OUT_IDLE(Units.Volts.of(0.0), Units.Degrees.of(180.0)), - INTAKE(Units.Volts.of(8.4), Units.Degrees.of(0.0)), - OUTTAKE(Units.Volts.of(-4.8), Units.Degrees.of(180.0)); + INTAKE(Units.Volts.of(-3), Units.Degrees.of(0.0)), + OUTTAKE(Units.Volts.of(2.8), Units.Degrees.of(180.0)); public Voltage voltage; public Angle position; @@ -31,7 +33,7 @@ public static final class IntakeRollerConstants { public static final boolean attached = true; - public static final int id = -1; + public static final int id = 46; PIDConstants pidConstants = new PIDConstants(1, 0, 0); public static final double p = 1; @@ -53,25 +55,27 @@ public static final class IntakeRollerConstants { public static final Angle maxReverseRotation = Units.Rotation.of(-0.1); public static final Angle maxFowardRotation = Units.Rotation.of(5); - public static final Current stallLimit = Units.Amps.of(80); - public static final Current supplyLimit = Units.Amps.of(60); + public static final Current stallLimit = Units.Amps.of(70); + public static final Current supplyLimit = Units.Amps.of(50); } public static final class IntakePivotConstants { public static final boolean attached = true; - public static final int id = -1; + public static final int id = 14; - public static final double p = 1; - public static final double i = 0; - public static final double d = 0; + public static LoggedNetworkNumber p = new LoggedNetworkNumber("/Tuning/Shooter/P", 0.003); + public static final LoggedNetworkNumber i = new LoggedNetworkNumber("/Tuning/Shooter/I", 0); + public static final LoggedNetworkNumber d = new LoggedNetworkNumber("/Tuning/Shooter/D", 0.00002); + public static final LoggedNetworkNumber kS = new LoggedNetworkNumber("/Tuning/Shooter/kS", 0.375); + public static final LoggedNetworkNumber kV = new LoggedNetworkNumber("/Tuning/Shooter/kV", 0.0021); public static final double maxIAccum = 0.2; public static final double gearRatio = 1 / 1; public static final boolean invert = false; public static final boolean gravityType = false; - public static final boolean breakType = false; + public static final boolean breakType = true; public static final FeedbackSensorSourceValue feedbackSensorCTRE = FeedbackSensorSourceValue.FusedCANcoder; public static final FeedbackSensor feedbackSensorREV = FeedbackSensor.kAbsoluteEncoder; @@ -83,5 +87,6 @@ public static final class IntakePivotConstants { public static final Current stallLimit = Units.Amps.of(80); public static final Current supplyLimit = Units.Amps.of(60); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIO.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIO.java index d9d6c6b..4eda308 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIO.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIO.java @@ -6,7 +6,7 @@ public interface PivotIO { @AutoLog public static class PivotIOInputs { - public boolean pivotConnected = false; + public boolean pivotConnected = true; public double appliedVoltage = 0.0; public double positionAngle = 0.0; public double currentAmps = 0.0; diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java index 5e7485a..23a40ad 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java @@ -8,36 +8,45 @@ import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import edu.wpi.first.math.controller.PIDController; import frc.robot.subsystems.intake.IntakeConstants.IntakePivotConstants; +import frc.robot.subsystems.shooter.ShooterConstants.ShooterFlywheelConstants; import frc.team5431.titan.core.subsystem.REVMechanism; public class PivotIOSparkFlex implements PivotIO { - private final SparkFlex sparkFlex = new SparkFlex(IntakePivotConstants.id, null); + private final SparkFlex sparkFlex = new SparkFlex(IntakePivotConstants.id, MotorType.kBrushless); private final RelativeEncoder encoder = sparkFlex.getEncoder(); + public final PIDController pid = new PIDController(ShooterFlywheelConstants.testp.get(), ShooterFlywheelConstants.testi.get(), ShooterFlywheelConstants.testd.get()); + public static class PivotSparkFlexConfig extends REVMechanism.Config { public PivotSparkFlexConfig() { - super("PivotSparkFlex", IntakePivotConstants.id); - configPIDGains(IntakePivotConstants.p, IntakePivotConstants.i, IntakePivotConstants.d); - configFeedbackSensorSource(IntakePivotConstants.feedbackSensorREV); - // configGear(IntakePivotConstants.gearRatio); - // configGravity(IntakePivotConstants.gravityType); - configSmartCurrentLimit(IntakePivotConstants.stallLimit, IntakePivotConstants.supplyLimit); - configSmartStallCurrentLimit(IntakePivotConstants.stallLimit); - configReverseSoftLimit( - IntakePivotConstants.maxReverseRotation, IntakePivotConstants.useRMaxRotation); - configForwardSoftLimit( - IntakePivotConstants.maxFowardRotation, IntakePivotConstants.useFMaxRotation); + super("PivotSparkFlex", IntakePivotConstants.id); + configFeedbackSensorSource(IntakePivotConstants.feedbackSensorREV); + // configGear(IntakePivotConstants.gearRatio); + // configGravity(IntakePivotConstants.gravityType); + configSmartCurrentLimit(IntakePivotConstants.stallLimit, IntakePivotConstants.supplyLimit); + configSmartStallCurrentLimit(IntakePivotConstants.stallLimit); + configReverseSoftLimit( + IntakePivotConstants.maxReverseRotation, IntakePivotConstants.useRMaxRotation); + configForwardSoftLimit( + IntakePivotConstants.maxFowardRotation, IntakePivotConstants.useFMaxRotation); } - } + } public PivotIOSparkFlex() { - sparkFlex.configure(new PivotSparkFlexConfig().sparkConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + sparkFlex.configure(new PivotSparkFlexConfig().sparkConfig, ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters); } @Override public void updateInputs(PivotIOInputs inputs) { + pid.setP(ShooterFlywheelConstants.testp.get()); + pid.setI(ShooterFlywheelConstants.testi.get()); + pid.setD(ShooterFlywheelConstants.testd.get()); + ifOk(sparkFlex, encoder::getPosition, (value) -> inputs.positionAngle = value); ifOk(sparkFlex, sparkFlex::getBusVoltage, (value) -> inputs.appliedVoltage = value); ifOk(sparkFlex, sparkFlex::getOutputCurrent, (value) -> inputs.currentAmps = value); @@ -51,6 +60,6 @@ public void setPivotVoltage(double voltage) { @Override public void setPosition(double positionAngle) { sparkFlex.getClosedLoopController().setSetpoint((positionAngle), ControlType.kPosition, - ClosedLoopSlot.kSlot0); - } + ClosedLoopSlot.kSlot0); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java index 92baf0b..6e1d44b 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.intake.pivot; + package frc.robot.subsystems.intake.pivot; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; @@ -17,12 +17,11 @@ import frc.team5431.titan.core.subsystem.CTREMechanism; public class PivotIOTalonFX implements PivotIO { - private final TalonFX talon = new TalonFX(IntakePivotConstants.id, Constants.CANBUS); + private final TalonFX talon = new TalonFX(IntakePivotConstants.id, Constants.RIO_CANBUS); public static class PivotTalonFXConfig extends CTREMechanism.Config { public PivotTalonFXConfig() { - super("PivotTalonFX", Constants.CANBUS); - configPIDGains(IntakePivotConstants.p, IntakePivotConstants.i, IntakePivotConstants.d); + super("PivotTalonFX", Constants.RIO_CANBUS); configNeutralBrakeMode(IntakePivotConstants.breakType); configFeedbackSensorSource(IntakePivotConstants.feedbackSensorCTRE); configGearRatio(IntakePivotConstants.gearRatio); diff --git a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java index 488d0a8..90ae857 100644 --- a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java @@ -8,12 +8,13 @@ import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; import frc.robot.subsystems.intake.IntakeConstants.IntakeRollerConstants; import frc.team5431.titan.core.subsystem.REVMechanism; public class RollerIOSparkFlex implements RollerIO { - private final SparkFlex sparkFlex = new SparkFlex(IntakeRollerConstants.id, null); + private final SparkFlex sparkFlex = new SparkFlex(IntakeRollerConstants.id, MotorType.kBrushless); private final RelativeEncoder encoder = sparkFlex.getEncoder(); public static class RollerIOSparkFlexConfig extends REVMechanism.Config { public RollerIOSparkFlexConfig() { diff --git a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java index 53b06b2..34c10df 100644 --- a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java @@ -15,11 +15,11 @@ import frc.team5431.titan.core.subsystem.CTREMechanism; public class RollerIOTalonFX implements RollerIO { - private final TalonFX talon = new TalonFX(IntakeRollerConstants.id, Constants.CANBUS); + private final TalonFX talon = new TalonFX(IntakeRollerConstants.id, Constants.RIO_CANBUS); public static class RollerTalonFXConfig extends CTREMechanism.Config { public RollerTalonFXConfig() { - super("RollerTalonFX",Constants.CANBUS); + super("RollerTalonFX", Constants.RIO_CANBUS); configPIDGains(IntakeRollerConstants.p, IntakeRollerConstants.i, IntakeRollerConstants.d); configNeutralBrakeMode(IntakeRollerConstants.breakType); configFeedbackSensorSource(IntakeRollerConstants.feedbackSensorCTRE); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index d455c1d..3a1c4ee 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,24 +1,57 @@ package frc.robot.subsystems.shooter; +import static edu.wpi.first.units.Units.RPM; + +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + import org.littletonrobotics.junction.Logger; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.shooter.ShooterConstants.ShooterAnglerConstants; +import frc.robot.subsystems.shooter.ShooterConstants.ShooterFlywheelConstants; +import frc.robot.subsystems.shooter.ShooterConstants.ShooterModes; import frc.robot.subsystems.shooter.angler.AnglerIO; import frc.robot.subsystems.shooter.angler.AnglerIOInputsAutoLogged; import frc.robot.subsystems.shooter.flywheel.FlywheelIO; import frc.robot.subsystems.shooter.flywheel.FlywheelIOInputsAutoLogged; +import lombok.Getter; public class Shooter extends SubsystemBase { + /* + * + */ private final AnglerIO anglerIO; private final FlywheelIO flywheelIO; private final AnglerIOInputsAutoLogged anglerInputs = new AnglerIOInputsAutoLogged(); private final FlywheelIOInputsAutoLogged flywheelInputs = new FlywheelIOInputsAutoLogged(); - + + private ShooterModes shooterMode; + private static InterpolatingDoubleTreeMap speedMap = new InterpolatingDoubleTreeMap(); + + @Getter private boolean zeroed = false; public Shooter(AnglerIO anglerIO, FlywheelIO flywheelIO) { this.anglerIO = anglerIO; this.flywheelIO = flywheelIO; + this.shooterMode = ShooterModes.IDLE; + speedMap.put(1.9, 2000.0); + speedMap.put(2.75, 2500.0); + speedMap.put(2.9, 2600.0); + speedMap.put(3.1, 2700.0); + speedMap.put(3.3, 2800.0); + speedMap.put(3.5, 3000.0); + speedMap.put(3.8, 3500.0); } @Override @@ -28,23 +61,106 @@ public void periodic() { flywheelIO.updateInputs(flywheelInputs); Logger.processInputs("Shooter/Flywheel", flywheelInputs); - // Logger.recordOutput("Intake/Mode", mode); + + Logger.recordOutput("Shooter/Mode", shooterMode); + Logger.recordOutput("Shooter/Zeroed", zeroed); + // System.out.println("***********************"); + // System.out.println(FlywheelIOTalonFX.plotOutput); + // System.out.println("***********************"); } - // public void runFlywheelEnum(IntakeMode intakeMode) { - // this.mode = intakeMode; - // flywheelIO.setRPM(mode.voltage.baseUnitMagnitude()); - // } + public void runShooterEnum(ShooterModes mode) { + this.shooterMode = mode; + flywheelIO.setRPM(mode.speed); + anglerIO.setPosition(mode.angle.magnitude()); + } - // public void runAnglerEnum(IntakeMode intakeMode) { - // this.mode = intakeMode; - // anglerIO.setPosition(mode.position.magnitude()); - // } + public void runShooterCustom(double rpm, double position) { + flywheelIO.setRPM(Units.RPM.of(rpm)); + anglerIO.setPosition(position); + } + + public Command runShootAuto(DoubleSupplier dist) { + return new RunCommand(() -> flywheelIO.setRPM(Units.RPM.of(speedMap.get(dist.getAsDouble()))), this); + } - // public Command runIntakeCommand(IntakeMode intakeMode) { - // return new RunCommand(() -> { - // this.runFlywheelEnum(intakeMode); - // this.runAnglerEnum(intakeMode); - // }, this).withName("Shooter.runIntakeEnum"); + // public Command runShootAuto(DoubleSupplier dist) { + // return new RunCommand(() -> runShootAutotest(dist.getAsDouble()), this); // } -} + + public void runShootAutotest(double dist) { + System.out.println("((((((((((()))))))))))"); + System.out.println(dist); + System.out.println(speedMap.get(dist)); + System.out.println(speedMap.get(10.1)); + System.out.println(speedMap.get(10.8)); + System.out.println("((((((((((()))))))))))"); + flywheelIO.setRPM(Units.RPM.of(speedMap.get(dist))); + } + + public BooleanSupplier atSetpoint() { + return () -> MathUtil.isNear(flywheelInputs.setpointRPM, flywheelInputs.leaderRPM, flywheelInputs.setpointRPM * 0.10); + } + + public Command runAngler(ShooterModes mode) { + return new RunCommand(() -> { + // this.runShooterEnum(mode); + anglerIO.setPosition(mode.angle.magnitude()); + }, this).withName("Shooter.runAngler" + mode.toString()); + } + + public Command runShooterCommand(ShooterModes mode) { + return new RunCommand(() -> { + this.runShooterEnum(mode); + }, this).withName("Shooter.runShooterEnum" + mode.toString()); + } + + public Command runShooterVoltageCommand(double voltage){ + return new RunCommand(()-> { + flywheelIO.setVoltage(voltage); + }); + } + + public Command stop() { + return new RunCommand(() -> { + flywheelIO.setRPM(AngularVelocity.ofBaseUnits(0, RPM)); + anglerIO.setVoltage(0); + }, this).withName("Intake.Stop"); + + } + + public InstantCommand setZero() { + return new InstantCommand(() -> anglerIO.setZero(), this); + } + + public Command tune() { + return new RunCommand(() -> { + flywheelIO.setRPM(AngularVelocity.ofRelativeUnits(ShooterFlywheelConstants.tuneDesiredSpeed.get(), RPM)); + anglerIO.setPosition(ShooterAnglerConstants.tuneDesiredPosition.get()); + }, this); + } + + public Command homing() { + return new SequentialCommandGroup( + new RunCommand(() -> { + zeroed = false; + anglerIO.setVoltage(-1); + }, this).until( + () -> anglerInputs.currentAmps > ShooterAnglerConstants.homingCurrent.baseUnitMagnitude() + ).withTimeout(2), + Commands.runOnce(() -> { + zeroed = true; + anglerIO.setZero(); + }) + ); + } + + public double getPosition() { + return anglerInputs.positionAngle; + } + + public double getSpeed() { + return flywheelInputs.leaderRPM; + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 72be9c4..f616643 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -1,7 +1,10 @@ package frc.robot.subsystems.shooter; +import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; + import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import edu.wpi.first.math.controller.BangBangController; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; @@ -9,44 +12,60 @@ public class ShooterConstants { - // public enum ShooterModes { - // SHOOT( Units.RPM.of(2000)), - // IDLE( Units.RPM.of(0)); + public enum ShooterModes { + SHOOT_FAR(Units.RPM.of(3500), Units.Rotations.of(.75)), + SHOOT_CLOSE(Units.RPM.of(2500), Units.Rotations.of(.1)), + IDLE(Units.RPM.of(0), Units.Degree.of(0)), + REVERSE(Units.RPM.of(-500), Units.Degree.of(0)); - // public AngularVelocity speed; + public AngularVelocity speed; + public Angle angle; - // ShooterModes(AngularVelocity speed) { - // this.speed = speed; - // } - // } + ShooterModes(AngularVelocity speed, Angle angle) { + this.speed = speed; + this.angle = angle; + } + } public static class ShooterFlywheelConstants { public static final boolean attached = true; - public static final int followerId = 50; - public static final int leaderId = 50; + public static final int leaderId = 51; + public static final int followerId = 52; + public static final boolean inverted = false; public static final boolean breakType = false; public static final double gearRatio = 1 / 1; - public static final double p = 1; + public static final double p = 0.002000; public static final double i = 0; public static final double d = 0; + + public static LoggedNetworkNumber testp = new LoggedNetworkNumber("/Tuning/Shooter/P", 0.003); + public static final LoggedNetworkNumber testi = new LoggedNetworkNumber("/Tuning/Shooter/I", 0); + public static final LoggedNetworkNumber testd = new LoggedNetworkNumber("/Tuning/Shooter/D", 0.00002); + public static final LoggedNetworkNumber testkS = new LoggedNetworkNumber("/Tuning/Shooter/kS", 0.375); + public static final LoggedNetworkNumber testkV = new LoggedNetworkNumber("/Tuning/Shooter/kV", 0.0021); + public static final LoggedNetworkNumber tuneDesiredSpeed = new LoggedNetworkNumber("/Tuning/Shooter/desiredSpeed", + 0); + + public static final double kS = 0; + public static final double kV = 0.001000; // feedforward // public static final double maxIAccum = 2 * i; //CTRE Doesn't have one? Might // Add later - public static final FeedbackSensorSourceValue feedbackSensorCTRE = FeedbackSensorSourceValue.FusedCANcoder; + public static final FeedbackSensorSourceValue feedbackSensorCTRE = FeedbackSensorSourceValue.RotorSensor; public static final Current stallLimit = Units.Amps.of(60); public static final Current supplyLimit = Units.Amps.of(80); } - + public static class ShooterAnglerConstants { public static final boolean attached = true; - public static final int id = 50; + public static final int id = 53; public static final boolean inverted = false; - public static final boolean breakType = false; - public static final double gearRatio = 3.0 / 1; + public static final boolean breakType = true; + public static final double gearRatio = 1 / 1; public static final double p = 1; public static final double i = 0; @@ -54,15 +73,33 @@ public static class ShooterAnglerConstants { // public static final double maxIAccum = 2 * i; //CTRE Doesn't have one? Might // Add later - public static final FeedbackSensorSourceValue feedbackSensorCTRE = FeedbackSensorSourceValue.FusedCANcoder; - + public static final FeedbackSensorSourceValue feedbackSensorCTRE = FeedbackSensorSourceValue.RotorSensor; + public static final Current stallLimit = Units.Amps.of(60); public static final Current supplyLimit = Units.Amps.of(80); + public static final Current homingCurrent = Units.Amps.of(35); // public static final double maxForwardOutput = 1; // public static final double maxReverseOutput = 0.5; + public static LoggedNetworkNumber anglerP = new LoggedNetworkNumber("/Tuning/Angler/P", .002000); + public static final LoggedNetworkNumber anglerI = new LoggedNetworkNumber("/Tuning/Angler/I", 0); + public static final LoggedNetworkNumber anglerD = new LoggedNetworkNumber("/Tuning/Angler/D", 0); + public static final LoggedNetworkNumber anglerkS = new LoggedNetworkNumber("/Tuning/Angler/kS", 0); + public static final LoggedNetworkNumber anglerkV = new LoggedNetworkNumber("/Tuning/Angler/kV", 0); + public static final LoggedNetworkNumber tuneDesiredPosition = new LoggedNetworkNumber( + "/Tuning/Angler/desiredPosition", 0); + + public static final boolean tunePID = true; + + public static final BangBangController bangBangController = new BangBangController(); + public static final Angle maxReverseRotation = Units.Rotation.of(-0.1); public static final Angle maxFowardRotation = Units.Rotation.of(2); + public static final double tolerance = 0.1; + public static LoggedNetworkNumber tunableTolerance = new LoggedNetworkNumber("/Tuning/Angler/tolerance", .1); + public static LoggedNetworkNumber bangBangForwardVoltage = new LoggedNetworkNumber("/Tuning/Angler/bangBangForwardVoltage", 1); + public static LoggedNetworkNumber bangBangReversedVoltage = new LoggedNetworkNumber("/Tuning/Angler/bangBangReversedVoltage", 1); + public static LoggedNetworkNumber bangBangAngle = new LoggedNetworkNumber("/Tuning/Angler/bangBangAngle", 1); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterMath.java b/src/main/java/frc/robot/subsystems/shooter/ShooterMath.java new file mode 100644 index 0000000..e2f7a3b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterMath.java @@ -0,0 +1,20 @@ +package frc.robot.subsystems.shooter; + +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; + +public class ShooterMath { + + private static InterpolatingDoubleTreeMap hoodMap = new InterpolatingDoubleTreeMap(); + private static InterpolatingDoubleTreeMap speedMap = new InterpolatingDoubleTreeMap(); + + public static double calculateHoodPosition(double distanceToHub) { + return hoodMap.get(distanceToHub); + } + + public static double calculateSpeed(double distanceToHub) { + return speedMap.get(distanceToHub); + } + + + +} diff --git a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIO.java b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIO.java index d732bc9..8925839 100644 --- a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIO.java @@ -15,6 +15,10 @@ public static class AnglerIOInputs { public default void updateInputs(AnglerIOInputs inputs) {} /** Run the motor to the specified position. */ - public default void setPosition(double positionAngle) { - } + public default void setPosition(double positionAngle) {} + + /** Run the motor to the specified voltage. */ + public default void setVoltage(double voltage) {} + + public default void setZero() {} } diff --git a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java index f7041a8..f16bef0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java @@ -2,11 +2,14 @@ import static edu.wpi.first.units.Units.*; +import org.littletonrobotics.junction.Logger; + import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Current; @@ -16,11 +19,14 @@ import frc.team5431.titan.core.subsystem.CTREMechanism; public class AnglerIOTalonFX implements AnglerIO { - private final TalonFX talon = new TalonFX(ShooterAnglerConstants.id, Constants.CANBUS); + private final TalonFX talon = new TalonFX(ShooterAnglerConstants.id, Constants.CANIVORE_CANBUS); + + public final PIDController pid = new PIDController(ShooterAnglerConstants.anglerP.get(), + ShooterAnglerConstants.anglerI.get(), ShooterAnglerConstants.anglerD.get()); public static class PivotTalonFXConfig extends CTREMechanism.Config { public PivotTalonFXConfig() { - super("AnglerTalonFX", Constants.CANBUS); + super("AnglerTalonFX", Constants.CANIVORE_CANBUS); configPIDGains(ShooterAnglerConstants.p, ShooterAnglerConstants.i, ShooterAnglerConstants.d); configNeutralBrakeMode(ShooterAnglerConstants.breakType); configFeedbackSensorSource(ShooterAnglerConstants.feedbackSensorCTRE); @@ -30,14 +36,12 @@ public PivotTalonFXConfig() { } } - private StatusSignal appliedVoltage; private StatusSignal pivotPosition; private StatusSignal currentAmps; // No clue stole from ModuleIO - private final Debouncer anglerConnectedDebounce = - new Debouncer(0.5, Debouncer.DebounceType.kFalling); + private final Debouncer anglerConnectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); private PivotTalonFXConfig config = new PivotTalonFXConfig(); @@ -53,6 +57,13 @@ public AnglerIOTalonFX() { @Override public void updateInputs(AnglerIOInputs inputs) { + + if (ShooterAnglerConstants.tunePID) { + pid.setP(ShooterAnglerConstants.anglerP.get()); + pid.setI(ShooterAnglerConstants.anglerI.get()); + pid.setD(ShooterAnglerConstants.anglerD.get()); + } + var anglerStatus = BaseStatusSignal.refreshAll(appliedVoltage, currentAmps, pivotPosition); inputs.anglerConnected = anglerConnectedDebounce.calculate(anglerStatus.isOK()); @@ -63,8 +74,74 @@ public void updateInputs(AnglerIOInputs inputs) { } @Override - public void setPosition(double positionAngle) { - PositionVoltage mm = config.positionVoltage.withPosition(positionAngle); - talon.setControl(mm); + public void setPosition(double angleSetpoint) { + // PositionVoltage mm = config.positionVoltage.withPosition(positionAngle); + // talon.setControl(mm); + + // far is 0.75, near is 1 + // angleSetpoint = ShooterAnglerConstants.bangBangAngle.getAsDouble(); + + Logger.recordOutput("/ShooterAngler/DesiredAngle", angleSetpoint); + Logger.recordOutput("/ShooterAngler/Voltage", talon.getMotorVoltage().getValueAsDouble()); + double currentAngle = talon.getPosition().getValueAsDouble(); + double voltage = 0; + + ShooterAnglerConstants.bangBangController.setTolerance(ShooterAnglerConstants.tolerance); + // ShooterAnglerConstants.bangBangController.setTolerance(ShooterAnglerConstants.tunableTolerance.getAsDouble()); + + if (angleSetpoint > currentAngle) { + voltage = ShooterAnglerConstants.bangBangController.calculate(currentAngle, angleSetpoint) * 1; + // voltage = ShooterAnglerConstants.bangBangController.calculate(currentAngle, angleSetpoint) * ShooterAnglerConstants.bangBangForwardVoltage.getAsDouble(); + } else { + voltage = -ShooterAnglerConstants.bangBangController.calculate(-currentAngle, -angleSetpoint) * 1; + // voltage = -ShooterAnglerConstants.bangBangController.calculate(-currentAngle, -angleSetpoint) * ShooterAnglerConstants.bangBangReversedVoltage.getAsDouble(); + } + + if(ShooterAnglerConstants.bangBangController.atSetpoint()){ + voltage = 0; + } + // if (angleDi][\fference > 0.05) { + // voltage = ShooterAnglerConstants.anglerP.get(); + // } else if (angleDifference < -0.05) { + // voltage = -ShooterAnglerConstants.anglerD.get(); + // } else { + // voltage = ShooterAnglerConstants.anglerkS.get(); + // } + + // if (currentAngle.in(Rotations) > positionAngle) { + + // } + // double pidOutput = pid.calculate(currentAngle.in(Units.Rotations), + // positionAngle); + + // double voltage = pidOutput + ShooterAnglerConstants.anglerkS.get() + + // ShooterAnglerConstants.anglerkV.get() * positionAngle; + + voltage = Math.max(Math.min(voltage, 12), -12); + + // System.out.println(voltage); + // talon.setVoltage( ShooterAnglerConstants.anglerkV.get()); + + talon.setVoltage(voltage); + + // if(positionAngle > 0){ + // talon.setVoltage(voltage); + // } + // else { + // talon.set(0); + // } + + } + + @Override + public void setVoltage(double voltage) { + talon.setVoltage(voltage); + } + + @Override + public void setZero() { + talon.setControl(new NeutralOut()); + talon.setPosition(0.0); + talon.getPosition().waitForUpdate(0.1); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java index 31cee28..852782d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java @@ -2,6 +2,8 @@ import org.littletonrobotics.junction.AutoLog; +import edu.wpi.first.units.measure.AngularVelocity; + public interface FlywheelIO { @AutoLog public static class FlywheelIOInputs { @@ -13,11 +15,15 @@ public static class FlywheelIOInputs { public double followerAppliedVoltage = 0.0; public double followerRPM = 0.0; public double followerAmps = 0.0; + + public double setpointRPM = 0.0; } /** Updates the set of loggable inputs. */ public default void updateInputs(FlywheelIOInputs inputs) {} /** Run the motor to the specified rotation per minute. */ - public default void setRPM(double rpm) {} + public default void setRPM(AngularVelocity rpm) {} + + public default void setVoltage(double voltage) {} } diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java index c692a8d..5837592 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java @@ -2,53 +2,78 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.system.LinearSystem; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; public class FlywheelIOSim implements FlywheelIO { - // private DCMotorSim flywheelMotorSim; - // private PIDController flywheelController = new PIDController(ROLLER_KP, 0, ROLLER_KD); - // private boolean flywheelClosedLoop = false; - // private double appliedVoltage = 0.0; - // private double rollerFFVolts = 0.0; - - // // From ModuleIOSim no clue tbh - // private static final double ROLLER_KV_ROT = 0.91035; // Same units as TunerConstants: (volt * secs) / rotation - // private static final double ROLLER_KV = 1.0 / Units.rotationsToRadians(1.0 / ROLLER_KV_ROT); - // private static final double ROLLER_KS = 0.0; - // private static final double ROLLER_KP = 1.0; - // private static final double ROLLER_KD = 0.0; - - - // public FlywheelIOSim() { - // this.flywheelMotorSim = new DCMotorSim( - // LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), .0004, 1.0), DCMotor.getKrakenX60(1)); + private FlywheelSim flywheelMotorSim; + private PIDController flywheelController = new PIDController(FLYWHEEL_KP, 0, FLYWHEEL_KD); + private boolean flywheelClosedLoop = false; + private double appliedVoltage = 0.0; + private double flywheelFFVolts = 0.0; + private double flywheelSetpoint = 0.0; + + // From ModuleIOSim no clue tbh + private static final double FLYWHEEL_KV_ROT = 0.91035; // Same units as TunerConstants: (volt * secs) / rotation + private static final double FLYWHEEL_KV = FLYWHEEL_KV_ROT / (2.0 * Math.PI); + private static final double FLYWHEEL_KS = 0.0; + private static final double FLYWHEEL_KP = .03; + private static final double FLYWHEEL_KD = 0.0; + + + public FlywheelIOSim() { + LinearSystem plant = LinearSystemId.createFlywheelSystem(DCMotor.getKrakenX60(2), 1.0, 0.004); + + this.flywheelMotorSim = new FlywheelSim( + plant, DCMotor.getKrakenX60(2) + ); + } + + @Override + public void updateInputs(FlywheelIOInputs inputs) { + if (flywheelClosedLoop) { + appliedVoltage = flywheelFFVolts + flywheelController.calculate(flywheelMotorSim.getAngularVelocityRadPerSec()); + } else { + flywheelController.reset(); + } + + flywheelMotorSim.setInputVoltage(MathUtil.clamp(appliedVoltage, -12.0, 12.0)); + flywheelMotorSim.update(0.02); + + inputs.flywheelConnected = true; - // } - - // @Override - // public void updateInputs(FlywheelIOInputs inputs) { - // if (flywheelClosedLoop) { - // appliedVoltage = rollerFFVolts + flywheelController.calculate(flywheelMotorSim.getAngularVelocityRadPerSec()); - // } else { - // flywheelController.reset(); - // } - - // flywheelMotorSim.setInputVoltage(MathUtil.clamp(appliedVoltage, -12.0, 12.0)); - // flywheelMotorSim.update(0.02); - - // inputs.flywheelConnected = true; - // inputs.l = flywheelMotorSim.getAngularVelocityRadPerSec(); - // inputs.appliedVoltage = appliedVoltage; - // inputs.currentAmps = Math.abs(flywheelMotorSim.getCurrentDrawAmps()); - // } - - // @Override - // public void setRPM(double RPM) { - // flywheelClosedLoop = true; - // rollerFFVolts = ROLLER_KS * Math.signum(RPM) + ROLLER_KV * RPM; - // flywheelController.setSetpoint(RPM); - // } + inputs.leaderRPM = flywheelMotorSim.getAngularVelocityRPM(); + inputs.leaderAppliedVoltage = appliedVoltage; + inputs.leaderAmps = Math.abs(flywheelMotorSim.getCurrentDrawAmps()); + + inputs.followerRPM = flywheelMotorSim.getAngularVelocityRPM(); + inputs.followerAppliedVoltage = appliedVoltage; + inputs.followerAmps = Math.abs(flywheelMotorSim.getCurrentDrawAmps()); + inputs.setpointRPM = flywheelSetpoint; + } + + @Override + public void setRPM(AngularVelocity rpm) { + flywheelSetpoint = rpm.magnitude(); + + if (rpm.in(Units.RPM) == 0) { + flywheelClosedLoop = false; + appliedVoltage = 0.0; + return; + } + + flywheelClosedLoop = true; + + double setpointRadPerSec = rpm.in(Units.RadiansPerSecond); + + flywheelFFVolts = FLYWHEEL_KS * Math.signum(setpointRadPerSec) + + FLYWHEEL_KV * setpointRadPerSec; + + flywheelController.setSetpoint(setpointRadPerSec); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java index d37be19..1de8ae0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java @@ -2,6 +2,8 @@ import static edu.wpi.first.units.Units.*; +import org.littletonrobotics.junction.Logger; + import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.Follower; @@ -9,28 +11,34 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.MotorAlignmentValue; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants; import frc.robot.subsystems.shooter.ShooterConstants.ShooterFlywheelConstants; import frc.team5431.titan.core.subsystem.CTREMechanism; public class FlywheelIOTalonFX implements FlywheelIO { - private final TalonFX follower = new TalonFX(ShooterFlywheelConstants.followerId, Constants.CANBUS); - private final TalonFX leader = new TalonFX(ShooterFlywheelConstants.leaderId, Constants.CANBUS); + private final TalonFX follower = new TalonFX(ShooterFlywheelConstants.followerId, Constants.CANIVORE_CANBUS); + private final TalonFX leader = new TalonFX(ShooterFlywheelConstants.leaderId, Constants.CANIVORE_CANBUS); + + public final PIDController pid = new PIDController(ShooterFlywheelConstants.testp.get(), ShooterFlywheelConstants.testi.get(), ShooterFlywheelConstants.testd.get()); public static class FlywheelTalonFXConfig extends CTREMechanism.Config { public FlywheelTalonFXConfig() { - super("FlywheelTalonFX", Constants.CANBUS); + super("FlywheelTalonFX", Constants.CANIVORE_CANBUS); configNeutralBrakeMode(ShooterFlywheelConstants.breakType); configFeedbackSensorSource(ShooterFlywheelConstants.feedbackSensorCTRE); - configNeutralBrakeMode(ShooterFlywheelConstants.breakType); - configPIDGains(ShooterFlywheelConstants.p, ShooterFlywheelConstants.i, ShooterFlywheelConstants.d); + // configPIDGains(0, ShooterFlywheelConstants.p, ShooterFlywheelConstants.i, ShooterFlywheelConstants.d); configGearRatio(ShooterFlywheelConstants.gearRatio); configMotorInverted(ShooterFlywheelConstants.inverted); + configFeedForwardGains(0, 0.35, 0.12, 0, 0); + configSupplyCurrentLimit(ShooterFlywheelConstants.supplyLimit); + configStatorCurrentLimit(Amps.of(100)); } } @@ -41,34 +49,47 @@ public FlywheelTalonFXConfig() { private StatusSignal followerAppliedVoltage; private StatusSignal followerFlywheelRPM; private StatusSignal followerAmps; + public static VelocityVoltage plotOutput; + public static double plotrps; + public double setpointRPM = 0.0; // No clue stole from ModuleIO - private final Debouncer flywheelConnectedDebounce = - new Debouncer(0.5, Debouncer.DebounceType.kFalling); + private final Debouncer flywheelConnectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); - private FlywheelTalonFXConfig config = new FlywheelTalonFXConfig(); + private FlywheelTalonFXConfig config = new FlywheelTalonFXConfig(); public FlywheelIOTalonFX() { leaderAppliedVoltage = leader.getMotorVoltage(); leaderFlywheelRPM = leader.getVelocity(); leaderAmps = leader.getStatorCurrent(); - + followerAppliedVoltage = follower.getMotorVoltage(); followerFlywheelRPM = follower.getVelocity(); followerAmps = follower.getStatorCurrent(); + // TalonFXConfiguration config1 = new TalonFXConfiguration(); + // config1. + // config.talonConfig.Slot0.k; config.applyTalonConfig(leader); config.applyTalonConfig(follower); - + // will need to config whether aligned or inverted later - follower.setControl(new Follower(ShooterFlywheelConstants.leaderId, MotorAlignmentValue.Aligned)); + follower.setControl(new Follower(ShooterFlywheelConstants.leaderId, MotorAlignmentValue.Opposed)); - BaseStatusSignal.setUpdateFrequencyForAll(50, leaderAppliedVoltage, leaderAmps, leaderFlywheelRPM, followerAppliedVoltage, followerAmps, followerFlywheelRPM); + BaseStatusSignal.setUpdateFrequencyForAll(50, leaderAppliedVoltage, leaderAmps, leaderFlywheelRPM, + followerAppliedVoltage, followerAmps, followerFlywheelRPM); } @Override public void updateInputs(FlywheelIOInputs inputs) { - var flywheelStatus = BaseStatusSignal.refreshAll(leaderAppliedVoltage, leaderAmps, leaderFlywheelRPM, followerAppliedVoltage, followerAmps, followerFlywheelRPM); + + pid.setP(ShooterFlywheelConstants.testp.get()); + pid.setI(ShooterFlywheelConstants.testi.get()); + pid.setD(ShooterFlywheelConstants.testd.get()); + + + var flywheelStatus = BaseStatusSignal.refreshAll(leaderAppliedVoltage, leaderAmps, leaderFlywheelRPM, + followerAppliedVoltage, followerAmps, followerFlywheelRPM); inputs.flywheelConnected = flywheelConnectedDebounce.calculate(flywheelStatus.isOK()); @@ -79,11 +100,53 @@ public void updateInputs(FlywheelIOInputs inputs) { inputs.followerAppliedVoltage = followerAppliedVoltage.getValueAsDouble(); inputs.followerRPM = followerFlywheelRPM.getValue().in(RPM); inputs.followerAmps = followerAmps.getValueAsDouble(); + + inputs.setpointRPM = setpointRPM; + if (plotrps > 0 && plotOutput.Velocity > 0) { + SmartDashboard.putNumber("FlyhweelRPS", plotrps); + SmartDashboard.putNumber("FlyhweelOutputVelocity", plotOutput.Velocity); + } + + SmartDashboard.putNumber("Flywheel RPM", leader.getVelocity().getValue().in(Units.RPM)); + } + @Override - public void setRPM(double rpm) { - VelocityVoltage output = config.velocityControl.withVelocity(Units.RPM.of(rpm)); - leader.setControl(output); + public void setRPM(AngularVelocity rpm){ + setpointRPM = rpm.in(Units.RPM); + + Logger.recordOutput("/Shooter/Voltage", leader.getMotorVoltage().getValueAsDouble()); + AngularVelocity currentRPM = leader.getVelocity().getValue(); + double pidOutput = pid.calculate(currentRPM.in(Units.RPM), rpm.in(Units.RPM)); + + double voltage = pidOutput + ShooterFlywheelConstants.testkS.get() + ShooterFlywheelConstants.testkV.get() * rpm.in(Units.RPM); + + voltage = Math.max(Math.min(voltage, 12), -12); + + leader.setVoltage(voltage); + + if(rpm.in(Units.RotationsPerSecond) > 0){ + leader.setVoltage(voltage); + } + else { + leader.set(0); + } + + + // System.out.println("******************"); + // System.out.println(ShooterFlywheelConstants.testp.getAsDouble()); + // System.out.println("******************"); + // if (rpm == 0 || rpm < 0) { + // leader.setVoltage(0); + // } else { + // leader.setVoltage(5); + // } + } + + @Override + public void setVoltage(double voltage) { + + leader.setVoltage(voltage); } } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 9241469..7b7c162 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -19,6 +19,7 @@ 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.PoseObservation; import frc.robot.subsystems.vision.VisionIO.PoseObservationType; import java.util.LinkedList; import java.util.List; @@ -91,8 +92,9 @@ public void periodic() { } // Loop over pose observations - for (var observation : inputs[cameraIndex].poseObservations) { + for (PoseObservation observation : inputs[cameraIndex].poseObservations) { // Check whether to reject pose + boolean rejectPose = observation.tagCount() == 0 // Must have at least one tag || (observation.tagCount() == 1 @@ -106,6 +108,9 @@ public void periodic() { || observation.pose().getY() < 0.0 || observation.pose().getY() > aprilTagLayout.getFieldWidth(); + if (PoseObservationType.MEGATAG_2.equals(observation.type())) { + rejectPose = true; + } // Add pose to log robotPoses.add(observation.pose()); if (rejectPose) { diff --git a/src/main/java/frc/robot/util/AllianceFlipUtil.java b/src/main/java/frc/robot/util/AllianceFlipUtil.java new file mode 100644 index 0000000..4214ff8 --- /dev/null +++ b/src/main/java/frc/robot/util/AllianceFlipUtil.java @@ -0,0 +1,66 @@ +// Copyright (c) 2025-2026 Littleton Robotics +// 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 edu.wpi.first.math.geometry.*; +import edu.wpi.first.wpilibj.DriverStation; +import frc.robot.FieldConstants; + +public class AllianceFlipUtil { + public static double applyX(double x) { + return shouldFlip() ? FieldConstants.fieldLength - x : x; + } + + public static double applyY(double y) { + return shouldFlip() ? FieldConstants.fieldWidth - y : y; + } + + public static Translation2d apply(Translation2d translation) { + return new Translation2d(applyX(translation.getX()), applyY(translation.getY())); + } + + public static Rotation2d apply(Rotation2d rotation) { + return shouldFlip() ? rotation.rotateBy(Rotation2d.kPi) : rotation; + } + + public static Pose2d apply(Pose2d pose) { + return shouldFlip() + ? new Pose2d(apply(pose.getTranslation()), apply(pose.getRotation())) + : pose; + } + + public static Translation3d apply(Translation3d translation) { + return new Translation3d( + applyX(translation.getX()), applyY(translation.getY()), translation.getZ()); + } + + public static Rotation3d apply(Rotation3d rotation) { + return shouldFlip() ? rotation.rotateBy(new Rotation3d(0.0, 0.0, Math.PI)) : rotation; + } + + public static Pose3d apply(Pose3d pose) { + return new Pose3d(apply(pose.getTranslation()), apply(pose.getRotation())); + } + + public static Bounds apply(Bounds bounds) { + if (shouldFlip()) { + return new Bounds( + applyX(bounds.maxX()), + applyX(bounds.minX()), + applyY(bounds.maxY()), + applyY(bounds.minY())); + } else { + return bounds; + } + } + + public static boolean shouldFlip() { + return DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == DriverStation.Alliance.Red; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/util/Bounds.java b/src/main/java/frc/robot/util/Bounds.java new file mode 100644 index 0000000..c9bd54d --- /dev/null +++ b/src/main/java/frc/robot/util/Bounds.java @@ -0,0 +1,28 @@ +// Copyright (c) 2025-2026 Littleton Robotics +// 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 edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Translation2d; + +public record Bounds(double minX, double maxX, double minY, double maxY) { + /** Whether the translation is contained within the bounds. */ + public boolean contains(Translation2d translation) { + return translation.getX() >= minX() + && translation.getX() <= maxX() + && translation.getY() >= minY() + && translation.getY() <= maxY(); + } + + /** Clamps the translation to the bounds. */ + public Translation2d clamp(Translation2d translation) { + return new Translation2d( + MathUtil.clamp(translation.getX(), minX(), maxX()), + MathUtil.clamp(translation.getY(), minY(), maxY())); + } +} \ No newline at end of file diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 2faa4db..868ae9d 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,35 +1,35 @@ { - "fileName": "AdvantageKit.json", - "name": "AdvantageKit", - "version": "26.0.0", - "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", - "frcYear": "2026", - "mavenUrls": [ - "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" - ], - "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", - "javaDependencies": [ - { - "groupId": "org.littletonrobotics.akit", - "artifactId": "akit-java", - "version": "26.0.0" - } - ], - "jniDependencies": [ - { - "groupId": "org.littletonrobotics.akit", - "artifactId": "akit-wpilibio", - "version": "26.0.0", - "skipInvalidPlatforms": false, - "isJar": false, - "validPlatforms": [ - "linuxathena", - "linuxx86-64", - "linuxarm64", - "osxuniversal", - "windowsx86-64" - ] - } - ], - "cppDependencies": [] -} + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "26.0.1", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2026", + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" + ], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-java", + "version": "26.0.1" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-wpilibio", + "version": "26.0.1", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [] +} \ No newline at end of file diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index d8ba7ce..a5af3e9 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2026.0.2", + "version": "2026.0.3", "frcYear": "2026", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2026.0.2" + "version": "2026.0.3" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.2", + "version": "2026.0.3", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -34,7 +34,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.2", + "version": "2026.0.3", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -49,7 +49,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.2", + "version": "2026.0.3", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -66,7 +66,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2026.0.2", + "version": "2026.0.3", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -83,7 +83,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.2", + "version": "2026.0.3", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, @@ -100,7 +100,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.2", + "version": "2026.0.3", "libName": "BackendDriver", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -116,7 +116,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.2", + "version": "2026.0.3", "libName": "REVLibWpi", "sharedLibrary": true, "skipInvalidPlatforms": true,