diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 67136f49..0b989078 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -13,9 +13,9 @@ "Picture4" ], "autoFolders": [], - "defaultMaxVel": 3.2, - "defaultMaxAccel": 3.2, - "defaultMaxAngVel": 540.0, - "defaultMaxAngAccel": 720.0, + "defaultMaxVel": 2.5, + "defaultMaxAccel": 2.5, + "defaultMaxAngVel": 400.0, + "defaultMaxAngAccel": 400.0, "maxModuleSpeed": 4.5 } \ No newline at end of file diff --git a/build.gradle b/build.gradle index b0d27e6b..84ffc8be 100644 --- a/build.gradle +++ b/build.gradle @@ -88,7 +88,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() implementation 'com.google.code.gson:gson:2.10.1' - implementation 'com.github.Programming-TRIGON:TRIGONLib:2024.2.4' + implementation 'com.github.Programming-TRIGON:TRIGONLib:6e39012' def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" diff --git a/simgui-window.json b/simgui-window.json index d1d16c31..ca3d2c29 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -15,6 +15,11 @@ } }, "Table": { + "0x542B5671,2": { + "Column 0 Width": "347", + "Column 1 Width": "204", + "RefScale": "16" + }, "0xE56EC1C2,4": { "Column 0 Weight": "1.0000", "Column 1 Weight": "1.0000", @@ -71,7 +76,7 @@ "###/SmartDashboard/AutoChooser": { "Collapsed": "0", "Pos": "-3,891", - "Size": "307,60" + "Size": "32,38" }, "###/SmartDashboard/Field": { "Collapsed": "0", @@ -81,12 +86,12 @@ "###FMS": { "Collapsed": "0", "Pos": "-1,712", - "Size": "202,214" + "Size": "32,38" }, "###Joysticks": { "Collapsed": "0", "Pos": "320,888", - "Size": "976,278" + "Size": "32,38" }, "###NetworkTables": { "Collapsed": "0", @@ -116,12 +121,12 @@ "###System Joysticks": { "Collapsed": "0", "Pos": "1,346", - "Size": "232,254" + "Size": "32,38" }, "###Timing": { "Collapsed": "0", "Pos": "0,172", - "Size": "169,168" + "Size": "32,38" }, "Debug##Default": { "Collapsed": "0", @@ -131,7 +136,7 @@ "Robot State": { "Collapsed": "0", "Pos": "2,25", - "Size": "109,134" + "Size": "32,38" } } } diff --git a/src/main/deploy/2024-crescendo.json b/src/main/deploy/2024-crescendo.json new file mode 100644 index 00000000..8cb837a5 --- /dev/null +++ b/src/main/deploy/2024-crescendo.json @@ -0,0 +1,296 @@ +{ + "tags": [ + { + "ID": 1, + "pose": { + "translation": { + "x": 15.079471999999997, + "y": 0.24587199999999998, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 2, + "pose": { + "translation": { + "x": 16.185134, + "y": 0.883666, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 3, + "pose": { + "translation": { + "x": 16.579342, + "y": 4.982717999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 16.579342, + "y": 5.547867999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 14.700757999999999, + "y": 8.2042, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 1.8415, + "y": 8.2042, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": -0.038099999999999995, + "y": 5.547867999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": -0.038099999999999995, + "y": 4.982717999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 9, + "pose": { + "translation": { + "x": 0.356108, + "y": 0.883666, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 10, + "pose": { + "translation": { + "x": 1.4615159999999998, + "y": 0.24587199999999998, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 11, + "pose": { + "translation": { + "x": 11.904726, + "y": 3.7132259999999997, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": -0.8660254037844387, + "X": -0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 12, + "pose": { + "translation": { + "x": 11.904726, + "y": 4.49834, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 13, + "pose": { + "translation": { + "x": 11.220196, + "y": 4.105148, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 14, + "pose": { + "translation": { + "x": 5.320792, + "y": 4.105148, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 15, + "pose": { + "translation": { + "x": 4.641342, + "y": 4.49834, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 16, + "pose": { + "translation": { + "x": 4.641342, + "y": 3.7132259999999997, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": -0.4999999999999998, + "X": -0.0, + "Y": 0.0, + "Z": 0.8660254037844387 + } + } + } + } + ], + "field": { + "length": 16.541, + "width": 8.211 + } +} diff --git a/src/main/deploy/Pathplanner/paths/Picture2Collect2.path b/src/main/deploy/Pathplanner/paths/Picture2Collect2.path index af0b6149..2aaca779 100644 --- a/src/main/deploy/Pathplanner/paths/Picture2Collect2.path +++ b/src/main/deploy/Pathplanner/paths/Picture2Collect2.path @@ -67,10 +67,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/Pathplanner/paths/Picture2Collect4.path b/src/main/deploy/Pathplanner/paths/Picture2Collect4.path index e3e3e877..7b61a7bc 100644 --- a/src/main/deploy/Pathplanner/paths/Picture2Collect4.path +++ b/src/main/deploy/Pathplanner/paths/Picture2Collect4.path @@ -72,10 +72,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/Pathplanner/paths/Picture2Collect6.path b/src/main/deploy/Pathplanner/paths/Picture2Collect6.path index f1236051..f6385196 100644 --- a/src/main/deploy/Pathplanner/paths/Picture2Collect6.path +++ b/src/main/deploy/Pathplanner/paths/Picture2Collect6.path @@ -50,10 +50,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/Pathplanner/paths/Picture2CollectAndShoot1.path b/src/main/deploy/Pathplanner/paths/Picture2CollectAndShoot1.path index 4162020e..d3e2c4e3 100644 --- a/src/main/deploy/Pathplanner/paths/Picture2CollectAndShoot1.path +++ b/src/main/deploy/Pathplanner/paths/Picture2CollectAndShoot1.path @@ -56,10 +56,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/Pathplanner/paths/Picture2Shoot3.path b/src/main/deploy/Pathplanner/paths/Picture2Shoot3.path index 38185e15..a0025b91 100644 --- a/src/main/deploy/Pathplanner/paths/Picture2Shoot3.path +++ b/src/main/deploy/Pathplanner/paths/Picture2Shoot3.path @@ -48,10 +48,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/Pathplanner/paths/Picture2Shoot5.path b/src/main/deploy/Pathplanner/paths/Picture2Shoot5.path index 1b576dfa..d9098d1d 100644 --- a/src/main/deploy/Pathplanner/paths/Picture2Shoot5.path +++ b/src/main/deploy/Pathplanner/paths/Picture2Shoot5.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/Pathplanner/paths/Picture3Collect4.path b/src/main/deploy/Pathplanner/paths/Picture3Collect4.path index 8d4a5d5a..de9025f8 100644 --- a/src/main/deploy/Pathplanner/paths/Picture3Collect4.path +++ b/src/main/deploy/Pathplanner/paths/Picture3Collect4.path @@ -62,10 +62,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/Pathplanner/paths/Picture3CollectAndShoot2.path b/src/main/deploy/Pathplanner/paths/Picture3CollectAndShoot2.path index 92f6e49e..0ccbd9e0 100644 --- a/src/main/deploy/Pathplanner/paths/Picture3CollectAndShoot2.path +++ b/src/main/deploy/Pathplanner/paths/Picture3CollectAndShoot2.path @@ -83,10 +83,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/Pathplanner/paths/Picture3CollectAndShoot3.path b/src/main/deploy/Pathplanner/paths/Picture3CollectAndShoot3.path index ab04c7e2..ac3540db 100644 --- a/src/main/deploy/Pathplanner/paths/Picture3CollectAndShoot3.path +++ b/src/main/deploy/Pathplanner/paths/Picture3CollectAndShoot3.path @@ -28,7 +28,7 @@ "y": 5.782000590087349 }, "isLocked": false, - "linkedName": null + "linkedName": "Picture3ThirdCollect" }, { "anchor": { @@ -78,10 +78,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/Pathplanner/paths/Picture4Collect3.path b/src/main/deploy/Pathplanner/paths/Picture4Collect3.path index c2ef5cc1..128a51c0 100644 --- a/src/main/deploy/Pathplanner/paths/Picture4Collect3.path +++ b/src/main/deploy/Pathplanner/paths/Picture4Collect3.path @@ -56,10 +56,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/Pathplanner/paths/Picture4Collect5.path b/src/main/deploy/Pathplanner/paths/Picture4Collect5.path index 0617e98b..2e067e61 100644 --- a/src/main/deploy/Pathplanner/paths/Picture4Collect5.path +++ b/src/main/deploy/Pathplanner/paths/Picture4Collect5.path @@ -50,10 +50,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/Pathplanner/paths/Picture4Collect7.path b/src/main/deploy/Pathplanner/paths/Picture4Collect7.path index 82fb02a2..b817e69e 100644 --- a/src/main/deploy/Pathplanner/paths/Picture4Collect7.path +++ b/src/main/deploy/Pathplanner/paths/Picture4Collect7.path @@ -72,10 +72,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/Pathplanner/paths/Picture4CollectAndShoot1.path b/src/main/deploy/Pathplanner/paths/Picture4CollectAndShoot1.path index ac8ab80b..9f015bc4 100644 --- a/src/main/deploy/Pathplanner/paths/Picture4CollectAndShoot1.path +++ b/src/main/deploy/Pathplanner/paths/Picture4CollectAndShoot1.path @@ -56,10 +56,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/Pathplanner/paths/Picture4CollectAndShoot2.path b/src/main/deploy/Pathplanner/paths/Picture4CollectAndShoot2.path index 1d214842..e4141562 100644 --- a/src/main/deploy/Pathplanner/paths/Picture4CollectAndShoot2.path +++ b/src/main/deploy/Pathplanner/paths/Picture4CollectAndShoot2.path @@ -56,10 +56,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/Pathplanner/paths/Picture4Shoot4.path b/src/main/deploy/Pathplanner/paths/Picture4Shoot4.path index 734b7f0d..2c907c29 100644 --- a/src/main/deploy/Pathplanner/paths/Picture4Shoot4.path +++ b/src/main/deploy/Pathplanner/paths/Picture4Shoot4.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/Pathplanner/paths/Picture4Shoot6.path b/src/main/deploy/Pathplanner/paths/Picture4Shoot6.path index 32107812..bdc121f1 100644 --- a/src/main/deploy/Pathplanner/paths/Picture4Shoot6.path +++ b/src/main/deploy/Pathplanner/paths/Picture4Shoot6.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/autos/CloseNotesLeft.auto b/src/main/deploy/pathplanner/autos/CloseNotesLeft.auto index 84a4bc13..6cf4725d 100644 --- a/src/main/deploy/pathplanner/autos/CloseNotesLeft.auto +++ b/src/main/deploy/pathplanner/autos/CloseNotesLeft.auto @@ -28,26 +28,13 @@ { "type": "wait", "data": { - "waitTime": 0.3 + "waitTime": 0.6 } }, { - "type": "race", + "type": "named", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "FeedNoteWithoutWaiting" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - } - ] + "name": "FeedNote" } }, { diff --git a/src/main/deploy/pathplanner/autos/CloseNotesMiddle.auto b/src/main/deploy/pathplanner/autos/CloseNotesMiddle.auto index 0ec82613..9d679e2d 100644 --- a/src/main/deploy/pathplanner/autos/CloseNotesMiddle.auto +++ b/src/main/deploy/pathplanner/autos/CloseNotesMiddle.auto @@ -28,7 +28,7 @@ { "type": "wait", "data": { - "waitTime": 0.3 + "waitTime": 0.6 } }, { @@ -79,6 +79,12 @@ "pathName": "CloseNotesMiddle4" } }, + { + "type": "named", + "data": { + "name": "FeedNote" + } + }, { "type": "path", "data": { @@ -96,12 +102,6 @@ } ] } - }, - { - "type": "named", - "data": { - "name": "StopShooting" - } } ] } diff --git a/src/main/deploy/pathplanner/autos/Picture3.auto b/src/main/deploy/pathplanner/autos/Picture3.auto index a0f1b295..db4b4a29 100644 --- a/src/main/deploy/pathplanner/autos/Picture3.auto +++ b/src/main/deploy/pathplanner/autos/Picture3.auto @@ -52,7 +52,13 @@ { "type": "path", "data": { - "pathName": "Picture3CollectAndShoot2" + "pathName": "Picture3SecondCollect" + } + }, + { + "type": "path", + "data": { + "pathName": "Picture3ThirdShoot" } }, { @@ -70,7 +76,13 @@ { "type": "path", "data": { - "pathName": "Picture3CollectAndShoot3" + "pathName": "Picture3ThirdCollect" + } + }, + { + "type": "path", + "data": { + "pathName": "Picture3ForthShoot" } }, { diff --git a/src/main/deploy/pathplanner/paths/CloseLeftNotesShoot2.path b/src/main/deploy/pathplanner/paths/CloseLeftNotesShoot2.path index 6f7a111c..325302b7 100644 --- a/src/main/deploy/pathplanner/paths/CloseLeftNotesShoot2.path +++ b/src/main/deploy/pathplanner/paths/CloseLeftNotesShoot2.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/CloseNotesLeft5.path b/src/main/deploy/pathplanner/paths/CloseNotesLeft5.path index 7ea834b8..746c4aaf 100644 --- a/src/main/deploy/pathplanner/paths/CloseNotesLeft5.path +++ b/src/main/deploy/pathplanner/paths/CloseNotesLeft5.path @@ -53,13 +53,30 @@ ] } } + }, + { + "name": "New Event Marker", + "waypointRelativePos": 0.75, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AlignToNote" + } + } + ] + } + } } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/CloseNotesLeftCollect1.path b/src/main/deploy/pathplanner/paths/CloseNotesLeftCollect1.path index d629511b..6a24a42c 100644 --- a/src/main/deploy/pathplanner/paths/CloseNotesLeftCollect1.path +++ b/src/main/deploy/pathplanner/paths/CloseNotesLeftCollect1.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 1.5974921231201995, - "y": 7.121079443568702 + "x": 1.5928943538798759, + "y": 6.969716047694203 }, "isLocked": false, "linkedName": null @@ -20,15 +20,21 @@ "y": 7.0 }, "prevControl": { - "x": 2.0955857298725507, - "y": 7.108274947832159 + "x": 2.072354686019913, + "y": 7.016492665463964 }, "nextControl": null, "isLocked": false, "linkedName": "EndCloseNotesCollect1" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.35, + "rotationDegrees": 0, + "rotateFast": false + } + ], "constraintZones": [], "eventMarkers": [ { @@ -53,18 +59,35 @@ ] } } + }, + { + "name": "AlignToNote", + "waypointRelativePos": 0.5, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AlignToNote" + } + } + ] + } + } } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, "rotation": 0.0, - "rotateFast": true + "rotateFast": false }, "reversed": false, "folder": "CloseNotesLeft", diff --git a/src/main/deploy/pathplanner/paths/CloseNotesLeftCollectAndShoot3.path b/src/main/deploy/pathplanner/paths/CloseNotesLeftCollectAndShoot3.path index 9d76b256..f1a9e2b8 100644 --- a/src/main/deploy/pathplanner/paths/CloseNotesLeftCollectAndShoot3.path +++ b/src/main/deploy/pathplanner/paths/CloseNotesLeftCollectAndShoot3.path @@ -49,6 +49,12 @@ "data": { "name": "FeedNoteWithoutWaiting" } + }, + { + "type": "named", + "data": { + "name": "AlignToNote" + } } ] } @@ -56,10 +62,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/CloseNotesLeftCollectAndShoot4.path b/src/main/deploy/pathplanner/paths/CloseNotesLeftCollectAndShoot4.path index 52aad7b0..82f074ea 100644 --- a/src/main/deploy/pathplanner/paths/CloseNotesLeftCollectAndShoot4.path +++ b/src/main/deploy/pathplanner/paths/CloseNotesLeftCollectAndShoot4.path @@ -59,13 +59,30 @@ ] } } + }, + { + "name": "Align", + "waypointRelativePos": 0.75, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AlignToNote" + } + } + ] + } + } } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/CloseNotesMiddle2.path b/src/main/deploy/pathplanner/paths/CloseNotesMiddle2.path index 6649bf8c..88c6e80e 100644 --- a/src/main/deploy/pathplanner/paths/CloseNotesMiddle2.path +++ b/src/main/deploy/pathplanner/paths/CloseNotesMiddle2.path @@ -59,13 +59,30 @@ ] } } + }, + { + "name": "Align", + "waypointRelativePos": 0.5, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AlignToNote" + } + } + ] + } + } } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/CloseNotesMiddle3.path b/src/main/deploy/pathplanner/paths/CloseNotesMiddle3.path index 671f713d..d19500e7 100644 --- a/src/main/deploy/pathplanner/paths/CloseNotesMiddle3.path +++ b/src/main/deploy/pathplanner/paths/CloseNotesMiddle3.path @@ -55,6 +55,29 @@ "data": { "name": "FeedNoteWithoutWaiting" } + }, + { + "type": "named", + "data": { + "name": "AlignToNote" + } + } + ] + } + } + }, + { + "name": "Align", + "waypointRelativePos": 0.55, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AlignToNote" + } } ] } @@ -62,10 +85,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/CloseNotesMiddle4.path b/src/main/deploy/pathplanner/paths/CloseNotesMiddle4.path index 368d15a1..3e67732f 100644 --- a/src/main/deploy/pathplanner/paths/CloseNotesMiddle4.path +++ b/src/main/deploy/pathplanner/paths/CloseNotesMiddle4.path @@ -59,13 +59,30 @@ ] } } + }, + { + "name": "Align", + "waypointRelativePos": 0.7, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AlignToNote" + } + } + ] + } + } } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/CloseNotesMiddle5.path b/src/main/deploy/pathplanner/paths/CloseNotesMiddle5.path index 57e806e5..222494bf 100644 --- a/src/main/deploy/pathplanner/paths/CloseNotesMiddle5.path +++ b/src/main/deploy/pathplanner/paths/CloseNotesMiddle5.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/CloseNotesMiddleCollectAndShoot1.path b/src/main/deploy/pathplanner/paths/CloseNotesMiddleCollectAndShoot1.path index d68b29b6..1ed9ec30 100644 --- a/src/main/deploy/pathplanner/paths/CloseNotesMiddleCollectAndShoot1.path +++ b/src/main/deploy/pathplanner/paths/CloseNotesMiddleCollectAndShoot1.path @@ -49,6 +49,12 @@ "data": { "name": "TransporterCollection" } + }, + { + "type": "named", + "data": { + "name": "AlignToNote" + } } ] } @@ -56,10 +62,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/FinishOrbitNotesCollect1.path b/src/main/deploy/pathplanner/paths/FinishOrbitNotesCollect1.path index 378f1e9b..fe79c313 100644 --- a/src/main/deploy/pathplanner/paths/FinishOrbitNotesCollect1.path +++ b/src/main/deploy/pathplanner/paths/FinishOrbitNotesCollect1.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/FinishOrbitNotesShoot3.path b/src/main/deploy/pathplanner/paths/FinishOrbitNotesShoot3.path index 9a0ea480..83598de8 100644 --- a/src/main/deploy/pathplanner/paths/FinishOrbitNotesShoot3.path +++ b/src/main/deploy/pathplanner/paths/FinishOrbitNotesShoot3.path @@ -48,10 +48,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/FinishOrbitNotesShootAndCollect2.path b/src/main/deploy/pathplanner/paths/FinishOrbitNotesShootAndCollect2.path index bbe69c49..d1e90e66 100644 --- a/src/main/deploy/pathplanner/paths/FinishOrbitNotesShootAndCollect2.path +++ b/src/main/deploy/pathplanner/paths/FinishOrbitNotesShootAndCollect2.path @@ -127,10 +127,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/New New New New Path.path b/src/main/deploy/pathplanner/paths/New New New New Path.path index 118b513c..a3742555 100644 --- a/src/main/deploy/pathplanner/paths/New New New New Path.path +++ b/src/main/deploy/pathplanner/paths/New New New New Path.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.094760892609672, - "y": 7.273319629774651 + "x": 7.997263310441111, + "y": 7.341567937292645 }, "prevControl": null, "nextControl": { - "x": 6.954039181237508, - "y": 7.195321564039803 + "x": 6.856541599068947, + "y": 7.263569871557797 }, "isLocked": false, "linkedName": "end1" @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/New New New Path.path b/src/main/deploy/pathplanner/paths/New New New Path.path index 54a219dd..fadcf8a6 100644 --- a/src/main/deploy/pathplanner/paths/New New New Path.path +++ b/src/main/deploy/pathplanner/paths/New New New Path.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.094760892609672, - "y": 7.273319629774651 + "x": 7.997263310441111, + "y": 7.341567937292645 }, "prevControl": { - "x": 7.266031444176903, - "y": 6.5128384888598765 + "x": 7.168533862008342, + "y": 6.58108679637787 }, "nextControl": null, "isLocked": false, @@ -56,10 +56,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/New New Path.path b/src/main/deploy/pathplanner/paths/New New Path.path index 248b77d6..6caf7384 100644 --- a/src/main/deploy/pathplanner/paths/New New Path.path +++ b/src/main/deploy/pathplanner/paths/New New Path.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path index 2f7461ad..b8cc44dd 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -56,10 +56,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Option3P1.path b/src/main/deploy/pathplanner/paths/Option3P1.path index 65268be3..424ca6e5 100644 --- a/src/main/deploy/pathplanner/paths/Option3P1.path +++ b/src/main/deploy/pathplanner/paths/Option3P1.path @@ -50,10 +50,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Option3P2.path b/src/main/deploy/pathplanner/paths/Option3P2.path index b8046298..91d1465e 100644 --- a/src/main/deploy/pathplanner/paths/Option3P2.path +++ b/src/main/deploy/pathplanner/paths/Option3P2.path @@ -62,10 +62,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Option3P3.path b/src/main/deploy/pathplanner/paths/Option3P3.path index 699afbd8..ffaab5f2 100644 --- a/src/main/deploy/pathplanner/paths/Option3P3.path +++ b/src/main/deploy/pathplanner/paths/Option3P3.path @@ -50,10 +50,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Option3P4.path b/src/main/deploy/pathplanner/paths/Option3P4.path index acb8cc48..191fad25 100644 --- a/src/main/deploy/pathplanner/paths/Option3P4.path +++ b/src/main/deploy/pathplanner/paths/Option3P4.path @@ -62,10 +62,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Option3P5.path b/src/main/deploy/pathplanner/paths/Option3P5.path index 95cada5c..b46665a1 100644 --- a/src/main/deploy/pathplanner/paths/Option3P5.path +++ b/src/main/deploy/pathplanner/paths/Option3P5.path @@ -68,10 +68,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Option3P6.path b/src/main/deploy/pathplanner/paths/Option3P6.path index 23360ef2..1827a494 100644 --- a/src/main/deploy/pathplanner/paths/Option3P6.path +++ b/src/main/deploy/pathplanner/paths/Option3P6.path @@ -50,10 +50,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Option4P1.path b/src/main/deploy/pathplanner/paths/Option4P1.path index f211ba9a..68455034 100644 --- a/src/main/deploy/pathplanner/paths/Option4P1.path +++ b/src/main/deploy/pathplanner/paths/Option4P1.path @@ -56,10 +56,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Option4P2.path b/src/main/deploy/pathplanner/paths/Option4P2.path index e72d193c..0cbecb7a 100644 --- a/src/main/deploy/pathplanner/paths/Option4P2.path +++ b/src/main/deploy/pathplanner/paths/Option4P2.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Option4P3.path b/src/main/deploy/pathplanner/paths/Option4P3.path index d75c1bbb..8f21a246 100644 --- a/src/main/deploy/pathplanner/paths/Option4P3.path +++ b/src/main/deploy/pathplanner/paths/Option4P3.path @@ -50,10 +50,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Option4P4.path b/src/main/deploy/pathplanner/paths/Option4P4.path index efb22661..e2b6c3cc 100644 --- a/src/main/deploy/pathplanner/paths/Option4P4.path +++ b/src/main/deploy/pathplanner/paths/Option4P4.path @@ -62,10 +62,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Option4P5.path b/src/main/deploy/pathplanner/paths/Option4P5.path index dbba094d..f52c1d92 100644 --- a/src/main/deploy/pathplanner/paths/Option4P5.path +++ b/src/main/deploy/pathplanner/paths/Option4P5.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Option4P6.path b/src/main/deploy/pathplanner/paths/Option4P6.path index 726713db..bafe706a 100644 --- a/src/main/deploy/pathplanner/paths/Option4P6.path +++ b/src/main/deploy/pathplanner/paths/Option4P6.path @@ -56,10 +56,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Option4P7.path b/src/main/deploy/pathplanner/paths/Option4P7.path index 34a2db17..1585b301 100644 --- a/src/main/deploy/pathplanner/paths/Option4P7.path +++ b/src/main/deploy/pathplanner/paths/Option4P7.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Pic3skip.path b/src/main/deploy/pathplanner/paths/Pic3skip.path index c8cf2e84..c30f1bc3 100644 --- a/src/main/deploy/pathplanner/paths/Pic3skip.path +++ b/src/main/deploy/pathplanner/paths/Pic3skip.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Picture3Collect1.path b/src/main/deploy/pathplanner/paths/Picture3Collect1.path index d4a52582..62c9f4cf 100644 --- a/src/main/deploy/pathplanner/paths/Picture3Collect1.path +++ b/src/main/deploy/pathplanner/paths/Picture3Collect1.path @@ -41,7 +41,7 @@ { "type": "named", "data": { - "name": "FeedNoteWithoutWaiting" + "name": "TransporterCollection" } }, { @@ -49,6 +49,12 @@ "data": { "name": "IntakeCollection" } + }, + { + "type": "named", + "data": { + "name": "AlignToNote" + } } ] } @@ -56,10 +62,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0.0, diff --git a/src/main/deploy/pathplanner/paths/Picture3ForthShoot.path b/src/main/deploy/pathplanner/paths/Picture3ForthShoot.path new file mode 100644 index 00000000..2c2ddd57 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Picture3ForthShoot.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.28, + "y": 5.78 + }, + "prevControl": null, + "nextControl": { + "x": 6.495800545045272, + "y": 6.288594049872185 + }, + "isLocked": false, + "linkedName": "Picture3ThirdCollect" + }, + { + "anchor": { + "x": 3.4171824469005063, + "y": 6.303149244475128 + }, + "prevControl": { + "x": 4.477600594156059, + "y": 6.395841390257602 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "EndPicture3CollectAndShoot3" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 12.159999999999968, + "rotateFast": false + }, + "reversed": false, + "folder": "Picture3", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Picture3SecondCollect.path b/src/main/deploy/pathplanner/paths/Picture3SecondCollect.path new file mode 100644 index 00000000..29c760ea --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Picture3SecondCollect.path @@ -0,0 +1,93 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.9026396514331485, + "y": 6.993104356579084 + }, + "prevControl": null, + "nextControl": { + "x": 3.8536160682772684, + "y": 6.600586312811581 + }, + "isLocked": false, + "linkedName": "EndPicture3Collect1" + }, + { + "anchor": { + "x": 7.997263310441111, + "y": 7.341567937292645 + }, + "prevControl": { + "x": 6.97353869767122, + "y": 6.980826883268969 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "end1" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Collect", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "TransporterCollection" + } + }, + { + "type": "named", + "data": { + "name": "IntakeCollection" + } + } + ] + } + } + }, + { + "name": "New Event Marker", + "waypointRelativePos": 0.6, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AlignToNote" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 14.574216198038673, + "rotateFast": false + }, + "reversed": false, + "folder": "Picture3", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Picture3Shoot1.path b/src/main/deploy/pathplanner/paths/Picture3Shoot1.path index 6cf2ed16..48a8cd0c 100644 --- a/src/main/deploy/pathplanner/paths/Picture3Shoot1.path +++ b/src/main/deploy/pathplanner/paths/Picture3Shoot1.path @@ -56,10 +56,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0.0, diff --git a/src/main/deploy/pathplanner/paths/Picture3Shoot5.path b/src/main/deploy/pathplanner/paths/Picture3Shoot5.path index e18db354..f62d1324 100644 --- a/src/main/deploy/pathplanner/paths/Picture3Shoot5.path +++ b/src/main/deploy/pathplanner/paths/Picture3Shoot5.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Picture3SkipCollect1.path b/src/main/deploy/pathplanner/paths/Picture3SkipCollect1.path index ff7ae0a6..bd000e92 100644 --- a/src/main/deploy/pathplanner/paths/Picture3SkipCollect1.path +++ b/src/main/deploy/pathplanner/paths/Picture3SkipCollect1.path @@ -56,10 +56,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0.0, diff --git a/src/main/deploy/pathplanner/paths/Picture3SkipShoot2.path b/src/main/deploy/pathplanner/paths/Picture3SkipShoot2.path index 02ec47df..83355d7b 100644 --- a/src/main/deploy/pathplanner/paths/Picture3SkipShoot2.path +++ b/src/main/deploy/pathplanner/paths/Picture3SkipShoot2.path @@ -62,10 +62,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.2, - "maxAcceleration": 3.2, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Picture3ThirdCollect.path b/src/main/deploy/pathplanner/paths/Picture3ThirdCollect.path new file mode 100644 index 00000000..28af3dbf --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Picture3ThirdCollect.path @@ -0,0 +1,93 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.4171824469005063, + "y": 6.303149244475128 + }, + "prevControl": null, + "nextControl": { + "x": 4.506849868806627, + "y": 6.42509066490817 + }, + "isLocked": false, + "linkedName": "EndPicture3CollectAndShoot2" + }, + { + "anchor": { + "x": 8.28, + "y": 5.78 + }, + "prevControl": { + "x": 7.217282653092623, + "y": 5.9473525122822215 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Picture3ThirdCollect" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Collect", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeCollection" + } + }, + { + "type": "named", + "data": { + "name": "TransporterCollection" + } + } + ] + } + } + }, + { + "name": "New Event Marker", + "waypointRelativePos": 0.6, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AlignToNote" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -9.64280250924343, + "rotateFast": false + }, + "reversed": false, + "folder": "Picture3", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Picture3ThirdShoot.path b/src/main/deploy/pathplanner/paths/Picture3ThirdShoot.path new file mode 100644 index 00000000..89805f0d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Picture3ThirdShoot.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.997263310441111, + "y": 7.341567937292645 + }, + "prevControl": null, + "nextControl": { + "x": 6.544549336129552, + "y": 6.93207809218469 + }, + "isLocked": false, + "linkedName": "end1" + }, + { + "anchor": { + "x": 3.4171824469005063, + "y": 6.303149244475128 + }, + "prevControl": { + "x": 4.7408440660111735, + "y": 6.590836554594724 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "EndPicture3CollectAndShoot2" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 400.0, + "maxAngularAcceleration": 400.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 12.699999999999989, + "rotateFast": true + }, + "reversed": false, + "folder": "Picture3", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/Robot.java b/src/main/java/frc/trigon/robot/Robot.java index 64947f2a..e11f4c83 100644 --- a/src/main/java/frc/trigon/robot/Robot.java +++ b/src/main/java/frc/trigon/robot/Robot.java @@ -6,6 +6,8 @@ package frc.trigon.robot; import com.pathplanner.lib.pathfinding.Pathfinding; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.trigon.robot.constants.RobotConstants; @@ -15,6 +17,7 @@ import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; +import org.trigon.hardware.RobotHardwareStats; import org.trigon.utilities.LocalADStarAK; public class Robot extends LoggedRobot { @@ -22,12 +25,40 @@ public class Robot extends LoggedRobot { private final CommandScheduler commandScheduler = CommandScheduler.getInstance(); private Command autonomousCommand; private RobotContainer robotContainer; + private Pose2d pose1 = null, pose2 = null; @Override public void robotInit() { + RobotConstants.init(); Pathfinding.setPathfinder(new LocalADStarAK()); configLogger(); robotContainer = new RobotContainer(); +// OperatorConstants.OPERATOR_CONTROLLER.numpad0().onTrue(new InstantCommand( +// () -> { +// CameraConstants.REAR_MIDDLE_CAMERA.update(); +// pose1 = CameraConstants.REAR_MIDDLE_CAMERA.getEstimatedRobotPose().transformBy(new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180))); +// System.out.println("Pose 1: " + pose1); +// } +// )); +// OperatorConstants.OPERATOR_CONTROLLER.numpad1().onTrue(new InstantCommand( +// () -> { +// CameraConstants.REAR_MIDDLE_CAMERA.update(); +// pose2 = CameraConstants.REAR_MIDDLE_CAMERA.getEstimatedRobotPose().transformBy(new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180))); +// System.out.println("Pose 2: " + pose2); +// } +// )); +// OperatorConstants.OPERATOR_CONTROLLER.numpad2().onTrue(new InstantCommand( +// () -> { +// final Translation2d translation2d = calc( +// pose1.getRotation().getRadians(), pose2.getRotation().getRadians(), +// pose1.getX(), pose2.getX(), +// pose1.getY(), pose2.getY() +// ); +// System.out.println("__________________________" + translation2d); +// Logger.recordOutput("TranslationX", translation2d.getX()); +// Logger.recordOutput("TranslationY", translation2d.getY()); +// } +// )); } @Override @@ -35,8 +66,34 @@ public void robotPeriodic() { commandScheduler.run(); } + public static Translation2d calc(double a1, double a2, double x1, double x2, double y1, double y2) { + return new Translation2d( + calculateX(a1, a2, x1, x2, y1, y2), + calculateY(a1, a2, x1, x2, y1, y2) + ); + } + + public static double calculateX(double a1, double a2, double x1, double x2, double y1, double y2) { + double A = Math.cos(a1) - Math.cos(a2); + double B = Math.sin(a1) - Math.sin(a2); + double C = x2 - x1; + double D = y2 - y1; + + return (C * A + D * B) / (A * A + B * B); + } + + public static double calculateY(double a1, double a2, double x1, double x2, double y1, double y2) { + double A = Math.cos(a1) - Math.cos(a2); + double B = Math.sin(a1) - Math.sin(a2); + double C = x2 - x1; + double D = y2 - y1; + + return (D * A - C * B) / (A * A + B * B); + } + @Override public void autonomousInit() { + RobotContainer.POSE_ESTIMATOR.resetPose(RobotContainer.POSE_ESTIMATOR.getCurrentPose()); autonomousCommand = robotContainer.getAutonomousCommand(); if (autonomousCommand != null) @@ -76,7 +133,7 @@ public void testPeriodic() { } private void configLogger() { - if (RobotConstants.IS_REPLAY) { + if (RobotHardwareStats.isReplay()) { setUseTiming(false); final String logPath = LogFileUtil.findReplayLog(); final String logWriterPath = LogFileUtil.addPathSuffix(logPath, "_replay"); diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 2bd310bf..77dbe16d 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -11,11 +11,11 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.commands.Commands; import frc.trigon.robot.commands.LEDAutoSetupCommand; import frc.trigon.robot.constants.AutonomousConstants; import frc.trigon.robot.constants.CameraConstants; -import frc.trigon.robot.constants.CommandConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator; import frc.trigon.robot.subsystems.MotorSubsystem; @@ -30,7 +30,6 @@ import frc.trigon.robot.subsystems.ledstrip.LEDStrip; import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands; import frc.trigon.robot.subsystems.pitcher.Pitcher; -import frc.trigon.robot.subsystems.pitcher.PitcherCommands; import frc.trigon.robot.subsystems.shooter.Shooter; import frc.trigon.robot.subsystems.shooter.ShooterCommands; import frc.trigon.robot.subsystems.swerve.Swerve; @@ -43,6 +42,12 @@ import java.awt.*; public class RobotContainer { + public static final PoseEstimator POSE_ESTIMATOR = new PoseEstimator( +// CameraConstants.REAR_LEFT_CAMERA, +// CameraConstants.REAR_RIGHT_CAMERA, +// CameraConstants.FRONT_MIDDLE_CAMERA, + CameraConstants.REAR_MIDDLE_CAMERA + ); public static final Swerve SWERVE = new Swerve(); public static final Shooter SHOOTER = new Shooter(); public static final Pitcher PITCHER = new Pitcher(); @@ -50,12 +55,6 @@ public class RobotContainer { public static final Elevator ELEVATOR = new Elevator(); public static final Transporter TRANSPORTER = new Transporter(); public static final Climber CLIMBER = new Climber(); - public static final PoseEstimator POSE_ESTIMATOR = new PoseEstimator( -// CameraConstants.REAR_LEFT_CAMERA, -// CameraConstants.REAR_RIGHT_CAMERA, -// CameraConstants.FRONT_MIDDLE_CAMERA, - CameraConstants.REAR_MIDDLE_CAMERA - ); private final LoggedDashboardChooser autoChooser; public RobotContainer() { @@ -87,13 +86,14 @@ private void bindDefaultCommands() { ELEVATOR.setDefaultCommand(new WaitUntilCommand(() -> ELEVATOR.isBelowCameraPlate() && ELEVATOR.didOpenElevator()).andThen(Commands.withoutRequirements(TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.ALIGNING_FOR_AMP_BACKWARDS)).withTimeout(0.13).andThen(new InstantCommand(() -> ELEVATOR.setDidOpenElevator(false)))).alongWith(ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.RESTING))); TRANSPORTER.setDefaultCommand(edu.wpi.first.wpilibj2.command.Commands.idle(TRANSPORTER)); CLIMBER.setDefaultCommand(ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.RESTING)); - LEDStrip.setDefaultCommandForAllLEDS((ledStrip) -> LEDStripCommands.getAnimateColorFlowCommand(new Color(0, 150, 255), 0.5, ledStrip)); + LEDStrip.setDefaultCommandForAllLEDS((ledStrip) -> Commands.getContinuousConditionalCommand(LEDStripCommands.getStaticColorCommand(Color.green, ledStrip), LEDStripCommands.getAnimateColorFlowCommand(new Color(0, 150, 255), 0.5, ledStrip), TRANSPORTER::isNoteDetected)); } private void bindControllerCommands() { OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND); + OperatorConstants.SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_TRIGGER.onTrue(CommandConstants.SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_COMMAND); OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND); - OperatorConstants.TOGGLE_FIELD_AND_SELF_RELATIVE_DRIVE_TRIGGER.onTrue(Commands.getToggleFieldAndSelfRelativeDriveCommand()); +// OperatorConstants.TOGGLE_FIELD_AND_SELF_RELATIVE_DRIVE_TRIGGER.onTrue(Commands.getToggleFieldAndSelfRelativeDriveCommand()); OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(Commands.getToggleBrakeCommand()); OperatorConstants.SHOOT_AT_SPEAKER_TRIGGER.whileTrue(Commands.getShootAtShootingTargetCommand(false)); @@ -119,7 +119,7 @@ private void bindControllerCommands() { OperatorConstants.TURN_AUTOMATIC_NOTE_ALIGNING_ON_TRIGGER.onTrue(CommandConstants.TURN_AUTOMATIC_NOTE_ALIGNING_ON_COMMAND); OperatorConstants.TURN_AUTOMATIC_NOTE_ALIGNING_OFF_TRIGGER.onTrue(CommandConstants.TURN_AUTOMATIC_NOTE_ALIGNING_OFF_COMMAND); - OperatorConstants.DEBUGGING_BUTTON.whileTrue(PitcherCommands.getDebuggingCommand()); + OperatorConstants.DEBUGGING_BUTTON.whileTrue(CommandConstants.WHEEL_RADIUS_CHARACTERIZATION_COMMAND); } private void configureSysIdBindings(MotorSubsystem subsystem) { diff --git a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java index eaae663b..aea0211c 100644 --- a/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java +++ b/src/main/java/frc/trigon/robot/commands/AlignToNoteCommand.java @@ -3,60 +3,35 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.CameraConstants; -import frc.trigon.robot.constants.CommandConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands; import frc.trigon.robot.subsystems.ledstrip.LEDStripConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; +import org.trigon.hardware.RobotHardwareStats; import org.trigon.utilities.mirrorable.MirrorableRotation2d; import java.awt.*; public class AlignToNoteCommand extends ParallelCommandGroup { private static final ObjectDetectionCamera CAMERA = CameraConstants.NOTE_DETECTION_CAMERA; - private static final PIDController Y_PID_CONTROLLER = new PIDController(0.005, 0, 0); - private boolean didCollect = false; - private boolean wasVisible = false; - private double trackedNoteYaw = 0; + private static final PIDController Y_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? + new PIDController(0.015, 0, 0) : + new PIDController(0.005, 0, 0); public AlignToNoteCommand() { - configureNoteCollectionDetectionTrigger(); addCommands( - new InstantCommand(() -> { - didCollect = false; - wasVisible = false; - }), - getCurrentLEDColorCommand().asProxy(), - Commands.getContinuousConditionalCommand(getDriveWhileAligningToNoteCommand(), Commands.duplicate(CommandConstants.SELF_RELATIVE_DRIVE_COMMAND), this::hasTarget).asProxy(), - new RunCommand(this::trackObject) + getSetCurrentLEDColorCommand().asProxy(), + Commands.getContinuousConditionalCommand(getDriveWhileAligningToNoteCommand(), Commands.duplicate(CommandConstants.FIELD_RELATIVE_DRIVE_COMMAND), this::hasTarget).asProxy(), + new RunCommand(CAMERA::trackObject) ); } - private void trackObject() { - if (hasTarget() && !wasVisible) { - wasVisible = true; - CAMERA.startTrackingBestObject(); - trackedNoteYaw = CAMERA.getTrackedObjectYaw(); - return; - } - - if (!hasTarget()) { - wasVisible = false; - return; - } - - if (hasTarget()) - trackedNoteYaw = CAMERA.getTrackedObjectYaw(); - } - - private Command getCurrentLEDColorCommand() { + private Command getSetCurrentLEDColorCommand() { return Commands.getContinuousConditionalCommand( LEDStripCommands.getStaticColorCommand(Color.green, LEDStripConstants.LED_STRIPS), LEDStripCommands.getStaticColorCommand(Color.red, LEDStripConstants.LED_STRIPS), @@ -66,23 +41,23 @@ private Command getCurrentLEDColorCommand() { private Command getDriveWhileAligningToNoteCommand() { return SwerveCommands.getClosedLoopSelfRelativeDriveCommand( - () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), - () -> Y_PID_CONTROLLER.calculate(-trackedNoteYaw), + () -> fieldRelativePowersToSelfRelativeXPower(OperatorConstants.DRIVER_CONTROLLER.getLeftY(), OperatorConstants.DRIVER_CONTROLLER.getLeftX()), + () -> -Y_PID_CONTROLLER.calculate(CAMERA.getTrackedObjectYaw().getDegrees()), this::getTargetAngle ); } - private MirrorableRotation2d getTargetAngle() { - final Rotation2d currentRotation = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation(); - return new MirrorableRotation2d(currentRotation.plus(Rotation2d.fromDegrees(trackedNoteYaw)), false); + private double fieldRelativePowersToSelfRelativeXPower(double xPower, double yPower) { + final Rotation2d driveRelativeAngle = RobotContainer.SWERVE.getDriveRelativeAngle(); + return (CommandConstants.calculateDriveStickAxisValue(xPower) * driveRelativeAngle.getCos()) + (CommandConstants.calculateDriveStickAxisValue(yPower) * driveRelativeAngle.getSin()); } - private void configureNoteCollectionDetectionTrigger() { - final Trigger noteCollectionTrigger = RobotContainer.INTAKE.getEarlyNoteCollectionDetectionTrigger(); - noteCollectionTrigger.onTrue(new InstantCommand(() -> didCollect = true)); + private MirrorableRotation2d getTargetAngle() { + final Rotation2d currentRotation = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation(); + return new MirrorableRotation2d(currentRotation.plus(CAMERA.getTrackedObjectYaw()), false); } private boolean hasTarget() { - return CAMERA.hasTargets() && !didCollect; + return CAMERA.hasTargets() && !RobotContainer.INTAKE.isEarlyNoteCollectionDetected(); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java similarity index 81% rename from src/main/java/frc/trigon/robot/constants/CommandConstants.java rename to src/main/java/frc/trigon/robot/commands/CommandConstants.java index d2ca3d89..18e203ab 100644 --- a/src/main/java/frc/trigon/robot/constants/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -1,16 +1,20 @@ -package frc.trigon.robot.constants; +package frc.trigon.robot.commands; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.constants.AutonomousConstants; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.climber.ClimberCommands; import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands; import frc.trigon.robot.subsystems.ledstrip.LEDStripConstants; import frc.trigon.robot.subsystems.pitcher.PitcherCommands; import frc.trigon.robot.subsystems.pitcher.PitcherConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; +import frc.trigon.robot.subsystems.swerve.SwerveConstants; import org.littletonrobotics.junction.Logger; import org.trigon.hardware.misc.XboxController; import org.trigon.utilities.mirrorable.Mirrorable; @@ -25,21 +29,22 @@ public class CommandConstants { private static final XboxController DRIVER_CONTROLLER = OperatorConstants.DRIVER_CONTROLLER; private static final double MINIMUM_TRANSLATION_SHIFT_POWER = 0.18, - MINIMUM_ROTATION_SHIFT_POWER = 0.4; + MINIMUM_ROTATION_SHIFT_POWER = 0.6; public static final Command - FIELD_RELATIVE_DRIVE_COMMAND = SwerveCommands.getOpenLoopFieldRelativeDriveCommand( + FIELD_RELATIVE_DRIVE_COMMAND = SwerveCommands.getClosedLoopFieldRelativeDriveCommand( () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftY()), () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftX()), () -> calculateRotationStickAxisValue(DRIVER_CONTROLLER.getRightX()) ), - SELF_RELATIVE_DRIVE_COMMAND = SwerveCommands.getOpenLoopSelfRelativeDriveCommand( + SELF_RELATIVE_DRIVE_COMMAND = SwerveCommands.getClosedLoopSelfRelativeDriveCommand( () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftY()), () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftX()), () -> calculateRotationStickAxisValue(DRIVER_CONTROLLER.getRightX()) ), + SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_COMMAND = new InstantCommand(RobotContainer.POSE_ESTIMATOR::setGyroHeadingToBestSolvePNPHeading).ignoringDisable(true), RESET_HEADING_COMMAND = new InstantCommand(() -> RobotContainer.POSE_ESTIMATOR.resetPose(changeRotation(RobotContainer.POSE_ESTIMATOR.getCurrentPose(), new MirrorableRotation2d(0, true)))), - SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND = SwerveCommands.getOpenLoopSelfRelativeDriveCommand( + SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND = SwerveCommands.getClosedLoopSelfRelativeDriveCommand( () -> getXPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), () -> getYPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), () -> 0 @@ -79,15 +84,22 @@ public class CommandConstants { IS_CLIMBING = false; Logger.recordOutput("IsClimbing", false); }).ignoringDisable(true), + WHEEL_RADIUS_CHARACTERIZATION_COMMAND = new WheelRadiusCharacterizationCommand( + new double[]{SwerveConstants.FURTHEST_MODULE_DISTANCE_FROM_CENTER, SwerveConstants.FURTHEST_MODULE_DISTANCE_FROM_CENTER, SwerveConstants.FURTHEST_MODULE_DISTANCE_FROM_CENTER, SwerveConstants.FURTHEST_MODULE_DISTANCE_FROM_CENTER}, + RobotContainer.SWERVE::getDriveWheelPositionsRadians, + () -> RobotContainer.SWERVE.getHeading().getRadians(), + RobotContainer.SWERVE::runWheelRadiusCharacterization, + RobotContainer.SWERVE + ), ALIGN_TO_RIGHT_STAGE_COMMAND = SwerveCommands.getClosedLoopFieldRelativeDriveCommand( () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), - () -> Mirrorable.isRedAlliance() ? MirrorableRotation2d.fromDegrees(60, false) : MirrorableRotation2d.fromDegrees(-120, false) + () -> Mirrorable.isRedAlliance() ? MirrorableRotation2d.fromDegrees(-120, false) : MirrorableRotation2d.fromDegrees(60, false) ), ALIGN_TO_LEFT_STAGE_COMMAND = SwerveCommands.getClosedLoopFieldRelativeDriveCommand( () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), - () -> Mirrorable.isRedAlliance() ? MirrorableRotation2d.fromDegrees(-60, false) : MirrorableRotation2d.fromDegrees(120, false) + () -> Mirrorable.isRedAlliance() ? MirrorableRotation2d.fromDegrees(120, false) : MirrorableRotation2d.fromDegrees(-60, false) ), ALIGN_TO_MIDDLE_STAGE_COMMAND = SwerveCommands.getClosedLoopFieldRelativeDriveCommand( () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), @@ -100,7 +112,7 @@ public static double calculateDriveStickAxisValue(double axisValue) { } public static double calculateRotationStickAxisValue(double axisValue) { - return axisValue / OperatorConstants.STICKS_SPEED_DIVIDER / calculateShiftModeValue(MINIMUM_ROTATION_SHIFT_POWER); + return axisValue / OperatorConstants.STICKS_SPEED_DIVIDER / calculateShiftModeValue(MINIMUM_ROTATION_SHIFT_POWER) / 1.3; } /** @@ -111,7 +123,7 @@ public static double calculateRotationStickAxisValue(double axisValue) { * @return the power to apply to the robot */ public static double calculateShiftModeValue(double minimumPower) { - final double squaredShiftModeValue = DRIVER_CONTROLLER.getRightTriggerAxis(); + final double squaredShiftModeValue = Math.sqrt(DRIVER_CONTROLLER.getRightTriggerAxis()); final double minimumShiftValueCoefficient = 1 - (1 / minimumPower); return 1 - squaredShiftModeValue * minimumShiftValueCoefficient; diff --git a/src/main/java/frc/trigon/robot/commands/Commands.java b/src/main/java/frc/trigon/robot/commands/Commands.java index 3c95a282..50fa544d 100644 --- a/src/main/java/frc/trigon/robot/commands/Commands.java +++ b/src/main/java/frc/trigon/robot/commands/Commands.java @@ -1,6 +1,7 @@ package frc.trigon.robot.commands; import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; @@ -8,6 +9,7 @@ import frc.trigon.robot.Robot; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.*; +import frc.trigon.robot.misc.ShootingCalculations; import frc.trigon.robot.misc.objectdetectioncamera.SimulationObjectDetectionCameraIO; import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.climber.ClimberCommands; @@ -24,11 +26,11 @@ import frc.trigon.robot.subsystems.transporter.TransporterCommands; import frc.trigon.robot.subsystems.transporter.TransporterConstants; import org.littletonrobotics.junction.Logger; -import org.trigon.utilities.ShootingCalculations; import org.trigon.utilities.mirrorable.MirrorablePose2d; import org.trigon.utilities.mirrorable.MirrorableRotation2d; import java.awt.*; +import java.util.Optional; import java.util.function.BooleanSupplier; import java.util.function.Supplier; @@ -99,6 +101,7 @@ private static boolean atAmpPose() { public static Command getAutonomousScoreInAmpCommand() { return new SequentialCommandGroup( + new InstantCommand(() -> RobotContainer.ELEVATOR.setDidOpenElevator(true)), TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.ALIGNING_FOR_AMP).withTimeout(0.13), ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_AMP).alongWith( runWhenContinueTriggerPressed(TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.SCORE_AMP)), @@ -116,6 +119,35 @@ public static Command getShootAtShootingTargetCommand(boolean isDelivery) { ); } + public static Command getAlignToNoteCommand() { + return new StartEndCommand( + () -> { + CameraConstants.NOTE_DETECTION_CAMERA.startTrackingBestObject(); + PPHolonomicDriveController.setRotationTargetOverride(Commands::calculateRotationOverride); + }, + () -> PPHolonomicDriveController.setRotationTargetOverride(Optional::empty) + ).alongWith(getSetCurrentLEDColorCommand()); + } + + private static Optional calculateRotationOverride() { + CameraConstants.NOTE_DETECTION_CAMERA.trackObject(); + if (RobotContainer.TRANSPORTER.isNoteDetected() || !CameraConstants.NOTE_DETECTION_CAMERA.hasTargets()) + return Optional.empty(); + + final Rotation2d currentRotation = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation(); + final Rotation2d targetRotation = CameraConstants.NOTE_DETECTION_CAMERA.getTrackedObjectYaw().plus(currentRotation); + return Optional.of(targetRotation); + } + + private static Command getSetCurrentLEDColorCommand() { + return getContinuousConditionalCommand( + LEDStripCommands.getStaticColorCommand(Color.green, LEDStripConstants.LED_STRIPS), + LEDStripCommands.getStaticColorCommand(Color.red, LEDStripConstants.LED_STRIPS), + CameraConstants.NOTE_DETECTION_CAMERA::hasTargets + ).asProxy(); + } + + public static Command getShootAtSpeakerTestingCommand() { return new ParallelCommandGroup( getPrepareShootingTestingCommand(), diff --git a/src/main/java/frc/trigon/robot/commands/GearRatioCalculationCommand.java b/src/main/java/frc/trigon/robot/commands/GearRatioCalculationCommand.java new file mode 100644 index 00000000..7351ec26 --- /dev/null +++ b/src/main/java/frc/trigon/robot/commands/GearRatioCalculationCommand.java @@ -0,0 +1,79 @@ +// +// Source code recreated from a .class file by IntelliJ IDEA +// (powered by FernFlower decompiler) +// + +package frc.trigon.robot.commands; + +import edu.wpi.first.wpilibj2.command.*; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.LoggedDashboardBoolean; +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; + +import java.util.function.DoubleConsumer; +import java.util.function.DoubleSupplier; + +public class GearRatioCalculationCommand extends Command { + private final DoubleSupplier rotorPositionSupplier; + private final DoubleSupplier encoderPositionSupplier; + private final DoubleConsumer runGearRatioCalculation; + private final String subsystemName; + private final LoggedDashboardNumber movementVoltage; + private final LoggedDashboardBoolean shouldMoveClockwise; + private double startingRotorPosition; + private double startingEncoderPosition; + private double gearRatio; + + public GearRatioCalculationCommand(DoubleSupplier rotorPositionSupplier, DoubleSupplier encoderPositionSupplier, DoubleConsumer runGearRatioCalculation, SubsystemBase requirement) { + this.rotorPositionSupplier = rotorPositionSupplier; + this.encoderPositionSupplier = encoderPositionSupplier; + this.runGearRatioCalculation = runGearRatioCalculation; + this.subsystemName = requirement.getName(); + this.movementVoltage = new LoggedDashboardNumber("GearRatioCalculation/" + this.subsystemName + "/Voltage", 1.0); + this.shouldMoveClockwise = new LoggedDashboardBoolean("GearRatioCalculation/" + this.subsystemName + "/ShouldMoveClockwise", false); + this.addRequirements(new Subsystem[]{requirement}); + } + + public void initialize() { + new WaitCommand(0.5).andThen(new InstantCommand( () -> { + this.startingRotorPosition = this.rotorPositionSupplier.getAsDouble(); + this.startingEncoderPosition = this.encoderPositionSupplier.getAsDouble(); + })).schedule(); + this.startingRotorPosition = this.rotorPositionSupplier.getAsDouble(); + this.startingEncoderPosition = this.encoderPositionSupplier.getAsDouble(); + } + + public void execute() { + this.runGearRatioCalculation.accept(this.movementVoltage.get() * (double)this.getRotationDirection()); + this.gearRatio = this.calculateGearRatio(); + Logger.recordOutput("GearRatioCalculation/" + this.subsystemName + "/RotorDistance", this.getRotorDistance()); + Logger.recordOutput("GearRatioCalculation/" + this.subsystemName + "/EncoderDistance", this.getEncoderDistance()); + Logger.recordOutput("GearRatioCalculation/" + this.subsystemName + "/GearRatio", this.gearRatio); + } + + public void end(boolean interrupted) { + this.printResult(); + } + + private double calculateGearRatio() { + double currentRotorPosition = this.getRotorDistance(); + double currentEncoderPosition = this.getEncoderDistance(); + return currentRotorPosition / currentEncoderPosition; + } + + private double getRotorDistance() { + return this.startingRotorPosition - this.rotorPositionSupplier.getAsDouble(); + } + + private double getEncoderDistance() { + return this.startingEncoderPosition - this.encoderPositionSupplier.getAsDouble(); + } + + private int getRotationDirection() { + return this.shouldMoveClockwise.get() ? -1 : 1; + } + + private void printResult() { + System.out.println(this.subsystemName + " Gear Ratio: " + this.gearRatio); + } +} diff --git a/src/main/java/frc/trigon/robot/commands/VisualizeNoteShootingCommand.java b/src/main/java/frc/trigon/robot/commands/VisualizeNoteShootingCommand.java index 7e536198..cb747c28 100644 --- a/src/main/java/frc/trigon/robot/commands/VisualizeNoteShootingCommand.java +++ b/src/main/java/frc/trigon/robot/commands/VisualizeNoteShootingCommand.java @@ -5,8 +5,8 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.ShootingConstants; +import frc.trigon.robot.misc.ShootingCalculations; import org.littletonrobotics.junction.Logger; -import org.trigon.utilities.ShootingCalculations; import org.trigon.utilities.mirrorable.MirrorableRotation2d; /** diff --git a/src/main/java/frc/trigon/robot/commands/WheelRadiusCharacterizationCommand.java b/src/main/java/frc/trigon/robot/commands/WheelRadiusCharacterizationCommand.java new file mode 100644 index 00000000..f653f074 --- /dev/null +++ b/src/main/java/frc/trigon/robot/commands/WheelRadiusCharacterizationCommand.java @@ -0,0 +1,113 @@ +// +// Source code recreated from a .class file by IntelliJ IDEA +// (powered by FernFlower decompiler) +// + +package frc.trigon.robot.commands; + +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.LoggedDashboardBoolean; +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; + +import java.util.Arrays; +import java.util.function.DoubleConsumer; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +public class WheelRadiusCharacterizationCommand extends Command { + private static final LoggedDashboardNumber CHARACTERIZATION_SPEED = new LoggedDashboardNumber("WheelRadiusCharacterization/SpeedRadiansPerSecond", 1.0); + private static final LoggedDashboardNumber ROTATION_RATE_LIMIT = new LoggedDashboardNumber("WheelRadiusCharacterization/RotationRateLimit", 1.0); + private static final LoggedDashboardBoolean SHOULD_MOVE_CLOCKWISE = new LoggedDashboardBoolean("WheelRadiusCharacterization/ShouldMoveClockwise", false); + private final double[] wheelDistancesFromCenterMeters; + private final Supplier wheelPositionsRadiansSupplier; + private final DoubleSupplier gyroYawRadiansSupplier; + private final DoubleConsumer runWheelRadiusCharacterization; + private SlewRateLimiter rotationSlewRateLimiter; + private double startingGyroYawRadians; + private double accumulatedGyroYawRadians; + private double[] startingWheelPositions; + private double driveWheelsRadius; + + public WheelRadiusCharacterizationCommand(Translation2d[] wheelDistancesFromCenterMeters, Supplier wheelPositionsRadiansSupplier, DoubleSupplier gyroYawRadiansSupplier, DoubleConsumer runWheelRadiusCharacterization, SubsystemBase requirement) { + this.rotationSlewRateLimiter = new SlewRateLimiter(ROTATION_RATE_LIMIT.get()); + this.wheelDistancesFromCenterMeters = Arrays.stream(wheelDistancesFromCenterMeters).mapToDouble(Translation2d::getNorm).toArray(); + this.wheelPositionsRadiansSupplier = wheelPositionsRadiansSupplier; + this.gyroYawRadiansSupplier = gyroYawRadiansSupplier; + this.runWheelRadiusCharacterization = runWheelRadiusCharacterization; + this.addRequirements(new Subsystem[]{requirement}); + } + + public WheelRadiusCharacterizationCommand(double[] wheelDistancesFromCenterMeters, Supplier wheelPositionsRadiansSupplier, DoubleSupplier gyroYawRadiansSupplier, DoubleConsumer runWheelRadiusCharacterization, SubsystemBase requirement) { + this.rotationSlewRateLimiter = new SlewRateLimiter(ROTATION_RATE_LIMIT.get()); + this.wheelDistancesFromCenterMeters = wheelDistancesFromCenterMeters; + this.wheelPositionsRadiansSupplier = wheelPositionsRadiansSupplier; + this.gyroYawRadiansSupplier = gyroYawRadiansSupplier; + this.runWheelRadiusCharacterization = runWheelRadiusCharacterization; + this.addRequirements(new Subsystem[]{requirement}); + } + + public void initialize() { + this.configureStartingStats(); + } + + public void execute() { + this.driveMotors(); + this.accumulatedGyroYawRadians = this.getAccumulatedGyroYaw(); + this.driveWheelsRadius = this.calculateDriveWheelRadius(); + Logger.recordOutput("WheelRadiusCharacterization/AccumulatedGyroYawRadians", this.accumulatedGyroYawRadians); + Logger.recordOutput("RadiusCharacterization/DriveWheelRadius", this.driveWheelsRadius); + } + + public void end(boolean interrupted) { + this.printResults(); + } + + private void configureStartingStats() { + this.startingGyroYawRadians = this.gyroYawRadiansSupplier.getAsDouble(); + this.startingWheelPositions = (double[])this.wheelPositionsRadiansSupplier.get(); + this.accumulatedGyroYawRadians = 0.0; + this.rotationSlewRateLimiter = new SlewRateLimiter(ROTATION_RATE_LIMIT.get()); + this.rotationSlewRateLimiter.reset(0.0); + } + + private void driveMotors() { + this.runWheelRadiusCharacterization.accept(this.rotationSlewRateLimiter.calculate((double)this.getRotationDirection() * CHARACTERIZATION_SPEED.get())); + } + + private double getAccumulatedGyroYaw() { + return Math.abs(this.startingGyroYawRadians - this.gyroYawRadiansSupplier.getAsDouble()); + } + + private double calculateDriveWheelRadius() { + this.driveWheelsRadius = 0.0; + double[] wheelPositionsRadians = (double[])this.wheelPositionsRadiansSupplier.get(); + + for(int i = 0; i < 4; ++i) { + final double accumulatedWheelRadians = Math.abs(wheelPositionsRadians[i] - this.startingWheelPositions[i]); + final double currentWheelRadius = accumulatedGyroYawRadians * this.wheelDistancesFromCenterMeters[i] / accumulatedWheelRadians; + this.driveWheelsRadius += currentWheelRadius; + Logger.recordOutput("RadiusCharacterization/AccumulatedWheelRadians" + i, accumulatedWheelRadians); + Logger.recordOutput("RadiusCharacterization/WheelRadius" + i, currentWheelRadius); + } + + return this.driveWheelsRadius /= 4.0; + } + + private void printResults() { + if (this.accumulatedGyroYawRadians <= 6.283185307179586) { + System.out.println("Not enough data for characterization"); + } else { + System.out.println("Drive Wheel Radius: " + this.driveWheelsRadius + " meters"); + } + + } + + private int getRotationDirection() { + return SHOULD_MOVE_CLOCKWISE.get() ? -1 : 1; + } +} diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java index 81fa70d8..233a5512 100644 --- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -3,6 +3,7 @@ import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.path.PathConstraints; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.Commands; import frc.trigon.robot.subsystems.intake.IntakeCommands; @@ -35,5 +36,6 @@ private static void registerCommands() { NamedCommands.registerCommand("IntakeCollection", IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECTING)); NamedCommands.registerCommand("IntakeStopping", IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.STOPPED)); NamedCommands.registerCommand("ShootingEject", ShooterCommands.getSetTargetShootingVelocityCommand(30).alongWith(PitcherCommands.getSetTargetPitchCommand(Rotation2d.fromDegrees(26)))); + NamedCommands.registerCommand("AlignToNote", new InstantCommand()); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index edbbf73d..d86349f8 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -5,11 +5,11 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; -import frc.trigon.robot.poseestimation.robotposesources.RobotPoseSource; -import frc.trigon.robot.poseestimation.robotposesources.RobotPoseSourceConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCamera; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; public class CameraConstants { - public static final ObjectDetectionCamera NOTE_DETECTION_CAMERA = new ObjectDetectionCamera("Collection Camera"); + public static final ObjectDetectionCamera NOTE_DETECTION_CAMERA = new ObjectDetectionCamera("CollectionCamera"); private static final Transform3d REAR_LEFT_CENTER_TO_CAMERA = new Transform3d( new Translation3d(-0.353, 0.298, 0.282), @@ -24,28 +24,30 @@ public class CameraConstants { new Rotation3d(0, Units.degreesToRadians(-31.7), 0) ), REAR_MIDDLE_CENTER_TO_CAMERA = new Transform3d( - new Translation3d(0, 0, 0.632), - new Rotation3d(0, Units.degreesToRadians(-24), Units.degreesToRadians(180)) + new Translation3d(-0.02, 0, 0.63), + new Rotation3d(0, Units.degreesToRadians(-24.2), Units.degreesToRadians(180)) ); - public static final RobotPoseSource - REAR_LEFT_CAMERA = new RobotPoseSource( - RobotPoseSourceConstants.RobotPoseSourceType.PHOTON_CAMERA, - "Rear Left Camera", - REAR_LEFT_CENTER_TO_CAMERA - ), - REAR_RIGHT_CAMERA = new RobotPoseSource( - RobotPoseSourceConstants.RobotPoseSourceType.PHOTON_CAMERA, - "Rear Right Camera", - REAR_RIGHT_CENTER_TO_CAMERA - ), + public static final AprilTagCamera +// REAR_LEFT_CAMERA = new AprilTagCamera( +// AprilTagCameraConstants.RobotPoseSourceType.PHOTON_CAMERA, +// "Rear Left Camera", +// REAR_LEFT_CENTER_TO_CAMERA +// ), +// REAR_RIGHT_CAMERA = new AprilTagCamera( +// AprilTagCameraConstants.RobotPoseSourceType.PHOTON_CAMERA, +// "Rear Right Camera", +// REAR_RIGHT_CENTER_TO_CAMERA +// ), // FRONT_MIDDLE_CAMERA = new RobotPoseSource( // RobotPoseSourceConstants.RobotPoseSourceType.PHOTON_CAMERA, // "Front Middle Camera", // FRONT_MIDDLE_CENTER_TO_CAMERA // ), - REAR_MIDDLE_CAMERA = new RobotPoseSource( - RobotPoseSourceConstants.RobotPoseSourceType.PHOTON_CAMERA, - "Rear Middle Camera", - REAR_MIDDLE_CENTER_TO_CAMERA + REAR_MIDDLE_CAMERA = new AprilTagCamera( + AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, + "RearMiddleCamera", + REAR_MIDDLE_CENTER_TO_CAMERA, + 0.010, + 0.005 ); } diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 5e53b531..22af2706 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -6,22 +6,25 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation3d; +import org.trigon.utilities.FilesHandler; import org.trigon.utilities.mirrorable.MirrorablePose2d; import org.trigon.utilities.mirrorable.MirrorableTranslation3d; +import java.io.IOException; import java.util.HashMap; public class FieldConstants { - public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); + public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo); public static final HashMap TAG_ID_TO_POSE = fieldLayoutToTagIdToPoseMap(); public static final double FIELD_LENGTH_METERS = 16.54175, FIELD_WIDTH_METERS = 8.21; private static final int SPEAKER_TAG_ID = 7; - private static final Translation3d SPEAKER_TAG_TO_SPEAKER = new Translation3d(0.15, 0.0, 0.82); + private static final Translation3d SPEAKER_TAG_TO_SPEAKER = new Translation3d(0.14, 0.0, 0.65); public static final MirrorableTranslation3d SPEAKER_TRANSLATION = new MirrorableTranslation3d(TAG_ID_TO_POSE.get(SPEAKER_TAG_ID).getTranslation().plus(SPEAKER_TAG_TO_SPEAKER), true), TARGET_DELIVERY_POSITION = new MirrorableTranslation3d(2.5, 7, 0, true); + public static final double MINIMUM_DISTANCE_FROM_AMP_FOR_AUTONOMOUS_AMP_PREPARATION_METERS = 2.5; public static final MirrorablePose2d IN_FRONT_OF_AMP_POSE = new MirrorablePose2d(1.842, 8.204 - 0.48, Rotation2d.fromDegrees(90), true); private static HashMap fieldLayoutToTagIdToPoseMap() { @@ -30,4 +33,15 @@ private static HashMap fieldLayoutToTagIdToPoseMap() { tagIdToPose.put(aprilTag.ID, aprilTag.pose); return tagIdToPose; } + + private static AprilTagFieldLayout loadFieldLayout() { + final String fieldFile = FilesHandler.DEPLOY_PATH + "2024-crescendo"; + AprilTagFieldLayout layout; + try { + layout = AprilTagFieldLayout.loadFromResource(fieldFile); + } catch (IOException e) { + throw new RuntimeException("Couldn't load field file!!!!!!!"); + } + return layout; + } } diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index ee0a8bd4..803d801c 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -9,7 +9,7 @@ public class OperatorConstants { private static final int DRIVER_CONTROLLER_PORT = 0; private static final int DRIVER_CONTROLLER_EXPONENT = 1; - private static final double DRIVER_CONTROLLER_DEADBAND = 0.1; + private static final double DRIVER_CONTROLLER_DEADBAND = 0.05; public static final XboxController DRIVER_CONTROLLER = new XboxController( DRIVER_CONTROLLER_PORT, DRIVER_CONTROLLER_EXPONENT, DRIVER_CONTROLLER_DEADBAND ); @@ -22,18 +22,19 @@ public class OperatorConstants { public static final Trigger DEBUGGING_BUTTON = OPERATOR_CONTROLLER.f3(), RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.y(), + SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_TRIGGER = OPERATOR_CONTROLLER.r(), TOGGLE_BRAKE_TRIGGER = OPERATOR_CONTROLLER.g().or(RobotController::getUserButton), TOGGLE_FIELD_AND_SELF_RELATIVE_DRIVE_TRIGGER = DRIVER_CONTROLLER.b(), DRIVE_FROM_DPAD_TRIGGER = new Trigger(() -> DRIVER_CONTROLLER.getPov() != -1), COLLECT_TRIGGER = DRIVER_CONTROLLER.leftTrigger()/*.or(DEBUGGING_BUTTON)*/, CONTINUE_TRIGGER = DRIVER_CONTROLLER.leftBumper().or(OPERATOR_CONTROLLER.k()), - FACE_AMP_TRIGGER = DRIVER_CONTROLLER.x(), FACE_SPEAKER_TRIGGER = DRIVER_CONTROLLER.a(), SECOND_CONTINUE_TRIGGER = OPERATOR_CONTROLLER.m(), CLOSE_SHOT_TRIGGER = OPERATOR_CONTROLLER.x(), TURN_AUTOMATIC_NOTE_ALIGNING_ON_TRIGGER = OPERATOR_CONTROLLER.o(), TURN_AUTOMATIC_NOTE_ALIGNING_OFF_TRIGGER = OPERATOR_CONTROLLER.p(), SCORE_IN_AMP_TRIGGER = OPERATOR_CONTROLLER.a(), + FACE_AMP_TRIGGER = DRIVER_CONTROLLER.x().and(SCORE_IN_AMP_TRIGGER.negate()), AUTONOMOUS_SCORE_IN_AMP_TRIGGER = OPERATOR_CONTROLLER.z(), SHOOT_AT_SPEAKER_TRIGGER = OPERATOR_CONTROLLER.s().or(DRIVER_CONTROLLER.rightBumper()), CLIMB_TRIGGER = OPERATOR_CONTROLLER.c(), @@ -46,7 +47,7 @@ public class OperatorConstants { RESET_AUTO_POSE_TRIGGER = OPERATOR_CONTROLLER.period(), EJECT_NOTE_TRIGGER = OPERATOR_CONTROLLER.e(), AMPLIFY_LEDS_TRIGGER = OPERATOR_CONTROLLER.h(), - WARM_SPEAKER_SHOOTING_TRIGGER = OPERATOR_CONTROLLER.w(), + WARM_SPEAKER_SHOOTING_TRIGGER = OPERATOR_CONTROLLER.w().and(SHOOT_AT_SPEAKER_TRIGGER.negate()), MOVE_CLIMBER_DOWN_MANUALLY_TRIGGER = OPERATOR_CONTROLLER.f1(), MOVE_CLIMBER_UP_MANUALLY_TRIGGER = OPERATOR_CONTROLLER.f2(), DELIVERY_TRIGGER = OPERATOR_CONTROLLER.d(), diff --git a/src/main/java/frc/trigon/robot/constants/RobotConstants.java b/src/main/java/frc/trigon/robot/constants/RobotConstants.java index 6b683a96..2ab5d7ad 100644 --- a/src/main/java/frc/trigon/robot/constants/RobotConstants.java +++ b/src/main/java/frc/trigon/robot/constants/RobotConstants.java @@ -1,15 +1,19 @@ package frc.trigon.robot.constants; import frc.trigon.robot.Robot; +import org.trigon.hardware.RobotHardwareStats; +import org.trigon.utilities.FilesHandler; public class RobotConstants { private static final boolean - UNFILTERED_IS_SIMULATION = true, - UNFILTERED_IS_REPLAY = false; - public static final boolean - IS_SIMULATION = UNFILTERED_IS_SIMULATION && !Robot.IS_REAL, - IS_REPLAY = UNFILTERED_IS_REPLAY && !Robot.IS_REAL; - public static final double PERIODIC_TIME_SECONDS = 0.02; + IS_SIMULATION = false, + IS_REPLAY = true; + private static final double PERIODIC_TIME_SECONDS = 0.02; public static final String CANIVORE_NAME = "CANivore"; - public static final String LOGGING_PATH = "/media/sda1/akitlogs/"; -} + public static final String LOGGING_PATH = IS_SIMULATION && !Robot.IS_REAL ? FilesHandler.DEPLOY_PATH + "logs/" : "/media/sda1/akitlogs/"; + + public static void init() { + RobotHardwareStats.setCurrentRobotStats(Robot.IS_REAL, IS_SIMULATION, IS_REPLAY); + RobotHardwareStats.setPeriodicTimeSeconds(PERIODIC_TIME_SECONDS); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java index d3c284ed..f4f342c3 100644 --- a/src/main/java/frc/trigon/robot/constants/ShootingConstants.java +++ b/src/main/java/frc/trigon/robot/constants/ShootingConstants.java @@ -8,13 +8,13 @@ public class ShootingConstants { public static final double G_FORCE = 9.80665; public static final double - SPEAKER_SHOT_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 45, + SPEAKER_SHOT_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 50, DELIVERY_STANDING_VELOCITY_ROTATIONS_PER_SECOND = 35; public static final double CLOSE_SHOT_VELOCITY_METERS_PER_SECOND = 45; // public static final Rotation2d CLOSE_SHOT_ANGLE = Rotation2d.fromDegrees(60.5); - public static final Rotation2d CLOSE_SHOT_ANGLE = Rotation2d.fromDegrees(40); + public static final Rotation2d CLOSE_SHOT_ANGLE = Rotation2d.fromDegrees(60.5); public static final Pose3d ROBOT_RELATIVE_PIVOT_POINT = new Pose3d(-0.025, 0, 0.190, new Rotation3d(0, 0, Math.PI)); - public static final Transform3d PIVOT_POINT_TO_NOTE_EXIT_POSITION = new Transform3d(0.122446, 0, -0.046625, new Rotation3d()); + public static final Transform3d PIVOT_POINT_TO_NOTE_EXIT_POSITION = new Transform3d(0.1224469, 0, -0.046625, new Rotation3d()); } diff --git a/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java b/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java index b7e3996b..6bc14cf1 100644 --- a/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java +++ b/src/main/java/frc/trigon/robot/misc/ShootingCalculations.java @@ -1,4 +1,4 @@ -package org.trigon.utilities; +package frc.trigon.robot.misc; import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -8,6 +8,7 @@ import frc.trigon.robot.subsystems.pitcher.PitcherConstants; import frc.trigon.robot.subsystems.shooter.ShooterConstants; import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; import org.trigon.utilities.mirrorable.MirrorableRotation2d; import org.trigon.utilities.mirrorable.MirrorableTranslation3d; @@ -29,7 +30,7 @@ private ShootingCalculations() { */ public void updateCalculationsForDelivery() { Logger.recordOutput("ShootingCalculations/TargetDeliveryPose", FieldConstants.TARGET_DELIVERY_POSITION.get()); - targetShootingState = calculateTargetShootingState(FieldConstants.TARGET_DELIVERY_POSITION, ShootingConstants.DELIVERY_STANDING_VELOCITY_ROTATIONS_PER_SECOND, true); + targetShootingState = calculateTargetShootingState(FieldConstants.TARGET_DELIVERY_POSITION, ShootingConstants.DELIVERY_STANDING_VELOCITY_ROTATIONS_PER_SECOND, false); } /** @@ -47,6 +48,8 @@ public TargetShootingState getTargetShootingState() { return targetShootingState; } + LoggedDashboardNumber noteSpeedLoss = new LoggedDashboardNumber("NoteSpeedLoss", 0.685); + /** * Converts a given shooter's angular velocity to the shooter's tangential velocity. * This is specific for our shooter's specs. @@ -55,7 +58,7 @@ public TargetShootingState getTargetShootingState() { * @return the tangential velocity of the shooter */ public double angularVelocityToTangentialVelocity(double angularVelocity) { - return angularVelocity / ShooterConstants.ROTATIONS_TO_METERS; + return angularVelocity * (ShooterConstants.METERS_TO_ROTATIONS * noteSpeedLoss.get()); } /** @@ -66,7 +69,7 @@ public double angularVelocityToTangentialVelocity(double angularVelocity) { * @return the angular velocity of the shooter */ public double tangentialVelocityToAngularVelocity(double tangentialVelocity) { - return tangentialVelocity * ShooterConstants.ROTATIONS_TO_METERS; + return tangentialVelocity / (ShooterConstants.METERS_TO_ROTATIONS * noteSpeedLoss.get()); } /** @@ -100,7 +103,7 @@ private Translation2d predictFutureTranslation(double predictionTime) { * @return the target state of the robot so the note will reach the shooting target, as a {@linkplain ShootingCalculations.TargetShootingState} */ private TargetShootingState calculateTargetShootingState(MirrorableTranslation3d shootingTarget, double standingShootingVelocityRotationsPerSecond, boolean reachFromAbove) { - final Translation2d currentTranslation = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getTranslation(); + final Translation2d currentTranslation = predictFutureTranslation(0); final MirrorableRotation2d standingTargetRobotAngle = getAngleToTarget(currentTranslation, shootingTarget); final double standingTangentialVelocity = angularVelocityToTangentialVelocity(standingShootingVelocityRotationsPerSecond); final Rotation2d standingTargetPitch = calculateTargetPitch(standingTangentialVelocity, reachFromAbove, currentTranslation, standingTargetRobotAngle, shootingTarget); @@ -137,7 +140,7 @@ private TargetShootingState calculateTargetShootingState(Translation3d shootingV final MirrorableRotation2d targetRobotAngle = new MirrorableRotation2d(getYaw(shootingVector), false); final Rotation2d targetPitch = getPitch(shootingVector); final double targetVelocity = tangentialVelocityToAngularVelocity(shootingVector.getNorm()); - + return new TargetShootingState(targetRobotAngle, targetPitch, targetVelocity); } @@ -161,6 +164,8 @@ private Rotation2d getPitch(Translation3d vector) { return new Rotation2d(Math.atan2(vector.getZ(), Math.hypot(vector.getX(), vector.getY()))); } + final LoggedDashboardNumber heightAddition = new LoggedDashboardNumber("HeightAddition"); + /** * Calculates the optimal pitch for the given parameters, using the Projectile Motion calculation. * @@ -173,10 +178,13 @@ private Rotation2d getPitch(Translation3d vector) { * @return the pitch the pitcher should reach in order to shoot to the shooting target */ private Rotation2d calculateTargetPitch(double noteTangentialVelocity, boolean reachFromAbove, Translation2d currentTranslation, MirrorableRotation2d targetRobotAngle, MirrorableTranslation3d shootingTarget) { - final Pose3d endEffectorFieldRelativePose = calculateShooterEndEffectorFieldRelativePose(RobotContainer.PITCHER.getTargetPitch(), currentTranslation, targetRobotAngle); + Rotation2d lastCalculatedPitch = RobotContainer.PITCHER.getTargetPitch(); + if (lastCalculatedPitch == null || Double.isNaN(lastCalculatedPitch.getRadians())) + lastCalculatedPitch = PitcherConstants.DEFAULT_PITCH; + final Pose3d endEffectorFieldRelativePose = calculateShooterEndEffectorFieldRelativePose(lastCalculatedPitch, currentTranslation, targetRobotAngle); final double endEffectorXYDistanceFromShootingTarget = endEffectorFieldRelativePose.getTranslation().toTranslation2d().getDistance(shootingTarget.get().toTranslation2d()); final double endEffectorHeightDifferenceFromTarget = shootingTarget.get().getZ() - endEffectorFieldRelativePose.getZ(); - return calculateTargetPitchUsingProjectileMotion(noteTangentialVelocity, endEffectorXYDistanceFromShootingTarget, endEffectorHeightDifferenceFromTarget, reachFromAbove); + return calculateTargetPitchUsingProjectileMotion(noteTangentialVelocity, endEffectorXYDistanceFromShootingTarget + heightAddition.get(), endEffectorHeightDifferenceFromTarget, reachFromAbove); } /** @@ -204,9 +212,7 @@ private Rotation2d calculateTargetPitchUsingProjectileMotion(double noteTangenti final double numerator = reachFromAbove ? velocitySquared + squareRoot : velocitySquared - squareRoot; final double denominator = gForce * shooterEndEffectorXYDistanceFromShootingTarget; final double fraction = numerator / denominator; - double angleRadians = Math.atan(fraction); - if (Double.isNaN(angleRadians) || Double.isInfinite(angleRadians) || angleRadians < 0) - angleRadians = PitcherConstants.DEFAULT_PITCH.getRadians(); + final double angleRadians = Math.atan(fraction); Logger.recordOutput("ShootingCalculations/TargetPitch", Math.toDegrees(angleRadians)); return Rotation2d.fromRadians(angleRadians); } diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java index e4e8f732..d9a76845 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java @@ -1,14 +1,17 @@ package frc.trigon.robot.misc.objectdetectioncamera; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.trigon.robot.constants.RobotConstants; import org.littletonrobotics.junction.Logger; +import org.trigon.hardware.RobotHardwareStats; public class ObjectDetectionCamera extends SubsystemBase { private final ObjectDetectionCameraInputsAutoLogged objectDetectionCameraInputs = new ObjectDetectionCameraInputsAutoLogged(); private final ObjectDetectionCameraIO objectDetectionCameraIO; private final String hostname; private double lastVisibleObjectYaw = 0; + private Rotation2d trackedObjectYaw = new Rotation2d(); + private boolean wasVisible = false; public ObjectDetectionCamera(String hostname) { this.hostname = hostname; @@ -21,6 +24,20 @@ public void periodic() { Logger.processInputs(hostname, objectDetectionCameraInputs); } + public void trackObject() { + if (hasTargets() && !wasVisible) { + wasVisible = true; + startTrackingBestObject(); + trackedObjectYaw = calculateTrackedObjectYaw(); + return; + } + if (!hasTargets()) { + wasVisible = false; + return; + } + trackedObjectYaw = calculateTrackedObjectYaw(); + } + public boolean hasTargets() { return objectDetectionCameraInputs.hasTargets; } @@ -32,7 +49,11 @@ public double getBestObjectYaw() { return objectDetectionCameraInputs.bestObjectYaw; } - public double getTrackedObjectYaw() { + public Rotation2d getTrackedObjectYaw() { + return trackedObjectYaw; + } + + private Rotation2d calculateTrackedObjectYaw() { double closestYawDifference = 10000000; double closestYaw = 10000000; for (double currentYaw : objectDetectionCameraInputs.visibleObjectsYaw) { @@ -44,9 +65,9 @@ public double getTrackedObjectYaw() { } if (closestYawDifference != 10000000) { lastVisibleObjectYaw = closestYaw; - return lastVisibleObjectYaw; + return Rotation2d.fromDegrees(lastVisibleObjectYaw); } - return lastVisibleObjectYaw; + return Rotation2d.fromDegrees(lastVisibleObjectYaw); } public void startTrackingBestObject() { @@ -54,10 +75,10 @@ public void startTrackingBestObject() { } private ObjectDetectionCameraIO generateIO(String hostname) { - if (RobotConstants.IS_REPLAY) + if (RobotHardwareStats.isReplay()) return new ObjectDetectionCameraIO(); - if (RobotConstants.IS_SIMULATION) + if (RobotHardwareStats.isSimulation()) return new SimulationObjectDetectionCameraIO(hostname); return new PhotonObjectDetectionCameraIO(hostname); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/PhotonObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/PhotonObjectDetectionCameraIO.java index ed5da4d9..0fea0e74 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/PhotonObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/PhotonObjectDetectionCameraIO.java @@ -26,13 +26,13 @@ protected void updateInputs(ObjectDetectionCameraInputsAutoLogged inputs) { } private double getBestTargetYaw(PhotonPipelineResult result) { - double lowestSum = 100000; + double lowestSum = -100000; double chosenOne = 0; for (PhotonTrackedTarget target : result.getTargets()) { - final double current = Math.abs(target.getYaw()) + Math.abs(target.getPitch()); - if (lowestSum > current) { + final double current = Math.abs(target.getPitch()); + if (lowestSum < current) { lowestSum = current; - chosenOne = -target.getYaw(); + chosenOne = target.getYaw(); } } return chosenOne; diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java new file mode 100644 index 00000000..b48d8feb --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -0,0 +1,244 @@ +package frc.trigon.robot.poseestimation.apriltagcamera; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; +import frc.trigon.robot.Robot; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator6328; +import org.littletonrobotics.junction.Logger; + +/** + * An april tag camera is a class that provides the robot's pose, from a camera using one or multiple apriltags. + * An april tag is like a 2D barcode used to find the robot's position on the field. + * Since the tag's position on the field is known, we can calculate our position relative to it, therefore estimating our position on the field. + */ +public class AprilTagCamera { + protected final String name; + private final AprilTagCameraInputsAutoLogged inputs = new AprilTagCameraInputsAutoLogged(); + protected final Transform3d robotCenterToCamera; + private final double + thetaStandardDeviationExponent, + translationStandardDeviationExponent; + private final AprilTagCameraIO aprilTagCameraIO; + private double lastUpdatedTimestamp; + private Pose2d robotPose = null; + + /** + * Constructs a new AprilTagCamera. + * + * @param aprilTagCameraType the type of camera + * @param name the camera's name + * @param robotCenterToCamera the transform of the robot's origin point to the camera + * @param thetaStandardDeviationExponent a calibrated gain for the standard theta deviations of the estimated robot pose + * @param translationStandardDeviationExponent a calibrated gain for the standard translation deviations of the estimated robot pose + */ + public AprilTagCamera(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraType, + String name, Transform3d robotCenterToCamera, + double thetaStandardDeviationExponent, + double translationStandardDeviationExponent) { + this.name = name; + this.robotCenterToCamera = robotCenterToCamera; + this.thetaStandardDeviationExponent = thetaStandardDeviationExponent; + this.translationStandardDeviationExponent = translationStandardDeviationExponent; + + if (Robot.IS_REAL) + aprilTagCameraIO = aprilTagCameraType.createIOFunction.apply(name); + else + aprilTagCameraIO = new AprilTagCameraIO(); + } + + public void update() { + aprilTagCameraIO.updateInputs(inputs); + Logger.processInputs("Cameras/" + name, inputs); + + robotPose = calculateBestRobotPose(); + logCameraInfo(); + } + + public boolean hasNewResult() { + return (inputs.hasResult && inputs.distanceFromBestTag != 0) && isNewTimestamp(); + } + + public Pose2d getEstimatedRobotPose() { + return robotPose; + } + + public Rotation2d getSolvePNPHeading() { + return inputs.cameraSolvePNPPose.getRotation().toRotation2d().minus(robotCenterToCamera.getRotation().toRotation2d()); + } + + public String getName() { + return name; + } + + public double getLatestResultTimestampSeconds() { + return inputs.latestResultTimestampSeconds; + } + + /** + * Calculates the range of how inaccurate the estimated pose could be using the distance from the target, the number of targets, and a calculated gain. + * The theta deviation is infinity when we assume the robot's heading because we already assume that the heading is correct. + * + * @return the standard deviations for the pose estimation strategy used + */ + public Matrix calculateStandardDeviations() { + final double translationStandardDeviation = calculateStandardDeviations(translationStandardDeviationExponent, inputs.distanceFromBestTag, inputs.visibleTagIDs.length); + final double thetaStandardDeviation = isWithinBestTagRangeForSolvePNP() ? calculateStandardDeviations(thetaStandardDeviationExponent, inputs.distanceFromBestTag, inputs.visibleTagIDs.length) : Double.POSITIVE_INFINITY; + + return VecBuilder.fill(translationStandardDeviation, translationStandardDeviation, thetaStandardDeviation); + } + + public double getDistanceToBestTagMeters() { + return inputs.distanceFromBestTag; + } + + /** + * If the robot is close enough to the tag, it calculates the pose using the solve PNP heading. + * If it's too far, it assumes the robot's heading from the gyro and calculates its position from there. + * Assuming the robot's heading from the gyro is more robust, but won't fix current wrong heading. + * To fix this, we use solve PNP to reset the gyro when we are close enough for an accurate result. + * + * @return the robot's pose + */ + private Pose2d calculateBestRobotPose() { + final Rotation2d gyroHeadingAtTimestamp = PoseEstimator6328.getInstance().samplePose(inputs.latestResultTimestampSeconds).getRotation(); + return calculateAssumedRobotHeadingPose(gyroHeadingAtTimestamp); + } + + /** + * Calculates the robot's pose by assuming its heading, the apriltag's position, and the camera's position on the robot. + * This method of pose estimation is more robust than solve PNP, but relies on knowing the robot's heading beforehand. + * + * @return the robot's pose + */ + private Pose2d calculateAssumedRobotHeadingPose(Rotation2d gyroHeading) { + if (inputs.visibleTagIDs.length == 0 || !inputs.hasResult) + return null; + + final Translation2d fieldRelativeRobotTranslation = getFieldRelativeRobotTranslation(gyroHeading); + + if (!isWithinBestTagRangeForSolvePNP()) + return new Pose2d(fieldRelativeRobotTranslation, gyroHeading); + + final Rotation2d solvePNPHeading = inputs.cameraSolvePNPPose.getRotation().toRotation2d().minus(robotCenterToCamera.getRotation().toRotation2d()); + return new Pose2d(fieldRelativeRobotTranslation, solvePNPHeading); + } + + private Translation2d getFieldRelativeRobotTranslation(Rotation2d gyroHeading) { + final Pose3d bestTagPose = FieldConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[0]).plus(AprilTagCameraConstants.TAG_OFFSET); + + final Translation2d tagRelativeCameraTranslation = calculateTagRelativeCameraTranslation(gyroHeading, bestTagPose); + final Translation2d fieldRelativeRobotPose = getFieldRelativeRobotPose(tagRelativeCameraTranslation, bestTagPose); + return new Pose2d(fieldRelativeRobotPose, gyroHeading.minus(robotCenterToCamera.getRotation().toRotation2d())).transformBy(toTransform2d(robotCenterToCamera.inverse())).getTranslation(); + } + + private Transform2d toTransform2d(Transform3d transform3d) { + return new Transform2d(transform3d.getTranslation().toTranslation2d(), transform3d.getRotation().toRotation2d()); + } + + private Translation2d calculateTagRelativeCameraTranslation(Rotation2d gyroHeading, Pose3d tagPose) { + final double robotPlaneTargetYawRadians = getRobotPlaneTargetYawRadians(); + final double robotPlaneCameraDistanceToUsedTagMeters = calculateRobotPlaneDistanceToTag(tagPose, robotPlaneTargetYawRadians); + var actualDist = PoseEstimator6328.getInstance().getOdometryPose().getTranslation().getDistance(tagPose.getTranslation().toTranslation2d()); + if (inputs.visibleTagIDs[0] == 7) + Logger.recordOutput("ActualPitch", Units.radiansToDegrees(calc(tagPose, robotPlaneTargetYawRadians, actualDist))); + final double headingOffsetToUsedTagRadians = gyroHeading.getRadians() - robotPlaneTargetYawRadians + robotCenterToCamera.getRotation().getZ(); + return new Translation2d(robotPlaneCameraDistanceToUsedTagMeters, Rotation2d.fromRadians(headingOffsetToUsedTagRadians)); + } + + + private double calc(Pose3d usedTagPose, double robotPlaneTargetYaw, double dist) { + double zDistanceToUsedTagMeters = Math.abs(usedTagPose.getZ() - robotCenterToCamera.getTranslation().getZ()); + var lol = dist * Math.cos(robotPlaneTargetYaw); + var lol2 = zDistanceToUsedTagMeters / lol; + var atan = Math.atan(lol2); + return atan - inputs.bestTargetRelativePitchRadians; + } + private double getRobotPlaneTargetYawRadians() { + double targetYawRadians = inputs.bestTargetRelativeYawRadians; + for (int i = 0; i < AprilTagCameraConstants.CALCULATE_YAW_ITERATIONS; i++) { + final double projectedPitch = projectToPlane(-robotCenterToCamera.getRotation().getY(), targetYawRadians + Math.PI / 2); + targetYawRadians = inputs.bestTargetRelativeYawRadians - Math.tan(projectedPitch) * inputs.bestTargetRelativePitchRadians; + } + return projectToPlane(targetYawRadians, robotCenterToCamera.getRotation().getY()); + } + + private double projectToPlane(double targetAngleRadians, double cameraAngleRadians) { + if (cameraAngleRadians < 0) + return Math.atan(Math.tan(targetAngleRadians) / Math.cos(cameraAngleRadians)); + return Math.atan(Math.tan(targetAngleRadians) * Math.cos(cameraAngleRadians)); + } + + private double calculateRobotPlaneDistanceToTag(Pose3d usedTagPose, double robotPlaneTargetYaw) { + double zDistanceToUsedTagMeters = Math.abs(usedTagPose.getZ() - robotCenterToCamera.getTranslation().getZ()); + double robotPlaneDistanceFromUsedTagMeters = zDistanceToUsedTagMeters / Math.tan(-robotCenterToCamera.getRotation().getY() + inputs.bestTargetRelativePitchRadians); + return robotPlaneDistanceFromUsedTagMeters / Math.cos(robotPlaneTargetYaw); + } + + private Translation2d getFieldRelativeRobotPose(Translation2d tagRelativeCameraTranslation, Pose3d tagPose) { + return tagPose.getTranslation().toTranslation2d().minus(tagRelativeCameraTranslation); + } + + private boolean isNewTimestamp() { + if (lastUpdatedTimestamp == getLatestResultTimestampSeconds()) + return false; + + lastUpdatedTimestamp = getLatestResultTimestampSeconds(); + return true; + } + + /** + * Calculates the standard deviation of the estimated pose using a formula. + * As we get further from the tag(s), this will return a less trusting (higher deviation) result. + * + * @param exponent a calibrated gain, different for each pose estimating strategy + * @param distance the distance from the tag(s) + * @param numberOfVisibleTags the number of visible tags + * @return the standard deviation + */ + private double calculateStandardDeviations(double exponent, double distance, int numberOfVisibleTags) { + return exponent * (distance * distance) / numberOfVisibleTags; + } + + private boolean isWithinBestTagRangeForSolvePNP() { + return inputs.distanceFromBestTag < AprilTagCameraConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS; + } + + private void logCameraInfo() { + if (!FieldConstants.TAG_ID_TO_POSE.isEmpty()) + logUsedTags(); + + if (inputs.hasResult && inputs.distanceFromBestTag != 0 && robotPose != null) { + logEstimatedRobotPose(); + logSolvePNPPose(); + } else { + Logger.recordOutput("Poses/Robot/" + name + "/Pose", AprilTagCameraConstants.EMPTY_POSE_LIST); + Logger.recordOutput("Poses/Robot/" + name + "/SolvePNPPose", new Pose3d[]{}); + } + } + + private void logUsedTags() { + if (!inputs.hasResult) { + Logger.recordOutput("UsedTags/" + this.getName(), new Pose3d[0]); + return; + } + + final Pose3d[] usedTagPoses = isWithinBestTagRangeForSolvePNP() ? new Pose3d[inputs.visibleTagIDs.length] : new Pose3d[1]; + for (int i = 0; i < usedTagPoses.length; i++) + usedTagPoses[i] = FieldConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[i]); + Logger.recordOutput("UsedTags/" + this.getName(), usedTagPoses); + } + + private void logEstimatedRobotPose() { + Logger.recordOutput("Poses/Robot/" + name + "/Pose", robotPose); + } + + private void logSolvePNPPose() { + Logger.recordOutput("Test/Rotation", new double[]{Units.radiansToDegrees(inputs.cameraSolvePNPPose.getRotation().getY()), Units.radiansToDegrees(inputs.cameraSolvePNPPose.getRotation().getZ())}); + Logger.recordOutput("Poses/Robot/" + name + "/SolvePNPPose", new Pose3d[]{inputs.cameraSolvePNPPose.plus(robotCenterToCamera.inverse())}); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java new file mode 100644 index 00000000..ce8eaf98 --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -0,0 +1,27 @@ +package frc.trigon.robot.poseestimation.apriltagcamera; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagLimelightIO; +import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagPhotonCameraIO; + +import java.util.function.Function; + +public class AprilTagCameraConstants { + public static final Transform3d TAG_OFFSET = new Transform3d(0, 0, 0, new Rotation3d(0, 0, 0)); + static final double MAXIMUM_DISTANCE_FROM_TAG_FOR_PNP_METERS = 1.5; + static final int CALCULATE_YAW_ITERATIONS = 5; + static final Pose2d[] EMPTY_POSE_LIST = new Pose2d[0]; + + public enum AprilTagCameraType { + PHOTON_CAMERA(AprilTagPhotonCameraIO::new), + LIMELIGHT(AprilTagLimelightIO::new); + + final Function createIOFunction; + + AprilTagCameraType(Function createIOFunction) { + this.createIOFunction = createIOFunction; + } + } +} diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java new file mode 100644 index 00000000..c9233bb2 --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java @@ -0,0 +1,20 @@ +package frc.trigon.robot.poseestimation.apriltagcamera; + +import edu.wpi.first.math.geometry.Pose3d; +import org.littletonrobotics.junction.AutoLog; + +public class AprilTagCameraIO { + protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { + } + + @AutoLog + public static class AprilTagCameraInputs { + public boolean hasResult = false; + public double latestResultTimestampSeconds = 0; + public Pose3d cameraSolvePNPPose = new Pose3d(); + public int[] visibleTagIDs = new int[0]; + public double bestTargetRelativeYawRadians = 0; + public double bestTargetRelativePitchRadians = 0; + public double distanceFromBestTag = 0; + } +} diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java new file mode 100644 index 00000000..065e8275 --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -0,0 +1,87 @@ +package frc.trigon.robot.poseestimation.apriltagcamera.io; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.util.Units; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged; +import org.trigon.utilities.LimelightHelpers; + +public class AprilTagLimelightIO extends AprilTagCameraIO { + private final String hostname; + + public AprilTagLimelightIO(String hostname) { + this.hostname = hostname; + } + + @Override + protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { + final LimelightHelpers.LimelightResults results = LimelightHelpers.getLatestResults(hostname); + + inputs.hasResult = results.targets_Fiducials.length > 0; + + if (inputs.hasResult) + updateHasResultInputs(inputs, results); + else + updateNoResultInputs(inputs); + } + + private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, LimelightHelpers.LimelightResults results) { + final Rotation3d bestTagRelativeRotation = getBestTargetRelativeRotation(results); + + inputs.cameraSolvePNPPose = results.getBotPose3d_wpiBlue(); + inputs.latestResultTimestampSeconds = results.timestamp_RIOFPGA_capture; + inputs.visibleTagIDs = getVisibleTagIDs(results); + inputs.bestTargetRelativeYawRadians = bestTagRelativeRotation.getZ(); + inputs.bestTargetRelativePitchRadians = bestTagRelativeRotation.getY(); + inputs.distanceFromBestTag = getDistanceFromBestTag(results); + } + + private void updateNoResultInputs(AprilTagCameraInputsAutoLogged inputs) { + inputs.visibleTagIDs = new int[0]; + inputs.cameraSolvePNPPose = new Pose3d(); + } + + private int[] getVisibleTagIDs(LimelightHelpers.LimelightResults results) { + final LimelightHelpers.LimelightTarget_Fiducial[] visibleTags = results.targets_Fiducials; + final int[] visibleTagIDs = new int[visibleTags.length]; + visibleTagIDs[0] = (int) getBestTarget(results).fiducialID; + int idAddition = 1; + + for (int i = 0; i < visibleTagIDs.length; i++) { + final int currentID = (int) visibleTags[i].fiducialID; + + if (currentID == visibleTagIDs[0]) { + idAddition = 0; + continue; + } + + visibleTagIDs[i + idAddition] = currentID; + } + return visibleTagIDs; + } + + /** + * Estimates the camera's rotation relative to the apriltag. + * + * @param results the camera's pipeline result + * @return the estimated rotation + */ + private Rotation3d getBestTargetRelativeRotation(LimelightHelpers.LimelightResults results) { + final LimelightHelpers.LimelightTarget_Fiducial bestTag = getBestTarget(results); + return new Rotation3d(0, -Units.degreesToRadians(bestTag.tx), -Units.degreesToRadians(bestTag.ty)); + } + + private double getDistanceFromBestTag(LimelightHelpers.LimelightResults results) { + return getDistanceFromTag((int) getBestTarget(results).fiducialID, results.getBotPose3d_wpiBlue()); + } + + private double getDistanceFromTag(int fiducialID, Pose3d estimatedRobotPose) { + return FieldConstants.TAG_ID_TO_POSE.get(fiducialID).getTranslation().getDistance(estimatedRobotPose.getTranslation()); + } + + private LimelightHelpers.LimelightTarget_Fiducial getBestTarget(LimelightHelpers.LimelightResults results) { + return results.targets_Fiducials[0]; + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java new file mode 100644 index 00000000..7218d3c0 --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -0,0 +1,194 @@ +package frc.trigon.robot.poseestimation.apriltagcamera.io; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; +import org.opencv.core.Point; +import org.photonvision.PhotonCamera; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.targeting.TargetCorner; + +import java.util.HashMap; +import java.util.List; + +public class AprilTagPhotonCameraIO extends AprilTagCameraIO { + private final PhotonCamera photonCamera; + + public AprilTagPhotonCameraIO(String cameraName) { + photonCamera = new PhotonCamera(cameraName); + } + + @Override + protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { + final PhotonPipelineResult latestResult = photonCamera.getLatestResult(); + + inputs.hasResult = latestResult.hasTargets() && !latestResult.getTargets().isEmpty() && getBestTarget(latestResult).getPoseAmbiguity() < 0.4; + if (inputs.hasResult) + updateHasResultInputs(inputs, latestResult); + else + updateNoResultInputs(inputs); + } + + private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, PhotonPipelineResult latestResult) { + logEstimatedTagHeights(latestResult); + final Rotation3d bestTargetRelativeRotation3d = getBestTargetRelativeRotation(latestResult); + + inputs.cameraSolvePNPPose = getSolvePNPPose(latestResult); + inputs.latestResultTimestampSeconds = latestResult.getTimestampSeconds(); + inputs.bestTargetRelativePitchRadians = bestTargetRelativeRotation3d.getY(); + inputs.bestTargetRelativeYawRadians = bestTargetRelativeRotation3d.getZ(); + inputs.visibleTagIDs = getVisibleTagIDs(latestResult); + inputs.distanceFromBestTag = getDistanceFromBestTag(latestResult); + } + + private void updateNoResultInputs(AprilTagCameraInputsAutoLogged inputs) { + inputs.visibleTagIDs = new int[]{}; + inputs.cameraSolvePNPPose = new Pose3d(); + } + + private Point getTagCenter(List tagCorners) { + double tagCornerSumX = 0; + double tagCornerSumY = 0; + for (TargetCorner tagCorner : tagCorners) { + tagCornerSumX += tagCorner.x; + tagCornerSumY += tagCorner.y; + } + return new Point(tagCornerSumX / tagCorners.size(), tagCornerSumY / tagCorners.size()); + } + + /** + * Tag ID to a pair of Height Difference and Distance + */ + private HashMap> thingLol = new HashMap<>(); + + private void logEstimatedTagHeights(PhotonPipelineResult result) { + for (PhotonTrackedTarget target : result.getTargets()) { + thingLol.putIfAbsent(target.getFiducialId(), new Pair<>(0.0, 10000000.0)); + if (target.getPoseAmbiguity() > 0.4) + continue; + var dist = target.getBestCameraToTarget().getTranslation().getNorm(); + if (thingLol.get(target.getFiducialId()).getSecond() < dist) + continue; + var camPose = FieldConstants.TAG_ID_TO_POSE.get(target.getFiducialId()).plus(target.getBestCameraToTarget().inverse()); + var diff = camPose.getY() - 0.63; + if (Math.abs(diff) > 0.2) + continue; + thingLol.put(target.getFiducialId(), new Pair<>(diff, dist)); + Logger.recordOutput("TagHeights/Tag" + target.getFiducialId() + "/Distance", dist); + Logger.recordOutput("TagHeights/Tag" + target.getFiducialId() + "/Diff", camPose.getY() - 0.63); + } + } + + LoggedDashboardNumber l = new LoggedDashboardNumber("Roll", 0); + + /** + * Estimates the camera's rotation relative to the apriltag. + * + * @param result the camera's pipeline result + * @return the estimated rotation + */ + private Rotation3d getBestTargetRelativeRotation(PhotonPipelineResult result) { +// final List tagCorners = getBestTarget(result).getDetectedCorners(); +// final Point tagCenter = getTagCenter(tagCorners); +// if (photonCamera.getCameraMatrix().isPresent()) +// return correctPixelRot(tagCenter, photonCamera.getCameraMatrix().get()); +// return null; + final PhotonTrackedTarget bestTarget = getBestTarget(result); + return new Rotation3d( + 0, + Units.degreesToRadians(bestTarget.getPitch()), + Units.degreesToRadians(bestTarget.getYaw()) + ); + } + + /** + * Estimates the camera's pose using Solve PNP using as many tags as possible. + * + * @param result the camera's pipeline result + * @return the estimated pose + */ + private Pose3d getSolvePNPPose(PhotonPipelineResult result) { +// final Pose3d rawTgPose = FieldConstants.TAG_ID_TO_POSE.get(4); +// final Pose3d rawTg1Pose = FieldConstants.TAG_ID_TO_POSE.get(3); +// PhotonTrackedTarget tag4 = null, tag3 = null; +// for (PhotonTrackedTarget re : result.getTargets()) { +// if (re.getFiducialId() == 4) +// tag4 = re; +// if (re.getFiducialId() == 3) +// tag3 = re; +// } +// var tag4Estimate = rawTgPose.transformBy(tag4.getBestCameraToTarget().inverse()); +// var tag3Estimate = rawTg1Pose.transformBy(tag3.getBestCameraToTarget().inverse()); +// Logger.recordOutput("Diff", tag4Estimate.minus(tag3Estimate)); + if (result.getMultiTagResult().isPresent()) { + final Transform3d cameraPoseTransform = result.getMultiTagResult().get().estimatedPose.best; + return new Pose3d().plus(cameraPoseTransform).relativeTo(FieldConstants.APRIL_TAG_FIELD_LAYOUT.getOrigin()); + } + + var best = getBestTarget(result); + final Pose3d rawTagPose = FieldConstants.TAG_ID_TO_POSE.get(best.getFiducialId()); + final Pose3d tagPose = rawTagPose.transformBy(AprilTagCameraConstants.TAG_OFFSET); + final Transform3d targetToCamera = best.getBestCameraToTarget().inverse(); + return tagPose.transformBy(targetToCamera); + } + + private int[] getVisibleTagIDs(PhotonPipelineResult result) { + return new int[]{getBestTarget(result).getFiducialId()}; +// final int[] visibleTagIDs = new int[result.getTargets().size()]; +// +// for (int i = 0; i < visibleTagIDs.length; i++) +// visibleTagIDs[i] = result.getTargets().get(i).getFiducialId(); +// return visibleTagIDs; + } + + private double getDistanceFromBestTag(PhotonPipelineResult result) { + return getBestTarget(result).getBestCameraToTarget().getTranslation().getNorm(); + } + + private PhotonTrackedTarget getBestTarget(PhotonPipelineResult result) { + PhotonTrackedTarget best = result.getBestTarget(); + if (result.getTargets().size() == 1) + return best; + var l = result.getTargets(); + if (best.getFiducialId() == 8 || best.getFiducialId() == 6) { + l.remove(best); + best = l.get(0); + } + for (PhotonTrackedTarget currentTarget : l) { + if (currentTarget.getFiducialId() == 8 || currentTarget.getFiducialId() == 6) + continue; + if (currentTarget.getArea() > best.getArea()) + best = currentTarget; + } + + return best; + } + + private Rotation3d correctPixelRot(Point pixel, Matrix camIntrinsics) { + double fx = camIntrinsics.get(0, 0); + double cx = camIntrinsics.get(0, 2); + double xOffset = cx - pixel.x; + + double fy = camIntrinsics.get(1, 1); + double cy = camIntrinsics.get(1, 2); + double yOffset = cy - pixel.y; + + // calculate yaw normally + var yaw = new Rotation2d(fx, xOffset); + // correct pitch based on yaw + var pitch = new Rotation2d(fy / Math.cos(Math.atan(xOffset / fx)), -yOffset); + var nonRolled = new Translation2d(-yaw.getRadians(), -pitch.getRadians()); + var rolled = nonRolled.rotateBy(Rotation2d.fromDegrees(l.get())); + + return new Rotation3d(0, rolled.getY(), rolled.getX()); + } +} diff --git a/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/EstimatedRobotPose.java b/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/EstimatedRobotPose.java deleted file mode 100644 index dcc605b7..00000000 --- a/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/EstimatedRobotPose.java +++ /dev/null @@ -1,72 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package frc.trigon.robot.poseestimation.photonposeestimator; - -import edu.wpi.first.math.geometry.Pose3d; -import org.photonvision.targeting.PhotonTrackedTarget; - -import java.util.List; - -/** - * An estimated pose based on pipeline result - */ -public class EstimatedRobotPose { - /** - * The estimated pose - */ - public final Pose3d estimatedPose; - - /** - * The estimated time the frame used to derive the robot pose was taken - */ - public final double timestampSeconds; - - /** - * A list of the targets used to compute this pose - */ - public final List targetsUsed; - - /** - * The strategy actually used to produce this pose - */ - public final PhotonPoseEstimator.PoseStrategy strategy; - - /** - * Constructs an EstimatedRobotPose - * - * @param estimatedPose estimated pose - * @param timestampSeconds timestamp of the estimate - */ - public EstimatedRobotPose( - Pose3d estimatedPose, - double timestampSeconds, - List targetsUsed, - PhotonPoseEstimator.PoseStrategy strategy) { - this.estimatedPose = estimatedPose; - this.timestampSeconds = timestampSeconds; - this.targetsUsed = targetsUsed; - this.strategy = strategy; - } -} diff --git a/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/PhotonPoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/PhotonPoseEstimator.java deleted file mode 100644 index 9c932c21..00000000 --- a/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/PhotonPoseEstimator.java +++ /dev/null @@ -1,791 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package frc.trigon.robot.poseestimation.photonposeestimator; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.hal.FRCNetComm.tResourceType; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.numbers.N5; -import edu.wpi.first.wpilibj.DriverStation; -import frc.trigon.robot.RobotContainer; -import org.photonvision.PhotonCamera; -import org.photonvision.estimation.TargetModel; -import org.photonvision.estimation.VisionEstimation; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; - -import java.util.HashSet; -import java.util.Optional; -import java.util.Set; - -/** - * The PhotonPoseEstimator class filters or combines readings from all the AprilTags visible at a - * given timestamp on the field to produce a single robot in field pose, using the strategy set - * below. Example usage can be found in our apriltagExample example project. - */ -public class PhotonPoseEstimator { - private static int InstanceCount = 0; - - /** - * Position estimation strategies that can be used by the {@link PhotonPoseEstimator} class. - */ - public enum PoseStrategy { - /** - * Choose the Pose with the lowest ambiguity. - */ - LOWEST_AMBIGUITY, - - /** - * Choose the Pose which is closest to the camera height. - */ - CLOSEST_TO_CAMERA_HEIGHT, - - /** - * Choose the Pose which is closest to a set Reference position. - */ - CLOSEST_TO_REFERENCE_POSE, - - /** - * Choose the Pose which is closest to the last pose calculated - */ - CLOSEST_TO_LAST_POSE, - - /** - * Return the average of the best target poses using ambiguity as weight. - */ - AVERAGE_BEST_TARGETS, - - /** - * Use all visible tags to compute a single pose estimate on coprocessor. This option needs to - * be enabled on the PhotonVision web UI as well. - */ - MULTI_TAG_PNP_ON_COPROCESSOR, - - /** - * Use all visible tags to compute a single pose estimate. This runs on the RoboRIO, and can - * take a lot of time. - */ - MULTI_TAG_PNP_ON_RIO, - - CLOSEST_TO_HEADING - } - - private AprilTagFieldLayout fieldTags; - private TargetModel tagModel = TargetModel.kAprilTag16h5; - private PoseStrategy primaryStrategy; - private PoseStrategy multiTagFallbackStrategy = PoseStrategy.LOWEST_AMBIGUITY; - private final PhotonCamera camera; - private Transform3d robotToCamera; - - private Pose3d lastPose; - private Pose3d referencePose; - protected double poseCacheTimestampSeconds = -1; - private final Set reportedErrors = new HashSet<>(); - - /** - * Create a new PhotonPoseEstimator. - * - * @param fieldTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs to Pose3d objects - * with respect to the FIRST field using the Field - * Coordinate System. Note that setting the origin of this layout object will affect the - * results from this class. - * @param strategy The strategy it should use to determine the best pose. - * @param camera PhotonCamera - * @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie, - * robot -> camera) in the Robot - * Coordinate System. - */ - public PhotonPoseEstimator( - AprilTagFieldLayout fieldTags, - PoseStrategy strategy, - PhotonCamera camera, - Transform3d robotToCamera) { - this.fieldTags = fieldTags; - this.primaryStrategy = strategy; - this.camera = camera; - this.robotToCamera = robotToCamera; - - HAL.report(tResourceType.kResourceType_PhotonPoseEstimator, InstanceCount); - InstanceCount++; - } - - public PhotonPoseEstimator( - AprilTagFieldLayout fieldTags, PoseStrategy strategy, Transform3d robotToCamera) { - this(fieldTags, strategy, null, robotToCamera); - } - - /** - * Invalidates the pose cache. - */ - private void invalidatePoseCache() { - poseCacheTimestampSeconds = -1; - } - - private void checkUpdate(Object oldObj, Object newObj) { - if (oldObj != newObj && oldObj != null && !oldObj.equals(newObj)) { - invalidatePoseCache(); - } - } - - /** - * Get the AprilTagFieldLayout being used by the PositionEstimator. - * - *

