diff --git a/elastic-layout.json b/elastic-layout.json new file mode 100644 index 00000000..86d81634 --- /dev/null +++ b/elastic-layout.json @@ -0,0 +1,165 @@ +{ + "version": 1.0, + "grid_size": 128, + "tabs": [ + { + "name": "Teleoperated", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "BatteryVoltage", + "x": 0.0, + "y": 512.0, + "width": 384.0, + "height": 128.0, + "type": "Voltage View", + "properties": { + "topic": "/AdvantageKit/SystemStats/BatteryVoltage", + "period": 0.06, + "data_type": "double", + "min_value": 4.0, + "max_value": 13.0, + "divisions": 5, + "inverted": false, + "orientation": "horizontal" + } + }, + { + "title": "limelight", + "x": 896.0, + "y": 0.0, + "width": 640.0, + "height": 384.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/limelight", + "period": 0.06, + "rotation_turns": 0 + } + }, + { + "title": "Field", + "x": 0.0, + "y": 0.0, + "width": 512.0, + "height": 384.0, + "type": "Field", + "properties": { + "topic": "/SmartDashboard/Field", + "period": 0.06, + "field_game": "Reefscape", + "robot_width": 0.85, + "robot_length": 0.85, + "show_other_objects": true, + "show_trajectories": true, + "field_rotation": 0.0, + "robot_color": 4294198070, + "trajectory_color": 4294967295 + } + }, + { + "title": "FMSInfo", + "x": 0.0, + "y": 384.0, + "width": 384.0, + "height": 128.0, + "type": "FMSInfo", + "properties": { + "topic": "/FMSInfo", + "period": 0.06 + } + }, + { + "title": "MatchTime", + "x": 512.0, + "y": 0.0, + "width": 384.0, + "height": 256.0, + "type": "Match Time", + "properties": { + "topic": "/AdvantageKit/DriverStation/MatchTime", + "period": 0.06, + "data_type": "double", + "time_display_mode": "Minutes and Seconds", + "red_start_time": 15, + "yellow_start_time": 30 + } + }, + { + "title": "Auto Chooser", + "x": 512.0, + "y": 256.0, + "width": 384.0, + "height": 128.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/SmartDashboard/Auto Chooser", + "period": 0.06, + "sort_options": false + } + }, + { + "title": "Alerts", + "x": 384.0, + "y": 384.0, + "width": 384.0, + "height": 256.0, + "type": "Alerts", + "properties": { + "topic": "/SmartDashboard/Alerts", + "period": 0.06 + } + }, + { + "title": "limelight-twoplus", + "x": 896.0, + "y": 384.0, + "width": 640.0, + "height": 384.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/limelight-twoplus", + "period": 0.06, + "rotation_turns": 0 + } + }, + { + "title": "Coral Slot Position", + "x": 768.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Large Text Display", + "properties": { + "topic": "/AdvantageKit/RealOutputs/CoralIntake/Coral Slot Position", + "period": 0.06, + "data_type": "int" + } + } + ] + } + }, + { + "name": "Autonomous", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Auto Chooser", + "x": 640.0, + "y": 256.0, + "width": 384.0, + "height": 128.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/SmartDashboard/Auto Chooser", + "period": 0.06, + "sort_options": false + } + } + ] + } + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3 Piece April Tag 9 7 8 .auto b/src/main/deploy/pathplanner/autos/3 Piece April Tag 9 7 8 .auto new file mode 100644 index 00000000..187afb4a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3 Piece April Tag 9 7 8 .auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Proc3P Part 1" + } + }, + { + "type": "path", + "data": { + "pathName": "Proc3P Part 2" + } + }, + { + "type": "path", + "data": { + "pathName": "Proc3P Part 3" + } + }, + { + "type": "path", + "data": { + "pathName": "Proc3P Part 4" + } + }, + { + "type": "path", + "data": { + "pathName": "Proc3P Part 5" + } + }, + { + "type": "path", + "data": { + "pathName": "Proc3P Part 6" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/One Piece Barge.auto b/src/main/deploy/pathplanner/autos/One Piece Barge.auto new file mode 100644 index 00000000..7be283cf --- /dev/null +++ b/src/main/deploy/pathplanner/autos/One Piece Barge.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "OneP Center" + } + }, + { + "type": "path", + "data": { + "pathName": "One piece part 2 barge" + } + }, + { + "type": "path", + "data": { + "pathName": "One piece part 3 barge" + } + }, + { + "type": "path", + "data": { + "pathName": "One piece part 4 barge" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Proc3P Back.auto b/src/main/deploy/pathplanner/autos/Proc3P Back.auto index 28c84e80..00106838 100644 --- a/src/main/deploy/pathplanner/autos/Proc3P Back.auto +++ b/src/main/deploy/pathplanner/autos/Proc3P Back.auto @@ -31,7 +31,7 @@ { "type": "path", "data": { - "pathName": "Proc Part 5 Back" + "pathName": "Proc3P Part 5" } }, { diff --git a/src/main/deploy/pathplanner/paths/Non Proc 3P Part 1.path b/src/main/deploy/pathplanner/paths/Non Proc 3P Part 1.path index b7dc0e8e..6f1cb69e 100644 --- a/src/main/deploy/pathplanner/paths/Non Proc 3P Part 1.path +++ b/src/main/deploy/pathplanner/paths/Non Proc 3P Part 1.path @@ -4,24 +4,24 @@ { "anchor": { "x": 7.480327868852459, - "y": 6.744 + "y": 6.51 }, "prevControl": null, "nextControl": { - "x": 6.693459821961271, - "y": 6.435926031716894 + "x": 6.403623204075984, + "y": 6.070663406486054 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.149692622950819, - "y": 6.151 + "x": 5.814036885245901, + "y": 5.757223360655737 }, "prevControl": { - "x": 6.803183415463562, - "y": 6.404541372953622 + "x": 6.415737410077909, + "y": 6.004650025675354 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Non Proc 3P Part 2.path b/src/main/deploy/pathplanner/paths/Non Proc 3P Part 2.path index e3acbfb9..7d262423 100644 --- a/src/main/deploy/pathplanner/paths/Non Proc 3P Part 2.path +++ b/src/main/deploy/pathplanner/paths/Non Proc 3P Part 2.path @@ -41,13 +41,13 @@ "unlimited": false }, "goalEndState": { - "velocity": 0.0, + "velocity": 0.5, "rotation": 131.83680398463187 }, "reversed": false, "folder": null, "idealStartingState": { - "velocity": 0, + "velocity": 0.0, "rotation": -123.01789886636891 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Non Proc 3P Part 3.path b/src/main/deploy/pathplanner/paths/Non Proc 3P Part 3.path index b78eb4b9..ad14f7f4 100644 --- a/src/main/deploy/pathplanner/paths/Non Proc 3P Part 3.path +++ b/src/main/deploy/pathplanner/paths/Non Proc 3P Part 3.path @@ -42,7 +42,7 @@ }, "goalEndState": { "velocity": 2.0, - "rotation": 1.3342198686546403 + "rotation": -5.230938713244494 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/Non Proc 3P Part 4.path b/src/main/deploy/pathplanner/paths/Non Proc 3P Part 4.path index 07a04a26..110165d8 100644 --- a/src/main/deploy/pathplanner/paths/Non Proc 3P Part 4.path +++ b/src/main/deploy/pathplanner/paths/Non Proc 3P Part 4.path @@ -41,7 +41,7 @@ "unlimited": false }, "goalEndState": { - "velocity": 0.0, + "velocity": 0.5, "rotation": 127.06544922742414 }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Non Proc 3P Part 6.path b/src/main/deploy/pathplanner/paths/Non Proc 3P Part 6.path index 2b10d35f..9fbbcc96 100644 --- a/src/main/deploy/pathplanner/paths/Non Proc 3P Part 6.path +++ b/src/main/deploy/pathplanner/paths/Non Proc 3P Part 6.path @@ -41,7 +41,7 @@ "unlimited": false }, "goalEndState": { - "velocity": 0, + "velocity": 0.5, "rotation": 128.49168164709494 }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/One piece part 2 barge.path b/src/main/deploy/pathplanner/paths/One piece part 2 barge.path new file mode 100644 index 00000000..bc78034a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/One piece part 2 barge.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.778073770491804, + "y": 4.186834016393442 + }, + "prevControl": null, + "nextControl": { + "x": 6.602930227919898, + "y": 4.010654731384542 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.521380450274493, + "y": 4.020078421927787 + }, + "prevControl": { + "x": 5.683538694167147, + "y": 4.226226857329675 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.752, + "maxAcceleration": 2.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/One piece part 3 barge.path b/src/main/deploy/pathplanner/paths/One piece part 3 barge.path new file mode 100644 index 00000000..deb153fe --- /dev/null +++ b/src/main/deploy/pathplanner/paths/One piece part 3 barge.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.790061475409836, + "y": 4.00701844262295 + }, + "prevControl": null, + "nextControl": { + "x": 6.960536674076111, + "y": 3.9176740761456714 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.4084016393442615, + "y": 5.133862704918033 + }, + "prevControl": { + "x": 7.160265360572086, + "y": 5.103393258630037 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.752, + "maxAcceleration": 2.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 540.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -179.3634064240365 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/One piece part 4 barge.path b/src/main/deploy/pathplanner/paths/One piece part 4 barge.path new file mode 100644 index 00000000..fe3fbf64 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/One piece part 4 barge.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.408, + "y": 5.134 + }, + "prevControl": null, + "nextControl": { + "x": 6.317118334326491, + "y": 5.150351736239266 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.945901639344262, + "y": 6.692264344262294 + }, + "prevControl": { + "x": 5.97482823306017, + "y": 6.443943478896545 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.752, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 125.21759296819275 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/OneP Center.path b/src/main/deploy/pathplanner/paths/OneP Center.path index b858cad8..16ffce48 100644 --- a/src/main/deploy/pathplanner/paths/OneP Center.path +++ b/src/main/deploy/pathplanner/paths/OneP Center.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.61219262295082, + "x": 7.252561475409835, "y": 4.168852459016394 }, "prevControl": null, "nextControl": { - "x": 6.422023805391631, - "y": 4.192444655441673 + "x": 6.624777998655663, + "y": 4.174562042674747 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.171749999999999, + "x": 6.3534836065573765, "y": 4.168852459016394 }, "prevControl": { - "x": 7.062123889342438, - "y": 4.185162909488503 + "x": 7.00914605019554, + "y": 4.1579986667419675 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Proc Part 3 Back.path b/src/main/deploy/pathplanner/paths/Proc Part 3 Back.path index bc3ef8fb..54850318 100644 --- a/src/main/deploy/pathplanner/paths/Proc Part 3 Back.path +++ b/src/main/deploy/pathplanner/paths/Proc Part 3 Back.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.3376024590163933, - "y": 3.623411885245902 + "x": 2.2177254098360653, + "y": 3.1798668032786876 }, "prevControl": { - "x": 2.053961680497683, - "y": 2.8685853299533908 + "x": 1.9340846313173548, + "y": 2.4250402479861766 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Proc2P Part 1.path b/src/main/deploy/pathplanner/paths/Proc2P Part 1.path index d8371bc5..322d9526 100644 --- a/src/main/deploy/pathplanner/paths/Proc2P Part 1.path +++ b/src/main/deploy/pathplanner/paths/Proc2P Part 1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.730122950819672, - "y": 2.292776639344262 + "x": 5.993852459016393, + "y": 1.9331454918032784 }, "prevControl": { - "x": 6.252487509980028, - "y": 2.0414527288053086 + "x": 6.51621701817675, + "y": 1.6818215812643251 }, "nextControl": null, "isLocked": false, @@ -42,7 +42,7 @@ }, "goalEndState": { "velocity": 2.0, - "rotation": 119.89890183861452 + "rotation": 128.1572265873691 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/Proc2P Part 2.path b/src/main/deploy/pathplanner/paths/Proc2P Part 2.path index 0bf82dbf..23a4f677 100644 --- a/src/main/deploy/pathplanner/paths/Proc2P Part 2.path +++ b/src/main/deploy/pathplanner/paths/Proc2P Part 2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 5.730122950819672, - "y": 2.292776639344262 + "x": 5.993852459016393, + "y": 1.9331454918032784 }, "prevControl": null, "nextControl": { - "x": 4.993283348415611, - "y": 1.9623858494624056 + "x": 5.257012856612333, + "y": 1.602754701921422 }, "isLocked": false, "linkedName": "Proc2P" @@ -48,7 +48,7 @@ "folder": null, "idealStartingState": { "velocity": 2.0, - "rotation": 119.89890183861452 + "rotation": 128.1572265873691 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Proc3P Part 1.path b/src/main/deploy/pathplanner/paths/Proc3P Part 1.path new file mode 100644 index 00000000..5fb5fa5c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Proc3P Part 1.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.22858606557377, + "y": 1.4896004098360651 + }, + "prevControl": null, + "nextControl": { + "x": 6.109042693807643, + "y": 1.997689896083843 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.993852459016393, + "y": 1.9331454918032784 + }, + "prevControl": { + "x": 6.3915899988967935, + "y": 1.7784793821527525 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Proc2P" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.752, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 2.0, + "rotation": 128.1572265873691 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 140.19442890773485 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Proc3P Part 2.path b/src/main/deploy/pathplanner/paths/Proc3P Part 2.path new file mode 100644 index 00000000..b6daea08 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Proc3P Part 2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.399, + "y": 2.777 + }, + "prevControl": null, + "nextControl": { + "x": 5.885963114754098, + "y": 2.3647028688524587 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.282684426229508, + "y": 0.5545594262295083 + }, + "prevControl": { + "x": 2.0019467213114748, + "y": 0.6025102459016399 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.752, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.5, + "rotation": -125.53767779197437 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 122.33928922146787 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Proc3P Part 3.path b/src/main/deploy/pathplanner/paths/Proc3P Part 3.path new file mode 100644 index 00000000..e65c060d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Proc3P Part 3.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.331, + "y": 0.603 + }, + "prevControl": null, + "nextControl": { + "x": 2.3665107361052646, + "y": 1.7395974393605889 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.906045081967213, + "y": 3.1678790983606553 + }, + "prevControl": { + "x": 1.5895321169699508, + "y": 2.5357140728236676 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.752, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 2.0, + "rotation": 10.17551084304327 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -126.34002826914755 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Proc3P Part 4.path b/src/main/deploy/pathplanner/paths/Proc3P Part 4.path new file mode 100644 index 00000000..b9a725bd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Proc3P Part 4.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.9969262295081966, + "y": 3.863165983606556 + }, + "prevControl": null, + "nextControl": { + "x": 2.673258196721311, + "y": 2.8801741803278675 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.0549180327868852, + "y": 0.8542520491803282 + }, + "prevControl": { + "x": 1.2082526448497288, + "y": 1.0517070991120465 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.752, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.5, + "rotation": -130.31410016049722 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Proc3P Part 5.path b/src/main/deploy/pathplanner/paths/Proc3P Part 5.path new file mode 100644 index 00000000..f1bc4ad8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Proc3P Part 5.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.127, + "y": 0.902 + }, + "prevControl": null, + "nextControl": { + "x": 4.06001084483205, + "y": 2.7843665031316966 + }, + "isLocked": false, + "linkedName": "Proc2Part4" + }, + { + "anchor": { + "x": 3.4164959016393444, + "y": 2.34672131147541 + }, + "prevControl": { + "x": 2.9064023128329173, + "y": 2.138533048038979 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.24946695095948712, + "rotationDegrees": 37.756118012436026 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.752, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 2.0, + "rotation": 59.53445508054019 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -126.34002826914755 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Proc3P Part 6.path b/src/main/deploy/pathplanner/paths/Proc3P Part 6.path new file mode 100644 index 00000000..3aeaaa94 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Proc3P Part 6.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.9199795081967217, + "y": 2.7722848360655736 + }, + "prevControl": null, + "nextControl": { + "x": 4.469026420073917, + "y": 2.804738359672578 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.2227459016393443, + "y": 0.8662397540983602 + }, + "prevControl": { + "x": 1.033578339278004, + "y": 0.6951455551907284 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.752, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 1.5, + "rotation": -128.4296930429381 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 60.25511870305778 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/team1403/robot/Constants.java b/src/main/java/team1403/robot/Constants.java index cac47e86..55a4f763 100644 --- a/src/main/java/team1403/robot/Constants.java +++ b/src/main/java/team1403/robot/Constants.java @@ -36,6 +36,24 @@ public static class RioPorts { public static final int kAlgaeIntakePhotogateID = kTBD; } + public static class Swerve { + public static final double KpDrive = 0.14923; + public static final double KiDrive = 0; + public static final double KdDrive = 0; + + public static final double KsDrive = 0.13382; + public static final double KvDrive = 0.11367; + public static final double KaDrive = 0.0074265; + + public static final double KpSteer = 65; + public static final double KiSteer = 0; + public static final double KdSteer = 0; + + public static final double KsSteer = 0.01; + public static final double KvSteer = 2.62; + public static final double KaSteer = 0; + } + /** * Configures the CAN bus. These are grouped together * rather than by subsystem to more easily detect conflict @@ -97,6 +115,13 @@ public static class Vision { //public static final Transform3d kCameraTransfrom = new Transform3d(kCameraOffset, kCameraRotation); public static final Transform3d kLimelightTransform = new Transform3d(kCameraOffset, kLimelightRotation); + public static final Transform3d kLimelight2Transform = new Transform3d( + new Translation3d( + Units.inchesToMeters(3.123) - 0.08, + Units.inchesToMeters(-0.495) + 0.02, + Units.inchesToMeters(17.936) - 0.05 + ), new Rotation3d(Math.PI, 0, Math.PI)); //TODO: TUNE! + public static final double closeAlignDistance = 1.25; } @@ -160,10 +185,11 @@ public static class Setpoints{ public static final double L2 = 17 / 360.; public static final double L3 = 20 / 360.; public static final double L4 = 41 / 360.; - public static final double Source = - 12 / 360.; + public static final double Source = -16.5 / 360.; public static final double Current = 0.23 * 360.; public static final double Drive = -64 / 360.; - public static final double Barge = -75 / 360; + public static final double Barge = -75. / 360.; + public static final double Processor = 40. / 360.; } public static final double WristKS = 0; @@ -182,10 +208,11 @@ public static class Setpoints{ } public static class Climber { - public static final double upSpeed = 1; + public static final double upSpeed = 0.8; public static final double downSpeed = -1; + - public static final double upPosition = 13; + public static final double upPosition = 30; public static final double downPosition = -77; public static final double ratchetDisengage = 0.125; diff --git a/src/main/java/team1403/robot/RobotContainer.java b/src/main/java/team1403/robot/RobotContainer.java index 46df7a56..3fb9d8fc 100644 --- a/src/main/java/team1403/robot/RobotContainer.java +++ b/src/main/java/team1403/robot/RobotContainer.java @@ -10,6 +10,7 @@ import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; +import com.ctre.phoenix.motorcontrol.RemoteFeedbackDevice; import com.fasterxml.jackson.databind.util.Named; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; @@ -127,10 +128,9 @@ public RobotContainer() { m_autoChooser.addOption("Swerve SysID QR", m_swerve.sysIdQuasistatic(Direction.kReverse)); m_autoChooser.addOption("Swerve SysID DF", m_swerve.sysIdDynamic(Direction.kForward)); m_autoChooser.addOption("Swerve SysID DR", m_swerve.sysIdDynamic(Direction.kReverse)); + m_autoChooser.addOption("Drive Wheel Characterization", new DriveWheelCharacterization(m_swerve)); } - m_autoChooser.addOption("Drive Wheel Characterization", new DriveWheelCharacterization(m_swerve)); - // autoChooser.addOption("Choreo Auto", AutoUtil.loadChoreoAuto("test", m_swerve)); // autoChooser.addOption("FivePieceCenter", AutoHelper.getFivePieceAuto(m_swerve)); @@ -148,6 +148,92 @@ public RobotContainer() { } private Command getAlignCommand(ReefSelect select) { + final double timeout = 1.4; + Command vibrationCmd = new ControllerVibrationCommand(m_driverController.getHID(), 0.28, 1); + return Commands.sequence( + Blackbox.setAligningCmd(true), + new DeferredCommand(() -> { + Blackbox.reefSelect(select); + Pose2d currentPose = m_swerve.getPose(); + Pose2d target = Blackbox.getNearestAlignPositionReef(currentPose); + if (target == null) return Commands.none(); + + + if(select == ReefSelect.LEFT) { + target = CougarUtil.addDistanceToPoseLeft(target,((m_coralIntake.getAlignOffset() - 0.201)) + 0.05); + switch(Blackbox.reefLevel) { + case L1: target = CougarUtil.addDistanceToPose(target, Units.inchesToMeters(0)); break; + case L2: target = CougarUtil.addDistanceToPose(target, Units.inchesToMeters(-0.5)); break; + case L3: target = CougarUtil.addDistanceToPose(target, Units.inchesToMeters(-0.5)); break; + case L4: target = CougarUtil.addDistanceToPose(target, Units.inchesToMeters(2)); break; + case drive: default: /* do nothing */ break; + } + } + else if(select == ReefSelect.RIGHT){ + target = CougarUtil.addDistanceToPoseLeft(target,((m_coralIntake.getAlignOffset() - 0.201)) + 0.03); + switch(Blackbox.reefLevel) { + case L1: target = CougarUtil.addDistanceToPose(target, Units.inchesToMeters(0)); break; + case L2: target = CougarUtil.addDistanceToPose(target, Units.inchesToMeters(-0.5)); break; + case L3: target = CougarUtil.addDistanceToPose(target, Units.inchesToMeters(-0.5)); break; + case L4: target = CougarUtil.addDistanceToPose(target, Units.inchesToMeters(2)); break; + case drive: default: /* do nothing */ break; + } + } + + Command finalAlign = new AlignCommand(m_swerve, target); + if (DriverStation.isAutonomous()) finalAlign = finalAlign.withTimeout(timeout); + + if(CougarUtil.getDistance(target, m_swerve.getPose()) > 0.2) + return Commands.sequence( + AutoBuilder.pathfindToPose(target, TunerConstants.kAutoAlignConstraints), + finalAlign + ); + else + return finalAlign; + }, Set.of(m_swerve)), + Blackbox.setAligningCmd(false) + ).finallyDo((interrupted) -> { + if(!interrupted){ + vibrationCmd.schedule(); + } + //just in case + Blackbox.setAligning(false); + }); + } + + private Command getAlignCommandCenter() { + final double timeout = 1.4; + Command vibrationCmd = new ControllerVibrationCommand(m_driverController.getHID(), 0.28, 1); + return Commands.sequence( + Blackbox.setAligningCmd(true), + new DeferredCommand(() -> { + Pose2d currentPose = m_swerve.getPose(); + Pose2d target = Blackbox.getNearestAlignPositionReef(currentPose); + if (target == null) return Commands.none(); + target = CougarUtil.addDistanceToPoseLeft(target, 0.26); + target = CougarUtil.addDistanceToPose(target, 0.15); + Command finalAlign = new AlignCommand(m_swerve, target); + if (DriverStation.isAutonomous()) finalAlign = finalAlign.withTimeout(timeout); + if(CougarUtil.getDistance(target, m_swerve.getPose()) > 0.2) + return Commands.sequence( + AutoBuilder.pathfindToPose(target, TunerConstants.kAutoAlignConstraints), + finalAlign + ); + else + return finalAlign; + }, Set.of(m_swerve)), + Blackbox.setAligningCmd(false) + ).finallyDo((interrupted) -> { + if(!interrupted){ + vibrationCmd.schedule(); + } + //just in case + Blackbox.setAligning(false); + }); + } + + + private Command getAlignCommand(ReefSelect select, double timeout) { Command vibrationCmd = new ControllerVibrationCommand(m_driverController.getHID(), 0.28, 1); return Commands.sequence( Blackbox.setAligningCmd(true), @@ -177,15 +263,18 @@ private Command getAlignCommand(ReefSelect select) { case L4: target = CougarUtil.addDistanceToPose(target, Units.inchesToMeters(2)); break; case drive: default: /* do nothing */ break; } - } + } + + Command finalAlign = new AlignCommand(m_swerve, target); + if (DriverStation.isAutonomous()) finalAlign = finalAlign.withTimeout(timeout); if(CougarUtil.getDistance(target, m_swerve.getPose()) > 0.2) return Commands.sequence( AutoBuilder.pathfindToPose(target, TunerConstants.kAutoAlignConstraints), - new AlignCommand(m_swerve, target) + finalAlign ); else - return new AlignCommand(m_swerve, target); + return finalAlign; }, Set.of(m_swerve)), Blackbox.setAligningCmd(false) ).finallyDo((interrupted) -> { @@ -297,6 +386,7 @@ private void configureBindings() { m_operatorController.rightBumper().onTrue( Commands.sequence( Blackbox.robotStateCmd(State.loading), + Blackbox.reefScoreLevelCmd(ReefScoreLevel.drive), Blackbox.setAligningCmd(false))); m_operatorController.povUp().debounce(0.5).onTrue( @@ -351,7 +441,7 @@ private void configureBindings() { new WristCommand(m_wrist, Constants.Wrist.Setpoints.Source) //switch back to manual elevator intentionally omitted )); - m_operatorController.povDown() + m_operatorController.povLeft() //.and(() -> Blackbox.robotState == State.ManualElevator) .onTrue( Commands.sequence(Blackbox.robotStateCmd(State.MoveElevator), @@ -359,6 +449,14 @@ private void configureBindings() { new WristCommand(m_wrist, Constants.Wrist.Setpoints.Barge) //Blackbox.robotStateCmd(State.ManualElevator) )); + m_operatorController.povDown() + .onTrue( + Commands.sequence( + Blackbox.robotStateCmd(State.MoveElevator), + new ElevatorCommand(m_elevator, Constants.Elevator.Setpoints.Min), + new WristCommand(m_wrist, Constants.Wrist.Setpoints.Processor), + Blackbox.robotStateCmd(State.ManualElevator) + )); /* m_operatorController.rightBumper() .and(() -> Blackbox.robotState == State.ManualElevator).onTrue( @@ -383,7 +481,7 @@ private void configureBindings() { .debounce(0.3, DebounceType.kFalling).whileTrue( new CoralIntakeSpeed(m_coralIntake, Constants.CoralIntake.release) ); - m_operatorController.leftStick().onTrue( + m_operatorController.leftTrigger().onTrue( Commands.sequence( //initially run inward new CoralIntakeSpeed(m_coralIntake, Constants.CoralIntake.wiggle).withTimeout(0.3), @@ -391,14 +489,14 @@ private void configureBindings() { new RepeatNTimes(Commands.sequence( new CoralIntakeSpeed(m_coralIntake, -Constants.CoralIntake.wiggle).withTimeout(0.3), new CoralIntakeSpeed(m_coralIntake, Constants.CoralIntake.wiggle).withTimeout(0.4) //runs inward for longer to avoid piece falling out - ), 1))); + ), 2))); new Trigger(() -> Blackbox.robotState == State.placing && m_wrist.isAtSetpoint() && m_elevator.isAtSetpoint() && Blackbox.getCloseAlign(m_swerve.getPose()) && !Blackbox.isAligning()) - .debounce(0.1).onTrue( + .debounce(0.06).onTrue( Commands.parallel( opVibrationCmd.asProxy(), m_led.requestState(LEDConfig.Style.Strobe, LEDConfig.Color.Green).repeatedly() @@ -413,26 +511,60 @@ private void configureBindings() { NamedCommands.registerCommand("WaitForPlace", Commands.waitUntil(() -> !Blackbox.isCoralLoaded()).withTimeout(0.1)); NamedCommands.registerCommand("WaitForCoral", - Commands.waitUntil(() -> Blackbox.isCoralLoaded())); NamedCommands.registerCommand("WaitForSetpoint", new WaitUntilDebounced(() -> m_wrist.isAtSetpoint() && m_elevator.isAtSetpoint(), 0.1).withTimeout(3)); - NamedCommands.registerCommand("ReefAlignL", getAlignCommand(Blackbox.ReefSelect.LEFT).withTimeout(2.7)); - NamedCommands.registerCommand("ReefAlignR", getAlignCommand(Blackbox.ReefSelect.RIGHT).withTimeout(2.7)); + NamedCommands.registerCommand("ReefAlignL", getAlignCommand(Blackbox.ReefSelect.LEFT)); + NamedCommands.registerCommand("ReefAlignR", getAlignCommand(Blackbox.ReefSelect.RIGHT)); + NamedCommands.registerCommand("ReefAlignCenter", getAlignCommandCenter()); NamedCommands.registerCommand("Loading", Blackbox.robotStateCmd(Blackbox.State.loading)); - - - + NamedCommands.registerCommand("LoadingFromBarge", Commands.sequence( + Blackbox.robotStateCmd(State.MoveElevator), + new WristCommand(m_wrist, Constants.Wrist.Setpoints.Barge).asProxy(), + new ElevatorCommand(m_elevator, Constants.Elevator.Setpoints.Source).asProxy(), + Blackbox.robotStateCmd(State.loading) + )); + NamedCommands.registerCommand("Barge L3", Commands.sequence( + Blackbox.robotStateCmd(State.MoveElevator), + new ElevatorCommand(m_elevator, Constants.Elevator.Setpoints.L3Algae).asProxy(), + new WristCommand(m_wrist, Constants.Wrist.Setpoints.Source).asProxy())); + + NamedCommands.registerCommand("BargeSetpoint", Commands.sequence( + Blackbox.robotStateCmd(State.MoveElevator), + new ElevatorCommand(m_elevator, Constants.Elevator.Setpoints.Barge), + new WristCommand(m_wrist, Constants.Wrist.Setpoints.Barge))); + + NamedCommands.registerCommand("Algae Harvest", + new CoralIntakeSpeed(m_coralIntake, Constants.CoralIntake.release).asProxy()); + + NamedCommands.registerCommand("Algae Expel", + new CoralIntakeSpeed(m_coralIntake, -Constants.CoralIntake.release).asProxy()); + + NamedCommands.registerCommand("AutoWiggle", + Commands.sequence( + //initially run inward + Commands.waitSeconds(0.1), + new CoralIntakeSpeed(m_coralIntake, Constants.CoralIntake.wiggle + 0.1).withTimeout(0.1).asProxy(), + new CoralIntakeSpeed(m_coralIntake, -Constants.CoralIntake.wiggle).withTimeout(0.25).asProxy(), + new CoralIntakeSpeed(m_coralIntake, Constants.CoralIntake.wiggle + 0.1).withTimeout(0.15).asProxy(), + new CoralIntakeSpeed(m_coralIntake, -Constants.CoralIntake.wiggle).withTimeout(0.25).asProxy(), + new CoralIntakeSpeed(m_coralIntake, Constants.CoralIntake.wiggle + 0.1).withTimeout(0.15).asProxy() + + )); //wiggle twice, 0.1 + 0.5 * 2 + 0.2 = 1.3 s total ... not good (tune the timing) + /* Move forward 1 m from any position on the starting line (make sure robot is facing a tag to seed the position) */ m_autoChooser.addOption("THREE PIECE BACK FINAL PROCESSOR SIDE", AutoHelper.getThreePieceBackProc(m_swerve)); + m_autoChooser.addOption("THREE PIECE APRIL TAG 9 7 8 ALL RIGHT REEF UNTESTED", AutoHelper.getThreePieceBackRightProc978(m_swerve)); + m_autoChooser.addOption("ONE PIECE CENTER", AutoHelper.getOnePCenter(m_swerve)); m_autoChooser.addOption("MOVE AUTO ANYWHERE", AutoHelper.getMoveAuto(m_swerve)); m_autoChooser.addOption("THREE PIECE BACK FINAL NON PROCESSOR SIDE UNTESTED", AutoHelper.getThreePieceBackNonProc(m_swerve)); - m_autoChooser.addOption("ONE PIECE CENTER UNTESTED", AutoHelper.getOnePCenter(m_swerve)); + m_autoChooser.addOption("ONE PIECE CENTER ALGAE", AutoHelper.getOnePCenterAlgae(m_swerve)); m_autoChooser.addOption("THREE PIECE SIDE PROCESSOR UNTESTED", AutoHelper.getThreePieceSideProc(m_swerve)); m_autoChooser.addOption("TWO PIECE PROCESSOR UNTESTED", AutoHelper.getTwoPieceProc(m_swerve)); m_autoChooser.addOption("TWO PIECE PROCESSOR + ALGAE REMOVAL UNTESTED", AutoHelper.getTwoPieceProc_algaeRemoval(m_swerve)); + //m_autoChooser.addOption("wiggle test", AutoHelper.wiggle(m_swerve)); //m_autoChooser.addOption("Testing Auto Align", AutoHelper.testAutoAlign(m_swerve)); //m_autoChooser.addOption("Test 2 Piece", AutoHelper.getTwoPieceProcTest(m_swerve)); diff --git a/src/main/java/team1403/robot/commands/ClimberCommand.java b/src/main/java/team1403/robot/commands/ClimberCommand.java index 1d59d193..1061e1a0 100644 --- a/src/main/java/team1403/robot/commands/ClimberCommand.java +++ b/src/main/java/team1403/robot/commands/ClimberCommand.java @@ -30,10 +30,8 @@ public ClimberCommand(ClimberSubsystem climber, boolean goingDown) { @Override public void initialize() { - if(isGoingDown){ m_climber.setServo(Constants.Climber.ratchetDisengage); Commands.waitSeconds(2); - } // m_timer.reset(); } @@ -51,7 +49,7 @@ public void execute() { if(m_climber.getMotorPosition() > Constants.Climber.downPosition) m_climber.setMotorSpeed(Constants.Climber.downSpeed); } else { - m_climber.setServo(Constants.Climber.ratchetEngage); + m_climber.setServo(Constants.Climber.ratchetDisengage); if(m_climber.getMotorPosition() < Constants.Climber.upPosition) m_climber.setMotorSpeed(Constants.Climber.upSpeed); } diff --git a/src/main/java/team1403/robot/commands/DefaultSwerveCommand.java b/src/main/java/team1403/robot/commands/DefaultSwerveCommand.java index 7de864e5..4b588cfd 100644 --- a/src/main/java/team1403/robot/commands/DefaultSwerveCommand.java +++ b/src/main/java/team1403/robot/commands/DefaultSwerveCommand.java @@ -151,7 +151,7 @@ public void execute() { } if(!Blackbox.isCoralLoaded() && m_autoRotate.getAsBoolean()) { - Pose2d target = Blackbox.getNearestSourcePose(m_drivetrainSubsystem.getPose()); + Pose2d target = Blackbox.getNearestHeuristic(m_drivetrainSubsystem.getPose(), Blackbox.getReefPoses()); if (target != null) { m_targetState.position = target.getRotation().getRadians(); m_targetState.velocity = 0; diff --git a/src/main/java/team1403/robot/commands/StateMachine.java b/src/main/java/team1403/robot/commands/StateMachine.java index 872e9fa8..1afe99e6 100644 --- a/src/main/java/team1403/robot/commands/StateMachine.java +++ b/src/main/java/team1403/robot/commands/StateMachine.java @@ -46,20 +46,20 @@ public void execute() { if (Blackbox.isCoralLoaded()) Blackbox.robotState = State.driving; } break; - case driving: + case driving: if(Blackbox.reefLevel != Blackbox.ReefScoreLevel.drive) m_wristSubsystem.moveToSetpoint(Constants.Wrist.Setpoints.Drive); if(Blackbox.isAligning() && Blackbox.reefLevel != Blackbox.ReefScoreLevel.drive) Blackbox.robotState = State.aligning; break; case aligning: - if(Blackbox.isAligning() && Blackbox.getCloseAlign(m_swerve.getPose()) && Blackbox.reefLevel == Blackbox.ReefScoreLevel.L4) + if(Blackbox.isAligning() && Blackbox.getCloseAlign(m_swerve.getPose()) && (Blackbox.reefLevel == Blackbox.ReefScoreLevel.L2 || Blackbox.reefLevel == Blackbox.ReefScoreLevel.L4)) Blackbox.robotState = State.placing; - if(!Blackbox.isAligning()) + if(!Blackbox.isAligning() && (Blackbox.reefLevel == Blackbox.ReefScoreLevel.L1 || Blackbox.reefLevel == Blackbox.ReefScoreLevel.L3)) //l4 goes really high, so let's wait until we are close Blackbox.robotState = State.placing; break; case placing: - if (Blackbox.reefLevel == Blackbox.ReefScoreLevel.L4) { + if (Blackbox.reefLevel != Blackbox.ReefScoreLevel.L1) { m_elevatorSubsystem.moveToSetpoint(Blackbox.getElevatorSetpointLevel(Blackbox.reefLevel)); m_wristSubsystem.moveToSetpoint(Blackbox.getWristSetpointLevel(Blackbox.reefLevel)); if(!Blackbox.isCoralLoaded()) { diff --git a/src/main/java/team1403/robot/commands/auto/AutoHelper.java b/src/main/java/team1403/robot/commands/auto/AutoHelper.java index d8d65b61..f3ad17f6 100644 --- a/src/main/java/team1403/robot/commands/auto/AutoHelper.java +++ b/src/main/java/team1403/robot/commands/auto/AutoHelper.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import team1403.lib.util.AutoUtil; import team1403.lib.util.CougarUtil; import team1403.robot.commands.AlignCommand; @@ -37,11 +38,12 @@ public static Command getOnePCenter(SwerveSubsystem m_swerve) { try { return Commands.sequence( Commands.parallel( - AutoUtil.loadPathPlannerPath("OneP Center", m_swerve), + AutoUtil.loadPathPlannerPath("OneP Center", m_swerve, true), Commands.sequence( Commands.waitSeconds(0.75), NamedCommands.getCommand("CoralL4")) ), + Commands.waitSeconds(1), NamedCommands.getCommand("ReefAlignR"), NamedCommands.getCommand("WaitForSetpoint"), NamedCommands.getCommand("CoralScore"), @@ -53,6 +55,48 @@ public static Command getOnePCenter(SwerveSubsystem m_swerve) { } } + public static Command getOnePCenterAlgae(SwerveSubsystem m_swerve) { + try { + return Commands.sequence( + Commands.parallel( + AutoUtil.loadPathPlannerPath("OneP Center", m_swerve, true), + Commands.sequence( + Commands.waitSeconds(0.75), + NamedCommands.getCommand("CoralL4")) + ), + Commands.waitSeconds(1), + NamedCommands.getCommand("ReefAlignR"), + NamedCommands.getCommand("WaitForSetpoint"), + NamedCommands.getCommand("CoralScore"), + NamedCommands.getCommand("Loading"), + AutoUtil.loadPathPlannerPath("One piece part 2 barge", m_swerve), + //NamedCommands.getCommand("Barge L3"), + Commands.waitSeconds(1), + Commands.parallel( + NamedCommands.getCommand("ReefAlignCenter"), + NamedCommands.getCommand("Algae Harvest"), + Commands.waitSeconds(3.0) + ), + Commands.parallel( + NamedCommands.getCommand("Algae Harvest"), + AutoUtil.loadPathPlannerPath("One piece part 3 barge", m_swerve), + Commands.waitSeconds(0.5) + //NamedCommands.getCommand("BargeSetpoint"), + //NamedCommands.getCommand("WaitForSetpoint"), + //NamedCommands.getCommand("Algae Expel") // default command will expel + ) + //Commands.waitSeconds(0.5), + //NamedCommands.getCommand("LoadingFromBarge"), //make sure we don't kill ourselves on the barge + //AutoUtil.loadPathPlannerPath("One piece part 4 barge", m_swerve) + ); + } catch (Exception e) { + System.err.println("Could not load auto: " + e.getMessage()); + return Commands.none(); + } + } + + + public static Command getThreePieceSideProc(SwerveSubsystem m_swerve) { try { return Commands.sequence( @@ -106,22 +150,24 @@ public static Command getThreePieceBackProc(SwerveSubsystem m_swerve) { NamedCommands.getCommand("Loading"), Commands.waitSeconds(0.1), AutoUtil.loadPathPlannerPath("Proc Two Piece Part 2 test", m_swerve), - //Commands.waitSeconds(0.05), - // alignToStartingPose(m_swerve, "Proc2P Part 2"), NamedCommands.getCommand("WaitForCoral"), - AutoUtil.loadPathPlannerPath("Proc Part 3 Back", m_swerve), + Commands.parallel( + AutoUtil.loadPathPlannerPath("Proc Part 3 Back", m_swerve), + NamedCommands.getCommand("AutoWiggle") + ), NamedCommands.getCommand("CoralL4"), NamedCommands.getCommand("ReefAlignL"), NamedCommands.getCommand("WaitForSetpoint"), NamedCommands.getCommand("CoralScore"), Commands.parallel( NamedCommands.getCommand("Loading"), - // Commands.waitSeconds(0.05), - //alignToStartingPose(m_swerve, "3p Part 4"), AutoUtil.loadPathPlannerPath("3p Part 4", m_swerve) ), NamedCommands.getCommand("WaitForCoral"), - AutoUtil.loadPathPlannerPath("Proc Part 5 Back", m_swerve), + Commands.parallel( + AutoUtil.loadPathPlannerPath("Proc Part 5 Back", m_swerve), + NamedCommands.getCommand("AutoWiggle") + ), NamedCommands.getCommand("CoralL4"), NamedCommands.getCommand("ReefAlignR"), NamedCommands.getCommand("WaitForSetpoint"), @@ -138,6 +184,55 @@ public static Command getThreePieceBackProc(SwerveSubsystem m_swerve) { } } + public static Command getThreePieceBackRightProc978(SwerveSubsystem m_swerve) { + try { + return Commands.sequence( + Commands.parallel( + AutoUtil.loadPathPlannerPath("Proc3P Part 1", m_swerve, true), + Commands.sequence( + Commands.waitSeconds(0.75), + NamedCommands.getCommand("CoralL4")) + ), + NamedCommands.getCommand("ReefAlignR"), + NamedCommands.getCommand("WaitForSetpoint"), + NamedCommands.getCommand("CoralScore"), + NamedCommands.getCommand("Loading"), + Commands.waitSeconds(0.1), + AutoUtil.loadPathPlannerPath("Proc3P Part 2", m_swerve), + NamedCommands.getCommand("WaitForCoral"), + Commands.parallel( + AutoUtil.loadPathPlannerPath("Proc3P Part 3", m_swerve), + NamedCommands.getCommand("AutoWiggle") + ), + NamedCommands.getCommand("CoralL4"), + NamedCommands.getCommand("ReefAlignR"), + NamedCommands.getCommand("WaitForSetpoint"), + NamedCommands.getCommand("CoralScore"), + Commands.parallel( + NamedCommands.getCommand("Loading"), + AutoUtil.loadPathPlannerPath("Proc3P Part 4", m_swerve) + ), + NamedCommands.getCommand("WaitForCoral"), + Commands.parallel( + AutoUtil.loadPathPlannerPath("Proc3P Part 5", m_swerve), + NamedCommands.getCommand("AutoWiggle") + ), + NamedCommands.getCommand("CoralL4"), + NamedCommands.getCommand("ReefAlignR"), + NamedCommands.getCommand("WaitForSetpoint"), + NamedCommands.getCommand("CoralScore"), + Commands.parallel( + NamedCommands.getCommand("Loading"), + AutoUtil.loadPathPlannerPath("Proc3P Part 6", m_swerve) + ) + + ); + } catch (Exception e) { + System.err.println("Could not load auto: " + e.getMessage()); + return Commands.none(); + } + } + public static Command getThreePieceBackNonProc(SwerveSubsystem m_swerve) { try { return Commands.sequence( @@ -145,16 +240,20 @@ public static Command getThreePieceBackNonProc(SwerveSubsystem m_swerve) { AutoUtil.loadPathPlannerPath("Non Proc 3P Part 1", m_swerve, true), Commands.sequence( Commands.waitSeconds(0.75), - NamedCommands.getCommand("CoralL4")) + NamedCommands.getCommand("CoralL4") + ) ), - NamedCommands.getCommand("ReefAlignL"), + NamedCommands.getCommand("ReefAlignR"), NamedCommands.getCommand("WaitForSetpoint"), NamedCommands.getCommand("CoralScore"), NamedCommands.getCommand("Loading"), Commands.waitSeconds(0.1), AutoUtil.loadPathPlannerPath("Non Proc 3P Part 2", m_swerve), NamedCommands.getCommand("WaitForCoral"), - AutoUtil.loadPathPlannerPath("Non Proc 3P Part 3", m_swerve), + Commands.parallel( + AutoUtil.loadPathPlannerPath("Non Proc 3P Part 3", m_swerve), + NamedCommands.getCommand("AutoWiggle") + ), NamedCommands.getCommand("CoralL4"), NamedCommands.getCommand("ReefAlignR"), NamedCommands.getCommand("WaitForSetpoint"), @@ -164,7 +263,10 @@ public static Command getThreePieceBackNonProc(SwerveSubsystem m_swerve) { AutoUtil.loadPathPlannerPath("Non Proc 3P Part 4", m_swerve) ), NamedCommands.getCommand("WaitForCoral"), - AutoUtil.loadPathPlannerPath("Non Proc 3P Part 5", m_swerve), + Commands.parallel( + AutoUtil.loadPathPlannerPath("Non Proc 3P Part 5", m_swerve), + NamedCommands.getCommand("AutoWiggle") + ), NamedCommands.getCommand("CoralL4"), NamedCommands.getCommand("ReefAlignL"), NamedCommands.getCommand("WaitForSetpoint"), @@ -172,8 +274,7 @@ public static Command getThreePieceBackNonProc(SwerveSubsystem m_swerve) { Commands.parallel( NamedCommands.getCommand("Loading"), AutoUtil.loadPathPlannerPath("Non Proc 3P Part 6", m_swerve) - ) - + ) ); } catch (Exception e) { System.err.println("Could not load auto: " + e.getMessage()); @@ -181,6 +282,9 @@ public static Command getThreePieceBackNonProc(SwerveSubsystem m_swerve) { } } + + + public static Command testAutoAlign(SwerveSubsystem m_swerve){ try{ return Commands.sequence( @@ -198,6 +302,20 @@ public static Command testAutoAlign(SwerveSubsystem m_swerve){ } } + + public static Command wiggle(SwerveSubsystem m_swerve){ + try{ + return Commands.sequence( + NamedCommands.getCommand("AutoWiggle") + ); + } + catch(Exception e){ + System.err.println("Could not load"); + return Commands.none(); + } + + } + public static Command getTwoPieceProc(SwerveSubsystem m_swerve) { try { return Commands.sequence( diff --git a/src/main/java/team1403/robot/subsystems/Blackbox.java b/src/main/java/team1403/robot/subsystems/Blackbox.java index 3db13c2b..fc8eaf05 100644 --- a/src/main/java/team1403/robot/subsystems/Blackbox.java +++ b/src/main/java/team1403/robot/subsystems/Blackbox.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; @@ -31,7 +32,8 @@ public enum State { public enum ReefSelect { LEFT, - RIGHT + RIGHT, + CENTER } public enum ReefScoreLevel { @@ -139,7 +141,7 @@ public static Command robotStateCmd(State state) { return new InstantCommand(() -> setRobotState(state)); } - private static Pose2d[] getReefPoses() { + public static Pose2d[] getReefPoses() { if(reefSide == ReefSelect.LEFT) return CougarUtil.getAlliance() == Alliance.Blue ? reefPosesLeftBLUE : reefPosesLeftRED; else @@ -231,7 +233,7 @@ public static Pose2d getNearestHeuristic(Pose2d a, Pose2d[] list) { public static Pose2d getNearestAlignPositionReef(Pose2d currentPose) { Pose2d nearest = null; - if (isCoralLoaded()) nearest = getNearestHeuristic(currentPose, getReefPoses()); + if (isCoralLoaded() || DriverStation.isAutonomous()) nearest = getNearestHeuristic(currentPose, getReefPoses()); if (nearest == null) return null; if (CougarUtil.getDistance(currentPose, nearest) > kMaxAlignDist) return null; diff --git a/src/main/java/team1403/robot/subsystems/CoralIntakeSubsystem.java b/src/main/java/team1403/robot/subsystems/CoralIntakeSubsystem.java index 898cbebf..49b64674 100644 --- a/src/main/java/team1403/robot/subsystems/CoralIntakeSubsystem.java +++ b/src/main/java/team1403/robot/subsystems/CoralIntakeSubsystem.java @@ -70,6 +70,22 @@ else if(distance > 0.12){ return Constants.CoralIntake.Setpoints.pose1; } } + + public int getCoralPosition() { + double distance = getDistance(); + if(distance > 0.29){ + return 4; + } + else if(distance > 0.2){ + return 3; + } + else if(distance > 0.12){ + return 2; + } + else { + return 1; + } + } private double getFilteredCurrent() { return m_filter.calculate(m_intakeMotor.getOutputCurrent()); @@ -92,6 +108,7 @@ public void periodic() { Logger.recordOutput("CoralIntake/CoralDistance", getDistance()); Logger.recordOutput("CoralIntake/Current", getFilteredCurrent()); Logger.recordOutput("CoralIntake/Has Piece", Blackbox.isCoralLoaded()); - Logger.recordOutput("CoralIntake/CoralPosition", getAlignOffset()); + Logger.recordOutput("CoralIntake/CoralPositionOffset", getAlignOffset()); + Logger.recordOutput("CoralIntake/Coral Slot Position", getCoralPosition()); } } \ No newline at end of file diff --git a/src/main/java/team1403/robot/swerve/SwerveSubsystem.java b/src/main/java/team1403/robot/swerve/SwerveSubsystem.java index 523ebd70..637aa802 100644 --- a/src/main/java/team1403/robot/swerve/SwerveSubsystem.java +++ b/src/main/java/team1403/robot/swerve/SwerveSubsystem.java @@ -5,8 +5,6 @@ import java.util.ArrayList; import java.util.function.Supplier; -import org.littletonrobotics.junction.Logger; - import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; @@ -21,13 +19,10 @@ import com.pathplanner.lib.pathfinding.LocalADStar; import com.pathplanner.lib.pathfinding.Pathfinding; import com.pathplanner.lib.util.DriveFeedforwards; -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.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveModuleState; @@ -40,8 +35,8 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.Alert.AlertType; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -55,6 +50,7 @@ import team1403.robot.vision.AprilTagCamera; import team1403.robot.vision.ITagCamera; import team1403.robot.vision.LimelightWrapper; +import team1403.robot.vision.VisionConfigurator; import team1403.robot.vision.VisionSimUtil; import team1403.robot.vision.ITagCamera.VisionData; @@ -174,15 +170,46 @@ private void onConstruct() { ).schedule(); VisionSimUtil.initVisionSim(); - m_cameras.add(new LimelightWrapper("limelight", - () -> Constants.Vision.kLimelightTransform, - () -> new Rotation3d(getRotation()))); + + VisionConfigurator config = new VisionConfigurator() + .withRobotPose(this::getPose, () -> Timer.getFPGATimestamp()) /* find a way to convert m_state.Timestamp to fpga time */ + .withYawRate(() -> getPigeon2().getAngularVelocityZWorld().getValue().in(RadiansPerSecond)); + + if (Robot.isReal()) + { + m_cameras.add(new LimelightWrapper(config + .withName("limelight") + .withTransform(() -> Constants.Vision.kLimelightTransform) + .withDeviations(VecBuilder.fill(2, 2, 3)) + .withDeviationsTrig(VecBuilder.fill(2, 2, Double.POSITIVE_INFINITY)) + .withTrigSolve(!true) + )); + m_cameras.add(new LimelightWrapper(config + .withName("limelight-twoplus") + .withTransform(() -> Constants.Vision.kLimelight2Transform) + .withDeviations(VecBuilder.fill(2, 2, 3)) + .withDeviationsTrig(VecBuilder.fill(1, 1, Double.POSITIVE_INFINITY)) + .withTrigSolve(!true) + )); + } //test camera for simulation - if(Robot.isSimulation()) - m_cameras.add(new AprilTagCamera("simlimelight", - () -> Constants.Vision.kLimelightTransform, - () -> m_state.Timestamp, - () -> getPose())); + else + { + m_cameras.add(new AprilTagCamera(config + .withName("simlimelight") + .withTransform(() -> Constants.Vision.kLimelightTransform) + .withDeviations(VecBuilder.fill(2, 2, 3)) + .withDeviationsTrig(VecBuilder.fill(1, 1, Double.POSITIVE_INFINITY)) + .withTrigSolve(true) + )); + m_cameras.add(new AprilTagCamera(config + .withName("simlimelight-2") + .withTransform(() -> Constants.Vision.kLimelight2Transform) + .withDeviations(VecBuilder.fill(2, 2, 3)) + .withDeviationsTrig(VecBuilder.fill(1, 1, Double.POSITIVE_INFINITY)) + .withTrigSolve(true) + )); + } SmartDashboard.putData("Gyro", super.getPigeon2()); @@ -340,8 +367,10 @@ public void periodic() { { //todo: pass this consumer into the contructor in the future c.refreshEstimate((VisionData data) -> { - if(data.pose != null) //last minute safety check! - addVisionMeasurement(data.pose.toPose2d(), data.timestamp, data.stdv); + if(data.pose != null){ //last minute safety check! + if(!(c.getName().equals("limelight-twoplus") && DriverStation.isAutonomous())) //ignore back limelight in auto + addVisionMeasurement(data.pose.toPose2d(), data.timestamp, data.stdv); + } }); } diff --git a/src/main/java/team1403/robot/swerve/Telemetry.java b/src/main/java/team1403/robot/swerve/Telemetry.java index 679de859..ed0f9d2b 100644 --- a/src/main/java/team1403/robot/swerve/Telemetry.java +++ b/src/main/java/team1403/robot/swerve/Telemetry.java @@ -29,11 +29,11 @@ public Telemetry(double maxSpeed) { SignalLogger.start(); PathPlannerLogging.setLogActivePathCallback((activePath) -> { - Logger.recordOutput("Odometry/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); + Logger.recordOutput("Swerve/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); m_field.getObject("traj").setPoses(activePath); }); PathPlannerLogging.setLogTargetPoseCallback((targetPose) -> { - Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); + Logger.recordOutput("Swerve/TrajectorySetpoint", targetPose); }); SmartDashboard.putData("Field", m_field); diff --git a/src/main/java/team1403/robot/swerve/TunerConstants.java b/src/main/java/team1403/robot/swerve/TunerConstants.java index 7dfe5883..969ed37a 100644 --- a/src/main/java/team1403/robot/swerve/TunerConstants.java +++ b/src/main/java/team1403/robot/swerve/TunerConstants.java @@ -16,6 +16,7 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.*; +import team1403.robot.Constants.Swerve; // Generated by the Tuner X Swerve Project Generator @@ -27,14 +28,14 @@ public class TunerConstants { // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(65).withKI(0).withKD(0) - .withKS(0.01).withKV(2.62).withKA(0) + .withKP(Swerve.KpSteer).withKI(Swerve.KiSteer).withKD(Swerve.KdSteer) + .withKS(Swerve.KsSteer).withKV(Swerve.KvSteer).withKA(Swerve.KaSteer) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0.068717).withKI(0).withKD(0) //kP = 0.16815 //kp= 0.1 - .withKS(0.11913).withKV(0.11491).withKA(0.0038214); + .withKP(Swerve.KpDrive).withKI(Swerve.KiDrive).withKD(Swerve.KdDrive) //kP = 0.16815 //kp= 0.1 + .withKS(Swerve.KsDrive).withKV(Swerve.KvDrive).withKA(Swerve.KaDrive); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors @@ -88,7 +89,7 @@ public class TunerConstants { public static final double kDriveGearRatio = 6.746031746031747; private static final double kSteerGearRatio = 21.428571428571427; - public static final Distance kWheelRadius = Inches.of(1.925); + public static final Distance kWheelRadius = Inches.of(1.934); private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; @@ -135,7 +136,7 @@ public class TunerConstants { private static final int kFrontLeftDriveMotorId = 12; private static final int kFrontLeftSteerMotorId = 15; private static final int kFrontLeftEncoderId = 23; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.008056640625); + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.01123046875); private static final boolean kFrontLeftSteerMotorInverted = true; private static final boolean kFrontLeftEncoderInverted = false; @@ -146,7 +147,7 @@ public class TunerConstants { private static final int kFrontRightDriveMotorId = 8; private static final int kFrontRightSteerMotorId = 7; private static final int kFrontRightEncoderId = 22; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.16796875); + private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.16650390625); private static final boolean kFrontRightSteerMotorInverted = true; private static final boolean kFrontRightEncoderInverted = false; @@ -157,7 +158,7 @@ public class TunerConstants { private static final int kBackLeftDriveMotorId = 16; private static final int kBackLeftSteerMotorId = 2; private static final int kBackLeftEncoderId = 24; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.18115234375); + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.1806640625); private static final boolean kBackLeftSteerMotorInverted = true; private static final boolean kBackLeftEncoderInverted = false; @@ -168,7 +169,7 @@ public class TunerConstants { private static final int kBackRightDriveMotorId = 5; private static final int kBackRightSteerMotorId = 13; private static final int kBackRightEncoderId = 21; - private static final Angle kBackRightEncoderOffset = Rotations.of(-0.43994140625); + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.443115234375); private static final boolean kBackRightSteerMotorInverted = true; private static final boolean kBackRightEncoderInverted = false; diff --git a/src/main/java/team1403/robot/vision/AprilTagCamera.java b/src/main/java/team1403/robot/vision/AprilTagCamera.java index 1be9d332..b83af48d 100644 --- a/src/main/java/team1403/robot/vision/AprilTagCamera.java +++ b/src/main/java/team1403/robot/vision/AprilTagCamera.java @@ -37,20 +37,25 @@ public class AprilTagCamera extends SubsystemBase implements ITagCamera { private final PhotonCamera m_camera; private final PhotonCameraSim m_cameraSim; private final PhotonPoseEstimator m_poseEstimator; + private final PhotonPoseEstimator m_trigSolveEstimator; private final Supplier m_cameraTransform; private EstimatedRobotPose m_estPos; + private EstimatedRobotPose m_estTrigPos; private final Supplier m_referencePose; private final DoubleSupplier m_poseTimestamp; + private final DoubleSupplier m_angularVelocity; private final Alert m_cameraAlert; - private static final Matrix kDefaultStdv = VecBuilder.fill(2, 2, 3); + private final Matrix kDefaultStdv; + private final Matrix kDefaultStdvTrig; + private final boolean kTrigSolveEnabled; private final VisionData m_returnedData = new VisionData(); - public AprilTagCamera(String cameraName, Supplier cameraTransform, DoubleSupplier poseTimestamp, Supplier referenceSupplier) { + public AprilTagCamera(VisionConfigurator config) { // Photonvision // PortForwarder.add(5800, // "photonvision.local", 5800); - m_camera = new PhotonCamera(cameraName); + m_camera = new PhotonCamera(config.getName()); if (Robot.isSimulation()) { SimCameraProperties cameraProp = new SimCameraProperties(); @@ -67,21 +72,27 @@ public AprilTagCamera(String cameraName, Supplier cameraTransform, m_cameraSim = new PhotonCameraSim(m_camera, cameraProp); - VisionSimUtil.addCamera(m_cameraSim, cameraTransform.get()); + VisionSimUtil.addCamera(m_cameraSim, config.getTransform3d().get()); } else { m_cameraSim = null; } m_camera.setPipelineIndex(0); - m_poseEstimator = new PhotonPoseEstimator(Constants.Vision.kFieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, cameraTransform.get()); + m_poseEstimator = new PhotonPoseEstimator(Constants.Vision.kFieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, config.getTransform3d().get()); m_poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.AVERAGE_BEST_TARGETS); - m_estPos = null; - m_referencePose = referenceSupplier; - m_cameraTransform = cameraTransform; - m_poseTimestamp = poseTimestamp; + m_trigSolveEstimator = new PhotonPoseEstimator(Constants.Vision.kFieldLayout, PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, config.getTransform3d().get()); - m_cameraAlert = new Alert("Photon Camera " + cameraName + " Disconnected!", AlertType.kError); + m_estPos = null; + m_referencePose = config.getRobotPose(); + m_cameraTransform = config.getTransform3d(); + m_poseTimestamp = config.getPoseTimestamp(); + kDefaultStdv = config.getDeviations(); + kDefaultStdvTrig = config.getDeviationsTrig(); + kTrigSolveEnabled = config.getTrigSolveEnabled(); + m_angularVelocity = config.getYawRate(); + + m_cameraAlert = new Alert("Photon Camera " + m_camera.getName() + " Disconnected!", AlertType.kError); } @Override @@ -89,87 +100,103 @@ public String getName() { return m_camera.getName(); } - public boolean hasPose() { - return m_estPos != null; + public boolean hasPose(EstimatedRobotPose pos) { + return pos != null; } - public Pose3d getPose() { - if(hasPose()) + public Pose3d getPose(EstimatedRobotPose pos) { + if(hasPose(pos)) { - return m_estPos.estimatedPose; + return pos.estimatedPose; } return null; } //gets the timestamp of the latest pose - public double getTimestamp() { - if(hasPose()) + public double getTimestamp(EstimatedRobotPose pos) { + if(hasPose(pos)) { - return m_estPos.timestampSeconds; + return pos.timestampSeconds; } return -1; } - private List getTargets() { - if(hasPose()) + private List getTargets(EstimatedRobotPose pos) { + if(hasPose(pos)) { - return m_estPos.targetsUsed; + return pos.targetsUsed; } return new ArrayList<>(); } - private double getTagAreas() { + private double getTagAreas(EstimatedRobotPose pos) { double ret = 0; - if(!hasPose()) return 0; - for(PhotonTrackedTarget t : getTargets()) { + if(!hasPose(pos)) return 0; + for(PhotonTrackedTarget t : getTargets(pos)) { ret += t.getArea(); } return ret; } - public Matrix getEstStdv() { - return kDefaultStdv.div(getTagAreas()); + public Matrix getEstStdv(EstimatedRobotPose pos) { + if(pos.strategy == PoseStrategy.PNP_DISTANCE_TRIG_SOLVE) + { + return kDefaultStdvTrig.div(getTagAreas(pos)); + } + + return kDefaultStdv.div(getTagAreas(pos)); } - public boolean checkVisionResult() { - if(!hasPose()) return false; + public boolean checkVisionResult(EstimatedRobotPose pos) { + if(!hasPose(pos)) return false; - if(getTagAreas() < 0.3) return false; + if(getTagAreas(pos) < 0.3) return false; - if(getPose().getZ() > 1) return false; + if(getPose(pos).getZ() > 1) return false; - if(getTargets().size() == 1) { - if(getTargets().get(0).getPoseAmbiguity() > 0.6) + if(getTargets(pos).size() == 1) { + if(getTargets(pos).get(0).getPoseAmbiguity() > 0.6) return false; } return true; } - private void copyVisionData() { - m_returnedData.pose = getPose(); - m_returnedData.stdv = getEstStdv(); - m_returnedData.timestamp = getTimestamp(); + private void copyVisionData(EstimatedRobotPose pos) { + m_returnedData.pose = getPose(pos); + m_returnedData.stdv = getEstStdv(pos); + m_returnedData.timestamp = getTimestamp(pos); } public void refreshEstimate(Consumer data) { - if(checkVisionResult()) { - copyVisionData(); + if (checkVisionResult(m_estPos)) { + copyVisionData(m_estPos); + data.accept(m_returnedData); + } + if (checkVisionResult(m_estTrigPos)) { + copyVisionData(m_estTrigPos); data.accept(m_returnedData); } } private final ArrayList m_visionTargets = new ArrayList<>(); - private final ArrayList m_corners = new ArrayList<>(); private static final Transform3d kZeroTransform = new Transform3d(); @Override public void periodic() { - m_poseEstimator.setReferencePose(m_referencePose.get()); + if (m_referencePose != null) { + m_poseEstimator.setReferencePose(m_referencePose.get()); + m_trigSolveEstimator.setReferencePose(m_referencePose.get()); + } //think about if we want to use the trig solver or constrained solve pnp - m_poseEstimator.addHeadingData(m_poseTimestamp.getAsDouble(), m_referencePose.get().getRotation()); + if (kTrigSolveEnabled) { + m_trigSolveEstimator.addHeadingData(m_poseTimestamp.getAsDouble(), m_referencePose.get().getRotation()); + m_poseEstimator.addHeadingData(m_poseTimestamp.getAsDouble(), m_referencePose.get().getRotation()); + } + m_poseEstimator.setRobotToCameraTransform(m_cameraTransform.get()); + m_trigSolveEstimator.setRobotToCameraTransform(m_cameraTransform.get()); if(m_cameraSim != null) { VisionSimUtil.adjustCamera(m_cameraSim, m_cameraTransform.get()); @@ -180,47 +207,53 @@ public void periodic() { m_cameraAlert.set(!m_camera.isConnected()); + Pose3d robot_pose3d = new Pose3d(m_referencePose.get()); + Pose3d robot_pose_transformed = robot_pose3d.transformBy(m_cameraTransform.get()); + + Logger.recordOutput(m_camera.getName() + "/Camera Transform", robot_pose_transformed); + for(PhotonPipelineResult result : m_camera.getAllUnreadResults()) { //fixme: indentation m_estPos = m_poseEstimator.update(result).orElse(null); + if (kTrigSolveEnabled && Math.abs(m_angularVelocity.getAsDouble()) < 0.5) + m_estTrigPos = m_trigSolveEstimator.update(result).orElse(null); + else + m_estTrigPos = null; Logger.recordOutput(m_camera.getName() + "/Target Visible", result.hasTargets()); if(Constants.Vision.kExtraVisionDebugInfo) { - Pose3d robot_pose3d = new Pose3d(m_referencePose.get()); - Pose3d robot_pose_transformed = robot_pose3d.transformBy(m_cameraTransform.get()); + List targets = getTargets(m_estPos); double ambiguity = 0; - Logger.recordOutput(m_camera.getName() + "/Camera Transform", robot_pose_transformed); - m_visionTargets.clear(); - m_corners.clear(); - for(int i = 0; i < getTargets().size(); i++) { - PhotonTrackedTarget t = getTargets().get(i); + for(int i = 0; i < targets.size(); i++) { + PhotonTrackedTarget t = getTargets(m_estPos).get(i); Transform3d trf = t.getBestCameraToTarget(); if(trf.equals(kZeroTransform)) continue; m_visionTargets.add(robot_pose_transformed.transformBy(trf)); - for(TargetCorner c : t.getDetectedCorners()) - m_corners.add(new Translation2d(c.x, c.y)); } - if(getTargets().size() == 1) ambiguity = getTargets().get(0).poseAmbiguity; + if(targets.size() == 1) ambiguity = targets.get(0).poseAmbiguity; Logger.recordOutput(m_camera.getName() + "/Vision Targets", m_visionTargets.toArray(new Pose3d[m_visionTargets.size()])); - Logger.recordOutput(m_camera.getName() + "/Corners", m_corners.toArray(new Translation2d[m_corners.size()])); Logger.recordOutput(m_camera.getName() + "/PoseAmbiguity", ambiguity); } - Logger.recordOutput(m_camera.getName() + "/hasPose", hasPose()); + Logger.recordOutput(m_camera.getName() + "/hasPose", hasPose(m_estPos) || hasPose(m_estTrigPos)); - if(hasPose()) + if (hasPose(m_estPos)) + { + Logger.recordOutput(m_camera.getName() + "/Combined Area", getTagAreas(m_estPos)); + Logger.recordOutput(m_camera.getName() + "/Pose3d", getPose(m_estPos)); + } + if (hasPose(m_estTrigPos)) { - Logger.recordOutput(m_camera.getName() + "/Combined Area", getTagAreas()); - Logger.recordOutput(m_camera.getName() + "/Pose3d", getPose()); + Logger.recordOutput(m_camera.getName() + "/Pose3dTrig", getPose(m_estTrigPos)); } } } diff --git a/src/main/java/team1403/robot/vision/LimelightWrapper.java b/src/main/java/team1403/robot/vision/LimelightWrapper.java index ee922741..2b7e30aa 100644 --- a/src/main/java/team1403/robot/vision/LimelightWrapper.java +++ b/src/main/java/team1403/robot/vision/LimelightWrapper.java @@ -6,11 +6,13 @@ import java.util.ArrayList; import java.util.Optional; import java.util.function.Consumer; +import java.util.function.DoubleSupplier; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; 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.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; @@ -27,21 +29,27 @@ public class LimelightWrapper extends SubsystemBase implements ITagCamera { private final String m_name; - private final Supplier m_imuRotation; + private final Supplier m_robotPose; private final Supplier m_camTransform; + private final DoubleSupplier m_yawRate; private LimelightHelpers.PoseEstimate m_poseEstimateMT1; private LimelightHelpers.PoseEstimate m_poseEstimateMT2; - private final static Matrix kDefaultStdv = VecBuilder.fill(2, 2, 3); //TODO: adjust this - private final static Matrix kDefaultStdvMT2 = VecBuilder.fill(2, 2, Double.POSITIVE_INFINITY); + private final boolean kMT2Enabled; + private final Matrix kDefaultStdv; + private final Matrix kDefaultStdvMT2; private final Alert m_camDisconnected; private final DoubleSubscriber m_latencySubscriber; private final VisionData m_dataReturned = new VisionData(); //data we return to swerve subsystem - public LimelightWrapper(String name, Supplier cameraTransform, Supplier imuRotation) { - m_name = name.toLowerCase(); //hostname must be lowercase - m_imuRotation = imuRotation; - m_camTransform = cameraTransform; + public LimelightWrapper(VisionConfigurator config) { + m_name = config.getName().toLowerCase(); //hostname must be lowercase + m_robotPose = config.getRobotPose(); + m_camTransform = config.getTransform3d(); + kDefaultStdv = config.getDeviations(); + kDefaultStdvMT2 = config.getDeviationsTrig(); + kMT2Enabled = config.getTrigSolveEnabled(); + m_yawRate = config.getYawRate(); m_poseEstimateMT1 = null; m_poseEstimateMT2 = null; @@ -131,11 +139,15 @@ public void refreshEstimate(Consumer data) { private final ArrayList targets = new ArrayList<>(); @Override - public void periodic() { - LimelightHelpers.SetRobotOrientation(m_name, m_imuRotation.get()); + public void periodic() { + if (kMT2Enabled) LimelightHelpers.SetRobotOrientation(m_name, + new Rotation3d(m_robotPose.get().getRotation())); LimelightHelpers.setCameraPose_RobotSpace(m_name, m_camTransform.get()); m_poseEstimateMT1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(m_name); - m_poseEstimateMT2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(m_name); + if (kMT2Enabled && Math.abs(m_yawRate.getAsDouble()) < 0.5) m_poseEstimateMT2 = + LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(m_name); + else + m_poseEstimateMT2 = null; m_camDisconnected.set(!isConnected()); @@ -162,10 +174,9 @@ public void periodic() { Optional pose = Constants.Vision.kFieldLayout.getTagPose(f.id); if(pose.isPresent()) targets.add(pose.get()); } - Logger.recordOutput(m_name + "/visionTargets", targets.toArray(new Pose3d[targets.size()])); } } - + Logger.recordOutput(m_name + "/visionTargets", targets.toArray(new Pose3d[targets.size()])); } } diff --git a/src/main/java/team1403/robot/vision/VisionConfigurator.java b/src/main/java/team1403/robot/vision/VisionConfigurator.java new file mode 100644 index 00000000..83a0cb78 --- /dev/null +++ b/src/main/java/team1403/robot/vision/VisionConfigurator.java @@ -0,0 +1,115 @@ +package team1403.robot.vision; + +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +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.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; + +public class VisionConfigurator { + private String m_name; + private Supplier m_cameraTransform = () -> Transform3d.kZero; + private Supplier m_robotPose = null; + private DoubleSupplier m_poseTimestamp = null; + + /* Yaw rate CCW+, rad/s */ + private DoubleSupplier m_yawRate = () -> 0.0; + + // Default deviations for normal april tag detection + private Matrix m_deviations = VecBuilder.fill(2, 2, 3); + + // Deviations for trig solve (make sure to set third element to infinity) + private Matrix m_deviationsTrig = VecBuilder.fill(1, 1, Double.POSITIVE_INFINITY); + + // Should we even do trig solve ? + // Requirements: Set Robot Rotation Supplier and Trig Deviations + private boolean m_enableTrigSolve = false; + + public VisionConfigurator() {} + + /* Configuration chaining methods */ + + // In case we want to chnage name after init (for a second camera perhaps) + public VisionConfigurator withName(String name) + { + m_name = name; + return this; + } + + public VisionConfigurator withTrigSolve(boolean enabled) + { + m_enableTrigSolve = enabled; + return this; + } + + public VisionConfigurator withDeviations(Matrix dev) + { + m_deviations = dev; + return this; + } + + public VisionConfigurator withDeviationsTrig(Matrix trigDev) + { + m_deviationsTrig = trigDev; + return this; + } + + public VisionConfigurator withTransform(Supplier transform) + { + m_cameraTransform = transform; + return this; + } + + public VisionConfigurator withRobotPose(Supplier pose, DoubleSupplier pose_timestamp) + { + m_robotPose = pose; + m_poseTimestamp = pose_timestamp; + return this; + } + + public VisionConfigurator withYawRate(DoubleSupplier rate) + { + m_yawRate = rate; + return this; + } + + /* Getter methods */ + + public boolean getTrigSolveEnabled() { + return m_enableTrigSolve && m_robotPose != null && m_poseTimestamp != null; + } + + public String getName() { + return m_name; + } + + public Supplier getTransform3d() { + return m_cameraTransform; + } + + public Supplier getRobotPose() { + return m_robotPose; + } + + public DoubleSupplier getPoseTimestamp() { + return m_poseTimestamp; + } + + public DoubleSupplier getYawRate() { + return m_yawRate; + } + + public Matrix getDeviations() { + return m_deviations; + } + + public Matrix getDeviationsTrig() { + return m_deviationsTrig; + } +} diff --git a/src/main/java/team1403/robot/vision/VisionUtil.java b/src/main/java/team1403/robot/vision/VisionUtil.java deleted file mode 100644 index d634d9ed..00000000 --- a/src/main/java/team1403/robot/vision/VisionUtil.java +++ /dev/null @@ -1,17 +0,0 @@ -package team1403.robot.vision; - -import java.util.function.Supplier; - -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.wpilibj2.command.Command; - -public class VisionUtil { - - //find camera offset through measurements - public Transform3d findOffset(Pose3d current, Pose3d target) { - return target.minus(current); - } - -} diff --git a/tuner-swerve-project.json b/tuner-swerve-project.json index 4f2f5404..b260a058 100644 --- a/tuner-swerve-project.json +++ b/tuner-swerve-project.json @@ -1 +1 @@ -{"Version":"1.0.0.0","LastState":11,"Modules":[{"ModuleName":"Front Left","ModuleId":0,"Encoder":{"Id":23,"Name":"Front Left CANCoder","Model":"CANCoder vers. H","CANbus":"CFC879FC394C485320202050134718FF","CANbusFriendly":"Swerve","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":15,"Name":"Front Left Steer","Model":"Talon FXS","CANbus":"CFC879FC394C485320202050134718FF","CANbusFriendly":"Swerve","SelectedMotorType":{"Name":"REV Robotics NEO","FreeSpeedRps":98.0,"SlipCurrentLimit":80,"StatorCurrentLimit":60},"IsStandaloneFx":true},"DriveMotor":{"Id":12,"Name":"Front Left Drive","Model":"Talon FX vers. C","CANbus":"CFC879FC394C485320202050134718FF","CANbusFriendly":"Swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":0.0078125,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":15,"ValidatedDriveId":12,"ValidatedEncoderId":23},{"ModuleName":"Front Right","ModuleId":1,"Encoder":{"Id":22,"Name":"Front Right CANCoder","Model":"CANCoder vers. H","CANbus":"CFC879FC394C485320202050134718FF","CANbusFriendly":"Swerve","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":7,"Name":"Front Right Steer","Model":"Talon FXS","CANbus":"CFC879FC394C485320202050134718FF","CANbusFriendly":"Swerve","SelectedMotorType":{"Name":"REV Robotics NEO","FreeSpeedRps":98.0,"SlipCurrentLimit":80,"StatorCurrentLimit":60},"IsStandaloneFx":true},"DriveMotor":{"Id":8,"Name":"Front Right Drive","Model":"Talon FX vers. C","CANbus":"CFC879FC394C485320202050134718FF","CANbusFriendly":"Swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":0.47314453125,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":7,"ValidatedDriveId":8,"ValidatedEncoderId":22},{"ModuleName":"Back Left","ModuleId":2,"Encoder":{"Id":24,"Name":"Back Left CANCoder","Model":"CANCoder","CANbus":"CFC879FC394C485320202050134718FF","CANbusFriendly":"Swerve","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":13,"Name":"Back Left Steer","Model":"Talon FXS","CANbus":"CFC879FC394C485320202050134718FF","CANbusFriendly":"Swerve","SelectedMotorType":{"Name":"REV Robotics NEO","FreeSpeedRps":98.0,"SlipCurrentLimit":80,"StatorCurrentLimit":60},"IsStandaloneFx":true},"DriveMotor":{"Id":16,"Name":"Back Left Drive","Model":"Talon FX vers. C","CANbus":"CFC879FC394C485320202050134718FF","CANbusFriendly":"Swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":-0.185546875,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":13,"ValidatedDriveId":16,"ValidatedEncoderId":24},{"ModuleName":"Back Right","ModuleId":3,"Encoder":{"Id":21,"Name":"Back Right CANCoder","Model":"CANCoder","CANbus":"CFC879FC394C485320202050134718FF","CANbusFriendly":"Swerve","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":2,"Name":"Back Right Steer","Model":"Talon FXS","CANbus":"CFC879FC394C485320202050134718FF","CANbusFriendly":"Swerve","SelectedMotorType":{"Name":"REV Robotics NEO","FreeSpeedRps":98.0,"SlipCurrentLimit":80,"StatorCurrentLimit":60},"IsStandaloneFx":true},"DriveMotor":{"Id":5,"Name":"Back Right Drive","Model":"Talon FX vers. C","CANbus":"CFC879FC394C485320202050134718FF","CANbusFriendly":"Swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":-0.232177734375,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":2,"ValidatedDriveId":5,"ValidatedEncoderId":21}],"SwerveOptions":{"kSpeedAt12Volts":4.731460295787658,"Gyro":{"Id":25,"Name":"Pigeon 2 vers. S (Device ID 25)","Model":"Pigeon 2 vers. S","CANbus":"CFC879FC394C485320202050134718FF","CANbusFriendly":"Swerve","SelectedMotorType":null,"IsStandaloneFx":false},"IsValidGyroCANbus":true,"VerticalTrackSizeInches":24.0,"HorizontalTrackSizeInches":24.0,"WheelRadiusInches":2.0,"IsLeftSideInverted":false,"IsRightSideInverted":true,"SwerveModuleType":4,"SwerveModuleConfiguration":{"ModuleBrand":4,"DriveRatio":6.746031746031747,"SteerRatio":21.428571428571427,"CouplingRatio":3.5714285714285716,"CustomName":"L2"},"HasVerifiedSteer":true,"SelectedModuleManufacturer":"Swerve Drive Specialties (SDS)","HasVerifiedDrive":true,"IsValidConfiguration":true}} \ No newline at end of file +{"Version":"1.0.0.0","LastState":11,"Modules":[{"ModuleName":"Front Left","ModuleId":0,"Encoder":{"Id":23,"Name":"Front Left CANCoder","Model":"CANCoder vers. H","CANbus":"CFC879FC394C485320202050134718FF","CANbusFriendly":"Swerve","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":15,"Name":"Front Left Steer","Model":"Talon FXS","CANbus":"CFC879FC394C485320202050134718FF","CANbusFriendly":"Swerve","SelectedMotorType":{"Name":"REV Robotics NEO","FreeSpeedRps":98.0,"SlipCurrentLimit":80,"StatorCurrentLimit":60},"IsStandaloneFx":true},"DriveMotor":{"Id":12,"Name":"Front Left Drive","Model":"Talon FX vers. C","CANbus":"CFC879FC394C485320202050134718FF","CANbusFriendly":"Swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":0.0078125,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":15,"ValidatedDriveId":12,"ValidatedEncoderId":23},{"ModuleName":"Front Right","ModuleId":1,"Encoder":{"Id":22,"Name":"Front Right CANCoder","Model":"CANCoder vers. H","CANbus":"CFC879FC394C485320202050134718FF","CANbusFriendly":"Swerve","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":7,"Name":"Front Right Steer","Model":"Talon FXS","CANbus":"CFC879FC394C485320202050134718FF","CANbusFriendly":"Swerve","SelectedMotorType":{"Name":"REV Robotics NEO","FreeSpeedRps":98.0,"SlipCurrentLimit":80,"StatorCurrentLimit":60},"IsStandaloneFx":true},"DriveMotor":{"Id":8,"Name":"Front Right Drive","Model":"Talon FX vers. C","CANbus":"CFC879FC394C485320202050134718FF","CANbusFriendly":"Swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":0.47314453125,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":7,"ValidatedDriveId":8,"ValidatedEncoderId":22},{"ModuleName":"Back Left","ModuleId":2,"Encoder":{"Id":24,"Name":"Back Left CANCoder","Model":"CANCoder","CANbus":"CFC879FC394C485320202050134718FF","CANbusFriendly":"Swerve","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":2,"Name":"Back Left Steer","Model":"Talon FXS","CANbus":"CFC879FC394C485320202050134718FF","CANbusFriendly":"Swerve","SelectedMotorType":{"Name":"REV Robotics NEO","FreeSpeedRps":98.0,"SlipCurrentLimit":80,"StatorCurrentLimit":60},"IsStandaloneFx":true},"DriveMotor":{"Id":16,"Name":"Back Left Drive","Model":"Talon FX vers. C","CANbus":"CFC879FC394C485320202050134718FF","CANbusFriendly":"Swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":-0.185546875,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":13,"ValidatedDriveId":16,"ValidatedEncoderId":24},{"ModuleName":"Back Right","ModuleId":3,"Encoder":{"Id":21,"Name":"Back Right CANCoder","Model":"CANCoder","CANbus":"CFC879FC394C485320202050134718FF","CANbusFriendly":"Swerve","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":13,"Name":"Back Right Steer","Model":"Talon FXS","CANbus":"CFC879FC394C485320202050134718FF","CANbusFriendly":"Swerve","SelectedMotorType":{"Name":"REV Robotics NEO","FreeSpeedRps":98.0,"SlipCurrentLimit":80,"StatorCurrentLimit":60},"IsStandaloneFx":true},"DriveMotor":{"Id":5,"Name":"Back Right Drive","Model":"Talon FX vers. C","CANbus":"CFC879FC394C485320202050134718FF","CANbusFriendly":"Swerve","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":-0.232177734375,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":2,"ValidatedDriveId":5,"ValidatedEncoderId":21}],"SwerveOptions":{"kSpeedAt12Volts":4.731460295787658,"Gyro":{"Id":25,"Name":"Pigeon 2 vers. S (Device ID 25)","Model":"Pigeon 2 vers. S","CANbus":"CFC879FC394C485320202050134718FF","CANbusFriendly":"Swerve","SelectedMotorType":null,"IsStandaloneFx":false},"IsValidGyroCANbus":true,"VerticalTrackSizeInches":24.0,"HorizontalTrackSizeInches":24.0,"WheelRadiusInches":2.0,"IsLeftSideInverted":false,"IsRightSideInverted":true,"SwerveModuleType":4,"SwerveModuleConfiguration":{"ModuleBrand":4,"DriveRatio":6.746031746031747,"SteerRatio":21.428571428571427,"CouplingRatio":3.5714285714285716,"CustomName":"L2"},"HasVerifiedSteer":true,"SelectedModuleManufacturer":"Swerve Drive Specialties (SDS)","HasVerifiedDrive":true,"IsValidConfiguration":true}} \ No newline at end of file diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 1219919e..2d7b1d8e 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2025.2.1", + "version": "v2025.3.1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2025", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.2.1", + "version": "v2025.3.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2025.2.1", + "version": "v2025.3.1", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.2.1", + "version": "v2025.3.1", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2025.2.1" + "version": "v2025.3.1" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2025.2.1" + "version": "v2025.3.1" } ] } \ No newline at end of file