diff --git a/elastic-layout.json b/elastic-layout.json index 7b5f2dbb..94c5295c 100644 --- a/elastic-layout.json +++ b/elastic-layout.json @@ -96,14 +96,14 @@ } }, { - "title": "Back Limelight", + "title": "Swerve", "x": 0.0, "y": 0.0, "width": 166.0, "height": 166.0, "type": "Toggle Switch", "properties": { - "topic": "SmartDashboard/Enabled Subsystems/Back Limelight Is Enabled", + "topic": "SmartDashboard/Enabled Subsystems/Swerve Is Enabled", "period": 0.06, "data_type": "boolean" } @@ -122,27 +122,14 @@ } }, { - "title": "LEDs Is Enabled", - "x": 0.0, - "y": 0.0, - "width": 140.0, - "height": 140.0, - "type": "Toggle Switch", - "properties": { - "topic": "SmartDashboard/Enabled Subsystems/LEDs Is Enabled", - "period": 0.5, - "data_type": "boolean" - } - }, - { - "title": "Swerve", + "title": "Back Limelight", "x": 0.0, "y": 0.0, "width": 166.0, "height": 166.0, "type": "Toggle Switch", "properties": { - "topic": "SmartDashboard/Enabled Subsystems/Swerve Is Enabled", + "topic": "SmartDashboard/Enabled Subsystems/Back Limelight Is Enabled", "period": 0.06, "data_type": "boolean" } @@ -193,7 +180,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Handoff/At Tolerance?", + "topic": "/Robot/Handoff/At Tolerance?", "period": 0.06, "true_color": 4283215696, "false_color": 4294198070, @@ -209,7 +196,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Handoff/State", + "topic": "/Robot/Handoff/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -223,7 +210,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Intake/Pivot At Tolerance?", + "topic": "/Robot/Intake/Pivot At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -240,7 +227,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Intake/Pivot State", + "topic": "/Robot/Intake/Pivot State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -254,7 +241,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Hood/State", + "topic": "/Robot/Superstructure/Hood/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -268,7 +255,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Shooter/State", + "topic": "/Robot/Superstructure/Shooter/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -282,7 +269,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Turret/State", + "topic": "/Robot/Superstructure/Turret/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -296,7 +283,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Superstructure/Hood At Tolerance?", + "topic": "/Robot/Superstructure/Hood At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -313,7 +300,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Superstructure/Shooter At Tolerance?", + "topic": "/Robot/Superstructure/Shooter At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -330,7 +317,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Superstructure/Turret At Tolerance?", + "topic": "/Robot/Superstructure/Turret At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -375,9 +362,8 @@ "height": 140.0, "type": "Match Time", "properties": { - "topic": "/SmartDashboard/FMSUtil/time left in shift", + "topic": "/Robot/FMSUtil/time left in shift", "period": 0.06, - "data_type": "double", "time_display_mode": "Minutes and Seconds", "red_start_time": 20, "yellow_start_time": 30 @@ -419,7 +405,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Spindexer/At Tolerance", + "topic": "/Robot/Spindexer/At Tolerance", "period": 0.5, "true_color": 4283215696, "false_color": 4294198070, @@ -435,7 +421,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Spindexer/State", + "topic": "/Robot/Spindexer/State", "period": 0.5, "data_type": "string", "show_submit_button": false @@ -449,7 +435,7 @@ "height": 140.0, "type": "Match Time", "properties": { - "topic": "/SmartDashboard/Robot/Match Time", + "topic": "/Robot/Match Time", "period": 0.5, "time_display_mode": "Minutes and Seconds", "red_start_time": 20, @@ -519,6 +505,34 @@ "period": 0.5, "sort_options": false } + }, + { + "title": "Wait Time 2", + "x": 980.0, + "y": 560.0, + "width": 140.0, + "height": 140.0, + "type": "Text Display", + "properties": { + "topic": "SmartDashboard/Robot/Auton/Wait Time 2", + "period": 0.5, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Wait Time 1", + "x": 840.0, + "y": 560.0, + "width": 140.0, + "height": 140.0, + "type": "Text Display", + "properties": { + "topic": "SmartDashboard/Robot/Auton/Wait Time 1", + "period": 0.5, + "data_type": "double", + "show_submit_button": false + } } ] } @@ -701,7 +715,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Handoff/At Tolerance?", + "topic": "/Robot/Handoff/At Tolerance?", "period": 0.06, "true_color": 4283215696, "false_color": 4294198070, @@ -717,7 +731,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Handoff/State", + "topic": "/Robot/Handoff/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -731,7 +745,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Intake/Pivot At Tolerance?", + "topic": "/Robot/Intake/Pivot At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -748,7 +762,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Intake/Pivot State", + "topic": "/Robot/Intake/Pivot State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -762,7 +776,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Hood/State", + "topic": "/Robot/Superstructure/Hood/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -776,7 +790,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Turret/State", + "topic": "/Robot/Superstructure/Turret/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -790,7 +804,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Superstructure/Hood At Tolerance?", + "topic": "/Robot/Superstructure/Hood At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -807,7 +821,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Superstructure/Shooter At Tolerance?", + "topic": "/Robot/Superstructure/Shooter At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -824,7 +838,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Superstructure/Turret At Tolerance?", + "topic": "/Robot/Superstructure/Turret At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -841,7 +855,7 @@ "height": 140.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/is active shift", + "topic": "/Robot/is active shift", "period": 0.06, "true_color": 4283215696, "false_color": 4294198070, @@ -857,7 +871,7 @@ "height": 140.0, "type": "Match Time", "properties": { - "topic": "/SmartDashboard/FMSUtil/time left in shift", + "topic": "/Robot/FMSUtil/time left in shift", "period": 0.06, "data_type": "double", "time_display_mode": "Minutes and Seconds", @@ -873,9 +887,8 @@ "height": 140.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/FMSUtil/No Auto Winner Data", + "topic": "/Robot/FMSUtil/No Auto Winner Data", "period": 0.06, - "data_type": "boolean", "true_color": 4283215696, "false_color": 4294198070, "true_icon": "None", @@ -932,7 +945,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Spindexer/At Tolerance", + "topic": "/Robot/Spindexer/At Tolerance", "period": 0.06, "true_color": 4283215696, "false_color": 4294198070, @@ -948,7 +961,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Spindexer/State", + "topic": "/Robot/Spindexer/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -962,7 +975,7 @@ "height": 140.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Intake/Current Angle (deg)", + "topic": "/Robot/Intake/Current Angle (deg)", "period": 0.5, "data_type": "double", "show_submit_button": false @@ -976,7 +989,7 @@ "height": 140.0, "type": "Match Time", "properties": { - "topic": "/SmartDashboard/Robot/Match Time", + "topic": "/Robot/Match Time", "period": 0.5, "time_display_mode": "Minutes and Seconds", "red_start_time": 20, @@ -991,7 +1004,7 @@ "height": 140.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Hood/Current Angle (deg)", + "topic": "/Robot/Superstructure/Hood/Current Angle (deg)", "period": 0.5, "data_type": "double", "show_submit_button": false @@ -1047,7 +1060,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Shooter/State", + "topic": "/Robot/Superstructure/Shooter/State", "period": 0.5, "data_type": "string", "show_submit_button": false @@ -1102,7 +1115,7 @@ "height": 140.0, "type": "Large Text Display", "properties": { - "topic": "/SmartDashboard/FMSUtil/Field State", + "topic": "/Robot/FMSUtil/Field State", "period": 0.06 } } @@ -1287,7 +1300,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Handoff/At Tolerance?", + "topic": "/Robot/Handoff/At Tolerance?", "period": 0.06, "true_color": 4283215696, "false_color": 4294198070, @@ -1303,7 +1316,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Handoff/State", + "topic": "/Robot/Handoff/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -1317,7 +1330,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Intake/Pivot At Tolerance?", + "topic": "/Robot/Intake/Pivot At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -1334,7 +1347,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Intake/Pivot State", + "topic": "/Robot/Intake/Pivot State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -1348,7 +1361,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Hood/State", + "topic": "/Robot/Superstructure/Hood/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -1362,7 +1375,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Shooter/State", + "topic": "/Robot/Superstructure/Shooter/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -1376,7 +1389,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Turret/State", + "topic": "/Robot/Superstructure/Turret/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -1390,7 +1403,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Superstructure/Hood At Tolerance?", + "topic": "/Robot/Superstructure/Hood At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -1407,7 +1420,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Superstructure/Shooter At Tolerance?", + "topic": "/Robot/Superstructure/Shooter At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -1424,7 +1437,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Superstructure/Turret At Tolerance?", + "topic": "/Robot/Superstructure/Turret At Tolerance?", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -1441,7 +1454,7 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Spindexer/At Tolerance", + "topic": "/Robot/Spindexer/At Tolerance", "period": 0.06, "true_color": 4283215696, "false_color": 4294198070, @@ -1457,7 +1470,7 @@ "height": 70.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Spindexer/State", + "topic": "/Robot/Spindexer/State", "period": 0.06, "data_type": "string", "show_submit_button": false @@ -1485,7 +1498,7 @@ "height": 140.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Turret/Current Angle", + "topic": "/Robot/Superstructure/Turret/Current Angle", "period": 0.5, "data_type": "double", "show_submit_button": false @@ -1527,7 +1540,7 @@ "height": 140.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Superstructure/Hood/Current Angle (deg)", + "topic": "/Robot/Superstructure/Hood/Current Angle (deg)", "period": 0.5, "data_type": "double", "show_submit_button": false @@ -1666,7 +1679,7 @@ "height": 140.0, "type": "Text Display", "properties": { - "topic": "/SmartDashboard/Intake/Current Angle (deg)", + "topic": "/Robot/Intake/Current Angle (deg)", "period": 0.5, "data_type": "double", "show_submit_button": false diff --git a/simgui.json b/simgui.json index 407c8652..92d7c18b 100644 --- a/simgui.json +++ b/simgui.json @@ -94,6 +94,9 @@ } }, "NetworkTables": { + "Persistent Values": { + "open": false + }, "transitory": { "SmartDashboard": { "FieldPositions": { @@ -102,6 +105,11 @@ "Intake": { "open": true }, + "Robot": { + "Auton": { + "open": true + } + }, "Spindexer": { "open": true }, @@ -109,6 +117,9 @@ "open": false }, "Superstructure": { + "Turret": { + "open": true + }, "open": true }, "open": true diff --git a/src/main/deploy/pathplanner/autos/Center Hub + Depot.auto b/src/main/deploy/pathplanner/autos/Center Hub + Depot.auto new file mode 100644 index 00000000..175aaa05 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center Hub + Depot.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Hub To Depot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left Middy.auto b/src/main/deploy/pathplanner/autos/Left Bump.auto similarity index 85% rename from src/main/deploy/pathplanner/autos/Left Middy.auto rename to src/main/deploy/pathplanner/autos/Left Bump.auto index 427c8f10..067218bb 100644 --- a/src/main/deploy/pathplanner/autos/Left Middy.auto +++ b/src/main/deploy/pathplanner/autos/Left Bump.auto @@ -7,13 +7,13 @@ { "type": "path", "data": { - "pathName": "Left Bump To NZ" + "pathName": "Left Bump To Score (Start)" } }, { "type": "path", "data": { - "pathName": "Left Middy To Bump Score" + "pathName": "Left Bump To Score" } }, { diff --git a/src/main/deploy/pathplanner/autos/Left Corner Bite.auto b/src/main/deploy/pathplanner/autos/Left Corner Bite.auto new file mode 100644 index 00000000..55916030 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left Corner Bite.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Corner Bite" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Corner Bite To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Bite Score To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score Jiggle" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left Follow.auto b/src/main/deploy/pathplanner/autos/Left Follow.auto new file mode 100644 index 00000000..065f1d16 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left Follow.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Follow To Bump" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Follow To Score" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 2 Cycle.auto b/src/main/deploy/pathplanner/autos/Left Two Cycle.auto similarity index 78% rename from src/main/deploy/pathplanner/autos/Left 2 Cycle.auto rename to src/main/deploy/pathplanner/autos/Left Two Cycle.auto index 4a2aa495..513b4a1c 100644 --- a/src/main/deploy/pathplanner/autos/Left 2 Cycle.auto +++ b/src/main/deploy/pathplanner/autos/Left Two Cycle.auto @@ -25,13 +25,7 @@ { "type": "path", "data": { - "pathName": "Left Score To NZ (F)" - } - }, - { - "type": "path", - "data": { - "pathName": "Left NZ To Score" + "pathName": "Left Score Jiggle" } } ] diff --git a/src/main/deploy/pathplanner/autos/Right Middy.auto b/src/main/deploy/pathplanner/autos/Right Bump.auto similarity index 85% rename from src/main/deploy/pathplanner/autos/Right Middy.auto rename to src/main/deploy/pathplanner/autos/Right Bump.auto index b1b5803c..baaf959b 100644 --- a/src/main/deploy/pathplanner/autos/Right Middy.auto +++ b/src/main/deploy/pathplanner/autos/Right Bump.auto @@ -7,13 +7,13 @@ { "type": "path", "data": { - "pathName": "Right Bump To NZ" + "pathName": "Right Bump To Score (Start)" } }, { "type": "path", "data": { - "pathName": "Right Middy To Bump Score" + "pathName": "Right Bump To Score" } }, { diff --git a/src/main/deploy/pathplanner/autos/Right Corner Bite.auto b/src/main/deploy/pathplanner/autos/Right Corner Bite.auto new file mode 100644 index 00000000..9000ca89 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right Corner Bite.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Corner Bite" + } + }, + { + "type": "path", + "data": { + "pathName": "Right NZ To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Bite Score To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score Jiggle" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right Follow.auto b/src/main/deploy/pathplanner/autos/Right Follow.auto new file mode 100644 index 00000000..4857d51d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right Follow.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Follow To Bump" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Follow To Score" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 2 Cycle.auto b/src/main/deploy/pathplanner/autos/Right Two Cycle.auto similarity index 78% rename from src/main/deploy/pathplanner/autos/Right 2 Cycle.auto rename to src/main/deploy/pathplanner/autos/Right Two Cycle.auto index 4c0a5a8e..78af2d36 100644 --- a/src/main/deploy/pathplanner/autos/Right 2 Cycle.auto +++ b/src/main/deploy/pathplanner/autos/Right Two Cycle.auto @@ -25,13 +25,7 @@ { "type": "path", "data": { - "pathName": "Right Score To NZ (F)" - } - }, - { - "type": "path", - "data": { - "pathName": "Right NZ To Score" + "pathName": "Right Score Jiggle" } } ] diff --git a/src/main/deploy/pathplanner/paths/Center Hub To Depot.path b/src/main/deploy/pathplanner/paths/Center Hub To Depot.path new file mode 100644 index 00000000..11090489 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Hub To Depot.path @@ -0,0 +1,84 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6767760342368048, + "y": 4.015592011412268 + }, + "prevControl": null, + "nextControl": { + "x": 1.8783024251069897, + "y": 4.106162624821684 + }, + "isLocked": false, + "linkedName": "Center Hub" + }, + { + "anchor": { + "x": 1.9947503566333804, + "y": 5.374151212553495 + }, + "prevControl": { + "x": 1.9947503566333804, + "y": 4.339058487874466 + }, + "nextControl": { + "x": 1.9947503566333804, + "y": 5.859787410660789 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.5456205420827387, + "y": 5.930513552068473 + }, + "prevControl": { + "x": 1.6583452211126959, + "y": 6.059900142653353 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Depot" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 1.7771775827143685, + "constraints": { + "maxVelocity": 0.75, + "maxAcceleration": 1.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.75, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "To Depot", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Depot To Score.path b/src/main/deploy/pathplanner/paths/Depot To Score.path index d3c9f452..ee4f9b3e 100644 --- a/src/main/deploy/pathplanner/paths/Depot To Score.path +++ b/src/main/deploy/pathplanner/paths/Depot To Score.path @@ -3,24 +3,24 @@ "waypoints": [ { "anchor": { - "x": 0.5795133587786261, - "y": 5.959713740458016 + "x": 0.5456205420827387, + "y": 5.930513552068473 }, "prevControl": null, "nextControl": { - "x": 1.617398935069807, - "y": 5.933846094852308 + "x": 1.5835061183739199, + "y": 5.904645906462766 }, "isLocked": false, "linkedName": "Depot" }, { "anchor": { - "x": 2.7074970344009497, + "x": 1.4642653352353778, "y": 5.959713740458016 }, "prevControl": { - "x": 1.856554697887364, + "x": 0.6133229987217922, "y": 5.954351642472394 }, "nextControl": null, @@ -28,27 +28,8 @@ "linkedName": "Depot Out" } ], - "rotationTargets": [ - { - "waypointRelativePos": 0.3262260127931784, - "rotationDegrees": 180.0 - } - ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.14848143982002254, - "maxWaypointRelativePos": 0.9448818897637792, - "constraints": { - "maxVelocity": 0.5, - "maxAcceleration": 0.75, - "maxAngularVelocity": 50.0, - "maxAngularAcceleration": 100.0, - "nominalVoltage": 12.5, - "unlimited": false - } - } - ], + "rotationTargets": [], + "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { @@ -61,7 +42,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "reversed": false, "folder": "To Score", diff --git a/src/main/deploy/pathplanner/paths/Left Bite Score To Score.path b/src/main/deploy/pathplanner/paths/Left Bite Score To Score.path new file mode 100644 index 00000000..0a057cd8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Bite Score To Score.path @@ -0,0 +1,131 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": null, + "nextControl": { + "x": 7.894778887303849, + "y": 7.509029957203994 + }, + "isLocked": false, + "linkedName": "Left Trench Score" + }, + { + "anchor": { + "x": 7.7265763195435095, + "y": 5.904636233951498 + }, + "prevControl": { + "x": 7.687760342368046, + "y": 7.884251069900142 + }, + "nextControl": { + "x": 7.76360928403665, + "y": 4.015955044801327 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.794992867332382, + "y": 3.782696148359487 + }, + "prevControl": { + "x": 7.6166741233975355, + "y": 3.782696148359487 + }, + "nextControl": { + "x": 5.966918687589157, + "y": 3.782696148359487 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.109243937232525, + "y": 6.059900142653353 + }, + "prevControl": { + "x": 6.143981742457801, + "y": 4.624070860008561 + }, + "nextControl": { + "x": 6.070427960057062, + "y": 7.664293865905849 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 6.0057346647646215, + "y": 7.664293865905849 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.307036247334758, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.8272921108741973, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.2366737739872133, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.9104477611940158, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 2.6183368869935886, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.0533049040511733, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.3176972281449895, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path b/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path index 01589c7e..ccdab241 100644 --- a/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path +++ b/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 0.7207369258195486, - "y": 6.064140721945863 + "x": 1.0114122681883013, + "y": 6.1375320970042795 }, "isLocked": false, "linkedName": "Left Bump Score" }, { "anchor": { - "x": 0.5795133587786261, - "y": 5.959713740458016 + "x": 0.5456205420827387, + "y": 5.930513552068473 }, "prevControl": { - "x": 1.4665553435114504, - "y": 5.959713740458016 + "x": 1.4326625268155633, + "y": 5.930513552068473 }, "nextControl": null, "isLocked": false, @@ -40,11 +40,11 @@ "minWaypointRelativePos": 0, "maxWaypointRelativePos": 0.5016872890888694, "constraints": { - "maxVelocity": 0.5, - "maxAcceleration": 0.75, + "maxVelocity": 0.75, + "maxAcceleration": 1.0, "maxAngularVelocity": 50.0, "maxAngularAcceleration": 100.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false } } @@ -52,7 +52,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.19, + "maxVelocity": 1.75, "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, @@ -67,7 +67,7 @@ "folder": "To Depot", "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Middy To Bump Score.path b/src/main/deploy/pathplanner/paths/Left Bump To Score (Start).path similarity index 61% rename from src/main/deploy/pathplanner/paths/Left Middy To Bump Score.path rename to src/main/deploy/pathplanner/paths/Left Bump To Score (Start).path index 7a194703..b6a8bcfd 100644 --- a/src/main/deploy/pathplanner/paths/Left Middy To Bump Score.path +++ b/src/main/deploy/pathplanner/paths/Left Bump To Score (Start).path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 8.144475190839694, - "y": 4.026631679389314 + "x": 3.650898716119829, + "y": 5.503537803138374 }, "prevControl": null, "nextControl": { - "x": 4.997986641221376, - "y": 5.876030534351146 + "x": 3.240349381820576, + "y": 5.531115411724985 }, "isLocked": false, - "linkedName": "Middy" + "linkedName": "Left Bump Start" }, { "anchor": { - "x": 3.4749522900763368, - "y": 5.507824427480916 + "x": 2.9263338088445074, + "y": 5.503537803138374 }, "prevControl": { - "x": 5.516822519083972, - "y": 5.616612595419848 + "x": 3.24637099264866, + "y": 5.505130255141134 }, "nextControl": null, "isLocked": false, - "linkedName": "Left Bump Score" + "linkedName": "Left Bump Pre" } ], "rotationTargets": [], @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.19, - "maxAcceleration": 10.0, + "maxVelocity": 1.5, + "maxAcceleration": 1.75, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.7, @@ -45,10 +45,10 @@ "rotation": 0.0 }, "reversed": false, - "folder": "To Score", + "folder": "Bump Stuff", "idealStartingState": { "velocity": 0, "rotation": 0.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Bump To Score.path b/src/main/deploy/pathplanner/paths/Left Bump To Score.path new file mode 100644 index 00000000..20dca98f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Bump To Score.path @@ -0,0 +1,146 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.9263338088445074, + "y": 5.503537803138374 + }, + "prevControl": null, + "nextControl": { + "x": 3.8708559201141224, + "y": 5.5477887913667505 + }, + "isLocked": false, + "linkedName": "Left Bump Pre" + }, + { + "anchor": { + "x": 5.59169757489301, + "y": 5.503537803138374 + }, + "prevControl": { + "x": 5.365151789894193, + "y": 5.609259169471154 + }, + "nextControl": { + "x": 7.100692772580049, + "y": 4.799340044217763 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.075920114122681, + "y": 3.9897146932952925 + }, + "prevControl": { + "x": 6.1480599144079875, + "y": 3.52392296718973 + }, + "nextControl": { + "x": 8.683392720506964, + "y": 4.136486598193509 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.623067047075606, + "y": 5.9952068473609135 + }, + "prevControl": { + "x": 8.540539193302806, + "y": 6.260957400061206 + }, + "nextControl": { + "x": 5.746961483594864, + "y": 5.4517831669044226 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.4749522900763368, + "y": 5.507824427480916 + }, + "prevControl": { + "x": 5.7340228245363765, + "y": 5.555292439372327 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Bump Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.03, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.4, + "rotationDegrees": -45.0 + }, + { + "waypointRelativePos": 2.35, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.0, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.588116137744762, + "maxWaypointRelativePos": 3.0357866306549584, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.35651586765697285, + "maxWaypointRelativePos": 0.7454422687373548, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Bump Stuff", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Corner Bite To Score.path b/src/main/deploy/pathplanner/paths/Left Corner Bite To Score.path new file mode 100644 index 00000000..5e2c9e3f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Corner Bite To Score.path @@ -0,0 +1,87 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.231184022824536, + "y": 5.257703281027104 + }, + "prevControl": null, + "nextControl": { + "x": 7.403109843081312, + "y": 4.339058487874465 + }, + "isLocked": false, + "linkedName": "Left NZ Corner" + }, + { + "anchor": { + "x": 6.125962127480992, + "y": 6.067266414244474 + }, + "prevControl": { + "x": 6.122182596291013, + "y": 4.183794579172611 + }, + "nextControl": { + "x": 6.129040098834367, + "y": 7.601126584153624 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 6.743238231098433, + "y": 7.6125392296718974 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2558635394456302, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 0.7249466950959463, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.117270788912574, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.518123667377378, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Corner Bite.path b/src/main/deploy/pathplanner/paths/Left Corner Bite.path new file mode 100644 index 00000000..9ec5ab2a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Corner Bite.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.445677480916031, + "y": 7.675219465648855 + }, + "prevControl": null, + "nextControl": { + "x": 6.639728958630526, + "y": 7.677232524964337 + }, + "isLocked": false, + "linkedName": "Left Trench Start" + }, + { + "anchor": { + "x": 8.231184022824536, + "y": 5.257703281027104 + }, + "prevControl": { + "x": 6.782054208273893, + "y": 7.276134094151212 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left NZ Corner" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.26652452025586154, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 0.6162046908315488, + "rotationDegrees": -55.0 + }, + { + "waypointRelativePos": 0.9253731343283487, + "rotationDegrees": -55.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6212018906144481, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "To NZ", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Corner To Depot.path b/src/main/deploy/pathplanner/paths/Left Corner To Depot.path new file mode 100644 index 00000000..af090dfd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Corner To Depot.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.4701897983392653, + "y": 5.966275207591934 + }, + "prevControl": null, + "nextControl": { + "x": 1.0089571213446336, + "y": 5.9663124527470375 + }, + "isLocked": false, + "linkedName": "Left Follower Bump Post" + }, + { + "anchor": { + "x": 0.5556583629893244, + "y": 5.966275207591934 + }, + "prevControl": { + "x": 1.0669518189123153, + "y": 5.97347056501368 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.75, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "To Depot", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Follow To Bump.path b/src/main/deploy/pathplanner/paths/Left Follow To Bump.path new file mode 100644 index 00000000..89e92769 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Follow To Bump.path @@ -0,0 +1,145 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.63796005706134, + "y": 7.586661911554922 + }, + "prevControl": null, + "nextControl": { + "x": 7.0796433666191145, + "y": 7.53490727532097 + }, + "isLocked": false, + "linkedName": "Left Follower Start" + }, + { + "anchor": { + "x": 8.489957203994294, + "y": 5.956390870185448 + }, + "prevControl": { + "x": 8.489957203994294, + "y": 7.534907275320969 + }, + "nextControl": { + "x": 8.489957203994294, + "y": 4.544594021362044 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.489957203994294, + "y": 2.126547788873039 + }, + "prevControl": { + "x": 8.515834522111271, + "y": 4.222610556348073 + }, + "nextControl": { + "x": 8.480896453405302, + "y": 1.3926269911648226 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.260784593437946, + "y": 2.644094151212554 + }, + "prevControl": { + "x": 7.210655247921225, + "y": 1.0113097543821579 + }, + "nextControl": { + "x": 7.3513552068473595, + "y": 5.59410841654779 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.489957203994294, + "y": 7.4572753209700435 + }, + "prevControl": { + "x": 6.976134094151213, + "y": 7.625477888730385 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Follower Bump" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.40085287846482387, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.742004264392322, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.927505330490407, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.2004264392324204, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 3.0191897654584188, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.5650319829424046, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.8379530916844278, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7778528021606868, + "maxWaypointRelativePos": 3.6623902768399907, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Follow", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Bump To NZ.path b/src/main/deploy/pathplanner/paths/Left Follow To Score.path similarity index 63% rename from src/main/deploy/pathplanner/paths/Left Bump To NZ.path rename to src/main/deploy/pathplanner/paths/Left Follow To Score.path index ea3c532e..5d36788c 100644 --- a/src/main/deploy/pathplanner/paths/Left Bump To NZ.path +++ b/src/main/deploy/pathplanner/paths/Left Follow To Score.path @@ -3,56 +3,56 @@ "waypoints": [ { "anchor": { - "x": 3.500057251908398, - "y": 5.122881679389313 + "x": 8.489957203994294, + "y": 7.4572753209700435 }, "prevControl": null, "nextControl": { - "x": 6.378759541984733, - "y": 5.315353053435115 + "x": 7.907824662507084, + "y": 7.468414584642698 }, "isLocked": false, - "linkedName": "Left Bump Start" + "linkedName": "Left Follower Bump" }, { "anchor": { - "x": 6.110973282442748, - "y": 4.562204198473283 + "x": 3.5036773428232504, + "y": 7.4572753209700435 }, "prevControl": { - "x": 5.914394099487028, - "y": 5.110601060796104 + "x": 5.1102575154516465, + "y": 7.487520923728483 }, "nextControl": { - "x": 6.354700620229008, - "y": 3.8822781488549625 + "x": 2.0296678529062877, + "y": 7.429525504151838 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.144475190839694, - "y": 4.026631679389314 + "x": 1.4701897983392653, + "y": 5.966275207591934 }, "prevControl": { - "x": 7.03148854961832, - "y": 3.951316793893131 + "x": 1.5239857651245559, + "y": 7.730782918149466 }, "nextControl": null, "isLocked": false, - "linkedName": "Middy" + "linkedName": "Left Follower Bump Post" } ], "rotationTargets": [], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 1.6600517687661842, + "minWaypointRelativePos": 0.8317617866005099, "maxWaypointRelativePos": 2.0, "constraints": { "maxVelocity": 1.75, - "maxAcceleration": 7.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.7, @@ -72,13 +72,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "reversed": false, - "folder": "To NZ", + "folder": "Follow", "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score.path b/src/main/deploy/pathplanner/paths/Left NZ To Score.path index 52c62217..88a55f71 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -36,7 +36,7 @@ "y": 7.440906488549619 }, "prevControl": { - "x": 6.018673323823109, + "x": 6.01867332382311, "y": 7.444336661911555 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Left Score Jiggle.path b/src/main/deploy/pathplanner/paths/Left Score Jiggle.path new file mode 100644 index 00000000..75ea4914 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Score Jiggle.path @@ -0,0 +1,134 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": null, + "nextControl": { + "x": 3.3895155587886268, + "y": 7.413061717442285 + }, + "isLocked": false, + "linkedName": "Left Trench Score" + }, + { + "anchor": { + "x": 3.648639663594734, + "y": 7.442254096935589 + }, + "prevControl": { + "x": 3.400211195655958, + "y": 7.4142666655137415 + }, + "nextControl": { + "x": 3.89706813153351, + "y": 7.470241528357436 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.648639663594734, + "y": 7.458193687201929 + }, + "prevControl": { + "x": 3.8970801117553107, + "y": 7.486074571652958 + }, + "nextControl": { + "x": 3.400199215434157, + "y": 7.430312802750899 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.648639663594734, + "y": 7.458193687201929 + }, + "prevControl": { + "x": 3.894935520576916, + "y": 7.4153176935293486 + }, + "nextControl": { + "x": 3.4023438066125493, + "y": 7.501069680874509 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 3.87561279347943, + "y": 7.5185027311970165 + }, + "nextControl": { + "x": 3.400307320643253, + "y": 7.3633102459022215 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 3.391228490406765, + "y": 7.40061338720238 + }, + "nextControl": { + "x": 3.884691623715916, + "y": 7.481199589896857 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.2808444444444445, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 3.530395607526691, + "y": 7.455880365946537 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Corner" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 1.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Score To Corner.path b/src/main/deploy/pathplanner/paths/Left Score To Corner.path new file mode 100644 index 00000000..0be2c04f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Score To Corner.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": null, + "nextControl": { + "x": 3.2245896483318734, + "y": 7.343909351042221 + }, + "isLocked": false, + "linkedName": "Left Trench Score" + }, + { + "anchor": { + "x": 3.2808444444444445, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 3.524888980928413, + "y": 7.49514913056054 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Corner" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.1, + "maxAcceleration": 4.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Score To Score.path b/src/main/deploy/pathplanner/paths/Left Score To Score.path index 8a3f8436..7e3d5fa7 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Score To Score.path @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 6.717360912981454, + "x": 6.717360912981455, "y": 7.521968616262482 }, "isLocked": false, @@ -68,7 +68,7 @@ "y": 7.440906488549619 }, "prevControl": { - "x": 5.294108416547789, + "x": 5.29410841654779, "y": 7.399604078672524 }, "nextControl": null, @@ -98,7 +98,7 @@ "rotationDegrees": 90.0 }, { - "waypointRelativePos": 3.2, + "waypointRelativePos": 2.882729211087425, "rotationDegrees": 0.0 } ], diff --git a/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path b/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path new file mode 100644 index 00000000..2d259744 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Bite Score To Score.path @@ -0,0 +1,131 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6120827389443653, + "y": 0.5868473609129818 + }, + "prevControl": null, + "nextControl": { + "x": 5.565820256776034, + "y": 0.5997860199714706 + }, + "isLocked": false, + "linkedName": "Right Jiggle" + }, + { + "anchor": { + "x": 7.739514978601996, + "y": 1.0008844507845935 + }, + "prevControl": { + "x": 7.7167792788333145, + "y": 0.31502417442937847 + }, + "nextControl": { + "x": 7.817146932952923, + "y": 3.3427817403708993 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.002011412268189, + "y": 3.925021398002854 + }, + "prevControl": { + "x": 8.114526380651917, + "y": 3.9034191656070543 + }, + "nextControl": { + "x": 5.669329529243937, + "y": 3.9508987161198283 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.031611982881596, + "y": 2.0877318116975756 + }, + "prevControl": { + "x": 6.009732033987853, + "y": 3.174435940086786 + }, + "nextControl": { + "x": 6.070427960057061, + "y": 0.15987161198288136 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6120827389443653, + "y": 0.5868473609129818 + }, + "prevControl": { + "x": 6.1480599144079875, + "y": 0.5739087018544944 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Trench Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5714285714285716, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.9722814498933815, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.3475479744136523, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.936034115138597, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 2.4818763326225852, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 3.0618336886993425, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 3.4968017057569094, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path b/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path index 70b3af07..3a10d5d4 100644 --- a/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path +++ b/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path @@ -3,41 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.4749522900763368, - "y": 2.486860687022901 + "x": 2.175891583452211, + "y": 2.514707560627676 }, "prevControl": null, "nextControl": { - "x": 1.8598664122137403, - "y": 4.838358778625954 + "x": 2.2535235378031384, + "y": 6.49981455064194 }, "isLocked": false, "linkedName": "Right Bump Score" }, { "anchor": { - "x": 1.8933396946564884, - "y": 4.938778625954199 + "x": 0.5456205420827387, + "y": 5.930513552068473 }, "prevControl": { - "x": 1.9041990007255656, - "y": 4.689014586365434 - }, - "nextControl": { - "x": 1.8369555219739746, - "y": 6.235614597651982 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.5795133587786261, - "y": 5.959713740458016 - }, - "prevControl": { - "x": 1.684131679389313, - "y": 5.859293893129771 + "x": 2.460542082738944, + "y": 5.775249643366618 }, "nextControl": null, "isLocked": false, @@ -46,7 +30,7 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1.6, + "waypointRelativePos": 0.8, "rotationDegrees": 180.0 } ], @@ -54,13 +38,13 @@ { "name": "Constraints Zone", "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 1.65, + "maxWaypointRelativePos": 0.8129642133693463, "constraints": { - "maxVelocity": 0.75, - "maxAcceleration": 1.0, + "maxVelocity": 1.25, + "maxAcceleration": 1.75, "maxAngularVelocity": 50.0, "maxAngularAcceleration": 100.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false } } @@ -68,7 +52,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.19, + "maxVelocity": 1.75, "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, @@ -83,7 +67,7 @@ "folder": "To Depot", "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Middy To Bump Score.path b/src/main/deploy/pathplanner/paths/Right Bump To Score (Start).path similarity index 61% rename from src/main/deploy/pathplanner/paths/Right Middy To Bump Score.path rename to src/main/deploy/pathplanner/paths/Right Bump To Score (Start).path index b9e05c6d..35964a50 100644 --- a/src/main/deploy/pathplanner/paths/Right Middy To Bump Score.path +++ b/src/main/deploy/pathplanner/paths/Right Bump To Score (Start).path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 8.144475190839694, - "y": 4.026631679389314 + "x": 3.663837375178317, + "y": 2.514707560627676 }, "prevControl": null, "nextControl": { - "x": 5.533559160305344, - "y": 2.2023377862595424 + "x": 3.271444471107021, + "y": 2.511236323347807 }, "isLocked": false, - "linkedName": "Middy" + "linkedName": "Right Bump Start" }, { "anchor": { - "x": 3.4749522900763368, - "y": 2.486860687022901 + "x": 2.9263338088445074, + "y": 2.514707560627676 }, "prevControl": { - "x": 5.223931297709925, - "y": 2.5119656488549618 + "x": 3.2846211203086297, + "y": 2.4831967246180415 }, "nextControl": null, "isLocked": false, - "linkedName": "Right Bump Score" + "linkedName": "Right Bump Pre" } ], "rotationTargets": [], @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.19, - "maxAcceleration": 10.0, + "maxVelocity": 1.5, + "maxAcceleration": 1.75, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.7, @@ -45,10 +45,10 @@ "rotation": 0.0 }, "reversed": false, - "folder": "To Score", + "folder": "Bump Stuff", "idealStartingState": { "velocity": 0, "rotation": 0.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Bump To Score.path b/src/main/deploy/pathplanner/paths/Right Bump To Score.path new file mode 100644 index 00000000..a0d18b65 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Bump To Score.path @@ -0,0 +1,146 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.9263338088445074, + "y": 2.514707560627676 + }, + "prevControl": null, + "nextControl": { + "x": 5.759900142653352, + "y": 2.4241369472182615 + }, + "isLocked": false, + "linkedName": "Right Bump Pre" + }, + { + "anchor": { + "x": 6.80793152639087, + "y": 3.692125534950071 + }, + "prevControl": { + "x": 6.290385164051354, + "y": 2.7864194008559195 + }, + "nextControl": { + "x": 7.1974561009137075, + "y": 4.373793540365035 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.386447931526389, + "y": 3.8603281027104144 + }, + "prevControl": { + "x": 8.27, + "y": 4.106162624821684 + }, + "nextControl": { + "x": 8.741324417931276, + "y": 3.1111444091889777 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.390171184022824, + "y": 2.035977175463623 + }, + "prevControl": { + "x": 8.68906165529474, + "y": 1.5971628270609481 + }, + "nextControl": { + "x": 5.475249643366619, + "y": 2.682910128388018 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.175891583452211, + "y": 2.514707560627676 + }, + "prevControl": { + "x": 3.335176097940783, + "y": 2.594309070466402 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Bump Score" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4690831556503233, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.9552238805970014, + "rotationDegrees": 45.0 + }, + { + "waypointRelativePos": 2.1918976545841944, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.8230277185500956, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.1127616475354434, + "maxWaypointRelativePos": 2.9817690749493897, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.12964213369344296, + "maxWaypointRelativePos": 0.38, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Bump Stuff", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Corner Bite To Score.path b/src/main/deploy/pathplanner/paths/Right Corner Bite To Score.path new file mode 100644 index 00000000..c533a95b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Corner Bite To Score.path @@ -0,0 +1,87 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.295877318116975, + "y": 3.45922967189729 + }, + "prevControl": null, + "nextControl": { + "x": 7.577712672795853, + "y": 4.435875879737979 + }, + "isLocked": false, + "linkedName": "Right NZ Corner" + }, + { + "anchor": { + "x": 6.1480599144079875, + "y": 2.372382310984309 + }, + "prevControl": { + "x": 6.070427960057061, + "y": 4.377874465049929 + }, + "nextControl": { + "x": 6.231289621491587, + "y": 0.2222815446579549 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6120827389443653, + "y": 0.5868473609129818 + }, + "prevControl": { + "x": 6.62679029957204, + "y": 0.5739087018544944 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4179104477611916, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 0.7889125799573445, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.181236673773988, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.5820895522387919, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Corner Bite.path b/src/main/deploy/pathplanner/paths/Right Corner Bite.path new file mode 100644 index 00000000..b8e70915 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Corner Bite.path @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.412204198473283, + "y": 0.3947805343511448 + }, + "prevControl": null, + "nextControl": { + "x": 7.0667047075606275, + "y": 0.4315834522111266 + }, + "isLocked": false, + "linkedName": "Right Trench Start" + }, + { + "anchor": { + "x": 8.237722419928826, + "y": 3.4271055753262156 + }, + "prevControl": { + "x": 7.687760342368046, + "y": 2.1265477888730384 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right NZ" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2665245202558647, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 0.5, + "rotationDegrees": 55.0 + }, + { + "waypointRelativePos": 0.8102345415778156, + "rotationDegrees": 55.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.5077650236326793, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "To NZ", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Follow To Bump.path b/src/main/deploy/pathplanner/paths/Right Follow To Bump.path new file mode 100644 index 00000000..04285bbe --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Follow To Bump.path @@ -0,0 +1,141 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6767760342368048, + "y": 0.48333808844507875 + }, + "prevControl": null, + "nextControl": { + "x": 7.739514978601996, + "y": 0.6903566333808839 + }, + "isLocked": false, + "linkedName": "Right Follower Start" + }, + { + "anchor": { + "x": 8.451141226818828, + "y": 1.4278601997146938 + }, + "prevControl": { + "x": 8.451141226818828, + "y": 0.08223965763195662 + }, + "nextControl": { + "x": 8.451141226818828, + "y": 3.591094105762126 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.791269614835947, + "y": 4.403751783166904 + }, + "prevControl": { + "x": 8.800407075295771, + "y": 4.385731471372979 + }, + "nextControl": { + "x": 7.066704707560627, + "y": 4.416690442225392 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.4678031383737515, + "y": 2.1653637660485026 + }, + "prevControl": { + "x": 7.4678031383737515, + "y": 2.7346647646219684 + }, + "nextControl": { + "x": 7.4678031383737515, + "y": 1.0765927118872947 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.95947218259629, + "y": 0.6256633380884444 + }, + "prevControl": { + "x": 7.4548644793152645, + "y": 0.910313837375179 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Follower Bump" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4690831556503221, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.0234541577825158, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.3731343283582114, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.9, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 2.4051172707889066, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 3.445628997867779, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.961512491559757, + "maxWaypointRelativePos": 3.500337609723156, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.19, + "maxAcceleration": 10.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Follow", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Bump To NZ.path b/src/main/deploy/pathplanner/paths/Right Follow To Score.path similarity index 57% rename from src/main/deploy/pathplanner/paths/Right Bump To NZ.path rename to src/main/deploy/pathplanner/paths/Right Follow To Score.path index e249ebb2..90adc8cf 100644 --- a/src/main/deploy/pathplanner/paths/Right Bump To NZ.path +++ b/src/main/deploy/pathplanner/paths/Right Follow To Score.path @@ -3,56 +3,40 @@ "waypoints": [ { "anchor": { - "x": 3.4833206106870227, - "y": 2.9220133587786257 + "x": 7.95947218259629, + "y": 0.6256633380884444 }, "prevControl": null, "nextControl": { - "x": 5.4749809160305345, - "y": 2.813225190839695 + "x": 6.402342812097675, + "y": 0.6304982186835882 }, "isLocked": false, - "linkedName": "Right Bump Start" + "linkedName": "Right Follower Bump" }, { "anchor": { - "x": 6.336917938931298, - "y": 3.4492175572519077 + "x": 1.7509666666666668, + "y": 0.6256633380884444 }, "prevControl": { - "x": 6.36674218315174, - "y": 2.6439629632999404 - }, - "nextControl": { - "x": 6.3034446564885505, - "y": 4.352996183206107 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.144475190839694, - "y": 4.026631679389314 - }, - "prevControl": { - "x": 7.144475190839694, - "y": 4.026631679389314 + "x": 3.7688065844293814, + "y": 0.6251768362147326 }, "nextControl": null, "isLocked": false, - "linkedName": "Middy" + "linkedName": null } ], "rotationTargets": [], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 1.6496980155306307, - "maxWaypointRelativePos": 2.0, + "minWaypointRelativePos": 0.5455773126266057, + "maxWaypointRelativePos": 1.0, "constraints": { "maxVelocity": 1.75, - "maxAcceleration": 7.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.7, @@ -75,7 +59,7 @@ "rotation": 0.0 }, "reversed": false, - "folder": "To NZ", + "folder": "Follow", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/Right Score Jiggle.path b/src/main/deploy/pathplanner/paths/Right Score Jiggle.path new file mode 100644 index 00000000..29c167c7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Score Jiggle.path @@ -0,0 +1,134 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6120827389443653, + "y": 0.5868473609129818 + }, + "prevControl": null, + "nextControl": { + "x": 3.3131806164768474, + "y": 0.5777032745480257 + }, + "isLocked": false, + "linkedName": "Right Trench Score" + }, + { + "anchor": { + "x": 3.6120827389443653, + "y": 0.5868473609129818 + }, + "prevControl": { + "x": 3.858050216469342, + "y": 0.6315687204629786 + }, + "nextControl": { + "x": 3.3651621367923017, + "y": 0.541952705976242 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.470799257649026, + "y": 0.5880453279537765 + }, + "prevControl": { + "x": 3.220842004837689, + "y": 0.5834223671050037 + }, + "nextControl": { + "x": 3.720756510460363, + "y": 0.5926682888025493 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.330297914985203, + "y": 0.585259293085392 + }, + "prevControl": { + "x": 3.0833481036127086, + "y": 0.54632613673904 + }, + "nextControl": { + "x": 3.5772477263576974, + "y": 0.6241924494317441 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.424220644336901, + "y": 0.5868906824364817 + }, + "prevControl": { + "x": 3.17424461599113, + "y": 0.5834287100977454 + }, + "nextControl": { + "x": 3.6741966726826725, + "y": 0.5903526547752179 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.5181433736885994, + "y": 0.5885220717875714 + }, + "prevControl": { + "x": 3.768126174889212, + "y": 0.5914544946586795 + }, + "nextControl": { + "x": 3.2681605724879867, + "y": 0.5855896489164634 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.2711, + "y": 0.5868473609129818 + }, + "prevControl": { + "x": 3.021101790448806, + "y": 0.5877935222084847 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Corner" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 1.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.7, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Score To Corner.path b/src/main/deploy/pathplanner/paths/Right Score To Corner.path new file mode 100644 index 00000000..7697becc --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Score To Corner.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6120827389443653, + "y": 0.5868473609129818 + }, + "prevControl": null, + "nextControl": { + "x": 3.36543714002239, + "y": 0.5460313281265844 + }, + "isLocked": false, + "linkedName": "Right Trench Score" + }, + { + "anchor": { + "x": 3.2711, + "y": 0.5868473609129818 + }, + "prevControl": { + "x": 3.717247367182559, + "y": 0.5630880724749401 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Corner" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.1, + "maxAcceleration": 4.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Score To Score.path b/src/main/deploy/pathplanner/paths/Right Score To Score.path index d0f23ac2..22f7f845 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Score.path @@ -8,11 +8,11 @@ }, "prevControl": null, "nextControl": { - "x": 6.575035663338087, - "y": 0.6515406562054205 + "x": 7.0796433666191145, + "y": 0.5868473609129818 }, "isLocked": false, - "linkedName": "Right Trench Score" + "linkedName": "Right Jiggle" }, { "anchor": { @@ -52,7 +52,7 @@ "y": 0.5868473609129818 }, "prevControl": { - "x": 7.673089129599265, + "x": 7.673089129599263, "y": 0.5487960706826538 }, "nextControl": null, @@ -78,11 +78,11 @@ "rotationDegrees": -90.0 }, { - "waypointRelativePos": 2.289978678038381, + "waypointRelativePos": 2.1812366737739914, "rotationDegrees": -90.0 }, { - "waypointRelativePos": 2.6703967446591785, + "waypointRelativePos": 2.4179104477611943, "rotationDegrees": 0.0 } ], diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index a693d1fb..551c8ead 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -3,6 +3,8 @@ "robotLength": 0.762, "holonomicMode": true, "pathFolders": [ + "Bump Stuff", + "Follow", "To Depot", "To NZ", "To Score" diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index d2b0eec1..72ead732 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -1,13 +1,15 @@ -/************************ PROJECT TRIBECBOT *************************/ +/** ********************** PROJECT TRIBECBOT ************************ */ /* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ + /* Use of this source code is governed by an MIT-style license */ + /* that can be found in the repository LICENSE file. */ +/** ************************************************************ */ package com.stuypulse.robot; import java.lang.management.GarbageCollectorMXBean; import java.lang.management.ManagementFactory; +import java.lang.reflect.Field; import java.util.List; +import java.util.function.BiConsumer; import com.pathplanner.lib.commands.FollowPathCommand; import com.pathplanner.lib.commands.PathfindingCommand; @@ -28,16 +30,22 @@ import com.stuypulse.robot.util.EnergyUtil; import com.stuypulse.robot.util.FMSUtil; import com.stuypulse.robot.util.PhoenixUtil; +import com.stuypulse.robot.util.superstructure.InterpolationCalculator; import com.stuypulse.robot.util.superstructure.SOTMCalculator; +import dev.doglog.DogLog; +import dev.doglog.DogLogOptions; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.IterativeRobotBase; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.Watchdog; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; + public class Robot extends TimedRobot { public enum RobotMode { @@ -56,7 +64,7 @@ public enum RobotMode { private GcStatsCollector gcStatsCollector; public static boolean fmsAttached; - private static int periodicCounter = 0; + private static int periodicCounter = 0; public static boolean isBlue() { return alliance == Alliance.Blue; @@ -74,12 +82,12 @@ public static int getPeriodicCounter() { return periodicCounter; } - /*************************/ - /*** ROBOT SCHEDULEING ***/ - /*************************/ - + /**************************/ + /**** ROBOT SCHEDULEING ***/ + /**************************/ @Override public void robotInit() { + DogLog.setOptions(new DogLogOptions().withCaptureDs(true)); robot = new RobotContainer(); mode = RobotMode.DISABLED; energyUtil = new EnergyUtil(); @@ -93,8 +101,34 @@ public void robotInit() { CommandScheduler.getInstance().schedule(PathfindingCommand.warmupCommand()); energyUtil = new EnergyUtil(); + + // TODO: UNCOMMENT WHEN TESTING ALL OF THESE CHANGES. + try { + Field watchdogField = IterativeRobotBase.class.getDeclaredField("m_watchdog"); + watchdogField.setAccessible(true); + Watchdog watchdog = (Watchdog) watchdogField.get(this); + watchdog.setTimeout(Settings.WATCHDOG_TIMEOUT); + } catch (Exception e) { + DriverStation.reportWarning("Failed to disable loop overrun warnings.", false); + } + CommandScheduler.getInstance().setPeriod(Settings.WATCHDOG_TIMEOUT); + CommandScheduler.getInstance().schedule(new SwerveAutonInit()); - RobotController.setBrownoutVoltage(6.3); + RobotController.setBrownoutVoltage(5.5); + + // BiConsumer logCommandFunction + // = (Command command, Boolean active) -> { + // String name = command.getName(); + // SmartDashboard.putBoolean( + // "CommandsUnique/" + name + "_" + Integer.toHexString(command.hashCode()), active); + // }; + + // CommandScheduler.getInstance() + // .onCommandInitialize((Command command) -> logCommandFunction.accept(command, true)); + // CommandScheduler.getInstance() + // .onCommandFinish((Command command) -> logCommandFunction.accept(command, false)); + // CommandScheduler.getInstance() + // .onCommandInterrupt((Command command) -> logCommandFunction.accept(command, false)); } @Override @@ -122,21 +156,30 @@ public void robotPeriodic() { if (!Robot.isReal()) { SmartDashboard.putData(CommandScheduler.getInstance()); } - - SmartDashboard.putNumber("Robot/Match Time", DriverStation.getMatchTime()); + DogLog.log("Robot/Match Time", DriverStation.getMatchTime()); SmartDashboard.putData("Robot/Scheduled Commands", CommandScheduler.getInstance()); - SmartDashboard.putNumber("Robot/Battery Voltage", batteryVoltage); - SmartDashboard.putNumber("Robot/CPU Temperature (C)", RobotController.getCPUTemp()); - + DogLog.log("Robot/Battery Voltage", batteryVoltage); + DogLog.log("Robot/CPU Temperature (C)", RobotController.getCPUTemp()); + robot.periodicAfterScheduler(); + + //clearing memoized values + InterpolationCalculator.clearMemoized(); + CommandSwerveDrivetrain.getInstance().clearMemoized(); + Superstructure.getInstance().clearMemoized(); energyUtil.periodic(); } - /*********************/ - /*** DISABLED MODE ***/ - /*********************/ - + /** + * ****************** + */ + /** + * * DISABLED MODE ** + */ + /** + * ****************** + */ @Override public void disabledInit() { mode = RobotMode.DISABLED; @@ -160,12 +203,21 @@ public void disabledPeriodic() { fmsAttached = true; } } - } - /***********************/ - /*** AUTONOMOUS MODE ***/ - /***********************/ + if ((periodicCounter % 50 == 0 && robot.hasWaitTimeOneChanged()) || (periodicCounter % 50 == 0 && robot.hasWaitTimeTwoChanged())) { + robot.configureAutons(); + } + } + /** + * ******************** + */ + /** + * * AUTONOMOUS MODE ** + */ + /** + * ******************** + */ @Override public void autonomousInit() { mode = RobotMode.AUTON; @@ -189,10 +241,15 @@ public void autonomousExit() { CommandScheduler.getInstance().schedule(new SwerveTeleopInit()); } - /*******************/ - /*** TELEOP MODE ***/ - /*******************/ - + /** + * **************** + */ + /** + * * TELEOP MODE ** + */ + /** + * **************** + */ @Override public void teleopInit() { mode = RobotMode.TELEOP; @@ -208,9 +265,9 @@ public void teleopInit() { @Override public void teleopPeriodic() { - SmartDashboard.putNumber("FMSUtil/time left in shift", fmsUtil.getTimeLeftInShift()); - SmartDashboard.putBoolean("FMSUtil/is active shift", fmsUtil.isActiveShift()); - SmartDashboard.putBoolean("FMSUtil/won auto?", fmsUtil.didWinAuto()); + DogLog.log("FMSUtil/time left in shift", fmsUtil.getTimeLeftInShift()); + DogLog.log("FMSUtil/is active shift", fmsUtil.isActiveShift()); + DogLog.log("FMSUtil/won auto?", fmsUtil.didWinAuto()); if (CommandSwerveDrivetrain.getInstance().isOutsideAllianceZone() && Superstructure.getInstance().getState() == SuperstructureState.SOTM) { CommandScheduler.getInstance().schedule( @@ -225,10 +282,15 @@ public void teleopPeriodic() { public void teleopExit() { } - /*****************/ - /*** TEST MODE ***/ - /*****************/ - + /** + * ************** + */ + /** + * * TEST MODE ** + */ + /** + * ************** + */ @Override public void testInit() { mode = RobotMode.TEST; @@ -244,6 +306,7 @@ public void testExit() { } private static final class GcStatsCollector { + private List gcBeans = ManagementFactory.getGarbageCollectorMXBeans(); private final long[] lastTimes = new long[gcBeans.size()]; private final long[] lastCounts = new long[gcBeans.size()]; @@ -266,10 +329,10 @@ public void update() { totalTime += (int) accumTime; totalCount += (int) accumCounts; - SmartDashboard.putNumber("Robot/GC Time MS", (double) accumTime); - SmartDashboard.putNumber("Robot/GC Counts", (double) accumCounts); - SmartDashboard.putNumber("Robot/Sum of GC Time MS", totalTime); - SmartDashboard.putNumber("Robot/Sum of GC Counts", totalCount); + DogLog.log("Robot/GC Time MS", (double) accumTime); + DogLog.log("Robot/GC Counts", (double) accumCounts); + DogLog.log("Robot/Sum of GC Time MS", totalTime); + DogLog.log("Robot/Sum of GC Counts", totalCount); // Logger.recordOutput("LoggedRobot/GCTimeMS", (double) accumTime); // Logger.recordOutput("LoggedRobot/GCCounts", (double) accumCounts); diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index ac0e8b58..f3c29829 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -7,10 +7,17 @@ import com.stuypulse.robot.commands.BuzzController; import com.stuypulse.robot.commands.auton.DoNothingAuton; -import com.stuypulse.robot.commands.auton.regular.LeftMiddy; +import com.stuypulse.robot.commands.auton.regular.Depot; +import com.stuypulse.robot.commands.auton.regular.LeftBump; +import com.stuypulse.robot.commands.auton.regular.LeftFollow; +import com.stuypulse.robot.commands.auton.regular.LeftTwoCorner; import com.stuypulse.robot.commands.auton.regular.LeftTwoCycle; -import com.stuypulse.robot.commands.auton.regular.RightMiddy; +import com.stuypulse.robot.commands.auton.regular.RightBump; +import com.stuypulse.robot.commands.auton.regular.RightFollow; +import com.stuypulse.robot.commands.auton.regular.RightTwoCorner; import com.stuypulse.robot.commands.auton.regular.RightTwoCycle; +import com.stuypulse.robot.commands.auton.test.BoxTest; +import com.stuypulse.robot.commands.auton.test.EmptyTest; import com.stuypulse.robot.commands.handoff.HandoffReverse; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; @@ -25,6 +32,7 @@ import com.stuypulse.robot.commands.intake.IntakeSetState; import com.stuypulse.robot.commands.intake.IntakeStopRollers; import com.stuypulse.robot.commands.intake.IntakeStow; +import com.stuypulse.robot.commands.intake.IntakeTeleopDigest; import com.stuypulse.robot.commands.intake.SeedPivotDeployed; import com.stuypulse.robot.commands.intake.SeedPivotStowed; import com.stuypulse.robot.commands.leds.LEDApplyPattern; @@ -75,10 +83,13 @@ import com.stuypulse.robot.subsystems.vision.LimelightVision; import com.stuypulse.robot.subsystems.vision.LimelightVision.MegaTagMode; import com.stuypulse.robot.util.PathUtil.AutonConfig; +import com.stuypulse.robot.util.superstructure.InterpolationCalculator; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.input.gamepads.AutoGamepad; import com.stuypulse.stuylib.network.SmartBoolean; +import com.stuypulse.stuylib.network.SmartNumber; +import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -88,6 +99,7 @@ import edu.wpi.first.wpilibj2.command.RepeatCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; public class RobotContainer { @@ -126,6 +138,12 @@ public interface EnabledSubsystems { // Autons private static SendableChooser autonChooser = new SendableChooser<>(); + private static SmartNumber waitTimeOne = new SmartNumber("Robot/Auton/Wait Time 1", 0.0); + private static SmartNumber waitTimeTwo = new SmartNumber("Robot/Auton/Wait Time 2", 0.0); + private static double prevWaitTimeOne = 0.0; + private static double prevWaitTimeTwo = 0.0; + private static boolean hasWaitTimeOneChanged = false; + private static boolean hasWaitTimeTwoChanged = false; // Robot container public RobotContainer() { @@ -178,7 +196,7 @@ private void configureButtonBindings() { // Digest (TR) driver.getTopButton() - .whileTrue(new IntakeAutoDigest().repeatedly()) + .whileTrue(new IntakeTeleopDigest().repeatedly()) .onFalse(new IntakeDeploy()); // Intake Stow @@ -230,7 +248,7 @@ private void configureButtonBindings() { () -> superstructure.getState() == SuperstructureState.SOTM && swerve.canShootIntoHub()) ); - // FOTM + // FOTM (BL) driver.getLeftMenuButton() .onTrue(new LEDApplyPattern(Settings.LED.FOTM_ON)) .onTrue(new IntakeRunRollers()) @@ -355,36 +373,77 @@ private void configureElasticButtons() { /*** AUTONS ***/ /**************/ + public void configureAutons() { autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton()); // DEPOT - AutonConfig LEFT_MIDDY = new AutonConfig("Left Middy", LeftMiddy::new, - "Left Bump To NZ", "Left Middy To Bump Score", "Left Bump Score To Depot", "Depot To Score"); - LEFT_MIDDY.register(autonChooser); - AutonConfig RIGHT_MIDDY = new AutonConfig("Right Middy", RightMiddy::new, - "Right Bump To NZ", "Right Middy To Bump Score", "Right Bump Score To Depot", "Depot To Score"); - RIGHT_MIDDY.register(autonChooser); + AutonConfig DEPOT_ONLY = new AutonConfig("Depot Only", Depot::new, prevWaitTimeOne, prevWaitTimeTwo, + "Center Hub To Depot"); + DEPOT_ONLY.register(autonChooser); + + AutonConfig LEFT_BUMP = new AutonConfig("Left Bump", LeftBump::new, prevWaitTimeOne, prevWaitTimeTwo, + "Left Bump To Score (Start)", "Left Bump To Score", "Left Bump Score To Depot"); + LEFT_BUMP.register(autonChooser); + + AutonConfig RIGHT_BUMP = new AutonConfig("Right Bump", RightBump::new, prevWaitTimeOne, prevWaitTimeTwo, + "Right Bump To Score (Start)", "Right Bump To Score", "Right Bump Score To Depot"); + RIGHT_BUMP.register(autonChooser); // TWO CYCLES (TRENCH) - AutonConfig LEFT_TWO_CYCLE = new AutonConfig("Left Two Cycle", LeftTwoCycle::new, - "Left Trench To NZ", "Left NZ To Score", "Left Score To Score", "Left Score To NZ (F)", "Left NZ To Score"); + AutonConfig LEFT_TWO_CYCLE = new AutonConfig("Left Two Cycle", LeftTwoCycle::new, prevWaitTimeOne, prevWaitTimeTwo, + "Left Trench To NZ", "Left NZ To Score", "Left Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); LEFT_TWO_CYCLE.register(autonChooser); - AutonConfig RIGHT_TWO_CYCLE = new AutonConfig("Right Two Cycle", RightTwoCycle::new, - "Right Trench To NZ", "Right NZ To Score", "Right Score To Score", "Right Score To NZ (F)", "Right NZ To Score"); + AutonConfig RIGHT_TWO_CYCLE = new AutonConfig("Right Two Cycle", RightTwoCycle::new, prevWaitTimeOne, prevWaitTimeTwo, + "Right Trench To NZ", "Right NZ To Score", "Right Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); RIGHT_TWO_CYCLE.register(autonChooser); + // TWO CYCLES (CORNER) + AutonConfig LEFT_TWO_CORNER = new AutonConfig("Left Two Corner", LeftTwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, + "Left Corner Bite", "Left NZ To Score", "Left Bite Score To Score", "Left Score To Corner", "Left Score To NZ (F)"); + LEFT_TWO_CORNER.register(autonChooser); + + AutonConfig RIGHT_TWO_CORNER = new AutonConfig("Right Two Corner", RightTwoCorner::new, prevWaitTimeOne, prevWaitTimeTwo, + "Right Corner Bite", "Right NZ To Score", "Right Bite Score To Score", "Right Score To Corner", "Right Score To NZ (F)"); + RIGHT_TWO_CORNER.register(autonChooser); + + // FOLLOWS + AutonConfig LEFT_FOLLOW = new AutonConfig("Left Follow", LeftFollow::new, prevWaitTimeOne, prevWaitTimeTwo, + "Left Follow To Bump", "Left Follow To Score", "Left Corner To Depot"); + LEFT_FOLLOW.register(autonChooser); + + AutonConfig RIGHT_FOLLOW = new AutonConfig("Right Follow", RightFollow::new, prevWaitTimeOne, prevWaitTimeTwo, + "Right Follow To Bump", "Right Follow To Score"); + RIGHT_FOLLOW.register(autonChooser); + + AutonConfig EMPTY_TEST = new AutonConfig("Empty Test", EmptyTest::new, prevWaitTimeOne, prevWaitTimeTwo, + "Right Trench Score To Corner"); + EMPTY_TEST.register(autonChooser); + SmartDashboard.putData("Autonomous", autonChooser); } + public boolean hasWaitTimeOneChanged() { + hasWaitTimeOneChanged = prevWaitTimeOne != getWaitTimeOne(); + prevWaitTimeOne = getWaitTimeOne(); + prevWaitTimeTwo = getWaitTimeTwo(); + return hasWaitTimeOneChanged; + } + + public boolean hasWaitTimeTwoChanged() { + hasWaitTimeTwoChanged = prevWaitTimeTwo != getWaitTimeTwo(); + prevWaitTimeTwo = getWaitTimeTwo(); + return hasWaitTimeTwoChanged; + } + public void configureSysids() { - // autonChooser.addOption("SysID Module Translation Dynamic Forwards", swerve.sysIdDynamic(Direction.kForward)); - // autonChooser.addOption("SysID Module Translation Dynamic Backwards", swerve.sysIdDynamic(Direction.kReverse)); - // autonChooser.addOption("SysID Module Translation Quasi Forwards", swerve.sysIdQuasistatic(Direction.kForward)); - // autonChooser.addOption("SysID Module Translation Quasi Backwards", swerve.sysIdQuasistatic(Direction.kReverse)); + autonChooser.addOption("SysID Module Translation Dynamic Forwards", swerve.sysIdDynamic(Direction.kForward)); + autonChooser.addOption("SysID Module Translation Dynamic Backwards", swerve.sysIdDynamic(Direction.kReverse)); + autonChooser.addOption("SysID Module Translation Quasi Forwards", swerve.sysIdQuasistatic(Direction.kForward)); + autonChooser.addOption("SysID Module Translation Quasi Backwards", swerve.sysIdQuasistatic(Direction.kReverse)); // autonChooser.addOption("SysID Rotation Translation Dynamic Forwards", swerve.sysidRotationDynamic(Direction.kForward)); // autonChooser.addOption("SysID Rotation Translation Dynamic Backwards", swerve.sysidRotationDynamic(Direction.kReverse)); @@ -444,6 +503,14 @@ public Command getAutonomousCommand() { } } + public static double getWaitTimeOne() { + return waitTimeOne.get(); + } + + public static double getWaitTimeTwo() { + return waitTimeTwo.get(); + } + public void periodicAfterScheduler() { //TODO: get from energy util after testing double totalCurrentDraw = handoff.getCurrentDraw() + @@ -452,7 +519,7 @@ public void periodicAfterScheduler() { superstructure.getCurrentDraw() + swerve.getTotalDriveSupplyCurrent() + swerve.getTotalSteerSupplyCurrent(); - SmartDashboard.putNumber("Robot/Total Current Draw", totalCurrentDraw); + DogLog.log("Robot/Total Current Draw", totalCurrentDraw); handoff.periodicAfterScheduler(); intake.periodicAfterScheduler(); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/Depot.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/Depot.java new file mode 100644 index 00000000..02bf7230 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/Depot.java @@ -0,0 +1,54 @@ +package com.stuypulse.robot.commands.auton.regular; + +import java.util.Set; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.subsystems.handoff.Handoff; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class Depot extends SequentialCommandGroup { + + public Depot(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new HandoffRun().alongWith(new SpindexerRun()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + new WaitCommand(4.5).andThen(new HandoffStop().alongWith(new SpindexerStop())), + new WaitCommand(0.5).andThen(new IntakeDeploy()) + ), + + new WaitCommand(0.5), + + new HandoffRun().alongWith(new SpindexerRun()) + + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftMiddy.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftBump.java similarity index 62% rename from src/main/java/com/stuypulse/robot/commands/auton/regular/LeftMiddy.java rename to src/main/java/com/stuypulse/robot/commands/auton/regular/LeftBump.java index 44b4abea..e62711b5 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftMiddy.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftBump.java @@ -1,6 +1,9 @@ package com.stuypulse.robot.commands.auton.regular; +import java.util.Set; + import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.RobotContainer; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.intake.IntakeAutoDigest; @@ -10,51 +13,61 @@ import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.subsystems.handoff.Handoff; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -public class LeftMiddy extends SequentialCommandGroup { +public class LeftBump extends SequentialCommandGroup { - public LeftMiddy(PathPlannerPath... paths) { + public LeftBump(PathPlannerPath... paths) { addCommands( new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), - // NZ Trip 1 - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( - new WaitCommand(0.75).andThen(new IntakeDeploy()) + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + new HandoffRun(), + new SpindexerRun(), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + new WaitCommand(0.5).andThen(new IntakeDeploy()).andThen(new WaitCommand(1.0)) ), - // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new SuperstructureAutoInterpolation() + // NZ Trip 1 + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), + new IntakeDeploy(), + new HandoffStop(), + new SpindexerStop() ), + new WaitCommand(0.5), + // SOTM To Depot - new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new HandoffRun().alongWith(new SpindexerRun()), new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), - new WaitCommand(3.0).andThen( - new IntakeAutoDigest().repeatedly().withTimeout(2.0).andThen(new IntakeDeploy()) - ), new WaitCommand(5.0).andThen( new HandoffStop().alongWith(new SpindexerStop()) ) ), + + new WaitCommand(0.5), // Off Depot new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), - new WaitCommand(1.0).andThen(new HandoffRun().alongWith(new SpindexerRun())), - new WaitCommand(3.0).andThen(new IntakeAutoDigest().repeatedly()) + new HandoffRun().alongWith(new SpindexerRun()), + new WaitCommand(5.0).andThen(new IntakeAutoDigest().repeatedly()) ) ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollow.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollow.java new file mode 100644 index 00000000..822385c7 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftFollow.java @@ -0,0 +1,82 @@ +package com.stuypulse.robot.commands.auton.regular; + +import java.util.Set; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.constants.Gains.Spindexer; +import com.stuypulse.robot.subsystems.handoff.Handoff; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class LeftFollow extends SequentialCommandGroup { + + public LeftFollow(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + // Preloads + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + new HandoffRun(), + new SpindexerRun(), + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne() + 1.0), Set.of()), + new WaitCommand(1.0).andThen(new IntakeDeploy()) + ), + + // To NZ + new ParallelCommandGroup( + new HandoffStop(), + new SpindexerStop(), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) + ), + + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeTwo()), Set.of()), + + // Back + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), + new WaitCommand(3.0).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()) + .andThen( + new ParallelCommandGroup( + new HandoffRun(), + new SpindexerRun())) + ) + ), + + new HandoffStop().alongWith(new SpindexerStop()), + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + + // SOTM To Corner + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + new HandoffRun(), + new SpindexerRun(), + new IntakeAutoDigest().repeatedly() + ) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCorner.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCorner.java new file mode 100644 index 00000000..c4e05f49 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCorner.java @@ -0,0 +1,89 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.regular; + +import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.intake.IntakeDigest; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetHeading; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +import java.util.Set; + +import com.pathplanner.lib.path.PathPlannerPath; + +public class LeftTwoCorner extends SequentialCommandGroup { + + public LeftTwoCorner(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + + // NZ Trip 1 + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( + new WaitCommand(0.2).andThen(new IntakeDeploy()) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new SuperstructureAutoInterpolation() + ), + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), + new WaitCommand(1.0).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) + ), + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), + + // NZ Trip 2 + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new HandoffStop(), + new SpindexerStop() + ), + + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0) + ), + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]).alongWith(new IntakeDeploy()) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java index d2ca9149..a6586094 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java @@ -5,6 +5,7 @@ /***************************************************************/ package com.stuypulse.robot.commands.auton.regular; +import com.stuypulse.robot.RobotContainer; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.intake.IntakeAutoDigest; @@ -19,11 +20,15 @@ import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import java.util.Set; + import com.pathplanner.lib.path.PathPlannerPath; public class LeftTwoCycle extends SequentialCommandGroup { @@ -34,9 +39,11 @@ public LeftTwoCycle(PathPlannerPath... paths) { new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + // NZ Trip 1 CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( - new WaitCommand(0.5).andThen(new IntakeDeploy()) + new WaitCommand(0.2).andThen(new IntakeDeploy()) ), // Trip 1 To Score @@ -45,10 +52,14 @@ public LeftTwoCycle(PathPlannerPath... paths) { ), new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new HandoffRun().andThen( - new SpindexerRun() - ).andThen(new WaitCommand(1.75) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.0), + new ParallelCommandGroup( + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), + new WaitCommand(1.0).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) + ), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), // NZ Trip 2 @@ -60,25 +71,16 @@ public LeftTwoCycle(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new HandoffRun().andThen( - new SpindexerRun() - ).andThen(new WaitCommand(1.75) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0), - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), - new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), - new HandoffStop(), - new SpindexerStop() - ) + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0) + ), - // new SuperstructureSOTM(), - // new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - // new HandoffRun().andThen( - // new SpindexerRun() - // ).andThen(new WaitCommand(2.5) - // .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.5), - // new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()) + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]).alongWith(new IntakeDeploy()) ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightMiddy.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightBump.java similarity index 64% rename from src/main/java/com/stuypulse/robot/commands/auton/regular/RightMiddy.java rename to src/main/java/com/stuypulse/robot/commands/auton/regular/RightBump.java index 2cbffe4f..7b9d645a 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightMiddy.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightBump.java @@ -1,6 +1,9 @@ package com.stuypulse.robot.commands.auton.regular; +import java.util.Set; + import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.RobotContainer; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.intake.IntakeAutoDigest; @@ -10,50 +13,60 @@ import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.subsystems.handoff.Handoff; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -public class RightMiddy extends SequentialCommandGroup { +public class RightBump extends SequentialCommandGroup { - public RightMiddy(PathPlannerPath... paths) { + public RightBump(PathPlannerPath... paths) { addCommands( new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), - // NZ Trip 1 - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( - new WaitCommand(0.75).andThen(new IntakeDeploy()) + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + new HandoffRun(), + new SpindexerRun(), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + new WaitCommand(0.5).andThen(new IntakeDeploy()).andThen(new WaitCommand(1.0)) ), - // Trip 1 To Score - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( - new SuperstructureAutoInterpolation() + // NZ Trip 1 + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), + new IntakeDeploy(), + new HandoffStop(), + new SpindexerStop() ), + new WaitCommand(0.5), + // SOTM To Depot - new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), new HandoffRun().alongWith(new SpindexerRun()), new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), - new WaitCommand(3.0).andThen( - new IntakeAutoDigest().repeatedly().withTimeout(3.0).andThen(new IntakeDeploy()) - ), new WaitCommand(6.0).andThen( new HandoffStop().alongWith(new SpindexerStop()) ) ), + + new WaitCommand(0.5), // Off Depot new ParallelCommandGroup( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), - new WaitCommand(1.0).andThen(new HandoffRun().alongWith(new SpindexerRun())), + new HandoffRun().alongWith(new SpindexerRun()), new WaitCommand(3.0).andThen(new IntakeAutoDigest().repeatedly()) ) diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollow.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollow.java new file mode 100644 index 00000000..6e0ea55e --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightFollow.java @@ -0,0 +1,72 @@ +package com.stuypulse.robot.commands.auton.regular; + +import java.util.Set; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.constants.Gains.Spindexer; +import com.stuypulse.robot.subsystems.handoff.Handoff; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class RightFollow extends SequentialCommandGroup { + + public RightFollow(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + // Preloads + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + new HandoffRun(), + new SpindexerRun(), + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne() + 1.0), Set.of()), + new WaitCommand(1.0).andThen(new IntakeDeploy()) + ), + + // To NZ + new ParallelCommandGroup( + new HandoffStop(), + new SpindexerStop(), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) + ), + + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeTwo()), Set.of()), + + // SOTM To Corner + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), + new WaitCommand(3.0).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()) + .andThen( + new ParallelCommandGroup( + new HandoffRun(), + new SpindexerRun())) + ) + ), + + new IntakeAutoDigest().repeatedly() + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCorner.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCorner.java new file mode 100644 index 00000000..d8aed42f --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCorner.java @@ -0,0 +1,89 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.regular; + +import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.intake.IntakeDigest; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetHeading; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +import java.util.Set; + +import com.pathplanner.lib.path.PathPlannerPath; + +public class RightTwoCorner extends SequentialCommandGroup { + + public RightTwoCorner(PathPlannerPath... paths) { + + addCommands( + + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + + // NZ Trip 1 + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( + new WaitCommand(0.2).andThen(new IntakeDeploy()) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new SuperstructureAutoInterpolation() + ), + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), + new WaitCommand(1.0).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) + ), + new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), + + // NZ Trip 2 + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new HandoffStop(), + new SpindexerStop() + ), + + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0) + ), + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]).alongWith(new IntakeDeploy()) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java index 2224300c..2e860ec2 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java @@ -5,6 +5,7 @@ /***************************************************************/ package com.stuypulse.robot.commands.auton.regular; +import com.stuypulse.robot.RobotContainer; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; import com.stuypulse.robot.commands.intake.IntakeAutoDigest; @@ -14,15 +15,20 @@ import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.swerve.SwerveResetHeading; import com.stuypulse.robot.commands.swerve.SwerveResetPose; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import java.util.Set; + import com.pathplanner.lib.path.PathPlannerPath; public class RightTwoCycle extends SequentialCommandGroup { @@ -32,10 +38,12 @@ public RightTwoCycle(PathPlannerPath... paths) { addCommands( new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), - + + Commands.defer(() -> new WaitCommand(RobotContainer.getWaitTimeOne()), Set.of()), + // NZ Trip 1 CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( - new WaitCommand(0.5).andThen(new IntakeDeploy()) + new WaitCommand(0.2).andThen(new IntakeDeploy()) ), // Trip 1 To Score @@ -44,10 +52,14 @@ public RightTwoCycle(PathPlannerPath... paths) { ), new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new HandoffRun().andThen( - new SpindexerRun() - ).andThen(new WaitCommand(1.75) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.0), + new ParallelCommandGroup( + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), + new WaitCommand(1.0).andThen( + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(4.0)) + ), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), // NZ Trip 2 @@ -59,25 +71,16 @@ public RightTwoCycle(PathPlannerPath... paths) { new SuperstructureSOTM(), new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new HandoffRun().andThen( - new SpindexerRun() - ).andThen(new WaitCommand(1.75) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0), - new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), - new ParallelCommandGroup( CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]), - new HandoffStop(), - new SpindexerStop() - ) + new HandoffRun(), + new SpindexerRun(), + new WaitCommand(0.5) + .andThen(new IntakeAutoDigest().until(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0)), + new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()).withTimeout(15.0) + ), - // new SuperstructureSOTM(), - // new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - // new HandoffRun().andThen( - // new SpindexerRun() - // ).andThen(new WaitCommand(2.5) - // .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.5), - // new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()) + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[4]).alongWith(new IntakeDeploy()) ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java b/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java index 1ad35b24..7f158341 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java @@ -5,21 +5,23 @@ /***************************************************************/ package com.stuypulse.robot.commands.auton.test; +import com.stuypulse.robot.RobotContainer; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import com.pathplanner.lib.path.PathPlannerPath; public class BoxTest extends SequentialCommandGroup { + + // private RobotContainer robot = new RobotContainer(); + // private double waitTime = robot.getWaitTime(); public BoxTest(PathPlannerPath... paths) { addCommands( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]) + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/test/EmptyTest.java b/src/main/java/com/stuypulse/robot/commands/auton/test/EmptyTest.java new file mode 100644 index 00000000..896031f0 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/test/EmptyTest.java @@ -0,0 +1,43 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.test; + +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +import com.pathplanner.lib.path.PathPlannerPath; + +public class EmptyTest extends SequentialCommandGroup { + + public EmptyTest(PathPlannerPath... paths) { + + addCommands( + + new SuperstructureSOTM(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new HandoffRun().alongWith(new SpindexerRun()).andThen(new WaitCommand(0.5)) + .andThen(new IntakeAutoDigest().alongWith(new WaitUntilCommand(() -> Superstructure.getInstance().isHopperEmpty()))), + + new IntakeDeploy().alongWith(new HandoffStop()).alongWith(new SpindexerStop()), + + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeAutoDigest.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeAutoDigest.java index e8ea4209..58b14871 100644 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeAutoDigest.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeAutoDigest.java @@ -9,8 +9,10 @@ public IntakeAutoDigest() { addCommands( - new IntakeDigest().andThen(new WaitCommand(0.25)).andThen(new IntakeDeploy()).andThen(new WaitCommand(0.25)) - .andThen(new IntakeDigest()).andThen(new WaitCommand(0.25)).andThen(new IntakeDeploy()).andThen(new WaitCommand(0.25)) + new IntakeDigest().andThen(new WaitCommand(0.5)).andThen(new IntakeDeploy()).andThen(new WaitCommand(0.5)).andThen( + new IntakeDigest().andThen(new WaitCommand(0.5)).andThen(new IntakeDeploy())).andThen(new WaitCommand(0.5)).andThen( + new IntakeDigest().andThen(new WaitCommand(0.5)).andThen(new IntakeDeploy()) + ) ); diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeTeleopDigest.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeTeleopDigest.java new file mode 100644 index 00000000..f805a619 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeTeleopDigest.java @@ -0,0 +1,18 @@ +package com.stuypulse.robot.commands.intake; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class IntakeTeleopDigest extends SequentialCommandGroup { + + public IntakeTeleopDigest() { + + addCommands( + + new IntakeDigest().andThen(new WaitCommand(0.5)).andThen(new IntakeDeploy()).andThen(new WaitCommand(0.5)) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java index 9453134f..08192002 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultCommand.java @@ -26,8 +26,8 @@ import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.subsystems.vision.LimelightVision; +import dev.doglog.DogLog; import edu.wpi.first.wpilibj.LEDPattern; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; @@ -121,7 +121,7 @@ else if (intake.getPivotState() == PivotState.DEPLOY) { } } - SmartDashboard.putString("Leds/State", state); + DogLog.log("Leds/State", state); } @Override diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java index 62213abb..cfdba21b 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java @@ -5,26 +5,24 @@ /***************************************************************/ package com.stuypulse.robot.commands.swerve; -import com.stuypulse.stuylib.control.angle.AngleController; -import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; -import com.stuypulse.stuylib.math.Angle; -import com.stuypulse.stuylib.streams.angles.filters.AMotionProfile; - +import com.ctre.phoenix6.swerve.SwerveRequest; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Gains.Swerve.Alignment; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.superstructure.turret.Turret; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.superstructure.TurretAngleCalculator; +import com.stuypulse.stuylib.control.angle.AngleController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.math.Angle; +import com.stuypulse.stuylib.streams.angles.filters.AMotionProfile; +import dev.doglog.DogLog; 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.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import com.ctre.phoenix6.swerve.SwerveRequest; - public class SwerveDriveAlignTurretToHub extends Command { private final CommandSwerveDrivetrain swerve; @@ -64,8 +62,8 @@ public void execute() { Angle.fromRotation2d(getTargetAngle()), Angle.fromRotation2d(robot.getRotation())))); - SmartDashboard.putNumber("Swerve/Angle Error", angleController.getError().toDegrees()); - SmartDashboard.putNumber("Swerve/Target Angle Hub Deg", TurretAngleCalculator.getPointAtTargetAngle(Field.getHubPose().getTranslation(), swerve.getTurretPose().getTranslation(), robot.getRotation()).getDegrees()); + DogLog.log("Swerve/Angle Error", angleController.getError().toDegrees()); + DogLog.log("Swerve/Target Angle Hub Deg", TurretAngleCalculator.getPointAtTargetAngle(Field.getHubPose().getTranslation(), swerve.getTurretPose().getTranslation(), robot.getRotation()).getDegrees()); } @Override diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java index 792d9711..e8ea6c32 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java @@ -5,6 +5,13 @@ /***************************************************************/ package com.stuypulse.robot.commands.swerve; +import com.stuypulse.robot.constants.DriverConstants.Driver.Drive; +import com.stuypulse.robot.constants.DriverConstants.Driver.Turn; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.math.SLMath; import com.stuypulse.stuylib.math.Vector2D; @@ -15,15 +22,7 @@ import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; -import com.stuypulse.robot.constants.DriverConstants.Driver.Drive; -import com.stuypulse.robot.constants.DriverConstants.Driver.Turn; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.constants.Settings.Swerve; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import dev.doglog.DogLog; import edu.wpi.first.wpilibj2.command.Command; public class SwerveDriveFOTM extends Command{ @@ -76,8 +75,8 @@ public void execute() { .withVelocityY(speed.get().y) .withRotationalRate(-turn.get())); - SmartDashboard.putNumber("Swerve/Speed x", speed.get().x); - SmartDashboard.putNumber("Swerve/Speed y", speed.get().y); + DogLog.log("Swerve/Speed x", speed.get().x); + DogLog.log("Swerve/Speed y", speed.get().y); } @Override diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java index 3d0e744a..89303369 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java @@ -5,6 +5,16 @@ /***************************************************************/ package com.stuypulse.robot.commands.swerve; +import com.ctre.phoenix6.swerve.SwerveRequest; +import com.stuypulse.robot.constants.DriverConstants.Driver.Drive; +import com.stuypulse.robot.constants.DriverConstants.Driver.Turn; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.math.SLMath; import com.stuypulse.stuylib.math.Vector2D; @@ -17,17 +27,13 @@ import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; -import com.ctre.phoenix6.swerve.SwerveRequest; -import com.stuypulse.robot.constants.DriverConstants.Driver.Drive; -import com.stuypulse.robot.constants.DriverConstants.Driver.Turn; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.constants.Settings.Swerve; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import dev.doglog.DogLog; +import edu.wpi.first.wpilibj.event.EventLoop; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import edu.wpi.first.wpilibj2.command.button.Trigger; public class SwerveDriveSOTM extends Command { @@ -40,6 +46,7 @@ public class SwerveDriveSOTM extends Command { private final IStream turn; private final BStream isIdle; + private boolean isIdleInit; public SwerveDriveSOTM(Gamepad driver) { swerve = CommandSwerveDrivetrain.getInstance(); @@ -68,8 +75,9 @@ public SwerveDriveSOTM(Gamepad driver) { .filtered(new BDebounce.Rising(0.5), new BDebounce.Falling(0.1)); this.driver = driver; + isIdleInit = false; - addRequirements(swerve); + addRequirements(swerve); } private Vector2D getDriverInputAsVelocity() { @@ -78,18 +86,23 @@ private Vector2D getDriverInputAsVelocity() { @Override public void execute() { + DogLog.log("Swerve/SOTM/Idle?", isIdle.get()); + if (isIdle.get()) { - swerve.setControl(new SwerveRequest.SwerveDriveBrake()); + // if (!isIdleInit) { + // CommandScheduler.getInstance().schedule(new IntakeAutoDigest().repeatedly().onlyWhile(() -> isIdle.get()).andThen(new IntakeDeploy())); + swerve.setControl(new SwerveRequest.SwerveDriveBrake()); + // } + isIdleInit = true; } else { Vector2D velocity = speed.get(); - swerve.setControl(swerve.getFieldCentricSwerveRequest() .withVelocityX(velocity.x) .withVelocityY(velocity.y) .withRotationalRate(-turn.get())); + isIdleInit = false; } - SmartDashboard.putBoolean("Swerve/SOTM/Idle?", isIdle.get()); } @Override diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelRadiusCharacterization.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelRadiusCharacterization.java index 4f480ae0..f39f6490 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelRadiusCharacterization.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelRadiusCharacterization.java @@ -7,10 +7,10 @@ import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; public class SwerveWheelRadiusCharacterization extends Command { @@ -91,10 +91,10 @@ public void execute() { double wheelRadius = (gyroDeltaRad * DRIVE_RADIUS_METERS) / wheelDeltaRad; - SmartDashboard.putNumber("Radius Characterization/Radius (m)", wheelRadius); - SmartDashboard.putNumber("Radius Characterization/Radius (in.)", Units.metersToInches(wheelRadius)); - SmartDashboard.putNumber("Radius Characterization/Gyro Delta (rad)", gyroDeltaRad); - SmartDashboard.putNumber("Radius Characterization/Wheel Delta (rad)", wheelDeltaRad); + DogLog.log("Radius Characterization/Radius (m)", wheelRadius); + DogLog.log("Radius Characterization/Radius (in.)", Units.metersToInches(wheelRadius)); + DogLog.log("Radius Characterization/Gyro Delta (rad)", gyroDeltaRad); + DogLog.log("Radius Characterization/Wheel Delta (rad)", wheelDeltaRad); } } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/pidToPose/SwerveDrivePIDToPose.java b/src/main/java/com/stuypulse/robot/commands/swerve/pidToPose/SwerveDrivePIDToPose.java index f2936cd6..b8640092 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/pidToPose/SwerveDrivePIDToPose.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/pidToPose/SwerveDrivePIDToPose.java @@ -5,6 +5,14 @@ /***************************************************************/ package com.stuypulse.robot.commands.swerve.pidToPose; +import java.util.function.Supplier; + +import com.stuypulse.robot.Robot; +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Gains.Swerve.Alignment; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +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.math.Vector2D; @@ -17,22 +25,13 @@ import com.stuypulse.stuylib.streams.vectors.VStream; import com.stuypulse.stuylib.streams.vectors.filters.VFilter; -import com.stuypulse.robot.Robot; -import com.stuypulse.robot.constants.Field; -import com.stuypulse.robot.constants.Gains.Swerve.Alignment; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import com.stuypulse.robot.util.HolonomicController; - +import dev.doglog.DogLog; 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.wpilibj.smartdashboard.FieldObject2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import java.util.function.Supplier; - public class SwerveDrivePIDToPose extends Command { private final CommandSwerveDrivetrain swerve; @@ -160,20 +159,20 @@ public void execute() { .withVelocityY(controller.getOutput().vyMetersPerSecond) .withRotationalRate(controller.getOutput().omegaRadiansPerSecond)); - SmartDashboard.putNumber("Alignment/Target x", targetPose.get().getX()); - SmartDashboard.putNumber("Alignment/Target y", targetPose.get().getY()); - SmartDashboard.putNumber("Alignment/Target angle", targetPose.get().getRotation().getDegrees()); + DogLog.log("Alignment/Target x", targetPose.get().getX()); + DogLog.log("Alignment/Target y", targetPose.get().getY()); + DogLog.log("Alignment/Target angle", targetPose.get().getRotation().getDegrees()); - SmartDashboard.putNumber("Alignment/Error of Angle Controller)", controller.getError().omegaRadiansPerSecond); + DogLog.log("Alignment/Error of Angle Controller)", controller.getError().omegaRadiansPerSecond); - SmartDashboard.putNumber("Alignment/Target Velocity Robot Relative X (m per s)", controller.getOutput().vxMetersPerSecond); - SmartDashboard.putNumber("Alignment/Target Velocity Robot Relative Y (m per s)", controller.getOutput().vyMetersPerSecond); - SmartDashboard.putNumber("Alignment/Target Angular Velocity (rad per s)", controller.getOutput().omegaRadiansPerSecond); + DogLog.log("Alignment/Target Velocity Robot Relative X (m per s)", controller.getOutput().vxMetersPerSecond); + DogLog.log("Alignment/Target Velocity Robot Relative Y (m per s)", controller.getOutput().vyMetersPerSecond); + DogLog.log("Alignment/Target Angular Velocity (rad per s)", controller.getOutput().omegaRadiansPerSecond); - SmartDashboard.putBoolean("Alignment/Is Aligned", isAligned()); - SmartDashboard.putBoolean("Alignment/Is Aligned X", isAlignedX()); - SmartDashboard.putBoolean("Alignment/Is Aligned Y", isAlignedY()); - SmartDashboard.putBoolean("Alignment/Is Aligned Theta", isAlignedTheta()); + DogLog.log("Alignment/Is Aligned", isAligned()); + DogLog.log("Alignment/Is Aligned X", isAlignedX()); + DogLog.log("Alignment/Is Aligned Y", isAlignedY()); + DogLog.log("Alignment/Is Aligned Theta", isAlignedTheta()); } @Override diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index 5176c811..fc9e2cfb 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -5,14 +5,16 @@ /***************************************************************/ package com.stuypulse.robot.constants; +import com.stuypulse.robot.Robot; import com.stuypulse.robot.RobotContainer; import com.stuypulse.robot.util.vision.LimelightHelpers; +import com.stuypulse.robot.util.vision.LimelightHelpers.RawFiducial; import com.stuypulse.stuylib.network.SmartBoolean; +import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** This interface stores information about each camera. */ public interface Cameras { @@ -40,6 +42,7 @@ public static class Camera { private String name; private Pose3d location; private SmartBoolean isEnabled; + private String keyName; private int rejectedCounterNotNull; private int rejectedCounterAngularVelocity; @@ -48,6 +51,13 @@ public static class Camera { private Pipeline currentPipeline; + public Camera(String name, Pose3d location, SmartBoolean isEnabled) { + this.name = name; + this.location = location; + this.isEnabled = isEnabled; + this.keyName = "Vision/" + name + "/"; + } + public enum Pipeline { NO_SUN, LOW_SUN, @@ -103,17 +113,26 @@ public void incrementRejection(RejectionValue rejectionValue) { } } - public void logRejections() { - SmartDashboard.putNumber("Vision/" + name + "/# Rejected Not Null", rejectedCounterNotNull); - SmartDashboard.putNumber("Vision/" + name + "/# Rejected Target Area", rejectedCounterTargetArea); - SmartDashboard.putNumber("Vision/" + name + "/# Rejected Angular Velocity", rejectedCounterAngularVelocity); - SmartDashboard.putNumber("Vision/" + name + "/# Rejected Invalid Position", rejectedCounterInvalidPosition); - } - - public Camera(String name, Pose3d location, SmartBoolean isEnabled) { - this.name = name; - this.location = location; - this.isEnabled = isEnabled; + public void log() { + DogLog.log(keyName + "# Rejected Not Null", rejectedCounterNotNull); + DogLog.log(keyName + "# Rejected Target Area", rejectedCounterTargetArea); + DogLog.log(keyName + "# Rejected Angular Velocity", rejectedCounterAngularVelocity); + DogLog.log(keyName + "# Rejected Invalid Position", rejectedCounterInvalidPosition); + + DogLog.log(keyName + "Heartbeat", LimelightHelpers.getHeartbeat(name)); + DogLog.log(keyName + "Temp (C)", LimelightHelpers.getLimelightDoubleArrayEntry(name, "hw").get()); + DogLog.log(keyName + "Pose MT1", (Robot.isBlue() + ? LimelightHelpers.getBotPoseEstimate_wpiBlue(name).pose + : LimelightHelpers.getBotPoseEstimate_wpiRed(name).pose)); + DogLog.log(keyName + "Pose MT2", (Robot.isBlue() + ? LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(name).pose + : LimelightHelpers.getBotPoseEstimate_wpiRed_MegaTag2(name).pose)); + DogLog.log(keyName + "Pipeline", LimelightHelpers.getCurrentPipelineIndex(name)); + + RawFiducial[] rawFiducials = LimelightHelpers.getRawFiducials(name); + for(Integer i = 0; i < rawFiducials.length; i++) { + DogLog.log(keyName + "Raw Fiducials[" + i.toString() + "]", rawFiducials[i].id); + } } public String getName() { diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 0ce7ce96..5a1c76b9 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -44,7 +44,7 @@ public interface Field { public static final double OPPONENT_ZONE_X = LENGTH - Units.inchesToMeters(158.6); public static final double BEHIND_HUB_TOLERANCE_X = Units.inchesToMeters(144); // To extend the triangle vertex - public static final double BEHIND_HUB_TOLERANCE_Y = Units.inchesToMeters(12); // To extend base of triangle (colinear with back hub) + public static final double BEHIND_HUB_TOLERANCE_Y = Units.inchesToMeters(12) + Units.inchesToMeters(2); // To extend base of triangle (colinear with back hub) public static final Pose2d BEHIND_HUB_TRIANGLE_VERTEX = new Pose2d(Units.inchesToMeters(182.11) + Field.BEHIND_HUB_TOLERANCE_X, WIDTH / 2.0, new Rotation2d()); @@ -64,25 +64,49 @@ public static boolean closerToTop() { return CommandSwerveDrivetrain.getInstance().getPose().getY() >= Field.TOWER_FAR_CENTER.getY(); } - public final Pose2d LEFT_FERRY_ZONE = new Pose2d( + public final Pose2d INNER_LEFT_FERRY_ZONE = new Pose2d( Units.inchesToMeters(31.5), - WIDTH - Units.inchesToMeters(34.5) - Units.inchesToMeters(12), + WIDTH - Units.inchesToMeters(34.5) - Units.inchesToMeters(48), new Rotation2d() ); - public final Pose2d RIGHT_FERRY_ZONE = new Pose2d( + public final Pose2d INNER_RIGHT_FERRY_ZONE = new Pose2d( Units.inchesToMeters(20.75), - Units.inchesToMeters(76) + Units.inchesToMeters(12), + Units.inchesToMeters(76) + Units.inchesToMeters(48), new Rotation2d() ); + public final Pose2d OUTER_LEFT_FERRY_ZONE = new Pose2d( + Units.inchesToMeters(31.5), + WIDTH - Units.inchesToMeters(34.5), + new Rotation2d() + ); + + public final Pose2d OUTER_RIGHT_FERRY_ZONE = new Pose2d( + Units.inchesToMeters(20.75), + Units.inchesToMeters(76), + new Rotation2d() + ); + + public final double FERRY_SWITCH_TRIGGER_METERS_FROM_EDGE = Units.inchesToMeters(75); + public static Pose2d getFerryZonePose(Translation2d robot) { double fieldMidY = WIDTH / 2.0; if (robot.getY() > fieldMidY) { - return LEFT_FERRY_ZONE; + if(robot.getY() > WIDTH - FERRY_SWITCH_TRIGGER_METERS_FROM_EDGE) { + return INNER_LEFT_FERRY_ZONE; + } + else { + return OUTER_LEFT_FERRY_ZONE; + } } else { - return RIGHT_FERRY_ZONE; + if(robot.getY() < FERRY_SWITCH_TRIGGER_METERS_FROM_EDGE) { + return INNER_RIGHT_FERRY_ZONE; + } + else { + return OUTER_RIGHT_FERRY_ZONE; + } } } diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index 0c9bc260..c375402e 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -45,9 +45,9 @@ public interface slot0 { } public interface slot1 { - SmartNumber kP = new SmartNumber("Superstructure/Turret/Gains/kP", 100.0); + SmartNumber kP = new SmartNumber("Superstructure/Turret/Gains/kP", 150.0); // 80 SmartNumber kI = new SmartNumber("Superstructure/Turret/Gains/kI", 0.0); - SmartNumber kD = new SmartNumber("Superstructure/Turret/Gains/kD", 10.0); + SmartNumber kD = new SmartNumber("Superstructure/Turret/Gains/kD", 3.0); // 10 SmartNumber kS = new SmartNumber("Superstructure/Turret/Gains/kS", 0.4775); SmartNumber kV = new SmartNumber("Superstructure/Turret/Gains/kV", 0.0); @@ -71,7 +71,7 @@ public interface Spindexer { public interface Intake { public interface Pivot { - SmartNumber kP = new SmartNumber("Intake/Pivot/Gains/kP", 200.0); + SmartNumber kP = new SmartNumber("Intake/Pivot/Gains/kP", 125.0); SmartNumber kI = new SmartNumber("Intake/Pivot/Gains/kI", 0.0); SmartNumber kD = new SmartNumber("Intake/Pivot/Gains/kD", 10.0); diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 8d72c96b..7a504b27 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -32,6 +32,7 @@ public interface Settings { public final double DT = 0.020; + public final double WATCHDOG_TIMEOUT = 0.2; public final int LOGGING_FREQUENCY = 5; public final double SECONDS_IN_A_MINUTE = 60.0; public final SmartBoolean DEBUG_MODE = new SmartBoolean("Robot/DebugMode", true); @@ -51,6 +52,8 @@ public interface Handoff { double RPM_SOTM_TOLERANCE = 700.0; SmartNumber HANDOFF_RPM = new SmartNumber("Handoff/Target RPM", HANDOFF_MAX); + double IS_EMPTY_AMPERAGE = 8; //TODO: update IS EMPTY VALUE + double FORWARD_DUTY_CYCLE = 1.0; double REVERSE_DUTY_CYCLE = -1.0; @@ -77,14 +80,17 @@ public interface Intake { double PUSHDOWN_CURRENT_TELEOP = -75.0;//new SmartNumber("Intake/Pushdown Current", -65.0); //TODO: GET ACTUAL TYTY double PUSHDOWN_CURRENT_AUTON = -80.0; - double GEAR_RATIO = 37.93; + double GEAR_RATIO = 32.0/20.0 * 64.0/18.0 * 60.0/8.0; - double STALL_CURRENT_LIMIT = 0; //TODO: set value - double STALL_DEBOUNCE = 1.0; //TODO: VERIFY + double PIVOT_STALL_CURRENT = 0; //TODO: set value + double PIVOT_STALL_DEBOUNCE = 1.0; //TODO: VERIFY + + double ROLLER_STALL_DEBOUNCE = 0.05; //TODO: VERIFY + double ROLLER_STALL_CURRENT = 50.0; } public interface Spindexer { - double FORWARD_DUTY_CYCLE = 1.0; //TODO: GET + double FORWARD_DUTY_CYCLE = 1.0; double ANTI_POPCORN_DUTY_CYCLE = 0.2; double REVERSE_DUTY_CYCLE = -1.0; double STOP_SPEED = 0.0; @@ -96,8 +102,10 @@ public interface Spindexer { double TOLERANCE_TO_START_INTAKE_ROLLERS_DURING_SCORING_ROUTINE = 1500.0; double STALL_CURRENT_LIMIT = 40.0; // random number as of 3/9 + double IS_EMPTY_AMPERAGE = 10; //TODO: update IS EMPTY VALUE + /* CONSTANTS */ - double GEAR_RATIO = 9.6 / 1.0; + double GEAR_RATIO = 11.04 / 1.0; } public interface Superstructure { @@ -107,6 +115,9 @@ public interface Superstructure { public final double SHOOTER_SOTM_TOLERANCE_RPM_LOW = 100.0; public final double SHOOTER_FOTM_TOLERANCE_RPM_HIGH = 150.0; public final double SHOOTER_FOTM_TOLERANCE_RPM_LOW = 250.0; + + public final double IS_EMPTY_RPM_TOLERANCE = 150; //TODO: update IS EMPTY VALUE + public final double IS_EMPTY_DEBOUNCE_TIME = 0.4; //TODO: update IS EMPTY VALUE public final Rotation2d HOOD_TOLERANCE = Rotation2d.fromDegrees(0.5); public final Rotation2d HOOD_SOTM_TOLERANCE = Rotation2d.fromDegrees(2); @@ -149,18 +160,17 @@ public interface TOFInterpolation{ public interface FerryRPMInterpolation { double[][] ferryDistanceRPMInterpolation = { + //Lab {1, 2000}, {5.16, 3300.0}, {6.94, 3600.0}, {7.87, 3800.0}, {9.77, 4300.0}, - {10.694, 4700.0}, //STARTING FROM HERE THE DATA IS UNRELIABLE!!! + {10.694, 4700.0}, //STARTING FROM HERE THE DATA IS EXTRAPOLATED!!! {11.516, 4900.0}, {12.416, 5200.0}, {13.316, 5500.0}, {14.216, 5600.0} - // {15.148, 5200.0}, - // {16.54, 5300} //FIELD LENGTH }; } @@ -182,6 +192,8 @@ public interface FerryTOFInterpolation { public interface Shooter { + public final double IS_SHOOTING_CURRENT = 25.0; + public final double GEAR_RATIO = 1.0; public final double FLYWHEEL_RADIUS = Units.inchesToMeters(3.965 / 2.0); @@ -241,7 +253,9 @@ public interface Turret { public final Rotation2d MAX_VEL = new Rotation2d(Units.degreesToRadians(600.0)); public final Rotation2d MAX_ACCEL = new Rotation2d(Units.degreesToRadians(600.0)); public final Rotation2d TOLERANCE = Rotation2d.fromDegrees(2.0); - public final SmartNumber SOTM_TOLERANCE = new SmartNumber("Superstructure/Turret/SOTM Tolerance", 6);//Rotation2d.fromDegrees(10.0); + public final SmartNumber SOTM_TOLERANCE_THRESHOLD_METERS = new SmartNumber("Superstructure/Turret/SOTM Tolerance Dist Threshold (Meters)", 1.75); + public final SmartNumber SOTM_TOLERANCE_CLOSE = new SmartNumber("Superstructure/Turret/SOTM Tolerance (Close)", 10.0); + public final SmartNumber SOTM_TOLERANCE_FAR = new SmartNumber("Superstructure/Turret/SOTM Tolerance (Far)", 6.0);//Rotation2d.fromDegrees(10.0); public final Rotation2d FOTM_TOLERANCE = Rotation2d.fromDegrees(10.0); public final Rotation2d KB = Rotation2d.fromDegrees(0.0); @@ -259,8 +273,9 @@ public interface Turret { public final double RANGE_CW = 90.0;//-360.0; public final double RANGE_CCW = -360.0;//85.0; // -397.0 is further - public final Rotation2d GAIN_SWITCHING_THRESHOLD = Rotation2d.fromDegrees(30); - + public final Rotation2d GAIN_SWITCHING_THRESHOLD_START = Rotation2d.fromDegrees(30); + public final Rotation2d GAIN_SWITCHING_THRESHOLD_END = Rotation2d.fromDegrees(3); + public final Transform2d TURRET_OFFSET = new Transform2d(Units.inchesToMeters(-4.0), Units.inchesToMeters(8.0), Rotation2d.kZero); public final double TURRET_HEIGHT = Units.inchesToMeters(0.0); diff --git a/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java b/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java index e9f8a71e..accaf1f2 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java @@ -5,15 +5,15 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.handoff; +import java.util.Optional; + import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import dev.doglog.DogLog; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import java.util.Optional; - public abstract class Handoff extends SubsystemBase { private static final Handoff instance; private HandoffState state; @@ -76,10 +76,10 @@ public void setState(HandoffState state) { public abstract double getCurrentDraw(); public void periodicAfterScheduler() { - SmartDashboard.putString("Handoff/State", getState().toString()); + DogLog.log("Handoff/State", getState().toString()); // SmartDashboard.putNumber("Handoff/Target RPM", getTargetRPM()); - SmartDashboard.putNumber("Handoff/Current RPM", getLeaderRPM()); + DogLog.log("Handoff/Current RPM", getLeaderRPM()); // SmartDashboard.putBoolean("Handoff/At Tolerance?", atTolerance()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java index 399913ad..0a5da6c1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java @@ -23,17 +23,15 @@ import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.PhoenixUtil; import com.stuypulse.robot.util.SysId; import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; +import dev.doglog.DogLog; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; public class HandoffImpl extends Handoff { @@ -134,22 +132,22 @@ public void periodicAfterScheduler() { motorFollow.stopMotor(); } - SmartDashboard.putNumber("Handoff/Lead Velocity", getLeaderRPM()); - SmartDashboard.putNumber("Handoff/Follow Velocity", getLeaderRPM()); + DogLog.log("Handoff/Lead Velocity", getLeaderRPM()); + DogLog.log("Handoff/Follow Velocity", getLeaderRPM()); if (Settings.DEBUG_MODE.get()) { - SmartDashboard.putNumber("Handoff/Lead Voltage", motorLeadVoltage.getValueAsDouble()); - SmartDashboard.putNumber("Handoff/Lead Supply Current", motorLeadSupplyCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Handoff/Lead Stator Current", motorLeadStatorCurrent.getValueAsDouble()); + DogLog.log("Handoff/Lead Voltage", motorLeadVoltage.getValueAsDouble()); + DogLog.log("Handoff/Lead Supply Current", motorLeadSupplyCurrent.getValueAsDouble()); + DogLog.log("Handoff/Lead Stator Current", motorLeadStatorCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Handoff/Follow Voltage", motorLeadVoltage.getValueAsDouble()); - SmartDashboard.putNumber("Handoff/Follow Supply Current", motorLeadSupplyCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Handoff/Follow Stator Current", motorLeadStatorCurrent.getValueAsDouble()); + DogLog.log("Handoff/Follow Voltage", motorLeadVoltage.getValueAsDouble()); + DogLog.log("Handoff/Follow Supply Current", motorLeadSupplyCurrent.getValueAsDouble()); + DogLog.log("Handoff/Follow Stator Current", motorLeadStatorCurrent.getValueAsDouble()); if(Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { - SmartDashboard.putBoolean("Robot/CAN/Main/Handoff Lead Motor Connected? (ID " + String.valueOf(Ports.Handoff.MOTOR_LEAD) + ")", motorLead.isConnected()); - SmartDashboard.putBoolean("Robot/CAN/Main/Handoff Follow Motor Connected? (ID " + String.valueOf(Ports.Handoff.MOTOR_FOLLOW) + ")", motorFollow.isConnected()); + DogLog.log("Robot/CAN/Main/Handoff Lead Motor Connected? (ID " + String.valueOf(Ports.Handoff.MOTOR_LEAD) + ")", motorLead.isConnected()); + DogLog.log("Robot/CAN/Main/Handoff Follow Motor Connected? (ID " + String.valueOf(Ports.Handoff.MOTOR_FOLLOW) + ")", motorFollow.isConnected()); } Robot.getEnergyUtil().logEnergyUsage(getName(), getCurrentDraw()); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffSim.java b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffSim.java index 8150d7ed..b85888cb 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffSim.java @@ -5,13 +5,14 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.handoff; +import java.util.Optional; + import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.SysId; +import dev.doglog.DogLog; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.LinearQuadraticRegulator; @@ -22,11 +23,8 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.simulation.LinearSystemSim; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import java.util.Optional; - public class HandoffSim extends Handoff { private LinearSystemSim sim; @@ -76,22 +74,22 @@ public void periodic() { if (EnabledSubsystems.HANDOFF.get()) { if (voltageOverride.isPresent()) { sim.setInput(voltageOverride.get()); - SmartDashboard.putNumber("Handoff/Input Voltage", voltageOverride.get()); + DogLog.log("Handoff/Input Voltage", voltageOverride.get()); } else if (Superstructure.getInstance().shouldStop()) { sim.setInput(0.0); - SmartDashboard.putNumber("Handoff/Input Voltage", 0.0); + DogLog.log("Handoff/Input Voltage", 0.0); } else { - SmartDashboard.putNumber("Handoff/Input Voltage", controller.getU(0)); + DogLog.log("Handoff/Input Voltage", controller.getU(0)); sim.setInput(getTargetDutyCycle() * 12); } } else { sim.setInput(0); - SmartDashboard.putNumber("Handoff/Input Voltage", 0.0); + DogLog.log("Handoff/Input Voltage", 0.0); } sim.update(Settings.DT); - SmartDashboard.putNumber("Handoff/Target Duty Cycle", getTargetDutyCycle()); + DogLog.log("Handoff/Target Duty Cycle", getTargetDutyCycle()); } @Override diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java index 65ed6419..cb0c1496 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -5,16 +5,16 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.intake; +import java.util.Optional; + import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; +import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import java.util.Optional; - public abstract class Intake extends SubsystemBase { private static final Intake instance; @@ -100,15 +100,15 @@ public void setRollerState(RollerState state) { public abstract double getCurrentDraw(); public void periodicAfterScheduler() { - SmartDashboard.putString("Intake/Pivot State", getPivotState().toString()); - SmartDashboard.putString("Intake/Roller State", getRollerState().toString()); + DogLog.log("Intake/Pivot State", getPivotState().toString()); + DogLog.log("Intake/Roller State", getRollerState().toString()); - SmartDashboard.putNumber("Intake/Current Angle (deg)", getPivotAngle().getDegrees()); - SmartDashboard.putNumber("Intake/Target Angle (deg)", getPivotState().getTargetAngle().getDegrees()); + DogLog.log("Intake/Current Angle (deg)", getPivotAngle().getDegrees()); + DogLog.log("Intake/Target Angle (deg)", getPivotState().getTargetAngle().getDegrees()); - SmartDashboard.putNumber("Intake/Target Duty Cycle", getRollerState().getTargetDutyCycle()); + DogLog.log("Intake/Target Duty Cycle", getRollerState().getTargetDutyCycle()); - SmartDashboard.putBoolean("Intake/Pivot At Tolerance?", pivotAtTolerance()); + DogLog.log("Intake/Pivot At Tolerance?", pivotAtTolerance()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index 15e217da..330d5fa1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -31,12 +31,12 @@ import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; +import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; public class IntakeImpl extends Intake { @@ -56,6 +56,7 @@ public class IntakeImpl extends Intake { private PositionVoltage positionVoltage; private BStream pivotStalling; + private final BStream isPivotBelowPushDownThreshold; StatusSignal pivotSupplyCurrent; StatusSignal pivotStatorCurrent; @@ -137,8 +138,11 @@ public IntakeImpl() { rollerLeaderVoltage, rollerFollowerVoltage); pivotStalling = BStream.create( - () -> Math.abs(pivotSupplyCurrent.getValueAsDouble()) > Settings.Intake.STALL_CURRENT_LIMIT) - .filtered(new BDebounce.Both(Settings.Intake.STALL_DEBOUNCE)); + () -> Math.abs(pivotSupplyCurrent.getValueAsDouble()) > Settings.Intake.PIVOT_STALL_CURRENT) + .filtered(new BDebounce.Both(Settings.Intake.PIVOT_STALL_DEBOUNCE)); + isPivotBelowPushDownThreshold = BStream.create(() -> isBelowPushDownThreshold()) + .filtered(new BDebounce.Rising(0.5)) + .filtered(new BDebounce.Falling(0.1)); } @Override @@ -167,6 +171,10 @@ public void seedPivotDeployed() { pivot.setPosition(Settings.Intake.PIVOT_MIN_ANGLE.getRotations()); } + private boolean isBelowPushDownThreshold() { + return getPivotAngle().getDegrees() <= Settings.Intake.ANGLE_THRESHOLD_FOR_HOLDING_VOLTAGE.getDegrees(); + } + @Override public void periodicAfterScheduler() { super.periodicAfterScheduler(); @@ -191,7 +199,7 @@ public void periodicAfterScheduler() { } else { // PIVOT if (pivotState == PivotState.DEPLOY && - getPivotAngle().getDegrees() <= Settings.Intake.ANGLE_THRESHOLD_FOR_HOLDING_VOLTAGE.getDegrees() + isPivotBelowPushDownThreshold.get() && rollerState != RollerState.STOP) { // pivot.setControl(new VoltageOut(Settings.Intake.PUSHDOWN_VOLTAGE)); // applying 3 volts double pushdownCurrent = @@ -224,52 +232,53 @@ && getPivotAngle().getDegrees() <= Settings.Intake.THRESHOLD_TO_START_ROLLERS.ge rollerFollower.stopMotor(); } - SmartDashboard.putNumber("Intake/Pivot Angle Error (deg)", + DogLog.log("Intake/Pivot Angle Error (deg)", Math.abs(pivotState.getTargetAngle().getDegrees() - getPivotAngle().getDegrees())); if (Robot.getPeriodicCounter() % Settings.LOGGING_FREQUENCY == 0) { // PIVOT - SmartDashboard.putBoolean("Intake/Pivot Pushdown Voltage Applied?", applyingPushdownCurrent); + DogLog.log("Intake/Pivot Pushdown Voltage Applied?", applyingPushdownCurrent); - SmartDashboard.putNumber("Intake/Pivot Closed Loop Error (deg)", + DogLog.log("Intake/Pivot Closed Loop Error (deg)", pivot.getClosedLoopError().getValueAsDouble() * 360.0); if (Settings.DEBUG_MODE.get()) { - SmartDashboard.putBoolean("Intake/Voltage Override", pivotVoltageOverride.isPresent()); - SmartDashboard.putNumber("Intake/Pivot Temperature (C)", pivotTemperature.getValueAsDouble()); - SmartDashboard.putNumber("Intake/Leader Temperature (C)", + DogLog.log("Intake/Voltage Override", pivotVoltageOverride.isPresent()); + DogLog.log("Intake/Pivot Temperature (C)", pivotTemperature.getValueAsDouble()); + DogLog.log("Intake/Leader Temperature (C)", rollerLeaderTemperature.getValueAsDouble()); - SmartDashboard.putNumber("Intake/Follower Temperature (C)", + DogLog.log("Intake/Follower Temperature (C)", rollerFollowerTemperature.getValueAsDouble()); // Rolers - SmartDashboard.putNumber("Intake/Roller Leader Voltage (volts)", + DogLog.log("Intake/Roller Leader Voltage (volts)", rollerLeaderVoltage.getValueAsDouble()); - SmartDashboard.putNumber("Intake/Roller Leader Current (amps)", + DogLog.log("Intake/Roller Leader Current (amps)", rollerLeaderSupplyCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Intake/Roller Leader Stator Current (amps)", + DogLog.log("Intake/Roller Leader Stator Current (amps)", rollerLeaderStatorCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Intake/Roller Follower Voltage (volts)", + DogLog.log("Intake/Roller Follower Voltage (volts)", rollerFollowerVoltage.getValueAsDouble()); - SmartDashboard.putNumber("Intake/Roller Follower Current (amps)", + DogLog.log("Intake/Roller Follower Current (amps)", rollerFollowerSupplyCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Intake/Roller Follower Stator Current (amps)", + DogLog.log("Intake/Roller Follower Stator Current (amps)", rollerFollowerStatorCurrent.getValueAsDouble()); // Pivot - SmartDashboard.putNumber("Intake/Pivot Voltage (volts)", pivotMotorVoltage.getValueAsDouble()); - SmartDashboard.putNumber("Intake/Pivot Supply Current (amps)", + DogLog.log("Intake/Pivot Voltage (volts)", pivotMotorVoltage.getValueAsDouble()); + DogLog.log("Intake/Pivot Supply Current (amps)", pivotSupplyCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Intake/Pivot Stator Current (amps)", + DogLog.log("Intake/Pivot Stator Current (amps)", pivotStatorCurrent.getValueAsDouble()); + DogLog.log("Intake/Pivot is below pushdown Threshold", isPivotBelowPushDownThreshold.get()); if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { - SmartDashboard.putBoolean("Robot/CAN/Main/Intake Pivot Motor Connected? (ID " + DogLog.log("Robot/CAN/Main/Intake Pivot Motor Connected? (ID " + String.valueOf(Ports.Intake.PIVOT) + ")", pivot.isConnected()); - SmartDashboard.putBoolean("Robot/CAN/Main/Intake Roller Leader Motor Connected? (ID " + DogLog.log("Robot/CAN/Main/Intake Roller Leader Motor Connected? (ID " + String.valueOf(Ports.Intake.ROLLER_LEADER) + ")", rollerLeader.isConnected()); - SmartDashboard.putBoolean("Robot/CAN/Main/Intake Roller Follower Motor Connected? (ID " + DogLog.log("Robot/CAN/Main/Intake Roller Follower Motor Connected? (ID " + String.valueOf(Ports.Intake.ROLLER_FOLLOWER) + ")", rollerFollower.isConnected()); } Robot.getEnergyUtil().logEnergyUsage(getName(), getCurrentDraw()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeSim.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeSim.java index b5e46563..ca3dbffb 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeSim.java @@ -5,10 +5,13 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.intake; +import java.util.Optional; + import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.util.SysId; +import dev.doglog.DogLog; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.LinearQuadraticRegulator; @@ -23,11 +26,8 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.simulation.LinearSystemSim; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import java.util.Optional; - public class IntakeSim extends Intake { private static final double ARM_LENGTH_METERS = 0.4; @@ -186,10 +186,10 @@ public void periodic() { rollerFollowerSim.update(Settings.DT); if (Settings.DEBUG_MODE.get()) { - SmartDashboard.putNumber("Intake/Sim Pivot Angle (deg)", getPivotAngle().getDegrees()); - SmartDashboard.putNumber("Intake/Sim Pivot Velocity (deg per s)", Units.radiansToDegrees(pivotSim.getVelocityRadPerSec())); - SmartDashboard.putNumber("Intake/Sim Roller Leader Velocity (RPM)", rollerLeaderSim.getOutput(0) * 60.0 / (2.0 * Math.PI)); - SmartDashboard.putNumber("Intake/Sim Roller Follower Velocity (RPM)", rollerFollowerSim.getOutput(0) * 60.0 / (2.0 * Math.PI)); + DogLog.log("Intake/Sim Pivot Angle (deg)", getPivotAngle().getDegrees()); + DogLog.log("Intake/Sim Pivot Velocity (deg per s)", Units.radiansToDegrees(pivotSim.getVelocityRadPerSec())); + DogLog.log("Intake/Sim Roller Leader Velocity (RPM)", rollerLeaderSim.getOutput(0) * 60.0 / (2.0 * Math.PI)); + DogLog.log("Intake/Sim Roller Follower Velocity (RPM)", rollerFollowerSim.getOutput(0) * 60.0 / (2.0 * Math.PI)); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/spindexer/Spindexer.java b/src/main/java/com/stuypulse/robot/subsystems/spindexer/Spindexer.java index de13f04a..a91710e0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/Spindexer.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/Spindexer.java @@ -5,15 +5,15 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.spindexer; +import java.util.Optional; + import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import dev.doglog.DogLog; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import java.util.Optional; - public abstract class Spindexer extends SubsystemBase { private static final Spindexer instance; private SpindexerState spindexerState; @@ -65,7 +65,7 @@ public void setState(SpindexerState state) { public abstract double getCurrentDraw(); public void periodicAfterScheduler() { - SmartDashboard.putString("Spindexer/State", getState().name()); - SmartDashboard.putNumber("Spindexer/Target Duty Cycle", getTargetDutyCycle()); + DogLog.log("Spindexer/State", getState().name()); + DogLog.log("Spindexer/Target Duty Cycle", getTargetDutyCycle()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java index 7002c909..88f93592 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -21,18 +21,16 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.handoff.Handoff; import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.PhoenixUtil; import com.stuypulse.robot.util.SysId; import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; +import dev.doglog.DogLog; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; public class SpindexerImpl extends Spindexer { @@ -129,18 +127,18 @@ public void periodicAfterScheduler() { leaderMotor.stopMotor(); } - SmartDashboard.putNumber("Spindexer/Leader Motor RPM", getMotorRPM()); + DogLog.log("Spindexer/Leader Motor RPM", getMotorRPM()); // SmartDashboard.putBoolean("Spindexer/Unjamming", unJamming); - SmartDashboard.putNumber("Spindexer/Leader Voltage (volts)", leaderMotorVoltage.getValueAsDouble()); - SmartDashboard.putNumber("Spindexer/Leader Supply Current (amps)", leaderSupplyCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Spindexer/Leader Stator Current (amps)", leaderStatorCurrent.getValueAsDouble()); + DogLog.log("Spindexer/Leader Voltage (volts)", leaderMotorVoltage.getValueAsDouble()); + DogLog.log("Spindexer/Leader Supply Current (amps)", leaderSupplyCurrent.getValueAsDouble()); + DogLog.log("Spindexer/Leader Stator Current (amps)", leaderStatorCurrent.getValueAsDouble()); // SmartDashboard.putBoolean("Spindexer/Should Stop?", shouldStop()); if (Settings.DEBUG_MODE.get()) { if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { - SmartDashboard.putBoolean( + DogLog.log( "Robot/CAN/Canivore/Spindexer Leader Motor Connected? (ID " + String.valueOf(Ports.Spindexer.MOTOR) + ")", leaderMotor.isConnected()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerSim.java b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerSim.java index 09bf056b..4fa13b41 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerSim.java @@ -11,10 +11,9 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.handoff.Handoff; import com.stuypulse.robot.subsystems.superstructure.Superstructure; -import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.SysId; +import dev.doglog.DogLog; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.LinearQuadraticRegulator; @@ -26,7 +25,6 @@ import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.simulation.LinearSystemSim; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; public class SpindexerSim extends Spindexer { @@ -98,20 +96,20 @@ public void periodic() { if (EnabledSubsystems.SHOOTER.get()) { if (voltageOverride.isPresent()) { sim.setInput(voltageOverride.get()); - SmartDashboard.putNumber("Spindexer/Input Voltage", voltageOverride.get()); + DogLog.log("Spindexer/Input Voltage", voltageOverride.get()); } else if (Superstructure.getInstance().shouldStop() && !isUnjamming) { sim.setInput(0); } else { - SmartDashboard.putNumber("Spindexer/Input Voltage", controller.getU(0)); + DogLog.log("Spindexer/Input Voltage", controller.getU(0)); sim.setInput(controller.getU(0)); } } else { sim.setInput(0); - SmartDashboard.putNumber("Spindexer/Input Voltage", 0.0); + DogLog.log("Spindexer/Input Voltage", 0.0); } - SmartDashboard.putNumber("Spindexer/Current RPM", getCurrentRPM()); - SmartDashboard.putBoolean("Spindexer/Unjamming", isUnjamming); + DogLog.log("Spindexer/Current RPM", getCurrentRPM()); + DogLog.log("Spindexer/Unjamming", isUnjamming); sim.update(Settings.DT); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java index 7cad5f63..c2c07c70 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -5,6 +5,8 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.superstructure; +import java.util.Optional; + import com.stuypulse.robot.Robot; import com.stuypulse.robot.Robot.RobotMode; import com.stuypulse.robot.subsystems.handoff.Handoff; @@ -21,10 +23,10 @@ import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; +import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Superstructure extends SubsystemBase { @@ -44,11 +46,13 @@ public static Superstructure getInstance(){ private SuperstructureState state; + private Optional shouldStop; + private final Hood hood; private final Shooter shooter; private final Turret turret; - private final BStream readyToShoot; + // private final BStream readyToShoot; public Superstructure() { state = SuperstructureState.INTERPOLATION; @@ -56,14 +60,19 @@ public Superstructure() { shooter = Shooter.getInstance(); turret = Turret.getInstance(); - readyToShoot = BStream.create(this::atTolerance) - .filtered(new BDebounce.Both(0.05)); + // readyToShoot = BStream.create(this::atTolerance) + // .filtered(new BDebounce.Both(0.05)); sotmStoppedTimer = new Timer(); sotmStoppedTimer.restart(); + sotmStoppedTimer.stop(); + fotmStoppedTimer = new Timer(); fotmStoppedTimer.restart(); + fotmStoppedTimer.stop(); + + this.shouldStop = Optional.empty(); } public enum SuperstructureState { @@ -115,7 +124,7 @@ public SuperstructureState getState(){ } public boolean isReadyToShoot() { - return readyToShoot.get(); + return turret.turretReadyToShoot() && shooter.shooterReadyToShoot() && hood.hoodReadyToShoot(); } public boolean atTolerance() { @@ -157,8 +166,11 @@ public boolean isTurretWrapping() { public double getCurrentDraw() { return turret.getCurrentDraw() + shooter.getCurrentDraw() + hood.getCurrentDraw(); } - + public boolean shouldStop() { + if (!shouldStop.isEmpty()) { + return shouldStop.get(); + } CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); boolean isSpindexerStopState = Spindexer.getInstance().getState() == SpindexerState.STOP; @@ -166,7 +178,7 @@ public boolean shouldStop() { boolean isTurretWrapping = isTurretWrapping(); boolean isBehindHubWhileFerrying = getState() == SuperstructureState.FOTM - && swerve.getIsBehindHub(); + && swerve.isBehindHub(); boolean isOutsideAllianceZone = CommandSwerveDrivetrain.getInstance().isOutsideAllianceZone() && getState() != SuperstructureState.FOTM; @@ -180,23 +192,33 @@ public boolean shouldStop() { boolean turretLaggingSOTM = !isTurretAtTolerance() && getState() == SuperstructureState.SOTM; - - SmartDashboard.putBoolean("Spindexer/Should Stop/turret lagging sotm", turretLaggingSOTM); - SmartDashboard.putBoolean("Spindexer/Should Stop/is Behind Hub While Ferrying?", isBehindHubWhileFerrying); - SmartDashboard.putBoolean("Spindexer/Should Stop/is Turret Wrapping?", isTurretWrapping); - SmartDashboard.putBoolean("Spindexer/Should Stop/is Outside Alliance zone?", isOutsideAllianceZone); - SmartDashboard.putBoolean("Spindexer/Should Stop/is Under Trenche?", isUnderTrench); - SmartDashboard.putBoolean("Spindexer/Should Stop/turret lagging sotm", turretLaggingSOTM); - SmartDashboard.putBoolean("Spindexer/Should Stop/inManualState", inManualState); + DogLog.log("Spindexer/Should Stop/Is Behind Hub While Ferrying?", isBehindHubWhileFerrying); + DogLog.log("Spindexer/Should Stop/Is Turret Wrapping?", isTurretWrapping); + DogLog.log("Spindexer/Should Stop/Is Outside Alliance Zone?", isOutsideAllianceZone); + DogLog.log("Spindexer/Should Stop/Is Under Trench?", isUnderTrench); + DogLog.log("Spindexer/Should Stop/Turret Lagging SOTM", turretLaggingSOTM); + DogLog.log("Spindexer/Should Stop/In Manual State", inManualState); - return isSpindexerStopState || - isHandOffStopState || - isTurretWrapping || - (isBehindHubWhileFerrying && !inManualState) || - turretLaggingSOTM || - (isOutsideAllianceZone && !inManualState) || - (isUnderTrench && !inManualState) || - isBehindTower; + boolean shouldStop = isSpindexerStopState || + isHandOffStopState || + isTurretWrapping || + (isBehindHubWhileFerrying && !inManualState) || + turretLaggingSOTM || + (isOutsideAllianceZone && !inManualState) || + (isUnderTrench && !inManualState) || + isBehindTower; + + this.shouldStop = Optional.of(shouldStop); + + return shouldStop; + } + + public void clearMemoized() { + this.shouldStop = Optional.empty(); + } + + public boolean isHopperEmpty() { + return !shooter.isShooting(); } public void periodicAfterScheduler() { @@ -224,15 +246,18 @@ else if (getState() == SuperstructureState.FOTM && shouldStop() && DriverStation Handoff.getInstance().setState(HandoffState.STOP); } - SmartDashboard.putString("Superstructure/State", state.name()); + DogLog.log("Superstructure/State", state.name()); + + DogLog.log("Superstructure/SOTM Stopped Timer", sotmStoppedTimer.get()); + DogLog.log("Superstructure/FOTM Stopped Timer", fotmStoppedTimer.get()); - SmartDashboard.putNumber("Superstructure/SOTM Stopped Timer", sotmStoppedTimer.get()); - SmartDashboard.putNumber("Superstructure/FOTM Stopped Timer", fotmStoppedTimer.get()); + DogLog.log("Superstructure/Shooter At Tolerance?", isShooterAtTolerance()); + DogLog.log("Superstructure/Hood At Tolerance?", isHoodAtTolerance()); + DogLog.log("Superstructure/Turret At Tolerance?", isTurretAtTolerance()); - SmartDashboard.putBoolean("Superstructure/Shooter At Tolerance?", isShooterAtTolerance()); - SmartDashboard.putBoolean("Superstructure/Hood At Tolerance?", isHoodAtTolerance()); - SmartDashboard.putBoolean("Superstructure/Turret At Tolerance?", isTurretAtTolerance()); + DogLog.log("Superstructure/Is Hopper Empty?", isHopperEmpty()); + DogLog.log("Superstructure/Is Ready To Shoot?", isReadyToShoot()); - SmartDashboard.putBoolean("Superstructure/Should Stop?", shouldStop()); + DogLog.log("Superstructure/Should Stop?", shouldStop()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java index 4f15f808..30dc518f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java @@ -5,8 +5,6 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.superstructure.hood; -import com.stuypulse.stuylib.input.Gamepad; - import com.stuypulse.robot.Robot; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Settings; @@ -14,9 +12,12 @@ import com.stuypulse.robot.util.superstructure.InterpolationCalculator; import com.stuypulse.robot.util.superstructure.SOTMCalculator; import com.stuypulse.robot.util.superstructure.VisualizerHood; +import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; +import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -24,6 +25,7 @@ public abstract class Hood extends SubsystemBase{ private static final Hood instance; private HoodState state; + private BStream readyToShoot; private Rotation2d driverInput; @@ -57,6 +59,8 @@ public enum HoodState { public Hood() { state = HoodState.STOW; + readyToShoot = BStream.create(this::atTolerance) + .filtered(new BDebounce.Both(0.05)); } public HoodState getState(){ @@ -74,12 +78,12 @@ public Rotation2d getTargetAngle() { return switch(state) { case STOW -> Settings.Superstructure.Hood.Angles.STOW; - case FERRY -> Settings.Superstructure.Hood.Angles.FERRY_ANGLE; + case FERRY -> InterpolationCalculator.getInterpolatedFerryAngle(); case MANUAL_OVERRIDE -> Rotation2d.fromDegrees(Settings.Superstructure.Hood.Angles.MANUAL_OVERRIDE.get()); case KB -> Settings.Superstructure.Hood.Angles.KB; case LEFT_CORNER -> Settings.Superstructure.Hood.Angles.LEFT_CORNER; case RIGHT_CORNER -> Settings.Superstructure.Hood.Angles.RIGHT_CORNER; - case INTERPOLATION -> InterpolationCalculator.interpolateShotInfo().targetHoodAngle(); + case INTERPOLATION -> InterpolationCalculator.getInterpolatedShotAngle(); case HOMING_UPPER -> new Rotation2d(); //should just apply a voltage, not an angle! case HOMING_LOWER -> new Rotation2d(); case SOTM -> SOTMCalculator.calculateHoodAngleSOTM(); @@ -102,6 +106,10 @@ public boolean atTolerance() { } } + public boolean hoodReadyToShoot() { + return readyToShoot.get(); + } + public abstract Rotation2d getAngle(); public void hoodAnalogToInput(Gamepad gamepad) { @@ -130,10 +138,10 @@ public Rotation2d hoodAnalogToOutput() { public void periodicAfterScheduler() { - SmartDashboard.putString("Superstructure/Hood/State", state.name()); + DogLog.log("Superstructure/Hood/State", state.name()); - SmartDashboard.putNumber("Superstructure/Hood/Target Angle (deg)", getTargetAngle().getDegrees()); - SmartDashboard.putNumber("Superstructure/Hood/Current Angle (deg)", getAngle().getDegrees()); + DogLog.log("Superstructure/Hood/Target Angle (deg)", getTargetAngle().getDegrees()); + DogLog.log("Superstructure/Hood/Current Angle (deg)", getAngle().getDegrees()); if (Settings.DEBUG_MODE.get()) { if (EnabledSubsystems.HOOD.get()) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java index 0950ddef..21fc5de5 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java @@ -26,11 +26,11 @@ import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; +import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; public class HoodImpl extends Hood { @@ -156,13 +156,13 @@ public void periodicAfterScheduler() { if (isStalling() && state == HoodState.HOMING_UPPER) { seedHoodAtUpperHardStop(); setState(HoodState.STOW); - SmartDashboard.putBoolean("Superstructure/Hood/SUCCESFULLY HOMED UPPER", true); + DogLog.log("Superstructure/Hood/SUCCESFULLY HOMED UPPER", true); } if (isStalling() && state == HoodState.HOMING_LOWER) { seedHoodAtLowerHardStop(); setState(HoodState.STOW); - SmartDashboard.putBoolean("Superstructure/Hood/SUCCESFULLY HOMED LOWER", true); + DogLog.log("Superstructure/Hood/SUCCESFULLY HOMED LOWER", true); } if (EnabledSubsystems.HOOD.get()) { @@ -181,21 +181,21 @@ public void periodicAfterScheduler() { // SmartDashboard.putBoolean("Superstructure/Hood/Has Used Absolute Encoder", hasUsedAbsoluteEncoder); - SmartDashboard.putBoolean("Prematch Checks/Hood at Bottom?", getAngle().getDegrees() < Settings.Superstructure.Hood.REVERSE_SOFT_LIMIT.getDegrees()); - SmartDashboard.putNumber("Superstructure/Hood/Correct Hood Angle (deg)", getAbsoluteHoodAngleDeg()); - SmartDashboard.putNumber("Superstructure/Hood/Closed Loop Error (deg)", hoodMotorClosedLoopError.getValueAsDouble() * 360.0); - SmartDashboard.putNumber("Superstructure/Hood/Implemented Error (Degrees)", getTargetAngle().getDegrees() - getAngle().getDegrees()); + DogLog.log("Prematch Checks/Hood at Bottom?", getAngle().getDegrees() < Settings.Superstructure.Hood.REVERSE_SOFT_LIMIT.getDegrees()); + DogLog.log("Superstructure/Hood/Correct Hood Angle (deg)", getAbsoluteHoodAngleDeg()); + DogLog.log("Superstructure/Hood/Closed Loop Error (deg)", hoodMotorClosedLoopError.getValueAsDouble() * 360.0); + DogLog.log("Superstructure/Hood/Implemented Error (Degrees)", getTargetAngle().getDegrees() - getAngle().getDegrees()); if (Settings.DEBUG_MODE.get()) { - SmartDashboard.putNumber("Superstructure/Hood/Applied Voltage (amps)", hoodMotorVoltage.getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Hood/Supply Current (amps)", hoodMotorSupplyCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Hood/Stator Current (amps)", hoodMotorStatorCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Hood/Raw Motor Encoder Value",hoodMotorStatorCurrent.getValueAsDouble()); - SmartDashboard.putBoolean("Superstructure/Hood/is stalling", isStalling()); + DogLog.log("Superstructure/Hood/Applied Voltage (amps)", hoodMotorVoltage.getValueAsDouble()); + DogLog.log("Superstructure/Hood/Supply Current (amps)", hoodMotorSupplyCurrent.getValueAsDouble()); + DogLog.log("Superstructure/Hood/Stator Current (amps)", hoodMotorStatorCurrent.getValueAsDouble()); + DogLog.log("Superstructure/Hood/Raw Motor Encoder Value",hoodMotorStatorCurrent.getValueAsDouble()); + DogLog.log("Superstructure/Hood/is stalling", isStalling()); Robot.getEnergyUtil().logEnergyUsage(getName(), getCurrentDraw()); if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { - SmartDashboard.putBoolean("Robot/CAN/Rio/Hood Motor Connected? (ID " + String.valueOf(Ports.Superstructure.Hood.MOTOR) + ")", hoodMotor.isConnected()); + DogLog.log("Robot/CAN/Rio/Hood Motor Connected? (ID " + String.valueOf(Ports.Superstructure.Hood.MOTOR) + ")", hoodMotor.isConnected()); // SmartDashboard.putBoolean("Robot/CAN/Rio/Hood Encoder Connected? (ID " + String.valueOf(hoodEncoder.getDeviceID()) + ")", hoodEncoder.isConnected()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java index dd459edc..f982e6a0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java @@ -5,19 +5,19 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.superstructure.hood; +import java.util.Optional; + import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.util.SysId; import com.stuypulse.robot.util.superstructure.VisualizerHood; +import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.simulation.ElevatorSim; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import java.util.Optional; - public class HoodSim extends Hood { // Simulates hood motion as a linear elevator, then converts to angle @@ -112,7 +112,7 @@ public void periodic() { VisualizerHood.getInstance().update(getAngle(), atTolerance()); if (Settings.DEBUG_MODE.get()) { - SmartDashboard.putNumber("Superstructure/Hood/Sim Height (m)", sim.getPositionMeters()); + DogLog.log("Superstructure/Hood/Sim Height (m)", sim.getPositionMeters()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java index fedb0915..7d359959 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java @@ -7,16 +7,21 @@ import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.util.superstructure.InterpolationCalculator; import com.stuypulse.robot.util.superstructure.SOTMCalculator; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import dev.doglog.DogLog; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; public abstract class Shooter extends SubsystemBase { private static final Shooter instance; + private BStream readyToShoot; + private ShooterState state; static { @@ -46,6 +51,9 @@ public enum ShooterState { public Shooter() { state = ShooterState.MANUAL_OVERRIDE; + + readyToShoot = BStream.create(this::atTolerance) + .filtered(new BDebounce.Both(0.05)); } public void setState(ShooterState state) { @@ -64,12 +72,12 @@ public double getTargetRPM() { return switch(state) { case STOP -> 0; case MANUAL_OVERRIDE -> getShootRPM(); - case FERRY -> InterpolationCalculator.interpolateFerryingInfo().targetRPM(); + case FERRY -> InterpolationCalculator.getInterpolatedFerryRPM(); case REVERSE -> Settings.Superstructure.Shooter.RPM.REVERSE; case KB -> Settings.Superstructure.Shooter.RPM.KB; case LEFT_CORNER -> Settings.Superstructure.Shooter.RPM.LEFT_CORNER; case RIGHT_CORNER -> Settings.Superstructure.Shooter.RPM.RIGHT_CORNER; - case INTERPOLATION -> InterpolationCalculator.interpolateShotInfo().targetRPM(); + case INTERPOLATION -> InterpolationCalculator.getInterpolatedShotRPM(); case SOTM -> SOTMCalculator.calculateShooterRPMSOTM(); case FOTM -> SOTMCalculator.calculateShooterRPMFOTM(); }; @@ -97,16 +105,21 @@ public boolean atTolerance() { return error > -toleranceLow && error < toleranceHigh; } + public boolean shooterReadyToShoot() { + return readyToShoot.get(); + } + public abstract double getRPM(); public abstract SysIdRoutine getShooterSysIdRoutine(); public abstract double getCurrentDraw(); - public void periodicAfterScheduler() { - SmartDashboard.putString("Superstructure/Shooter/State", state.name()); + public abstract boolean isShooting(); - SmartDashboard.putNumber("Superstructure/Shooter/Current RPM (Leader)", getRPM()); - SmartDashboard.putNumber("Superstructure/Shooter/Target RPM", getTargetRPM()); + public void periodicAfterScheduler() { + DogLog.log("Superstructure/Shooter/State", state.name()); + DogLog.log("Superstructure/Shooter/Current RPM (Leader)", getRPM()); + DogLog.log("Superstructure/Shooter/Target RPM", getTargetRPM()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java index e1f454f7..30bdae5b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java @@ -23,11 +23,14 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.util.PhoenixUtil; import com.stuypulse.robot.util.SysId; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; +import dev.doglog.DogLog; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; public class ShooterImpl extends Shooter { @@ -49,6 +52,10 @@ public class ShooterImpl extends Shooter { private StatusSignal shooterLeaderVoltage; private StatusSignal shooterFollowerVoltage; private StatusSignal shooterLeaderClosedLoopError; + private StatusSignal shooterLeaderTemperature; + private StatusSignal shooterFollowerTemperature; + + private final BStream currentlyShooting; public ShooterImpl() { shooterConfig = new Motors.TalonFXConfig() @@ -99,12 +106,21 @@ public ShooterImpl() { shooterLeaderVoltage = shooterLeader.getMotorVoltage(); shooterFollowerVoltage = shooterLeader.getMotorVoltage(); shooterLeaderClosedLoopError = shooterLeader.getClosedLoopError(); + + shooterLeaderTemperature = shooterLeader.getDeviceTemp(); + shooterFollowerTemperature = shooterFollower.getDeviceTemp(); + + currentlyShooting = BStream.create(() -> (shooterLeadStatorCurrent.getValueAsDouble() > Settings.Superstructure.Shooter.IS_SHOOTING_CURRENT)) + .filtered(new BDebounce.Falling(1.0)); + PhoenixUtil.registerToRio(shooterLeaderSpeed, shooterFollowerSpeed, shooterFollowSupplyCurrent, shooterFollowStatorCurrent, shooterLeadSupplyCurrent, shooterLeadStatorCurrent, - shooterLeaderVoltage, shooterFollowerVoltage, shooterLeaderClosedLoopError); + shooterLeaderVoltage, shooterFollowerVoltage, shooterLeaderClosedLoopError, + shooterLeaderTemperature, shooterFollowerTemperature); voltageOverride = Optional.empty(); } + //TODO: make all this RPM stuff one method. Should use a paramaeter to identify the RPM to get - like the motor @Override public double getRPM() { return getLeaderRPM(); @@ -137,33 +153,41 @@ public void periodicAfterScheduler() { shooterLeader.stopMotor(); } - SmartDashboard.putNumber("Superstructure/Shooter/Leader RPM", getLeaderRPM()); - SmartDashboard.putNumber("Superstructure/Shooter/Follower RPM", getFollowerRPM()); + DogLog.log("Superstructure/Shooter/Leader RPM", getLeaderRPM()); + DogLog.log("Superstructure/Shooter/Follower RPM", getFollowerRPM()); if (Settings.DEBUG_MODE.get()) { - SmartDashboard.putNumber("InterpolationTesting/Shooter Applied Voltage", + DogLog.log("InterpolationTesting/Shooter Applied Voltage", shooterLeaderVoltage.getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Shooter/Leader Voltage (volts)", + DogLog.log("Superstructure/Shooter/Leader Voltage (volts)", shooterLeaderVoltage.getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Shooter/Leader Supply Current (amps)", + DogLog.log("Superstructure/Shooter/Leader Supply Current (amps)", shooterLeadSupplyCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Shooter/Leader Stator Current (amps)", + DogLog.log("Superstructure/Shooter/Leader Stator Current (amps)", shooterLeadStatorCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Shooter/Follower Voltage (volts)", + DogLog.log("Superstructure/Shooter/Leader Motor Temp (C)", + shooterLeaderTemperature.getValueAsDouble()); + + DogLog.log("Superstructure/Shooter/Follower Voltage (volts)", shooterFollowerVoltage.getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Shooter/Follower Supply Current (amps)", + DogLog.log("Superstructure/Shooter/Follower Supply Current (amps)", shooterFollowSupplyCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Shooter/Follower Stator Current (amps)", + DogLog.log("Superstructure/Shooter/Follower Stator Current (amps)", shooterFollowStatorCurrent.getValueAsDouble()); + + DogLog.log("Superstructure/Shooter/Follower Motor Temp (C)", + shooterFollowerTemperature.getValueAsDouble()); + + if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { - SmartDashboard.putBoolean( + DogLog.log( "Robot/CAN/Main/Shooter Leader Motor Connected? (ID " + String.valueOf(Ports.Superstructure.Shooter.MOTOR_LEAD) + ")", shooterLeader.isConnected()); - SmartDashboard.putBoolean( + DogLog.log( "Robot/CAN/Main/Shooter Follower Motor Connected? (ID " + String.valueOf(Ports.Superstructure.Shooter.MOTOR_FOLLOW) + ")", shooterFollower.isConnected()); @@ -171,10 +195,10 @@ public void periodicAfterScheduler() { Robot.getEnergyUtil().logEnergyUsage(getName(), getCurrentDraw()); } - SmartDashboard.putNumber("InterpolationTesting/Shooter Closed Loop Error (RPM)", + DogLog.log("InterpolationTesting/Shooter Closed Loop Error (RPM)", shooterLeaderClosedLoopError.getValueAsDouble() * 60.0); - SmartDashboard.putNumber("Superstructure/Shooter/Implemented Error (RPM)", getTargetRPM() - getLeaderRPM()); + DogLog.log("Superstructure/Shooter/Implemented Error (RPM)", getTargetRPM() - getLeaderRPM()); } private void setVoltageOverride(Optional voltageOverride) { @@ -199,4 +223,9 @@ public double getCurrentDraw() { return Double.max(0, shooterLeadSupplyCurrent.getValueAsDouble()) + Double.max(0, shooterFollowSupplyCurrent.getValueAsDouble()); } + + @Override + public boolean isShooting() { + return currentlyShooting.get(); + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java index 57742fff..61a2afe1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java @@ -5,10 +5,13 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.superstructure.shooter; +import java.util.Optional; + import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.util.SysId; +import dev.doglog.DogLog; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.LinearQuadraticRegulator; @@ -19,11 +22,8 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.simulation.LinearSystemSim; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import java.util.Optional; - public class ShooterSim extends Shooter { private LinearSystemSim sim; @@ -75,14 +75,14 @@ public void periodic() { if (EnabledSubsystems.SHOOTER.get()) { if (voltageOverride.isPresent()) { sim.setInput(voltageOverride.get()); - SmartDashboard.putNumber("Superstructure/Shooter/Input Voltage", voltageOverride.get()); + DogLog.log("Superstructure/Shooter/Input Voltage", voltageOverride.get()); } else { - SmartDashboard.putNumber("Superstructure/Shooter/Input Voltage", controller.getU(0)); + DogLog.log("Superstructure/Shooter/Input Voltage", controller.getU(0)); sim.setInput(controller.getU(0)); } } else { sim.setInput(0); - SmartDashboard.putNumber("Superstructure/Shooter/Input Voltage", 0.0); + DogLog.log("Superstructure/Shooter/Input Voltage", 0.0); } sim.update(Settings.DT); @@ -105,4 +105,9 @@ public SysIdRoutine getShooterSysIdRoutine() { public double getCurrentDraw() { return 0; } + + @Override + public boolean isShooting() { + return true; + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java index d1458669..b9866e4d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java @@ -5,9 +5,6 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.superstructure.turret; -import com.stuypulse.stuylib.input.Gamepad; -import com.stuypulse.stuylib.math.Vector2D; - import com.stuypulse.robot.Robot; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Field; @@ -16,11 +13,15 @@ import com.stuypulse.robot.util.superstructure.SOTMCalculator; import com.stuypulse.robot.util.superstructure.TurretAngleCalculator; import com.stuypulse.robot.util.superstructure.VisualizerTurret; +import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.stuylib.math.Vector2D; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; +import dev.doglog.DogLog; 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.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -29,6 +30,8 @@ public abstract class Turret extends SubsystemBase { private TurretState state; private Vector2D driverInput; + private final BStream readyToShoot; + static { instance = Robot.isReal() ? new TurretImpl() : new TurretSim(); } @@ -40,6 +43,8 @@ public static Turret getInstance() { public Turret() { driverInput = Vector2D.kOrigin; state = TurretState.SCORE; + readyToShoot = BStream.create(this::atTolerance) + .filtered(new BDebounce.Both(0.05)); } public void setDriverInput(Gamepad gamepad) { @@ -75,15 +80,20 @@ public Rotation2d getTargetAngle() { } public Rotation2d driverInputToAngle() { - SmartDashboard.putNumber("Superstructure/Turret/Driver Input", driverInput.x); + DogLog.log("Superstructure/Turret/Driver Input", driverInput.x); return Rotation2d.fromDegrees(driverInput.x * 180); } public boolean atTolerance() { double error = getAngle().minus(getTargetAngle()).getRotations(); + CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); double tolerance = switch (state) { - case SOTM -> Settings.Superstructure.Turret.SOTM_TOLERANCE.get() / 360.0; + case SOTM -> + swerve.getTurretPose().getTranslation().getDistance(Field.HUB_CENTER.getTranslation()) > + Settings.Superstructure.Turret.SOTM_TOLERANCE_THRESHOLD_METERS.get() ? + Settings.Superstructure.Turret.SOTM_TOLERANCE_CLOSE.get() / 360.0 : + Settings.Superstructure.Turret.SOTM_TOLERANCE_FAR.get() / 360.0; case FOTM -> Settings.Superstructure.Turret.FOTM_TOLERANCE.getRotations(); default -> Settings.Superstructure.Turret.TOLERANCE.getRotations(); }; @@ -91,6 +101,10 @@ public boolean atTolerance() { return Math.abs(error) < tolerance; } + public boolean turretReadyToShoot() { + return readyToShoot.get(); + } + public Rotation2d getScoringAngle() { CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); @@ -129,10 +143,10 @@ public TurretState getState() { } public void periodicAfterScheduler() { - SmartDashboard.putString("Superstructure/Turret/State", state.name()); + DogLog.log("Superstructure/Turret/State", state.name()); - SmartDashboard.putNumber("Superstructure/Turret/Target Angle", getTargetAngle().getDegrees()); - SmartDashboard.putNumber("Superstructure/Turret/Current Angle", getAngle().getDegrees()); + DogLog.log("Superstructure/Turret/Target Angle", getTargetAngle().getDegrees()); + DogLog.log("Superstructure/Turret/Current Angle", getAngle().getDegrees()); if (Settings.DEBUG_MODE.get()) { if (EnabledSubsystems.TURRET.get()) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java index fbbb3d88..05119635 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -29,11 +29,11 @@ import com.stuypulse.robot.util.SysId; import com.stuypulse.robot.util.superstructure.TurretAngleCalculator; +import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; public class TurretImpl extends Turret { @@ -69,7 +69,7 @@ public TurretImpl() { .withSupplyCurrentLimitAmps(80) .withStatorCurrentLimitEnabled(false) - .withRampRate(0.25) + .withRampRate(0.0) .withPIDConstants(Gains.Superstructure.Turret.slot0.kP, Gains.Superstructure.Turret.slot0.kI, Gains.Superstructure.Turret.slot0.kD, 0) @@ -207,14 +207,14 @@ public double getWrappedTargetAngle() { public void periodicAfterScheduler() { super.periodicAfterScheduler(); - // turretConfig.updateGainsConfig( - // turretMotor, 1, - // Gains.Superstructure.Turret.slot1.kP, - // Gains.Superstructure.Turret.slot1.kI, - // Gains.Superstructure.Turret.slot1.kD, - // Gains.Superstructure.Turret.slot1.kS, - // Gains.Superstructure.Turret.slot1.kV, - // Gains.Superstructure.Turret.slot1.kA); + turretConfig.updateGainsConfig( + turretMotor, 1, + Gains.Superstructure.Turret.slot1.kP, + Gains.Superstructure.Turret.slot1.kI, + Gains.Superstructure.Turret.slot1.kD, + Gains.Superstructure.Turret.slot1.kS, + Gains.Superstructure.Turret.slot1.kV, + Gains.Superstructure.Turret.slot1.kA); if (!hasUsedAbsoluteEncoder) { seedTurret(); @@ -241,8 +241,14 @@ public void periodicAfterScheduler() { prevActualTargetAngle = actualTargetAngle; } - isWrapping = Math.abs(getWrappedTargetAngle() - - currentAngle) > Settings.Superstructure.Turret.GAIN_SWITCHING_THRESHOLD.getDegrees(); + if(isWrapping) { + isWrapping = Math.abs(getWrappedTargetAngle() + - currentAngle) > Settings.Superstructure.Turret.GAIN_SWITCHING_THRESHOLD_END.getDegrees(); + } else { + isWrapping = Math.abs(getWrappedTargetAngle() + - currentAngle) > Settings.Superstructure.Turret.GAIN_SWITCHING_THRESHOLD_START.getDegrees(); + } + int slot = 0; if (isWrapping) { @@ -275,34 +281,34 @@ public void periodicAfterScheduler() { turretMotor.stopMotor(); } - SmartDashboard.putNumber("Superstructure/Turret/Relative Encoder Position (deg)", + DogLog.log("Superstructure/Turret/Relative Encoder Position (deg)", turretMotorPos.getValueAsDouble() * 360.0); - SmartDashboard.putNumber("Superstructure/Turret/Closed Loop Error (deg)", + DogLog.log("Superstructure/Turret/Closed Loop Error (deg)", turretMotorClosedLoopError.getValueAsDouble() * 360.0); - SmartDashboard.putNumber("Superstructure/Turret/Encoder18t Abs Position (Rot)", + DogLog.log("Superstructure/Turret/Encoder18t Abs Position (Rot)", encoder18tPos.getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Turret/Encoder17t Abs Position (Rot)", + DogLog.log("Superstructure/Turret/Encoder17t Abs Position (Rot)", encoder17tPos.getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Turret/Voltage (volts)", + DogLog.log("Superstructure/Turret/Voltage (volts)", turretMotorVoltage.getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Turret/Wrapped Target Angle (deg)", prevActualTargetAngle); + DogLog.log("Superstructure/Turret/Wrapped Target Angle (deg)", prevActualTargetAngle); if (Settings.DEBUG_MODE.get()) { - SmartDashboard.putNumber("Superstructure/Turret/Stator Current (amps)", + DogLog.log("Superstructure/Turret/Stator Current (amps)", turretMotorStatorCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Turret/Supply Current (amps)", + DogLog.log("Superstructure/Turret/Supply Current (amps)", turretMotorSupplyCurrent.getValueAsDouble()); if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { - SmartDashboard.putBoolean( + DogLog.log( "Robot/CAN/Main/Turret Motor Connected? (ID " + String.valueOf(Ports.Superstructure.Turret.MOTOR) + ")", turretMotor.isConnected()); - SmartDashboard.putBoolean("Robot/CAN/Main/Turret 17t Encoder Connected? (ID " + DogLog.log("Robot/CAN/Main/Turret 17t Encoder Connected? (ID " + String.valueOf(Ports.Superstructure.Turret.ENCODER17T) + ")", encoder17t.isConnected()); - SmartDashboard.putBoolean("Robot/CAN/Main/Turret 18t Encoder Connected? (ID " + DogLog.log("Robot/CAN/Main/Turret 18t Encoder Connected? (ID " + String.valueOf(Ports.Superstructure.Turret.ENCODER18T) + ")", encoder18t.isConnected()); } Robot.getEnergyUtil().logEnergyUsage(getName(), getCurrentDraw()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java index e902fac9..f9e882c3 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java @@ -5,12 +5,15 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.superstructure.turret; +import java.util.Optional; + import com.stuypulse.robot.RobotContainer; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.DriverConstants; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.util.SysId; +import dev.doglog.DogLog; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.LinearQuadraticRegulator; @@ -25,11 +28,8 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.simulation.LinearSystemSim; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import java.util.Optional; - public class TurretSim extends Turret { private LinearSystemSim sim; @@ -156,20 +156,20 @@ public void periodic() { prevActualTargetAngle = actualTargetAngle; } isWrapping = Math.abs(actualTargetAngle - currentAngle) > - Settings.Superstructure.Turret.GAIN_SWITCHING_THRESHOLD.getDegrees(); + Settings.Superstructure.Turret.GAIN_SWITCHING_THRESHOLD_START.getDegrees(); goal = new TrapezoidProfile.State(Units.degreesToRadians(actualTargetAngle), 0.0); setpoint = profile.calculate(Settings.DT, setpoint, goal); - SmartDashboard.putNumber("Superstructure/Turret/Constraints/Max Vel (deg per s)", Units.radiansToDegrees(maxAngularVelRadiansPerSecond)); - SmartDashboard.putNumber("Superstructure/Turret/Constraints/Max Accel (deg per s per s)", Units.radiansToDegrees(maxAngularAccelRadiansPerSecondSquared)); + DogLog.log("Superstructure/Turret/Constraints/Max Vel (deg per s)", Units.radiansToDegrees(maxAngularVelRadiansPerSecond)); + DogLog.log("Superstructure/Turret/Constraints/Max Accel (deg per s per s)", Units.radiansToDegrees(maxAngularAccelRadiansPerSecondSquared)); - SmartDashboard.putNumber("Superstructure/Turret/Motion Profile Setpoint (deg)", Units.radiansToDegrees(setpoint.position)); - SmartDashboard.putNumber("Superstructure/Turret/Error: abs(turret - target) (deg)", Math.abs(getAngle().minus(getTargetAngle()).getDegrees())); - SmartDashboard.putNumber("Superstructure/Turret/Current Angle (deg)", Units.radiansToDegrees(sim.getOutput(0))); - SmartDashboard.putNumber("Superstructure/Turret/Wrapped Target Angle (deg)", actualTargetAngle); - SmartDashboard.putNumber("Superstructure/Turret/Setpoint Filtered Angle (deg)", prevActualTargetAngle); - SmartDashboard.putBoolean("Superstructure/Turret/Is Wrapping", isWrapping); + DogLog.log("Superstructure/Turret/Motion Profile Setpoint (deg)", Units.radiansToDegrees(setpoint.position)); + DogLog.log("Superstructure/Turret/Error: abs(turret - target) (deg)", Math.abs(getAngle().minus(getTargetAngle()).getDegrees())); + DogLog.log("Superstructure/Turret/Current Angle (deg)", Units.radiansToDegrees(sim.getOutput(0))); + DogLog.log("Superstructure/Turret/Wrapped Target Angle (deg)", actualTargetAngle); + DogLog.log("Superstructure/Turret/Setpoint Filtered Angle (deg)", prevActualTargetAngle); + DogLog.log("Superstructure/Turret/Is Wrapping", isWrapping); controller.setNextR(VecBuilder.fill(setpoint.position, 0.0)); controller.correct(VecBuilder.fill(sim.getOutput(0), sim.getOutput(1))); diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java index e0c31827..1368e99a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -1,10 +1,15 @@ -/************************ PROJECT TRIBECBOT *************************/ +/** ********************** PROJECT TRIBECBOT ************************ */ /* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ + /* Use of this source code is governed by an MIT-style license */ + /* that can be found in the repository LICENSE file. */ +/** ************************************************************ */ package com.stuypulse.robot.subsystems.swerve; +import static edu.wpi.first.units.Units.Second; +import static edu.wpi.first.units.Units.Volts; + +import java.util.Optional; + import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.Utils; @@ -37,27 +42,21 @@ import com.stuypulse.stuylib.math.Angle; import com.stuypulse.stuylib.math.Vector2D; +import dev.doglog.DogLog; import edu.wpi.first.math.Matrix; 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.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.StructPublisher; -import edu.wpi.first.units.measure.AngularAcceleration; +import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.LinearAcceleration; - -import static edu.wpi.first.units.Units.Second; -import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -67,753 +66,768 @@ * Subsystem so it can easily be used in command-based projects. */ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem { - private final static CommandSwerveDrivetrain instance; - - private FieldObject2d turret2d = Field.FIELD2D.getObject("Turret 2D"); - private Pose2d turretPose = new Pose2d(); - private StructPublisher leftBehindHubYPlublisher; - private StructPublisher rightBehindHubYPlublisher; - private StructPublisher vertexBehindHubPublisher; - private StatusSignal robotAccelerationX; - private StatusSignal robotAccelerationY; - private boolean isBehindHub = false; - - private StructPublisher robotPose = NetworkTableInstance.getDefault() - .getStructTopic("Robot Pose", Pose2d.struct).publish(); - - static { - instance = TunerConstants.createDrivetrain(); - // instance.registerTelemetry(instance::telemeterize); - } - - // public void telemeterize(SwerveDriveState state) { - // /* Write drive state to the log file */ - // SignalLogger.writeStruct("DriveState/Pose", Pose2d.struct, state.Pose); - // SignalLogger.writeStruct("DriveState/Speeds", ChassisSpeeds.struct, state.Speeds); - // SignalLogger.writeStructArray("DriveState/ModuleStates", SwerveModuleState.struct, state.ModuleStates); - // SignalLogger.writeStructArray("DriveState/ModuleTargets", SwerveModuleState.struct, state.ModuleTargets); - // SignalLogger.writeStructArray("DriveState/ModulePositions", SwerveModulePosition.struct, state.ModulePositions); - // SignalLogger.writeStruct("DriveState/RawHeading", Rotation2d.struct, state.RawHeading); - // SignalLogger.writeDouble("DriveState/Timestamp", state.Timestamp, "seconds"); - // SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds"); - // SignalLogger.writeInteger("DriveState/FailedDaqs", state.FailedDaqs); - // } - - public static CommandSwerveDrivetrain getInstance() { - return instance; - } - - private static final double kSimLoopPeriod = 0.005; // 5 ms - private Notifier m_simNotifier = null; - private double m_lastSimTime; - - private Pose2d lastGoodPose = new Pose2d(1.5, 1.5, Rotation2d.kZero); - - /* Swerve requests to apply during SysId characterization */ - private final SwerveRequest.SysIdSwerveTranslation m_moduleTranslationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); - private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); - private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); - - private final SwerveRequest.FieldCentric fieldCentricRequest = new SwerveRequest.FieldCentric() - .withDriveRequestType(DriveRequestType.OpenLoopVoltage) - .withDeadband(Settings.Swerve.MODULE_VELOCITY_DEADBAND_M_PER_S) - .withRotationalDeadband(Settings.Swerve.ROTATIONAL_DEADBAND_RAD_PER_S) - .withDesaturateWheelSpeeds(true); - - private final SwerveRequest.RobotCentric robotCentricRequest = new SwerveRequest.RobotCentric() - .withDriveRequestType(DriveRequestType.OpenLoopVoltage) - .withDeadband(Settings.Swerve.MODULE_VELOCITY_DEADBAND_M_PER_S) - .withRotationalDeadband(Settings.Swerve.ROTATIONAL_DEADBAND_RAD_PER_S) - .withDesaturateWheelSpeeds(true); - - public SwerveRequest.FieldCentric getFieldCentricSwerveRequest() { - return this.fieldCentricRequest; - } - - public SwerveRequest.RobotCentric getRobotCentricSwerveRequest() { - return this.robotCentricRequest; - } - - /* + + private final static CommandSwerveDrivetrain instance; + + private FieldObject2d turret2d = Field.FIELD2D.getObject("Turret 2D"); + private Pose2d turretPose = new Pose2d(); + // private StructPublisher leftBehindHubYPlublisher; + // private StructPublisher rightBehindHubYPlublisher; + // private StructPublisher vertexBehindHubPublisher; + private StatusSignal robotAccelerationX; + private StatusSignal robotAccelerationY; + + private StatusSignal[] driveMotorSupplyCurrents; + private StatusSignal[] steerMotorSupplyCurrents; + + //TODO: might wanna change some of these initialized values like isBehindTower indicates that we are in the alliance zone + private Optional isBehindHub = Optional.empty(); + private Optional isOutsideAllianceZone = Optional.empty(); + private Optional isInOpponentZone = Optional.empty(); + private Optional isUnderTrench = Optional.empty(); + private Optional isBehindTower = Optional.empty(); + + // private StructPublisher robotPose = NetworkTableInstance.getDefault() + // .getStructTopic("Robot Pose", Pose2d.struct).publish(); + + static { + instance = TunerConstants.createDrivetrain(); + // instance.registerTelemetry(instance::telemeterize); + } + + // public void telemeterize(SwerveDriveState state) { + // /* Write drive state to the log file */ + // SignalLogger.writeStruct("DriveState/Pose", Pose2d.struct, state.Pose); + // SignalLogger.writeStruct("DriveState/Speeds", ChassisSpeeds.struct, state.Speeds); + // SignalLogger.writeStructArray("DriveState/ModuleStates", SwerveModuleState.struct, state.ModuleStates); + // SignalLogger.writeStructArray("DriveState/ModuleTargets", SwerveModuleState.struct, state.ModuleTargets); + // SignalLogger.writeStructArray("DriveState/ModulePositions", SwerveModulePosition.struct, state.ModulePositions); + // SignalLogger.writeStruct("DriveState/RawHeading", Rotation2d.struct, state.RawHeading); + // SignalLogger.writeDouble("DriveState/Timestamp", state.Timestamp, "seconds"); + // SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds"); + // SignalLogger.writeInteger("DriveState/FailedDaqs", state.FailedDaqs); + // } + public static CommandSwerveDrivetrain getInstance() { + return instance; + } + + private static final double kSimLoopPeriod = 0.005; // 5 ms + private Notifier m_simNotifier = null; + private double m_lastSimTime; + + private Pose2d lastGoodPose = new Pose2d(1.5, 1.5, Rotation2d.kZero); + + /* Swerve requests to apply during SysId characterization */ + private final SwerveRequest.SysIdSwerveTranslation m_moduleTranslationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); + private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); + private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); + + private final SwerveRequest.FieldCentric fieldCentricRequest = new SwerveRequest.FieldCentric() + .withDriveRequestType(DriveRequestType.OpenLoopVoltage) + .withDeadband(Settings.Swerve.MODULE_VELOCITY_DEADBAND_M_PER_S) + .withRotationalDeadband(Settings.Swerve.ROTATIONAL_DEADBAND_RAD_PER_S) + .withDesaturateWheelSpeeds(true); + + private final SwerveRequest.RobotCentric robotCentricRequest = new SwerveRequest.RobotCentric() + .withDriveRequestType(DriveRequestType.OpenLoopVoltage) + .withDeadband(Settings.Swerve.MODULE_VELOCITY_DEADBAND_M_PER_S) + .withRotationalDeadband(Settings.Swerve.ROTATIONAL_DEADBAND_RAD_PER_S) + .withDesaturateWheelSpeeds(true); + + public SwerveRequest.FieldCentric getFieldCentricSwerveRequest() { + return this.fieldCentricRequest; + } + + public SwerveRequest.RobotCentric getRobotCentricSwerveRequest() { + return this.robotCentricRequest; + } + + /* * SysId routine for characterizing module translation. This is used to find PID * gains for the drive motors. - */ - private final SysIdRoutine m_sysIdRoutineModuleTranslation = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdModuleTranslation_State", state.toString())), - new SysIdRoutine.Mechanism( - output -> setControl(m_moduleTranslationCharacterization.withVolts(output)), - null, - this)); - - /* + */ + private final SysIdRoutine m_sysIdRoutineModuleTranslation = new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdModuleTranslation_State", state.toString())), + new SysIdRoutine.Mechanism( + output -> setControl(m_moduleTranslationCharacterization.withVolts(output)), + null, + this)); + + /* * SysId routine for characterizing chassis translation. This is used to find * PID gains PID to pose. - */ - private final SysIdRoutine m_sysIdRoutineChassisTranslation = new SysIdRoutine( - new SysIdRoutine.Config( - /* This is in meters per second², but SysId only supports "volts per second" */ - Volts.of(0.5).per(Second), - /* This is in meters per second, but SysId only supports "volts" */ - Volts.of(Settings.Swerve.Constraints.MAX_VELOCITY_M_PER_S), - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdChassisTranslation_State", state.toString())), - new SysIdRoutine.Mechanism( - output -> { - /* output is actually meters per second, but SysId only supports "volts" */ - setControl(getRobotCentricSwerveRequest().withVelocityX(output.in(Volts)).withVelocityY(0) - .withRotationalRate(0)); - /* also log the requested output for SysId */ - SignalLogger.writeDouble("Target X Velocity ('voltage')", output.in(Volts)); - SignalLogger.writeDouble("X Position", getPose().getX()); - SignalLogger.writeDouble("X Velocity", - getChassisSpeeds().vxMetersPerSecond * getPose().getRotation().getCos()); - }, - null, - this)); - - /* + */ + private final SysIdRoutine m_sysIdRoutineChassisTranslation = new SysIdRoutine( + new SysIdRoutine.Config( + /* This is in meters per second², but SysId only supports "volts per second" */ + Volts.of(0.5).per(Second), + /* This is in meters per second, but SysId only supports "volts" */ + Volts.of(Settings.Swerve.Constraints.MAX_VELOCITY_M_PER_S), + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdChassisTranslation_State", state.toString())), + new SysIdRoutine.Mechanism( + output -> { + /* output is actually meters per second, but SysId only supports "volts" */ + setControl(getRobotCentricSwerveRequest().withVelocityX(output.in(Volts)).withVelocityY(0) + .withRotationalRate(0)); + /* also log the requested output for SysId */ + SignalLogger.writeDouble("Target X Velocity ('voltage')", output.in(Volts)); + SignalLogger.writeDouble("X Position", getPose().getX()); + SignalLogger.writeDouble("X Velocity", + getChassisSpeeds().vxMetersPerSecond * getPose().getRotation().getCos()); + }, + null, + this)); + + /* * SysId routine for characterizing steer. This is used to find PID gains for * the steer motors. - */ - private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(7), // Use dynamic voltage of 7 V - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdSteer_State", state.toString())), - new SysIdRoutine.Mechanism( - volts -> setControl(m_steerCharacterization.withVolts(volts)), - null, - this)); - - /* + */ + private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(7), // Use dynamic voltage of 7 V + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdSteer_State", state.toString())), + new SysIdRoutine.Mechanism( + volts -> setControl(m_steerCharacterization.withVolts(volts)), + null, + this)); + + /* * SysId routine for characterizing rotation. * This is used to find PID gains for the FieldCentricFacingAngle * HeadingController. * See the documentation of SwerveRequest.SysIdSwerveRotation for info on * importing the log to SysId. - */ - private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine( - new SysIdRoutine.Config( - /* This is in radians per second², but SysId only supports "volts per second" */ - Volts.of(Math.PI / 6).per(Second), - /* This is in radians per second, but SysId only supports "volts" */ - Volts.of(Math.PI), - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdRotation_State", state.toString())), - new SysIdRoutine.Mechanism( - output -> { - /* output is actually radians per second, but SysId only supports "volts" */ - setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); - /* also log the requested output for SysId */ - SignalLogger.writeDouble("Rotational_Target_Rate ('voltage')", output.in(Volts)); - SignalLogger.writeDouble("Rotational Position", getPose().getRotation().getRadians()); - SignalLogger.writeDouble("Rotational_Velocity", getState().Speeds.omegaRadiansPerSecond); - }, - null, - this)); - - /* The SysId routine to test */ - private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineModuleTranslation; - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not - * construct - * the devices themselves. If they need the devices, they can access them - * through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - protected CommandSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - SwerveModuleConstants... modules) { - super(drivetrainConstants, modules); - if (Utils.isSimulation()) { - startSimThread(); - } - - leftBehindHubYPlublisher = NetworkTableInstance.getDefault().getStructTopic("FieldPositions/LeftBehindHubY", Pose2d.struct).publish(); - rightBehindHubYPlublisher = NetworkTableInstance.getDefault().getStructTopic("FieldPositions/RightBehindHubY", Pose2d.struct).publish(); - vertexBehindHubPublisher = NetworkTableInstance.getDefault().getStructTopic("FieldPositions/VertexBehindHub", Pose2d.struct).publish(); - - robotAccelerationX = this.getPigeon2().getAccelerationX(); - robotAccelerationY = this.getPigeon2().getAccelerationY(); - - PhoenixUtil.registerToCanivore( - robotAccelerationX, - robotAccelerationY - ); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

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

- * This constructs the underlying hardware devices, so users should not - * construct - * the devices themselves. If they need the devices, they can access them - * through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve - * drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz - * on - * CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry - * calculation - * in the form [x, y, theta]ᵀ, with units in - * meters - * and radians - * @param visionStandardDeviation The standard deviation for vision - * calculation - * in the form [x, y, theta]ᵀ, with units in - * meters - * and radians - * @param modules Constants for each specific module - */ - private CommandSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules) { - super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, - modules); - if (Utils.isSimulation()) { - startSimThread(); - } - } - - @Override - public void setControl(SwerveRequest request) { - if (EnabledSubsystems.SWERVE.get()) { - super.setControl(request); - } else { - super.setControl(new SwerveRequest.Idle()); - } - } - - /** - * Runs the SysId Quasistatic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Quasistatic test - * @return Command to run - */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.quasistatic(direction); - } - - /** - * Runs the SysId Dynamic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Dynamic test - * @return Command to run - */ - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.dynamic(direction); - } - - public Command sysidRotationDynamic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineRotation.dynamic(direction); - } - - public Command sysidRotationQuasiStatic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineRotation.quasistatic(direction); - } - - private void startSimThread() { - m_lastSimTime = Utils.getCurrentTimeSeconds(); - - /* Run simulation at a faster rate so PID gains behave more reasonably */ - m_simNotifier = new Notifier(() -> { - final double currentTime = Utils.getCurrentTimeSeconds(); - double deltaTime = currentTime - m_lastSimTime; - m_lastSimTime = currentTime; - - /* use the measured time delta, get battery voltage from WPILib */ - updateSimState(deltaTime, RobotController.getBatteryVoltage()); - }); - m_simNotifier.startPeriodic(kSimLoopPeriod); - } - - private boolean checkIfVisionMeasurementValid(Pose2d visionPose) { - return !Settings.ENABLE_DISTANCE_CHECK.get() || visionPose.getTranslation().getDistance(getState().Pose.getTranslation()) < Settings.Swerve.MAX_ACCEPTABLE_VISION_DEVIATION_METERS; - } - - /** - * Adds a vision measurement to the Kalman Filter. This will correct the - * odometry pose estimate - * while still accounting for measurement noise. - * - * @param visionRobotPoseMeters The pose of the robot as measured by the vision - * camera. - * @param timestampSeconds The timestamp of the vision measurement in - * seconds. - */ - @Override - public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { - // SignalLogger.writeStruct("Vision/Pose", Pose2d.struct, visionRobotPoseMeters); - // SignalLogger.writeDouble("Vision/Timestamp", timestampSeconds); - - if(checkIfVisionMeasurementValid(visionRobotPoseMeters)) { - super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds)); - } - } - - /** - * Adds a vision measurement to the Kalman Filter. This will correct the - * odometry pose estimate - * while still accounting for measurement noise. - *

- * Note that the vision measurement standard deviations passed into this method - * will continue to apply to future measurements until a subsequent call to - * {@link #setVisionMeasurementStdDevs(Matrix)} or this method. - * - * @param visionRobotPoseMeters The pose of the robot as measured by the - * vision camera. - * @param timestampSeconds The timestamp of the vision measurement in - * seconds. - * @param visionMeasurementStdDevs Standard deviations of the vision pose - * measurement - * in the form [x, y, theta]ᵀ, with units in - * meters and radians. - */ - @Override - public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix visionMeasurementStdDevs) { - // SignalLogger.writeStruct("Vision/Pose", Pose2d.struct, visionRobotPoseMeters); - // SignalLogger.writeDouble("Vision/Timestamp", timestampSeconds); - - if(checkIfVisionMeasurementValid(visionRobotPoseMeters)) { - super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), - visionMeasurementStdDevs); - } - } - - public Pose2d getPose() { - Pose2d proposedPose = getState().Pose; - double proposedX = proposedPose.getX(); - double proposedY = proposedPose.getY(); - double poseDelta = lastGoodPose.getTranslation().getDistance(proposedPose.getTranslation()); - - // if (!(proposedX > Field.LENGTH || proposedX < 0 || proposedY > Field.WIDTH || proposedY < 0) && - // poseDelta <= Settings.Swerve.MAX_ACCEPTABLE_POSE_DELTA_METERS) { - lastGoodPose = proposedPose; - // } - - return lastGoodPose; - } - - public void configureAutoBuilder() { - try { - AutoBuilder.configure( - this::getPose, - this::resetPose, - this::getChassisSpeeds, - this::setChassisSpeeds, - new PPHolonomicDriveController(Gains.Swerve.Alignment.XY, Gains.Swerve.Alignment.THETA), - RobotConfig.fromGUISettings(), - () -> false, - instance); - PathPlannerLogging.setLogActivePathCallback((poses) -> { - if (Robot.isBlue()) { - Field.FIELD2D.getObject("path").setPoses(poses); - - } else { - Field.FIELD2D.getObject("path").setPoses(Field.transformToOppositeAlliance(poses)); - } - }); - } catch (Exception e) { - e.printStackTrace(); - } - } - - public Command followPathCommand(String pathName) { - try { - return followPathCommand(PathPlannerPath.fromPathFile(pathName)); - } catch (Exception e) { - throw new IllegalArgumentException(pathName + " does not exist"); - } - } - - public Command followPathCommand(PathPlannerPath path) { - return AutoBuilder.followPath(path); - } - - public SwerveModuleState[] getModuleStates() { - return getState().ModuleStates; - } - - public ChassisSpeeds getChassisSpeeds() { - return getState().Speeds; - } - - public Vector2D getFieldRelativeSpeeds() { - return new Vector2D(getChassisSpeeds().vxMetersPerSecond, getChassisSpeeds().vyMetersPerSecond) - .rotate(Angle.fromRotation2d(getPose().getRotation())); - } - - private void setChassisSpeeds(ChassisSpeeds robotSpeeds) { - setControl(new SwerveRequest.RobotCentric() - .withVelocityX(robotSpeeds.vxMetersPerSecond) - .withVelocityY(robotSpeeds.vyMetersPerSecond) - .withRotationalRate(robotSpeeds.omegaRadiansPerSecond)); - } - - public void drive(Vector2D velocity, double rotation) { - ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds( - Robot.isBlue() ? velocity.y : -velocity.y, - Robot.isBlue() ? -velocity.x : velocity.x, - -rotation, - getPose().getRotation()); - - Pose2d robotVel = new Pose2d( - Settings.DT * speeds.vxMetersPerSecond, - Settings.DT * speeds.vyMetersPerSecond, - Rotation2d.fromRadians(Settings.DT * speeds.omegaRadiansPerSecond)); - Twist2d twistVel = new Pose2d().log(robotVel); - - setChassisSpeeds(new ChassisSpeeds( - twistVel.dx / Settings.DT, - twistVel.dy / Settings.DT, - twistVel.dtheta / Settings.DT)); - } - - public Pose2d getTurretPose() { - return turretPose; - } - - public double[] getWheelDrivePositionsRadians() { - double[] positions = new double[4]; - double kDriveGearRatio = 7.67; - for (int i = 0; i < 4; i++) { - positions[i] = getModule(i).getDriveMotor().getPosition().getValueAsDouble() * 2 * Math.PI - / kDriveGearRatio; - } - return positions; - } - - public boolean isUnderTrench() { - Translation2d turretTranslation = getTurretPose().getTranslation(); - - boolean isBetweenRightTrenchesY = Field.AllianceRightTrench.rightEdge.getY() < turretTranslation.getY() - && Field.AllianceRightTrench.leftEdge.getY() > turretTranslation.getY(); - - boolean isBetweenLeftTrenchesY = Field.AllianceLeftTrench.rightEdge.getY() < turretTranslation.getY() - && Field.AllianceLeftTrench.leftEdge.getY() > turretTranslation.getY(); - - boolean isUnderAllianceTrenchX = Math.abs( - turretTranslation.getX() - Field.AllianceRightTrench.rightEdge.getX()) < Field.TRENCH_HOOD_TOLERANCE; - - boolean isUnderOpponentTrenchX = Math.abs( - turretTranslation.getX() - Field.OpponentRightTrench.rightEdge.getX()) < Field.TRENCH_HOOD_TOLERANCE; - - boolean isUnderTrench = (isBetweenRightTrenchesY || isBetweenLeftTrenchesY) - && (isUnderAllianceTrenchX || isUnderOpponentTrenchX); - - return isUnderTrench; - } - - public boolean isInOpponentZone() { - Translation2d turretTranslation = getTurretPose().getTranslation(); - return turretTranslation.getX() > Field.OPPONENT_ZONE_X; - } - - public boolean isBehindTower() { - boolean withinTowerX = getPose().getTranslation().getX() < Field.TOWER_FAR_CENTER.getX(); - boolean withinTowerY = Field.TOWER_FAR_RIGHT.getY() < getTurretPose().getTranslation().getY() - && getTurretPose().getTranslation().getY() < Field.TOWER_FAR_LEFT.getY(); - return withinTowerX && withinTowerY; - } - - /** - * Checks whether the robot turret pose is behind the hub or not in the neutral zone. - * For stopping ferrying to prevent fuel hitting the hub. - * - * @return true if robot turret pose is behind the hub. - */ - public void updateIsBehindHub() { - // === TRIANGLE === (CUSTOM VERTEX) - Translation2d turretTranslation = getTurretPose().getTranslation(); - - boolean behindHubX = Field.HUB_FAR_LEFT_CORNER.getX() < turretTranslation.getX(); - // && turretTranslation.getX() < Field.hubFarLeftCorner.getX() + Field.hubToleranceX; // With this line the triangle will be cut to more like a trapezoid. - - Pose2d hubFarLeftCornerWithTolerance = new Pose2d(Field.HUB_FAR_LEFT_CORNER.getX(), Field.HUB_FAR_LEFT_CORNER.getY() + Field.BEHIND_HUB_TOLERANCE_Y, new Rotation2d()); - Pose2d hubFarRightCornerWithTolerance = new Pose2d(Field.HUB_FAR_RIGHT_CORNER.getX(), Field.HUB_FAR_RIGHT_CORNER.getY() - Field.BEHIND_HUB_TOLERANCE_Y, new Rotation2d()); - - // Find point on triangle using the point-slope formula (of the line constructed by the hub corner pose and ferry pose) - // y = (slope)(robotX - hubCornerX) + (hubCornerY) - // where the slope = (hubCornerY - vertexY)/(hubCornerX - vertexX) - double leftY = ((hubFarLeftCornerWithTolerance.getY() - Field.BEHIND_HUB_TRIANGLE_VERTEX.getY()) - /(hubFarLeftCornerWithTolerance.getX() - Field.BEHIND_HUB_TRIANGLE_VERTEX.getX())) // (Slope) - * (turretTranslation.getX() - hubFarLeftCornerWithTolerance.getX()) + hubFarLeftCornerWithTolerance.getY(); // *(robotX - hubCornerX) + (hubCornerY) - double rightY = ((hubFarRightCornerWithTolerance.getY() - Field.BEHIND_HUB_TRIANGLE_VERTEX.getY()) - /(hubFarRightCornerWithTolerance.getX() - Field.BEHIND_HUB_TRIANGLE_VERTEX.getX())) // (Slope) - * (turretTranslation.getX() - hubFarRightCornerWithTolerance.getX()) + hubFarRightCornerWithTolerance.getY(); // *(robotX - hubCornerX) + (hubCornerY) - - // Debug: - leftBehindHubYPlublisher.set(new Pose2d(getTurretPose().getX(), leftY, new Rotation2d())); - rightBehindHubYPlublisher.set(new Pose2d(getTurretPose().getX(), rightY, new Rotation2d())); - vertexBehindHubPublisher.set(Field.BEHIND_HUB_TRIANGLE_VERTEX); - - boolean withinHubY = rightY < getTurretPose().getY() - && getTurretPose().getY() < leftY; - - isBehindHub = behindHubX && withinHubY; - - // === TRIANGLE === (FROM FERRY ZONES): - // Translation2d turretTranslation = getTurretPose().getTranslation(); - // boolean behindHubX = Field.hubFarLeftCorner.getX() < turretTranslation.getX(); - // // && turretTranslation.getX() < Field.hubFarLeftCorner.getX() + Field.hubToleranceX; // With this line the triangle will be cut to more like a trapezoid. - - // // Find point on triangle using the point-slope formula (of the line constructed by the hub corner pose and ferry pose) - // // y = (slope)(robotX - hubCornerX) + (hubCornerY) - // // where the slope = (hubCornerY - ferryY)/(hubCornerX - ferryX) - // double leftY = ((Field.hubFarLeftCorner.getY() - Field.leftFerryZone.getY())/(Field.hubFarLeftCorner.getX() - Field.leftFerryZone.getX())) // (Slope) - // * (turretTranslation.getX() - Field.hubFarLeftCorner.getX()) + Field.hubFarLeftCorner.getY(); // *(robotX - hubCornerX) + (hubCornerY) - // double rightY = ((Field.hubFarRightCorner.getY() - Field.rightFerryZone.getY())/(Field.hubFarRightCorner.getX() - Field.rightFerryZone.getX())) // (Slope) - // * (turretTranslation.getX() - Field.hubFarRightCorner.getX()) + Field.hubFarRightCorner.getY(); // *(robotX - hubCornerX) + (hubCornerY) - - // leftBehindHubYPlublisher.set(new Pose2d(getTurretPose().getX(), leftY - Field.hubToleranceY, new Rotation2d())); - // rightBehindHubYPlublisher.set(new Pose2d(getTurretPose().getX(), rightY + Field.hubToleranceY, new Rotation2d())); - - // boolean withinHubY = rightY + Field.hubToleranceY < getTurretPose().getY() - // && getTurretPose().getY() < leftY - Field.hubToleranceY; - - // return behindHubX && withinHubY; - - // === RECTANGLE ===: - // Translation2d turretTranslation = getTurretPose().getTranslation(); - // boolean behindHubX = Field.hubFarLeftCorner.getX() < turretTranslation.getX() - // && turretTranslation.getX() < Field.hubFarLeftCorner.getX() + Field.hubToleranceX; - // boolean withinHubY = Field.hubFarRightCorner.getY() + Field.hubToleranceY < getTurretPose().getY() - // && getTurretPose().getY() < Field.hubFarLeftCorner.getY() - Field.hubToleranceY; - - // return behindHubX && withinHubY; - } - - public boolean isOutsideAllianceZone() { - return getPose().getX() > Field.AllianceRightTrench.rightEdge.getX() + Field.TRENCH_HOOD_TOLERANCE; - } - - public boolean getIsBehindHub() { - return isBehindHub; - } - - public void teleopInit() { - for (SwerveModule module : getModules()) { - TalonFXConfiguration newConfigs = new TalonFXConfiguration(); - module.getDriveMotor().getConfigurator().refresh(newConfigs); - newConfigs = newConfigs - .withCurrentLimits(new CurrentLimitsConfigs() - .withStatorCurrentLimit(80) - .withStatorCurrentLimitEnable(true)); - - module.getDriveMotor().getConfigurator().apply(newConfigs); - } - } - - public void autonInit() { - for (SwerveModule module : getModules()) { - TalonFXConfiguration newConfigs = new TalonFXConfiguration(); - module.getDriveMotor().getConfigurator().refresh(newConfigs); - newConfigs = newConfigs - .withCurrentLimits(new CurrentLimitsConfigs() - .withStatorCurrentLimit(120) - .withStatorCurrentLimitEnable(true)); - - module.getDriveMotor().getConfigurator().apply(newConfigs); - } - } - - public double getTotalDriveSupplyCurrent() { - double total = 0.0; - - for (SwerveModule module : getModules()) { - total += Double.max(0, module.getDriveMotor().getSupplyCurrent().getValueAsDouble()); - } - - return total; - } - - public double getTotalSteerSupplyCurrent() { - double total = 0.0; - - for (SwerveModule module : getModules()) { - total += Double.max(0, module.getSteerMotor().getSupplyCurrent().getValueAsDouble()); - } - - return total; - } - - public boolean canShootIntoHub() { - Superstructure superstructure = Superstructure.getInstance(); - SuperstructureState state = superstructure.getState(); - return !isOutsideAllianceZone() || (state == SuperstructureState.KB || state == SuperstructureState.LEFT_CORNER || state == SuperstructureState.RIGHT_CORNER); - } - - public void periodicAfterScheduler() { - Pose2d pose = getPose(); - turretPose = new Pose2d( - pose.getTranslation().plus( - Settings.Superstructure.Turret.TURRET_OFFSET.getTranslation().rotateBy(pose.getRotation())), - pose.getRotation().plus(Turret.getInstance().getAngle())); - - robotPose.set(pose); - - turret2d.setPose(Robot.isBlue() ? turretPose : Field.transformToOppositeAlliance(turretPose)); - - updateIsBehindHub(); - - ChassisSpeeds chassisSpeeds = getChassisSpeeds(); - Vector2D fieldRelativeSpeeds = getFieldRelativeSpeeds(); - SmartDashboard.putNumber("Swerve/Velocity Robot Relative X (m per s)", chassisSpeeds.vxMetersPerSecond); - SmartDashboard.putNumber("Swerve/Velocity Robot Relative Y (m per s)", chassisSpeeds.vyMetersPerSecond); - - SmartDashboard.putNumber("Swerve/Velocity Field Relative X (m per s)", fieldRelativeSpeeds.x); - SmartDashboard.putNumber("Swerve/Field Relative Rotation", pose.getRotation().getDegrees()); - SmartDashboard.putNumber("Swerve/Velocity Field Relative Y (m per s)", getFieldRelativeSpeeds().y); - - SmartDashboard.putNumber("Swerve/Angular Velocity (rad per s)", chassisSpeeds.omegaRadiansPerSecond); - SmartDashboard.putNumber("Swerve/Distance From Hub (meters)", - Field.HUB_CENTER.getTranslation().getDistance(pose.getTranslation())); - - Field.FIELD2D.getRobotObject().setPose(Robot.isBlue() ? pose : Field.transformToOppositeAlliance(pose)); - - if (Robot.getPeriodicCounter() % Settings.LOGGING_FREQUENCY == 0) { - SmartDashboard.putNumber("Swerve/Robot Accel X", robotAccelerationX.getValueAsDouble() * 9.81); - SmartDashboard.putNumber("Swerve/Robot Accel Y", robotAccelerationY.getValueAsDouble() * 9.81); - - SmartDashboard.putNumber("Swerve/Failed DAQ Count", this.getState().FailedDaqs); - SmartDashboard.putNumber("Swerve/CANBus Utiliaztion", Ports.CANIVORE.getStatus().BusUtilization); - // will confirm whether we are even getting data - - SmartDashboard.putBoolean("FieldPositions/isBehindTower", isBehindTower()); - SmartDashboard.putBoolean("FieldPositions/isUnderTrench", isUnderTrench()); - SmartDashboard.putBoolean("FieldPositions/isInOpponentZone", isInOpponentZone()); - - SmartDashboard.putNumber("Superstructure/Turret/Dist From Hub", - turretPose.getTranslation().getDistance(Field.HUB_CENTER.getTranslation())); - SmartDashboard.putNumber("InterpolationTesting/Turret Dist From Hub", - turretPose.getTranslation().getDistance(Field.HUB_CENTER.getTranslation())); - SmartDashboard.putNumber("InterpolationTesting/Turret Dist From Ferry Zone", turretPose.getTranslation() - .getDistance(Field.getFerryZonePose(pose.getTranslation()).getTranslation())); - - for (int i = 0; i < 4; i++) { - String prefix = "Swerve/Modules/Module " + i; - SwerveModuleState current = getModule(i).getCurrentState(); - SwerveModuleState target = getModule(i).getTargetState(); - - SmartDashboard.putNumber(prefix + "/Speed (m per s)", current.speedMetersPerSecond); - SmartDashboard.putNumber(prefix + "/Target Speed (m per s)", target.speedMetersPerSecond); - SmartDashboard.putNumber(prefix + "/Angle (deg)", current.angle.getDegrees() % 360); - SmartDashboard.putNumber(prefix + "/Target Angle (deg)", target.angle.getDegrees() % 360); - - if (Settings.DEBUG_MODE.get()) { - SmartDashboard.putNumber(prefix + "/Stator Current", - getModule(i).getDriveMotor().getStatorCurrent().getValueAsDouble()); - SmartDashboard.putNumber(prefix + "/Supply Current", - getModule(i).getDriveMotor().getSupplyCurrent().getValueAsDouble()); - } - } - Robot.getEnergyUtil().logEnergyUsage(getName() + " Drive", getTotalDriveSupplyCurrent()); - Robot.getEnergyUtil().logEnergyUsage(getName() + " Turn", getTotalSteerSupplyCurrent()); - - // CAN SIGNAL LOGGING - if (Settings.DEBUG_MODE.get() && Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { - SmartDashboard.putBoolean( - "Robot/CAN/Canivore/Front Left Drive Motor Connected? (ID " - + String.valueOf(TunerConstants.kFrontLeftDriveMotorId) + ")", - getModule(0).getDriveMotor().isConnected()); - SmartDashboard.putBoolean( - "Robot/CAN/Canivore/Front Left Steer Motor Connected? (ID " - + String.valueOf(TunerConstants.kFrontLeftSteerMotorId) + ")", - getModule(0).getSteerMotor().isConnected()); - SmartDashboard.putBoolean( - "Robot/CAN/Canivore/Front Left CANcoder Connected? (ID " - + String.valueOf(TunerConstants.kFrontLeftEncoderId) + ")", - getModule(0).getEncoder().isConnected()); - - SmartDashboard.putBoolean( - "Robot/CAN/Canivore/Front Right Drive Motor Connected? (ID " - + String.valueOf(TunerConstants.kFrontRightDriveMotorId) + ")", - getModule(1).getDriveMotor().isConnected()); - SmartDashboard.putBoolean( - "Robot/CAN/Canivore/Front Right Steer Motor Connected? (ID " - + String.valueOf(TunerConstants.kFrontRightSteerMotorId) + ")", - getModule(1).getSteerMotor().isConnected()); - SmartDashboard.putBoolean( - "Robot/CAN/Canivore/Front Right CANcoder Connected? (ID " - + String.valueOf(TunerConstants.kFrontRightEncoderId) + ")", - getModule(1).getEncoder().isConnected()); - - SmartDashboard.putBoolean( - "Robot/CAN/Canivore/Back Left Drive Motor Connected? (ID " - + String.valueOf(TunerConstants.kBackLeftDriveMotorId) + ")", - getModule(2).getDriveMotor().isConnected()); - SmartDashboard.putBoolean( - "Robot/CAN/Canivore/Back Left Steer Motor Connected? (ID " - + String.valueOf(TunerConstants.kBackLeftSteerMotorId) + ")", - getModule(2).getSteerMotor().isConnected()); - SmartDashboard.putBoolean( - "Robot/CAN/Canivore/Back Left CANcoder Connected? (ID " - + String.valueOf(TunerConstants.kBackLeftEncoderId) + ")", - getModule(2).getEncoder().isConnected()); - - SmartDashboard.putBoolean( - "Robot/CAN/Canivore/Back Right Drive Motor Connected? (ID " - + String.valueOf(TunerConstants.kBackRightDriveMotorId) + ")", - getModule(3).getDriveMotor().isConnected()); - SmartDashboard.putBoolean( - "Robot/CAN/Canivore/Back Right Steer Motor Connected? (ID " - + String.valueOf(TunerConstants.kBackRightSteerMotorId) + ")", - getModule(3).getSteerMotor().isConnected()); - SmartDashboard.putBoolean( - "Robot/CAN/Canivore/Back Right CANcoder Connected? (ID " - + String.valueOf(TunerConstants.kBackRightEncoderId) + ")", - getModule(3).getEncoder().isConnected()); - - } - } - } -} \ No newline at end of file + */ + private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine( + new SysIdRoutine.Config( + /* This is in radians per second², but SysId only supports "volts per second" */ + Volts.of(Math.PI / 6).per(Second), + /* This is in radians per second, but SysId only supports "volts" */ + Volts.of(Math.PI), + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdRotation_State", state.toString())), + new SysIdRoutine.Mechanism( + output -> { + /* output is actually radians per second, but SysId only supports "volts" */ + setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); + /* also log the requested output for SysId */ + SignalLogger.writeDouble("Rotational_Target_Rate ('voltage')", output.in(Volts)); + SignalLogger.writeDouble("Rotational Position", getPose().getRotation().getRadians()); + SignalLogger.writeDouble("Rotational_Velocity", getState().Speeds.omegaRadiansPerSecond); + }, + null, + this)); + + /* The SysId routine to test */ + private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineModuleTranslation; + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not + * construct the devices themselves. If they need the devices, they can + * access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + protected CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + SwerveModuleConstants... modules) { + super(drivetrainConstants, modules); + if (Utils.isSimulation()) { + startSimThread(); + } + + // leftBehindHubYPlublisher = NetworkTableInstance.getDefault().getStructTopic("FieldPositions/LeftBehindHubY", Pose2d.struct).publish(); + // rightBehindHubYPlublisher = NetworkTableInstance.getDefault().getStructTopic("FieldPositions/RightBehindHubY", Pose2d.struct).publish(); + // vertexBehindHubPublisher = NetworkTableInstance.getDefault().getStructTopic("FieldPositions/VertexBehindHub", Pose2d.struct).publish(); + + robotAccelerationX = this.getPigeon2().getAccelerationX(); + robotAccelerationY = this.getPigeon2().getAccelerationY(); + + driveMotorSupplyCurrents = new StatusSignal[4]; + steerMotorSupplyCurrents = new StatusSignal[4]; + + for (int i = 0; i < 4; i++) { + driveMotorSupplyCurrents[i] = getModule(i).getDriveMotor().getSupplyCurrent(); + steerMotorSupplyCurrents[i] = getModule(i).getSteerMotor().getSupplyCurrent(); + } + + PhoenixUtil.registerToCanivore( + robotAccelerationX, + robotAccelerationY + ); + + PhoenixUtil.registerToCanivore(driveMotorSupplyCurrents); + PhoenixUtil.registerToCanivore(steerMotorSupplyCurrents); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

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

+ * This constructs the underlying hardware devices, so users should not + * construct the devices themselves. If they need the devices, they can + * access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN + * 2.0. + * @param odometryStandardDeviation The standard deviation for odometry + * calculation in the form [x, y, theta]ᵀ, with units in meters and radians + * @param visionStandardDeviation The standard deviation for vision + * calculation in the form [x, y, theta]ᵀ, with units in meters and radians + * @param modules Constants for each specific module + */ + private CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules) { + super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, + modules); + if (Utils.isSimulation()) { + startSimThread(); + } + } + + @Override + public void setControl(SwerveRequest request) { + if (EnabledSubsystems.SWERVE.get()) { + super.setControl(request); + } else { + super.setControl(new SwerveRequest.Idle()); + } + } + + /** + * Runs the SysId Quasistatic test in the given direction for the routine + * specified by {@link #m_sysIdRoutineToApply}. + * + * @param direction Direction of the SysId Quasistatic test + * @return Command to run + */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineToApply.quasistatic(direction); + } + + /** + * Runs the SysId Dynamic test in the given direction for the routine + * specified by {@link #m_sysIdRoutineToApply}. + * + * @param direction Direction of the SysId Dynamic test + * @return Command to run + */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineToApply.dynamic(direction); + } + + public Command sysidRotationDynamic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineRotation.dynamic(direction); + } + + public Command sysidRotationQuasiStatic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineRotation.quasistatic(direction); + } + + private void startSimThread() { + m_lastSimTime = Utils.getCurrentTimeSeconds(); + + /* Run simulation at a faster rate so PID gains behave more reasonably */ + m_simNotifier = new Notifier(() -> { + final double currentTime = Utils.getCurrentTimeSeconds(); + double deltaTime = currentTime - m_lastSimTime; + m_lastSimTime = currentTime; + + /* use the measured time delta, get battery voltage from WPILib */ + updateSimState(deltaTime, RobotController.getBatteryVoltage()); + }); + m_simNotifier.startPeriodic(kSimLoopPeriod); + } + + private boolean checkIfVisionMeasurementValid(Pose2d visionPose) { + return !Settings.ENABLE_DISTANCE_CHECK.get() || visionPose.getTranslation().getDistance(getState().Pose.getTranslation()) < Settings.Swerve.MAX_ACCEPTABLE_VISION_DEVIATION_METERS; + } + + /** + * Adds a vision measurement to the Kalman Filter. This will correct the + * odometry pose estimate while still accounting for measurement noise. + * + * @param visionRobotPoseMeters The pose of the robot as measured by the + * vision camera. + * @param timestampSeconds The timestamp of the vision measurement in + * seconds. + */ + @Override + public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { + // SignalLogger.writeStruct("Vision/Pose", Pose2d.struct, visionRobotPoseMeters); + // SignalLogger.writeDouble("Vision/Timestamp", timestampSeconds); + + if (checkIfVisionMeasurementValid(visionRobotPoseMeters)) { + super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds)); + } + } + + /** + * Adds a vision measurement to the Kalman Filter. This will correct the + * odometry pose estimate while still accounting for measurement noise. + *

+ * Note that the vision measurement standard deviations passed into this + * method will continue to apply to future measurements until a subsequent + * call to {@link #setVisionMeasurementStdDevs(Matrix)} or this method. + * + * @param visionRobotPoseMeters The pose of the robot as measured by the + * vision camera. + * @param timestampSeconds The timestamp of the vision measurement in + * seconds. + * @param visionMeasurementStdDevs Standard deviations of the vision pose + * measurement in the form [x, y, theta]ᵀ, with units in meters and radians. + */ + @Override + public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds, Matrix visionMeasurementStdDevs) { + // SignalLogger.writeStruct("Vision/Pose", Pose2d.struct, visionRobotPoseMeters); + // SignalLogger.writeDouble("Vision/Timestamp", timestampSeconds); + + if (checkIfVisionMeasurementValid(visionRobotPoseMeters)) { + super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), + visionMeasurementStdDevs); + } + } + + public Pose2d getPose() { + Pose2d proposedPose = getState().Pose; + double proposedX = proposedPose.getX(); + double proposedY = proposedPose.getY(); + double poseDelta = lastGoodPose.getTranslation().getDistance(proposedPose.getTranslation()); + + // if (!(proposedX > Field.LENGTH || proposedX < 0 || proposedY > Field.WIDTH || proposedY < 0) && + // poseDelta <= Settings.Swerve.MAX_ACCEPTABLE_POSE_DELTA_METERS) { + lastGoodPose = proposedPose; + // } + + return lastGoodPose; + } + + public void configureAutoBuilder() { + try { + AutoBuilder.configure( + this::getPose, + this::resetPose, + this::getChassisSpeeds, + this::setChassisSpeeds, + new PPHolonomicDriveController(Gains.Swerve.Alignment.XY, Gains.Swerve.Alignment.THETA), + RobotConfig.fromGUISettings(), + () -> false, + instance); + PathPlannerLogging.setLogActivePathCallback((poses) -> { + if (Robot.isBlue()) { + Field.FIELD2D.getObject("path").setPoses(poses); + + } else { + Field.FIELD2D.getObject("path").setPoses(Field.transformToOppositeAlliance(poses)); + } + }); + } catch (Exception e) { + e.printStackTrace(); + } + } + + public Command followPathCommand(String pathName) { + try { + return followPathCommand(PathPlannerPath.fromPathFile(pathName)); + } catch (Exception e) { + throw new IllegalArgumentException(pathName + " does not exist"); + } + } + + public Command followPathCommand(PathPlannerPath path) { + return AutoBuilder.followPath(path); + } + + public SwerveModuleState[] getModuleStates() { + return getState().ModuleStates; + } + + public ChassisSpeeds getChassisSpeeds() { + return getState().Speeds; + } + + public Vector2D getFieldRelativeSpeeds() { + return new Vector2D(getChassisSpeeds().vxMetersPerSecond, getChassisSpeeds().vyMetersPerSecond) + .rotate(Angle.fromRotation2d(getPose().getRotation())); + } + + private void setChassisSpeeds(ChassisSpeeds robotSpeeds) { + setControl(new SwerveRequest.RobotCentric() + .withVelocityX(robotSpeeds.vxMetersPerSecond) + .withVelocityY(robotSpeeds.vyMetersPerSecond) + .withRotationalRate(robotSpeeds.omegaRadiansPerSecond)); + } + + public void drive(Vector2D velocity, double rotation) { + ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds( + Robot.isBlue() ? velocity.y : -velocity.y, + Robot.isBlue() ? -velocity.x : velocity.x, + -rotation, + getPose().getRotation()); + + Pose2d robotVel = new Pose2d( + Settings.DT * speeds.vxMetersPerSecond, + Settings.DT * speeds.vyMetersPerSecond, + Rotation2d.fromRadians(Settings.DT * speeds.omegaRadiansPerSecond)); + Twist2d twistVel = new Pose2d().log(robotVel); + + setChassisSpeeds(new ChassisSpeeds( + twistVel.dx / Settings.DT, + twistVel.dy / Settings.DT, + twistVel.dtheta / Settings.DT)); + } + + public Pose2d getTurretPose() { + return turretPose; + } + + public double[] getWheelDrivePositionsRadians() { + double[] positions = new double[4]; + double kDriveGearRatio = 7.67; + for (int i = 0; i < 4; i++) { + positions[i] = getModule(i).getDriveMotor().getPosition().getValueAsDouble() * 2 * Math.PI + / kDriveGearRatio; + } + return positions; + } + + public boolean isUnderTrench() { + if (isUnderTrench.isEmpty()) { + Translation2d turretTranslation = getTurretPose().getTranslation(); + + boolean isBetweenRightTrenchesY = Field.AllianceRightTrench.rightEdge.getY() < turretTranslation.getY() + && Field.AllianceRightTrench.leftEdge.getY() > turretTranslation.getY(); + + boolean isBetweenLeftTrenchesY = Field.AllianceLeftTrench.rightEdge.getY() < turretTranslation.getY() + && Field.AllianceLeftTrench.leftEdge.getY() > turretTranslation.getY(); + + boolean isUnderAllianceTrenchX = Math.abs( + turretTranslation.getX() - Field.AllianceRightTrench.rightEdge.getX()) < Field.TRENCH_HOOD_TOLERANCE; + + boolean isUnderOpponentTrenchX = Math.abs( + turretTranslation.getX() - Field.OpponentRightTrench.rightEdge.getX()) < Field.TRENCH_HOOD_TOLERANCE; + + boolean isUnderTrench = (isBetweenRightTrenchesY || isBetweenLeftTrenchesY) + && (isUnderAllianceTrenchX || isUnderOpponentTrenchX); + + this.isUnderTrench = Optional.of(isUnderTrench); + } + + return isUnderTrench.get(); + } + + public boolean isInOpponentZone() { + if (isInOpponentZone.isEmpty()) { + Translation2d turretTranslation = getTurretPose().getTranslation(); + isInOpponentZone = Optional.of(turretTranslation.getX() > Field.OPPONENT_ZONE_X); + } + return isInOpponentZone.get(); + } + + public boolean isBehindTower() { + if (isBehindTower.isEmpty()) { + boolean withinTowerX = getPose().getTranslation().getX() < Field.TOWER_FAR_CENTER.getX(); + boolean withinTowerY = Field.TOWER_FAR_RIGHT.getY() < getTurretPose().getTranslation().getY() + && getTurretPose().getTranslation().getY() < Field.TOWER_FAR_LEFT.getY(); + isBehindTower = Optional.of(withinTowerX && withinTowerY); + } + + return isBehindTower.get(); + } + + /** + * Checks whether the robot turret pose is behind the hub or not in the + * neutral zone. For stopping ferrying to prevent fuel hitting the hub. + * + * @return true if robot turret pose is behind the hub. + */ + public boolean isBehindHub() { + if (isBehindHub.isEmpty()) { + // === TRIANGLE === (CUSTOM VERTEX) + Translation2d turretTranslation = getTurretPose().getTranslation(); + + boolean behindHubX = Field.HUB_FAR_LEFT_CORNER.getX() < turretTranslation.getX(); + // && turretTranslation.getX() < Field.hubFarLeftCorner.getX() + Field.hubToleranceX; // With this line the triangle will be cut to more like a trapezoid. + + Pose2d hubFarLeftCornerWithTolerance = new Pose2d(Field.HUB_FAR_LEFT_CORNER.getX(), Field.HUB_FAR_LEFT_CORNER.getY() + Field.BEHIND_HUB_TOLERANCE_Y, new Rotation2d()); + Pose2d hubFarRightCornerWithTolerance = new Pose2d(Field.HUB_FAR_RIGHT_CORNER.getX(), Field.HUB_FAR_RIGHT_CORNER.getY() - Field.BEHIND_HUB_TOLERANCE_Y, new Rotation2d()); + + // Find point on triangle using the point-slope formula (of the line constructed by the hub corner pose and ferry pose) + // y = (slope)(robotX - hubCornerX) + (hubCornerY) + // where the slope = (hubCornerY - vertexY)/(hubCornerX - vertexX) + double leftY = ((hubFarLeftCornerWithTolerance.getY() - Field.BEHIND_HUB_TRIANGLE_VERTEX.getY()) + / (hubFarLeftCornerWithTolerance.getX() - Field.BEHIND_HUB_TRIANGLE_VERTEX.getX())) // (Slope) + * (turretTranslation.getX() - hubFarLeftCornerWithTolerance.getX()) + hubFarLeftCornerWithTolerance.getY(); // *(robotX - hubCornerX) + (hubCornerY) + double rightY = ((hubFarRightCornerWithTolerance.getY() - Field.BEHIND_HUB_TRIANGLE_VERTEX.getY()) + / (hubFarRightCornerWithTolerance.getX() - Field.BEHIND_HUB_TRIANGLE_VERTEX.getX())) // (Slope) + * (turretTranslation.getX() - hubFarRightCornerWithTolerance.getX()) + hubFarRightCornerWithTolerance.getY(); // *(robotX - hubCornerX) + (hubCornerY) + + // Debug: + // leftBehindHubYPlublisher.set(new Pose2d(getTurretPose().getX(), leftY, new Rotation2d())); + // rightBehindHubYPlublisher.set(new Pose2d(getTurretPose().getX(), rightY, new Rotation2d())); + // vertexBehindHubPublisher.set(Field.BEHIND_HUB_TRIANGLE_VERTEX); + + boolean withinHubY = rightY < getTurretPose().getY() + && getTurretPose().getY() < leftY; + + isBehindHub = Optional.of(behindHubX && withinHubY); + + // === TRIANGLE === (FROM FERRY ZONES): + // Translation2d turretTranslation = getTurretPose().getTranslation(); + // boolean behindHubX = Field.hubFarLeftCorner.getX() < turretTranslation.getX(); + // // && turretTranslation.getX() < Field.hubFarLeftCorner.getX() + Field.hubToleranceX; // With this line the triangle will be cut to more like a trapezoid. + // // Find point on triangle using the point-slope formula (of the line constructed by the hub corner pose and ferry pose) + // // y = (slope)(robotX - hubCornerX) + (hubCornerY) + // // where the slope = (hubCornerY - ferryY)/(hubCornerX - ferryX) + // double leftY = ((Field.hubFarLeftCorner.getY() - Field.leftFerryZone.getY())/(Field.hubFarLeftCorner.getX() - Field.leftFerryZone.getX())) // (Slope) + // * (turretTranslation.getX() - Field.hubFarLeftCorner.getX()) + Field.hubFarLeftCorner.getY(); // *(robotX - hubCornerX) + (hubCornerY) + // double rightY = ((Field.hubFarRightCorner.getY() - Field.rightFerryZone.getY())/(Field.hubFarRightCorner.getX() - Field.rightFerryZone.getX())) // (Slope) + // * (turretTranslation.getX() - Field.hubFarRightCorner.getX()) + Field.hubFarRightCorner.getY(); // *(robotX - hubCornerX) + (hubCornerY) + // leftBehindHubYPlublisher.set(new Pose2d(getTurretPose().getX(), leftY - Field.hubToleranceY, new Rotation2d())); + // rightBehindHubYPlublisher.set(new Pose2d(getTurretPose().getX(), rightY + Field.hubToleranceY, new Rotation2d())); + // boolean withinHubY = rightY + Field.hubToleranceY < getTurretPose().getY() + // && getTurretPose().getY() < leftY - Field.hubToleranceY; + // return behindHubX && withinHubY; + // === RECTANGLE ===: + // Translation2d turretTranslation = getTurretPose().getTranslation(); + // boolean behindHubX = Field.hubFarLeftCorner.getX() < turretTranslation.getX() + // && turretTranslation.getX() < Field.hubFarLeftCorner.getX() + Field.hubToleranceX; + // boolean withinHubY = Field.hubFarRightCorner.getY() + Field.hubToleranceY < getTurretPose().getY() + // && getTurretPose().getY() < Field.hubFarLeftCorner.getY() - Field.hubToleranceY; + // return behindHubX && withinHubY; + } + + return isBehindHub.get(); + } + + public boolean isOutsideAllianceZone() { + if (isOutsideAllianceZone.isEmpty()) { + isOutsideAllianceZone = Optional.of(getPose().getX() > Field.AllianceRightTrench.rightEdge.getX() + Field.TRENCH_HOOD_TOLERANCE); + } + + return isOutsideAllianceZone.get(); + } + + public void clearMemoized() { + isBehindHub = Optional.empty(); + isOutsideAllianceZone = Optional.empty(); + isInOpponentZone = Optional.empty(); + isUnderTrench = Optional.empty(); + isBehindTower = Optional.empty(); + } + + public void teleopInit() { + for (SwerveModule module : getModules()) { + TalonFXConfiguration newConfigs = new TalonFXConfiguration(); + module.getDriveMotor().getConfigurator().refresh(newConfigs); + newConfigs = newConfigs + .withCurrentLimits(new CurrentLimitsConfigs() + .withStatorCurrentLimit(80) + .withStatorCurrentLimitEnable(true)); + + module.getDriveMotor().getConfigurator().apply(newConfigs); + } + } + + public void autonInit() { + for (SwerveModule module : getModules()) { + TalonFXConfiguration newConfigs = new TalonFXConfiguration(); + module.getDriveMotor().getConfigurator().refresh(newConfigs); + newConfigs = newConfigs + .withCurrentLimits(new CurrentLimitsConfigs() + .withStatorCurrentLimit(120) + .withStatorCurrentLimitEnable(true)); + + module.getDriveMotor().getConfigurator().apply(newConfigs); + } + } + + public double getTotalDriveSupplyCurrent() { + double total = 0.0; + + for (StatusSignal s : driveMotorSupplyCurrents) { + total += Double.max(0, s.getValueAsDouble()); + } + + return total; + } + + public double getTotalSteerSupplyCurrent() { + double total = 0.0; + + for (StatusSignal s : steerMotorSupplyCurrents) { + total += Double.max(0, s.getValueAsDouble()); + } + + return total; + } + + public boolean canShootIntoHub() { + Superstructure superstructure = Superstructure.getInstance(); + SuperstructureState state = superstructure.getState(); + return !isOutsideAllianceZone() || (state == SuperstructureState.KB || state == SuperstructureState.LEFT_CORNER || state == SuperstructureState.RIGHT_CORNER); + } + + public void periodicAfterScheduler() { + Pose2d pose = getPose(); + turretPose = new Pose2d( + pose.getTranslation().plus( + Settings.Superstructure.Turret.TURRET_OFFSET.getTranslation().rotateBy(pose.getRotation())), + pose.getRotation().plus(Turret.getInstance().getAngle())); + + // robotPose.set(pose); + + turret2d.setPose(Robot.isBlue() ? turretPose : Field.transformToOppositeAlliance(turretPose)); + + ChassisSpeeds chassisSpeeds = getChassisSpeeds(); + Vector2D fieldRelativeSpeeds = getFieldRelativeSpeeds(); + DogLog.log("Swerve/Velocity Robot Relative X (m per s)", chassisSpeeds.vxMetersPerSecond); + DogLog.log("Swerve/Velocity Robot Relative Y (m per s)", chassisSpeeds.vyMetersPerSecond); + + DogLog.log("Swerve/Velocity Field Relative X (m per s)", fieldRelativeSpeeds.x); + DogLog.log("Swerve/Field Relative Rotation", pose.getRotation().getDegrees()); + DogLog.log("Swerve/Velocity Field Relative Y (m per s)", getFieldRelativeSpeeds().y); + + DogLog.log("Swerve/Angular Velocity (rad per s)", chassisSpeeds.omegaRadiansPerSecond); + DogLog.log("Swerve/Distance From Hub (meters)", Field.HUB_CENTER.getTranslation().getDistance(pose.getTranslation())); + + DogLog.log("Swerve/Pose", pose); + + Field.FIELD2D.getRobotObject().setPose(Robot.isBlue() ? pose : Field.transformToOppositeAlliance(pose)); + + Robot.getEnergyUtil().logEnergyUsage(getName() + " Drive", getTotalDriveSupplyCurrent()); + Robot.getEnergyUtil().logEnergyUsage(getName() + " Turn", getTotalSteerSupplyCurrent()); + + if (Robot.getPeriodicCounter() % Settings.LOGGING_FREQUENCY == 0) { + DogLog.log("Swerve/Robot Accel X", robotAccelerationX.getValueAsDouble() * 9.81); + DogLog.log("Swerve/Robot Accel Y", robotAccelerationY.getValueAsDouble() * 9.81); + + DogLog.log("Swerve/Failed DAQ Count", this.getState().FailedDaqs); + DogLog.log("Swerve/CANBus Utiliaztion", Ports.CANIVORE.getStatus().BusUtilization); + // will confirm whether we are even getting data + + DogLog.log("Superstructure/Turret/Dist From Hub", + turretPose.getTranslation().getDistance(Field.HUB_CENTER.getTranslation())); + DogLog.log("InterpolationTesting/Turret Dist From Hub", + turretPose.getTranslation().getDistance(Field.HUB_CENTER.getTranslation())); + DogLog.log("InterpolationTesting/Turret Dist From Ferry Zone", turretPose.getTranslation() + .getDistance(Field.getFerryZonePose(pose.getTranslation()).getTranslation())); + + for (int i = 0; i < 4; i++) { + String prefix = "Swerve/Modules/Module " + i; + SwerveModuleState current = getModule(i).getCurrentState(); + SwerveModuleState target = getModule(i).getTargetState(); + + DogLog.log(prefix + "/Speed (m per s)", current.speedMetersPerSecond); + DogLog.log(prefix + "/Target Speed (m per s)", target.speedMetersPerSecond); + DogLog.log(prefix + "/Angle (deg)", current.angle.getDegrees() % 360); + DogLog.log(prefix + "/Target Angle (deg)", target.angle.getDegrees() % 360); + + if (Settings.DEBUG_MODE.get()) { + DogLog.log(prefix + "/Stator Current", + getModule(i).getDriveMotor().getStatorCurrent().getValueAsDouble()); + DogLog.log(prefix + "/Supply Current", + getModule(i).getDriveMotor().getSupplyCurrent().getValueAsDouble()); + } + } + + // CAN SIGNAL LOGGING + if (Settings.DEBUG_MODE.get() && Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { + DogLog.log( + "Robot/CAN/Canivore/Front Left Drive Motor Connected? (ID " + + String.valueOf(TunerConstants.kFrontLeftDriveMotorId) + ")", + getModule(0).getDriveMotor().isConnected()); + DogLog.log( + "Robot/CAN/Canivore/Front Left Steer Motor Connected? (ID " + + String.valueOf(TunerConstants.kFrontLeftSteerMotorId) + ")", + getModule(0).getSteerMotor().isConnected()); + DogLog.log( + "Robot/CAN/Canivore/Front Left CANcoder Connected? (ID " + + String.valueOf(TunerConstants.kFrontLeftEncoderId) + ")", + getModule(0).getEncoder().isConnected()); + + DogLog.log( + "Robot/CAN/Canivore/Front Right Drive Motor Connected? (ID " + + String.valueOf(TunerConstants.kFrontRightDriveMotorId) + ")", + getModule(1).getDriveMotor().isConnected()); + DogLog.log( + "Robot/CAN/Canivore/Front Right Steer Motor Connected? (ID " + + String.valueOf(TunerConstants.kFrontRightSteerMotorId) + ")", + getModule(1).getSteerMotor().isConnected()); + DogLog.log( + "Robot/CAN/Canivore/Front Right CANcoder Connected? (ID " + + String.valueOf(TunerConstants.kFrontRightEncoderId) + ")", + getModule(1).getEncoder().isConnected()); + + DogLog.log( + "Robot/CAN/Canivore/Back Left Drive Motor Connected? (ID " + + String.valueOf(TunerConstants.kBackLeftDriveMotorId) + ")", + getModule(2).getDriveMotor().isConnected()); + DogLog.log( + "Robot/CAN/Canivore/Back Left Steer Motor Connected? (ID " + + String.valueOf(TunerConstants.kBackLeftSteerMotorId) + ")", + getModule(2).getSteerMotor().isConnected()); + DogLog.log( + "Robot/CAN/Canivore/Back Left CANcoder Connected? (ID " + + String.valueOf(TunerConstants.kBackLeftEncoderId) + ")", + getModule(2).getEncoder().isConnected()); + + DogLog.log( + "Robot/CAN/Canivore/Back Right Drive Motor Connected? (ID " + + String.valueOf(TunerConstants.kBackRightDriveMotorId) + ")", + getModule(3).getDriveMotor().isConnected()); + DogLog.log( + "Robot/CAN/Canivore/Back Right Steer Motor Connected? (ID " + + String.valueOf(TunerConstants.kBackRightSteerMotorId) + ")", + getModule(3).getSteerMotor().isConnected()); + DogLog.log( + "Robot/CAN/Canivore/Back Right CANcoder Connected? (ID " + + String.valueOf(TunerConstants.kBackRightEncoderId) + ")", + getModule(3).getEncoder().isConnected()); + + } + } + } +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java index 5d6c648f..19bb9e0f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java @@ -145,7 +145,7 @@ public class TunerConstants { public static final int kFrontLeftDriveMotorId = 14; public static final int kFrontLeftSteerMotorId = 15; public static final int kFrontLeftEncoderId = 2; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.220703125); + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.2275390625); private static final boolean kFrontLeftSteerMotorInverted = true; private static final boolean kFrontLeftEncoderInverted = false; @@ -167,7 +167,7 @@ public class TunerConstants { public static final int kBackLeftDriveMotorId = 16; public static final int kBackLeftSteerMotorId = 17; public static final int kBackLeftEncoderId = 1; - private static final Angle kBackLeftEncoderOffset = Rotations.of(0.488525390625); + private static final Angle kBackLeftEncoderOffset = Rotations.of(0.488037109375); private static final boolean kBackLeftSteerMotorInverted = true; private static final boolean kBackLeftEncoderInverted = false; @@ -178,7 +178,7 @@ public class TunerConstants { public static final int kBackRightDriveMotorId = 11; public static final int kBackRightSteerMotorId = 10; public static final int kBackRightEncoderId = 4; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.450927734375); + private static final Angle kBackRightEncoderOffset = Rotations.of(0.4453125); private static final boolean kBackRightSteerMotorInverted = true; private static final boolean kBackRightEncoderInverted = false; diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 4454b8ee..c7b02853 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -22,13 +22,11 @@ import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; +import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class LimelightVision extends SubsystemBase { @@ -50,9 +48,9 @@ public static LimelightVision getInstance() { private Pose2d[] limelightPoseArray; - private StructPublisher leftLimelightPosePublisher; - private StructPublisher rightLimelightPosePublisher; - private StructPublisher backLimelightPosePublisher; + // private StructPublisher leftLimelightPosePublisher; + // private StructPublisher rightLimelightPosePublisher; + // private StructPublisher backLimelightPosePublisher; private boolean hasData; private BStream debouncedHasData; @@ -73,9 +71,9 @@ public void setPipeline(Pipeline pipeline) { public LimelightVision() { limelightPoseArray = new Pose2d[Cameras.LimelightCameras.length]; - leftLimelightPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Limelight/Pose Left", Pose2d.struct).publish(); - rightLimelightPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Limelight/Pose Right", Pose2d.struct).publish(); - backLimelightPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Limelight/Pose Back", Pose2d.struct).publish(); + // leftLimelightPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Limelight/Pose Left", Pose2d.struct).publish(); + // rightLimelightPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Limelight/Pose Right", Pose2d.struct).publish(); + // backLimelightPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Limelight/Pose Back", Pose2d.struct).publish(); names = new String[Cameras.LimelightCameras.length]; @@ -180,7 +178,7 @@ public void setTagBlacklist(int[] tagsToBlacklist, String limelight) { } System.out.println(Arrays.toString(validTags)); - SmartDashboard.putString("Allowlisted Tags for " + limelight, Arrays.toString(validTags)); + DogLog.log("Allowlisted Tags for " + limelight, Arrays.toString(validTags)); LimelightHelpers.SetFiducialIDFiltersOverride(limelight, validTags); } @@ -284,41 +282,41 @@ public void periodicAfterScheduler() { hasData = true; } - SmartDashboard.putBoolean("Vision/Within Invalid Position Tolerance", withinInvalidPositionTolerance); - SmartDashboard.putBoolean("Vision/Within Angular Velocity Tolerance", withinAngularVelocityTolerance); - SmartDashboard.putBoolean("Vision/Not Null", notNull); - - SmartDashboard.putNumber("Vision/Pose X Component", robotPose.getX()); - SmartDashboard.putNumber("Vision/Pose Y Component", robotPose.getY()); - SmartDashboard.putNumber("Vision/Pose Theta (Degrees)", robotPose.getRotation().getDegrees()); - SmartDashboard.putNumber("Vision/Pose Estimate X " + limelightName, poseEstimate.pose.getX()); - SmartDashboard.putNumber("Vision/Pose Estimate Y " + limelightName, poseEstimate.pose.getY()); - SmartDashboard.putNumber("Vision/Pose Estimate Theta " + limelightName, poseEstimate.pose.getRotation().getDegrees()); - - switch (limelightName) { - case "limelight-right" -> - rightLimelightPosePublisher.set(robotPose); - case "limelight-left" -> - leftLimelightPosePublisher.set(robotPose); - case "limelight-back" -> - backLimelightPosePublisher.set(robotPose); - } + DogLog.log("Vision/Within Invalid Position Tolerance", withinInvalidPositionTolerance); + DogLog.log("Vision/Within Angular Velocity Tolerance", withinAngularVelocityTolerance); + DogLog.log("Vision/Not Null", notNull); + + DogLog.log("Vision/Pose X Component", robotPose.getX()); + DogLog.log("Vision/Pose Y Component", robotPose.getY()); + DogLog.log("Vision/Pose Theta (Degrees)", robotPose.getRotation().getDegrees()); + DogLog.log("Vision/Pose Estimate X " + limelightName, poseEstimate.pose.getX()); + DogLog.log("Vision/Pose Estimate Y " + limelightName, poseEstimate.pose.getY()); + DogLog.log("Vision/Pose Estimate Theta " + limelightName, poseEstimate.pose.getRotation().getDegrees()); + + // switch (limelightName) { + // case "limelight-right" -> + // rightLimelightPosePublisher.set(robotPose); + // case "limelight-left" -> + // leftLimelightPosePublisher.set(robotPose); + // case "limelight-back" -> + // backLimelightPosePublisher.set(robotPose); + // } - SmartDashboard.putBoolean("Vision/" + names[i] + " Has Data", true); + DogLog.log("Vision/" + names[i] + " Has Data", true); } else { - SmartDashboard.putBoolean("Vision/" + names[i] + " Has Data", false); + DogLog.log("Vision/" + names[i] + " Has Data", false); Cameras.LimelightCameras[i].incrementRejection(RejectionValue.NOT_NULL); } - SmartDashboard.putString("Vision/MegaTag Mode", megaTagMode.toString()); + DogLog.log("Vision/MegaTag Mode", megaTagMode.toString()); // this yaw is seems to be the robot yaw passed into the LL - SmartDashboard.putNumber("Vision/Limelight Robot Yaw", LimelightHelpers.getIMUData(limelightName).robotYaw); + DogLog.log("Vision/Limelight Robot Yaw", LimelightHelpers.getIMUData(limelightName).robotYaw); // this is just the yaw of the internal imu - SmartDashboard.putNumber("Vision/Limelight Yaw", LimelightHelpers.getIMUData(limelightName).Yaw); + DogLog.log("Vision/Limelight Yaw", LimelightHelpers.getIMUData(limelightName).Yaw); //Rejection counters - Cameras.LimelightCameras[i].logRejections(); + Cameras.LimelightCameras[i].log(); } } @@ -341,7 +339,7 @@ public void periodicAfterScheduler() { } } - SmartDashboard.putBoolean("Vision/Has Data", hasData); + DogLog.log("Vision/Has Data", hasData); } } } diff --git a/src/main/java/com/stuypulse/robot/util/EnergyUtil.java b/src/main/java/com/stuypulse/robot/util/EnergyUtil.java index 4344f167..cf367a6b 100644 --- a/src/main/java/com/stuypulse/robot/util/EnergyUtil.java +++ b/src/main/java/com/stuypulse/robot/util/EnergyUtil.java @@ -5,6 +5,7 @@ import com.stuypulse.robot.constants.Settings; +import dev.doglog.DogLog; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** @@ -17,9 +18,7 @@ public class EnergyUtil { private double totalCurrent = 0.0; private double totalPowerWatts = 0.0; - private double totalCurrentCurrent = 0.0; - private double totalCurrentPowerWatts = 0.0; - private double totalEnergyWattHours = 0.0; + private double totalEnergyJoules = 0.0; private double batteryVoltage = 12.6; private Map subsytemCurrents = new HashMap<>(); @@ -32,7 +31,7 @@ public void logEnergyUsage(String subsystem, double amps) { totalCurrent += amps; totalPowerWatts += powerWatts; - totalEnergyWattHours += energyWattHours; + totalEnergyJoules += energyWattHours; subsytemCurrents.put(subsystem, amps); subsytemPowers.put(subsystem, powerWatts); @@ -56,29 +55,23 @@ public void logEnergyUsage(String subsystem, double amps) { } public void periodic() { - // energy used over the total time the robot has been on - SmartDashboard.putNumber("EnergyUtil/Total used Supply Current draw Amps", totalCurrent); - SmartDashboard.putNumber("EnergyUtil/total used Power Watts", totalPowerWatts); - SmartDashboard.putNumber("EnergyUtil/Total Used Energy Watt Hours", joulesToWattHours(totalEnergyWattHours)); - + DogLog.log("EnergyUtil/Total Used Energy", joulesToWattHours(totalEnergyJoules), "Watt Hours"); // energy used over the current command schedule loop - totalCurrentCurrent = totalCurrent - totalCurrentCurrent; - totalCurrentPowerWatts = totalPowerWatts - totalCurrentPowerWatts; - SmartDashboard.putNumber("EnergyUtil/Total current Current", totalCurrentCurrent); - SmartDashboard.putNumber("EnergyUtil/Total current Power Watts", totalCurrentCurrent); + DogLog.log("EnergyUtil/Total Current", totalCurrent, "Amps"); + DogLog.log("EnergyUtil/Total Power", totalPowerWatts, "Watts"); for (var entry : subsytemCurrents.entrySet()) { - SmartDashboard.putNumber("EnergyUtil/Supply Current Amps/" + entry.getKey(), entry.getValue()); + DogLog.log("EnergyUtil/Supply Current Amps/" + entry.getKey(), entry.getValue()); subsytemCurrents.put(entry.getKey(), 0.0); } for (var entry : subsytemPowers.entrySet()) { - SmartDashboard.putNumber("EnergyUtil/Power Watts/" + entry.getKey(), entry.getValue()); + DogLog.log("EnergyUtil/Power Watts/" + entry.getKey(), entry.getValue()); subsytemPowers.put(entry.getKey(), 0.0); } for (var entry : subsytemEnergies.entrySet()) { - SmartDashboard.putNumber("EnergyUtil/Energy Watt Hours/" + entry.getKey(), joulesToWattHours(entry.getValue())); + DogLog.log("EnergyUtil/Energy Watt Hours/" + entry.getKey(), joulesToWattHours(entry.getValue())); } } @@ -93,5 +86,4 @@ private double joulesToWattHours(double joules) { public void setBatteryVoltage(double voltage) { this.batteryVoltage = voltage; } - } diff --git a/src/main/java/com/stuypulse/robot/util/FMSUtil.java b/src/main/java/com/stuypulse/robot/util/FMSUtil.java index 8158e407..d28a6535 100644 --- a/src/main/java/com/stuypulse/robot/util/FMSUtil.java +++ b/src/main/java/com/stuypulse/robot/util/FMSUtil.java @@ -12,6 +12,8 @@ import java.util.Optional; +import dev.doglog.DogLog; + public class FMSUtil { private final Timer timer = new Timer(); @@ -40,7 +42,11 @@ public boolean isActive(double time) { } public double timeLeft(double time) { - return Math.max(0.0, endTime - time); + if (this == FieldState.SHIFT_4) { + return Math.max(0.0, 140.0 - time); + } else { + return Math.max(0.0, endTime - time); + } } public double timeElapsed(double time) { @@ -111,7 +117,7 @@ public boolean didWinAuto() { if (winner == null || winner.isEmpty() || allianceOpt.isEmpty()) { DriverStation.reportWarning("No FMS auto winner data available", false); - SmartDashboard.putBoolean("FMSUtil/No Auto Winner Data", true); + DogLog.log("FMSUtil/No Auto Winner Data", true); return autoOverride; } diff --git a/src/main/java/com/stuypulse/robot/util/PathUtil.java b/src/main/java/com/stuypulse/robot/util/PathUtil.java index 01f7ba36..074501e5 100644 --- a/src/main/java/com/stuypulse/robot/util/PathUtil.java +++ b/src/main/java/com/stuypulse/robot/util/PathUtil.java @@ -8,6 +8,8 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.WaitCommand; import com.pathplanner.lib.path.PathPlannerPath; import java.io.IOException; @@ -19,6 +21,7 @@ import java.util.Collections; import java.util.HashMap; import java.util.List; +import java.util.Optional; import java.util.function.Function; public class PathUtil { @@ -27,11 +30,15 @@ public static class AutonConfig { private final String name; private final Function auton; private final String[] paths; + private final Optional waitTimeOne; + private final Optional waitTimeTwo; - public AutonConfig(String name, Function auton, String... paths) { + public AutonConfig(String name, Function auton, double waitTimeOne, double waitTimeTwo, String... paths) { this.name = name; this.auton = auton; this.paths = paths; + this.waitTimeOne = Optional.of(waitTimeOne); + this.waitTimeTwo = Optional.of(waitTimeTwo); for (String path : paths) { try { @@ -41,9 +48,21 @@ public AutonConfig(String name, Function auton, Stri } } } + + public AutonConfig(String name, Function auton, String... paths) { + this(name, auton, 0.0, 0.0, paths); + } + + private Command buildCommand() { + Command autonCommand = auton.apply(loadPaths(paths)); + // if (waitTimeOne.isPresent() && waitTimeOne.get() > 0.0) { + // return Commands.sequence(new WaitCommand(waitTimeOne.get()), autonCommand); + // } + return autonCommand; + } public AutonConfig register(SendableChooser chooser) { - chooser.addOption(name, auton.apply(loadPaths(paths))); + chooser.addOption(name, buildCommand()); return this; } diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java index 70658778..95fe7366 100644 --- a/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java @@ -5,6 +5,8 @@ /***************************************************************/ package com.stuypulse.robot.util.superstructure; +import java.util.Optional; + import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.Superstructure.AngleInterpolation; @@ -14,6 +16,7 @@ import com.stuypulse.robot.constants.Settings.Superstructure.TOFInterpolation; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -29,6 +32,42 @@ public class InterpolationCalculator { public static InterpolatingDoubleTreeMap ferryingDistanceRPMInterpolator; public static InterpolatingDoubleTreeMap ferryingDistanceTOFInterpolator; + private static Optional cachedInterpolatedShotInfo = Optional.empty(); + private static Optional cachedInterpolatedFerryInfo = Optional.empty(); + + public static void clearMemoized() { + cachedInterpolatedShotInfo = Optional.empty(); + cachedInterpolatedFerryInfo = Optional.empty(); + } + + public static double getInterpolatedShotRPM() { + if (cachedInterpolatedShotInfo.isEmpty()) { + cachedInterpolatedShotInfo = Optional.of(interpolateShotInfo()); + } + return cachedInterpolatedShotInfo.get().targetRPM(); + } + + public static Rotation2d getInterpolatedShotAngle() { + if (cachedInterpolatedShotInfo.isEmpty()) { + cachedInterpolatedShotInfo = Optional.of(interpolateShotInfo()); + } + return cachedInterpolatedShotInfo.get().targetHoodAngle(); + } + + public static double getInterpolatedFerryRPM() { + if (cachedInterpolatedFerryInfo.isEmpty()) { + cachedInterpolatedFerryInfo = Optional.of(interpolateFerryingInfo()); + } + return cachedInterpolatedFerryInfo.get().targetRPM(); + } + + public static Rotation2d getInterpolatedFerryAngle() { + if (cachedInterpolatedFerryInfo.isEmpty()) { + cachedInterpolatedFerryInfo = Optional.of(interpolateFerryingInfo()); + } + return cachedInterpolatedFerryInfo.get().targetHoodAngle(); + } + public record InterpolatedShotInfo( Rotation2d targetHoodAngle, double targetRPM, @@ -114,9 +153,9 @@ public static InterpolatedFerryInfo interpolateFerryingInfo(Pose2d turretPose, P double targetRPM = ferryingDistanceRPMInterpolator.get(distanceMeters); double flightTime = ferryingDistanceTOFInterpolator.get(distanceMeters); - SmartDashboard.putNumber("Superstructure/Interpolated Ferry Target Angle", targetAngle.getDegrees()); - SmartDashboard.putNumber("Superstructure/Interpolated Ferry RPM", targetRPM); - SmartDashboard.putNumber("Superstructure/Interpolated Ferry TOF", flightTime); + DogLog.log("Superstructure/Interpolated Ferry Target Angle", targetAngle.getDegrees()); + DogLog.log("Superstructure/Interpolated Ferry RPM", targetRPM); + DogLog.log("Superstructure/Interpolated Ferry TOF", flightTime); return new InterpolatedFerryInfo( targetAngle, diff --git a/vendordeps/DogLog.json b/vendordeps/DogLog.json new file mode 100644 index 00000000..7201ed05 --- /dev/null +++ b/vendordeps/DogLog.json @@ -0,0 +1,20 @@ +{ + "javaDependencies": [ + { + "groupId": "com.github.jonahsnider", + "artifactId": "doglog", + "version": "2026.5.0" + } + ], + "fileName": "DogLog.json", + "frcYear": "2026", + "jsonUrl": "https://doglog.dev/vendordep.json", + "name": "DogLog", + "jniDependencies": [], + "mavenUrls": [ + "https://jitpack.io" + ], + "cppDependencies": [], + "version": "2026.5.0", + "uuid": "65592ce1-2251-4a31-8e4b-2df20dacebe4" +} \ No newline at end of file