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..36eb85d2 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -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) { @@ -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( () -> { @@ -457,19 +460,36 @@ private Command getCoralBranchHeightCommand() { }; } - private Command getSoloCoralBranchHeightCommand() { + private Command getSoloCoralBranchHeightCommandL() { + 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()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_TWO -> superStructure + .coralLevelTwo(soloController.rightBumper(), () -> AutoAlign.poseInPlace()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_ONE -> superStructure + .coralLevelOne(soloController.rightBumper(), () -> AutoAlign.poseInPlaceL1L()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + }; + } + + private Command getSoloCoralBranchHeightCommandR() { 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()) + .coralLevelThree(soloController.rightBumper(), () -> AutoAlign.poseInPlace()) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_TWO -> superStructure - .coralLevelTwo(soloController.rightBumper()) + .coralLevelTwo(soloController.rightBumper(), () -> AutoAlign.poseInPlace()) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); case CORAL_LEVEL_ONE -> superStructure - .coralLevelOne(soloController.rightBumper()) + .coralLevelOne(soloController.rightBumper(), () -> AutoAlign.poseInPlaceL1R()) .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); }; } @@ -926,7 +946,7 @@ private void configureSoloControllerBindings() { switch (soloScoringMode) { case CORAL_IN_CLAW -> { scoreCommand = - getSoloCoralBranchHeightCommand() + getSoloCoralBranchHeightCommandL() .until( () -> soloController.a().getAsBoolean() @@ -969,6 +989,11 @@ private void configureSoloControllerBindings() { .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) .whileTrue(AutoAlign.autoAlignLeft(s.drivebaseSubsystem, this)); + soloController + .leftTrigger() + .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) + .and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE) + .whileTrue(AutoAlign.autoAlignL1L(s.drivebaseSubsystem, this)); // Processor + Auto align right + Select scoring mode Coral soloController .rightTrigger() @@ -977,7 +1002,7 @@ private void configureSoloControllerBindings() { () -> { Command scoreCommand = switch (soloScoringMode) { - case CORAL_IN_CLAW -> getSoloCoralBranchHeightCommand() + case CORAL_IN_CLAW -> getSoloCoralBranchHeightCommandR() .until( () -> soloController.a().getAsBoolean() @@ -1006,6 +1031,11 @@ private void configureSoloControllerBindings() { .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) .whileTrue(AutoAlign.autoAlignRight(s.drivebaseSubsystem, this)); + soloController + .rightTrigger() + .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) + .and(() -> branchHeight == BranchHeight.CORAL_LEVEL_ONE) + .whileTrue(AutoAlign.autoAlignL1R(s.drivebaseSubsystem, this)); // Ground Intake soloController .leftBumper() @@ -1095,11 +1125,11 @@ 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()); + // Ready to score rumble (Removal for autoscoring) + /* 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))); + Commands.runOnce(() -> soloController.setRumble(RumbleType.kBothRumble, 0.0))); */ } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 794d2e2f..bef99cec 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,6 +6,7 @@ import au.grapplerobotics.CanBridge; import com.pathplanner.lib.commands.FollowPathCommand; +import edu.wpi.first.math.util.Units; import edu.wpi.first.net.WebServer; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; @@ -16,7 +17,12 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.Subsystems.SubsystemConstants; import frc.robot.subsystems.SuperStructure; @@ -43,6 +49,11 @@ public static Robot getInstance() { public final SuperStructure superStructure; private final PowerDistribution PDH; + Mechanism2d mech; + MechanismRoot2d root; + MechanismLigament2d m_elevator; + MechanismLigament2d m_wrist; + protected Robot() { // non public for singleton. Protected so test class can subclass @@ -52,6 +63,19 @@ protected Robot() { PDH = new PowerDistribution(Hardware.PDH_ID, ModuleType.kRev); LiveWindow.disableAllTelemetry(); LiveWindow.enableTelemetry(PDH); + mech = new Mechanism2d(1, 2); + root = mech.getRoot("climber", 0.5 + Units.inchesToMeters(5.5), Units.inchesToMeters(19.5)); + SmartDashboard.putData("Mechanism", mech); + m_elevator = + root.append(new MechanismLigament2d("elevator", 1, 90, 2, new Color8Bit(Color.kRed))); + var pivot = + m_elevator.append( + new MechanismLigament2d( + "pivot offset", Units.inchesToMeters(4), -90, 2, new Color8Bit(Color.kDarkRed))); + m_wrist = + pivot.append( + new MechanismLigament2d( + "wrist", Units.inchesToMeters(14.5), 270, 6, new Color8Bit(Color.kFirstRed))); sensors = new Sensors(); subsystems = new Subsystems(sensors); @@ -117,6 +141,8 @@ public void robotPeriodic() { if (subsystems.visionSubsystem != null) { subsystems.visionSubsystem.update(); } + m_elevator.setLength(subsystems.elevatorSubsystem.getHeightMeters()); + m_wrist.setAngle(subsystems.armPivotSubsystem.getAngle()); } @Override diff --git a/src/main/java/frc/robot/subsystems/ArmPivot.java b/src/main/java/frc/robot/subsystems/ArmPivot.java index bdb98cae..815385b3 100644 --- a/src/main/java/frc/robot/subsystems/ArmPivot.java +++ b/src/main/java/frc/robot/subsystems/ArmPivot.java @@ -15,6 +15,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.Alert; @@ -146,6 +147,10 @@ private double getCurrentPosition() { return curPos.getValueAsDouble(); } + public Rotation2d getAngle() { // 0 is forward, + is up + return Rotation2d.fromRotations(getCurrentPosition()); + } + // preset command placeholder /* moves arm to the input position - sets the target position to the inputted position - shrinks the difference between the current position and the target position until they are close enough to work diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 371953a9..431ea58a 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -9,13 +9,21 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.MutVoltage; +import edu.wpi.first.units.measure.Time; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.Command; @@ -24,6 +32,9 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Hardware; +import jdk.jfr.Timestamp; + +import java.util.concurrent.TimeUnit; import java.util.function.DoubleConsumer; import java.util.function.DoubleSupplier; import java.util.function.Supplier; @@ -52,6 +63,7 @@ public class ElevatorSubsystem extends SubsystemBase { public static final double CORAL_QUICK_INTAKE = 1.6; public static final double MIN_EMPTY_GROUND_INTAKE = 4.5; public static final double MIN_FULL_GROUND_INTAKE = 8.0; + private static final double MOTOR_ROTATIONS_PER_METER = 40; // Inaccurate public static final double MANUAL = 0.1; private static final double POS_TOLERANCE = 0.1; private final double ELEVATOR_KP = 7.804; @@ -90,7 +102,10 @@ public class ElevatorSubsystem extends SubsystemBase { new Alert("Elevator", "Motor 2 not connected", AlertType.kError); private final Debouncer notConnectedDebouncerOne = new Debouncer(.1, DebounceType.kBoth); private final Debouncer notConnectedDebouncerTwo = new Debouncer(.1, DebounceType.kBoth); - + private final StructPublisher elevatorPose3d = NetworkTableInstance.getDefault().getStructTopic("elevator/heightPose", Pose3d.struct).publish(); + private final StructPublisher TESTpose = NetworkTableInstance.getDefault().getStructTopic("debug/TEST", Pose3d.struct).publish(); + private final StructPublisher TESTpose2 = NetworkTableInstance.getDefault().getStructTopic("debug/TEST2", Pose3d.struct).publish(); + // Creates a SysIdRoutine SysIdRoutine routine = new SysIdRoutine( @@ -107,6 +122,8 @@ public ElevatorSubsystem() { motorConfigs(); Shuffleboard.getTab("Elevator").addDouble("Motor Current Position", () -> getCurrentPosition()); + //Elevator pose test + Shuffleboard.getTab("Elevator").addDouble("Target Position", () -> getTargetPosition()); Shuffleboard.getTab("Elevator") .addDouble("M1 supply current", () -> m_motor.getSupplyCurrent().getValueAsDouble()); @@ -133,6 +150,7 @@ public ElevatorSubsystem() { "M2 at reverse softstop", () -> m_motor2.getFault_ReverseSoftLimit().getValue()); Shuffleboard.getTab("Elevator") .addDouble("Elevator Speed", () -> m_motor.getVelocity().getValueAsDouble()); + } public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { @@ -249,6 +267,10 @@ private double getCurrentPosition() { return curPos; } + public double getHeightMeters() { // Elevator height converted to Meters + return getCurrentPosition() / MOTOR_ROTATIONS_PER_METER; + } + private void setCurrentPosition(double pos) { m_motor.setPosition(pos); } @@ -370,6 +392,18 @@ public void periodic() { if (RobotBase.isSimulation()) { m_motorOneSimState.setRawRotorPosition(targetPos); m_motorTwoSimState.setRawRotorPosition(targetPos); + + elevatorPose3d.set(new Pose3d(0.0, 0.0, getHeightMeters(), new Rotation3d())); + + + + double smoothing = 0.01; + TESTpose.set(new Pose3d( + 0.0, 0.0, getHeightMeters() + (getTargetPosition() - getCurrentPosition()) * smoothing, + new Rotation3d(Math.PI / 2, Math.PI / 2, -Math.PI / 2))); + TESTpose2.set(new Pose3d(0.0,0.0,0.0,new Rotation3d(0.0,Math.sin((Timer.getFPGATimestamp())),0.0))); + + } } } diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index d37b680b..217675a6 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -67,6 +67,17 @@ 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(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 +179,80 @@ public Command coralLevelOne(BooleanSupplier score) { .withName("Coral Level 1"); } + 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 +301,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/AutoAlign.java b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java index 0f0ad2d4..726558d8 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java @@ -23,6 +23,16 @@ public static Command autoAlign(CommandSwerveDrivetrain drivebaseSubsystem, Cont return new AutoAlignCommand(drivebaseSubsystem, controls).withName("Auto Align"); } + public static Command autoAlignL1L( + CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { + return new AutoAlignCommandL1L(drivebaseSubsystem, controls).withName("Auto Align"); + } + + public static Command autoAlignL1R( + CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { + return new AutoAlignCommandL1R(drivebaseSubsystem, controls).withName("Auto Align"); + } + public static Command autoAlignLeft( CommandSwerveDrivetrain drivebaseSubsystem, Controls controls) { return new AutoAlignCommandLeft(drivebaseSubsystem, controls).withName("Auto Align"); @@ -52,6 +62,14 @@ public static boolean poseInPlace() { return isStationary() && isCloseEnough(); } + public static boolean poseInPlaceL1L() { + return isStationary() && isCloseEnoughL1L(); + } + + public static boolean poseInPlaceL1R() { + return isStationary() && isCloseEnoughL1R(); + } + public static boolean isStationary() { var speeds = AutoLogic.s.drivebaseSubsystem.getState().Speeds; return MathUtil.isNear(0, speeds.vxMetersPerSecond, 0.01) @@ -71,6 +89,18 @@ public static boolean isCloseEnough() { return currentPose.getTranslation().getDistance(branchPose.getTranslation()) < 0.05; } + public static boolean isCloseEnoughL1L() { + var currentPose = AutoLogic.s.drivebaseSubsystem.getState().Pose; + var branchPose = AutoAlignCommandL1L.getNearestReefFace(currentPose); + return currentPose.getTranslation().getDistance(branchPose.getTranslation()) < 0.05; + } + + public static boolean isCloseEnoughL1R() { + var currentPose = AutoLogic.s.drivebaseSubsystem.getState().Pose; + var branchPose = AutoAlignCommandL1R.getNearestReefFace(currentPose); + return currentPose.getTranslation().getDistance(branchPose.getTranslation()) < 0.05; + } + public static boolean oneSecondLeft() { // THIS WILL ONLY WORK ON THE REAL FIELD AND IN PRACTICE MODE! @@ -83,10 +113,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,6 +173,58 @@ public static boolean isCloseEnough() { private static final Pose2d redBranchL = aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(rightOfTag); + 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 final List blueBranchPoses = List.of( blueBranchA, @@ -294,4 +381,80 @@ public void initialize() { pidRotate.setSetpoint(branchPose.getRotation().getRadians()); } } + + private static class AutoAlignCommandL1L extends AutoAlignCommand { + private static final List blueReefFaces = + List.of( + lBlueReefFaceAB, + lBlueReefFaceCD, + lBlueReefFaceEF, + lBlueReefFaceGH, + lBlueReefFaceIJ, + lBlueReefFaceKL); + + private static final List redReefFaces = + List.of( + lRedReefFaceAB, + lRedReefFaceCD, + lRedReefFaceEF, + lRedReefFaceGH, + lRedReefFaceIJ, + lRedReefFaceKL); + + public static Pose2d getNearestReefFace(Pose2d p) { + List reefFacesPose2ds = isBlue() ? blueReefFaces : redReefFaces; + return p.nearest(reefFacesPose2ds); + } + + public AutoAlignCommandL1L(CommandSwerveDrivetrain drive, Controls controls) { + super(drive, controls); + } + + @Override + public void initialize() { + Pose2d robotPose = drive.getState().Pose; + branchPose = getNearestReefFace(robotPose); + pidX.setSetpoint(branchPose.getX()); + pidY.setSetpoint(branchPose.getY()); + pidRotate.setSetpoint(branchPose.getRotation().getRadians()); + } + } + + private static class AutoAlignCommandL1R extends AutoAlignCommand { + private static final List blueReefFaces = + List.of( + rBlueReefFaceAB, + rBlueReefFaceCD, + rBlueReefFaceEF, + rBlueReefFaceGH, + rBlueReefFaceIJ, + rBlueReefFaceKL); + + private static final List redReefFaces = + List.of( + rRedReefFaceAB, + rRedReefFaceCD, + rRedReefFaceEF, + rRedReefFaceGH, + rRedReefFaceIJ, + rRedReefFaceKL); + + public static Pose2d getNearestReefFace(Pose2d p) { + List reefFacesPose2ds = isBlue() ? blueReefFaces : redReefFaces; + return p.nearest(reefFacesPose2ds); + } + + public AutoAlignCommandL1R(CommandSwerveDrivetrain drive, Controls controls) { + super(drive, controls); + } + + @Override + public void initialize() { + Pose2d robotPose = drive.getState().Pose; + branchPose = getNearestReefFace(robotPose); + pidX.setSetpoint(branchPose.getX()); + pidY.setSetpoint(branchPose.getY()); + pidRotate.setSetpoint(branchPose.getRotation().getRadians()); + } + } }