diff --git a/src/main/deploy/elastic-layout.json b/src/main/deploy/elastic-layout.json index 54ccf5f3..40397540 100644 --- a/src/main/deploy/elastic-layout.json +++ b/src/main/deploy/elastic-layout.json @@ -3,130 +3,33 @@ "grid_size": 128, "tabs": [ { - "name": "Match", + "name": "Teleoperated", + "grid_layout": { + "layouts": [], + "containers": [] + } + }, + { + "name": "Autonomous", + "grid_layout": { + "layouts": [], + "containers": [] + } + }, + { + "name": "ArmSensor", "grid_layout": { "layouts": [], "containers": [ { - "title": "has arm sensor", + "title": "inTrough", "x": 0.0, "y": 0.0, "width": 128.0, "height": 128.0, "type": "Boolean Box", "properties": { - "topic": "/Shuffleboard/Match/has arm sensor", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "has ground intake sensor", - "x": 128.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Match/has ground intake sensor", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "has left branch sensor", - "x": 0.0, - "y": 128.0, - "width": 128.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Match/has left branch sensor", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "has right branch sensor", - "x": 128.0, - "y": 128.0, - "width": 128.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Match/has right branch sensor", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "has recent vision measurements", - "x": 384.0, - "y": 0.0, - "width": 256.0, - "height": 256.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Match/has recent vision measurements", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "left cam", - "x": 256.0, - "y": 256.0, - "width": 512.0, - "height": 512.0, - "type": "Camera Stream", - "properties": { - "topic": "/CameraPublisher/left", - "period": 0.06, - "rotation_turns": 0 - } - }, - { - "title": "right cam", - "x": 768.0, - "y": 256.0, - "width": 512.0, - "height": 512.0, - "type": "Camera Stream", - "properties": { - "topic": "/CameraPublisher/right", - "period": 0.06, - "rotation_turns": 0 - } - }, - { - "title": "Is Zeroed", - "x": 640.0, - "y": 0.0, - "width": 256.0, - "height": 256.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Elevator/Is Zeroed", + "topic": "/Shuffleboard/ArmSensor/inTrough", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -139,465 +42,44 @@ } }, { - "name": "Autos", + "name": "IntakeSensor", "grid_layout": { "layouts": [], "containers": [ { - "title": "Auto Delay", - "x": 640.0, - "y": 384.0, - "width": 256.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Autos/Auto Delay", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "Starting Position", - "x": 512.0, - "y": 0.0, - "width": 256.0, - "height": 128.0, - "type": "ComboBox Chooser", - "properties": { - "topic": "/Shuffleboard/Autos/Starting Position", - "period": 0.06, - "sort_options": false - } - }, - { - "title": "Game Objects", - "x": 768.0, - "y": 0.0, - "width": 256.0, - "height": 128.0, - "type": "ComboBox Chooser", - "properties": { - "topic": "/Shuffleboard/Autos/Game Objects", - "period": 0.06, - "sort_options": false - } - }, - { - "title": "Available Auto Variants", - "x": 512.0, - "y": 128.0, - "width": 512.0, - "height": 256.0, - "type": "ComboBox Chooser", - "properties": { - "topic": "/Shuffleboard/Autos/Available Auto Variants", - "period": 0.06, - "sort_options": false - } - }, - { - "title": "Close Enough?", - "x": 1152.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Autos/Close Enough?", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "Level?", - "x": 1280.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Autos/Level?", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "Stationary?", - "x": 1408.0, + "title": "In Intake", + "x": 0.0, "y": 0.0, "width": 128.0, "height": 128.0, "type": "Boolean Box", "properties": { - "topic": "/Shuffleboard/Autos/Stationary?", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "readyToScore?", - "x": 1152.0, - "y": 128.0, - "width": 384.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Autos/readyToScore?", + "topic": "/Shuffleboard/IntakeSensor/In Intake", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, "false_color": 4294198070, "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "Selected auto", - "x": 0.0, - "y": 256.0, - "width": 512.0, - "height": 384.0, - "type": "Field", - "properties": { - "topic": "/Shuffleboard/Field/Selected auto", - "period": 0.06, - "field_game": "Reefscape", - "robot_width": 0.85, - "robot_length": 0.85, - "show_other_objects": true, - "show_trajectories": true, - "field_rotation": 0.0, - "robot_color": 4294198070, - "trajectory_color": 4294967295 - } - }, - { - "title": "Start pose", - "x": 1024.0, - "y": 256.0, - "width": 512.0, - "height": 384.0, - "type": "Field", - "properties": { - "topic": "/Shuffleboard/Field/Start pose", - "period": 0.06, - "field_game": "Reefscape", - "robot_width": 0.85, - "robot_length": 0.85, - "show_other_objects": true, - "show_trajectories": true, - "field_rotation": 0.0, - "robot_color": 4294198070, - "trajectory_color": 4294967295 - } - }, - { - "title": "Auto display speed", - "x": 0.0, - "y": 128.0, - "width": 256.0, - "height": 128.0, - "type": "Number Slider", - "properties": { - "topic": "/Shuffleboard/Field/Auto display speed", - "period": 0.06, - "data_type": "double", - "min_value": -0.5, - "max_value": 3.0, - "divisions": 5, - "update_continuously": false - } - } - ] - } - }, - { - "name": "AprilTags", - "grid_layout": { - "layouts": [], - "containers": [ - { - "title": "Last raw timestamp", - "x": 0.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/AprilTags/Last raw timestamp", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "Num targets", - "x": 0.0, - "y": 128.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/AprilTags/Num targets", - "period": 0.06, - "data_type": "int", - "show_submit_button": false - } - }, - { - "title": "Last timestamp", - "x": 128.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/AprilTags/Last timestamp", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "april tag distance meters", - "x": 128.0, - "y": 128.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/AprilTags/april tag distance meters", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "time since last reading", - "x": 256.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/AprilTags/time since last reading", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "Disable vision", - "x": 512.0, - "y": 0.0, - "width": 384.0, - "height": 256.0, - "type": "Toggle Button", - "properties": { - "topic": "/Shuffleboard/AprilTags/Disable vision", - "period": 0.06, - "data_type": "boolean" - } - } - ] - } - }, - { - "name": "Elevator", - "grid_layout": { - "layouts": [], - "containers": [ - { - "title": "Elevator Speed", - "x": 640.0, - "y": 0.0, - "width": 256.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Elevator/Elevator Speed", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "Is Zeroed", - "x": 512.0, - "y": 128.0, - "width": 512.0, - "height": 512.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Elevator/Is Zeroed", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "M1 at forward softstop", - "x": 0.0, - "y": 128.0, - "width": 256.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Elevator/M1 at forward softstop", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "M1 at reverse softstop", - "x": 0.0, - "y": 256.0, - "width": 256.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Elevator/M1 at reverse softstop", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "M1 output voltage", - "x": 0.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Elevator/M1 output voltage", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "M1 supply current", - "x": 128.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Elevator/M1 supply current", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "M2 at forward softstop", - "x": 1280.0, - "y": 128.0, - "width": 256.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Elevator/M2 at forward softstop", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "M2 at reverse softstop", - "x": 1280.0, - "y": 256.0, - "width": 256.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Elevator/M2 at reverse softstop", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "M2 output voltage", - "x": 1408.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Elevator/M2 output voltage", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "M2 supply current", - "x": 1280.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Elevator/M2 supply current", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "M2 temp", - "x": 1152.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Elevator/M2 temp", - "period": 0.06, - "data_type": "double", - "show_submit_button": false + "false_icon": "None" } - }, + } + ] + } + }, + { + "name": "BranchSensor", + "grid_layout": { + "layouts": [], + "containers": [ { - "title": "M1 temp", - "x": 256.0, + "title": "LeftDistance(m)", + "x": 0.0, "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Elevator/M1 temp", + "topic": "/Shuffleboard/BranchSensor/LeftDistance(m)", "period": 0.06, "data_type": "double", "show_submit_button": false @@ -607,179 +89,132 @@ } }, { - "name": "End Effector", + "name": "AprilTags", "grid_layout": { "layouts": [], "containers": [ { - "title": "Pivot Target Pos", - "x": 512.0, + "title": "Last raw timestamp", + "x": 0.0, "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Arm Pivot/Pivot Target Pos", + "topic": "/Shuffleboard/AprilTags/Last raw timestamp", "period": 0.06, "data_type": "double", "show_submit_button": false } }, { - "title": "Pivot Speed", - "x": 256.0, - "y": 0.0, + "title": "Num targets", + "x": 0.0, + "y": 128.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Arm Pivot/Pivot Speed", + "topic": "/Shuffleboard/AprilTags/Num targets", "period": 0.06, - "data_type": "double", + "data_type": "int", "show_submit_button": false } }, { - "title": "Pivot Position", + "title": "Last timestamp", "x": 128.0, "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Arm Pivot/Pivot Position", + "topic": "/Shuffleboard/AprilTags/Last timestamp", "period": 0.06, "data_type": "double", "show_submit_button": false } }, { - "title": "Pivot Motor rotor Pos", - "x": 0.0, - "y": 0.0, + "title": "april tag distance meters", + "x": 128.0, + "y": 128.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Arm Pivot/Pivot Motor rotor Pos", + "topic": "/Shuffleboard/AprilTags/april tag distance meters", "period": 0.06, "data_type": "double", "show_submit_button": false } }, { - "title": "Pivot Motor Temperature", - "x": 384.0, + "title": "time since last reading", + "x": 256.0, "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Arm Pivot/Pivot Motor Temperature", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "Distance(m)", - "x": 640.0, - "y": 512.0, - "width": 256.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/ArmSensor/Distance(m)", + "topic": "/Shuffleboard/AprilTags/time since last reading", "period": 0.06, "data_type": "double", "show_submit_button": false } }, { - "title": "inClaw", + "title": "Disable vision", "x": 512.0, - "y": 256.0, - "width": 256.0, - "height": 256.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/ArmSensor/inClaw", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "inTrough", - "x": 768.0, - "y": 256.0, - "width": 256.0, + "y": 0.0, + "width": 384.0, "height": 256.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/ArmSensor/inTrough", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "Claw Motor Temperature", - "x": 1280.0, - "y": 128.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Claw/Claw Motor Temperature", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "Claw Speed", - "x": 1024.0, - "y": 128.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", + "type": "Toggle Button", "properties": { - "topic": "/Shuffleboard/Claw/Claw Speed", + "topic": "/Shuffleboard/AprilTags/Disable vision", "period": 0.06, - "data_type": "double", - "show_submit_button": false + "data_type": "boolean" } - }, + } + ] + } + }, + { + "name": "Elevator", + "grid_layout": { + "layouts": [], + "containers": [ { - "title": "Last Set Power", - "x": 1152.0, - "y": 128.0, + "title": "Motor Current Position", + "x": 0.0, + "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Claw/Last Set Power", + "topic": "/Shuffleboard/Elevator/Motor Current Position", "period": 0.06, "data_type": "double", "show_submit_button": false } - }, + } + ] + } + }, + { + "name": "Arm Pivot", + "grid_layout": { + "layouts": [], + "containers": [ { - "title": "Motor Voltage", - "x": 1408.0, - "y": 128.0, + "title": "Pivot Speed", + "x": 0.0, + "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Claw/Motor Voltage", + "topic": "/Shuffleboard/Arm Pivot/Pivot Speed", "period": 0.06, "data_type": "double", "show_submit_button": false @@ -795,9 +230,9 @@ "containers": [ { "title": "Is Climb OUT?", - "x": 640.0, - "y": 128.0, - "width": 256.0, + "x": 0.0, + "y": 0.0, + "width": 128.0, "height": 128.0, "type": "Boolean Box", "properties": { @@ -809,137 +244,68 @@ "true_icon": "None", "false_icon": "None" } - }, + } + ] + } + }, + { + "name": "Claw", + "grid_layout": { + "layouts": [], + "containers": [ { - "title": "Is Climb STOWED?", - "x": 640.0, + "title": "Claw Speed", + "x": 0.0, "y": 0.0, - "width": 256.0, - "height": 128.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Climb/Is Climb STOWED?", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "Motor Position", - "x": 1152.0, - "y": 128.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Climb/Motor Position", + "topic": "/Shuffleboard/Claw/Claw Speed", "period": 0.06, "data_type": "double", "show_submit_button": false } - }, + } + ] + } + }, + { + "name": "GroundIntake", + "grid_layout": { + "layouts": [], + "containers": [ { - "title": "Motor Speed", - "x": 1152.0, + "title": "Speed", + "x": 0.0, "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Climb/Motor Speed", + "topic": "/Shuffleboard/GroundIntake/Speed", "period": 0.06, "data_type": "double", "show_submit_button": false } - }, - { - "title": "Move Complete?", - "x": 384.0, - "y": 256.0, - "width": 256.0, - "height": 256.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Climb/Move Complete?", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, + } + ] + } + }, + { + "name": "Ground Arm", + "grid_layout": { + "layouts": [], + "containers": [ { - "title": "Set speed", - "x": 1280.0, + "title": "Pivot Speed", + "x": 0.0, "y": 0.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/Climb/Set speed", - "period": 0.06, - "data_type": "double", - "show_submit_button": false - } - }, - { - "title": "Where Move next?", - "x": 640.0, - "y": 256.0, - "width": 256.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Climb/Where Move next?", - "period": 0.06, - "data_type": "string", - "show_submit_button": false - } - }, - { - "title": "Where moving?", - "x": 640.0, - "y": 384.0, - "width": 256.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Climb/Where moving?", - "period": 0.06, - "data_type": "string", - "show_submit_button": false - } - }, - { - "title": "Within Tolerance?", - "x": 896.0, - "y": 256.0, - "width": 256.0, - "height": 256.0, - "type": "Boolean Box", - "properties": { - "topic": "/Shuffleboard/Climb/Within Tolerance?", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "targetPos", - "x": 1280.0, - "y": 128.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Shuffleboard/Climb/targetPos", + "topic": "/Shuffleboard/Ground Arm/Pivot Speed", "period": 0.06, "data_type": "double", "show_submit_button": false @@ -949,212 +315,316 @@ } }, { - "name": "GroundIntake", + "name": "Controls", "grid_layout": { "layouts": [], "containers": [ { - "title": "Speed", + "title": "Swerve Coast Mode", "x": 0.0, "y": 0.0, "width": 128.0, "height": 128.0, - "type": "Text Display", + "type": "Toggle Button", "properties": { - "topic": "/Shuffleboard/GroundIntake/Speed", + "topic": "/Shuffleboard/Controls/Swerve Coast Mode", "period": 0.06, - "data_type": "double", - "show_submit_button": false + "data_type": "boolean" } - }, + } + ] + } + }, + { + "name": "Autos", + "grid_layout": { + "layouts": [], + "containers": [ { - "title": "Pivot Motor Temperature", + "title": "readyToScore?", "x": 0.0, - "y": 384.0, + "y": 0.0, "width": 128.0, "height": 128.0, - "type": "Text Display", + "type": "Boolean Box", "properties": { - "topic": "/Shuffleboard/Ground Arm/Pivot Motor Temperature", + "topic": "/Shuffleboard/Autos/readyToScore?", "period": 0.06, - "data_type": "double", - "show_submit_button": false + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" } }, { - "title": "Pivot Motor rotor Pos", - "x": 128.0, - "y": 384.0, - "width": 128.0, + "title": "Starting Position", + "x": 512.0, + "y": 0.0, + "width": 256.0, "height": 128.0, - "type": "Text Display", + "type": "ComboBox Chooser", "properties": { - "topic": "/Shuffleboard/Ground Arm/Pivot Motor rotor Pos", + "topic": "/Shuffleboard/Autos/Starting Position", "period": 0.06, - "data_type": "double", - "show_submit_button": false + "sort_options": false } }, { - "title": "Pivot Position", - "x": 256.0, - "y": 384.0, + "title": "Launch Type", + "x": 512.0, + "y": 128.0, "width": 128.0, "height": 128.0, - "type": "Text Display", + "type": "ComboBox Chooser", "properties": { - "topic": "/Shuffleboard/Ground Arm/Pivot Position", + "topic": "/Shuffleboard/Autos/Launch Type", "period": 0.06, - "data_type": "double", - "show_submit_button": false + "sort_options": false } }, { - "title": "Pivot Speed", - "x": 384.0, - "y": 384.0, + "title": "Game Objects", + "x": 640.0, + "y": 128.0, "width": 128.0, "height": 128.0, - "type": "Text Display", + "type": "ComboBox Chooser", "properties": { - "topic": "/Shuffleboard/Ground Arm/Pivot Speed", + "topic": "/Shuffleboard/Autos/Game Objects", "period": 0.06, - "data_type": "double", - "show_submit_button": false + "sort_options": false } }, { - "title": "Pivot Target Pos", + "title": "Available Auto Variants", "x": 512.0, - "y": 384.0, + "y": 256.0, "width": 256.0, "height": 128.0, - "type": "Text Display", + "type": "ComboBox Chooser", "properties": { - "topic": "/Shuffleboard/Ground Arm/Pivot Target Pos", + "topic": "/Shuffleboard/Autos/Available Auto Variants", "period": 0.06, - "data_type": "double", - "show_submit_button": false + "sort_options": false } }, { - "title": "Last Set Power", - "x": 0.0, - "y": 128.0, + "title": "Auto Delay", + "x": 512.0, + "y": 384.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/GroundIntake/Last Set Power", + "topic": "/Shuffleboard/Autos/Auto Delay", "period": 0.06, "data_type": "double", "show_submit_button": false } }, { - "title": "Motor Temperature", - "x": 128.0, - "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Text Display", + "title": "photonvision_Port_1182_Output_MJPEG_Server", + "x": 256.0, + "y": 512.0, + "width": 384.0, + "height": 384.0, + "type": "Camera Stream", "properties": { - "topic": "/Shuffleboard/GroundIntake/Motor Temperature", + "topic": "/CameraPublisher/photonvision_Port_1182_Output_MJPEG_Server", "period": 0.06, - "data_type": "double", - "show_submit_button": false + "rotation_turns": 0 } }, { - "title": "Motor Voltage", - "x": 128.0, - "y": 128.0, - "width": 128.0, + "title": "photonvision_Port_1186_Output_MJPEG_Server", + "x": 640.0, + "y": 512.0, + "width": 384.0, + "height": 384.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/photonvision_Port_1186_Output_MJPEG_Server", + "period": 0.06, + "rotation_turns": 0 + } + } + ] + } + }, + { + "name": "Field", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Auto display speed", + "x": 1280.0, + "y": 640.0, + "width": 384.0, "height": 128.0, - "type": "Text Display", + "type": "Number Slider", "properties": { - "topic": "/Shuffleboard/GroundIntake/Motor Voltage", + "topic": "/Shuffleboard/Field/Auto display speed", "period": 0.06, "data_type": "double", - "show_submit_button": false + "min_value": -1.0, + "max_value": 1.0, + "divisions": 5, + "update_continuously": false } }, { - "title": "Distance(m)", - "x": 640.0, - "y": 0.0, + "title": "Est. Time (s)", + "x": 1280.0, + "y": 512.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shuffleboard/IntakeSensor/Distance(m)", + "topic": "/Shuffleboard/Field/Est. Time (s)", "period": 0.06, "data_type": "double", "show_submit_button": false } }, { - "title": "In Intake", - "x": 768.0, + "title": "Selected auto", + "x": 0.0, "y": 0.0, - "width": 128.0, - "height": 128.0, - "type": "Boolean Box", + "width": 1280.0, + "height": 768.0, + "type": "Field", "properties": { - "topic": "/Shuffleboard/IntakeSensor/In Intake", + "topic": "/Shuffleboard/Field/Selected auto", "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" + "field_game": "Reefscape", + "robot_width": 0.85, + "robot_length": 0.85, + "show_other_objects": true, + "show_trajectories": true, + "field_rotation": 0.0, + "robot_color": 4294198070, + "trajectory_color": 4294967295 } } ] } }, { - "name": "Controls", + "name": "Match", "grid_layout": { "layouts": [], "containers": [ { - "title": "Elevator Coast Mode", - "x": 384.0, + "title": "has arm sensor", + "x": 0.0, "y": 0.0, "width": 128.0, "height": 128.0, - "type": "Toggle Button", + "type": "Boolean Box", "properties": { - "topic": "/Shuffleboard/Controls/Elevator Coast Mode", + "topic": "/Shuffleboard/Match/has arm sensor", "period": 0.06, - "data_type": "boolean" + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" } }, { - "title": "Swerve Coast Mode", - "x": 512.0, + "title": "has ground intake sensor", + "x": 128.0, "y": 0.0, "width": 128.0, "height": 128.0, - "type": "Toggle Button", + "type": "Boolean Box", "properties": { - "topic": "/Shuffleboard/Controls/Swerve Coast Mode", + "topic": "/Shuffleboard/Match/has ground intake sensor", "period": 0.06, - "data_type": "boolean" + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" } }, { - "title": "Climb Coast Mode", - "x": 640.0, - "y": 0.0, + "title": "has left branch sensor", + "x": 0.0, + "y": 128.0, "width": 128.0, "height": 128.0, - "type": "Toggle Button", + "type": "Boolean Box", "properties": { - "topic": "/Shuffleboard/Controls/Climb Coast Mode", + "topic": "/Shuffleboard/Match/has left branch sensor", "period": 0.06, - "data_type": "boolean" + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "has right branch sensor", + "x": 128.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Shuffleboard/Match/has right branch sensor", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "has recent vision measurements", + "x": 384.0, + "y": 0.0, + "width": 256.0, + "height": 256.0, + "type": "Boolean Box", + "properties": { + "topic": "/Shuffleboard/Match/has recent vision measurements", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "left cam", + "x": 256.0, + "y": 256.0, + "width": 512.0, + "height": 512.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/left", + "period": 0.06, + "rotation_turns": 0 + } + }, + { + "title": "right cam", + "x": 768.0, + "y": 256.0, + "width": 512.0, + "height": 512.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/right", + "period": 0.06, + "rotation_turns": 0 } } ] diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 2ba35c91..0adda27d 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -37,6 +37,7 @@ import frc.robot.util.BranchHeight; import frc.robot.util.RobotType; import frc.robot.util.ScoringMode; +import frc.robot.util.ScoringType; import frc.robot.util.SoloScoringMode; import java.util.function.BooleanSupplier; @@ -83,8 +84,7 @@ public class Controls { new SwerveRequest.FieldCentric() .withDeadband(0.0001) .withRotationalDeadband(0.0001) - .withDriveRequestType( - DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors + .withDriveRequestType(DriveRequestType.Velocity); private final Telemetry logger = new Telemetry(MaxSpeed); @@ -112,6 +112,14 @@ public Controls(Subsystems s, Sensors sensors, SuperStructure superStructure) { configureGroundSpinnyBindings(); configureGroundArmBindings(); configureSoloControllerBindings(); + Shuffleboard.getTab("Elevator") + .addBoolean("Intaking mode Algae", () -> intakeMode == ScoringMode.ALGAE); + Shuffleboard.getTab("Elevator") + .addString("Scoring Mode", () -> getSoloScoringMode().toString()); + } + + public SoloScoringMode getSoloScoringMode() { + return soloScoringMode; } private Trigger connected(CommandXboxController controller) { @@ -287,7 +295,7 @@ private void configureSuperStructureBindings() { Commands.deferredProxy( () -> switch (scoringMode) { - case CORAL -> getCoralBranchHeightCommand(); + case CORAL -> getCoralBranchHeightCommand(ScoringType.DRIVER); case ALGAE -> Commands.sequence( superStructure.algaeProcessorScore( driverController.rightBumper()), @@ -394,12 +402,7 @@ private void configureSuperStructureBindings() { case ALGAE -> soloScoringMode = SoloScoringMode.ALGAE_IN_CLAW; } }) - .withName("Set solo scoring mode")); - - sensors - .armSensor - .inClaw() - .and(RobotModeTriggers.teleop()) + .withName("Set solo scoring mode")) .onFalse( Commands.runOnce( () -> { @@ -423,7 +426,7 @@ private void configureSuperStructureBindings() { () -> { Command scoreCommand = switch (scoringMode) { - case CORAL -> getCoralBranchHeightCommand(); + case CORAL -> getCoralBranchHeightCommand(ScoringType.DRIVER); case ALGAE -> Commands.sequence( BargeAlign.bargeScore( s.drivebaseSubsystem, @@ -448,30 +451,55 @@ private Command getAlgaeIntakeCommand() { }; } - private Command getCoralBranchHeightCommand() { - return switch (branchHeight) { - case CORAL_LEVEL_FOUR -> superStructure.coralLevelFour(driverController.rightBumper()); - case CORAL_LEVEL_THREE -> superStructure.coralLevelThree(driverController.rightBumper()); - case CORAL_LEVEL_TWO -> superStructure.coralLevelTwo(driverController.rightBumper()); - case CORAL_LEVEL_ONE -> superStructure.coralLevelOne(driverController.rightBumper()); - }; - } - - private Command getSoloCoralBranchHeightCommand() { - return switch (branchHeight) { - case CORAL_LEVEL_FOUR -> superStructure - .coralLevelFour(soloController.rightBumper()) - .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); - case CORAL_LEVEL_THREE -> superStructure - .coralLevelThree(soloController.rightBumper()) - .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); - case CORAL_LEVEL_TWO -> superStructure - .coralLevelTwo(soloController.rightBumper()) - .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); - case CORAL_LEVEL_ONE -> superStructure - .coralLevelOne(soloController.rightBumper()) - .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); - }; + private Command getCoralBranchHeightCommand(ScoringType version) { + if (version == ScoringType.SOLOC_LEFT) { + return switch (branchHeight) { + case CORAL_LEVEL_FOUR -> superStructure + .coralLevelFour(soloController.rightBumper()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_THREE -> superStructure + .coralLevelThree( + soloController.rightBumper(), + () -> AutoAlign.poseInPlace(AutoAlign.AlignType.LEFTB)) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_TWO -> superStructure + .coralLevelTwo( + soloController.rightBumper(), + () -> AutoAlign.poseInPlace(AutoAlign.AlignType.LEFTB)) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_ONE -> superStructure + .coralLevelOne( + soloController.rightBumper(), () -> AutoAlign.poseInPlace(AutoAlign.AlignType.L1LB)) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + }; + } else if (version == ScoringType.SOLOC_RIGHT) { + return switch (branchHeight) { + case CORAL_LEVEL_FOUR -> superStructure + .coralLevelFour(soloController.rightBumper()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_THREE -> superStructure + .coralLevelThree( + soloController.rightBumper(), + () -> AutoAlign.poseInPlace(AutoAlign.AlignType.RIGHTB)) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_TWO -> superStructure + .coralLevelTwo( + soloController.rightBumper(), + () -> AutoAlign.poseInPlace(AutoAlign.AlignType.RIGHTB)) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_ONE -> superStructure + .coralLevelOne( + soloController.rightBumper(), () -> AutoAlign.poseInPlace(AutoAlign.AlignType.L1RB)) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + }; + } else { + return switch (branchHeight) { + case CORAL_LEVEL_FOUR -> superStructure.coralLevelFour(driverController.rightBumper()); + case CORAL_LEVEL_THREE -> superStructure.coralLevelThree(driverController.rightBumper()); + case CORAL_LEVEL_TWO -> superStructure.coralLevelTwo(driverController.rightBumper()); + case CORAL_LEVEL_ONE -> superStructure.coralLevelOne(driverController.rightBumper()); + }; + } } private void configureElevatorBindings() { @@ -806,12 +834,12 @@ private void configureAutoAlignBindings() { .rightTrigger() .and(() -> scoringMode == ScoringMode.CORAL) .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) - .whileTrue(AutoAlign.autoAlignRight(s.drivebaseSubsystem, this)); + .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.RIGHTB)); driverController .leftTrigger() .and(() -> scoringMode == ScoringMode.CORAL) .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) - .whileTrue(AutoAlign.autoAlignLeft(s.drivebaseSubsystem, this)); + .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.LEFTB)); } private void configureGroundSpinnyBindings() { @@ -926,7 +954,7 @@ private void configureSoloControllerBindings() { switch (soloScoringMode) { case CORAL_IN_CLAW -> { scoreCommand = - getSoloCoralBranchHeightCommand() + getCoralBranchHeightCommand(ScoringType.SOLOC_LEFT) .until( () -> soloController.a().getAsBoolean() @@ -968,7 +996,12 @@ private void configureSoloControllerBindings() { .leftTrigger() .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) - .whileTrue(AutoAlign.autoAlignLeft(s.drivebaseSubsystem, this)); + .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.LEFTB)); + soloController + .leftTrigger() + .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) + .and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE) + .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.L1LB)); // Processor + Auto align right + Select scoring mode Coral soloController .rightTrigger() @@ -977,7 +1010,8 @@ private void configureSoloControllerBindings() { () -> { Command scoreCommand = switch (soloScoringMode) { - case CORAL_IN_CLAW -> getSoloCoralBranchHeightCommand() + case CORAL_IN_CLAW -> getCoralBranchHeightCommand( + ScoringType.SOLOC_RIGHT) .until( () -> soloController.a().getAsBoolean() @@ -1005,7 +1039,12 @@ private void configureSoloControllerBindings() { .rightTrigger() .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) - .whileTrue(AutoAlign.autoAlignRight(s.drivebaseSubsystem, this)); + .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.RIGHTB)); + soloController + .rightTrigger() + .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) + .and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE) + .whileTrue(AutoAlign.autoAlign(s.drivebaseSubsystem, this, AutoAlign.AlignType.L1RB)); // Ground Intake soloController .leftBumper() @@ -1095,11 +1134,5 @@ private void configureSoloControllerBindings() { .startMovingVoltage( () -> Volts.of(ElevatorSubsystem.UP_VOLTAGE * -soloController.getLeftY())) .withName("Elevator Manual Control")); - // Ready to score rumble - Trigger readyToScore = new Trigger(() -> AutoAlign.poseInPlace()); - readyToScore.onTrue( - Commands.runOnce(() -> soloController.setRumble(RumbleType.kBothRumble, 1.0))); - readyToScore.onFalse( - Commands.runOnce(() -> soloController.setRumble(RumbleType.kBothRumble, 0.0))); } } diff --git a/src/main/java/frc/robot/generated/CompTunerConstants.java b/src/main/java/frc/robot/generated/CompTunerConstants.java index 179d2b32..a9394ce9 100644 --- a/src/main/java/frc/robot/generated/CompTunerConstants.java +++ b/src/main/java/frc/robot/generated/CompTunerConstants.java @@ -54,7 +54,7 @@ public class CompTunerConstants { // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = - new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124); + new Slot0Configs().withKP(0.2).withKI(0).withKD(0).withKS(0.1).withKV(0.124).withKA(0); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors @@ -80,7 +80,14 @@ public class CompTunerConstants { // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration driveInitialConfigs = + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(Amps.of(160)) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit(Amps.of(100)) + .withSupplyCurrentLimitEnable(true)); private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() .withCurrentLimits( @@ -89,7 +96,9 @@ public class CompTunerConstants { // low // stator current limit to help avoid brownouts without impacting performance. .withStatorCurrentLimit(Amps.of(60)) - .withStatorCurrentLimitEnable(true)); + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit(Amps.of(40)) + .withSupplyCurrentLimitEnable(true)); private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs private static final Pigeon2Configuration pigeonConfigs = null; diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index d37b680b..41ec29d6 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -67,6 +67,19 @@ private Command repeatPrescoreScoreSwing(Command command, BooleanSupplier score) } } + private Command repeatPrescoreScoreSwing( + Command command, BooleanSupplier score, BooleanSupplier ipScore) { + // repeats scoring sequence if the coral is still in the claw + if (armSensor == null) { + return Commands.sequence( + command, + Commands.waitUntil(() -> !score.getAsBoolean()), + Commands.waitUntil(ipScore).until(score)); + } else { + return command.repeatedly().onlyWhile(armSensor.inClaw()); + } + } + public Command coralLevelFour(BooleanSupplier score) { if (branchSensors != null) { // checks if sensor enabled then use for faster scoring score = branchSensors.withinScoreRange().or(score); @@ -168,6 +181,85 @@ public Command coralLevelOne(BooleanSupplier score) { .withName("Coral Level 1"); } + // New versions with ipScore + // if robot is in position for intermediate scoring, it can score early but if vision is dead + // manual control still works + // only used on solo controls + + public Command coralLevelThree(BooleanSupplier score, BooleanSupplier ipScore) { // same as L4 + return Commands.sequence( + Commands.parallel( + elevator + .setLevel(ElevatorSubsystem.CORAL_LEVEL_THREE_PRE_POS) + .deadlineFor( + armPivot + .moveToPosition(ArmPivot.CORAL_PRESET_UP) + .until(ipScore) + .until(score)), + spinnyClaw.stop()) + .withTimeout(0.5), + repeatPrescoreScoreSwing( + Commands.repeatingSequence( + armPivot + .moveToPosition(ArmPivot.CORAL_PRESET_L3) + .withDeadline(Commands.waitUntil(ipScore).until(score)), + armPivot + .moveToPosition(ArmPivot.CORAL_PRESET_DOWN) + .withTimeout(1.5) + .until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))), + score, + ipScore), + coralPreIntake()) + .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L3").asProxy()) + .withName("Coral Level 3"); + } + + public Command coralLevelTwo( + BooleanSupplier score, BooleanSupplier ipScore) { // same as L4 and L3 + return Commands.sequence( + Commands.parallel( + elevator + .setLevel(ElevatorSubsystem.CORAL_LEVEL_TWO_PRE_POS) + .deadlineFor( + armPivot + .moveToPosition(ArmPivot.CORAL_PRESET_UP) + .until(ipScore) + .until(score)), + spinnyClaw.stop()) + .withTimeout(0.5), + repeatPrescoreScoreSwing( + Commands.sequence( + armPivot + .moveToPosition(ArmPivot.CORAL_PRESET_L2) + .withDeadline(Commands.waitUntil(ipScore).until(score)), + armPivot + .moveToPosition(ArmPivot.CORAL_PRESET_DOWN) + .withTimeout(1.5) + .until(armPivot.atAngle(ArmPivot.CORAL_POST_SCORE))), + score, + ipScore), + coralPreIntake()) + .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L2").asProxy()) + .withName("Coral Level 2"); + } + + public Command coralLevelOne(BooleanSupplier score, BooleanSupplier ipScore) { + return Commands.sequence( + Commands.parallel( + elevator.setLevel(ElevatorSubsystem.CORAL_LEVEL_ONE_POS), + armPivot.moveToPosition(ArmPivot.CORAL_PRESET_L1), + spinnyClaw.stop()) // holds coral without wearing flywheels + .withTimeout(0.5) + .withDeadline(Commands.waitUntil(ipScore).until(score)), + spinnyClaw + .coralLevelOneHoldExtakePower() + .withTimeout(0.5 /* this time could be shorter */), // spits out coral + Commands.waitSeconds(1), // Wait to clear the reef + coralPreIntake()) + .deadlineFor(colorSet(0, 255, 0, "Green - Aligned With L1").asProxy()) + .withName("Coral Level 1"); + } + // quickGroundIntake() is used instead since it's faster and still reliable. // (This one moves the coral intake the normal intake position, then does the normal intake. // quickGroundIntake() instead hands the coral directly to the claw.) @@ -216,11 +308,11 @@ public Command quickGroundIntake(BooleanSupplier retract) { // thanks joseph // Make the big sequence. return Commands.sequence( // Initial setup- Move elevator high enough for ground arm to be clear, start moving - // arm pivot, stop the spinny claw, and start spinning the ground intake + // arm pivot, and start spinning the ground intake Commands.parallel( elevator.setLevel(ElevatorSubsystem.MIN_EMPTY_GROUND_INTAKE), armPivot.moveToPosition(ArmPivot.CORAL_QUICK_INTAKE), - spinnyClaw.stop(), // just as a backup in case things are silly + spinnyClaw.coralIntakePower(), groundSpinny.setGroundIntakePower()) // Move on even if arm isn't in position yet as long as elevator is high enough .until(elevator.above(ElevatorSubsystem.MIN_EMPTY_GROUND_INTAKE)), diff --git a/src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java b/src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java index f3ee497a..4592ed7d 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java @@ -3,12 +3,11 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Controls; import frc.robot.subsystems.SuperStructure; import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain; +import frc.robot.util.AllianceUtils; import frc.robot.util.ScoringMode; import java.util.List; @@ -54,20 +53,14 @@ public static Command autoAlgaeIntakeCommand( return new AutoAlgaeHeights(drivebase, s, c); } - private static boolean isBlue() { - return DriverStation.getAlliance() - .map(alliance -> alliance.equals(Alliance.Blue)) - .orElse(false); - } - private static Pose2d getNearestAlgae(Pose2d robotPose) { - List poses = isBlue() ? blueAlgaePoses : redAlgaePoses; + List poses = AllianceUtils.isBlue() ? blueAlgaePoses : redAlgaePoses; return robotPose.nearest(poses); } private String aprilTagToAlgaeHeight(Pose2d algaePose) { // Map poses to two heights only - List poses = isBlue() ? blueAlgaePoses : redAlgaePoses; + List poses = AllianceUtils.isBlue() ? blueAlgaePoses : redAlgaePoses; int index = poses.indexOf(algaePose); if (index >= 3) { diff --git a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java index 0f0ad2d4..93df784a 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java @@ -12,44 +12,28 @@ import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Controls; import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain; +import frc.robot.util.AllianceUtils; import java.util.List; public class AutoAlign { - public static Command autoAlign(CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { - return new AutoAlignCommand(drivebaseSubsystem, controls).withName("Auto Align"); + public enum AlignType { + ALLB, + LEFTB, + RIGHTB, + L1LB, + L1RB } - public static Command autoAlignLeft( - CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { - return new AutoAlignCommandLeft(drivebaseSubsystem, controls).withName("Auto Align"); - } - - public static Command autoAlignRight( - CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { - return new AutoAlignCommandRight(drivebaseSubsystem, controls).withName("Auto Align"); - } - - public static Boolean isBlue() { - boolean isBlue; - - if (!DriverStation.getAlliance().isEmpty()) { - isBlue = DriverStation.getAlliance().get().equals(Alliance.Blue); - } else { - isBlue = false; - } - return isBlue; + public static Command autoAlign( + CommandSwerveDrivetrain drivebaseSubsystem, Controls controls, AlignType type) { + return new AutoAlignCommand(drivebaseSubsystem, controls, type).withName("Auto Align"); } public static boolean readyToScore() { - return isStationary() && isLevel() && isCloseEnough(); - } - - public static boolean poseInPlace() { - return isStationary() && isCloseEnough(); + return isStationary() && isLevel() && isCloseEnough(AlignType.ALLB); } public static boolean isStationary() { @@ -65,15 +49,18 @@ public static boolean isLevel() { && MathUtil.isNear(0, rotation.getY(), Units.degreesToRadians(2)); } - public static boolean isCloseEnough() { + public static boolean isCloseEnough(AlignType type) { var currentPose = AutoLogic.s.drivebaseSubsystem.getState().Pose; - var branchPose = AutoAlignCommand.getNearestBranch(currentPose); + var branchPose = AutoAlignCommand.getTargetPose(currentPose, type); return currentPose.getTranslation().getDistance(branchPose.getTranslation()) < 0.05; } + public static boolean poseInPlace(AlignType type) { + return isStationary() && isCloseEnough(type); + } + public static boolean oneSecondLeft() { // THIS WILL ONLY WORK ON THE REAL FIELD AND IN PRACTICE MODE! - return DriverStation.getMatchTime() <= 1; } @@ -83,10 +70,15 @@ public static boolean isCloseEnough() { // left and right offsets from the april tags () private static final Transform2d leftOfTag = new Transform2d( - Units.inchesToMeters(36.5 / 2), Units.inchesToMeters(-12.97 / 2), Rotation2d.k180deg); + Units.inchesToMeters(34 / 2), Units.inchesToMeters(-12.97 / 2), Rotation2d.k180deg); private static final Transform2d rightOfTag = new Transform2d( - Units.inchesToMeters(36.5 / 2), Units.inchesToMeters(12.97 / 2), Rotation2d.k180deg); + Units.inchesToMeters(34 / 2), Units.inchesToMeters(12.97 / 2), Rotation2d.k180deg); + private static final Transform2d middleOfReef = + new Transform2d(Units.inchesToMeters(34 / 2), Units.inchesToMeters(0), Rotation2d.k180deg); + private static final Transform2d l1RightOfReef = + new Transform2d( + Units.inchesToMeters(34 / 2), Units.inchesToMeters(26 / 2), Rotation2d.k180deg); private static final Pose2d blueBranchA = aprilTagFieldLayout.getTagPose(18).get().toPose2d().plus(leftOfTag); @@ -138,41 +130,59 @@ public static boolean isCloseEnough() { private static final Pose2d redBranchL = aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(rightOfTag); - private static final List blueBranchPoses = - List.of( - blueBranchA, - blueBranchB, - blueBranchC, - blueBranchD, - blueBranchE, - blueBranchF, - blueBranchG, - blueBranchH, - blueBranchI, - blueBranchJ, - blueBranchK, - blueBranchL); - ; - private static final List redBranchPoses = - List.of( - redBranchA, - redBranchB, - redBranchC, - redBranchD, - redBranchE, - redBranchF, - redBranchG, - redBranchH, - redBranchI, - redBranchJ, - redBranchK, - redBranchL); + private static final Pose2d lBlueReefFaceAB = + aprilTagFieldLayout.getTagPose(18).get().toPose2d().plus(middleOfReef); + private static final Pose2d lBlueReefFaceCD = + aprilTagFieldLayout.getTagPose(17).get().toPose2d().plus(middleOfReef); + private static final Pose2d lBlueReefFaceEF = + aprilTagFieldLayout.getTagPose(22).get().toPose2d().plus(middleOfReef); + private static final Pose2d lBlueReefFaceGH = + aprilTagFieldLayout.getTagPose(21).get().toPose2d().plus(middleOfReef); + private static final Pose2d lBlueReefFaceIJ = + aprilTagFieldLayout.getTagPose(20).get().toPose2d().plus(middleOfReef); + private static final Pose2d lBlueReefFaceKL = + aprilTagFieldLayout.getTagPose(19).get().toPose2d().plus(middleOfReef); + + private static final Pose2d rBlueReefFaceAB = + aprilTagFieldLayout.getTagPose(18).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rBlueReefFaceCD = + aprilTagFieldLayout.getTagPose(17).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rBlueReefFaceEF = + aprilTagFieldLayout.getTagPose(22).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rBlueReefFaceGH = + aprilTagFieldLayout.getTagPose(21).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rBlueReefFaceIJ = + aprilTagFieldLayout.getTagPose(20).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rBlueReefFaceKL = + aprilTagFieldLayout.getTagPose(19).get().toPose2d().plus(l1RightOfReef); + + private static final Pose2d lRedReefFaceAB = + aprilTagFieldLayout.getTagPose(7).get().toPose2d().plus(middleOfReef); + private static final Pose2d lRedReefFaceCD = + aprilTagFieldLayout.getTagPose(8).get().toPose2d().plus(middleOfReef); + private static final Pose2d lRedReefFaceEF = + aprilTagFieldLayout.getTagPose(9).get().toPose2d().plus(middleOfReef); + private static final Pose2d lRedReefFaceGH = + aprilTagFieldLayout.getTagPose(10).get().toPose2d().plus(middleOfReef); + private static final Pose2d lRedReefFaceIJ = + aprilTagFieldLayout.getTagPose(11).get().toPose2d().plus(middleOfReef); + private static final Pose2d lRedReefFaceKL = + aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(middleOfReef); + + private static final Pose2d rRedReefFaceAB = + aprilTagFieldLayout.getTagPose(7).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rRedReefFaceCD = + aprilTagFieldLayout.getTagPose(8).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rRedReefFaceEF = + aprilTagFieldLayout.getTagPose(9).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rRedReefFaceGH = + aprilTagFieldLayout.getTagPose(10).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rRedReefFaceIJ = + aprilTagFieldLayout.getTagPose(11).get().toPose2d().plus(l1RightOfReef); + private static final Pose2d rRedReefFaceKL = + aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(l1RightOfReef); private static class AutoAlignCommand extends Command { - public static Pose2d getNearestBranch(Pose2d p) { - List branchPose2ds = isBlue() ? blueBranchPoses : redBranchPoses; - return p.nearest(branchPose2ds); - } protected final PIDController pidX = new PIDController(4, 0, 0); protected final PIDController pidY = new PIDController(4, 0, 0); @@ -181,23 +191,25 @@ public static Pose2d getNearestBranch(Pose2d p) { protected final CommandSwerveDrivetrain drive; protected final Controls controls; protected Pose2d branchPose; + protected AlignType type; private final SwerveRequest.FieldCentric driveRequest = new SwerveRequest.FieldCentric() // Add a 10% deadband .withDriveRequestType(DriveRequestType.OpenLoopVoltage) .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); - public AutoAlignCommand(CommandSwerveDrivetrain drive, Controls controls) { + public AutoAlignCommand(CommandSwerveDrivetrain drive, Controls controls, AlignType type) { this.drive = drive; pidRotate.enableContinuousInput(-Math.PI, Math.PI); this.controls = controls; + this.type = type; setName("Auto Align"); } @Override public void initialize() { Pose2d robotPose = drive.getState().Pose; - branchPose = getNearestBranch(robotPose); + branchPose = getTargetPose(robotPose, type); pidX.setSetpoint(branchPose.getX()); pidY.setSetpoint(branchPose.getY()); pidRotate.setSetpoint(branchPose.getRotation().getRadians()); @@ -241,57 +253,105 @@ public void end(boolean interrupted) { drive.setControl(stop); controls.vibrateDriveController(0); } - } - - private static class AutoAlignCommandLeft extends AutoAlignCommand { - private static final List blueLeftBranchPoses = - List.of(blueBranchA, blueBranchC, blueBranchE, blueBranchG, blueBranchI, blueBranchK); - private static final List redLeftBranchPoses = - List.of(redBranchA, redBranchC, redBranchE, redBranchG, redBranchI, redBranchK); - - public static Pose2d getNearestLeftBranch(Pose2d p, boolean isBlue) { - List branchPose2ds = isBlue ? blueLeftBranchPoses : redLeftBranchPoses; - return p.nearest(branchPose2ds); + public static Pose2d getTargetPose(Pose2d pose, AlignType type) { + return switch (type) { + case LEFTB -> getNearestLeftBranch(pose); + case RIGHTB -> getNearestRightBranch(pose); + case L1LB -> getNearestL1L(pose); + case L1RB -> getNearestL1R(pose); + case ALLB -> getNearestBranch(pose); + }; } - public AutoAlignCommandLeft(CommandSwerveDrivetrain drive, Controls controls) { - super(drive, controls); + private static Pose2d getNearestBranch(Pose2d p) { + List branchPose2ds = + AllianceUtils.isBlue() + ? List.of( + blueBranchA, + blueBranchB, + blueBranchC, + blueBranchD, + blueBranchE, + blueBranchF, + blueBranchG, + blueBranchH, + blueBranchI, + blueBranchJ, + blueBranchK, + blueBranchL) + : List.of( + redBranchA, + redBranchB, + redBranchC, + redBranchD, + redBranchE, + redBranchF, + redBranchG, + redBranchH, + redBranchI, + redBranchJ, + redBranchK, + redBranchL); + return p.nearest(branchPose2ds); } - @Override - public void initialize() { - Pose2d robotPose = drive.getState().Pose; - branchPose = getNearestLeftBranch(robotPose, isBlue()); - pidX.setSetpoint(branchPose.getX()); - pidY.setSetpoint(branchPose.getY()); - pidRotate.setSetpoint(branchPose.getRotation().getRadians()); + private static Pose2d getNearestLeftBranch(Pose2d p) { + List branchPose2ds = + AllianceUtils.isBlue() + ? List.of( + blueBranchA, blueBranchC, blueBranchE, blueBranchG, blueBranchI, blueBranchK) + : List.of(redBranchA, redBranchC, redBranchE, redBranchG, redBranchI, redBranchK); + return p.nearest(branchPose2ds); } - } - private static class AutoAlignCommandRight extends AutoAlignCommand { - private static final List blueRightBranchPoses = - List.of(blueBranchB, blueBranchD, blueBranchF, blueBranchH, blueBranchJ, blueBranchL); - - private static final List redRightBranchPoses = - List.of(redBranchB, redBranchD, redBranchF, redBranchH, redBranchJ, redBranchL); - - public static Pose2d getNearestRightBranch(Pose2d p) { - List branchPose2ds = isBlue() ? blueRightBranchPoses : redRightBranchPoses; + private static Pose2d getNearestRightBranch(Pose2d p) { + List branchPose2ds = + AllianceUtils.isBlue() + ? List.of( + blueBranchB, blueBranchD, blueBranchF, blueBranchH, blueBranchJ, blueBranchL) + : List.of(redBranchB, redBranchD, redBranchF, redBranchH, redBranchJ, redBranchL); return p.nearest(branchPose2ds); } - public AutoAlignCommandRight(CommandSwerveDrivetrain drive, Controls controls) { - super(drive, controls); + private static Pose2d getNearestL1L(Pose2d p) { + List reefFacesPose2ds = + AllianceUtils.isBlue() + ? List.of( + lBlueReefFaceAB, + lBlueReefFaceCD, + lBlueReefFaceEF, + lBlueReefFaceGH, + lBlueReefFaceIJ, + lBlueReefFaceKL) + : List.of( + lRedReefFaceAB, + lRedReefFaceCD, + lRedReefFaceEF, + lRedReefFaceGH, + lRedReefFaceIJ, + lRedReefFaceKL); + return p.nearest(reefFacesPose2ds); } - @Override - public void initialize() { - Pose2d robotPose = drive.getState().Pose; - branchPose = getNearestRightBranch(robotPose); - pidX.setSetpoint(branchPose.getX()); - pidY.setSetpoint(branchPose.getY()); - pidRotate.setSetpoint(branchPose.getRotation().getRadians()); + private static Pose2d getNearestL1R(Pose2d p) { + List reefFacesPose2ds = + AllianceUtils.isBlue() + ? List.of( + rBlueReefFaceAB, + rBlueReefFaceCD, + rBlueReefFaceEF, + rBlueReefFaceGH, + rBlueReefFaceIJ, + rBlueReefFaceKL) + : List.of( + rRedReefFaceAB, + rRedReefFaceCD, + rRedReefFaceEF, + rRedReefFaceGH, + rRedReefFaceIJ, + rRedReefFaceKL); + return p.nearest(reefFacesPose2ds); } } } diff --git a/src/main/java/frc/robot/subsystems/auto/AutoBuilderConfig.java b/src/main/java/frc/robot/subsystems/auto/AutoBuilderConfig.java index 02f94323..e4eae0da 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoBuilderConfig.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoBuilderConfig.java @@ -5,10 +5,10 @@ import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import edu.wpi.first.wpilibj.DriverStation; import frc.robot.Robot; import frc.robot.Subsystems; import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain; +import frc.robot.util.AllianceUtils; import java.io.IOException; import org.json.simple.parser.ParseException; @@ -42,12 +42,7 @@ public static void buildAuto(CommandSwerveDrivetrain drivebase) { // Boolean supplier that controls when the path will be mirrored for the red alliance // This will flip the path being followed to the red side of the field. // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return !AutoAlign.isBlue(); // Checking alliance is red - } - return false; + return AllianceUtils.isRed(); // Checking alliance is red }, s.drivebaseSubsystem // Reference to this subsystem to set requirements ); diff --git a/src/main/java/frc/robot/subsystems/auto/AutoLogic.java b/src/main/java/frc/robot/subsystems/auto/AutoLogic.java index 2f0a7a54..f65e48a9 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoLogic.java @@ -229,7 +229,7 @@ public static void initShuffleBoard() { tab.add("Available Auto Variants", availableAutos).withPosition(4, 2).withSize(2, 1); tab.addBoolean("readyToScore?", () -> AutoAlign.readyToScore()); tab.addBoolean("Level?", () -> AutoAlign.isLevel()); - tab.addBoolean("Close Enough?", () -> AutoAlign.isCloseEnough()); + tab.addBoolean("Close Enough?", () -> AutoAlign.isCloseEnough(AutoAlign.AlignType.ALLB)); tab.addBoolean("Stationary?", () -> AutoAlign.isStationary()); tab.addBoolean("Low on time?", () -> AutoAlign.oneSecondLeft()); tab.addDouble("MATCH TIME(TIMER FOR AUTO)", () -> DriverStation.getMatchTime()); @@ -290,7 +290,7 @@ public static Command scoreCommand() { if (r.superStructure != null) { return new ConditionalCommand( // If true: - AutoAlign.autoAlign(s.drivebaseSubsystem, controls) + AutoAlign.autoAlign(s.drivebaseSubsystem, controls, AutoAlign.AlignType.ALLB) .repeatedly() .withDeadline(r.superStructure.coralLevelFour(() -> AutoAlign.readyToScore())) .withName("scoreCommand"), @@ -299,7 +299,7 @@ public static Command scoreCommand() { // Condition: () -> ARMSENSOR_ENABLED && r.sensors.armSensor.booleanInClaw()); } - return AutoAlign.autoAlign(s.drivebaseSubsystem, controls) + return AutoAlign.autoAlign(s.drivebaseSubsystem, controls, AutoAlign.AlignType.ALLB) .withName("scoreCommand-noSuperstructure"); } diff --git a/src/main/java/frc/robot/util/AllianceUtils.java b/src/main/java/frc/robot/util/AllianceUtils.java new file mode 100644 index 00000000..1f0ef67e --- /dev/null +++ b/src/main/java/frc/robot/util/AllianceUtils.java @@ -0,0 +1,19 @@ +package frc.robot.util; + +import edu.wpi.first.wpilibj.DriverStation; + +public final class AllianceUtils { + public static boolean isBlue() { + if (!DriverStation.getAlliance().isEmpty()) { + return DriverStation.getAlliance().get().equals(DriverStation.Alliance.Blue); + } + return false; + } + + public static boolean isRed() { + if (!DriverStation.getAlliance().isEmpty()) { + return DriverStation.getAlliance().get().equals(DriverStation.Alliance.Red); + } + return false; + } +} diff --git a/src/main/java/frc/robot/util/ScoringType.java b/src/main/java/frc/robot/util/ScoringType.java new file mode 100644 index 00000000..b99ffad9 --- /dev/null +++ b/src/main/java/frc/robot/util/ScoringType.java @@ -0,0 +1,7 @@ +package frc.robot.util; + +public enum ScoringType { + SOLOC_RIGHT, + SOLOC_LEFT, + DRIVER; +}