From a81a87703d029df9939bec0f666262d88369eeb9 Mon Sep 17 00:00:00 2001 From: BenG49 <64862590+BenG49@users.noreply.github.com> Date: Sat, 10 Feb 2024 11:28:17 -0500 Subject: [PATCH] Note detection fieldrelative (#8) * Port note vision from retep nr/note-detection * Make note aligned drive command * Revert "Merge branch 'main' into note-detection" This reverts commit 974ad00c0041399d93b720d59dcf87f8df67386d, reversing changes made to ac7eb66c3e99e0deac593a1f7ddd1798c3ac7f4a. * deadbanding alignment + filters * Port note vision from retep nr/note-detection * Make note aligned drive command * Revert "Merge branch 'main' into note-detection" This reverts commit 974ad00c0041399d93b720d59dcf87f8df67386d, reversing changes made to ac7eb66c3e99e0deac593a1f7ddd1798c3ac7f4a. * deadbanding alignment + filters * Merge main into note-detection * Merge main into note-detection * Remove random Settings.NoteAlignment * Note detection code * Make note translation field relative (unfinished) * Fix note field relative positioning * remove turn stream from note aligned drive, add center to intake front constant * Update method name * Add note detectionn test auto * Add DriveToAutoStart * Add NaN and large value check to odometry * Use distance to determine isFinished * Tune values * Run other side autons * add SwerveDriveDriveToChain for test * Fix swervedrive to chain --------- Co-authored-by: Naowal Rahman Co-authored-by: Ivan Chen Co-authored-by: Keobkeig --- .pathplanner/settings.json | 4 +- .../autos/Note Detection Test.auto | 43 +++++++ .../pathplanner/paths/B To A (ABC).path | 4 +- .../pathplanner/paths/C To B (ABC).path | 4 +- .../pathplanner/paths/E To Shoot (HFE).path | 14 +-- .../pathplanner/paths/F To Shoot (HFE).path | 20 ++-- .../pathplanner/paths/FShoot To E (HFE).path | 18 +-- .../pathplanner/paths/H To Shoot (HFE).path | 22 ++-- .../pathplanner/paths/HShoot To F (HFE).path | 20 ++-- .../paths/Note Detection Path.path | 49 ++++++++ .../pathplanner/paths/Start To C (ABC).path | 4 +- .../pathplanner/paths/Start To H (HFE).path | 20 ++-- .../paths/deploy/pathplanner/navgrid.json | 1 + .../com/stuypulse/robot/RobotContainer.java | 32 +++-- .../swerve/SwerveDriveDriveToChain.java | 62 ++++++++++ .../swerve/SwerveDriveDriveToNote.java | 80 +++++++++++++ .../swerve/SwerveDriveNoteAlignedDrive.java | 72 ++++++++++++ .../swerve/SwerveDriveToAutoStart.java | 1 - .../com/stuypulse/robot/constants/Field.java | 26 ++++ .../stuypulse/robot/constants/Settings.java | 62 ++++++++-- .../notevision/AbstractNoteVision.java | 28 +++++ .../subsystems/notevision/NoteVision.java | 111 ++++++++++++++++++ .../robot/subsystems/odometry/Odometry.java | 15 +++ .../robot/subsystems/swerve/SwerveDrive.java | 1 + .../com/stuypulse/robot/util/Limelight.java | 95 +++++++++++++++ 25 files changed, 722 insertions(+), 86 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Note Detection Test.auto create mode 100644 src/main/deploy/pathplanner/paths/Note Detection Path.path create mode 100644 src/main/deploy/pathplanner/paths/deploy/pathplanner/navgrid.json create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToChain.java create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/notevision/AbstractNoteVision.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java create mode 100644 src/main/java/com/stuypulse/robot/util/Limelight.java diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 13def40..fe0b595 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -7,8 +7,8 @@ "HFE" ], "autoFolders": [], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 3.0, + "defaultMaxVel": 2.0, + "defaultMaxAccel": 4.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "maxModuleSpeed": 4.5 diff --git a/src/main/deploy/pathplanner/autos/Note Detection Test.auto b/src/main/deploy/pathplanner/autos/Note Detection Test.auto new file mode 100644 index 0000000..5475dbb --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Note Detection Test.auto @@ -0,0 +1,43 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3033866247492334, + "y": 6.814737809886499 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intake acquire" + } + }, + { + "type": "path", + "data": { + "pathName": "Note Detection Path" + } + }, + { + "type": "named", + "data": { + "name": "drive to note" + } + }, + { + "type": "named", + "data": { + "name": "intake stop" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/B To A (ABC).path b/src/main/deploy/pathplanner/paths/B To A (ABC).path index e0deb94..7eeb80a 100644 --- a/src/main/deploy/pathplanner/paths/B To A (ABC).path +++ b/src/main/deploy/pathplanner/paths/B To A (ABC).path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/C To B (ABC).path b/src/main/deploy/pathplanner/paths/C To B (ABC).path index 644d100..ea2bb56 100644 --- a/src/main/deploy/pathplanner/paths/C To B (ABC).path +++ b/src/main/deploy/pathplanner/paths/C To B (ABC).path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/E To Shoot (HFE).path b/src/main/deploy/pathplanner/paths/E To Shoot (HFE).path index da00da5..2dfbf96 100644 --- a/src/main/deploy/pathplanner/paths/E To Shoot (HFE).path +++ b/src/main/deploy/pathplanner/paths/E To Shoot (HFE).path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 3.976354741378645, - "y": 5.106639879824356 + "x": 3.4613550257373955, + "y": 5.349605268437967 }, "prevControl": { - "x": 5.288791546143204, - "y": 4.49370079566982 + "x": 4.746249330823314, + "y": 4.380929158993735 }, "nextControl": null, "isLocked": false, @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 9.0, + "maxVelocity": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -45,5 +45,5 @@ "reversed": false, "folder": "HFE", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F To Shoot (HFE).path b/src/main/deploy/pathplanner/paths/F To Shoot (HFE).path index 5f9ec94..30a9cc2 100644 --- a/src/main/deploy/pathplanner/paths/F To Shoot (HFE).path +++ b/src/main/deploy/pathplanner/paths/F To Shoot (HFE).path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 7.929545854615797, + "x": 8.12, "y": 2.432016158384531 }, "prevControl": null, "nextControl": { - "x": 6.995066434251791, + "x": 7.185520579635994, "y": 3.4562610112362155 }, "isLocked": false, @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 3.8814838652626524, - "y": 4.827042510937235 + "x": 3.4, + "y": 5.21 }, "prevControl": { - "x": 4.259643243410917, - "y": 4.815422774902082 + "x": 4.524927668353058, + "y": 4.997546496479711 }, "nextControl": null, "isLocked": false, @@ -32,18 +32,18 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 9.0, + "maxVelocity": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0.0, - "rotation": -13.0, + "rotation": -7.742166844819546, "rotateFast": false }, "reversed": false, "folder": "HFE", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FShoot To E (HFE).path b/src/main/deploy/pathplanner/paths/FShoot To E (HFE).path index 6632f81..529ca26 100644 --- a/src/main/deploy/pathplanner/paths/FShoot To E (HFE).path +++ b/src/main/deploy/pathplanner/paths/FShoot To E (HFE).path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.8814838652626524, - "y": 4.827042510937235 + "x": 3.4, + "y": 5.21 }, "prevControl": null, "nextControl": { - "x": 5.639808720888058, - "y": 4.489969784247294 + "x": 4.787004771012038, + "y": 4.42620908196786 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 4.097291348201403 }, "prevControl": { - "x": 6.460969663475785, - "y": 4.359129446883026 + "x": 6.165251635651925, + "y": 4.174203465458797 }, "nextControl": null, "isLocked": false, @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 9.0, + "maxVelocity": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -45,5 +45,5 @@ "reversed": false, "folder": "HFE", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/H To Shoot (HFE).path b/src/main/deploy/pathplanner/paths/H To Shoot (HFE).path index 0b9e815..98e6258 100644 --- a/src/main/deploy/pathplanner/paths/H To Shoot (HFE).path +++ b/src/main/deploy/pathplanner/paths/H To Shoot (HFE).path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.940567464034234, - "y": 0.7841651445745021 + "x": 8.12, + "y": 0.91 }, "prevControl": null, "nextControl": { - "x": 5.970951958009818, - "y": 1.1314614999083628 + "x": 6.09651200885518, + "y": 1.3734386164887789 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.932248020664661, - "y": 3.0409052404747614 + "x": 2.5372885306588757, + "y": 3.23 }, "prevControl": { - "x": 4.464336906902614, - "y": 1.7553300211016838 + "x": 4.555375655754567, + "y": 1.9908154535276095 }, "nextControl": null, "isLocked": false, @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 9.0, + "maxVelocity": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -45,5 +45,5 @@ "reversed": false, "folder": "HFE", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HShoot To F (HFE).path b/src/main/deploy/pathplanner/paths/HShoot To F (HFE).path index 09bd3d4..142f822 100644 --- a/src/main/deploy/pathplanner/paths/HShoot To F (HFE).path +++ b/src/main/deploy/pathplanner/paths/HShoot To F (HFE).path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.932248020664661, - "y": 3.0409052404747614 + "x": 2.54, + "y": 3.23 }, "prevControl": null, "nextControl": { - "x": 5.429728048854969, - "y": 1.1066294807828339 + "x": 5.3948334681608605, + "y": 1.3399464381665318 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.929545854615797, + "x": 8.12, "y": 2.432016158384531 }, "prevControl": { - "x": 7.178392770949869, - "y": 2.432016158384531 + "x": 6.386539843490928, + "y": 1.755946563650941 }, "nextControl": null, "isLocked": false, @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.5, - "maxAcceleration": 9.0, + "maxVelocity": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -45,5 +45,5 @@ "reversed": false, "folder": "HFE", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Note Detection Path.path b/src/main/deploy/pathplanner/paths/Note Detection Path.path new file mode 100644 index 0000000..1c78c21 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Note Detection Path.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3033866247492334, + "y": 6.814737809886499 + }, + "prevControl": null, + "nextControl": { + "x": 2.421420613499129, + "y": 6.814737809886499 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.9788388950331144, + "y": 5.384346760266148 + }, + "prevControl": { + "x": 3.3859563993722896, + "y": 6.314936615260962 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 1.0, + "rotation": -40.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Start To C (ABC).path b/src/main/deploy/pathplanner/paths/Start To C (ABC).path index c008fd4..c6e9919 100644 --- a/src/main/deploy/pathplanner/paths/Start To C (ABC).path +++ b/src/main/deploy/pathplanner/paths/Start To C (ABC).path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Start To H (HFE).path b/src/main/deploy/pathplanner/paths/Start To H (HFE).path index a53726c..fc9333d 100644 --- a/src/main/deploy/pathplanner/paths/Start To H (HFE).path +++ b/src/main/deploy/pathplanner/paths/Start To H (HFE).path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 3.361176508628245, - "y": 2.06581107755857 + "x": 3.401375224923844, + "y": 2.135191701481494 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.940567464034234, - "y": 0.7841651445745021 + "x": 8.12, + "y": 0.91 }, "prevControl": { - "x": 5.745230507252463, - "y": 1.1502945514300327 + "x": 6.0473689598811475, + "y": 1.4872642457647487 }, "nextControl": null, "isLocked": false, @@ -32,18 +32,18 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 10.0, + "maxVelocity": 2.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": -37.52421356547046, "rotateFast": false }, "reversed": false, "folder": "HFE", "previewStartingState": null, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/paths/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..bab0da9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 97a3c3d..587fe1b 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -7,9 +7,10 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; -import com.pathplanner.lib.path.PathConstraints; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; -import com.stuypulse.robot.commands.swerve.SwerveDriveDriveToScore; +import com.stuypulse.robot.commands.swerve.SwerveDriveDriveToChain; +import com.stuypulse.robot.commands.swerve.SwerveDriveDriveToNote; +import com.stuypulse.robot.commands.swerve.SwerveDriveNoteAlignedDrive; import com.stuypulse.robot.commands.swerve.SwerveDriveResetHeading; import com.stuypulse.robot.commands.swerve.SwerveDriveToAutoStart; import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; @@ -23,14 +24,11 @@ import com.stuypulse.robot.commands.intake.IntakeAcquire; import com.stuypulse.robot.commands.intake.IntakeDeacquire; import com.stuypulse.robot.commands.intake.IntakeStop; -import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.subsystems.intake.*; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.input.gamepads.AutoGamepad; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -66,6 +64,11 @@ public RobotContainer() { private void configureNamedCommands() { NamedCommands.registerCommand("intake acquire", new IntakeAcquire()); + NamedCommands.registerCommand("intake stop", new IntakeStop()); + NamedCommands.registerCommand("drive to note", new SwerveDriveDriveToNote() + .alongWith(new IntakeAcquire()) + .andThen(new IntakeStop())); + NamedCommands.registerCommand("drive to score", new SwerveDriveToScore()); } /****************/ @@ -91,20 +94,27 @@ private void configureButtonBindings() { .onFalse(new IntakeStop()); driver.getLeftTriggerButton() - .onFalse(new IntakeDeacquire()) + .onTrue(new IntakeDeacquire()) .onFalse(new IntakeStop()); - driver.getBottomButton().whileTrue(new SwerveDriveToPose(new Pose2d(2, 5.5, new Rotation2d(Units.degreesToRadians(180))))); - // driver.getRightButton().whileTrue(new SwerveDriveToPose(new Pose2d(1.5, 5.5, new Rotation2d()))); + driver.getStartButton() + .whileTrue(new SwerveDriveToAutoStart(() -> autonChooser.getSelected().getName())); driver.getTopButton().whileTrue(new SwerveDriveWithAiming(Field.getFiducial(7).getPose().toPose2d(), driver)); + + driver.getRightBumper() + .whileTrue(new SwerveDriveNoteAlignedDrive(driver)); + + driver.getLeftBumper() + .whileTrue(new SwerveDriveDriveToNote()) + .whileTrue(new IntakeAcquire()) + .onFalse(new IntakeStop()); + // driver.getBottomButton().whileTrue(new SwerveDriveWithAiming(Field.getFiducial(8).getPose().toPose2d(), driver)); driver.getLeftButton().whileTrue(new SwerveDriveToScore()); driver.getRightButton().whileTrue(new SwerveDriveDriveToScore(driver)); // driver.getBottomButton().whileTrue(AutoBuilder.pathfindToPose(new Pose2d(), new PathConstraints(3, 4, Units.degreesToRadians(540), Units.degreesToRadians(720)), 0, 0)); - - driver.getStartButton() - .whileTrue(new SwerveDriveToAutoStart(() -> autonChooser.getSelected().getName())); + } /**************/ diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToChain.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToChain.java new file mode 100644 index 0000000..baa50a1 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToChain.java @@ -0,0 +1,62 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Settings.Alignment; +import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.stuylib.math.Vector2D; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveDriveToChain extends Command { + + private final AbstractOdometry odometry; + private final SwerveDrive swerve; + + private Pose2d trapPose; + + public SwerveDriveDriveToChain() { + odometry = AbstractOdometry.getInstance(); + swerve = SwerveDrive.getInstance(); + + addRequirements(swerve); + } + + @Override + public void initialize() { + trapPose = Field.getClosestAllianceTrapPose(odometry.getPose()); + + odometry.getField().getObject("Trap").setPose(trapPose); + } + + @Override + public void execute() { + Rotation2d translationAngle = trapPose.getTranslation().minus(odometry.getPose().getTranslation()).getAngle(); + + Translation2d translation = new Translation2d(Alignment.INTO_CHAIN_SPEED.get(), translationAngle); + + swerve.setChassisSpeeds(ChassisSpeeds.fromFieldRelativeSpeeds( + translation.getX(), translation.getY(), 0, odometry.getRotation())); + } + + private double getDistanceToTrap() { + return odometry.getPose().minus(trapPose).getTranslation().getNorm(); + } + + @Override + public boolean isFinished() { + return getDistanceToTrap() <= Alignment.TRAP_CLIMB_DISTANCE.get(); + } + + @Override + public void end(boolean interrupted) { + swerve.stop(); + odometry.getField().getObject("Trap").setPose(new Pose2d(Double.NaN, Double.NaN, new Rotation2d(Double.NaN))); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java new file mode 100644 index 0000000..993a44c --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveToNote.java @@ -0,0 +1,80 @@ +/************************ PROJECT JIM *************************/ +/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved.*/ +/* This work is licensed under the terms of the MIT license. */ +/**************************************************************/ + +package com.stuypulse.robot.commands.swerve; + +import static com.stuypulse.robot.constants.Settings.NoteDetection.*; + +import com.stuypulse.robot.constants.Settings.NoteDetection.Rotation; +import com.stuypulse.robot.constants.Settings.NoteDetection.Translation; +import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.robot.subsystems.notevision.AbstractNoteVision; +import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.util.HolonomicController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveDriveToNote extends Command { + + // Subsystems + private final SwerveDrive swerve; + private final AbstractOdometry odometry; + private final AbstractNoteVision vision; + + // Holonomic control + private final HolonomicController controller; + + public SwerveDriveDriveToNote(){ + this.swerve = SwerveDrive.getInstance(); + this.odometry = AbstractOdometry.getInstance(); + this.vision = AbstractNoteVision.getInstance(); + + controller = new HolonomicController( + new PIDController(Translation.P,Translation.I,Translation.D), + new PIDController(Translation.P, Translation.I, Translation.D), + new AnglePIDController(Rotation.P, Rotation.I, Rotation.D) + ); + + SmartDashboard.putData("Note Detection/Controller", controller); + + addRequirements(swerve); + } + + @Override + public void execute() { + Translation2d targetTranslation = odometry.getTranslation().plus( + new Translation2d(Units.inchesToMeters(18), 0).rotateBy(odometry.getRotation())); + + Rotation2d targetRotation = vision.getEstimatedNoteTranslation().minus(targetTranslation).getAngle(); + + Pose2d targetPose = new Pose2d(targetTranslation, targetRotation); + + if (!vision.hasNoteData()) { + swerve.setChassisSpeeds(controller.update(targetPose, new Pose2d(targetTranslation, odometry.getRotation()))); + } else { + swerve.setChassisSpeeds(controller.update(targetPose, odometry.getPose())); + } + } + + @Override + public boolean isFinished() { + return vision.getEstimatedNoteTranslation().minus(odometry.getTranslation()).getNorm() < CUTOFF_DISTANCE; + } + + @Override + public void end(boolean interrupted) { + swerve.stop(); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java new file mode 100644 index 0000000..a6df51d --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveNoteAlignedDrive.java @@ -0,0 +1,72 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Driver.Drive; +import com.stuypulse.robot.constants.Settings.NoteDetection; +import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.robot.subsystems.notevision.AbstractNoteVision; +import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.stuylib.control.angle.AngleController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.stuylib.math.Angle; +import com.stuypulse.stuylib.streams.vectors.VStream; +import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone; +import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; +import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveNoteAlignedDrive extends Command { + + private final SwerveDrive swerve; + private final AbstractOdometry odometry; + private final AbstractNoteVision noteVision; + + private final VStream speed; + + private final AngleController alignController; + + public SwerveDriveNoteAlignedDrive(Gamepad driver) { + swerve = SwerveDrive.getInstance(); + odometry = AbstractOdometry.getInstance(); + noteVision = AbstractNoteVision.getInstance(); + + speed = VStream.create(driver::getLeftStick) + .filtered( + new VDeadZone(Drive.DEADBAND), + x -> x.clamp(1.0), + x -> Settings.vpow(x, Drive.POWER.get()), + x -> x.mul(Drive.MAX_TELEOP_SPEED.get()), + new VRateLimit(Drive.MAX_TELEOP_ACCEL), + new VLowPassFilter(Drive.RC) + ); + + alignController = new AnglePIDController(NoteDetection.Rotation.P, NoteDetection.Rotation.I, NoteDetection.Rotation.D); + + addRequirements(swerve); + } + + @Override + public void execute() { + Translation2d targetTranslation = odometry.getTranslation().plus( + new Translation2d(Swerve.CENTER_TO_INTAKE_FRONT, 0).rotateBy(odometry.getRotation())); + + Rotation2d targetRotation = noteVision.getEstimatedNoteTranslation().minus(targetTranslation).getAngle(); + + double angularVel = -alignController.update( + Angle.fromRotation2d(targetRotation), + Angle.fromRotation2d(odometry.getRotation()) + ); + + // robot relative + swerve.drive(speed.get(), angularVel); + + SmartDashboard.putNumber("Note Detection/Angle Output", alignController.getOutput()); + + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToAutoStart.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToAutoStart.java index 9fb8940..e7bc66d 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToAutoStart.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToAutoStart.java @@ -88,5 +88,4 @@ public void end(boolean interupted) { swerve.stop(); targetPose2d.setPose(Double.NaN, Double.NaN, new Rotation2d(Double.NaN)); } - } diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 1c54258..ae42b72 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -8,6 +8,7 @@ import com.stuypulse.stuylib.network.SmartNumber; import java.util.ArrayList; +import java.util.Arrays; import com.stuypulse.robot.util.Fiducial; @@ -108,4 +109,29 @@ public static Pose2d getSpeakerPose() { boolean isBlue = DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == DriverStation.Alliance.Blue; return SPEAKER_POSES[isBlue ? 0 : 1]; } + + /*** TRAP ***/ + + Pose2d TRAP_POSES[][] = { + // BLUE + { + getFiducial(14).getPose().toPose2d(), + getFiducial(15).getPose().toPose2d(), + getFiducial(16).getPose().toPose2d() + }, + // RED + { + getFiducial(11).getPose().toPose2d(), + getFiducial(12).getPose().toPose2d(), + getFiducial(13).getPose().toPose2d() + } + }; + public static Pose2d[] getAllianceTrapPoses() { + boolean isBlue = DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == DriverStation.Alliance.Blue; + return TRAP_POSES[isBlue ? 0 : 1]; + } + + public static Pose2d getClosestAllianceTrapPose(Pose2d robotPose) { + return robotPose.nearest(Arrays.asList(getAllianceTrapPoses())); + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index c69d84e..4688552 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -10,8 +10,11 @@ import com.stuypulse.stuylib.network.SmartBoolean; import com.stuypulse.stuylib.network.SmartNumber; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; /*- @@ -23,7 +26,7 @@ public interface Settings { public enum RobotType { JIM("03262B9F"), // DeviceCode=0x7AAE@EDITOR=vi@PWD=/@TERM=dumb@DeviceDesc=roboRIO 2.0@SHLVL=3@TargetClass=cRIO@serialnum=03262B9F@PATH=/usr/local/bin:/usr/bin:/bin:/usr/local/frc/bin:/usr/local/natinst/bin@FPGADeviceCode=0x7AAF@_=/usr/local/frc/JRE/bin/java - OFFSEASON_BOT("0305a69d"), // DeviceCode=0x76F2@PWD=/usr/local/natinst/labview@DeviceDesc=roboRIO@SHLVL=2@TargetClass=cRIO@serialnum=0305a69d@FPGADeviceCode=0x77A9@_=/sbin/start-stop-daemon@ + OFFSEASON_BOT("0305A69D"), // DeviceCode=0x76F2@PWD=/usr/local/natinst/labview@DeviceDesc=roboRIO@SHLVL=2@TargetClass=cRIO@serialnum=0305a69d@FPGADeviceCode=0x77A9@_=/sbin/start-stop-daemon@ SIM(""); public final String serialNum; @@ -34,7 +37,7 @@ public enum RobotType { public static RobotType fromString(String serialNum) { for (RobotType robot : RobotType.values()) { - if (robot.serialNum.equals(serialNum)) { + if (robot.serialNum.equals(serialNum.toUpperCase())) { return robot; } } @@ -48,6 +51,7 @@ public static RobotType fromString(String serialNum) { public interface Swerve { double WIDTH = Units.inchesToMeters(21); double LENGTH = Units.inchesToMeters(21); + double CENTER_TO_INTAKE_FRONT = Units.inchesToMeters(18); SmartNumber MODULE_VELOCITY_DEADBAND = new SmartNumber("Swerve/Module velocity deadband (m per s)", 0.05); double MAX_MODULE_SPEED = 5.88; @@ -112,7 +116,7 @@ public interface Drive { } } } - + public interface Driver { public interface Drive { SmartNumber DEADBAND = new SmartNumber("Driver Settings/Drive/Deadband", 0.03); @@ -140,11 +144,6 @@ public interface GyroFeedback { SmartNumber D = new SmartNumber("Driver Settings/Gyro Feedback/kD", 0.1); } } - - } - - public static Vector2D vpow(Vector2D vec, double power) { - return vec.mul(Math.pow(vec.magnitude(), power - 1)); } public interface Intake { @@ -155,8 +154,49 @@ public interface Intake { SmartNumber DEACQUIRE_SPEED_BOTTOM = new SmartNumber("Intake/Deacquire Speed Bottom", -1); } - public interface Alignment { + public interface Limelight { + String [] LIMELIGHTS = { + "limelight" + }; + + int[] PORTS = {5800, 5801, 5802, 5803, 5804, 5805}; + Pose3d [] POSITIONS = new Pose3d[] { + new Pose3d( + new Translation3d(Units.inchesToMeters(3), 0, Units.inchesToMeters(13.75)), + new Rotation3d(0, Math.toRadians(8), Math.toRadians(2))) + }; + } + + public static Vector2D vpow(Vector2D vec, double power) { + return vec.mul(Math.pow(vec.magnitude(), power - 1)); + } + + public interface NoteDetection { + + double CUTOFF_DISTANCE = Units.inchesToMeters(22); + + SmartNumber DEBOUNCE_TIME = new SmartNumber("Note Detection/Debounce Time", 0.15); + SmartNumber X_ANGLE_RC = new SmartNumber("Note Detection/X Angle RC", 0.05); + SmartNumber TARGET_NOTE_DISTANCE = new SmartNumber("Note Detection/Target Note Distance", 0.5); + + SmartNumber THRESHOLD_X = new SmartNumber("Note Detection/X Threshold", 0.2); + SmartNumber THRESHOLD_Y = new SmartNumber("Note Detection/Y Threshold", Units.inchesToMeters(2)); + SmartNumber THRESHOLD_ANGLE = new SmartNumber("Note Detection/Angle Threshold", 1); + + public interface Translation { + SmartNumber P = new SmartNumber("Note Detection/Translation/kP", 6.0); + SmartNumber I = new SmartNumber("Note Detection/Translation/kI", 0.0); + SmartNumber D = new SmartNumber("Note Detection/Translation/kD", 0.15); + } + public interface Rotation { + SmartNumber P = new SmartNumber("Note Detection/Rotation/kP", 3.0); + SmartNumber I = new SmartNumber("Note Detection/Rotation/kI", 0.0); + SmartNumber D = new SmartNumber("Note Detection/Rotation/kD", 0.03); + } + } + + public interface Alignment { SmartNumber DEBOUNCE_TIME = new SmartNumber("Alignment/Debounce Time", 0.15); SmartNumber X_TOLERANCE = new SmartNumber("Alignment/X Tolerance", 0.05); SmartNumber Y_TOLERANCE = new SmartNumber("Alignment/Y Tolerance", 0.05); @@ -165,6 +205,10 @@ public interface Alignment { SmartNumber TARGET_DISTANCE_IN = new SmartNumber("Alignment/Target Distance (in)", 110); SmartNumber TAKEOVER_DISTANCE_IN = new SmartNumber("Alignment/Takeover Distance (in)", 50); + SmartNumber TRAP_SETUP_DISTANCE = new SmartNumber("Alignment/Trap/Setup Pose Distance", Units.inchesToMeters(22.0)); + SmartNumber TRAP_CLIMB_DISTANCE = new SmartNumber("Alignment/Trap/Climb Distance", Units.inchesToMeters(18.0)); + + SmartNumber INTO_CHAIN_SPEED = new SmartNumber("Alignment/Trap/Into Chain Speed", 0.25); public interface Translation { SmartNumber P = new SmartNumber("Alignment/Translation/kP", 2.5); SmartNumber I = new SmartNumber("Alignment/Translation/kI", 0); diff --git a/src/main/java/com/stuypulse/robot/subsystems/notevision/AbstractNoteVision.java b/src/main/java/com/stuypulse/robot/subsystems/notevision/AbstractNoteVision.java new file mode 100644 index 0000000..022b133 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/notevision/AbstractNoteVision.java @@ -0,0 +1,28 @@ +/************************ PROJECT JIM *************************/ +/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved.*/ +/* This work is licensed under the terms of the MIT license. */ +/**************************************************************/ + +package com.stuypulse.robot.subsystems.notevision; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public abstract class AbstractNoteVision extends SubsystemBase { + + /** SINGLETON **/ + private static final AbstractNoteVision instance; + + static { + instance = new NoteVision(); + } + + public static AbstractNoteVision getInstance() { + return instance; + } + + public abstract boolean hasNoteData(); + + public abstract Translation2d getEstimatedNoteTranslation(); + +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java b/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java new file mode 100644 index 0000000..b92492d --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/notevision/NoteVision.java @@ -0,0 +1,111 @@ +/************************ PROJECT JIM *************************/ +/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved.*/ +/* This work is licensed under the terms of the MIT license. */ +/**************************************************************/ + +package com.stuypulse.robot.subsystems.notevision; + +import static com.stuypulse.robot.constants.Settings.Limelight.*; + +import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; +import com.stuypulse.robot.subsystems.odometry.Odometry; +import com.stuypulse.robot.util.Limelight; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.net.PortForwarder; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class NoteVision extends AbstractNoteVision { + + // store limelight network tables + private final Limelight[] limelights; + + // store fieldobject2d's to display limelight data + private final FieldObject2d[] limelightPoses; + + private Translation2d notePose; + + private FieldObject2d note2d; + + protected NoteVision() { + // reference to all limelights on robot + String[] hostNames = LIMELIGHTS; + + // setup limelight objects and field objects for april tag data + limelights = new Limelight[hostNames.length]; + limelightPoses = new FieldObject2d[limelights.length]; + + // setup all objects + Field2d field = Odometry.getInstance().getField(); + for(int i = 0; i < hostNames.length; i++){ + limelights[i] = new Limelight(hostNames[i], POSITIONS[i]); + limelightPoses[i] = field.getObject(hostNames[i] + " pose"); + + for (int port : PORTS) { + PortForwarder.add(port + i * 10, hostNames[i] + ".local", port); + } + } + + note2d = AbstractOdometry.getInstance().getField().getObject("Note"); + + notePose = new Translation2d(); + } + + @Override + public boolean hasNoteData() { + for (Limelight limelight : limelights) { + if (limelight.hasNoteData()) + return true; + } + + return false; + } + + @Override + public Translation2d getEstimatedNoteTranslation() { + return notePose; + } + + public void updateNotePose() { + Translation2d sum = new Translation2d(); + + for (Limelight limelight : limelights) { + AbstractOdometry odometry = AbstractOdometry.getInstance(); + + Translation2d limelightToNote = new Translation2d(limelight.getDistanceToNote(), Rotation2d.fromDegrees(limelight.getXAngle())); + + Translation2d robotToNote = limelightToNote + .minus(limelight.robotRelativePose.getTranslation().toTranslation2d()) + .rotateBy(limelight.robotRelativePose.getRotation().toRotation2d()); + + Translation2d fieldToNote = robotToNote.rotateBy(odometry.getRotation()).plus(odometry.getTranslation()); + + sum = sum.plus(fieldToNote); + } + + notePose = sum.div(limelights.length); + } + + @Override + public void periodic() { + for (int i = 0; i < limelights.length; ++i) { + limelights[i].updateData(); + } + + note2d.setPose(new Pose2d(notePose, new Rotation2d())); + + if (hasNoteData()) { + updateNotePose(); + + SmartDashboard.putNumber("Note Detection/X Angle", limelights[0].getXAngle()); + SmartDashboard.putNumber("Note Detection/Y Angle", limelights[0].getYAngle()); + SmartDashboard.putNumber("Note Detection/Distance", limelights[0].getDistanceToNote()); + SmartDashboard.putNumber("Note Detection/Estimated X", notePose.getX()); + SmartDashboard.putNumber("Note Detection/Estimated Y", notePose.getY()); + } + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java b/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java index e3ef34e..23fa8fe 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java +++ b/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java @@ -1,5 +1,6 @@ package com.stuypulse.robot.subsystems.odometry; +import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.robot.subsystems.vision.AbstractVision; import com.stuypulse.robot.util.Fiducial; @@ -8,6 +9,7 @@ import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; @@ -23,6 +25,7 @@ public class Odometry extends AbstractOdometry { private final FieldObject2d estimatorPose2D; private final SmartBoolean VISION_ACTIVE; + private Pose2d lastGoodPose; protected Odometry() { SwerveDrive swerve = SwerveDrive.getInstance(); @@ -48,6 +51,8 @@ protected Odometry() { swerve.initModule2ds(field); SmartDashboard.putData("Field", field); VISION_ACTIVE = new SmartBoolean("Odometry/Vision Active", true); + + lastGoodPose = new Pose2d(); } @Override @@ -97,6 +102,16 @@ public void periodic() { } } + if (estimator.getEstimatedPosition().getTranslation().getNorm() > new Translation2d(Field.WIDTH, Field.HEIGHT).getNorm() || + odometry.getPoseMeters().getTranslation().getNorm() > new Translation2d(Field.WIDTH, Field.HEIGHT).getNorm() || + estimator.getEstimatedPosition().getX() == Double.NaN || estimator.getEstimatedPosition().getY() == Double.NaN || + odometry.getPoseMeters().getX() == Double.NaN || odometry.getPoseMeters().getY() == Double.NaN + ) { + reset(lastGoodPose); + } else { + lastGoodPose = getPose(); + } + odometryPose2D.setPose(odometry.getPoseMeters()); estimatorPose2D.setPose(estimator.getEstimatedPosition()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index 88d56c4..403df28 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -141,6 +141,7 @@ public SwerveModulePosition[] getModulePositions() { for (int i = 0; i < modules.length; i++) { positions[i] = modules[i].getModulePosition(); + SmartDashboard.putNumber("Swerve/Modules/" + modules[i].getID() + "/Position", modules[i].getModulePosition().distanceMeters); } return positions; diff --git a/src/main/java/com/stuypulse/robot/util/Limelight.java b/src/main/java/com/stuypulse/robot/util/Limelight.java new file mode 100644 index 0000000..3baa752 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/Limelight.java @@ -0,0 +1,95 @@ +/************************ PROJECT JIM *************************/ +/* Copyright (c) 2023 StuyPulse Robotics. All rights reserved.*/ +/* This work is licensed under the terms of the MIT license. */ +/**************************************************************/ + +package com.stuypulse.robot.util; + +import static com.stuypulse.robot.constants.Settings.Limelight.LIMELIGHTS; +import static com.stuypulse.robot.constants.Settings.Limelight.POSITIONS; + +import com.stuypulse.robot.constants.Settings.NoteDetection; +import com.stuypulse.stuylib.network.SmartNumber; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; +import com.stuypulse.stuylib.streams.numbers.IStream; +import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.DoubleEntry; +import edu.wpi.first.networktables.IntegerEntry; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; + + +public class Limelight { + + private final String tableName; + + private final DoubleEntry txEntry; + private final DoubleEntry tyEntry; + private final IntegerEntry tvEntry; + + private int limelightId; + + private double txData; + private double tyData; + + private IStream xAngle; + private BStream noteData; + + public final Pose3d robotRelativePose; + + public Limelight(String tableName, Pose3d robotRelativePose) { + this.tableName = tableName; + this.robotRelativePose = robotRelativePose; + + for(int i = 0; i < LIMELIGHTS.length; i++) { + if(LIMELIGHTS[i].equals(tableName)) { + limelightId = i; + } + } + + NetworkTable limelight = NetworkTableInstance.getDefault().getTable(tableName); + + + txEntry = limelight.getDoubleTopic("tx").getEntry(0.0); + tyEntry = limelight.getDoubleTopic("ty").getEntry(0.0); + tvEntry = limelight.getIntegerTopic("tv").getEntry(0); + + xAngle = IStream.create(() -> txData) + .filtered(new LowPassFilter(NoteDetection.X_ANGLE_RC)); + noteData = BStream.create(() -> tvEntry.get() == 1) + .filtered(new BDebounceRC.Rising(NoteDetection.DEBOUNCE_TIME)); + } + + public String getTableName() { + return tableName; + } + + public boolean hasNoteData() { + return noteData.get(); + } + + public void updateData() { + if (tvEntry.get() == 1) { + txData = txEntry.get(); + tyData = tyEntry.get(); + } + } + + public double getXAngle() { + return -xAngle.get() + Units.radiansToDegrees(POSITIONS[limelightId].getRotation().getZ()); + } + + public double getYAngle() { + return tyData - Units.radiansToDegrees(POSITIONS[limelightId].getRotation().getY()); + } + + public double getDistanceToNote() { + Rotation2d yRotation = Rotation2d.fromDegrees(getYAngle()); + return POSITIONS[limelightId].getZ() / -yRotation.getTan() + POSITIONS[limelightId].getX() - Units.inchesToMeters(14.0 / 2.0); + } +} \ No newline at end of file