diff --git a/elastic-layout.json b/elastic-layout.json index f475b1d9..be17da66 100644 --- a/elastic-layout.json +++ b/elastic-layout.json @@ -70,93 +70,93 @@ } }, { - "title": "Swerve", + "title": "Shooter", "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/Shooter Is Enabled", "period": 0.06, "data_type": "boolean" } }, { - "title": "Shooter", + "title": "Turret", "x": 0.0, "y": 0.0, "width": 166.0, "height": 166.0, "type": "Toggle Switch", "properties": { - "topic": "SmartDashboard/Enabled Subsystems/Shooter Is Enabled", + "topic": "SmartDashboard/Enabled Subsystems/Turret Is Enabled", "period": 0.06, "data_type": "boolean" } }, { - "title": "Turret", + "title": "Back Limelight", "x": 0.0, "y": 0.0, "width": 166.0, "height": 166.0, "type": "Toggle Switch", "properties": { - "topic": "SmartDashboard/Enabled Subsystems/Turret Is Enabled", + "topic": "SmartDashboard/Enabled Subsystems/Back Limelight Is Enabled", "period": 0.06, "data_type": "boolean" } }, { - "title": "Back Limelight", + "title": "Right Limelight", "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/Right Limelight Is Enabled", "period": 0.06, "data_type": "boolean" } }, { - "title": "Left Limelight", + "title": "LEDs Is Enabled", "x": 0.0, "y": 0.0, - "width": 166.0, - "height": 166.0, + "width": 140.0, + "height": 140.0, "type": "Toggle Switch", "properties": { - "topic": "SmartDashboard/Enabled Subsystems/Left Limelight Is Enabled", - "period": 0.06, + "topic": "SmartDashboard/Enabled Subsystems/LEDs Is Enabled", + "period": 0.5, "data_type": "boolean" } }, { - "title": "Right Limelight", + "title": "Swerve", "x": 0.0, "y": 0.0, "width": 166.0, "height": 166.0, "type": "Toggle Switch", "properties": { - "topic": "SmartDashboard/Enabled Subsystems/Right Limelight Is Enabled", + "topic": "SmartDashboard/Enabled Subsystems/Swerve Is Enabled", "period": 0.06, "data_type": "boolean" } }, { - "title": "LEDs Is Enabled", + "title": "Left Limelight", "x": 0.0, "y": 0.0, - "width": 140.0, - "height": 140.0, + "width": 166.0, + "height": 166.0, "type": "Toggle Switch", "properties": { - "topic": "SmartDashboard/Enabled Subsystems/LEDs Is Enabled", - "period": 0.5, + "topic": "SmartDashboard/Enabled Subsystems/Left Limelight Is Enabled", + "period": 0.06, "data_type": "boolean" } } @@ -174,7 +174,7 @@ "properties": { "topic": "/SmartDashboard/Field", "period": 0.06, - "field_game": "Rebuilt (No Fuel)", + "field_game": "Rebuilt", "robot_width": 0.85, "robot_length": 0.85, "show_other_objects": true, @@ -193,9 +193,8 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Handoff/At Tolerance?", + "topic": "/Robot/Handoff/At Tolerance?", "period": 0.06, - "data_type": "boolean", "true_color": 4283215696, "false_color": 4294198070, "true_icon": "None", @@ -210,7 +209,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 @@ -224,7 +223,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, @@ -241,7 +240,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 @@ -255,7 +254,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 @@ -269,7 +268,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 @@ -283,7 +282,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 @@ -297,7 +296,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, @@ -314,7 +313,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, @@ -331,7 +330,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, @@ -349,8 +348,7 @@ "type": "Large Text Display", "properties": { "topic": "/SmartDashboard/FMSUtil/Field State", - "period": 0.06, - "data_type": "string" + "period": 0.06 } }, { @@ -361,9 +359,8 @@ "height": 140.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/FMSUtil/Is Active Shift?", + "topic": "/Robot/FMSUtil/Is Active Shift?", "period": 0.06, - "data_type": "boolean", "true_color": 4283215696, "false_color": 4294198070, "true_icon": "None", @@ -378,7 +375,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", @@ -386,19 +383,6 @@ "yellow_start_time": 30 } }, - { - "title": "Autonomous", - "x": 560.0, - "y": 420.0, - "width": 210.0, - "height": 140.0, - "type": "ComboBox Chooser", - "properties": { - "topic": "/SmartDashboard/Autonomous", - "period": 0.06, - "sort_options": false - } - }, { "title": "Set Megatag 2", "x": 770.0, @@ -409,8 +393,8 @@ "properties": { "topic": "/SmartDashboard/Robot/Set Megatag 2", "period": 0.5, - "show_type": true, - "maximize_button_space": false + "show_type": false, + "maximize_button_space": true } }, { @@ -423,8 +407,8 @@ "properties": { "topic": "/SmartDashboard/Robot/Set Megatag 1", "period": 0.5, - "show_type": true, - "maximize_button_space": false + "show_type": false, + "maximize_button_space": true } }, { @@ -435,9 +419,8 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Spindexer/At Tolerance", + "topic": "/Robot/Spindexer/At Tolerance", "period": 0.5, - "data_type": "boolean", "true_color": 4283215696, "false_color": 4294198070, "true_icon": "None", @@ -452,7 +435,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 @@ -466,23 +449,22 @@ "height": 140.0, "type": "Match Time", "properties": { - "topic": "/SmartDashboard/Robot/Match Time", + "topic": "/Robot/Robot/Match Time", "period": 0.5, - "data_type": "double", "time_display_mode": "Minutes and Seconds", "red_start_time": 20, "yellow_start_time": 30 } }, { - "title": "limelight-back Has Data", + "title": "Back Has Data", "x": 420.0, "y": 560.0, "width": 140.0, "height": 140.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Vision/limelight-back Has Data", + "topic": "/Robot/Vision/limelight-back Has Data", "period": 0.5, "data_type": "boolean", "true_color": 4283215696, @@ -492,14 +474,14 @@ } }, { - "title": "limelight-left Has Data", + "title": "Left Has Data", "x": 560.0, "y": 560.0, "width": 140.0, "height": 140.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Vision/limelight-left Has Data", + "topic": "/Robot/Vision/limelight-left Has Data", "period": 0.5, "data_type": "boolean", "true_color": 4283215696, @@ -509,14 +491,14 @@ } }, { - "title": "limelight-right Has Data", + "title": "Right Has Data", "x": 700.0, "y": 560.0, "width": 140.0, "height": 140.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Vision/limelight-right Has Data", + "topic": "/Robot/Vision/limelight-right Has Data", "period": 0.5, "data_type": "boolean", "true_color": 4283215696, @@ -524,6 +506,19 @@ "true_icon": "None", "false_icon": "None" } + }, + { + "title": "Autonomous", + "x": 560.0, + "y": 420.0, + "width": 210.0, + "height": 140.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/SmartDashboard/Autonomous", + "period": 0.5, + "sort_options": false + } } ] } @@ -687,7 +682,7 @@ "properties": { "topic": "/SmartDashboard/Field", "period": 0.06, - "field_game": "Rebuilt (No Fuel)", + "field_game": "Rebuilt", "robot_width": 0.85, "robot_length": 0.85, "show_other_objects": true, @@ -706,9 +701,8 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Handoff/At Tolerance?", + "topic": "/Robot/Handoff/At Tolerance?", "period": 0.06, - "data_type": "boolean", "true_color": 4283215696, "false_color": 4294198070, "true_icon": "None", @@ -723,7 +717,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 @@ -737,7 +731,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, @@ -754,7 +748,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 @@ -768,7 +762,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 @@ -782,7 +776,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 @@ -796,7 +790,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, @@ -813,7 +807,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, @@ -830,7 +824,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, @@ -839,19 +833,6 @@ "false_icon": "None" } }, - { - "title": "Shift", - "x": 0.0, - "y": 560.0, - "width": 280.0, - "height": 140.0, - "type": "Large Text Display", - "properties": { - "topic": "/SmartDashboard/FMSUtil/Field State", - "period": 0.06, - "data_type": "string" - } - }, { "title": "Is Active Shift?", "x": 280.0, @@ -860,9 +841,8 @@ "height": 140.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/FMSUtil/Is Active Shift?", + "topic": "/Robot/is active shift", "period": 0.06, - "data_type": "boolean", "true_color": 4283215696, "false_color": 4294198070, "true_icon": "None", @@ -877,7 +857,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", @@ -893,7 +873,7 @@ "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, @@ -904,10 +884,10 @@ }, { "title": "Handoff Reverse", - "x": 1120.0, + "x": 560.0, "y": 420.0, "width": 280.0, - "height": 280.0, + "height": 140.0, "type": "Command", "properties": { "topic": "/SmartDashboard/Robot/Handoff Reverse", @@ -918,10 +898,10 @@ }, { "title": "Intake Reverse", - "x": 560.0, + "x": 840.0, "y": 420.0, "width": 280.0, - "height": 280.0, + "height": 140.0, "type": "Command", "properties": { "topic": "/SmartDashboard/Robot/Intake Reverse", @@ -932,10 +912,10 @@ }, { "title": "Spindexer Reverse", - "x": 840.0, + "x": 1120.0, "y": 420.0, "width": 280.0, - "height": 280.0, + "height": 140.0, "type": "Command", "properties": { "topic": "/SmartDashboard/Robot/Spindexer Reverse", @@ -952,9 +932,8 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Spindexer/At Tolerance", + "topic": "/Robot/Spindexer/At Tolerance", "period": 0.06, - "data_type": "boolean", "true_color": 4283215696, "false_color": 4294198070, "true_icon": "None", @@ -969,15 +948,15 @@ "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 } }, { - "title": "Current Intake Angle (deg)", - "x": 910.0, + "title": "Current Intake Angle", + "x": 560.0, "y": 700.0, "width": 210.0, "height": 140.0, @@ -997,9 +976,8 @@ "height": 140.0, "type": "Match Time", "properties": { - "topic": "/SmartDashboard/Robot/Match Time", + "topic": "/Robot/Robot/Match Time", "period": 0.5, - "data_type": "double", "time_display_mode": "Minutes and Seconds", "red_start_time": 20, "yellow_start_time": 30 @@ -1007,7 +985,7 @@ }, { "title": "Current Hood Angle", - "x": 280.0, + "x": 1050.0, "y": 700.0, "width": 210.0, "height": 140.0, @@ -1021,7 +999,7 @@ }, { "title": "Hood Homing Routine Lower", - "x": 0.0, + "x": 770.0, "y": 700.0, "width": 280.0, "height": 140.0, @@ -1035,7 +1013,7 @@ }, { "title": "Seed Intake At Upper Limit", - "x": 630.0, + "x": 280.0, "y": 700.0, "width": 280.0, "height": 140.0, @@ -1049,7 +1027,7 @@ }, { "title": "Seed Intake At Lower Limit", - "x": 1120.0, + "x": 0.0, "y": 700.0, "width": 280.0, "height": 140.0, @@ -1069,11 +1047,64 @@ "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 } + }, + { + "title": "Enable Distance Check?", + "x": 560.0, + "y": 560.0, + "width": 280.0, + "height": 140.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Robot/Enable Distance Check?", + "period": 0.5, + "data_type": "boolean" + } + }, + { + "title": "Set Robot Pose Right Corner", + "x": 1120.0, + "y": 560.0, + "width": 280.0, + "height": 140.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Robot/Set Robot Pose Right Corner", + "period": 0.5, + "show_type": false, + "maximize_button_space": true + } + }, + { + "title": "Set Robot Pose Left Corner", + "x": 840.0, + "y": 560.0, + "width": 280.0, + "height": 140.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Robot/Set Robot Pose Left Corner", + "period": 0.5, + "show_type": false, + "maximize_button_space": true + } + }, + { + "title": "Shift", + "x": 0.0, + "y": 560.0, + "width": 280.0, + "height": 140.0, + "type": "Large Text Display", + "properties": { + "topic": "/SmartDashboard/FMSUtil/Field State", + "period": 0.06 + } } ] } @@ -1237,7 +1268,7 @@ "properties": { "topic": "/SmartDashboard/Field", "period": 0.06, - "field_game": "Rebuilt (No Fuel)", + "field_game": "Rebuilt", "robot_width": 0.85, "robot_length": 0.85, "show_other_objects": true, @@ -1256,9 +1287,8 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Handoff/At Tolerance?", + "topic": "/Robot/Handoff/At Tolerance?", "period": 0.06, - "data_type": "boolean", "true_color": 4283215696, "false_color": 4294198070, "true_icon": "None", @@ -1273,7 +1303,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 @@ -1287,7 +1317,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, @@ -1304,7 +1334,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 @@ -1318,7 +1348,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 @@ -1332,7 +1362,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 @@ -1346,7 +1376,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 @@ -1360,7 +1390,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, @@ -1377,7 +1407,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, @@ -1394,7 +1424,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, @@ -1411,9 +1441,8 @@ "height": 70.0, "type": "Boolean Box", "properties": { - "topic": "/SmartDashboard/Spindexer/At Tolerance", + "topic": "/Robot/Spindexer/At Tolerance", "period": 0.06, - "data_type": "boolean", "true_color": 4283215696, "false_color": 4294198070, "true_icon": "None", @@ -1428,26 +1457,12 @@ "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 } }, - { - "title": "Current Intake Angle (deg)", - "x": 420.0, - "y": 560.0, - "width": 210.0, - "height": 140.0, - "type": "Text Display", - "properties": { - "topic": "/SmartDashboard/Intake/Current Angle (deg)", - "period": 0.5, - "data_type": "double", - "show_submit_button": false - } - }, { "title": "Seed Turret", "x": 980.0, @@ -1476,6 +1491,34 @@ "show_submit_button": false } }, + { + "title": "Seed Intake At Lower Limit", + "x": 700.0, + "y": 420.0, + "width": 280.0, + "height": 140.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Robot/Seed Pivot Encoder at Lower Limit (Deployed)", + "period": 0.5, + "show_type": false, + "maximize_button_space": true + } + }, + { + "title": "Seed Intake At Upper Limit", + "x": 420.0, + "y": 420.0, + "width": 280.0, + "height": 140.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Robot/Seed Pivot Encoder at Upper Limit (Stowed)", + "period": 0.5, + "show_type": false, + "maximize_button_space": true + } + }, { "title": "Current Hood Angle", "x": 0.0, @@ -1491,45 +1534,142 @@ } }, { - "title": "Seed Intake At Lower Limit", - "x": 700.0, + "title": "Seed Hood At Lower Limit", + "x": 0.0, "y": 420.0, "width": 280.0, "height": 140.0, "type": "Command", "properties": { - "topic": "/SmartDashboard/Robot/Seed Pivot Encoder at Lower Limit (Deployed)", + "topic": "/SmartDashboard/Robot/Seed Hood Relative Encoder At Lower Hardstop", "period": 0.5, "show_type": false, "maximize_button_space": true } }, { - "title": "Seed Intake At Upper Limit", - "x": 420.0, + "title": "Pipeline back", + "x": 1260.0, + "y": 560.0, + "width": 140.0, + "height": 70.0, + "type": "Text Display", + "properties": { + "topic": "/limelight-back/getpipe", + "period": 0.5, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Pipeline right", + "x": 1260.0, + "y": 490.0, + "width": 140.0, + "height": 70.0, + "type": "Text Display", + "properties": { + "topic": "/limelight-right/getpipe", + "period": 0.5, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Pipeline Left", + "x": 1260.0, "y": 420.0, + "width": 140.0, + "height": 70.0, + "type": "Text Display", + "properties": { + "topic": "/limelight-left/getpipe", + "period": 0.5, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "HDR Enabled?", + "x": 1260.0, + "y": 630.0, + "width": 140.0, + "height": 140.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Vision/HDR Enabled?", + "period": 0.5, + "data_type": "boolean" + } + }, + { + "title": "Set Pipeline High Sun", + "x": 0.0, + "y": 700.0, "width": 280.0, "height": 140.0, "type": "Command", "properties": { - "topic": "/SmartDashboard/Robot/Seed Pivot Encoder at Upper Limit (Stowed)", + "topic": "/SmartDashboard/Robot/Set Pipeline High Sun", "period": 0.5, - "show_type": false, + "show_type": true, "maximize_button_space": true } }, { - "title": "Seed Hood Relative Encoder At Lower Hardstop", - "x": 0.0, - "y": 420.0, + "title": "Set Pipeline Med Sun", + "x": 280.0, + "y": 700.0, "width": 280.0, "height": 140.0, "type": "Command", "properties": { - "topic": "/SmartDashboard/Robot/Seed Hood Relative Encoder At Lower Hardstop", + "topic": "/SmartDashboard/Robot/Set Pipeline Med Sun", + "period": 0.5, + "show_type": true, + "maximize_button_space": true + } + }, + { + "title": "Set Pipeline Low Sun", + "x": 560.0, + "y": 700.0, + "width": 280.0, + "height": 140.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Robot/Set Pipeline Low Sun", + "period": 0.5, + "show_type": true, + "maximize_button_space": true + } + }, + { + "title": "Set Pipeline No Sun", + "x": 840.0, + "y": 700.0, + "width": 280.0, + "height": 140.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Robot/Set Pipeline No Sun", "period": 0.5, "show_type": true, - "maximize_button_space": false + "maximize_button_space": true + } + }, + { + "title": "Current Intake Angle ", + "x": 700.0, + "y": 560.0, + "width": 210.0, + "height": 140.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Intake/Current Angle (deg)", + "period": 0.5, + "data_type": "double", + "show_submit_button": false } } ] diff --git a/simgui-ds.json b/simgui-ds.json index ff81e307..3cddee95 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -99,6 +99,7 @@ } ], "robotJoysticks": [ + {}, { "guid": "Keyboard0" } diff --git a/simgui.json b/simgui.json index 9aec20cb..5bbb6609 100644 --- a/simgui.json +++ b/simgui.json @@ -31,10 +31,20 @@ "/SmartDashboard/Robot/Seed Pivot Encoder at Upper Limit (Stowed)": "Command", "/SmartDashboard/Robot/Seed Turret": "Command", "/SmartDashboard/Robot/Set Back LL PF": "Command", + "/SmartDashboard/Robot/Set Exposure 100": "Command", + "/SmartDashboard/Robot/Set Exposure 130": "Command", + "/SmartDashboard/Robot/Set Exposure 25": "Command", + "/SmartDashboard/Robot/Set Exposure 80": "Command", "/SmartDashboard/Robot/Set Left LL PF": "Command", "/SmartDashboard/Robot/Set Megatag 1": "Command", "/SmartDashboard/Robot/Set Megatag 2": "Command", + "/SmartDashboard/Robot/Set Pipeline High Sun": "Command", + "/SmartDashboard/Robot/Set Pipeline Low Sun": "Command", + "/SmartDashboard/Robot/Set Pipeline Med Sun": "Command", + "/SmartDashboard/Robot/Set Pipeline No Sun": "Command", "/SmartDashboard/Robot/Set Right LL PF": "Command", + "/SmartDashboard/Robot/Set Robot Pose Left Corner": "Command", + "/SmartDashboard/Robot/Set Robot Pose Right Corner": "Command", "/SmartDashboard/Robot/Spindexer Reverse": "Command", "/SmartDashboard/Robot/WL Outpost Tags Left-Camera": "Command", "/SmartDashboard/Robot/WL Tower Tags Right-Camera": "Command", diff --git a/src/main/deploy/pathplanner/autos/Depot Auto.auto b/src/main/deploy/pathplanner/autos/Depot Auto.auto deleted file mode 100644 index 03a0af76..00000000 --- a/src/main/deploy/pathplanner/autos/Depot Auto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Left Bump To Depot" - } - }, - { - "type": "path", - "data": { - "pathName": "Depot To Tower Left" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Box Test.auto b/src/main/deploy/pathplanner/autos/Left Middy.auto similarity index 70% rename from src/main/deploy/pathplanner/autos/Box Test.auto rename to src/main/deploy/pathplanner/autos/Left Middy.auto index 34e6aefd..427c8f10 100644 --- a/src/main/deploy/pathplanner/autos/Box Test.auto +++ b/src/main/deploy/pathplanner/autos/Left Middy.auto @@ -7,31 +7,31 @@ { "type": "path", "data": { - "pathName": "Box 1" + "pathName": "Left Bump To NZ" } }, { "type": "path", "data": { - "pathName": "Box 2" + "pathName": "Left Middy To Bump Score" } }, { "type": "path", "data": { - "pathName": "Box 3" + "pathName": "Left Bump Score To Depot" } }, { "type": "path", "data": { - "pathName": "Box 4" + "pathName": "Depot To Score" } } ] } }, "resetOdom": true, - "folder": "Tests", + "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right Middy.auto b/src/main/deploy/pathplanner/autos/Right Middy.auto new file mode 100644 index 00000000..b1b5803c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right Middy.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Bump To NZ" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Middy To Bump Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Bump Score To Depot" + } + }, + { + "type": "path", + "data": { + "pathName": "Depot To Score" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Straight Line Test.auto b/src/main/deploy/pathplanner/autos/Straight Line Test.auto deleted file mode 100644 index 92bbd21a..00000000 --- a/src/main/deploy/pathplanner/autos/Straight Line Test.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Straight Line" - } - } - ] - } - }, - "resetOdom": true, - "folder": "Tests", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Box 1.path b/src/main/deploy/pathplanner/paths/Box 1.path deleted file mode 100644 index 25f93397..00000000 --- a/src/main/deploy/pathplanner/paths/Box 1.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.0, - "y": 2.0 - }, - "prevControl": null, - "nextControl": { - "x": 3.0000000000000018, - "y": 2.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.0, - "y": 2.0 - }, - "prevControl": { - "x": 2.0, - "y": 2.0 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.16, - "maxAcceleration": 10.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 90.0 - }, - "reversed": false, - "folder": "Tests", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Box 2.path b/src/main/deploy/pathplanner/paths/Box 2.path deleted file mode 100644 index 8fc9e120..00000000 --- a/src/main/deploy/pathplanner/paths/Box 2.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.0, - "y": 2.0 - }, - "prevControl": null, - "nextControl": { - "x": 3.003409325748791, - "y": 1.720136381756153 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.0, - "y": 1.0 - }, - "prevControl": { - "x": 3.0045204355743644, - "y": 1.6922222258449668 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.16, - "maxAcceleration": 10.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": "Tests", - "idealStartingState": { - "velocity": 0, - "rotation": 90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Box 3.path b/src/main/deploy/pathplanner/paths/Box 3.path deleted file mode 100644 index f82e9f22..00000000 --- a/src/main/deploy/pathplanner/paths/Box 3.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.0, - "y": 1.0 - }, - "prevControl": null, - "nextControl": { - "x": 1.77619604200323, - "y": 0.9977414835927593 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.0, - "y": 1.0 - }, - "prevControl": { - "x": 3.0717077172960874, - "y": 0.9943971527224802 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.16, - "maxAcceleration": 10.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -90.0 - }, - "reversed": false, - "folder": "Tests", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score (B).path b/src/main/deploy/pathplanner/paths/Depot To Score.path similarity index 53% rename from src/main/deploy/pathplanner/paths/Right NZ To Score (B).path rename to src/main/deploy/pathplanner/paths/Depot To Score.path index ee55b03c..d3c9f452 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score (B).path +++ b/src/main/deploy/pathplanner/paths/Depot To Score.path @@ -3,48 +3,48 @@ "waypoints": [ { "anchor": { - "x": 8.56758915834522, - "y": 3.5498002853067057 + "x": 0.5795133587786261, + "y": 5.959713740458016 }, "prevControl": null, "nextControl": { - "x": 5.639047234674071, - "y": 2.8277371590830254 + "x": 1.617398935069807, + "y": 5.933846094852308 }, "isLocked": false, - "linkedName": "Right Center NZ" + "linkedName": "Depot" }, { "anchor": { - "x": 2.911921708185054, - "y": 2.491055753262159 + "x": 2.7074970344009497, + "y": 5.959713740458016 }, "prevControl": { - "x": 4.8270581257414005, - "y": 2.5771293001186257 + "x": 1.856554697887364, + "y": 5.954351642472394 }, "nextControl": null, "isLocked": false, - "linkedName": "Right Bump Score" + "linkedName": "Depot Out" } ], "rotationTargets": [ { - "waypointRelativePos": 0.3516873889875677, + "waypointRelativePos": 0.3262260127931784, "rotationDegrees": 180.0 } ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.42138765117759275, - "maxWaypointRelativePos": 0.8338637810311925, + "minWaypointRelativePos": 0.14848143982002254, + "maxWaypointRelativePos": 0.9448818897637792, "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 5.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, + "maxVelocity": 0.5, + "maxAcceleration": 0.75, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0, + "nominalVoltage": 12.5, "unlimited": false } } @@ -52,22 +52,22 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.16, + "maxVelocity": 4.19, "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 180.0 + "rotation": 0.0 }, "reversed": false, - "folder": "Right Bump", + "folder": "To Score", "idealStartingState": { "velocity": 0, - "rotation": 90.0 + "rotation": 180.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Depot To Tower Left.path b/src/main/deploy/pathplanner/paths/Depot To Tower Left.path deleted file mode 100644 index 451bc3cc..00000000 --- a/src/main/deploy/pathplanner/paths/Depot To Tower Left.path +++ /dev/null @@ -1,59 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 0.7278054567022547, - "y": 5.9339976275207595 - }, - "prevControl": null, - "nextControl": { - "x": 3.869489916963227, - "y": 6.149181494661922 - }, - "isLocked": false, - "linkedName": "Depot Collect" - }, - { - "anchor": { - "x": 1.2227283511269282, - "y": 4.922633451957295 - }, - "prevControl": { - "x": 3.643546856465006, - "y": 4.83655990510083 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Tower Left" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.06336939721792927, - "rotationDegrees": -90.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.16, - "maxAcceleration": 10.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "Misc", - "idealStartingState": { - "velocity": 0, - "rotation": -90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score (B).path b/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path similarity index 56% rename from src/main/deploy/pathplanner/paths/Left NZ To Score (B).path rename to src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path index 9cd4a964..01589c7e 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score (B).path +++ b/src/main/deploy/pathplanner/paths/Left Bump Score To Depot.path @@ -3,48 +3,48 @@ "waypoints": [ { "anchor": { - "x": 8.27, - "y": 4.61077032810271 + "x": 3.4749522900763368, + "y": 5.507824427480916 }, "prevControl": null, "nextControl": { - "x": 5.515784635743212, - "y": 5.167086622567657 + "x": 0.7207369258195486, + "y": 6.064140721945863 }, "isLocked": false, - "linkedName": "NZ Left Center" + "linkedName": "Left Bump Score" }, { "anchor": { - "x": 2.8904033214709375, - "y": 5.643499406880189 + "x": 0.5795133587786261, + "y": 5.959713740458016 }, "prevControl": { - "x": 4.644151838671412, - "y": 5.600462633451957 + "x": 1.4665553435114504, + "y": 5.959713740458016 }, "nextControl": null, "isLocked": false, - "linkedName": "Left Bump Score" + "linkedName": "Depot" } ], "rotationTargets": [ { - "waypointRelativePos": 0.3516873889875677, + "waypointRelativePos": 0.42984014209591426, "rotationDegrees": 180.0 } ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.42902609802673963, - "maxWaypointRelativePos": 0.846594525779758, + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.5016872890888694, "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 5.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, + "maxVelocity": 0.5, + "maxAcceleration": 0.75, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0, + "nominalVoltage": 12.5, "unlimited": false } } @@ -52,11 +52,11 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.16, + "maxVelocity": 4.19, "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { @@ -64,10 +64,10 @@ "rotation": 180.0 }, "reversed": false, - "folder": "Left Bump", + "folder": "To Depot", "idealStartingState": { "velocity": 0, - "rotation": -90.0 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Bump To Depot.path b/src/main/deploy/pathplanner/paths/Left Bump To Depot.path deleted file mode 100644 index b1438a51..00000000 --- a/src/main/deploy/pathplanner/paths/Left Bump To Depot.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.589750889679717, - "y": 5.9339976275207595 - }, - "prevControl": null, - "nextControl": { - "x": 3.8521567589767303, - "y": 5.92251805970181 - }, - "isLocked": false, - "linkedName": "Left Bump Start" - }, - { - "anchor": { - "x": 0.7278054567022547, - "y": 5.9339976275207595 - }, - "prevControl": { - "x": 1.6779675215686134, - "y": 5.934033465612401 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Depot Collect" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.16, - "maxAcceleration": 10.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -90.0 - }, - "reversed": false, - "folder": "Misc", - "idealStartingState": { - "velocity": 0, - "rotation": -90.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 Bump To NZ.path new file mode 100644 index 00000000..ea3c532e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Bump To NZ.path @@ -0,0 +1,84 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.500057251908398, + "y": 5.122881679389313 + }, + "prevControl": null, + "nextControl": { + "x": 6.378759541984733, + "y": 5.315353053435115 + }, + "isLocked": false, + "linkedName": "Left Bump Start" + }, + { + "anchor": { + "x": 6.110973282442748, + "y": 4.562204198473283 + }, + "prevControl": { + "x": 5.914394099487028, + "y": 5.110601060796104 + }, + "nextControl": { + "x": 6.354700620229008, + "y": 3.8822781488549625 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.144475190839694, + "y": 4.026631679389314 + }, + "prevControl": { + "x": 7.03148854961832, + "y": 3.951316793893131 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Middy" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.6600517687661842, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 1.75, + "maxAcceleration": 7.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": "To NZ", + "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 Trench.path b/src/main/deploy/pathplanner/paths/Left Bump To Trench.path deleted file mode 100644 index aa0da23f..00000000 --- a/src/main/deploy/pathplanner/paths/Left Bump To Trench.path +++ /dev/null @@ -1,59 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.8904033214709375, - "y": 5.643499406880189 - }, - "prevControl": null, - "nextControl": { - "x": 2.7290154211150655, - "y": 6.7732147093712936 - }, - "isLocked": false, - "linkedName": "Left Bump Score" - }, - { - "anchor": { - "x": 4.31061684460261, - "y": 7.408007117437722 - }, - "prevControl": { - "x": 2.57838671411625, - "y": 7.451043890865957 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Left Trench (B)" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.49378330373001517, - "rotationDegrees": 45.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 0.5, - "maxAcceleration": 1.5, - "maxAngularVelocity": 75.0, - "maxAngularAcceleration": 300.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "Left Bump", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Straight Line.path b/src/main/deploy/pathplanner/paths/Left Middy To Bump Score.path similarity index 64% rename from src/main/deploy/pathplanner/paths/Straight Line.path rename to src/main/deploy/pathplanner/paths/Left Middy To Bump Score.path index 8b505063..7a194703 100644 --- a/src/main/deploy/pathplanner/paths/Straight Line.path +++ b/src/main/deploy/pathplanner/paths/Left Middy To Bump Score.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 2.0, - "y": 7.0 + "x": 8.144475190839694, + "y": 4.026631679389314 }, "prevControl": null, "nextControl": { - "x": 3.0, - "y": 7.0 + "x": 4.997986641221376, + "y": 5.876030534351146 }, "isLocked": false, - "linkedName": null + "linkedName": "Middy" }, { "anchor": { - "x": 4.48276393831554, - "y": 7.0 + "x": 3.4749522900763368, + "y": 5.507824427480916 }, "prevControl": { - "x": 3.48276393831554, - "y": 7.0 + "x": 5.516822519083972, + "y": 5.616612595419848 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Left Bump Score" } ], "rotationTargets": [], @@ -33,11 +33,11 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.16, + "maxVelocity": 4.19, "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { @@ -45,7 +45,7 @@ "rotation": 0.0 }, "reversed": false, - "folder": "Tests", + "folder": "To Score", "idealStartingState": { "velocity": 0, "rotation": 0.0 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 183615ca..52c62217 100644 --- a/src/main/deploy/pathplanner/paths/Left NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -3,25 +3,41 @@ "waypoints": [ { "anchor": { - "x": 8.27, - "y": 4.61077032810271 + "x": 8.248481613285884, + "y": 4.621376037959667 }, "prevControl": null, "nextControl": { - "x": 7.325477888730385, - "y": 7.5996005706134095 + "x": 5.7738671411625155, + "y": 4.589098457888493 }, "isLocked": false, - "linkedName": "NZ Left Center" + "linkedName": "Left NZ" }, { "anchor": { - "x": 3.6120827389443653, - "y": 7.509029957203994 + "x": 6.052395038167939, + "y": 6.378129770992366 }, "prevControl": { - "x": 7.428987161198288, - "y": 7.5996005706134095 + "x": 6.055976129616249, + "y": 5.761441920527078 + }, + "nextControl": { + "x": 6.044550641940085, + "y": 7.7289871611982885 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.63796005706134, + "y": 7.440906488549619 + }, + "prevControl": { + "x": 6.018673323823109, + "y": 7.444336661911555 }, "nextControl": null, "isLocked": false, @@ -30,24 +46,28 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.8063943161634181, + "waypointRelativePos": 0.5, "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.3432835820895521, + "rotationDegrees": 0.0 } ], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.16, + "maxVelocity": 4.19, "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -90.0 + "rotation": 0.0 }, "reversed": false, "folder": "To Score", diff --git a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path index 8c5c7fa3..4bd6d40f 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Left Score To NZ (F).path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 3.6250213980028523, - "y": 7.483152639087019 + "x": 3.63796005706134, + "y": 7.440906488549619 }, "prevControl": null, "nextControl": { - "x": 6.833808844507845, - "y": 7.483152639087019 + "x": 6.846747503566332, + "y": 7.440906488549619 }, "isLocked": false, - "linkedName": "Left Score (F)" + "linkedName": "Left Trench Score" }, { "anchor": { - "x": 8.27, - "y": 4.61077032810271 + "x": 8.248481613285884, + "y": 4.621376037959667 }, "prevControl": { - "x": 7.881840228245363, - "y": 7.185563480741797 + "x": 7.860321841531247, + "y": 7.196169190598754 }, "nextControl": null, "isLocked": false, - "linkedName": "NZ Left Center" + "linkedName": "Left NZ" } ], "rotationTargets": [ @@ -38,11 +38,11 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.16, + "maxVelocity": 4.19, "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { 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 38f52769..8a3f8436 100644 --- a/src/main/deploy/pathplanner/paths/Left Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Left Score To Score.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.6120827389443653, - "y": 7.509029957203994 + "x": 3.63796005706134, + "y": 7.440906488549619 }, "prevControl": null, "nextControl": { - "x": 6.92437945791726, - "y": 7.651355206847361 + "x": 6.717360912981454, + "y": 7.521968616262482 }, "isLocked": false, "linkedName": "Left Trench Score" @@ -32,65 +32,73 @@ }, { "anchor": { - "x": 6.769115549215405, - "y": 4.481383737517832 + "x": 6.937318116975749, + "y": 4.571954350927247 }, "prevControl": { - "x": 6.398810472352867, - "y": 4.205129596104213 + "x": 6.567013040113212, + "y": 4.295700209513629 }, "nextControl": { - "x": 7.410830063131713, - "y": 4.960114122681885 + "x": 7.5790326308920575, + "y": 5.0506847360913 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.769115549215405, - "y": 7.392582025677603 + "x": 7.0667047075606275, + "y": 7.440906488549619 }, "prevControl": { - "x": 7.066884105261373, - "y": 7.384797226826728 + "x": 7.3644732636065955, + "y": 7.433121689698743 }, "nextControl": { - "x": 4.789500713266761, - "y": 7.444336661911555 + "x": 5.087089871611983, + "y": 7.49266112478357 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.6250213980028523, - "y": 7.483152639087019 + "x": 3.63796005706134, + "y": 7.440906488549619 }, "prevControl": { - "x": 5.281169757489301, - "y": 7.4418502292099244 + "x": 5.294108416547789, + "y": 7.399604078672524 }, "nextControl": null, "isLocked": false, - "linkedName": "Left Score (F)" + "linkedName": "Left Trench Score" } ], "rotationTargets": [ { - "waypointRelativePos": 0.8911495422177071, + "waypointRelativePos": 0.1891117478510029, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.6340248962655519, "rotationDegrees": -90.0 }, { - "waypointRelativePos": 2.2942430703624668, + "waypointRelativePos": 0.9, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.3, "rotationDegrees": 90.0 }, { - "waypointRelativePos": 2.6268656716418013, + "waypointRelativePos": 2.5, "rotationDegrees": 90.0 }, { - "waypointRelativePos": 3.1047812817904377, + "waypointRelativePos": 3.2, "rotationDegrees": 0.0 } ], @@ -99,7 +107,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.5, - "maxAcceleration": 7.5, + "maxAcceleration": 7.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, "nominalVoltage": 12.0, @@ -113,7 +121,7 @@ "folder": "To Score", "idealStartingState": { "velocity": 0.0, - "rotation": -90.0 + "rotation": 0.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Trench To NZ (B).path b/src/main/deploy/pathplanner/paths/Left Trench To NZ (B).path deleted file mode 100644 index 60cdf558..00000000 --- a/src/main/deploy/pathplanner/paths/Left Trench To NZ (B).path +++ /dev/null @@ -1,103 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 4.31061684460261, - "y": 7.408007117437722 - }, - "prevControl": null, - "nextControl": { - "x": 6.462455516014234, - "y": 7.612431791221827 - }, - "isLocked": false, - "linkedName": "Left Trench (B)" - }, - { - "anchor": { - "x": 6.064365361803085, - "y": 5.944756820877817 - }, - "prevControl": { - "x": 6.064365361803085, - "y": 7.378315878190629 - }, - "nextControl": { - "x": 6.064365361803085, - "y": 3.9005100830367745 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.064365361803085, - "y": 4.793523131672598 - }, - "prevControl": { - "x": 6.456698309590022, - "y": 3.6435817329867413 - }, - "nextControl": { - "x": 5.7523487544484, - "y": 5.708054567022539 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.8904033214709375, - "y": 5.643499406880189 - }, - "prevControl": { - "x": 6.2903084223013055, - "y": 5.632740213523132 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Left Bump Score" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 0.9005328596802835, - "rotationDegrees": -90.0 - }, - { - "waypointRelativePos": 1.641207815275316, - "rotationDegrees": -90.0 - }, - { - "waypointRelativePos": 2.26998223801069, - "rotationDegrees": 180.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.16, - "maxAcceleration": 10.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": "Left Bump", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path index 2edab82d..3568773d 100644 --- a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path @@ -3,48 +3,48 @@ "waypoints": [ { "anchor": { - "x": 3.998600237247925, - "y": 7.526358244365362 + "x": 4.445677480916031, + "y": 7.675219465648855 }, "prevControl": null, "nextControl": { - "x": 8.99456490727532, - "y": 7.509029957203994 + "x": 8.517461447212337, + "y": 7.70926453143535 }, "isLocked": false, "linkedName": "Left Trench Start" }, { "anchor": { - "x": 8.27, - "y": 4.61077032810271 + "x": 8.248481613285884, + "y": 4.621376037959667 }, "prevControl": { - "x": 8.218245363766048, - "y": 7.431398002853067 + "x": 8.196726977051933, + "y": 7.442003712710024 }, "nextControl": null, "isLocked": false, - "linkedName": "NZ Left Center" + "linkedName": "Left NZ" } ], "rotationTargets": [ { - "waypointRelativePos": 0.05, + "waypointRelativePos": 0.140401146131807, "rotationDegrees": -90.0 } ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.629304523970286, + "minWaypointRelativePos": 0.55, "maxWaypointRelativePos": 1.0, "constraints": { - "maxVelocity": 2.0, + "maxVelocity": 1.5, "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false } } @@ -52,11 +52,11 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.16, + "maxVelocity": 4.19, "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { @@ -67,7 +67,7 @@ "folder": "To NZ", "idealStartingState": { "velocity": 0, - "rotation": -76.49029381374567 + "rotation": -90.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ (B).path b/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path similarity index 54% rename from src/main/deploy/pathplanner/paths/Right Trench To NZ (B).path rename to src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path index 329b195e..70b3af07 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ (B).path +++ b/src/main/deploy/pathplanner/paths/Right Bump Score To Depot.path @@ -3,52 +3,48 @@ "waypoints": [ { "anchor": { - "x": 4.051997146932953, - "y": 0.6256633380884444 + "x": 3.4749522900763368, + "y": 2.486860687022901 }, "prevControl": null, "nextControl": { - "x": 6.784832259625718, - "y": 0.5611081779460949 + "x": 1.8598664122137403, + "y": 4.838358778625954 }, "isLocked": false, - "linkedName": "Right Trench (B)" + "linkedName": "Right Bump Score" }, { "anchor": { - "x": 7.092582025677602, - "y": 1.531369472182596 + "x": 1.8933396946564884, + "y": 4.938778625954199 }, "prevControl": { - "x": 7.054174783837401, - "y": 0.732498841906393 + "x": 1.9041990007255656, + "y": 4.689014586365434 }, "nextControl": { - "x": 7.157275320970041, - "y": 2.8769900142653353 + "x": 1.8369555219739746, + "y": 6.235614597651982 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.911921708185054, - "y": 2.491055753262159 + "x": 0.5795133587786261, + "y": 5.959713740458016 }, "prevControl": { - "x": 5.2897034400948995, - "y": 2.437259786476868 + "x": 1.684131679389313, + "y": 5.859293893129771 }, "nextControl": null, "isLocked": false, - "linkedName": "Right Bump Score" + "linkedName": "Depot" } ], "rotationTargets": [ - { - "waypointRelativePos": 0.12255772646536273, - "rotationDegrees": 0.0 - }, { "waypointRelativePos": 1.6, "rotationDegrees": 180.0 @@ -57,14 +53,14 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 1.5964353914703855, - "maxWaypointRelativePos": 1.866327180140039, + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 1.65, "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 5.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, + "maxVelocity": 0.75, + "maxAcceleration": 1.0, + "maxAngularVelocity": 50.0, + "maxAngularAcceleration": 100.0, + "nominalVoltage": 12.5, "unlimited": false } } @@ -72,11 +68,11 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.16, + "maxVelocity": 4.19, "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { @@ -84,7 +80,7 @@ "rotation": 180.0 }, "reversed": false, - "folder": "Right Bump", + "folder": "To Depot", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/Right Bump To NZ.path b/src/main/deploy/pathplanner/paths/Right Bump To NZ.path new file mode 100644 index 00000000..e249ebb2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Bump To NZ.path @@ -0,0 +1,84 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.4833206106870227, + "y": 2.9220133587786257 + }, + "prevControl": null, + "nextControl": { + "x": 5.4749809160305345, + "y": 2.813225190839695 + }, + "isLocked": false, + "linkedName": "Right Bump Start" + }, + { + "anchor": { + "x": 6.336917938931298, + "y": 3.4492175572519077 + }, + "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 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Middy" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.6496980155306307, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 1.75, + "maxAcceleration": 7.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": "To NZ", + "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 Trench.path b/src/main/deploy/pathplanner/paths/Right Bump To Trench.path deleted file mode 100644 index 80d24be7..00000000 --- a/src/main/deploy/pathplanner/paths/Right Bump To Trench.path +++ /dev/null @@ -1,59 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.911921708185054, - "y": 2.491055753262159 - }, - "prevControl": null, - "nextControl": { - "x": 2.567627520759194, - "y": 1.2752669039145905 - }, - "isLocked": false, - "linkedName": "Right Bump Score" - }, - { - "anchor": { - "x": 4.051997146932953, - "y": 0.6256633380884444 - }, - "prevControl": { - "x": 2.4087874465049923, - "y": 0.8973751783166917 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Right Trench (B)" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.49378330373001517, - "rotationDegrees": -45.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 75.0, - "maxAngularAcceleration": 300.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "Right Bump", - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Box 4.path b/src/main/deploy/pathplanner/paths/Right Middy To Bump Score.path similarity index 62% rename from src/main/deploy/pathplanner/paths/Box 4.path rename to src/main/deploy/pathplanner/paths/Right Middy To Bump Score.path index 7ea9100d..b9e05c6d 100644 --- a/src/main/deploy/pathplanner/paths/Box 4.path +++ b/src/main/deploy/pathplanner/paths/Right Middy To Bump Score.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 2.0, - "y": 1.0 + "x": 8.144475190839694, + "y": 4.026631679389314 }, "prevControl": null, "nextControl": { - "x": 2.0299091229524393, - "y": 1.2482044406617574 + "x": 5.533559160305344, + "y": 2.2023377862595424 }, "isLocked": false, - "linkedName": null + "linkedName": "Middy" }, { "anchor": { - "x": 2.0, - "y": 2.0 + "x": 3.4749522900763368, + "y": 2.486860687022901 }, "prevControl": { - "x": 2.0255951320775263, - "y": 1.751313673045875 + "x": 5.223931297709925, + "y": 2.5119656488549618 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Right Bump Score" } ], "rotationTargets": [], @@ -33,11 +33,11 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.16, + "maxVelocity": 4.19, "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { @@ -45,10 +45,10 @@ "rotation": 0.0 }, "reversed": false, - "folder": "Tests", + "folder": "To Score", "idealStartingState": { "velocity": 0, - "rotation": -90.0 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score.path b/src/main/deploy/pathplanner/paths/Right NZ To Score.path index 22c81e32..c569aff8 100644 --- a/src/main/deploy/pathplanner/paths/Right NZ To Score.path +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score.path @@ -3,25 +3,41 @@ "waypoints": [ { "anchor": { - "x": 8.56758915834522, - "y": 3.5498002853067057 + "x": 8.237722419928826, + "y": 3.4271055753262156 }, "prevControl": null, "nextControl": { - "x": 7.351355206847359, - "y": 0.43158345221112837 + "x": 5.763107947805457, + "y": 3.5131791221826814 }, "isLocked": false, - "linkedName": "Right Center NZ" + "linkedName": "Right NZ" }, { "anchor": { - "x": 3.5473894436519258, - "y": 0.5350927246790309 + "x": 6.053851272542522, + "y": 1.6151808747904899 }, "prevControl": { - "x": 6.885563480741796, - "y": 0.4962767475035673 + "x": 6.040480921648686, + "y": 2.6846376869648685 + }, + "nextControl": { + "x": 6.070427960057061, + "y": 0.289258202567761 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6120827389443653, + "y": 0.5868473609129818 + }, + "prevControl": { + "x": 6.290385164051354, + "y": 0.6127246790299581 }, "nextControl": null, "isLocked": false, @@ -30,24 +46,28 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.7921847246891675, + "waypointRelativePos": 0.23445825932504563, "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.488272921108742, + "rotationDegrees": 0.0 } ], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.16, + "maxVelocity": 4.19, "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { "velocity": 0.0, - "rotation": 90.0 + "rotation": 0.0 }, "reversed": false, "folder": "To Score", diff --git a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path index 863c4e8b..71ca80d6 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path +++ b/src/main/deploy/pathplanner/paths/Right Score To NZ (F).path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 3.6250213980028523, + "x": 3.6120827389443653, "y": 0.5868473609129818 }, "prevControl": null, "nextControl": { - "x": 7.196091298145506, + "x": 7.183152639087018, "y": 0.5739087018544944 }, "isLocked": false, - "linkedName": "Right Score (F)" + "linkedName": "Right Trench Score" }, { "anchor": { - "x": 8.56758915834522, - "y": 3.5498002853067057 + "x": 8.237722419928826, + "y": 3.4271055753262156 }, "prevControl": { - "x": 8.515834522111266, - "y": 0.819743223965764 + "x": 8.185967783694872, + "y": 0.697048513985274 }, "nextControl": null, "isLocked": false, - "linkedName": "Right Center NZ" + "linkedName": "Right NZ" } ], "rotationTargets": [ @@ -38,11 +38,11 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.16, + "maxVelocity": 4.19, "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { 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 8adafc11..d0f23ac2 100644 --- a/src/main/deploy/pathplanner/paths/Right Score To Score.path +++ b/src/main/deploy/pathplanner/paths/Right Score To Score.path @@ -3,28 +3,28 @@ "waypoints": [ { "anchor": { - "x": 3.5473894436519258, - "y": 0.5350927246790309 + "x": 3.6120827389443653, + "y": 0.5868473609129818 }, "prevControl": null, "nextControl": { - "x": 6.833808844507845, - "y": 0.37982881597717666 + "x": 6.575035663338087, + "y": 0.6515406562054205 }, "isLocked": false, "linkedName": "Right Trench Score" }, { "anchor": { - "x": 5.928102710413694, + "x": 5.850470756062768, "y": 2.851112696148359 }, "prevControl": { - "x": 5.928102710413694, + "x": 5.850470756062768, "y": 0.3280741797432247 }, "nextControl": { - "x": 5.928102710413694, + "x": 5.850470756062768, "y": 4.150659142168011 }, "isLocked": false, @@ -32,41 +32,49 @@ }, { "anchor": { - "x": 6.9502567760342355, + "x": 7.302881844380403, "y": 2.851112696148359 }, "prevControl": { - "x": 6.923667038863391, - "y": 3.8349329714696054 + "x": 7.215816731986725, + "y": 3.83143356936277 }, "nextControl": { - "x": 7.014950071326675, - "y": 0.45746077032810284 + "x": 7.525057636887608, + "y": 0.34949567723342856 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.6250213980028523, + "x": 3.6120827389443653, "y": 0.5868473609129818 }, "prevControl": { - "x": 6.989072753209699, - "y": 0.4962767475035662 + "x": 7.673089129599265, + "y": 0.5487960706826538 }, "nextControl": null, "isLocked": false, - "linkedName": "Right Score (F)" + "linkedName": "Right Trench Score" } ], "rotationTargets": [ { - "waypointRelativePos": 0.9722814498933927, + "waypointRelativePos": 0.17051509769094172, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.644760213143872, "rotationDegrees": 90.0 }, { - "waypointRelativePos": 1.9829424307036287, + "waypointRelativePos": 0.9, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.05, "rotationDegrees": -90.0 }, { @@ -97,7 +105,7 @@ "folder": "To Score", "idealStartingState": { "velocity": 0, - "rotation": 90.0 + "rotation": 0.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ (F).path b/src/main/deploy/pathplanner/paths/Right Trench To NZ (F).path deleted file mode 100644 index 900466c6..00000000 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ (F).path +++ /dev/null @@ -1,59 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 4.051997146932953, - "y": 0.6256633380884444 - }, - "prevControl": null, - "nextControl": { - "x": 6.9354609666245315, - "y": 0.6364225314455019 - }, - "isLocked": false, - "linkedName": "Right Trench (B)" - }, - { - "anchor": { - "x": 8.218245363766048, - "y": 3.0840085592011417 - }, - "prevControl": { - "x": 7.301672597864768, - "y": 0.6619928825622776 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.21871820956256674, - "rotationDegrees": 0.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.16, - "maxAcceleration": 10.0, - "maxAngularVelocity": 300.0, - "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 93.81407483429037 - }, - "reversed": false, - "folder": "Right Bump", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path index 89fc2c0b..13b73e23 100644 --- a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path +++ b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 4.039058487874465, - "y": 0.5221540656205423 + "x": 4.412204198473283, + "y": 0.3947805343511448 }, "prevControl": null, "nextControl": { - "x": 9.408601997146931, - "y": 0.44452211126961516 + "x": 9.06721902017291, + "y": 0.41484149855907726 }, "isLocked": false, "linkedName": "Right Trench Start" }, { "anchor": { - "x": 8.56758915834522, - "y": 3.5498002853067057 + "x": 8.237722419928826, + "y": 3.4271055753262156 }, "prevControl": { - "x": 8.748730385164048, - "y": -0.5258773181169758 + "x": 8.14623827007292, + "y": 0.4342669586115182 }, "nextControl": null, "isLocked": false, - "linkedName": "Right Center NZ" + "linkedName": "Right NZ" } ], "rotationTargets": [ @@ -37,14 +37,14 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.7346387575962167, + "minWaypointRelativePos": 0.55, "maxWaypointRelativePos": 1.0, "constraints": { - "maxVelocity": 2.0, + "maxVelocity": 1.5, "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.0, + "nominalVoltage": 12.7, "unlimited": false } } @@ -52,11 +52,11 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.16, + "maxVelocity": 4.19, "maxAcceleration": 10.0, "maxAngularVelocity": 300.0, "maxAngularAcceleration": 900.0, - "nominalVoltage": 12.5, + "nominalVoltage": 12.7, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index a5b88d8f..a693d1fb 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -3,21 +3,16 @@ "robotLength": 0.762, "holonomicMode": true, "pathFolders": [ - "Left Bump", - "Misc", - "Right Bump", - "Tests", + "To Depot", "To NZ", "To Score" ], - "autoFolders": [ - "Tests" - ], - "defaultMaxVel": 4.16, + "autoFolders": [], + "defaultMaxVel": 4.19, "defaultMaxAccel": 10.0, "defaultMaxAngVel": 300.0, "defaultMaxAngAccel": 900.0, - "defaultNominalVoltage": 12.5, + "defaultNominalVoltage": 12.7, "robotMass": 64.86, "robotMOI": 6.883, "robotTrackwidth": 0.546, diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 6bceaaef..d2b0eec1 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -5,50 +5,39 @@ /***************************************************************/ package com.stuypulse.robot; +import java.lang.management.GarbageCollectorMXBean; +import java.lang.management.ManagementFactory; +import java.util.List; + +import com.pathplanner.lib.commands.FollowPathCommand; +import com.pathplanner.lib.commands.PathfindingCommand; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureFOTM; import com.stuypulse.robot.commands.swerve.SwerveAutonInit; import com.stuypulse.robot.commands.swerve.SwerveTeleopInit; +import com.stuypulse.robot.commands.vision.BlackListAllTagsForAllCameras; import com.stuypulse.robot.commands.vision.SetMegaTagMode; import com.stuypulse.robot.commands.vision.WhitelistAllTagsForAllCameras; -import com.stuypulse.robot.commands.vision.WhitelistRoutineLeftSideAuto; -import com.stuypulse.robot.commands.vision.WhitelistRoutineRightSideAuto; -import com.stuypulse.robot.constants.Cameras; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.constants.Cameras.Camera; 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.subsystems.vision.LimelightVision; import com.stuypulse.robot.util.EnergyUtil; import com.stuypulse.robot.util.FMSUtil; import com.stuypulse.robot.util.PhoenixUtil; import com.stuypulse.robot.util.superstructure.SOTMCalculator; -import com.stuypulse.stuylib.network.SmartBoolean; -import edu.wpi.first.net.PortForwarder; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.IterativeRobotBase; -import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.Watchdog; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; - -import java.lang.management.GarbageCollectorMXBean; -import java.lang.management.ManagementFactory; -import java.lang.reflect.Field; -import java.util.List; -import java.util.Timer; - -import com.ctre.phoenix6.SignalLogger; -import com.pathplanner.lib.commands.FollowPathCommand; -import com.pathplanner.lib.commands.PathfindingCommand; -import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.spindexer.SpindexerStop; -import com.stuypulse.robot.commands.superstructure.SuperstructureFOTM; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; public class Robot extends TimedRobot { public enum RobotMode { @@ -58,18 +47,14 @@ public enum RobotMode { TEST } - private Timer threadTimer; - private RobotContainer robot; private Command auto; private static Alliance alliance; private static RobotMode mode; private static EnergyUtil energyUtil; private FMSUtil fmsUtil; - private SendableChooser cameras = new SendableChooser(); - private Camera selected; private GcStatsCollector gcStatsCollector; - private SmartBoolean shouldRunSecondThread; + public static boolean fmsAttached; private static int periodicCounter = 0; @@ -100,30 +85,16 @@ public void robotInit() { energyUtil = new EnergyUtil(); fmsUtil = new FMSUtil(true); gcStatsCollector = new GcStatsCollector(); - for (Camera camera : Cameras.LimelightCameras) { - cameras.setDefaultOption(camera.getName(), camera); - } - selected = cameras.getSelected(); - PortForwarder.add(5801, selected + ".local:5801", 5801); - SmartDashboard.putData("Selected Camera",cameras); - - try { - Field watchdogField = IterativeRobotBase.class.getDeclaredField("m_watchdog"); - watchdogField.setAccessible(true); - Watchdog watchdog = (Watchdog) watchdogField.get(this); - watchdog.setTimeout(Settings.LOOP_OVERRUN_WARNING_TIME_SEC); - } catch (Exception e) { - DriverStation.reportError("Failed to disable loop overrun warnings.", e.getStackTrace()); - } DataLogManager.start(); - SignalLogger.start(); + // SignalLogger.start(); CommandScheduler.getInstance().schedule(new SwerveAutonInit()); - FollowPathCommand.warmupCommand().schedule(); - PathfindingCommand.warmupCommand().schedule(); + CommandScheduler.getInstance().schedule(FollowPathCommand.warmupCommand()); + CommandScheduler.getInstance().schedule(PathfindingCommand.warmupCommand()); energyUtil = new EnergyUtil(); CommandScheduler.getInstance().schedule(new SwerveAutonInit()); + RobotController.setBrownoutVoltage(6.3); } @Override @@ -134,11 +105,7 @@ public void robotPeriodic() { if (periodicCounter % 50 == 0) { DataLogManager.getLog().resume(); } - if (cameras.getSelected() != selected) { - PortForwarder.remove(5801); - selected = cameras.getSelected(); - PortForwarder.add(5801, selected + ".local:5801", 5801); - } + periodicCounter++; double batteryVoltage = RobotController.getBatteryVoltage(); @@ -156,23 +123,12 @@ public void robotPeriodic() { SmartDashboard.putData(CommandScheduler.getInstance()); } - if (DriverStation.getAlliance().isPresent()) { - alliance = DriverStation.getAlliance().get(); - } - - if (CommandSwerveDrivetrain.getInstance().isOutsideAllianceZone() && Superstructure.getInstance().getState() == SuperstructureState.SOTM) { - CommandScheduler.getInstance().schedule( - new SuperstructureFOTM(), - new SpindexerStop(), - new HandoffStop() - ); - } SmartDashboard.putNumber("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()); - + robot.periodicAfterScheduler(); energyUtil.periodic(); } @@ -185,23 +141,23 @@ public void robotPeriodic() { public void disabledInit() { mode = RobotMode.DISABLED; - CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG1)); + CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); + + CommandScheduler.getInstance().schedule(new BlackListAllTagsForAllCameras()); + } @Override public void disabledPeriodic() { if (periodicCounter % Settings.LOGGING_FREQUENCY == 0) { auto = robot.getAutonomousCommand(); - switch (auto.getName()) { - case "LeftTwoCycle": - CommandScheduler.getInstance().schedule(new WhitelistRoutineLeftSideAuto()); - break; - case "RightTwoCycle": - CommandScheduler.getInstance().schedule(new WhitelistRoutineRightSideAuto()); - break; - default: - CommandScheduler.getInstance().schedule(new WhitelistAllTagsForAllCameras()); - break; + + if (DriverStation.getAlliance().isPresent()) { + alliance = DriverStation.getAlliance().get(); + } + + if (DriverStation.isFMSAttached()) { + fmsAttached = true; } } } @@ -219,7 +175,7 @@ public void autonomousInit() { auto = robot.getAutonomousCommand(); if (auto != null) { - auto.schedule(); + CommandScheduler.getInstance().schedule(auto); } } @@ -243,6 +199,7 @@ public void teleopInit() { fmsUtil.restartTimer(false); CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); CommandScheduler.getInstance().schedule(new WhitelistAllTagsForAllCameras()); + CommandScheduler.getInstance().schedule(new IntakeDeploy()); if (auto != null) { auto.cancel(); @@ -254,6 +211,14 @@ 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()); + + if (CommandSwerveDrivetrain.getInstance().isOutsideAllianceZone() && Superstructure.getInstance().getState() == SuperstructureState.SOTM) { + CommandScheduler.getInstance().schedule( + new SuperstructureFOTM(), + new SpindexerStop(), + new HandoffStop() + ); + } } @Override diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 4bc4b230..605ae076 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -7,8 +7,9 @@ import com.stuypulse.robot.commands.BuzzController; import com.stuypulse.robot.commands.auton.DoNothingAuton; -import com.stuypulse.robot.commands.auton.regular.DepotAuton; +import com.stuypulse.robot.commands.auton.regular.LeftMiddy; import com.stuypulse.robot.commands.auton.regular.LeftTwoCycle; +import com.stuypulse.robot.commands.auton.regular.RightMiddy; import com.stuypulse.robot.commands.auton.regular.RightTwoCycle; import com.stuypulse.robot.commands.handoff.HandoffReverse; import com.stuypulse.robot.commands.handoff.HandoffRun; @@ -17,6 +18,7 @@ import com.stuypulse.robot.commands.hood.HomingRoutineUpper; import com.stuypulse.robot.commands.hood.SeedHoodRelativeEncoderAtLowerHardstop; import com.stuypulse.robot.commands.hood.SeedHoodRelativeEncoderAtUpperHardstop; +import com.stuypulse.robot.commands.intake.IntakeAutoDigest; import com.stuypulse.robot.commands.intake.IntakeDeploy; import com.stuypulse.robot.commands.intake.IntakeOuttake; import com.stuypulse.robot.commands.intake.IntakeRunRollers; @@ -26,35 +28,34 @@ import com.stuypulse.robot.commands.intake.SeedPivotDeployed; import com.stuypulse.robot.commands.intake.SeedPivotStowed; import com.stuypulse.robot.commands.leds.LEDApplyPattern; -import com.stuypulse.robot.commands.leds.LEDDefaultCommand; import com.stuypulse.robot.commands.spindexer.SpindexerReverse; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.superstructure.SuperstructureFOTM; -import com.stuypulse.robot.commands.superstructure.SuperstructureInterpolation; import com.stuypulse.robot.commands.superstructure.SuperstructureKB; import com.stuypulse.robot.commands.superstructure.SuperstructureLeftCorner; import com.stuypulse.robot.commands.superstructure.SuperstructureRightCorner; import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; -import com.stuypulse.robot.commands.superstructure.SuperstructureManualOverride; import com.stuypulse.robot.commands.superstructure.SuperstructureStow; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveDriveFOTM; import com.stuypulse.robot.commands.swerve.SwerveDriveSOTM; import com.stuypulse.robot.commands.swerve.SwerveResetHeading; +import com.stuypulse.robot.commands.swerve.SwerveResetPose; +import com.stuypulse.robot.commands.swerve.SwerveResetPoseKBShot; +import com.stuypulse.robot.commands.swerve.SwerveResetPoseLeftCorner; +import com.stuypulse.robot.commands.swerve.SwerveResetPoseRightCorner; import com.stuypulse.robot.commands.swerve.SwerveXMode; -import com.stuypulse.robot.commands.swerve.pidToPose.SwerveDrivePIDToPose; import com.stuypulse.robot.commands.turret.SeedTurret; import com.stuypulse.robot.commands.turret.ZeroTurret; -import com.stuypulse.robot.commands.vision.EnableBackLimelight; -import com.stuypulse.robot.commands.vision.EnableLeftLimelight; -import com.stuypulse.robot.commands.vision.EnableRightLimelight; import com.stuypulse.robot.commands.vision.ResetLimelightIMU; import com.stuypulse.robot.commands.vision.SetIMUMode; import com.stuypulse.robot.commands.vision.SetMegaTagMode; +import com.stuypulse.robot.commands.vision.SetPipeline; import com.stuypulse.robot.commands.vision.WhitelistAllTagsForAllCameras; import com.stuypulse.robot.commands.vision.WhitelistOutpostTags; import com.stuypulse.robot.commands.vision.WhitelistTowerTags; +import com.stuypulse.robot.constants.Cameras.Camera.Pipeline; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; @@ -62,7 +63,6 @@ import com.stuypulse.robot.subsystems.handoff.Handoff.HandoffState; import com.stuypulse.robot.subsystems.intake.Intake; import com.stuypulse.robot.subsystems.intake.Intake.RollerState; -import com.stuypulse.robot.subsystems.leds.LEDController; import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.spindexer.Spindexer.SpindexerState; import com.stuypulse.robot.subsystems.superstructure.Superstructure; @@ -79,19 +79,14 @@ import com.stuypulse.stuylib.network.SmartBoolean; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RepeatCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.StartEndCommand; 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 { @@ -103,7 +98,7 @@ public interface EnabledSubsystems { SmartBoolean SPINDEXER = new SmartBoolean("Enabled Subsystems/Spindexer Is Enabled", true); SmartBoolean HOOD = new SmartBoolean("Enabled Subsystems/Hood Is Enabled", true); SmartBoolean SHOOTER = new SmartBoolean("Enabled Subsystems/Shooter Is Enabled", true); - SmartBoolean LEDS = new SmartBoolean("Enabled Subsystems/LEDs Is Enabled", true); + SmartBoolean LEDS = new SmartBoolean("Enabled Subsystems/LEDs Is Enabled", false); SmartBoolean BACK_LIMELIGHT = new SmartBoolean("Enabled Subsystems/Back Limelight Is Enabled", true); SmartBoolean LEFT_LIMELIGHT = new SmartBoolean("Enabled Subsystems/Left Limelight Is Enabled", true); @@ -126,7 +121,7 @@ public interface EnabledSubsystems { private final Shooter shooter = Shooter.getInstance(); private final Hood hood = Hood.getInstance(); - private final LEDController leds = LEDController.getInstance(); + // private final LEDController leds = LEDController.getInstance(); // Autons private static SendableChooser autonChooser = new SendableChooser<>(); @@ -149,7 +144,7 @@ public RobotContainer() { private void configureDefaultCommands() { swerve.setDefaultCommand(new SwerveDriveDrive(driver)); - leds.setDefaultCommand(new LEDDefaultCommand()); + // leds.setDefaultCommand(new LEDDefaultCommand()); } /***************/ @@ -158,27 +153,32 @@ private void configureDefaultCommands() { private void configureButtonBindings() { // Scoring Routine (TR) + // driver.getTopButton() + // .whileTrue(new LEDApplyPattern(Settings.LED.SHOOT_IN_PLACE)) + // .whileTrue(new WaitUntilCommand(() -> spindexer.getState() == SpindexerState.FORWARD) + // .andThen(new WaitCommand(0.75).andThen(new IntakeDeploy()))) + // .whileTrue(new SwerveXMode()) + // .whileTrue(new BuzzController(driver).onlyWhile(() -> !vision.hasData()).repeatedly()) + // .whileTrue( + // new SuperstructureInterpolation() + // .andThen(new WaitUntilCommand(superstructure::isReadyToShoot)) + // .andThen( + // Commands.parallel( + // new RunCommand( + // () -> handoff.setState(HandoffState.FORWARD), + // handoff), + // new RunCommand( + // () -> spindexer.setState(SpindexerState.FORWARD), + // spindexer) + // ) + // .repeatedly() + // ) + // ); + + // Digest (TR) driver.getTopButton() - .whileTrue(new LEDApplyPattern(Settings.LED.SHOOT_IN_PLACE)) - .whileTrue(new WaitUntilCommand(() -> spindexer.getState() == SpindexerState.FORWARD) - .andThen(new WaitCommand(0.75).andThen(new IntakeDeploy()))) - .whileTrue(new SwerveXMode()) - .whileTrue(new BuzzController(driver).onlyWhile(() -> !vision.hasData()).repeatedly()) - .whileTrue( - new SuperstructureInterpolation() - .andThen(new WaitUntilCommand(superstructure::isReadyToShoot)) - .andThen( - Commands.parallel( - new RunCommand( - () -> handoff.setState(HandoffState.FORWARD), - handoff), - new RunCommand( - () -> spindexer.setState(SpindexerState.FORWARD), - spindexer) - ) - .repeatedly() - ) - ); + .whileTrue(new IntakeAutoDigest().repeatedly()) + .onFalse(new IntakeDeploy()); // Intake Stow driver.getLeftTriggerButton() @@ -266,6 +266,7 @@ private void configureButtonBindings() { new SuperstructureLeftCorner().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) .andThen(new HandoffRun()) .andThen(new SpindexerRun()), + new SwerveResetPoseLeftCorner(), new SwerveXMode() ) ) @@ -276,6 +277,7 @@ private void configureButtonBindings() { .whileTrue(new LEDApplyPattern(Settings.LED.RIGHT_CORNER)) .whileTrue(new SwerveXMode()) .onTrue(new IntakeRunRollers()) + .onTrue(new SwerveResetPoseRightCorner()) .whileTrue(new SuperstructureRightCorner().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) .andThen(new HandoffRun()).alongWith(new WaitUntilCommand(() -> handoff.getState() == HandoffState.FORWARD) .andThen(new SpindexerRun()))) @@ -286,6 +288,7 @@ private void configureButtonBindings() { .whileTrue(new LEDApplyPattern(Settings.LED.KB_DISTANCE)) .whileTrue(new SwerveXMode()) .onTrue(new IntakeRunRollers()) + .onTrue(new SwerveResetPoseKBShot()) .whileTrue(new SuperstructureKB().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) .andThen(new HandoffRun()).alongWith(new WaitUntilCommand(() -> handoff.getState() == HandoffState.FORWARD) .andThen(new SpindexerRun()))) @@ -313,14 +316,22 @@ private void configureElasticButtons() { SmartDashboard.putData("Robot/Set Megatag 1", new SetMegaTagMode(MegaTagMode.MEGATAG1)); SmartDashboard.putData("Robot/Set Megatag 2", new SetMegaTagMode(MegaTagMode.MEGATAG2)); - SmartDashboard.putData("Robot/Set Left LL PF", new EnableLeftLimelight()); - SmartDashboard.putData("Robot/Set Right LL PF", new EnableRightLimelight()); - SmartDashboard.putData("Robot/Set Back LL PF", new EnableBackLimelight()); + SmartDashboard.putData("Robot/Set Robot Pose Left Corner", new SwerveResetPoseLeftCorner()); + SmartDashboard.putData("Robot/Set Robot Pose Right Corner", new SwerveResetPoseRightCorner()); + + // SmartDashboard.putData("Robot/Set Left LL PF", new EnableLeftLimelight()); + // SmartDashboard.putData("Robot/Set Right LL PF", new EnableRightLimelight()); + // SmartDashboard.putData("Robot/Set Back LL PF", new EnableBackLimelight()); SmartDashboard.putData("Robot/WL Outpost Tags Left-Camera", new WhitelistOutpostTags("limelight-left")); SmartDashboard.putData("Robot/WL Tower Tags Right-Camera", new WhitelistTowerTags("limelight-right")); SmartDashboard.putData("Robot/Whitelist All Cameras", new WhitelistAllTagsForAllCameras()); + SmartDashboard.putData("Robot/Set Pipeline No Sun", new SetPipeline(Pipeline.NO_SUN)); + SmartDashboard.putData("Robot/Set Pipeline Low Sun", new SetPipeline(Pipeline.LOW_SUN)); + SmartDashboard.putData("Robot/Set Pipeline Med Sun", new SetPipeline(Pipeline.MED_SUN)); + SmartDashboard.putData("Robot/Set Pipeline High Sun", new SetPipeline(Pipeline.HIGH_SUN)); + // Unjamming SmartDashboard.putData("Robot/Handoff Reverse", new ConditionalCommand( @@ -348,9 +359,12 @@ public void configureAutons() { autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton()); // DEPOT - AutonConfig DEPOT_AUTON = new AutonConfig("Depot Auton", DepotAuton::new, - "Left Bump To Depot", "Depot To Tower Left"); - DEPOT_AUTON.register(autonChooser); + 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); // TWO CYCLES (TRENCH) AutonConfig LEFT_TWO_CYCLE = new AutonConfig("Left Two Cycle", LeftTwoCycle::new, @@ -441,7 +455,7 @@ public void periodicAfterScheduler() { handoff.periodicAfterScheduler(); intake.periodicAfterScheduler(); - leds.periodicAfterScheduler(); + // leds.periodicAfterScheduler(); TODO: ADD THESE BACK TY spindexer.periodicAfterScheduler(); hood.periodicAfterScheduler(); shooter.periodicAfterScheduler(); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java deleted file mode 100644 index 0cbe7062..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java +++ /dev/null @@ -1,51 +0,0 @@ -/************************ 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.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.intake.IntakeDeploy; -import com.stuypulse.robot.commands.intake.IntakeStow; -import com.stuypulse.robot.commands.spindexer.SpindexerRun; -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.WaitUntilCommand; - -import com.pathplanner.lib.path.PathPlannerPath; - -public class DepotAuton extends SequentialCommandGroup { - - public DepotAuton(PathPlannerPath... paths) { - - addCommands( - - // To Depot - new IntakeDeploy().alongWith( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) - ), - - new IntakeStow().alongWith( - CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]) - ), - - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), - new SpindexerRun().alongWith( - new HandoffRun() - ) - // .until(() -> DriverStation.getMatchTime() < 2).andThen( - // new ParallelCommandGroup( - // new HandoffStop(), - // new SpindexerStop(), - // new ClimberDown() - // ) - // ) - - ); - - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java deleted file mode 100644 index 541e3a30..00000000 --- a/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java +++ /dev/null @@ -1,34 +0,0 @@ -/************************ 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.commands.handoff.HandoffRun; -import com.stuypulse.robot.commands.spindexer.SpindexerRun; -import com.stuypulse.robot.commands.superstructure.SuperstructureKB; -import com.stuypulse.robot.subsystems.superstructure.Superstructure; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -import com.pathplanner.lib.path.PathPlannerPath; - -public class EightFuel extends SequentialCommandGroup { - - public EightFuel(PathPlannerPath... paths) { - - addCommands( - - new SuperstructureKB().alongWith( - new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()).andThen( - new SpindexerRun().alongWith(new HandoffRun()) - ) - ) - - ); - - } - -} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftMiddy.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftMiddy.java new file mode 100644 index 00000000..44b4abea --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftMiddy.java @@ -0,0 +1,64 @@ +package com.stuypulse.robot.commands.auton.regular; + +import com.pathplanner.lib.path.PathPlannerPath; +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.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +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 LeftMiddy(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()) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new SuperstructureAutoInterpolation() + ), + + // 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()) + ) + ), + + // 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()) + ) + + ); + + } + +} 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 9b05bd07..d2ca9149 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 @@ -14,6 +14,8 @@ 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; @@ -30,6 +32,8 @@ public LeftTwoCycle(PathPlannerPath... paths) { addCommands( + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + // NZ Trip 1 CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( new WaitCommand(0.5).andThen(new IntakeDeploy()) @@ -44,7 +48,7 @@ public LeftTwoCycle(PathPlannerPath... paths) { new HandoffRun().andThen( new SpindexerRun() ).andThen(new WaitCommand(1.75) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(3.5), + .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.0), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), // NZ Trip 2 @@ -59,7 +63,7 @@ public LeftTwoCycle(PathPlannerPath... paths) { new HandoffRun().andThen( new SpindexerRun() ).andThen(new WaitCommand(1.75) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.5), + .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), new ParallelCommandGroup( diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightMiddy.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightMiddy.java new file mode 100644 index 00000000..2cbffe4f --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightMiddy.java @@ -0,0 +1,64 @@ +package com.stuypulse.robot.commands.auton.regular; + +import com.pathplanner.lib.path.PathPlannerPath; +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.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +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 RightMiddy(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()) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new SuperstructureAutoInterpolation() + ), + + // 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()) + ) + ), + + // 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()) + ) + + ); + + } + +} 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 92309c74..2224300c 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 @@ -14,6 +14,7 @@ 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.superstructure.Superstructure; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -30,6 +31,8 @@ public RightTwoCycle(PathPlannerPath... paths) { addCommands( + new SwerveResetPose(paths[0].getStartingHolonomicPose().get()), + // NZ Trip 1 CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( new WaitCommand(0.5).andThen(new IntakeDeploy()) @@ -44,7 +47,7 @@ public RightTwoCycle(PathPlannerPath... paths) { new HandoffRun().andThen( new SpindexerRun() ).andThen(new WaitCommand(1.75) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(3.5), + .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.0), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), // NZ Trip 2 @@ -59,7 +62,7 @@ public RightTwoCycle(PathPlannerPath... paths) { new HandoffRun().andThen( new SpindexerRun() ).andThen(new WaitCommand(1.75) - .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(4.5), + .andThen(new IntakeAutoDigest()).repeatedly()).withTimeout(15.0), new SuperstructureAutoInterpolation().alongWith(new IntakeDeploy()), new ParallelCommandGroup( 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 8085ffbb..3d0e744a 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java @@ -65,7 +65,7 @@ public SwerveDriveSOTM(Gamepad driver) { isIdle = BStream.create( () -> getDriverInputAsVelocity().magnitude() <= Drive.DEADBAND && Math.abs(driver.getRightX()) <= Turn.DEADBAND) - .filtered(new BDebounce.Rising(2.0), new BDebounce.Falling(0.1)); + .filtered(new BDebounce.Rising(0.5), new BDebounce.Falling(0.1)); this.driver = driver; diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPose.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPose.java new file mode 100644 index 00000000..0bddbe82 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPose.java @@ -0,0 +1,21 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class SwerveResetPose extends InstantCommand { + private final CommandSwerveDrivetrain swerve; + private final Pose2d poseToReset; + + public SwerveResetPose(Pose2d poseToReset) { + swerve = CommandSwerveDrivetrain.getInstance(); + this.poseToReset = poseToReset; + } + + @Override + public void initialize() { + swerve.resetPose(poseToReset); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseKBShot.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseKBShot.java new file mode 100644 index 00000000..818e36c1 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseKBShot.java @@ -0,0 +1,16 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.util.Units; + +public class SwerveResetPoseKBShot extends SwerveResetPose { + public SwerveResetPoseKBShot() { + super(new Pose2d( + Field.HUB_CENTER.getX() - Field.HUB_RADIUS - Units.inchesToMeters(23.5), + Field.WIDTH / 2 - Units.inchesToMeters(6.5), + CommandSwerveDrivetrain.getInstance().getPose().getRotation())); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseLeftCorner.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseLeftCorner.java new file mode 100644 index 00000000..b752c219 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseLeftCorner.java @@ -0,0 +1,12 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.math.geometry.Pose2d; + +public class SwerveResetPoseLeftCorner extends SwerveResetPose { + public SwerveResetPoseLeftCorner() { + super(new Pose2d(0, Field.WIDTH, CommandSwerveDrivetrain.getInstance().getPose().getRotation())); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseRightCorner.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseRightCorner.java new file mode 100644 index 00000000..ebd7ffab --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveResetPoseRightCorner.java @@ -0,0 +1,12 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.math.geometry.Pose2d; + +public class SwerveResetPoseRightCorner extends SwerveResetPose { + public SwerveResetPoseRightCorner() { + super(new Pose2d(0, 0, CommandSwerveDrivetrain.getInstance().getPose().getRotation())); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/vision/BlackListAllTagsForAllCameras.java b/src/main/java/com/stuypulse/robot/commands/vision/BlackListAllTagsForAllCameras.java new file mode 100644 index 00000000..31a93e71 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/vision/BlackListAllTagsForAllCameras.java @@ -0,0 +1,13 @@ +package com.stuypulse.robot.commands.vision; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; + +public class BlackListAllTagsForAllCameras extends ParallelCommandGroup{ + public BlackListAllTagsForAllCameras() { + addCommands( + new BlacklistAllTags("limelight-left"), + new BlacklistAllTags("limelight-right"), + new BlacklistAllTags("limelight-back") + ); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/vision/EnableBackLimelight.java b/src/main/java/com/stuypulse/robot/commands/vision/EnableBackLimelight.java index 5c222ead..fd33ab7d 100644 --- a/src/main/java/com/stuypulse/robot/commands/vision/EnableBackLimelight.java +++ b/src/main/java/com/stuypulse/robot/commands/vision/EnableBackLimelight.java @@ -1,9 +1,9 @@ -package com.stuypulse.robot.commands.vision; +// package com.stuypulse.robot.commands.vision; -public class EnableBackLimelight extends EnablePortForwarding { - public EnableBackLimelight() { - super("limelight-back"); - } -} +// public class EnableBackLimelight extends EnablePortForwarding { +// public EnableBackLimelight() { +// super("limelight-back.local"); +// } +// } diff --git a/src/main/java/com/stuypulse/robot/commands/vision/EnableLeftLimelight.java b/src/main/java/com/stuypulse/robot/commands/vision/EnableLeftLimelight.java index 01594c67..5aa48164 100644 --- a/src/main/java/com/stuypulse/robot/commands/vision/EnableLeftLimelight.java +++ b/src/main/java/com/stuypulse/robot/commands/vision/EnableLeftLimelight.java @@ -1,7 +1,7 @@ -package com.stuypulse.robot.commands.vision; +// package com.stuypulse.robot.commands.vision; -public class EnableLeftLimelight extends EnablePortForwarding { - public EnableLeftLimelight() { - super("limelight-left"); - } -} +// public class EnableLeftLimelight extends EnablePortForwarding { +// public EnableLeftLimelight() { +// super("limelight-left.local"); +// } +// } diff --git a/src/main/java/com/stuypulse/robot/commands/vision/EnablePortForwarding.java b/src/main/java/com/stuypulse/robot/commands/vision/EnablePortForwarding.java index a836cc62..78dac7ac 100644 --- a/src/main/java/com/stuypulse/robot/commands/vision/EnablePortForwarding.java +++ b/src/main/java/com/stuypulse/robot/commands/vision/EnablePortForwarding.java @@ -1,23 +1,25 @@ -package com.stuypulse.robot.commands.vision; +// package com.stuypulse.robot.commands.vision; -import edu.wpi.first.net.PortForwarder; -import edu.wpi.first.wpilibj2.command.InstantCommand; +// import edu.wpi.first.net.PortForwarder; +// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +// import edu.wpi.first.wpilibj2.command.InstantCommand; -public class EnablePortForwarding extends InstantCommand { - private String hostname; +// public class EnablePortForwarding extends InstantCommand { +// private String hostname; - public EnablePortForwarding(String hostname) { - this.hostname = hostname; - } +// public EnablePortForwarding(String hostname) { +// this.hostname = hostname; +// } - @Override - public void initialize() { - PortForwarder.remove(5801); - PortForwarder.add(5801, hostname, 5801); - } +// @Override +// public void initialize() { +// PortForwarder.remove(10000); +// PortForwarder.add(10000, hostname, 5801); +// SmartDashboard.putString("Limelight/Limelight Being Port Forwarded", hostname); +// } - @Override - public boolean runsWhenDisabled() { - return true; - } -} +// @Override +// public boolean runsWhenDisabled() { +// return true; +// } +// } diff --git a/src/main/java/com/stuypulse/robot/commands/vision/EnableRightLimelight.java b/src/main/java/com/stuypulse/robot/commands/vision/EnableRightLimelight.java index 96f4908b..418be5df 100644 --- a/src/main/java/com/stuypulse/robot/commands/vision/EnableRightLimelight.java +++ b/src/main/java/com/stuypulse/robot/commands/vision/EnableRightLimelight.java @@ -1,7 +1,7 @@ -package com.stuypulse.robot.commands.vision; +// package com.stuypulse.robot.commands.vision; -public class EnableRightLimelight extends EnablePortForwarding { - public EnableRightLimelight() { - super("limelight-right"); - } -} +// public class EnableRightLimelight extends EnablePortForwarding { +// public EnableRightLimelight() { +// super("limelight-right.local"); +// } +// } diff --git a/src/main/java/com/stuypulse/robot/commands/vision/SetPipeline.java b/src/main/java/com/stuypulse/robot/commands/vision/SetPipeline.java new file mode 100644 index 00000000..e20f816d --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/vision/SetPipeline.java @@ -0,0 +1,31 @@ +/************************ 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.vision; + +import com.stuypulse.robot.constants.Cameras.Camera.Pipeline; +import com.stuypulse.robot.subsystems.vision.LimelightVision; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class SetPipeline extends InstantCommand { + private LimelightVision vision; + private Pipeline pipeline; + + public SetPipeline(Pipeline pipeline) { + this.vision = LimelightVision.getInstance(); + this.pipeline = pipeline; + } + + @Override + public void initialize() { + vision.setPipeline(pipeline); + } + + @Override + public boolean runsWhenDisabled() { + return true; + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index ba29fc41..5176c811 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -5,13 +5,14 @@ /***************************************************************/ package com.stuypulse.robot.constants; -import com.stuypulse.stuylib.network.SmartBoolean; - import com.stuypulse.robot.RobotContainer; +import com.stuypulse.robot.util.vision.LimelightHelpers; +import com.stuypulse.stuylib.network.SmartBoolean; 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 +41,75 @@ public static class Camera { private Pose3d location; private SmartBoolean isEnabled; + private int rejectedCounterNotNull; + private int rejectedCounterAngularVelocity; + private int rejectedCounterInvalidPosition; + private int rejectedCounterTargetArea; + + private Pipeline currentPipeline; + + public enum Pipeline { + NO_SUN, + LOW_SUN, + MED_SUN, + HIGH_SUN + } + + private int getCurrentPipelineID() { + return switch(this.currentPipeline) { + case NO_SUN -> 3; + case LOW_SUN -> 2; + case MED_SUN -> 1; + case HIGH_SUN -> 0; + }; + } + + public enum RejectionValue { + NOT_NULL, + ANGULAR_VELOCITY, + INVALID_POSITION, + TARGET_AREA + }; + + public void setPipeline(Pipeline pipeline) { + this.currentPipeline = pipeline; + LimelightHelpers.setPipelineIndex(name, getCurrentPipelineID()); + } + + public void performHDR() { + Pipeline nextHdrPipeline = Pipeline.NO_SUN; + + if (currentPipeline == Pipeline.NO_SUN) { + nextHdrPipeline = Pipeline.HIGH_SUN; + } + + setPipeline(nextHdrPipeline); + } + + public void incrementRejection(RejectionValue rejectionValue) { + switch (rejectionValue) { + case NOT_NULL: + rejectedCounterNotNull++; + break; + case ANGULAR_VELOCITY: + rejectedCounterAngularVelocity++; + break; + case INVALID_POSITION: + rejectedCounterInvalidPosition++; + break; + case TARGET_AREA: + rejectedCounterTargetArea++; + break; + } + } + + 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; diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index d0ab89a2..0ce7ce96 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -39,7 +39,7 @@ public interface Field { public static final Pose2d HUB_FAR_RIGHT_CORNER = new Pose2d(Units.inchesToMeters(205.6), WIDTH / 2.0 - Units.inchesToMeters(47 / 2.0), Rotation2d.kZero); public static final Pose2d HUB_FAR_LEFT_CORNER = new Pose2d(Units.inchesToMeters(205.6), WIDTH / 2.0 + Units.inchesToMeters(47 / 2.0), Rotation2d.kZero); - public static final double HUB_RADIUS = Units.inchesToMeters(41.7 / 2); + public static final double HUB_RADIUS = Units.inchesToMeters(41.7 / 2.0); public static final double OPPONENT_ZONE_X = LENGTH - Units.inchesToMeters(158.6); @@ -66,13 +66,13 @@ public static boolean closerToTop() { public final Pose2d LEFT_FERRY_ZONE = new Pose2d( Units.inchesToMeters(31.5), - WIDTH - Units.inchesToMeters(34.5), + WIDTH - Units.inchesToMeters(34.5) - Units.inchesToMeters(12), new Rotation2d() ); public final Pose2d RIGHT_FERRY_ZONE = new Pose2d( Units.inchesToMeters(20.75), - Units.inchesToMeters(76), + Units.inchesToMeters(76) + Units.inchesToMeters(12), new Rotation2d() ); diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index b0104961..0c9bc260 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -55,6 +55,7 @@ public interface slot1 { } SmartNumber kOmega = new SmartNumber("Superstructure/Turret/Gains/kOmega", 3.43); + SmartNumber kTranslation = new SmartNumber("Superstructure/Turret/Gains/kTranslation", 0.0); } } @@ -114,7 +115,7 @@ public interface Turn { } public interface Alignment { - PIDConstants XY = new PIDConstants(4.0, 0, 0); + PIDConstants XY = new PIDConstants(5.0, 0, 0); PIDConstants THETA = new PIDConstants(4.0, 0, 0.0); } } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index eb220212..1c8a8e91 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -5,9 +5,6 @@ /***************************************************************/ package com.stuypulse.robot.constants; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; - import com.ctre.phoenix6.CANBus; import com.pathplanner.lib.path.PathConstraints; import com.stuypulse.stuylib.network.SmartBoolean; @@ -21,6 +18,8 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; import edu.wpi.first.wpilibj.LEDPattern; import edu.wpi.first.wpilibj.util.Color; @@ -33,11 +32,13 @@ public interface Settings { public final double DT = 0.020; - public final int LOGGING_FREQUENCY = 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); public final CANBus CANIVORE = new CANBus("canivore", "./logs/example.hoot"); public final double LOOP_OVERRUN_WARNING_TIME_SEC = 1; + SmartBoolean ENABLE_DISTANCE_CHECK = new SmartBoolean("Robot/Enable Distance Check?", false); + SmartBoolean ENABLE_OUT_OF_FIELD_CHECK = new SmartBoolean("Robot/Enable out of field check", true); public interface Handoff { public final double GEAR_RATIO = 3.0 / 1.0; @@ -72,7 +73,9 @@ public interface Intake { Rotation2d ANGLE_THRESHOLD_FOR_HOLDING_VOLTAGE = Rotation2d.fromDegrees(15.0); double HOMING_VOLTAGE = 3.0; - double PUSHDOWN_VOLTAGE = 2.5; + double PUSHDOWN_VOLTAGE = -3.0; + 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; @@ -100,13 +103,13 @@ public interface Spindexer { public interface Superstructure { public final double SHOOTER_TOLERANCE_RPM_HIGH = 50.0; public final double SHOOTER_TOLERANCE_RPM_LOW = 80.0; - public final double SHOOTER_SOTM_TOLERANCE_RPM_HIGH = 50.0; - public final double SHOOTER_SOTM_TOLERANCE_RPM_LOW = 80.0; + public final double SHOOTER_SOTM_TOLERANCE_RPM_HIGH = 100.0; + 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 Rotation2d HOOD_TOLERANCE = Rotation2d.fromDegrees(0.5); - public final Rotation2d HOOD_SOTM_TOLERANCE = Rotation2d.fromDegrees(0.5); + public final Rotation2d HOOD_SOTM_TOLERANCE = Rotation2d.fromDegrees(2); public interface AngleInterpolation { double[][] distanceAngleInterpolationValues = { @@ -140,17 +143,18 @@ public interface TOFInterpolation{ public interface FerryRPMInterpolation { double[][] ferryDistanceRPMInterpolation = { + {1, 2000}, {5.16, 3300.0}, {6.94, 3600.0}, {7.87, 3800.0}, {9.77, 4300.0}, - {10.694, 4595.0}, //STARTING FROM HERE THE DATA IS UNRELIABLE!!! - {11.516, 4750.0}, - {12.416, 4900.0}, - {13.316, 5050.0}, - {14.216, 5175.0}, - {15.148, 5200.0}, - {16.54, 5300} //FIELD LENGTH + {10.694, 4700.0}, //STARTING FROM HERE THE DATA IS UNRELIABLE!!! + {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 }; } @@ -179,7 +183,7 @@ public interface RPM { public final SmartNumber MANUAL_OVERRIDE = new SmartNumber("InterpolationTesting/Shoot State Target RPM", 3500.0); public final double REVERSE = 0.0; - public final double KB = 2600.0; + public final double KB = 2675.0; public final double LEFT_CORNER = 3650.0; public final double RIGHT_CORNER = 3650.0; } @@ -231,8 +235,8 @@ 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", 5);//Rotation2d.fromDegrees(10.0); - public final Rotation2d FOTM_TOLERANCE = Rotation2d.fromDegrees(5.0); + public final SmartNumber SOTM_TOLERANCE = new SmartNumber("Superstructure/Turret/SOTM Tolerance", 6);//Rotation2d.fromDegrees(10.0); + public final Rotation2d FOTM_TOLERANCE = Rotation2d.fromDegrees(10.0); public final Rotation2d KB = Rotation2d.fromDegrees(0.0); public final Rotation2d LEFT_CORNER = Rotation2d.fromDegrees(-233.0); @@ -289,10 +293,12 @@ public interface SOTM { public interface Swerve { public final double MODULE_VELOCITY_DEADBAND_M_PER_S = 0.1; public final double ROTATIONAL_DEADBAND_RAD_PER_S = 0.1; + double MAX_ACCEPTABLE_POSE_DELTA_METERS = Math.sqrt(Field.LENGTH * Field.LENGTH + Field.WIDTH * Field.WIDTH); //TODO: Might wanna make this smaller. + double MAX_ACCEPTABLE_VISION_DEVIATION_METERS = 1.0; public interface Constraints { public final double MAX_VELOCITY_M_PER_S = 4.16; - public final double MAX_VELOCITY_SOTM_M_PER_S = 2.0; + public final double MAX_VELOCITY_SOTM_M_PER_S = 1.75; public final double MAX_VELOCITY_FOTM_M_PER_S = 4.16; public final double MAX_ANGULAR_VEL_RAD_PER_S = Units.degreesToRadians(300.0); @@ -381,6 +387,10 @@ public interface Vision { public final Translation2d INVALID_POSITION = new Translation2d(8.2705, 4.0345); public final double INVALID_POSITION_TOLERANCE_M = 0.05; public final double MAX_ANGULAR_VELOCITY_RAD_SEC = 2 * Math.PI; + double MIN_TAG_AREA = 5; //TODO: MAKE SURE THIS IS A GOOD VALUE!!! + + SmartBoolean HDR_ENABLED = new SmartBoolean("Vision/HDR Enabled?", false); + double HDR_TIMEOUT_SEC = 0.25; public final double BUZZ_DEBOUNCE = 0.25; } 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 4b729b74..3ebb2b3a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java @@ -33,7 +33,6 @@ 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.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -187,7 +186,7 @@ public void periodicAfterScheduler() { SmartDashboard.putNumber("Handoff/Follow Supply Current", motorLeadSupplyCurrent.getValueAsDouble()); SmartDashboard.putNumber("Handoff/Follow Stator Current", motorLeadStatorCurrent.getValueAsDouble()); - if(Robot.getMode() == RobotMode.DISABLED && !DriverStation.isFMSAttached()) { + 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()); } 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 dfcdfb61..15e217da 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -5,8 +5,20 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.intake; -import com.stuypulse.stuylib.streams.booleans.BStream; -import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; +import java.util.Optional; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorAlignmentValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import com.stuypulse.robot.Robot; import com.stuypulse.robot.Robot.RobotMode; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; @@ -16,29 +28,17 @@ 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 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.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.controls.DutyCycleOut; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.GravityTypeValue; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.MotorAlignmentValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; -import java.util.Optional; - public class IntakeImpl extends Intake { private Motors.TalonFXConfig pivotConfig; private Motors.TalonFXConfig rollerConfig; @@ -51,6 +51,9 @@ public class IntakeImpl extends Intake { private final Follower follower; private Optional pivotVoltageOverride; + private TorqueCurrentFOC torqueCurrentFOC; + private VoltageOut voltageOut; + private PositionVoltage positionVoltage; private BStream pivotStalling; @@ -88,11 +91,15 @@ public IntakeImpl() { rollerConfig = new Motors.TalonFXConfig() .withInvertedValue(InvertedValue.Clockwise_Positive) - .withNeutralMode(NeutralModeValue.Brake) - .withSupplyCurrentLimitAmps(30.0) + .withNeutralMode(NeutralModeValue.Coast) + .withSupplyCurrentLimitAmps(37.0) .withStatorCurrentLimitEnabled(false) .withRampRate(0.50); + torqueCurrentFOC = new TorqueCurrentFOC(0.0); + voltageOut = new VoltageOut(0.0); + positionVoltage = new PositionVoltage(0.0); + pivot = new TalonFX(Ports.Intake.PIVOT, Ports.RIO); pivotConfig.configure(pivot); @@ -124,10 +131,10 @@ public IntakeImpl() { pivotMotorVoltage = pivot.getMotorVoltage(); rollerLeaderVoltage = rollerLeader.getMotorVoltage(); rollerFollowerVoltage = rollerFollower.getMotorVoltage(); - PhoenixUtil.registerToRio(pivotStatorCurrent, rollerLeaderSupplyCurrent, + PhoenixUtil.registerToRio(pivotSupplyCurrent, pivotStatorCurrent, pivotMotorPosition, rollerLeaderSupplyCurrent, rollerLeaderStatorCurrent, rollerFollowerSupplyCurrent, rollerFollowerStatorCurrent, rollerLeaderTemperature, rollerFollowerTemperature, pivotTemperature, pivotMotorVoltage, - rollerLeaderVoltage, rollerFollowerVoltage, pivotMotorPosition ); + rollerLeaderVoltage, rollerFollowerVoltage); pivotStalling = BStream.create( () -> Math.abs(pivotSupplyCurrent.getValueAsDouble()) > Settings.Intake.STALL_CURRENT_LIMIT) @@ -167,17 +174,17 @@ public void periodicAfterScheduler() { PivotState pivotState = getPivotState(); RollerState rollerState = getRollerState(); - pivotConfig.updateGainsConfig( - pivot, - 0, - Gains.Intake.Pivot.kP, - Gains.Intake.Pivot.kI, - Gains.Intake.Pivot.kD, - Gains.Intake.Pivot.kS, - Gains.Intake.Pivot.kV, - Gains.Intake.Pivot.kA); - - boolean applyingPushdownVoltage = false; + // pivotConfig.updateGainsConfig( + // pivot, + // 0, + // Gains.Intake.Pivot.kP, + // Gains.Intake.Pivot.kI, + // Gains.Intake.Pivot.kD, + // Gains.Intake.Pivot.kS, + // Gains.Intake.Pivot.kV, + // Gains.Intake.Pivot.kA); + + boolean applyingPushdownCurrent = false; if (EnabledSubsystems.INTAKE.get()) { if (pivotVoltageOverride.isPresent()) { pivot.setVoltage(pivotVoltageOverride.get()); @@ -186,12 +193,16 @@ public void periodicAfterScheduler() { if (pivotState == PivotState.DEPLOY && getPivotAngle().getDegrees() <= Settings.Intake.ANGLE_THRESHOLD_FOR_HOLDING_VOLTAGE.getDegrees() && rollerState != RollerState.STOP) { - pivot.setControl(new VoltageOut(-Settings.Intake.PUSHDOWN_VOLTAGE)); // applying 3 volts - applyingPushdownVoltage = true; + // pivot.setControl(new VoltageOut(Settings.Intake.PUSHDOWN_VOLTAGE)); // applying 3 volts + double pushdownCurrent = + Robot.getMode() == RobotMode.AUTON ? + Settings.Intake.PUSHDOWN_CURRENT_AUTON : Settings.Intake.PUSHDOWN_CURRENT_TELEOP; + pivot.setControl(torqueCurrentFOC.withOutput(pushdownCurrent)); + applyingPushdownCurrent = true; } else if (pivotState == PivotState.HOMING) { - pivot.setControl(new VoltageOut(-Settings.Intake.HOMING_VOLTAGE)); + pivot.setControl(voltageOut.withOutput(-Settings.Intake.HOMING_VOLTAGE)); } else { - pivot.setControl(new PositionVoltage(pivotState.getTargetAngle().getRotations())); + pivot.setControl(positionVoltage.withPosition(getPivotState().getTargetAngle().getRotations())); } // ROLLERS @@ -219,7 +230,7 @@ && getPivotAngle().getDegrees() <= Settings.Intake.THRESHOLD_TO_START_ROLLERS.ge if (Robot.getPeriodicCounter() % Settings.LOGGING_FREQUENCY == 0) { // PIVOT - SmartDashboard.putBoolean("Intake/Pivot Pushdown Voltage Applied?", applyingPushdownVoltage); + SmartDashboard.putBoolean("Intake/Pivot Pushdown Voltage Applied?", applyingPushdownCurrent); SmartDashboard.putNumber("Intake/Pivot Closed Loop Error (deg)", pivot.getClosedLoopError().getValueAsDouble() * 360.0); @@ -253,7 +264,7 @@ && getPivotAngle().getDegrees() <= Settings.Intake.THRESHOLD_TO_START_ROLLERS.ge SmartDashboard.putNumber("Intake/Pivot Stator Current (amps)", pivotStatorCurrent.getValueAsDouble()); - if (Robot.getMode() == RobotMode.DISABLED && !DriverStation.isFMSAttached()) { + if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { SmartDashboard.putBoolean("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 " 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 36ca82c1..463d3310 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -8,7 +8,7 @@ import java.util.Optional; import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -20,7 +20,6 @@ import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.handoff.Handoff; -import com.stuypulse.robot.subsystems.handoff.Handoff.HandoffState; import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; @@ -32,21 +31,10 @@ 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.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; - -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.controls.DutyCycleOut; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; - -import java.util.Optional; - public class SpindexerImpl extends Spindexer { private final Motors.TalonFXConfig spindexerLeadConfig; @@ -189,7 +177,7 @@ public void periodicAfterScheduler() { SmartDashboard.putBoolean("Spindexer/Should Stop?", shouldStop()); if (Settings.DEBUG_MODE.get()) { - if (Robot.getMode() == RobotMode.DISABLED && !DriverStation.isFMSAttached()) { + if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { SmartDashboard.putBoolean( "Robot/CAN/Canivore/Spindexer Leader Motor Connected? (ID " + String.valueOf(Ports.Spindexer.MOTOR) + ")", 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 a07d159d..0950ddef 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 @@ -5,8 +5,15 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.superstructure.hood; -import com.stuypulse.stuylib.streams.booleans.BStream; -import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; +import java.util.Optional; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import com.stuypulse.robot.Robot; import com.stuypulse.robot.Robot.RobotMode; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; @@ -16,24 +23,16 @@ 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 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.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; -import java.util.Optional; - public class HoodImpl extends Hood { private final Motors.TalonFXConfig hoodConfig; // private final Motors.CANCoderConfig hoodEncoderConfig; @@ -180,11 +179,12 @@ public void periodicAfterScheduler() { hoodMotor.stopMotor(); } - SmartDashboard.putBoolean("Superstructure/Hood/Has Used Absolute Encoder", hasUsedAbsoluteEncoder); + // SmartDashboard.putBoolean("Superstructure/Hood/Has Used Absolute Encoder", hasUsedAbsoluteEncoder); - SmartDashboard.putBoolean("Prematch Checks/Hood at Top?", getAngle().getDegrees() > 39.0); + 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()); if (Settings.DEBUG_MODE.get()) { SmartDashboard.putNumber("Superstructure/Hood/Applied Voltage (amps)", hoodMotorVoltage.getValueAsDouble()); @@ -194,10 +194,9 @@ public void periodicAfterScheduler() { SmartDashboard.putBoolean("Superstructure/Hood/is stalling", isStalling()); Robot.getEnergyUtil().logEnergyUsage(getName(), getCurrentDraw()); - - if (Robot.getMode() == RobotMode.DISABLED && !DriverStation.isFMSAttached()) { - SmartDashboard.putBoolean("Robot/CAN/Canivore/Hood Motor Connected? (ID " + String.valueOf(Ports.Superstructure.Hood.MOTOR) + ")", hoodMotor.isConnected()); - // SmartDashboard.putBoolean("Robot/CAN/Canivore/Hood Encoder Connected? (ID " + String.valueOf(hoodEncoder.getDeviceID()) + ")", hoodEncoder.isConnected()); + if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { + SmartDashboard.putBoolean("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/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java index b67ceb7d..fedb0915 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 @@ -80,7 +80,7 @@ public double getShootRPM() { } public boolean atTolerance() { - double error = getTargetRPM() - getRPM(); + double error = getRPM() - getTargetRPM(); double toleranceHigh = switch (state) { case SOTM -> Settings.Superstructure.SHOOTER_SOTM_TOLERANCE_RPM_HIGH; 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 f5dad04d..e1f454f7 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 @@ -5,6 +5,15 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.superstructure.shooter; +import java.util.Optional; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorAlignmentValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.stuypulse.robot.Robot; import com.stuypulse.robot.Robot.RobotMode; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; @@ -18,20 +27,9 @@ 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.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.MotorAlignmentValue; -import com.ctre.phoenix6.signals.NeutralModeValue; - -import java.util.Optional; - public class ShooterImpl extends Shooter { private final Motors.TalonFXConfig shooterConfig; @@ -73,14 +71,14 @@ public ShooterImpl() { .withLowerLimitSupplyCurrent(60, 1); shooterLeader = new TalonFX(Ports.Superstructure.Shooter.MOTOR_LEAD, Ports.RIO); - shooterLeader.getVelocity().setUpdateFrequency(1000.0); - shooterLeader.getTorqueCurrent().setUpdateFrequency(1000.0); + shooterLeader.getVelocity().setUpdateFrequency(200.0); + shooterLeader.getTorqueCurrent().setUpdateFrequency(500.0); shooterLeader.getStatorCurrent().setUpdateFrequency(50.0); shooterLeader.getSupplyCurrent().setUpdateFrequency(50.0); shooterFollower = new TalonFX(Ports.Superstructure.Shooter.MOTOR_FOLLOW, Ports.RIO); - shooterFollower.getVelocity().setUpdateFrequency(1000.0); - shooterFollower.getTorqueCurrent().setUpdateFrequency(1000.0); + shooterFollower.getVelocity().setUpdateFrequency(200.0); + shooterFollower.getTorqueCurrent().setUpdateFrequency(50.0); shooterFollower.getStatorCurrent().setUpdateFrequency(50.0); shooterFollower.getSupplyCurrent().setUpdateFrequency(50.0); @@ -129,26 +127,6 @@ public double getBangBangOutput(double mesurement, double setpoint) { public void periodicAfterScheduler() { super.periodicAfterScheduler(); - shooterConfig.updateGainsConfig( - shooterLeader, - 0, - Gains.Superstructure.Shooter.kP, - Gains.Superstructure.Shooter.kI, - Gains.Superstructure.Shooter.kD, - Gains.Superstructure.Shooter.kS, - Gains.Superstructure.Shooter.kV, - Gains.Superstructure.Shooter.kA); - - shooterConfig.updateGainsConfig( - shooterFollower, - 0, - Gains.Superstructure.Shooter.kP, - Gains.Superstructure.Shooter.kI, - Gains.Superstructure.Shooter.kD, - Gains.Superstructure.Shooter.kS, - Gains.Superstructure.Shooter.kV, - Gains.Superstructure.Shooter.kA); - if (EnabledSubsystems.SHOOTER.get() || getState() == ShooterState.STOP) { if (voltageOverride.isPresent()) { shooterLeader.setVoltage(voltageOverride.get()); @@ -180,7 +158,7 @@ public void periodicAfterScheduler() { SmartDashboard.putNumber("Superstructure/Shooter/Follower Stator Current (amps)", shooterFollowStatorCurrent.getValueAsDouble()); - if (Robot.getMode() == RobotMode.DISABLED && !DriverStation.isFMSAttached()) { + if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { SmartDashboard.putBoolean( "Robot/CAN/Main/Shooter Leader Motor Connected? (ID " + String.valueOf(Ports.Superstructure.Shooter.MOTOR_LEAD) + ")", @@ -196,6 +174,7 @@ public void periodicAfterScheduler() { SmartDashboard.putNumber("InterpolationTesting/Shooter Closed Loop Error (RPM)", shooterLeaderClosedLoopError.getValueAsDouble() * 60.0); + SmartDashboard.putNumber("Superstructure/Shooter/Implemented Error (RPM)", getTargetRPM() - getLeaderRPM()); } private void setVoltageOverride(Optional voltageOverride) { 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 c9ecd531..fbbb3d88 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 @@ -5,9 +5,19 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.superstructure.turret; +import java.util.Optional; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import com.stuypulse.robot.Robot; -import com.stuypulse.robot.RobotContainer; import com.stuypulse.robot.Robot.RobotMode; +import com.stuypulse.robot.RobotContainer; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.DriverConstants; import com.stuypulse.robot.constants.Gains; @@ -15,7 +25,6 @@ import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import com.stuypulse.robot.util.EnergyUtil; import com.stuypulse.robot.util.PhoenixUtil; import com.stuypulse.robot.util.SysId; import com.stuypulse.robot.util.superstructure.TurretAngleCalculator; @@ -24,21 +33,9 @@ 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.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.SensorDirectionValue; -import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; - -import java.util.Optional; - public class TurretImpl extends Turret { private final Motors.TalonFXConfig turretConfig; private final Motors.CANCoderConfig encoder17tConfig; @@ -123,7 +120,7 @@ public TurretImpl() { controller = new PositionVoltage(getTargetAngle().getRotations()).withEnableFOC(true); - turretMotor.getClosedLoopError().setUpdateFrequency(1000.0); + turretMotor.getClosedLoopError().setUpdateFrequency(50.0); encoder18tPos = encoder18t.getAbsolutePosition(); encoder17tPos = encoder17t.getAbsolutePosition(); @@ -210,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(); @@ -265,13 +262,13 @@ public void periodicAfterScheduler() { double setpointVelocityRPS = delta / (360 * Settings.DT); // the component of the turret's setpoint velocity that comes from robot translation - double translationalVelocityRPS = setpointVelocityRPS - omega / (2 * Math.PI); - double translationFF = Gains.Superstructure.Turret.slot0.kV * translationalVelocityRPS; + double translationalComponentVelocityRPS = setpointVelocityRPS - omega / (2 * Math.PI); + double translationFF = Gains.Superstructure.Turret.kTranslation.get() * translationalComponentVelocityRPS; turretMotor.setControl(controller .withPosition(prevActualTargetAngle / 360.0) .withSlot(slot) - .withFeedForward(omegaFF /* + translationFF */) + .withFeedForward(omegaFF + translationFF) ); } } else { @@ -296,10 +293,10 @@ public void periodicAfterScheduler() { if (Settings.DEBUG_MODE.get()) { SmartDashboard.putNumber("Superstructure/Turret/Stator Current (amps)", turretMotorStatorCurrent.getValueAsDouble()); - SmartDashboard.putNumber("Superstructure/Turret/Supply Curren (amps)", + SmartDashboard.putNumber("Superstructure/Turret/Supply Current (amps)", turretMotorSupplyCurrent.getValueAsDouble()); - if (Robot.getMode() == RobotMode.DISABLED && !DriverStation.isFMSAttached()) { + if (Robot.getMode() == RobotMode.DISABLED && !Robot.fmsAttached) { SmartDashboard.putBoolean( "Robot/CAN/Main/Turret Motor Connected? (ID " + String.valueOf(Ports.Superstructure.Turret.MOTOR) + ")", turretMotor.isConnected()); 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 8e97bd69..4453584c 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -5,12 +5,8 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.swerve; -import static edu.wpi.first.units.Units.Second; -import static edu.wpi.first.units.Units.Volts; - -import java.sql.Struct; - import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -31,11 +27,13 @@ import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Gains; +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.superstructure.turret.Turret; import com.stuypulse.robot.subsystems.swerve.TunerConstants.TunerSwerveDrivetrain; +import com.stuypulse.robot.util.PhoenixUtil; import com.stuypulse.stuylib.math.Angle; import com.stuypulse.stuylib.math.Vector2D; @@ -45,13 +43,17 @@ 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.math.util.Units; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructPublisher; -import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.units.measure.AngularAcceleration; +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; @@ -72,13 +74,29 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private StructPublisher leftBehindHubYPlublisher; private StructPublisher rightBehindHubYPlublisher; private StructPublisher vertexBehindHubPublisher; + private StatusSignal robotAccelerationX; + private StatusSignal robotAccelerationY; 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; @@ -88,6 +106,8 @@ public static CommandSwerveDrivetrain getInstance() { 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(); @@ -226,6 +246,14 @@ protected CommandSwerveDrivetrain( 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 + ); } /** @@ -347,6 +375,10 @@ private void startSimThread() { 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 @@ -359,7 +391,12 @@ private void startSimThread() { */ @Override public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { - super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds)); + // SignalLogger.writeStruct("Vision/Pose", Pose2d.struct, visionRobotPoseMeters); + // SignalLogger.writeDouble("Vision/Timestamp", timestampSeconds); + + if(checkIfVisionMeasurementValid(visionRobotPoseMeters)) { + super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds)); + } } /** @@ -381,16 +418,28 @@ public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampS * meters and radians. */ @Override - public void addVisionMeasurement( - Pose2d visionRobotPoseMeters, - double timestampSeconds, - Matrix visionMeasurementStdDevs) { - super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), - visionMeasurementStdDevs); + 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() { - return getState().Pose; + 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() { @@ -430,15 +479,11 @@ public Command followPathCommand(PathPlannerPath path) { } public SwerveModuleState[] getModuleStates() { - SwerveModuleState[] moduleStates = new SwerveModuleState[4]; - for (int i = 0; i < 4; i++) { - moduleStates[i] = getModule(i).getCurrentState(); - } - return moduleStates; + return getState().ModuleStates; } public ChassisSpeeds getChassisSpeeds() { - return getKinematics().toChassisSpeeds(getModuleStates()); + return getState().Speeds; } public Vector2D getFieldRelativeSpeeds() { @@ -669,6 +714,13 @@ public void periodicAfterScheduler() { 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/isBehindHub", isBehindHub()); @@ -702,7 +754,7 @@ public void periodicAfterScheduler() { Robot.getEnergyUtil().logEnergyUsage(getName() + " Turn", getTotalSteerSupplyCurrent()); // CAN SIGNAL LOGGING - if (Settings.DEBUG_MODE.get() && Robot.getMode() == RobotMode.DISABLED && !DriverStation.isFMSAttached()) { + 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) + ")", 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 40f684dc..b41a813e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -5,12 +5,16 @@ /** ************************************************************ */ package com.stuypulse.robot.subsystems.vision; +import java.nio.channels.Pipe; import java.util.Arrays; import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Cameras; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Cameras.Camera; +import com.stuypulse.robot.constants.Cameras.Camera.Pipeline; +import com.stuypulse.robot.constants.Cameras.Camera.RejectionValue; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.vision.LimelightHelpers; import com.stuypulse.robot.util.vision.LimelightHelpers.IMUData; @@ -21,9 +25,9 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.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; import edu.wpi.first.math.util.Units; @@ -54,11 +58,20 @@ public static LimelightVision getInstance() { private boolean hasData; private BStream debouncedHasData; + private Timer hdrTimer = new Timer(); + private boolean lastHdrEnabledVal = false; + public enum MegaTagMode { MEGATAG1, MEGATAG2 } + public void setPipeline(Pipeline pipeline) { + for(Camera camera: Cameras.LimelightCameras) { + camera.setPipeline(pipeline); + } + } + public LimelightVision() { limelightPoseArray = new Pose2d[Cameras.LimelightCameras.length]; leftLimelightPosePublisher = NetworkTableInstance.getDefault().getStructTopic("Limelight/Pose Left", Pose2d.struct).publish(); @@ -89,9 +102,14 @@ public LimelightVision() { megaTagMode = MegaTagMode.MEGATAG1; setIMUMode(1); + hdrTimer.reset(); + hdrTimer.start(); + debouncedHasData = BStream.create( () -> hasData) .filtered(new BDebounce.Both(Settings.Vision.BUZZ_DEBOUNCE)); + + setPipeline(Pipeline.NO_SUN); } public void setAllLTagWhitelist(int... ids) { @@ -231,22 +249,33 @@ public void periodicAfterScheduler() { boolean notNull = false; boolean withinAngularVelocityTolerance = false; boolean withinInvalidPositionTolerance = false; + // boolean withinTargetAreaTolerance = false; - if (poseEstimate != null && poseEstimate.tagCount > 0 ) { + if (poseEstimate != null && poseEstimate.tagCount > 0) { notNull = true; if (poseEstimate.pose.getTranslation().getDistance(Settings.Vision.INVALID_POSITION) < Settings.Vision.INVALID_POSITION_TOLERANCE_M){ withinInvalidPositionTolerance = true; + } else { + Cameras.LimelightCameras[i].incrementRejection(RejectionValue.INVALID_POSITION); } if (CommandSwerveDrivetrain.getInstance().getChassisSpeeds().omegaRadiansPerSecond < Settings.Vision.MAX_ANGULAR_VELOCITY_RAD_SEC) { withinAngularVelocityTolerance = true; + } else { + Cameras.LimelightCameras[i].incrementRejection(RejectionValue.ANGULAR_VELOCITY); } + // if (poseEstimate.avgTagArea >= Settings.Vision.MIN_TAG_AREA) { + // withinTargetAreaTolerance = true; + // } else { + // Cameras.LimelightCameras[i].incrementRejection(RejectionValue.TARGET_AREA); + // } + Pose2d robotPose = poseEstimate.pose; double timestamp = poseEstimate.timestampSeconds; - boolean isAcceptablePose = notNull && withinAngularVelocityTolerance && !withinInvalidPositionTolerance; + boolean isAcceptablePose = notNull && withinAngularVelocityTolerance && !withinInvalidPositionTolerance;// && withinTargetAreaTolerance; if (megaTagMode == MegaTagMode.MEGATAG1 && isAcceptablePose) { CommandSwerveDrivetrain.getInstance().addVisionMeasurement(robotPose, timestamp, Settings.Vision.MT1_STDEVS); @@ -280,6 +309,7 @@ public void periodicAfterScheduler() { } else { SmartDashboard.putBoolean("Vision/" + names[i] + " Has Data", false); + Cameras.LimelightCameras[i].incrementRejection(RejectionValue.NOT_NULL); } SmartDashboard.putString("Vision/MegaTag Mode", megaTagMode.toString()); @@ -287,6 +317,9 @@ public void periodicAfterScheduler() { SmartDashboard.putNumber("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); + + //Rejection counters + Cameras.LimelightCameras[i].logRejections(); } if (Settings.DEBUG_MODE.get()) { @@ -300,6 +333,25 @@ public void periodicAfterScheduler() { } } + // Alternating pipelines for hdr + if (lastHdrEnabledVal != Settings.Vision.HDR_ENABLED.get()) { + setPipeline(Pipeline.NO_SUN); + if(!lastHdrEnabledVal) { + Cameras.LimelightCameras[2].setPipeline(Pipeline.HIGH_SUN); + } + } + + lastHdrEnabledVal = Settings.Vision.HDR_ENABLED.get(); + + if(Settings.Vision.HDR_ENABLED.get()) { + if(hdrTimer.hasElapsed(Settings.Vision.HDR_TIMEOUT_SEC)) { + for(Camera camera: Cameras.LimelightCameras) { + camera.performHDR(); + } + hdrTimer.reset(); + } + } + SmartDashboard.putBoolean("Vision/Has Data", hasData); } } diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java index 0a2cc784..eae5f66a 100644 --- a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java @@ -25,8 +25,6 @@ import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - public class SOTMCalculator { public static SmartBoolean accountForAccel = new SmartBoolean("Superstructure/SOTM/account for accel", false); diff --git a/vendordeps/Phoenix6-26.1.3.json b/vendordeps/Phoenix6-26.1.3.json new file mode 100644 index 00000000..d5bc4a24 --- /dev/null +++ b/vendordeps/Phoenix6-26.1.3.json @@ -0,0 +1,449 @@ +{ + "fileName": "Phoenix6-26.1.3.json", + "name": "CTRE-Phoenix (v6)", + "version": "26.1.3", + "frcYear": "2026", + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2026-latest.json", + "conflictsWith": [ + { + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", + "offlineFileName": "Phoenix6-replay-frc2026-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "26.1.3" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "api-cpp-sim", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "26.1.3", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "26.1.3", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "26.1.3", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "26.1.3", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "26.1.3", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "26.1.3", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "26.1.3", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "26.1.3", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "26.1.3", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "26.1.3", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "26.1.3", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "26.1.3", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "26.1.3", + "libName": "CTRE_SimProCANrange", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "26.1.3", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "26.1.3", + "libName": "CTRE_SimProCANdle", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/Phoenix6-frc2026-latest.json b/vendordeps/Phoenix6-frc2026-latest.json deleted file mode 100644 index 07275f30..00000000 --- a/vendordeps/Phoenix6-frc2026-latest.json +++ /dev/null @@ -1,447 +0,0 @@ -{ - "fileName": "Phoenix6-frc2026-latest.json", - "name": "CTRE-Phoenix (v6)", - "version": "26.1.0", - "frcYear": "2026", - "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", - "mavenUrls": ["https://maven.ctr-electronics.com/release/"], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2026-latest.json", - "conflictsWith": [ - { - "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", - "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", - "offlineFileName": "Phoenix6-replay-frc2026-latest.json" - } - ], - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-java", - "version": "26.1.0" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "api-cpp", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "api-cpp-sim", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFXS", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANrange", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANdi", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANdle", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-cpp", - "version": "26.1.0", - "libName": "CTRE_Phoenix6_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "26.1.0", - "libName": "CTRE_PhoenixTools", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "26.1.0", - "libName": "CTRE_Phoenix6_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "26.1.0", - "libName": "CTRE_PhoenixTools_Sim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "26.1.0", - "libName": "CTRE_SimTalonSRX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "26.1.0", - "libName": "CTRE_SimVictorSPX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "26.1.0", - "libName": "CTRE_SimPigeonIMU", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "26.1.0", - "libName": "CTRE_SimProTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFXS", - "version": "26.1.0", - "libName": "CTRE_SimProTalonFXS", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "26.1.0", - "libName": "CTRE_SimProCANcoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "26.1.0", - "libName": "CTRE_SimProPigeon2", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANrange", - "version": "26.1.0", - "libName": "CTRE_SimProCANrange", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANdi", - "version": "26.1.0", - "libName": "CTRE_SimProCANdi", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANdle", - "version": "26.1.0", - "libName": "CTRE_SimProCANdle", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - } - ] -}