Note: Setting the origin of this layout will affect the results from this class. - * - * @return the AprilTagFieldLayout - */ - public AprilTagFieldLayout getFieldTags() { - return fieldTags; - } - - /** - * Set the AprilTagFieldLayout being used by the PositionEstimator. - * - *

Note: Setting the origin of this layout will affect the results from this class. - * - * @param fieldTags the AprilTagFieldLayout - */ - public void setFieldTags(AprilTagFieldLayout fieldTags) { - checkUpdate(this.fieldTags, fieldTags); - this.fieldTags = fieldTags; - } - - /** - * Get the TargetModel representing the tags being detected. This is used for on-rio multitag. - * - *

By default, this is {@link TargetModel#kAprilTag16h5}. - */ - public TargetModel getTagModel() { - return tagModel; - } - - /** - * Set the TargetModel representing the tags being detected. This is used for on-rio multitag. - * - * @param tagModel E.g. {@link TargetModel#kAprilTag16h5}. - */ - public void setTagModel(TargetModel tagModel) { - this.tagModel = tagModel; - } - - /** - * Get the Position Estimation Strategy being used by the Position Estimator. - * - * @return the strategy - */ - public PoseStrategy getPrimaryStrategy() { - return primaryStrategy; - } - - /** - * Set the Position Estimation Strategy used by the Position Estimator. - * - * @param strategy the strategy to set - */ - public void setPrimaryStrategy(PoseStrategy strategy) { - checkUpdate(this.primaryStrategy, strategy); - this.primaryStrategy = strategy; - } - - /** - * Set the Position Estimation Strategy used in multi-tag mode when only one tag can be seen. Must - * NOT be MULTI_TAG_PNP - * - * @param strategy the strategy to set - */ - public void setMultiTagFallbackStrategy(PoseStrategy strategy) { - checkUpdate(this.multiTagFallbackStrategy, strategy); - if (strategy == PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR - || strategy == PoseStrategy.MULTI_TAG_PNP_ON_RIO) { - DriverStation.reportWarning( - "Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity", false); - strategy = PoseStrategy.LOWEST_AMBIGUITY; - } - this.multiTagFallbackStrategy = strategy; - } - - /** - * Return the reference position that is being used by the estimator. - * - * @return the referencePose - */ - public Pose3d getReferencePose() { - return referencePose; - } - - /** - * Update the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE - * strategy. - * - * @param referencePose the referencePose to set - */ - public void setReferencePose(Pose3d referencePose) { - checkUpdate(this.referencePose, referencePose); - this.referencePose = referencePose; - } - - /** - * Update the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE - * strategy. - * - * @param referencePose the referencePose to set - */ - public void setReferencePose(Pose2d referencePose) { - setReferencePose(new Pose3d(referencePose)); - } - - /** - * Update the stored last pose. Useful for setting the initial estimate when using the - * CLOSEST_TO_LAST_POSE strategy. - * - * @param lastPose the lastPose to set - */ - public void setLastPose(Pose3d lastPose) { - this.lastPose = lastPose; - } - - /** - * Update the stored last pose. Useful for setting the initial estimate when using the - * CLOSEST_TO_LAST_POSE strategy. - * - * @param lastPose the lastPose to set - */ - public void setLastPose(Pose2d lastPose) { - setLastPose(new Pose3d(lastPose)); - } - - /** - * @return The current transform from the center of the robot to the camera mount position - */ - public Transform3d getRobotToCameraTransform() { - return robotToCamera; - } - - /** - * Useful for pan and tilt mechanisms and such. - * - * @param robotToCamera The current transform from the center of the robot to the camera mount - * position - */ - public void setRobotToCameraTransform(Transform3d robotToCamera) { - this.robotToCamera = robotToCamera; - } - - /** - * Poll data from the configured cameras and update the estimated position of the robot. Returns - * empty if: - * - *

    - *
  • New data has not been received since the last call to {@code update()}. - *
  • No targets were found from the camera - *
  • There is no camera set - *
- * - * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - */ - public Optional update() { - if (camera == null) { - DriverStation.reportError("[PhotonPoseEstimator] Missing camera!", false); - return Optional.empty(); - } - - PhotonPipelineResult cameraResult = camera.getLatestResult(); - - return update(cameraResult, camera.getCameraMatrix(), camera.getDistCoeffs()); - } - - /** - * Updates the estimated position of the robot. Returns empty if: - * - *
    - *
  • The timestamp of the provided pipeline result is the same as in the previous call to - * {@code update()}. - *
  • No targets were found in the pipeline results. - *
- * - * @param cameraResult The latest pipeline result from the camera - * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - */ - public Optional update(PhotonPipelineResult cameraResult) { - if (camera == null) return update(cameraResult, Optional.empty(), Optional.empty()); - return update(cameraResult, camera.getCameraMatrix(), camera.getDistCoeffs()); - } - - /** - * Updates the estimated position of the robot. Returns empty if: - * - *
    - *
  • The timestamp of the provided pipeline result is the same as in the previous call to - * {@code update()}. - *
  • No targets were found in the pipeline results. - *
- * - * @param cameraMatrix Camera calibration data that can be used in the case of no assigned - * PhotonCamera. - * @param distCoeffs Camera calibration data that can be used in the case of no assigned - * PhotonCamera - * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - */ - public Optional update( - PhotonPipelineResult cameraResult, - Optional> cameraMatrix, - Optional> distCoeffs) { - // Time in the past -- give up, since the following if expects times > 0 - if (cameraResult.getTimestampSeconds() < 0) { - return Optional.empty(); - } - - // If the pose cache timestamp was set, and the result is from the same - // timestamp, return an - // empty result -// - - // Remember the timestamp of the current result used - poseCacheTimestampSeconds = cameraResult.getTimestampSeconds(); - - // If no targets seen, trivial case -- return empty result - if (!cameraResult.hasTargets()) { - return Optional.empty(); - } - - return update(cameraResult, cameraMatrix, distCoeffs, this.primaryStrategy); - } - - private Optional update( - PhotonPipelineResult cameraResult, - Optional> cameraMatrix, - Optional> distCoeffs, - PoseStrategy strat) { - Optional estimatedPose; - switch (strat) { - case LOWEST_AMBIGUITY: - estimatedPose = lowestAmbiguityStrategy(cameraResult); - break; - case CLOSEST_TO_CAMERA_HEIGHT: - estimatedPose = closestToCameraHeightStrategy(cameraResult); - break; - case CLOSEST_TO_REFERENCE_POSE: - estimatedPose = closestToReferencePoseStrategy(cameraResult, referencePose); - break; - case CLOSEST_TO_LAST_POSE: - setReferencePose(lastPose); - estimatedPose = closestToReferencePoseStrategy(cameraResult, referencePose); - break; - case AVERAGE_BEST_TARGETS: - estimatedPose = averageBestTargetsStrategy(cameraResult); - break; - case MULTI_TAG_PNP_ON_RIO: - estimatedPose = multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs); - break; - case MULTI_TAG_PNP_ON_COPROCESSOR: - estimatedPose = multiTagOnCoprocStrategy(cameraResult, cameraMatrix, distCoeffs); - break; - case CLOSEST_TO_HEADING: - estimatedPose = closestToHeadingStrategy(cameraResult); - break; - default: - DriverStation.reportError( - "[PhotonPoseEstimator] Unknown Position Estimation Strategy!", false); - return Optional.empty(); - } - - if (estimatedPose.isPresent()) { - lastPose = estimatedPose.get().estimatedPose; - } - - return estimatedPose; - } - - private Optional closestToHeadingStrategy(PhotonPipelineResult result) { - double smallestAngleDifferenceRadians; - EstimatedRobotPose closestAngleTarget; - double currentHeadingRadians = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation().getRadians(); - - PhotonTrackedTarget target = result.getBestTarget(); - int targetFiducialId = target.getFiducialId(); - - // Don't report errors for non-fiducial targets. This could also be resolved by - // adding -1 to - // the initial HashSet. - if (targetFiducialId == -1) return Optional.empty(); - - Optional targetPosition = fieldTags.getTagPose(target.getFiducialId()); - - if (targetPosition.isEmpty()) { - reportFiducialPoseError(target.getFiducialId()); - return Optional.empty(); - } - - Transform3d bestCameraToTarget = target.getBestCameraToTarget(); - - Pose3d bestPose = - targetPosition - .get() - .transformBy(bestCameraToTarget.inverse()) - .transformBy(robotToCamera.inverse()); - double bestTransformAngle = bestPose.getRotation().getZ(); - smallestAngleDifferenceRadians = Math.abs(currentHeadingRadians - bestTransformAngle); - closestAngleTarget = - new EstimatedRobotPose( - bestPose, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.CLOSEST_TO_HEADING); - - Transform3d alternateCameraToTarget = target.getAlternateCameraToTarget(); - if (alternateCameraToTarget.getRotation().getZ() != 0) { - Pose3d altPose = - targetPosition - .get() - .transformBy(target.getAlternateCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()); - double alternateTransformAngle = altPose.getRotation().getZ(); - double alternateTransformDelta = Math.abs(currentHeadingRadians - alternateTransformAngle); - - if (alternateTransformDelta < smallestAngleDifferenceRadians) { - closestAngleTarget = - new EstimatedRobotPose( - altPose, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.CLOSEST_TO_HEADING); - } - } - - // Need to null check here in case none of the provided targets are fiducial. - return Optional.of(closestAngleTarget); - } - - private Optional multiTagOnCoprocStrategy( - PhotonPipelineResult result, - Optional> cameraMatrixOpt, - Optional> distCoeffsOpt) { - if (result.getMultiTagResult().estimatedPose.isPresent) { - var best_tf = result.getMultiTagResult().estimatedPose.best; - var cam = new Pose3d() - .plus(best_tf) // field-to-camera - .relativeTo(fieldTags.getOrigin()); - var best = cam.plus(robotToCamera.inverse()); // field-to-robot -// Logger.recordOutput(camera.getName(), Math.toDegrees(cam.getRotation().getY())); - - return Optional.of( - new EstimatedRobotPose( - best, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR)); - } else { - return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); - } - } - - private Optional multiTagOnRioStrategy( - PhotonPipelineResult result, - Optional> cameraMatrixOpt, - Optional> distCoeffsOpt) { - boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent(); - // cannot run multitagPNP, use fallback strategy - if (!hasCalibData || result.getTargets().size() < 2) { - return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); - } - - var pnpResult = - VisionEstimation.estimateCamPosePNP( - cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags, tagModel); - // try fallback strategy if solvePNP fails for some reason - if (!pnpResult.isPresent) - return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); - var best = - new Pose3d() - .plus(pnpResult.best) // field-to-camera - .plus(robotToCamera.inverse()); // field-to-robot - - return Optional.of( - new EstimatedRobotPose( - best, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.MULTI_TAG_PNP_ON_RIO)); - } - - /** - * Return the estimated position of the robot with the lowest position ambiguity from a List of - * pipeline results. - * - * @param result pipeline result - * @return the estimated position of the robot in the FCS and the estimated timestamp of this - * estimation. - */ - private Optional lowestAmbiguityStrategy(PhotonPipelineResult result) { - PhotonTrackedTarget lowestAmbiguityTarget = null; - - double lowestAmbiguityScore = 10; - - for (PhotonTrackedTarget target : result.targets) { - double targetPoseAmbiguity = target.getPoseAmbiguity(); - // Make sure the target is a Fiducial target. - if (targetPoseAmbiguity != -1 && targetPoseAmbiguity < lowestAmbiguityScore) { - lowestAmbiguityScore = targetPoseAmbiguity; - lowestAmbiguityTarget = target; - } - } - - // Although there are confirmed to be targets, none of them may be fiducial - // targets. - if (lowestAmbiguityTarget == null) return Optional.empty(); - - int targetFiducialId = lowestAmbiguityTarget.getFiducialId(); - - Optional targetPosition = fieldTags.getTagPose(targetFiducialId); - - if (targetPosition.isEmpty()) { - reportFiducialPoseError(targetFiducialId); - return Optional.empty(); - } - - return Optional.of( - new EstimatedRobotPose( - targetPosition - .get() - .transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.LOWEST_AMBIGUITY)); - } - - /** - * Return the estimated position of the robot using the target with the lowest delta height - * difference between the estimated and actual height of the camera. - * - * @param result pipeline result - * @return the estimated position of the robot in the FCS and the estimated timestamp of this - * estimation. - */ - private Optional closestToCameraHeightStrategy(PhotonPipelineResult result) { - double smallestHeightDifference = 10e9; - EstimatedRobotPose closestHeightTarget = null; - - for (PhotonTrackedTarget target : result.targets) { - int targetFiducialId = target.getFiducialId(); - - // Don't report errors for non-fiducial targets. This could also be resolved by - // adding -1 to - // the initial HashSet. - if (targetFiducialId == -1) continue; - - Optional targetPosition = fieldTags.getTagPose(target.getFiducialId()); - - if (targetPosition.isEmpty()) { - reportFiducialPoseError(target.getFiducialId()); - continue; - } - - - double alternateTransformDelta = - Math.abs( - robotToCamera.getZ() - - targetPosition - .get() - .transformBy(target.getAlternateCameraToTarget().inverse()) - .getZ()); - double bestTransformDelta = - Math.abs( - robotToCamera.getZ() - - targetPosition - .get() - .transformBy(target.getBestCameraToTarget().inverse()) - .getZ()); - - if (alternateTransformDelta < smallestHeightDifference) { - smallestHeightDifference = alternateTransformDelta; - closestHeightTarget = - new EstimatedRobotPose( - targetPosition - .get() - .transformBy(target.getAlternateCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT); - } - - if (bestTransformDelta < smallestHeightDifference) { - smallestHeightDifference = bestTransformDelta; - closestHeightTarget = - new EstimatedRobotPose( - targetPosition - .get() - .transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT); - } - } - - // Need to null check here in case none of the provided targets are fiducial. - return Optional.ofNullable(closestHeightTarget); - } - - /** - * Return the estimated position of the robot using the target with the lowest delta in the vector - * magnitude between it and the reference pose. - * - * @param result pipeline result - * @param referencePose reference pose to check vector magnitude difference against. - * @return the estimated position of the robot in the FCS and the estimated timestamp of this - * estimation. - */ - private Optional closestToReferencePoseStrategy( - PhotonPipelineResult result, Pose3d referencePose) { - if (referencePose == null) { -// DriverStation.reportError( -// "[PhotonPoseEstimator] Tried to use reference pose strategy without setting the reference!", -// false); - return Optional.empty(); - } - - double smallestPoseDelta = 10e9; - EstimatedRobotPose lowestDeltaPose = null; - - for (PhotonTrackedTarget target : result.targets) { - int targetFiducialId = target.getFiducialId(); - - // Don't report errors for non-fiducial targets. This could also be resolved by - // adding -1 to - // the initial HashSet. - if (targetFiducialId == -1) continue; - - Optional targetPosition = fieldTags.getTagPose(target.getFiducialId()); - - if (targetPosition.isEmpty()) { - reportFiducialPoseError(targetFiducialId); - continue; - } - - Pose3d altTransformPosition = - targetPosition - .get() - .transformBy(target.getAlternateCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()); - Pose3d bestTransformPosition = - targetPosition - .get() - .transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()); - - double altDifference = Math.abs(calculateDifference(referencePose, altTransformPosition)); - double bestDifference = Math.abs(calculateDifference(referencePose, bestTransformPosition)); - - if (altDifference < smallestPoseDelta) { - smallestPoseDelta = altDifference; - lowestDeltaPose = - new EstimatedRobotPose( - altTransformPosition, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.CLOSEST_TO_REFERENCE_POSE); - } - if (bestDifference < smallestPoseDelta) { - smallestPoseDelta = bestDifference; - lowestDeltaPose = - new EstimatedRobotPose( - bestTransformPosition, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.CLOSEST_TO_REFERENCE_POSE); - } - } - return Optional.ofNullable(lowestDeltaPose); - } - - /** - * Return the average of the best target poses using ambiguity as weight. - * - * @param result pipeline result - * @return the estimated position of the robot in the FCS and the estimated timestamp of this - * estimation. - */ - private Optional averageBestTargetsStrategy(PhotonPipelineResult result) { - if (!result.hasTargets()) - return Optional.empty(); - final PhotonTrackedTarget target = result.getBestTarget(); - int targetFiducialId = target.getFiducialId(); - - // Don't report errors for non-fiducial targets. This could also be resolved by - // adding -1 to - // the initial HashSet. - if (targetFiducialId == -1) return Optional.empty(); - - Optional targetPosition = fieldTags.getTagPose(target.getFiducialId()); - - if (targetPosition.isEmpty()) { - reportFiducialPoseError(targetFiducialId); - return Optional.empty(); - } - - final Pose3d cameraPose = targetPosition.get().transformBy(target.getBestCameraToTarget().inverse()); -// Logger.recordOutput(camera.getName(), Math.toDegrees(cameraPose.getRotation().getY())); - final Pose3d robotPose = cameraPose.transformBy(robotToCamera.inverse()); - - return Optional.of( - new EstimatedRobotPose( - robotPose, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.AVERAGE_BEST_TARGETS - ) - ); - } - - /** - * Difference is defined as the vector magnitude between the two poses - * - * @return The absolute "difference" (>=0) between two Pose3ds. - */ - private double calculateDifference(Pose3d x, Pose3d y) { - return x.getTranslation().getDistance(y.getTranslation()); - } - - private void reportFiducialPoseError(int fiducialId) { - if (!reportedErrors.contains(fiducialId)) { - DriverStation.reportError( - "[PhotonPoseEstimator] Tried to get pose of unknown AprilTag: " + fiducialId, false); - reportedErrors.add(fiducialId); - } - } -} diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index 1cc1deec..4b3447a5 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -1,19 +1,15 @@ package frc.trigon.robot.poseestimation.poseestimator; import com.pathplanner.lib.util.PathPlannerLogging; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveDriveWheelPositions; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.poseestimation.robotposesources.RobotPoseSource; -import frc.trigon.robot.poseestimation.robotposesources.RobotPoseSourceConstants; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCamera; import org.littletonrobotics.junction.Logger; import java.util.ArrayList; @@ -26,16 +22,16 @@ */ public class PoseEstimator implements AutoCloseable { private final Field2d field = new Field2d(); - private final RobotPoseSource[] robotPoseSources; + private final AprilTagCamera[] aprilTagCameras; private final PoseEstimator6328 poseEstimator6328 = PoseEstimator6328.getInstance(); /** * Constructs a new PoseEstimator. * - * @param robotPoseSources the sources that should update the pose estimator apart from the odometry. This should be cameras etc. + * @param aprilTagCameras the sources that should update the pose estimator apart from the odometry. This should be cameras etc. */ - public PoseEstimator(RobotPoseSource... robotPoseSources) { - this.robotPoseSources = robotPoseSources; + public PoseEstimator(AprilTagCamera... aprilTagCameras) { + this.aprilTagCameras = aprilTagCameras; putAprilTagsOnFieldWidget(); SmartDashboard.putData("Field", field); PathPlannerLogging.setLogActivePathCallback((pose) -> { @@ -84,6 +80,17 @@ public void updatePoseEstimatorStates(SwerveDriveWheelPositions[] swerveWheelPos poseEstimator6328.addOdometryObservation(new PoseEstimator6328.OdometryObservation(swerveWheelPositions[i], gyroRotations[i], timestamps[i])); } + public void setGyroHeadingToBestSolvePNPHeading() { + int closestCameraToTag = 0; + for (int i = 0; i < aprilTagCameras.length; i++) { + if (aprilTagCameras[i].getDistanceToBestTagMeters() < aprilTagCameras[closestCameraToTag].getDistanceToBestTagMeters()) + closestCameraToTag = i; + } + + final Rotation2d bestRobotHeading = aprilTagCameras[closestCameraToTag].getSolvePNPHeading(); + resetPose(new Pose2d(getCurrentPose().getTranslation(), bestRobotHeading)); + } + private void updateFromVision() { getViableVisionObservations().stream() .sorted(Comparator.comparingDouble(PoseEstimator6328.VisionObservation::timestamp)) @@ -92,38 +99,31 @@ private void updateFromVision() { private List getViableVisionObservations() { List viableVisionObservations = new ArrayList<>(); - for (RobotPoseSource robotPoseSource : robotPoseSources) { - final PoseEstimator6328.VisionObservation visionObservation = getVisionObservation(robotPoseSource); + for (AprilTagCamera aprilTagCamera : aprilTagCameras) { + final PoseEstimator6328.VisionObservation visionObservation = getVisionObservation(aprilTagCamera); if (visionObservation != null) viableVisionObservations.add(visionObservation); } return viableVisionObservations; } - private PoseEstimator6328.VisionObservation getVisionObservation(RobotPoseSource robotPoseSource) { - robotPoseSource.update(); - if (!robotPoseSource.hasNewResult()) + private PoseEstimator6328.VisionObservation getVisionObservation(AprilTagCamera aprilTagCamera) { + aprilTagCamera.update(); + if (!aprilTagCamera.hasNewResult()) return null; - final Pose2d robotPose = robotPoseSource.getRobotPose(); + final Pose2d robotPose = aprilTagCamera.getEstimatedRobotPose(); if (robotPose == null) return null; return new PoseEstimator6328.VisionObservation( robotPose, - robotPoseSource.getLastResultTimestamp(), - averageDistanceToStdDevs(robotPoseSource.getAverageDistanceFromTags(), robotPoseSource.getVisibleTags()) + aprilTagCamera.getLatestResultTimestampSeconds(), + aprilTagCamera.calculateStandardDeviations() ); } - private Matrix averageDistanceToStdDevs(double averageDistance, int visibleTags) { - final double translationStd = PoseEstimatorConstants.TRANSLATIONS_STD_EXPONENT * Math.pow(averageDistance, 2) / (visibleTags * visibleTags); - final double thetaStd = PoseEstimatorConstants.THETA_STD_EXPONENT * Math.pow(averageDistance, 2) / visibleTags; - - return VecBuilder.fill(translationStd, translationStd, thetaStd); - } - private void putAprilTagsOnFieldWidget() { - for (Map.Entry entry : RobotPoseSourceConstants.TAG_ID_TO_POSE.entrySet()) { + for (Map.Entry entry : FieldConstants.TAG_ID_TO_POSE.entrySet()) { final Pose2d tagPose = entry.getValue().toPose2d(); field.getObject("Tag " + entry.getKey()).setPose(tagPose); } diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java index 6819d8ea..4ada5453 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java @@ -19,6 +19,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.DriverStation; import frc.trigon.robot.subsystems.swerve.SwerveConstants; import org.littletonrobotics.junction.AutoLogOutput; @@ -89,6 +90,8 @@ public void addOdometryObservation(OdometryObservation observation) { } public void addVisionObservation(VisionObservation observation) { + if (DriverStation.isAutonomous() || DriverStation.isDisabled()) + return; // If measurement is old enough to be outside the pose buffer's timespan, skip. try { if (poseBuffer.getInternalBuffer().lastKey() - poseBufferSizeSeconds @@ -145,6 +148,13 @@ public void addVisionObservation(VisionObservation observation) { estimatedPose = estimateAtTime.plus(scaledTransform).plus(sampleToOdometryTransform); } + public Pose2d samplePose(double timestamp) { + Pose2d sample = poseBuffer.getSample(timestamp).orElse(new Pose2d()); + Transform2d odometryToSampleTransform = new Transform2d(odometryPose, sample); + + return estimatedPose.plus(odometryToSampleTransform); + } + /** * Reset estimated pose and odometry pose to pose
* Clear pose buffer diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java index a87aa90e..d8967b54 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java @@ -5,7 +5,7 @@ import edu.wpi.first.math.numbers.N3; public class PoseEstimatorConstants { - public static final double ODOMETRY_FREQUENCY_HERTZ = 50; + public static final double ODOMETRY_FREQUENCY_HERTZ = 250; /** * The vector represents how ambiguous each value of the odometry is. @@ -14,9 +14,5 @@ public class PoseEstimatorConstants { * Increase these numbers to trust the estimate less. */ static final Vector ODOMETRY_AMBIGUITY = VecBuilder.fill(0.003, 0.003, 0.0002); - - static final double - TRANSLATIONS_STD_EXPONENT = 0.005, - THETA_STD_EXPONENT = 0.01; } diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagLimelightIO.java deleted file mode 100644 index aecd3b22..00000000 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagLimelightIO.java +++ /dev/null @@ -1,21 +0,0 @@ -package frc.trigon.robot.poseestimation.robotposesources; - -import org.trigon.utilities.LimelightHelpers; - -public class AprilTagLimelightIO extends RobotPoseSourceIO { - private final String hostname; - - protected AprilTagLimelightIO(String hostname) { - this.hostname = hostname; - } - - @Override - protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { - inputs.hasResult = LimelightHelpers.getTV(hostname); - if (inputs.hasResult) { - final LimelightHelpers.Results results = LimelightHelpers.getLatestResults(hostname).targetingResults; - inputs.cameraPose = results.getBotPose3d_wpiBlue(); - inputs.lastResultTimestamp = results.timestamp_LIMELIGHT_publish; - } - } -} diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java deleted file mode 100644 index 3e40add5..00000000 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java +++ /dev/null @@ -1,91 +0,0 @@ -package frc.trigon.robot.poseestimation.robotposesources; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; -import frc.trigon.robot.poseestimation.photonposeestimator.EstimatedRobotPose; -import frc.trigon.robot.poseestimation.photonposeestimator.PhotonPoseEstimator; -import org.littletonrobotics.junction.Logger; -import org.photonvision.PhotonCamera; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; - -import java.util.List; -import java.util.Optional; - -public class AprilTagPhotonCameraIO extends RobotPoseSourceIO { - private final PhotonCamera photonCamera; - private final PhotonPoseEstimator photonPoseEstimator; - - protected AprilTagPhotonCameraIO(String cameraName, Transform3d robotCenterToCamera) { - photonCamera = new PhotonCamera(cameraName); - - photonPoseEstimator = new PhotonPoseEstimator( - RobotPoseSourceConstants.APRIL_TAG_FIELD_LAYOUT, - RobotPoseSourceConstants.PRIMARY_POSE_STRATEGY, - photonCamera, - robotCenterToCamera - ); - - photonPoseEstimator.setMultiTagFallbackStrategy(RobotPoseSourceConstants.SECONDARY_POSE_STRATEGY); - } - - @Override - protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { - final PhotonPipelineResult latestResult = photonCamera.getLatestResult(); - Optional optionalEstimatedRobotPose = photonPoseEstimator.update(latestResult); - - inputs.hasResult = hasResult(optionalEstimatedRobotPose); - if (inputs.hasResult) { - final EstimatedRobotPose estimatedRobotPose = optionalEstimatedRobotPose.get(); - inputs.cameraPose = estimatedRobotPose.estimatedPose; - inputs.lastResultTimestamp = estimatedRobotPose.timestampSeconds; - inputs.visibleTags = estimatedRobotPose.targetsUsed.size(); - inputs.averageDistanceFromTags = getAverageDistanceFromTags(latestResult); - } else { - inputs.visibleTags = 0; - inputs.cameraPose = new Pose3d(); - } - - logVisibleTags(inputs.hasResult, optionalEstimatedRobotPose); - } - - private void logVisibleTags(boolean hasResult, Optional optionalEstimatedRobotPose) { - if (!hasResult) { - Logger.recordOutput("VisibleTags/" + photonCamera.getName(), new Pose2d[0]); - return; - } - - final EstimatedRobotPose estimatedRobotPose = optionalEstimatedRobotPose.get(); - final Pose2d[] visibleTagPoses = new Pose2d[estimatedRobotPose.targetsUsed.size()]; - for (int i = 0; i < visibleTagPoses.length; i++) { - final int currentId = estimatedRobotPose.targetsUsed.get(i).getFiducialId(); - final Pose2d currentPose = RobotPoseSourceConstants.TAG_ID_TO_POSE.get(currentId).toPose2d(); - visibleTagPoses[i] = currentPose; - } - Logger.recordOutput("VisibleTags/" + photonCamera.getName(), visibleTagPoses); - } - - private boolean hasResult(Optional optionalEstimatedRobotPose) { - final boolean isEmpty = optionalEstimatedRobotPose.isEmpty(); - if (isEmpty) - return false; - final EstimatedRobotPose estimatedRobotPose = optionalEstimatedRobotPose.get(); - if (estimatedRobotPose.strategy == PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR) - return true; - return estimatedRobotPose.targetsUsed.get(0).getPoseAmbiguity() < RobotPoseSourceConstants.MAXIMUM_AMBIGUITY; - } - - private double getAverageDistanceFromTags(PhotonPipelineResult result) { - final List targets = result.targets; - double distanceSum = 0; - - for (PhotonTrackedTarget currentTarget : targets) { - final Translation2d distanceTranslation = currentTarget.getBestCameraToTarget().getTranslation().toTranslation2d(); - distanceSum += distanceTranslation.getNorm(); - } - - return distanceSum / targets.size(); - } -} diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java deleted file mode 100644 index 28a3fbd8..00000000 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java +++ /dev/null @@ -1,82 +0,0 @@ -package frc.trigon.robot.poseestimation.robotposesources; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Transform3d; -import frc.trigon.robot.Robot; -import org.littletonrobotics.junction.Logger; - -/** - * A pose source is a class that provides the robot's pose, from a camera. - */ -public class RobotPoseSource { - protected final String name; - private final RobotPoseSourceInputsAutoLogged inputs = new RobotPoseSourceInputsAutoLogged(); - private final Transform3d robotCenterToCamera; - private final RobotPoseSourceIO robotPoseSourceIO; - private double lastUpdatedTimestamp; - private Pose2d cachedPose = null; - - public RobotPoseSource(RobotPoseSourceConstants.RobotPoseSourceType robotPoseSourceType, String name, Transform3d robotCenterToCamera) { - this.name = name; - if (robotPoseSourceType != RobotPoseSourceConstants.RobotPoseSourceType.PHOTON_CAMERA) - this.robotCenterToCamera = robotCenterToCamera; - else - this.robotCenterToCamera = new Transform3d(); - - if (Robot.IS_REAL) - robotPoseSourceIO = robotPoseSourceType.createIOFunction.apply(name, robotCenterToCamera); - else - robotPoseSourceIO = new RobotPoseSourceIO(); - } - - public void update() { - robotPoseSourceIO.updateInputs(inputs); - Logger.processInputs("Cameras/" + name, inputs); - cachedPose = getUnCachedRobotPose(); - if (!inputs.hasResult || inputs.averageDistanceFromTags == 0 || cachedPose == null) - Logger.recordOutput("Poses/Robot/" + name + "Pose", RobotPoseSourceConstants.EMPTY_POSE_LIST); - else - Logger.recordOutput("Poses/Robot/" + name + "Pose", cachedPose); - } - - public int getVisibleTags() { - return inputs.visibleTags; - } - - public double getAverageDistanceFromTags() { - return inputs.averageDistanceFromTags; - } - - public boolean hasNewResult() { - return (inputs.hasResult && inputs.averageDistanceFromTags != 0) && isNewTimestamp(); - } - - public Pose2d getRobotPose() { - return cachedPose; - } - - public String getName() { - return name; - } - - public double getLastResultTimestamp() { - return inputs.lastResultTimestamp; - } - - private Pose2d getUnCachedRobotPose() { - final Pose3d cameraPose = inputs.cameraPose; - if (cameraPose == null) - return null; - - return cameraPose.transformBy(robotCenterToCamera.inverse()).toPose2d(); - } - - private boolean isNewTimestamp() { - if (lastUpdatedTimestamp == getLastResultTimestamp()) - return false; - - lastUpdatedTimestamp = getLastResultTimestamp(); - return true; - } -} diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceConstants.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceConstants.java deleted file mode 100644 index 8303aae5..00000000 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceConstants.java +++ /dev/null @@ -1,40 +0,0 @@ -package frc.trigon.robot.poseestimation.robotposesources; - -import edu.wpi.first.apriltag.AprilTag; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Transform3d; -import frc.trigon.robot.poseestimation.photonposeestimator.PhotonPoseEstimator; - -import java.util.HashMap; -import java.util.function.BiFunction; - -public class RobotPoseSourceConstants { - public static final HashMap TAG_ID_TO_POSE = new HashMap<>(); - static final PhotonPoseEstimator.PoseStrategy - PRIMARY_POSE_STRATEGY = PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - SECONDARY_POSE_STRATEGY = PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_HEADING; - static AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); - - static { - for (AprilTag aprilTag : APRIL_TAG_FIELD_LAYOUT.getTags()) - TAG_ID_TO_POSE.put(aprilTag.ID, aprilTag.pose); - } - - static final double MAXIMUM_AMBIGUITY = 0.2; - static final Pose2d[] EMPTY_POSE_LIST = new Pose2d[0]; - - public enum RobotPoseSourceType { - PHOTON_CAMERA(AprilTagPhotonCameraIO::new), - LIMELIGHT((name, transform3d) -> new AprilTagLimelightIO(name)), - T265((name, transform3d) -> new T265IO(name)); - - final BiFunction createIOFunction; - - RobotPoseSourceType(BiFunction createIOFunction) { - this.createIOFunction = createIOFunction; - } - } -} diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java deleted file mode 100644 index a37f35d6..00000000 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java +++ /dev/null @@ -1,18 +0,0 @@ -package frc.trigon.robot.poseestimation.robotposesources; - -import edu.wpi.first.math.geometry.Pose3d; -import org.littletonrobotics.junction.AutoLog; - -public class RobotPoseSourceIO { - protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { - } - - @AutoLog - public static class RobotPoseSourceInputs { - public boolean hasResult = false; - public double lastResultTimestamp = 0; - public Pose3d cameraPose = new Pose3d(); - public double averageDistanceFromTags = 0; - public int visibleTags = 0; - } -} diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/T265IO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/T265IO.java deleted file mode 100644 index 8bc6ea0d..00000000 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/T265IO.java +++ /dev/null @@ -1,76 +0,0 @@ -package frc.trigon.robot.poseestimation.robotposesources; - -import edu.wpi.first.math.geometry.*; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import org.trigon.utilities.JsonHandler; - -public class T265IO extends RobotPoseSourceIO { - private static final NetworkTable NETWORK_TABLE = NetworkTableInstance.getDefault().getTable("T265"); - private static final short CONFIDENCE_THRESHOLD = 2; - private final NetworkTableEntry jsonDump; - - protected T265IO(String name) { - jsonDump = NETWORK_TABLE.getEntry(name + "/jsonDump"); - } - - @Override - protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { - inputs.hasResult = canUseJsonDump(); - if (inputs.hasResult) - inputs.cameraPose = getCameraPose(); - inputs.lastResultTimestamp = (double) jsonDump.getLastChange() / 1000000; - } - - private Pose3d getCameraPose() { - if (!canUseJsonDump()) - return null; - - return getRobotPoseFromJsonDump(); - } - - private Pose3d getRobotPoseFromJsonDump() { - final T265JsonDump jsonDump = getJsonDump(); - final Translation3d translation = getTranslationFromDoubleArray(jsonDump.translation); - final Rotation3d rotation = getRotationFromDoubleArray(jsonDump.rotation); - - return t265PoseToWPIPose(new Pose3d(translation, rotation)); - } - - private Pose3d t265PoseToWPIPose(Pose3d t265Pose) { - final CoordinateSystem eusCoordinateSystem = new CoordinateSystem(CoordinateAxis.E(), CoordinateAxis.U(), CoordinateAxis.S()); - final Pose3d convertedPose = CoordinateSystem.convert(t265Pose, eusCoordinateSystem, CoordinateSystem.NWU()); - final Rotation3d convertedRotation = convertedPose.getRotation().plus(new Rotation3d(0, 0, Math.toRadians(90))); - - return new Pose3d(convertedPose.getTranslation(), convertedRotation); - } - - private Translation3d getTranslationFromDoubleArray(double[] xyz) { - return new Translation3d(xyz[0], xyz[1], xyz[2]); - } - - private Rotation3d getRotationFromDoubleArray(double[] wxyz) { - return new Rotation3d(new Quaternion(wxyz[0], wxyz[1], wxyz[2], wxyz[3])); - } - - private boolean canUseJsonDump() { - final T265JsonDump jsonDump = getJsonDump(); - - try { - return jsonDump.confidence >= CONFIDENCE_THRESHOLD && jsonDump.translation.length == 3 && jsonDump.rotation.length == 4; - } catch (NullPointerException e) { - return false; - } - } - - private T265JsonDump getJsonDump() { - return JsonHandler.parseJsonStringToObject(jsonDump.getString(""), T265JsonDump.class); - } - - private static class T265JsonDump { - private double[] translation; - private double[] rotation; - private int confidence; - } -} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java index 3c2b13f5..755bba8f 100644 --- a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java +++ b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java @@ -8,8 +8,10 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.commands.Commands; -import frc.trigon.robot.constants.CommandConstants; +import org.littletonrobotics.junction.networktables.LoggedDashboardBoolean; +import org.trigon.hardware.RobotHardwareStats; import java.util.ArrayList; import java.util.List; @@ -24,6 +26,7 @@ public abstract class MotorSubsystem extends edu.wpi.first.wpilibj2.command.SubsystemBase { private static final List REGISTERED_SUBSYSTEMS = new ArrayList<>(); private static final Trigger DISABLED_TRIGGER = new Trigger(DriverStation::isDisabled); + private static final LoggedDashboardBoolean ENABLE_EXTENSIVE_LOGGING = new LoggedDashboardBoolean("EnableExtensiveLogging", true); static { DISABLED_TRIGGER.onTrue(new InstantCommand(() -> forEach(MotorSubsystem::stop)).ignoringDisable(true)); @@ -59,6 +62,22 @@ public static void setAllSubsystemsBrakeAsync(boolean brake) { CompletableFuture.runAsync(() -> forEach((subsystem) -> subsystem.setBrake(brake))); } + public static boolean isExtensiveLoggingEnabled() { + return ENABLE_EXTENSIVE_LOGGING.get() || RobotHardwareStats.isReplay(); + } + + /** + * Runs periodically, to update the subsystem, and update the mechanism of the subsystem (if there is one). + * This only updates the mechanism if the robot is in replay mode or extensive logging is enabled. + * This function cannot be overridden. Use {@linkplain MotorSubsystem#updatePeriodically} or {@linkplain MotorSubsystem#updateMechanism} (depending on the usage) instead. + */ + @Override + public final void periodic() { + updatePeriodically(); + if (isExtensiveLoggingEnabled()) + updateMechanism(); + } + /** * Creates a quasistatic (ramp up) command for characterizing the subsystem's mechanism. * @@ -110,6 +129,19 @@ public void drive(Measure voltageMeasure) { public void updateLog(SysIdRoutineLog log) { } + /** + * Updates the mechanism of the subsystem periodically if the robot is in replay mode, or if {@linkplain MotorSubsystem#ENABLE_EXTENSIVE_LOGGING) is true. + * This doesn't always run in order to save resources. + */ + public void updateMechanism() { + } + + /** + * Updates periodically. Anything that should be updated periodically but isn't related to the mechanism (or shouldn't always be logged, in order to save resources) should be put here. + */ + public void updatePeriodically() { + } + public SysIdRoutine.Config getSysIdConfig() { return null; } @@ -137,4 +169,4 @@ private SysIdRoutine createSysIdRoutine() { ) ); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java index 561dd238..bffac19d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java @@ -13,8 +13,8 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.commands.Commands; -import frc.trigon.robot.constants.CommandConstants; import frc.trigon.robot.subsystems.MotorSubsystem; import org.littletonrobotics.junction.Logger; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; @@ -43,17 +43,26 @@ public class Climber extends MotorSubsystem { public Climber() { setName("Climber"); - configurePositionResettingLimitSwitch(); Commands.getDelayedCommand(3, this::configureChangingDefaultCommand).schedule(); + Commands.getDelayedCommand(3, this::configurePositionResettingLimitSwitch).schedule(); } @Override - public void periodic() { + public void updatePeriodically() { masterMotor.update(); ClimberConstants.LIMIT_SWITCH.updateSensor(); updateNetworkTables(); } + @Override + public void updateMechanism() { + ClimberConstants.MECHANISM.update( + getPositionMeters(), + toMeters(masterMotor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE)) + ); + Logger.recordOutput("Poses/Components/ClimberPose", getClimberPose()); + } + @Override public void drive(Measure voltageMeasure) { masterMotor.setControl(voltageRequest.withOutput(voltageMeasure.in(Units.Volts))); @@ -109,19 +118,10 @@ void setTargetPosition(double targetPositionMeters, boolean affectedByWeight) { } private void updateNetworkTables() { - updateMechanisms(); Logger.recordOutput("Climber/PositionMeters", getPositionMeters()); Logger.recordOutput("Climber/VelocityMeters", toMeters(masterMotor.getSignal(TalonFXSignal.VELOCITY))); } - private void updateMechanisms() { - ClimberConstants.MECHANISM.update( - getPositionMeters(), - toMeters(masterMotor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE)) - ); - Logger.recordOutput("Poses/Components/ClimberPose", getClimberPose()); - } - private Pose3d getClimberPose() { final Transform3d climberTransform = new Transform3d( new Translation3d(0, 0, getPositionMeters()), @@ -157,7 +157,7 @@ private double toRotations(double meters) { } private void configurePositionResettingLimitSwitch() { - final Trigger limitSwitchTrigger = new Trigger(() -> ClimberConstants.LIMIT_SWITCH.getBinaryValue() && !CommandConstants.IS_CLIMBING); + final Trigger limitSwitchTrigger = new Trigger(() -> !ClimberConstants.LIMIT_SWITCH.getBinaryValue() && !CommandConstants.IS_CLIMBING); limitSwitchTrigger.and(() -> masterMotor.getSignal(TalonFXSignal.POSITION) != 0).debounce(ClimberConstants.LIMIT_SWITCH_PRESSED_THRESHOLD_SECONDS).whileTrue(new InstantCommand(this::resetPosition).repeatedly().ignoringDisable(true)); } diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java index c9189fca..49e50591 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.constants.RobotConstants; +import org.trigon.hardware.RobotHardwareStats; import org.trigon.hardware.misc.simplesensor.SimpleSensor; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; @@ -55,21 +56,21 @@ public class ClimberConstants { MAX_CLIMBING_VELOCITY = 1, MAX_CLIMBING_ACCELERATION = 1; private static final double - NON_CLIMBING_P = RobotConstants.IS_SIMULATION ? 30 : 0, + NON_CLIMBING_P = RobotHardwareStats.isSimulation() ? 30 : 15, NON_CLIMBING_I = 0, NON_CLIMBING_D = 0, - NON_CLIMBING_KS = RobotConstants.IS_SIMULATION ? 0.030713 : 0.3081, - NON_CLIMBING_KG = RobotConstants.IS_SIMULATION ? 0.009791 : -0.094675, - NON_CLIMBING_KV = RobotConstants.IS_SIMULATION ? 2.385 : 2.3525, - NON_CLIMBING_KA = RobotConstants.IS_SIMULATION ? 0.049261 : 0.045652; + NON_CLIMBING_KS = RobotHardwareStats.isSimulation() ? 0.030713 : 0.3081, + NON_CLIMBING_KG = RobotHardwareStats.isSimulation() ? 0.009791 : -0.094675, + NON_CLIMBING_KV = RobotHardwareStats.isSimulation() ? 2.385 : 2.3525, + NON_CLIMBING_KA = RobotHardwareStats.isSimulation() ? 0.049261 : 0.045652; private static final double - CLIMBING_P = RobotConstants.IS_SIMULATION ? 30 : 15, + CLIMBING_P = RobotHardwareStats.isSimulation() ? 30 : 15, CLIMBING_I = 0, CLIMBING_D = 0, - CLIMBING_KS = RobotConstants.IS_SIMULATION ? 0.030713 : 0.30095, - CLIMBING_KG = RobotConstants.IS_SIMULATION ? 0.009791 : -0.58087, - CLIMBING_KV = RobotConstants.IS_SIMULATION ? 2.385 : 2.2353, - CLIMBING_KA = RobotConstants.IS_SIMULATION ? 0.049261 : 0.061477; + CLIMBING_KS = RobotHardwareStats.isSimulation() ? 0.030713 : 0.30095, + CLIMBING_KG = RobotHardwareStats.isSimulation() ? 0.009791 : -0.58087, + CLIMBING_KV = RobotHardwareStats.isSimulation() ? 2.385 : 2.2353, + CLIMBING_KA = RobotHardwareStats.isSimulation() ? 0.049261 : 0.061477; static final int NON_CLIMBING_SLOT = 0, CLIMBING_SLOT = 1; diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index 3fbb277e..f8e880eb 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.subsystems.MotorSubsystem; import org.littletonrobotics.junction.Logger; +import org.trigon.hardware.phoenix6.cancoder.CANcoderSignal; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; import org.trigon.utilities.Conversions; @@ -23,7 +24,7 @@ public class Elevator extends MotorSubsystem { 0, ElevatorConstants.MOTION_MAGIC_CRUISE_VELOCITY, ElevatorConstants.MOTION_MAGIC_ACCELERATION, - 0 + ElevatorConstants.MOTION_MAGIC_JERK ).withUpdateFreqHz(1000).withEnableFOC(ElevatorConstants.FOC_ENABLED); private final VoltageOut voltageRequest = new VoltageOut(0).withEnableFOC(ElevatorConstants.FOC_ENABLED).withUpdateFreqHz(1000); private ElevatorConstants.ElevatorState targetState = ElevatorConstants.ElevatorState.RESTING; @@ -34,16 +35,24 @@ public Elevator() { } @Override - public void periodic() { + public void updatePeriodically() { motor.update(); + ElevatorConstants.ENCODER.update(); updateNetworkTables(); } + @Override + public void updateMechanism() { + Logger.recordOutput("Poses/Components/ElevatorPose", getElevatorComponentPose()); + Logger.recordOutput("Poses/Components/TransporterPose", getTransporterComponentPose()); + ElevatorConstants.MECHANISM.update(getPositionMeters(), toMeters(motor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE))); + } + @Override public void updateLog(SysIdRoutineLog log) { log.motor("Elevator") - .linearPosition(Units.Meters.of(motor.getSignal(TalonFXSignal.POSITION))) - .linearVelocity(Units.MetersPerSecond.of(motor.getSignal(TalonFXSignal.VELOCITY))) + .angularPosition(Units.Rotations.of(getEncoderPosition())) + .angularVelocity(Units.RotationsPerSecond.of(motor.getSignal(TalonFXSignal.ROTOR_VELOCITY) / ElevatorConstants.GEAR_RATIO)) .voltage(Units.Volts.of(motor.getSignal(TalonFXSignal.MOTOR_VOLTAGE))); } @@ -55,6 +64,7 @@ public SysIdRoutine.Config getSysIdConfig() { @Override public void setBrake(boolean brake) { motor.setBrake(brake); + ElevatorConstants.FOLLOWER_MOTOR.setBrake(brake); } @Override @@ -67,6 +77,14 @@ public void drive(Measure voltageMeasure) { motor.setControl(voltageRequest.withOutput(voltageMeasure.in(Units.Volts))); } + public double getRotorPosition() { + return motor.getSignal(TalonFXSignal.ROTOR_POSITION); + } + + public double getEncoderPosition() { + return ElevatorConstants.ENCODER.getSignal(CANcoderSignal.POSITION); + } + public boolean atTargetState() { return Math.abs(this.targetState.positionMeters - getPositionMeters()) < ElevatorConstants.TOLERANCE_METERS; } @@ -105,7 +123,6 @@ void setTargetPosition(double targetPositionMeters, double speedPercentage) { } private void updateNetworkTables() { - updateMechanism(); Logger.recordOutput("Elevator/ElevatorPositionMeters", getPositionMeters()); Logger.recordOutput("Elevator/ElevatorVelocityMetersPerSecond", toMeters(motor.getSignal(TalonFXSignal.VELOCITY))); } @@ -114,12 +131,6 @@ private DynamicMotionMagicVoltage scaleProfile(DynamicMotionMagicVoltage profile return profile.withVelocity(ElevatorConstants.MOTION_MAGIC_CRUISE_VELOCITY * (speedPercentage / 100)).withAcceleration(ElevatorConstants.MOTION_MAGIC_ACCELERATION * (speedPercentage / 100)); } - private void updateMechanism() { - Logger.recordOutput("Poses/Components/ElevatorPose", getElevatorComponentPose()); - Logger.recordOutput("Poses/Components/TransporterPose", getTransporterComponentPose()); - ElevatorConstants.MECHANISM.update(getPositionMeters(), toMeters(motor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE))); - } - private Pose3d getElevatorComponentPose() { final Transform3d elevatorTransform = new Transform3d( new Translation3d(0, 0, getPositionMeters()), diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java index e18b2617..c342fafa 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java @@ -1,8 +1,10 @@ package frc.trigon.robot.subsystems.elevator; +import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.StartEndCommand; import frc.trigon.robot.RobotContainer; +import org.trigon.commands.GearRatioCalculationCommand; import org.trigon.commands.NetworkTablesCommand; public class ElevatorCommands { @@ -14,6 +16,15 @@ public static Command getDebuggingCommand() { ); } + public static Command getGearRatioCalibrationCommand() { + return new GearRatioCalculationCommand( + RobotContainer.ELEVATOR::getRotorPosition, + RobotContainer.ELEVATOR::getEncoderPosition, + (voltage) -> RobotContainer.ELEVATOR.drive(Units.Volt.of(voltage)), + RobotContainer.ELEVATOR + ); + } + public static Command getSetTargetPositionCommand(double targetPositionMeters) { return new StartEndCommand( () -> RobotContainer.ELEVATOR.setTargetPosition(targetPositionMeters, 100), diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java index 78f2c7d4..2e8260f2 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -12,7 +12,9 @@ import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.constants.RobotConstants; +import org.trigon.hardware.RobotHardwareStats; import org.trigon.hardware.phoenix6.cancoder.CANcoderEncoder; +import org.trigon.hardware.phoenix6.cancoder.CANcoderSignal; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; import org.trigon.hardware.simulation.ElevatorSimulation; @@ -39,20 +41,21 @@ public class ElevatorConstants { private static final boolean FOLLOWER_MOTOR_OPPOSITE_DIRECTION = true; private static final AbsoluteSensorRangeValue ENCODER_SENSOR_RANGE_VALUE = AbsoluteSensorRangeValue.Unsigned_0To1; private static final SensorDirectionValue ENCODER_SENSOR_DIRECTION_VALUE = SensorDirectionValue.CounterClockwise_Positive; - private static final double ENCODER_MAGNET_OFFSET_VALUE = -0.719726; + private static final double ENCODER_MAGNET_OFFSET_VALUE = -0.703125; private static final FeedbackSensorSourceValue ENCODER_TYPE = FeedbackSensorSourceValue.RemoteCANcoder; private static final double - P = RobotConstants.IS_SIMULATION ? 52 : 2.5, + P = RobotHardwareStats.isSimulation() ? 52 : 1.5, I = 0, D = 0, - KS = RobotConstants.IS_SIMULATION ? 0.019539 : 0.036646 + 0.0798, - KV = RobotConstants.IS_SIMULATION ? 0.987 : 0.44458, - KG = RobotConstants.IS_SIMULATION ? 0.12551 : 0.4, - KA = RobotConstants.IS_SIMULATION ? 0.017514 : 0.026516; + KS = RobotHardwareStats.isSimulation() ? 0.019539 : 0.02, + KV = RobotHardwareStats.isSimulation() ? 0.987 : 0.415, + KG = RobotHardwareStats.isSimulation() ? 0.12551 : 0.37, + KA = RobotHardwareStats.isSimulation() ? 0.017514 : 0.01; static final double - MOTION_MAGIC_CRUISE_VELOCITY = 25, - MOTION_MAGIC_ACCELERATION = 25; - private static final double GEAR_RATIO = 3.44; + MOTION_MAGIC_CRUISE_VELOCITY = 23, + MOTION_MAGIC_ACCELERATION = 30, + MOTION_MAGIC_JERK = MOTION_MAGIC_ACCELERATION * 7; + static final double GEAR_RATIO = 3.2; static final boolean FOC_ENABLED = true; private static final double @@ -66,7 +69,7 @@ public class ElevatorConstants { static final SysIdRoutine.Config SYSID_CONFIG = new SysIdRoutine.Config( Units.Volts.of(0.25).per(Units.Second.of(1)), - Units.Volts.of(2), + Units.Volts.of(1), Units.Second.of(1000) ); @@ -86,9 +89,23 @@ public class ElevatorConstants { CAMERA_PLATE_HEIGHT_METERS = 0.190193; static { + configureEncoder(); configureMasterMotor(); configureFollowerMotor(); - configureEncoder(); + } + + private static void configureEncoder() { + final CANcoderConfiguration config = new CANcoderConfiguration(); + + config.MagnetSensor.AbsoluteSensorRange = ENCODER_SENSOR_RANGE_VALUE; + config.MagnetSensor.SensorDirection = ENCODER_SENSOR_DIRECTION_VALUE; + config.MagnetSensor.MagnetOffset = ENCODER_MAGNET_OFFSET_VALUE; + + ENCODER.applyConfiguration(config); + ENCODER.setSimulationInputsFromTalonFX(MASTER_MOTOR); + + ENCODER.registerSignal(CANcoderSignal.POSITION, 100); + ENCODER.registerSignal(CANcoderSignal.VELOCITY, 100); } private static void configureMasterMotor() { @@ -115,20 +132,25 @@ private static void configureMasterMotor() { config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseClosedLoopSign; config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = 0; + config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = -0.01; config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 3.5; + config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = 4.440674; config.MotionMagic.MotionMagicCruiseVelocity = MOTION_MAGIC_CRUISE_VELOCITY; config.MotionMagic.MotionMagicAcceleration = MOTION_MAGIC_ACCELERATION; + config.MotionMagic.MotionMagicJerk = MOTION_MAGIC_JERK; MASTER_MOTOR.applyConfiguration(config); MASTER_MOTOR.setPhysicsSimulation(SIMULATION); MASTER_MOTOR.registerSignal(TalonFXSignal.POSITION, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.ROTOR_POSITION, 100); + MASTER_MOTOR.registerSignal(TalonFXSignal.ROTOR_VELOCITY, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.VELOCITY, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); MASTER_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); + + MASTER_MOTOR.setPosition(0); } private static void configureFollowerMotor() { @@ -145,22 +167,11 @@ private static void configureFollowerMotor() { FOLLOWER_MOTOR.setControl(new Follower(MASTER_MOTOR_ID, FOLLOWER_MOTOR_OPPOSITE_DIRECTION)); } - private static void configureEncoder() { - final CANcoderConfiguration config = new CANcoderConfiguration(); - - config.MagnetSensor.AbsoluteSensorRange = ENCODER_SENSOR_RANGE_VALUE; - config.MagnetSensor.SensorDirection = ENCODER_SENSOR_DIRECTION_VALUE; - config.MagnetSensor.MagnetOffset = ENCODER_MAGNET_OFFSET_VALUE; - - ENCODER.applyConfiguration(config); - ENCODER.setSimulationInputsFromTalonFX(MASTER_MOTOR); - } - public enum ElevatorState { - RESTING(0, 100), + RESTING(-0.005, 100), FEEDING_FOR_CLOSE_SHOT(0, 100), SCORE_AMP(0.45, 100), - SCORE_TRAP(0.5, 10), + SCORE_TRAP(0.53, 10), SCORE_TRAP_LOWERED(0.15, 10), FINISH_TRAP(0, 10); diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index 4e340f05..62ca9f66 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -1,7 +1,6 @@ package frc.trigon.robot.subsystems.intake; import com.ctre.phoenix6.controls.VoltageOut; -import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.trigon.robot.subsystems.MotorSubsystem; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; @@ -15,9 +14,13 @@ public Intake() { } @Override - public void periodic() { + public void updatePeriodically() { motor.update(); - updateMechanism(); + } + + @Override + public void updateMechanism() { + IntakeConstants.MECHANISM.update(motor.getSignal(TalonFXSignal.MOTOR_VOLTAGE)); } @Override @@ -35,11 +38,7 @@ void setTargetVoltage(double collectionVoltage) { IntakeConstants.MECHANISM.setTargetVelocity(collectionVoltage); } - public Trigger getEarlyNoteCollectionDetectionTrigger() { - return new Trigger(() -> motor.getSignal(TalonFXSignal.SUPPLY_CURRENT) > IntakeConstants.NOTE_COLLECTION_CURRENT).debounce(IntakeConstants.NOTE_COLLECTION_TIME_THRESHOLD_SECONDS); - } - - private void updateMechanism() { - IntakeConstants.MECHANISM.update(motor.getSignal(TalonFXSignal.MOTOR_VOLTAGE)); + public boolean isEarlyNoteCollectionDetected() { + return IntakeConstants.EARLY_NOTE_COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index a348cd4e..b3fc25c7 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -4,6 +4,8 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.event.BooleanEvent; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.trigon.robot.constants.RobotConstants; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; @@ -38,9 +40,13 @@ public class IntakeConstants { "IntakeMechanism", MAX_DISPLAYABLE_VELOCITY ); - static final double + private static final double NOTE_COLLECTION_CURRENT = 34, NOTE_COLLECTION_TIME_THRESHOLD_SECONDS = 0.15; + static final BooleanEvent EARLY_NOTE_COLLECTION_DETECTION_BOOLEAN_EVENT = new BooleanEvent( + CommandScheduler.getInstance().getActiveButtonLoop(), + () -> Math.abs(MOTOR.getSignal(TalonFXSignal.SUPPLY_CURRENT)) > IntakeConstants.NOTE_COLLECTION_CURRENT + ).debounce(NOTE_COLLECTION_TIME_THRESHOLD_SECONDS); static { configureMotor(); diff --git a/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java b/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java index d9b684c8..7e8d290a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java +++ b/src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java @@ -1,24 +1,27 @@ package frc.trigon.robot.subsystems.pitcher; -import com.ctre.phoenix6.controls.MotionMagicExpoVoltage; +import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.VoltageOut; import edu.wpi.first.math.geometry.*; import edu.wpi.first.units.Measure; import edu.wpi.first.units.Units; import edu.wpi.first.units.Voltage; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.robot.misc.ShootingCalculations; import frc.trigon.robot.subsystems.MotorSubsystem; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; +import org.trigon.hardware.phoenix6.cancoder.CANcoderSignal; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; -import org.trigon.utilities.ShootingCalculations; +import org.trigon.utilities.Conversions; public class Pitcher extends MotorSubsystem { private final ShootingCalculations shootingCalculations = ShootingCalculations.getInstance(); private final TalonFXMotor motor = PitcherConstants.MOTOR; - private final MotionMagicExpoVoltage motionMagicPositionRequest = new MotionMagicExpoVoltage(0).withEnableFOC(PitcherConstants.FOC_ENABLED).withUpdateFreqHz(1000); + private final MotionMagicVoltage motionMagicPositionRequest = new MotionMagicVoltage(0).withEnableFOC(PitcherConstants.FOC_ENABLED).withUpdateFreqHz(1000); private final VoltageOut voltageRequest = new VoltageOut(0); private Rotation2d targetPitch = null; @@ -32,27 +35,37 @@ public void stop() { } @Override - public void periodic() { + public void updatePeriodically() { motor.update(); - updateMechanism(); + PitcherConstants.ENCODER.update(); + var a = Conversions.rotationsToDegrees(motor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE) - PitcherConstants.GRAVITY_POSITION_TO_REAL_POSITION); + Logger.recordOutput("CurrentPitchDegrees", Conversions.rotationsToDegrees(motor.getSignal(TalonFXSignal.POSITION) - PitcherConstants.GRAVITY_POSITION_TO_REAL_POSITION)); + if (!DriverStation.isDisabled() && Math.abs(a - 478.8756) > 0.01) + Logger.recordOutput("TargetPitchDegrees", a); } @Override - public void drive(Measure voltageMeasure) { - motor.setControl(voltageRequest.withOutput(voltageMeasure.in(Units.Volts))); + public void updateMechanism() { + PitcherConstants.MECHANISM.update(getCurrentPitch(), Rotation2d.fromRotations(motor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE))); + Logger.recordOutput("Poses/Components/PitcherPose", getPitcherComponentPose()); } @Override - public void setBrake(boolean brake) { - motor.setBrake(brake); + public void drive(Measure voltageMeasure) { + motor.setControl(voltageRequest.withOutput(voltageMeasure.in(Units.Volts))); } @Override public void updateLog(SysIdRoutineLog log) { log.motor("Pitcher") - .voltage(Units.Volts.of(motor.getSignal(TalonFXSignal.TORQUE_CURRENT))) - .angularPosition(Units.Rotations.of(motor.getSignal(TalonFXSignal.POSITION))) - .angularVelocity(Units.RotationsPerSecond.of(motor.getSignal(TalonFXSignal.VELOCITY))); + .voltage(Units.Volts.of(motor.getSignal(TalonFXSignal.MOTOR_VOLTAGE))) + .angularPosition(Units.Rotations.of(PitcherConstants.ENCODER.getSignal(CANcoderSignal.POSITION))) + .angularVelocity(Units.RotationsPerSecond.of(motor.getSignal(TalonFXSignal.ROTOR_VELOCITY) / PitcherConstants.GEAR_RATIO)); + } + + @Override + public void setBrake(boolean brake) { + motor.setBrake(brake); } @Override @@ -70,7 +83,15 @@ public Rotation2d getTargetPitch() { } public Rotation2d getCurrentPitch() { - return Rotation2d.fromRotations(motor.getSignal(TalonFXSignal.POSITION)); + return Rotation2d.fromRotations(motor.getSignal(TalonFXSignal.POSITION) - PitcherConstants.GRAVITY_POSITION_TO_REAL_POSITION); + } + + public double getRotorPosition() { + return motor.getSignal(TalonFXSignal.ROTOR_POSITION); + } + + public double getEncoderPosition() { + return PitcherConstants.ENCODER.getSignal(CANcoderSignal.POSITION) - PitcherConstants.GRAVITY_POSITION_TO_REAL_POSITION; } void pitchToShootingTarget() { @@ -78,19 +99,16 @@ void pitchToShootingTarget() { } void setTargetPitch(Rotation2d targetPitch) { - if (targetPitch == null) + if (targetPitch == null || Double.isNaN(targetPitch.getRadians())) { + this.targetPitch = null; return; + } - motor.setControl(motionMagicPositionRequest.withPosition(targetPitch.getRotations())); + motor.setControl(motionMagicPositionRequest.withPosition(targetPitch.getRotations() + PitcherConstants.GRAVITY_POSITION_TO_REAL_POSITION)); Logger.recordOutput("Pitcher/TargetPitch", targetPitch.getDegrees()); this.targetPitch = targetPitch; } - private void updateMechanism() { - PitcherConstants.MECHANISM.update(getCurrentPitch(), Rotation2d.fromRotations(motor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE))); - Logger.recordOutput("Poses/Components/PitcherPose", getPitcherComponentPose()); - } - private Pose3d getPitcherComponentPose() { final Transform3d pitcherTransform = new Transform3d( new Translation3d(), diff --git a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherCommands.java b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherCommands.java index c77ab165..66512fbb 100644 --- a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherCommands.java @@ -1,9 +1,11 @@ package frc.trigon.robot.subsystems.pitcher; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.StartEndCommand; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.GearRatioCalculationCommand; import org.trigon.commands.ExecuteEndCommand; import org.trigon.commands.NetworkTablesCommand; @@ -16,6 +18,15 @@ public static Command getDebuggingCommand() { ); } + public static Command getGearRatioCalibrationCommand() { + return new GearRatioCalculationCommand( + RobotContainer.PITCHER::getRotorPosition, + RobotContainer.PITCHER::getEncoderPosition, + (voltage) -> RobotContainer.PITCHER.drive(Units.Volt.of(voltage)), + RobotContainer.PITCHER + ); + } + public static Command getSetTargetPitchCommand(Rotation2d targetPitch) { return new StartEndCommand( () -> RobotContainer.PITCHER.setTargetPitch(targetPitch), diff --git a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java index b94d0d81..3c8f44fc 100644 --- a/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.constants.RobotConstants; +import org.trigon.hardware.RobotHardwareStats; import org.trigon.hardware.phoenix6.cancoder.CANcoderEncoder; import org.trigon.hardware.phoenix6.cancoder.CANcoderSignal; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; @@ -42,20 +43,23 @@ public class PitcherConstants { private static final InvertedValue INVERTED_VALUE = InvertedValue.Clockwise_Positive; private static final SensorDirectionValue SENSOR_DIRECTION_VALUE = SensorDirectionValue.CounterClockwise_Positive; private static final AbsoluteSensorRangeValue ABSOLUTE_SENSOR_RANGE_VALUE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; - private static final FeedbackSensorSourceValue ENCODER_TYPE = FeedbackSensorSourceValue.FusedCANcoder; - private static final double OFFSET = Conversions.degreesToRotations(11.337891 + 90); + private static final FeedbackSensorSourceValue ENCODER_TYPE = FeedbackSensorSourceValue.RemoteCANcoder; + public static final double GRAVITY_POSITION_TO_REAL_POSITION = Conversions.degreesToRotations(-118.8756) + 0.082775; + private static final double OFFSET = -0.0487158583333333 + 0.082775; private static final double - MOTION_MAGIC_P = RobotConstants.IS_SIMULATION ? 100 : 90, + MOTION_MAGIC_P = RobotHardwareStats.isSimulation() ? 300 : 120, MOTION_MAGIC_I = 0, - MOTION_MAGIC_D = RobotConstants.IS_SIMULATION ? 0 : 45, - KS = RobotConstants.IS_SIMULATION ? 0.053988 : 1.4, - KV = RobotConstants.IS_SIMULATION ? 39 : 0, - KA = RobotConstants.IS_SIMULATION ? 0.85062 : 9.2523, - KG = RobotConstants.IS_SIMULATION ? 0.04366 : 1.2, - EXPO_KV = RobotConstants.IS_SIMULATION ? KV : 38.757, - EXPO_KA = RobotConstants.IS_SIMULATION ? KA : 0.6; + MOTION_MAGIC_D = RobotHardwareStats.isSimulation() ? 0 : 0, + KS = RobotHardwareStats.isSimulation() ? 1.0346 : 0.21662, + KV = RobotHardwareStats.isSimulation() ? 41 : 39.8, + KA = RobotHardwareStats.isSimulation() ? 0.85062 : 0, + KG = RobotHardwareStats.isSimulation() ? 0.04366 : 0.079831; + private static final double + MOTION_MAGIC_CRUISE_VELOCITY = 0.28, + MOTION_MAGIC_ACCELERATION = 4, + MOTION_MAGIC_JERK = 40; static final boolean FOC_ENABLED = true; - private static final double GEAR_RATIO = 352.8; + static final double GEAR_RATIO = 353.757785; private static final int MOTOR_AMOUNT = 1; private static final DCMotor GEARBOX = DCMotor.getFalcon500Foc(MOTOR_AMOUNT); @@ -76,8 +80,8 @@ public class PitcherConstants { ); static final SysIdRoutine.Config SYS_ID_CONFIG = new SysIdRoutine.Config( - Units.Volts.of(0.5).per(Units.Second), - Units.Volts.of(5), + Units.Volts.of(1).per(Units.Second), + Units.Volts.of(4), Units.Second.of(1000) ); @@ -87,7 +91,7 @@ public class PitcherConstants { ); public static final Rotation2d DEFAULT_PITCH = Rotation2d.fromDegrees(30); - static final double PITCH_TOLERANCE_DEGREES = 0.6; + static final double PITCH_TOLERANCE_DEGREES = 0.4; static { configuredEncoder(); @@ -108,21 +112,22 @@ private static void configureMotor() { config.Slot0.kA = KA; config.Slot0.kS = KS; config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; - config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; + config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseClosedLoopSign; config.MotorOutput.Inverted = INVERTED_VALUE; config.MotorOutput.NeutralMode = NEUTRAL_MODE_VALUE; config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Conversions.degreesToRotations(24); + config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Conversions.degreesToRotations(29) + PitcherConstants.GRAVITY_POSITION_TO_REAL_POSITION; config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Conversions.degreesToRotations(90); + config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Conversions.degreesToRotations(90) + PitcherConstants.GRAVITY_POSITION_TO_REAL_POSITION; - config.Feedback.RotorToSensorRatio = PitcherConstants.GEAR_RATIO; - config.Feedback.FeedbackRemoteSensorID = PitcherConstants.ENCODER_ID; + config.Feedback.RotorToSensorRatio = GEAR_RATIO; + config.Feedback.FeedbackRemoteSensorID = ENCODER_ID; config.Feedback.FeedbackSensorSource = ENCODER_TYPE; - config.MotionMagic.MotionMagicExpo_kA = EXPO_KA; - config.MotionMagic.MotionMagicExpo_kV = EXPO_KV; + config.MotionMagic.MotionMagicCruiseVelocity = MOTION_MAGIC_CRUISE_VELOCITY; + config.MotionMagic.MotionMagicAcceleration = MOTION_MAGIC_ACCELERATION; + config.MotionMagic.MotionMagicJerk = MOTION_MAGIC_JERK; MOTOR.applyConfiguration(config); MOTOR.setPhysicsSimulation(SIMULATION); @@ -130,7 +135,10 @@ private static void configureMotor() { MOTOR.registerSignal(TalonFXSignal.POSITION, 100); MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100); MOTOR.registerSignal(TalonFXSignal.VELOCITY, 100); - MOTOR.registerSignal(TalonFXSignal.TORQUE_CURRENT, 100); + MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); + MOTOR.registerSignal(TalonFXSignal.ROTOR_VELOCITY, 100); + MOTOR.registerSignal(TalonFXSignal.ROTOR_POSITION, 100); + MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); } private static void configuredEncoder() { diff --git a/src/main/java/frc/trigon/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/trigon/robot/subsystems/shooter/Shooter.java index 9cb7b1cd..5b770400 100644 --- a/src/main/java/frc/trigon/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/trigon/robot/subsystems/shooter/Shooter.java @@ -7,11 +7,11 @@ import edu.wpi.first.units.Voltage; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.robot.misc.ShootingCalculations; import frc.trigon.robot.subsystems.MotorSubsystem; import org.littletonrobotics.junction.AutoLogOutput; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; -import org.trigon.utilities.ShootingCalculations; public class Shooter extends MotorSubsystem { private final ShootingCalculations shootingCalculations = ShootingCalculations.getInstance(); @@ -49,9 +49,13 @@ public SysIdRoutine.Config getSysIdConfig() { } @Override - public void periodic() { + public void updatePeriodically() { motor.update(); - updateMechanism(); + } + + @Override + public void updateMechanism() { + ShooterConstants.SHOOTING_MECHANISM.update(getCurrentVelocityRotationsPerSecond(), targetVelocityRotationsPerSecond); } @AutoLogOutput(key = "Shooter/AtShootingVelocity") @@ -76,9 +80,5 @@ void setTargetVelocity(double targetVelocityRotationsPerSecond) { motor.setControl(velocityRequest.withVelocity(targetVelocityRotationsPerSecond)); this.targetVelocityRotationsPerSecond = targetVelocityRotationsPerSecond; } - - private void updateMechanism() { - ShooterConstants.SHOOTING_MECHANISM.update(getCurrentVelocityRotationsPerSecond(), targetVelocityRotationsPerSecond); - } } diff --git a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java index 649ddf69..14aa96ed 100644 --- a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java @@ -8,6 +8,7 @@ import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.constants.RobotConstants; +import org.trigon.hardware.RobotHardwareStats; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; import org.trigon.hardware.simulation.FlywheelSimulation; @@ -28,12 +29,12 @@ public class ShooterConstants { MASTER_MOTOR_INVERTED_VALUE = InvertedValue.Clockwise_Positive, FOLLOWER_MOTOR_INVERTED_VALUE = InvertedValue.Clockwise_Positive; private static final double - P = RobotConstants.IS_SIMULATION ? 12 : 12, + P = RobotHardwareStats.isSimulation() ? 12 : 12, I = 0, D = 0, - KV = RobotConstants.IS_SIMULATION ? 0 : 0, - KA = RobotConstants.IS_SIMULATION ? 0 : 0, - KS = RobotConstants.IS_SIMULATION ? 0 : 8; + KV = RobotHardwareStats.isSimulation() ? 0 : 0, + KA = RobotHardwareStats.isSimulation() ? 0 : 0, + KS = RobotHardwareStats.isSimulation() ? 0 : 8; private static final boolean FOLLOWER_OPPOSES_MASTER = false; private static final double GEAR_RATIO = 1; @@ -52,7 +53,8 @@ public class ShooterConstants { static final SpeedMechanism2d SHOOTING_MECHANISM = new SpeedMechanism2d("ShooterMechanism", MAX_DISPLAYABLE_VELOCITY); private static final double WHEEL_DIAMETER_METERS = 0.1016; - public static final double ROTATIONS_TO_METERS = GEAR_RATIO / (WHEEL_DIAMETER_METERS * Math.PI); + private static final double NOTE_SPEED_LOSE = 1; + public static final double METERS_TO_ROTATIONS = (WHEEL_DIAMETER_METERS * Math.PI * NOTE_SPEED_LOSE); static final double TOLERANCE_ROTATIONS_PER_SECOND = 2; static { diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java index 09846f55..a81b9f07 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -2,13 +2,18 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.PathfindingCommand; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.*; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Voltage; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants; import frc.trigon.robot.subsystems.MotorSubsystem; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -28,17 +33,22 @@ public class Swerve extends MotorSubsystem { public Swerve() { setName("Swerve"); configurePathPlanner(); + phoenix6SignalThread.setOdometryFrequencyHertz(PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.enableContinuousInput(-180, 180); } @Override - public void periodic() { + public void updatePeriodically() { Phoenix6SignalThread.SIGNALS_LOCK.lock(); updateHardware(); Phoenix6SignalThread.SIGNALS_LOCK.unlock(); updatePoseEstimatorStates(); RobotContainer.POSE_ESTIMATOR.periodic(); + } + + @Override + public void updateMechanism() { updateNetworkTables(); } @@ -54,9 +64,27 @@ public void setBrake(boolean brake) { currentModule.setBrake(brake); } + @Override + public void drive(Measure voltageMeasure) { + for (SwerveModule swerveModule : swerveModules) { + swerveModule.setTargetDriveMotorCurrent(voltageMeasure.in(edu.wpi.first.units.Units.Volts)); + swerveModule.setTargetAngle(new Rotation2d()); + } + } + + @Override + public void updateLog(SysIdRoutineLog log) { + for (SwerveModule swerveModule : swerveModules) + swerveModule.driveMotorUpdateLog(log); + } + + @Override + public SysIdRoutine.Config getSysIdConfig() { + return SwerveModuleConstants.DRIVE_MOTOR_SYSID_CONFIG; + } + public Rotation2d getHeading() { - final double inputtedHeading = MathUtil.inputModulus(gyro.getSignal(Pigeon2Signal.YAW), -0.5, 0.5); - return Rotation2d.fromRotations(inputtedHeading); + return Rotation2d.fromDegrees(SwerveConstants.GYRO.getSignal(Pigeon2Signal.YAW)); } public void setHeading(Rotation2d heading) { @@ -100,15 +128,20 @@ public boolean atAngle(MirrorableRotation2d angle) { return atTargetAngle/* && isAngleStill*/; } - public SwerveModulePosition[] getWheelPositions() { - final SwerveModulePosition[] swerveModulePositions = new SwerveModulePosition[swerveModules.length]; + public double[] getDriveWheelPositionsRadians() { + final double[] swerveModulesPositions = new double[swerveModules.length]; for (int i = 0; i < swerveModules.length; i++) - swerveModulePositions[i] = swerveModules[i].getOdometryPosition(swerveModules[i].getLastOdometryUpdateIndex()); - return swerveModulePositions; + swerveModulesPositions[i] = swerveModules[i].getDriveWheelPosition(); + return swerveModulesPositions; } - public void runWheelRadiusCharacterization(double omegaRadsPerSec) { - selfRelativeDrive(new ChassisSpeeds(0, 0, omegaRadsPerSec)); + public void runWheelRadiusCharacterization(double omegaRadiansPerSecond) { + selfRelativeDrive(new ChassisSpeeds(0, 0, omegaRadiansPerSecond)); + } + + public Rotation2d getDriveRelativeAngle() { + final Rotation2d currentAngle = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation(); + return Mirrorable.isRedAlliance() ? currentAngle.rotateBy(Rotation2d.fromDegrees(180)) : currentAngle; } /** @@ -271,13 +304,13 @@ private void updateHardware() { private void configurePathPlanner() { AutoBuilder.configureHolonomic( - () -> RobotContainer.POSE_ESTIMATOR.getCurrentPose(), + RobotContainer.POSE_ESTIMATOR::getCurrentPose, (pose) -> { }, this::getSelfRelativeVelocity, this::selfRelativeDrive, SwerveConstants.HOLONOMIC_PATH_FOLLOWER_CONFIG, - Mirrorable::isRedAlliance, + () -> DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue).equals(DriverStation.Alliance.Red), this ); PathfindingCommand.warmupCommand().schedule(); @@ -290,7 +323,11 @@ private boolean atTranslationPosition(double currentTranslationPosition, double private double calculateProfiledAngleSpeedToTargetAngle(MirrorableRotation2d targetAngle) { final Rotation2d currentAngle = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation(); - return Units.degreesToRadians(SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.calculate(currentAngle.getDegrees(), targetAngle.get().getDegrees())); + final Rotation2d mirroredTargetAngle = targetAngle.get(); + Logger.recordOutput("Swerve/TurnToAngle/CurrentAngleDegrees", currentAngle.getDegrees()); + Logger.recordOutput("Swerve/TurnToAngle/TargetAngleDegrees", mirroredTargetAngle.getDegrees()); + Logger.recordOutput("Swerve/TurnToAngle/ProfiledTargetAngleDegrees", SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.getSetpoint().position); + return Units.degreesToRadians(SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.calculate(currentAngle.getDegrees(), mirroredTargetAngle.getDegrees())); } private ChassisSpeeds selfRelativeSpeedsFromFieldRelativePowers(double xPower, double yPower, double thetaPower) { @@ -302,16 +339,11 @@ private ChassisSpeeds fieldRelativeSpeedsToSelfRelativeSpeeds(ChassisSpeeds fiel return ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, getDriveRelativeAngle()); } - private Rotation2d getDriveRelativeAngle() { - final Rotation2d currentAngle = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation(); - return Mirrorable.isRedAlliance() ? currentAngle.rotateBy(Rotation2d.fromDegrees(180)) : currentAngle; - } - private ChassisSpeeds powersToSpeeds(double xPower, double yPower, double thetaPower) { return new ChassisSpeeds( xPower * SwerveConstants.MAX_SPEED_METERS_PER_SECOND, yPower * SwerveConstants.MAX_SPEED_METERS_PER_SECOND, - Math.pow(thetaPower, 2) * Math.signum(thetaPower) * SwerveConstants.MAX_ROTATIONAL_SPEED_RADIANS_PER_SECOND + (thetaPower * thetaPower) * Math.signum(thetaPower) * SwerveConstants.MAX_ROTATIONAL_SPEED_RADIANS_PER_SECOND ); } diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index a3d44b29..3473e0eb 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -12,34 +12,40 @@ import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.RobotConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants; +import org.trigon.hardware.RobotHardwareStats; import org.trigon.hardware.phoenix6.pigeon2.Pigeon2Gyro; import org.trigon.hardware.phoenix6.pigeon2.Pigeon2Signal; -import org.trigon.utilities.Conversions; import java.util.function.DoubleSupplier; -public abstract class SwerveConstants { +public class SwerveConstants { private static final int PIGEON_ID = 0; static final Pigeon2Gyro GYRO = new Pigeon2Gyro(SwerveConstants.PIGEON_ID, "SwerveGyro", RobotConstants.CANIVORE_NAME); private static final double - GYRO_MOUNT_POSITION_YAW = 0, - GYRO_MOUNT_POSITION_PITCH = 0, - GYRO_MOUNT_POSITION_ROLL = 0; + GYRO_MOUNT_POSITION_PITCH = -1.4626965522766113, + GYRO_MOUNT_POSITION_ROLL = -0.621093213558197, + GYRO_MOUNT_POSITION_YAW = -2.4247663021087646; + private static final double lol = 2; private static final double - FRONT_LEFT_STEER_ENCODER_OFFSET = -Conversions.degreesToRotations(225.263672 - 360), - FRONT_RIGHT_STEER_ENCODER_OFFSET = -Conversions.degreesToRotations(-256.904297 + 360), - REAR_LEFT_STEER_ENCODER_OFFSET = -Conversions.degreesToRotations(108.369141), - REAR_RIGHT_STEER_ENCODER_OFFSET = -Conversions.degreesToRotations(-36.035156); + FRONT_LEFT_WHEEL_DIAMETER_METERS = 0.047853 * lol, + FRONT_RIGHT_WHEEL_DIAMETER_METERS = 0.047853 * lol, + REAR_LEFT_WHEEL_DIAMETER_METERS = 0.047853 * lol, + REAR_RIGHT_WHEEL_DIAMETER_METERS = 0.047853 * lol; + private static final double + FRONT_LEFT_STEER_ENCODER_OFFSET = 0.374267578125, + FRONT_RIGHT_STEER_ENCODER_OFFSET = -0.286376953125, + REAR_LEFT_STEER_ENCODER_OFFSET = -0.30078125, + REAR_RIGHT_STEER_ENCODER_OFFSET = 0.10009765625; private static final int FRONT_LEFT_ID = 1, FRONT_RIGHT_ID = 2, REAR_LEFT_ID = 3, REAR_RIGHT_ID = 4; static final SwerveModule[] SWERVE_MODULES = { - new SwerveModule(FRONT_LEFT_ID, FRONT_LEFT_STEER_ENCODER_OFFSET), - new SwerveModule(FRONT_RIGHT_ID, FRONT_RIGHT_STEER_ENCODER_OFFSET), - new SwerveModule(REAR_LEFT_ID, REAR_LEFT_STEER_ENCODER_OFFSET), - new SwerveModule(REAR_RIGHT_ID, REAR_RIGHT_STEER_ENCODER_OFFSET) + new SwerveModule(FRONT_LEFT_ID, FRONT_LEFT_STEER_ENCODER_OFFSET, FRONT_LEFT_WHEEL_DIAMETER_METERS), + new SwerveModule(FRONT_RIGHT_ID, FRONT_RIGHT_STEER_ENCODER_OFFSET, FRONT_RIGHT_WHEEL_DIAMETER_METERS), + new SwerveModule(REAR_LEFT_ID, REAR_LEFT_STEER_ENCODER_OFFSET, REAR_LEFT_WHEEL_DIAMETER_METERS), + new SwerveModule(REAR_RIGHT_ID, REAR_RIGHT_STEER_ENCODER_OFFSET, REAR_RIGHT_WHEEL_DIAMETER_METERS) }; private static final DoubleSupplier SIMULATION_YAW_VELOCITY_SUPPLIER = () -> RobotContainer.SWERVE.getSelfRelativeVelocity().omegaRadiansPerSecond; @@ -54,7 +60,7 @@ public abstract class SwerveConstants { new Translation2d(-MODULE_X_DISTANCE_FROM_CENTER, -MODULE_Y_DISTANCE_FROM_CENTER) }; public static final SwerveDriveKinematics KINEMATICS = new SwerveDriveKinematics(LOCATIONS); - public static final double DRIVE_RADIUS_METERS = Math.hypot( + public static final double FURTHEST_MODULE_DISTANCE_FROM_CENTER = Math.hypot( MODULE_X_DISTANCE_FROM_CENTER, MODULE_Y_DISTANCE_FROM_CENTER ); @@ -64,28 +70,28 @@ public abstract class SwerveConstants { TRANSLATION_VELOCITY_TOLERANCE = 0.05, ROTATION_VELOCITY_TOLERANCE = 0.3; static final double - DRIVE_NEUTRAL_DEADBAND = 0.2, - ROTATION_NEUTRAL_DEADBAND = 0.2; + DRIVE_NEUTRAL_DEADBAND = 0.07, + ROTATION_NEUTRAL_DEADBAND = 0.07; static final double - MAX_SPEED_METERS_PER_SECOND = RobotConstants.IS_SIMULATION ? 4.9 : 4.04502, - MAX_ROTATIONAL_SPEED_RADIANS_PER_SECOND = RobotConstants.IS_SIMULATION ? 12.03 : 12.03; + MAX_SPEED_METERS_PER_SECOND = RobotHardwareStats.isSimulation() ? 4.9 : 4.04502, + MAX_ROTATIONAL_SPEED_RADIANS_PER_SECOND = RobotHardwareStats.isSimulation() ? 12.03 : 12.03; private static final PIDConstants - TRANSLATION_PID_CONSTANTS = RobotConstants.IS_SIMULATION ? + TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(5, 0, 0) : new PIDConstants(5, 0, 0), - PROFILED_ROTATION_PID_CONSTANTS = RobotConstants.IS_SIMULATION ? - new PIDConstants(8, 0, 0) : - new PIDConstants(5, 0, 0), - AUTO_TRANSLATION_PID_CONSTANTS = RobotConstants.IS_SIMULATION ? + PROFILED_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? + new PIDConstants(4, 0, 0.05) : + new PIDConstants(5.6, 0, 0.12), + AUTO_TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(9, 0, 0) : - new PIDConstants(6.5, 0, 0), - AUTO_ROTATION_PID_CONSTANTS = RobotConstants.IS_SIMULATION ? + new PIDConstants(3, 0, 0), + AUTO_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(8.9, 0, 0) : new PIDConstants(3, 0, 0); private static final double - MAX_ROTATION_VELOCITY = RobotConstants.IS_SIMULATION ? 720 : 720, - MAX_ROTATION_ACCELERATION = RobotConstants.IS_SIMULATION ? 720 : 720; + MAX_ROTATION_VELOCITY = RobotHardwareStats.isSimulation() ? 720 : 700, + MAX_ROTATION_ACCELERATION = RobotHardwareStats.isSimulation() ? 720 : 660; private static final TrapezoidProfile.Constraints ROTATION_CONSTRAINTS = new TrapezoidProfile.Constraints( MAX_ROTATION_VELOCITY, MAX_ROTATION_ACCELERATION @@ -107,7 +113,7 @@ public abstract class SwerveConstants { AUTO_TRANSLATION_PID_CONSTANTS, AUTO_ROTATION_PID_CONSTANTS, MAX_ROTATIONAL_SPEED_RADIANS_PER_SECOND, - SwerveConstants.DRIVE_RADIUS_METERS, + SwerveConstants.FURTHEST_MODULE_DISTANCE_FROM_CENTER, REPLANNING_CONFIG ); @@ -119,6 +125,6 @@ public abstract class SwerveConstants { GYRO.applyConfiguration(config); GYRO.setSimulationYawVelocitySupplier(SIMULATION_YAW_VELOCITY_SUPPLIER); - GYRO.registerThreadedSignal(Pigeon2Signal.YAW, Pigeon2Signal.ANGULAR_VELOCITY_Z_WORLD, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + GYRO.registerThreadedSignal(Pigeon2Signal.YAW, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); } } diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java index fbc2f0b7..6259f04d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java @@ -1,11 +1,14 @@ package frc.trigon.robot.subsystems.swerve; import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; import com.ctre.phoenix6.controls.VoltageOut; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import frc.trigon.robot.constants.RobotConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants; import org.trigon.hardware.phoenix6.cancoder.CANcoderEncoder; @@ -20,19 +23,33 @@ public class SwerveModule { private final PositionVoltage steerPositionRequest = new PositionVoltage(0).withEnableFOC(SwerveModuleConstants.ENABLE_FOC); private final VelocityTorqueCurrentFOC driveVelocityRequest = new VelocityTorqueCurrentFOC(0); private final VoltageOut driveVoltageRequest = new VoltageOut(0); + private final TorqueCurrentFOC driveTorqueCurrentFOCRequest = new TorqueCurrentFOC(0); + private final double wheelDiameterMeters; private boolean driveMotorClosedLoop = false; private double[] latestOdometryDrivePositions = new double[0], latestOdometrySteerPositions = new double[0]; private SwerveModuleState targetState = new SwerveModuleState(); - public SwerveModule(int moduleID, double offsetRotations) { + public SwerveModule(int moduleID, double offsetRotations, double wheelDiameterMeters) { driveMotor = new TalonFXMotor(moduleID, "Module" + moduleID + "Drive", RobotConstants.CANIVORE_NAME); steerMotor = new TalonFXMotor(moduleID + 4, "Module" + moduleID + "Steer", RobotConstants.CANIVORE_NAME); steerEncoder = new CANcoderEncoder(moduleID, "Module" + moduleID + "SteerEncoder", RobotConstants.CANIVORE_NAME); + this.wheelDiameterMeters = wheelDiameterMeters; configureHardware(offsetRotations); } + void setTargetDriveMotorCurrent(double targetCurrent) { + driveMotor.setControl(driveTorqueCurrentFOCRequest.withOutput(targetCurrent)); + } + + void driveMotorUpdateLog(SysIdRoutineLog log) { + log.motor("Module" + driveMotor.getID() + "Drive") + .angularPosition(Units.Rotations.of(driveMotor.getSignal(TalonFXSignal.POSITION))) + .angularVelocity(Units.RotationsPerSecond.of(driveMotor.getSignal(TalonFXSignal.VELOCITY))) + .voltage(Units.Volts.of(driveMotor.getSignal(TalonFXSignal.TORQUE_CURRENT))); + } + void stop() { driveMotor.stopMotor(); steerMotor.stopMotor(); @@ -46,7 +63,7 @@ void setBrake(boolean brake) { void update() { driveMotor.update(); steerMotor.update(); - steerEncoder.update(); + latestOdometryDrivePositions = driveMotor.getThreadedSignal(TalonFXSignal.POSITION); latestOdometrySteerPositions = steerMotor.getThreadedSignal(TalonFXSignal.POSITION); } @@ -61,6 +78,14 @@ void setTargetState(SwerveModuleState targetState) { setTargetVelocity(this.targetState.speedMetersPerSecond, this.targetState.angle); } + void setTargetAngle(Rotation2d angle) { + steerMotor.setControl(steerPositionRequest.withPosition(angle.getRotations())); + } + + double getDriveWheelPosition() { + return edu.wpi.first.math.util.Units.rotationsToRadians(driveMotor.getSignal(TalonFXSignal.POSITION)); + } + /** * The odometry thread can update itself faster than the main code loop (which is 50 hertz). * Instead of using the latest odometry update, the accumulated odometry positions since the last loop to get a more accurate position. @@ -88,11 +113,7 @@ SwerveModuleState getTargetState() { } private double driveRotationsToMeters(double rotations) { - return Conversions.rotationsToDistance(rotations, SwerveModuleConstants.WHEEL_DIAMETER_METERS); - } - - private void setTargetAngle(Rotation2d angle) { - steerMotor.setControl(steerPositionRequest.withPosition(angle.getRotations())); + return Conversions.rotationsToDistance(rotations, wheelDiameterMeters); } /** @@ -111,7 +132,7 @@ private void setTargetVelocity(double targetVelocityMetersPerSecond, Rotation2d } private void setTargetClosedLoopVelocity(double targetVelocityMetersPerSecond) { - final double targetVelocityRotationsPerSecond = Conversions.distanceToRotations(targetVelocityMetersPerSecond, SwerveModuleConstants.WHEEL_DIAMETER_METERS); + final double targetVelocityRotationsPerSecond = Conversions.distanceToRotations(targetVelocityMetersPerSecond, wheelDiameterMeters); driveMotor.setControl(driveVelocityRequest.withVelocity(targetVelocityRotationsPerSecond)); } @@ -155,12 +176,16 @@ private void configureHardware(double offsetRotations) { } private void configureSignals() { - driveMotor.registerThreadedSignal(TalonFXSignal.POSITION, TalonFXSignal.VELOCITY, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + steerEncoder.registerSignal(CANcoderSignal.VELOCITY, 100); + steerEncoder.registerSignal(CANcoderSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + + driveMotor.registerSignal(TalonFXSignal.VELOCITY, 100); driveMotor.registerSignal(TalonFXSignal.TORQUE_CURRENT, 100); driveMotor.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); - steerMotor.registerThreadedSignal(TalonFXSignal.POSITION, TalonFXSignal.VELOCITY, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + driveMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + + steerMotor.registerSignal(TalonFXSignal.VELOCITY, 100); steerMotor.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); - steerEncoder.registerSignal(CANcoderSignal.POSITION, 100); - steerEncoder.registerSignal(CANcoderSignal.VELOCITY, 100); + steerMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java index 928a103a..c6bae68d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModuleConstants.java @@ -4,7 +4,9 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.*; import edu.wpi.first.math.system.plant.DCMotor; -import frc.trigon.robot.constants.RobotConstants; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import org.trigon.hardware.RobotHardwareStats; import org.trigon.hardware.simulation.SimpleMotorSimulation; public class SwerveModuleConstants { @@ -12,8 +14,8 @@ public class SwerveModuleConstants { DRIVE_GEAR_RATIO = 6.12, STEER_GEAR_RATIO = 12.8; private static final double - DRIVE_OPEN_LOOP_RAMP_RATE = RobotConstants.IS_SIMULATION ? 0.1 : 0.1, - DRIVE_CLOSED_LOOP_RAMP_RATE = RobotConstants.IS_SIMULATION ? 0.1 : 0.1; + DRIVE_OPEN_LOOP_RAMP_RATE = RobotHardwareStats.isSimulation() ? 0.1 : 0.2, + DRIVE_CLOSED_LOOP_RAMP_RATE = RobotHardwareStats.isSimulation() ? 0.1 : 0.2; private static final InvertedValue DRIVE_MOTOR_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive, STEER_MOTOR_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; @@ -23,16 +25,19 @@ public class SwerveModuleConstants { DRIVE_MOTOR_NEUTRAL_MODE_VALUE = NeutralModeValue.Brake, STEER_MOTOR_NEUTRAL_MODE_VALUE = NeutralModeValue.Brake; private static final double - DRIVE_SLIP_CURRENT = RobotConstants.IS_SIMULATION ? 1000 : 100, - STEER_CURRENT_LIMIT = RobotConstants.IS_SIMULATION ? 1000 : 50; + DRIVE_SLIP_CURRENT = RobotHardwareStats.isSimulation() ? 1000 : 80, + STEER_CURRENT_LIMIT = RobotHardwareStats.isSimulation() ? 1000 : 40; private static final double - STEER_MOTOR_P = RobotConstants.IS_SIMULATION ? 75 : 75, + STEER_MOTOR_P = RobotHardwareStats.isSimulation() ? 75 : 75, STEER_MOTOR_I = 0, STEER_MOTOR_D = 0; private static final double - DRIVE_MOTOR_P = RobotConstants.IS_SIMULATION ? 50 : 50, + DRIVE_MOTOR_P = RobotHardwareStats.isSimulation() ? 20 : 28, DRIVE_MOTOR_I = 0, - DRIVE_MOTOR_D = 0; + DRIVE_MOTOR_D = 0, + DRIVE_MOTOR_KS = RobotHardwareStats.isSimulation() ? 0.14031 : 6.1, + DRIVE_MOTOR_KV = RobotHardwareStats.isSimulation() ? 0.55781 : 0, + DRIVE_MOTOR_KA = RobotHardwareStats.isSimulation() ? 1.1359 : 2.7372; static final boolean ENABLE_FOC = true; static final TalonFXConfiguration DRIVE_MOTOR_CONFIGURATION = generateDriveConfiguration(), @@ -49,7 +54,13 @@ public class SwerveModuleConstants { DRIVE_MOTOR_GEARBOX = DCMotor.getKrakenX60Foc(DRIVE_MOTOR_AMOUNT), STEER_MOTOR_GEARBOX = DCMotor.getFalcon500Foc(STEER_MOTOR_AMOUNT); - static final double WHEEL_DIAMETER_METERS = RobotConstants.IS_SIMULATION ? 0.1016 : 0.049149 * 2; + static final SysIdRoutine.Config DRIVE_MOTOR_SYSID_CONFIG = new SysIdRoutine.Config( + Units.Volts.of(5).per(Units.Second), + Units.Volts.of(20), + Units.Second.of(1000) + ); + + static final double WHEEL_DIAMETER_METERS = RobotHardwareStats.isSimulation() ? 0.1016 : 0.048445 * 2; static final double VOLTAGE_COMPENSATION_SATURATION = 12; static SimpleMotorSimulation createDriveSimulation() { @@ -80,6 +91,9 @@ private static TalonFXConfiguration generateDriveConfiguration() { config.Slot0.kP = DRIVE_MOTOR_P; config.Slot0.kI = DRIVE_MOTOR_I; config.Slot0.kD = DRIVE_MOTOR_D; + config.Slot0.kS = DRIVE_MOTOR_KS; + config.Slot0.kV = DRIVE_MOTOR_KV; + config.Slot0.kA = DRIVE_MOTOR_KA; return config; } @@ -96,7 +110,7 @@ private static TalonFXConfiguration generateSteerConfiguration() { config.CurrentLimits.StatorCurrentLimitEnable = true; config.Feedback.RotorToSensorRatio = STEER_GEAR_RATIO; - config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; + config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; config.Slot0.kP = STEER_MOTOR_P; config.Slot0.kI = STEER_MOTOR_I; diff --git a/src/main/java/frc/trigon/robot/subsystems/transporter/Transporter.java b/src/main/java/frc/trigon/robot/subsystems/transporter/Transporter.java index a580f6e0..1dbb4311 100644 --- a/src/main/java/frc/trigon/robot/subsystems/transporter/Transporter.java +++ b/src/main/java/frc/trigon/robot/subsystems/transporter/Transporter.java @@ -8,6 +8,7 @@ import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.ledstrip.LEDStripCommands; import frc.trigon.robot.subsystems.ledstrip.LEDStripConstants; +import org.littletonrobotics.junction.AutoLogOutput; import org.trigon.hardware.misc.simplesensor.SimpleSensor; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; @@ -32,10 +33,14 @@ public void stop() { } @Override - public void periodic() { + public void updatePeriodically() { motor.update(); beamBreak.updateSensor(); - updateMechanism(); + } + + @Override + public void updateMechanism() { + TransporterConstants.MECHANISM.update(motor.getSignal(TalonFXSignal.MOTOR_VOLTAGE)); } public boolean isFeeding() { @@ -45,8 +50,9 @@ public boolean isFeeding() { targetState == TransporterConstants.TransporterState.SCORE_TRAP; } + @AutoLogOutput(key = "HasNote???") public boolean isNoteDetected() { - return beamBreak.getBinaryValue(); + return !beamBreak.getBinaryValue(); } void setTargetState(TransporterConstants.TransporterState targetState) { @@ -60,7 +66,7 @@ void setTargetVoltage(double targetVoltage) { } private void configureStoppingNoteCollectionTrigger() { - final Trigger trigger = new Trigger(beamBreak::getBinaryValue).debounce(TransporterConstants.NOTE_COLLECTION_THRESHOLD_SECONDS); + final Trigger trigger = new Trigger(this::isNoteDetected).debounce(TransporterConstants.NOTE_COLLECTION_THRESHOLD_SECONDS); trigger.whileTrue(new InstantCommand(() -> { if (!isCollecting() || this.getCurrentCommand() == null) return; @@ -78,9 +84,5 @@ private void configureStoppingNoteCollectionTrigger() { private boolean isCollecting() { return targetState == TransporterConstants.TransporterState.COLLECTING; } - - private void updateMechanism() { - TransporterConstants.MECHANISM.update(motor.getSignal(TalonFXSignal.MOTOR_VOLTAGE)); - } } diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index d4bf7dd6..63dacbb5 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "3.2.0", + "version": "3.2.1", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2024", "mavenUrls": [], @@ -10,24 +10,24 @@ { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "wpilib-shim", - "version": "3.2.0" + "version": "3.2.1" }, { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "junction-core", - "version": "3.2.0" + "version": "3.2.1" }, { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-api", - "version": "3.2.0" + "version": "3.2.1" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-wpilibio", - "version": "3.2.0", + "version": "3.2.1", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 0e80a16c..1d63e94f 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.3.1", + "version": "dev-v2024.3.0-87-g471c90e8", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.3.1", + "version": "dev-v2024.3.0-87-g471c90e8", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.3.1", + "version": "dev-v2024.3.0-87-g471c90e8", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.3.1" + "version": "dev-v2024.3.0-87-g471c90e8" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.3.1" + "version": "dev-v2024.3.0-87-g471c90e8" } ] } \ No newline at end of file