diff --git a/src/main/deploy/elastic-layout.json b/src/main/deploy/elastic-layout.json new file mode 100644 index 00000000..04c7035a --- /dev/null +++ b/src/main/deploy/elastic-layout.json @@ -0,0 +1,1164 @@ +{ + "version": 1.0, + "grid_size": 128, + "tabs": [ + { + "name": "Match", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "has arm sensor", + "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/photonvision_Port_1182_Output_MJPEG_Server", + "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/photonvision_Port_1183_Input_MJPEG_Server", + "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", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + } + ] + } + }, + { + "name": "Autos", + "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, + "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?", + "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 + } + }, + { + "title": "M1 temp", + "x": 256.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Elevator/M1 temp", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } + }, + { + "name": "End Effector", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Pivot Target Pos", + "x": 512.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Arm Pivot/Pivot Target Pos", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Pivot Speed", + "x": 256.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Arm Pivot/Pivot Speed", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Pivot Position", + "x": 128.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Arm Pivot/Pivot Position", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Pivot Motor rotor Pos", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Arm Pivot/Pivot Motor rotor Pos", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Pivot Motor Temperature", + "x": 384.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)", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "inClaw", + "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, + "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", + "properties": { + "topic": "/Shuffleboard/Claw/Claw Speed", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Last Set Power", + "x": 1152.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Claw/Last Set Power", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Motor Voltage", + "x": 1408.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Claw/Motor Voltage", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } + }, + { + "name": "Climb", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Is Climb OUT?", + "x": 640.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Shuffleboard/Climb/Is Climb OUT?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Is Climb STOWED?", + "x": 640.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", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Motor Speed", + "x": 1152.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Climb/Motor 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" + } + }, + { + "title": "Set speed", + "x": 1280.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", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } + }, + { + "name": "GroundIntake", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Speed", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/GroundIntake/Speed", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Pivot Motor Temperature", + "x": 0.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Ground Arm/Pivot Motor Temperature", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Pivot Motor rotor Pos", + "x": 128.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Ground Arm/Pivot Motor rotor Pos", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Pivot Position", + "x": 256.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Ground Arm/Pivot Position", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Pivot Speed", + "x": 384.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Ground Arm/Pivot Speed", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Pivot Target Pos", + "x": 512.0, + "y": 384.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/Ground Arm/Pivot Target Pos", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Last Set Power", + "x": 0.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/GroundIntake/Last Set Power", + "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", + "properties": { + "topic": "/Shuffleboard/GroundIntake/Motor Temperature", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Motor Voltage", + "x": 128.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/GroundIntake/Motor Voltage", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Distance(m)", + "x": 640.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shuffleboard/IntakeSensor/Distance(m)", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "In Intake", + "x": 768.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Shuffleboard/IntakeSensor/In Intake", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + } + ] + } + }, + { + "name": "Controls", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Elevator Coast Mode", + "x": 384.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Toggle Button", + "properties": { + "topic": "/Shuffleboard/Controls/Elevator Coast Mode", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Swerve Coast Mode", + "x": 512.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Toggle Button", + "properties": { + "topic": "/Shuffleboard/Controls/Swerve Coast Mode", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Climb Coast Mode", + "x": 640.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Toggle Button", + "properties": { + "topic": "/Shuffleboard/Controls/Climb Coast Mode", + "period": 0.06, + "data_type": "boolean" + } + } + ] + } + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/OSMCounterAlgaeEF.auto b/src/main/deploy/pathplanner/autos/OSMCounterAlgaeEF.auto new file mode 100644 index 00000000..f24b2739 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/OSMCounterAlgaeEF.auto @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "OSM to D" + } + }, + { + "type": "named", + "data": { + "name": "scoreCommand" + } + }, + { + "type": "path", + "data": { + "pathName": "D to E" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "E to RSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "isCollected" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "RSF to C" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "scoreCommand" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/OSWCounterAlgaeEF.auto b/src/main/deploy/pathplanner/autos/OSWCounterAlgaeEF.auto new file mode 100644 index 00000000..151ca79e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/OSWCounterAlgaeEF.auto @@ -0,0 +1,81 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "OSW to D" + } + }, + { + "type": "named", + "data": { + "name": "scoreCommand" + } + }, + { + "type": "path", + "data": { + "pathName": "D to E" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "E to RSF" + } + }, + { + "type": "named", + "data": { + "name": "readyIntake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "isCollected" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "RSF to C" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "scoreCommand" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/D to E.path b/src/main/deploy/pathplanner/paths/D to E.path new file mode 100644 index 00000000..c40ae10e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/D to E.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.681, + "y": 2.259 + }, + "prevControl": null, + "nextControl": { + "x": 3.935279605263158, + "y": 0.9941611842105258 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.171345707655665, + "y": 2.5998259860741233 + }, + "prevControl": { + "x": 5.31238849846513, + "y": 2.3934115215599387 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 119.99999999999999 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 59.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 87c9ed8e..dd7ed440 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -66,7 +66,8 @@ public class Controls { private AlgaeIntakeHeight algaeIntakeHeight = AlgaeIntakeHeight.ALGAE_LEVEL_THREE_FOUR; // Swerve stuff - // setting the max speed nad other similar variables depending on which drivebase it is + // setting the max speed and other similar variables depending on which + // drivebase it is public static final double MaxSpeed = RobotType.getCurrent() == RobotType.BONK ? BonkTunerConstants.kSpeedAt12Volts.in(MetersPerSecond) @@ -136,7 +137,7 @@ private double getDriveY() { return input * MaxSpeed * inputScale; } - // takes the rotation value from the joystick, and applies a deadband and input scaling + // takes the rotation value from the joystickk, and applies a deadband and input scaling private double getDriveRotate() { // Joystick +X is right // Robot +angle is CCW (left) @@ -147,7 +148,7 @@ private double getDriveRotate() { // all the current control bidings private void configureDrivebaseBindings() { - if (s.drivebaseSubsystem == null) { + if (s.getDrivetrain() == null) { // Stop running this method return; } @@ -156,8 +157,9 @@ private void configureDrivebaseBindings() { // and Y is defined as to the left according to WPILib convention. // the driving command for just driving around - s.drivebaseSubsystem.setDefaultCommand( - // s.drivebaseSubsystem will execute this command periodically + s.getDrivetrain() + .setDefaultCommand( + // s.drivebaseSubsystem will execute this command periodically // applying the request to drive with the inputs s.drivebaseSubsystem @@ -170,30 +172,31 @@ private void configureDrivebaseBindings() { soloController.isConnected() ? getSoloDriveRotate() : getDriveRotate())) .withName("Drive")); - // various former controls that were previously used and could be referenced in the future + // various former controls that were previously used and could be referenced in + // the future // operatorController - // .povUp() - // .whileTrue( - // s.drivebaseSubsystem - // .applyRequest( - // () -> - // drive - // .withVelocityX(MetersPerSecond.of(1.0)) - // .withVelocityY(0) - // .withRotationalRate(0)) - // .withName("1 m/s forward")); + // .povUp() + // .whileTrue( + // s.drivebaseSubsystem + // .applyRequest( + // () -> + // drive + // .withVelocityX(MetersPerSecond.of(1.0)) + // .withVelocityY(0) + // .withRotationalRate(0)) + // .withName("1 m/s forward")); // operatorController - // .povRight() - // .whileTrue( - // s.drivebaseSubsystem - // .applyRequest( - // () -> - // drive - // .withVelocityX(MetersPerSecond.of(2.0)) - // .withVelocityY(0) - // .withRotationalRate(0)) - // .withName("2 m/s forward")); + // .povRight() + // .whileTrue( + // s.drivebaseSubsystem + // .applyRequest( + // () -> + // drive + // .withVelocityX(MetersPerSecond.of(2.0)) + // .withVelocityY(0) + // .withRotationalRate(0)) + // .withName("2 m/s forward")); // driverController.a().whileTrue(s.drivebaseSubsystem.sysIdDynamic(Direction.kForward)); // driverController.b().whileTrue(s.drivebaseSubsystem.sysIdDynamic(Direction.kReverse)); // driverController.y().whileTrue(s.drivebaseSubsystem.sysIdQuasistatic(Direction.kForward)); @@ -210,13 +213,13 @@ private void configureDrivebaseBindings() { driverController .back() .onTrue( - s.drivebaseSubsystem - .runOnce(() -> s.drivebaseSubsystem.seedFieldCentric()) + s.getDrivetrain() + .runOnce(() -> s.getDrivetrain().seedFieldCentric()) .alongWith(rumble(driverController, 0.5, Seconds.of(0.3))) .withName("Reset gyro")); // logging the telemetry - s.drivebaseSubsystem.registerTelemetry(logger::telemeterize); + s.getDrivetrain().registerTelemetry(logger::telemeterize); // creats a swerve button that coasts the wheels var swerveCoastButton = @@ -226,7 +229,7 @@ private void configureDrivebaseBindings() { .getEntry(); // coast the wheels new Trigger(() -> swerveCoastButton.getBoolean(false)) - .whileTrue(s.drivebaseSubsystem.coastMotors()); + .whileTrue(s.getDrivetrain().coastMotors()); } private void configureSuperStructureBindings() { @@ -426,12 +429,13 @@ private void configureSuperStructureBindings() { case CORAL -> getCoralBranchHeightCommand(); case ALGAE -> Commands.sequence( BargeAlign.bargeScore( - s.drivebaseSubsystem, + s.getDrivetrain(), superStructure, () -> getDriveX(), () -> getDriveY(), () -> getDriveRotate(), - driverController.rightBumper()), + driverController.rightBumper(), + s.drivebaseWrapper), getAlgaeIntakeCommand()) .withName("Algae score then intake"); }; @@ -575,9 +579,9 @@ private void configureElevatorBindings() { .whileTrue(s.elevatorSubsystem.holdCoastMode()); // var elevatorZeroButton = new DigitalInput(Hardware.ELEVATOR_ZERO_BUTTON); // new Trigger(() -> elevatorZeroButton.get()) - // .debounce(1, DebounceType.kRising) - // .and(RobotModeTriggers.disabled()) - // .onTrue(s.elevatorSubsystem.resetPosZero()); + // .debounce(1, DebounceType.kRising) + // .and(RobotModeTriggers.disabled()) + // .onTrue(s.elevatorSubsystem.resetPosZero()); } private void configureArmPivotBindings() { @@ -650,12 +654,14 @@ private void configureClimbPivotBindings() { s.climbPivotSubsystem.setDefaultCommand( s.climbPivotSubsystem.advanceClimbCheck().withName("Advance Climb Check")); - // check if the climb controller is connected, and whne start is pressed move to the next climb + // check if the climb controller is connected, and when start is pressed move to + // the next climb // position connected(climbTestController) .and(climbTestController.start()) .onTrue(s.climbPivotSubsystem.advanceClimbTarget()); - // on start or right trigger, move to climb out or climbed respectively on operator + // on start or right trigger, move to climb out or climbed respectively on + // operator operatorController.start().onTrue(s.climbPivotSubsystem.toClimbOut()); operatorController.rightTrigger().onTrue(s.climbPivotSubsystem.toClimbed()); // manual control for climb test controller for negative direction @@ -730,7 +736,7 @@ private void configureElevatorLEDBindings() { } s.elevatorLEDSubsystem.setDefaultCommand( - s.elevatorLEDSubsystem.showScoringMode(() -> soloScoringMode)); + s.elevatorLEDSubsystem.showScoringMode(() -> soloScoringMode, intakeMode)); if (s.elevatorSubsystem != null) { Trigger hasBeenZeroed = new Trigger(s.elevatorSubsystem::getHasBeenZeroed); @@ -765,7 +771,8 @@ private void configureElevatorLEDBindings() { RobotModeTriggers.autonomous() .whileTrue(s.elevatorLEDSubsystem.animate(LEDPattern.rainbow(255, 255), "Auto Rainbow")); Timer teleopTimer = new Timer(); - // when in teleop for less than 5 seconds after autononomous ends, restart the timer + // when in teleop for less than 5 seconds after autononomous ends, restart the + // timer RobotModeTriggers.autonomous() .debounce(5, DebounceType.kFalling) .and(RobotModeTriggers.teleop()) @@ -794,7 +801,7 @@ private void configureElevatorLEDBindings() { } private void configureAutoAlignBindings() { - if (s.drivebaseSubsystem == null) { + if (s.getDrivetrain() == null) { return; } if (s.visionSubsystem != null) { @@ -806,12 +813,12 @@ private void configureAutoAlignBindings() { .rightTrigger() .and(() -> scoringMode == ScoringMode.CORAL) .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) - .whileTrue(AutoAlign.autoAlignRight(s.drivebaseSubsystem, this)); + .whileTrue(AutoAlign.autoAlignRight(s.getDrivetrain(), this)); driverController .leftTrigger() .and(() -> scoringMode == ScoringMode.CORAL) .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) - .whileTrue(AutoAlign.autoAlignLeft(s.drivebaseSubsystem, this)); + .whileTrue(AutoAlign.autoAlignLeft(s.getDrivetrain(), this)); } private void configureGroundSpinnyBindings() { @@ -925,7 +932,15 @@ private void configureSoloControllerBindings() { Command scoreCommand; switch (soloScoringMode) { case CORAL_IN_CLAW -> { - scoreCommand = getSoloCoralBranchHeightCommand(); + scoreCommand = + getSoloCoralBranchHeightCommand() + .until( + () -> + soloController.a().getAsBoolean() + || soloController.b().getAsBoolean() + || soloController.x().getAsBoolean() + || soloController.y().getAsBoolean()); + ; } case ALGAE_IN_CLAW -> { Command bargeScoreCommand = @@ -970,7 +985,13 @@ private void configureSoloControllerBindings() { () -> { Command scoreCommand = switch (soloScoringMode) { - case CORAL_IN_CLAW -> getSoloCoralBranchHeightCommand(); + case CORAL_IN_CLAW -> getSoloCoralBranchHeightCommand() + .until( + () -> + soloController.a().getAsBoolean() + || soloController.b().getAsBoolean() + || soloController.x().getAsBoolean() + || soloController.y().getAsBoolean()); case ALGAE_IN_CLAW -> Commands.sequence( superStructure.algaeProcessorScore( soloController.rightBumper()), diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f592b4fc..ecbfd6de 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,8 +6,10 @@ import au.grapplerobotics.CanBridge; import com.pathplanner.lib.commands.FollowPathCommand; +import edu.wpi.first.net.WebServer; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.RobotBase; @@ -37,7 +39,6 @@ public static Robot getInstance() { private final RobotType robotType; public final Controls controls; public final Subsystems subsystems; - public final Sensors sensors; public final SuperStructure superStructure; private final PowerDistribution PDH; @@ -55,7 +56,7 @@ protected Robot() { sensors = new Sensors(); subsystems = new Subsystems(sensors); if (SubsystemConstants.DRIVEBASE_ENABLED) { - AutoBuilderConfig.buildAuto(subsystems.drivebaseSubsystem); + AutoBuilderConfig.buildAuto(subsystems.getDrivetrain()); } if (SubsystemConstants.ELEVATOR_ENABLED && SubsystemConstants.ARMPIVOT_ENABLED @@ -107,6 +108,7 @@ protected Robot() { FollowPathCommand.warmupCommand().schedule(); } MatchTab.create(sensors, subsystems); + WebServer.start(5800, Filesystem.getDeployDirectory().getPath()); } @Override @@ -125,10 +127,11 @@ public void disabledPeriodic() {} @Override public void disabledExit() { - // on the end of diabling, make sure all of the motors are set to break and wont move upon + // on the end of diabling, make sure all of the motors are set to break and wont + // move upon // enabling - if (subsystems.drivebaseSubsystem != null) { - subsystems.drivebaseSubsystem.brakeMotors(); + if (subsystems.getDrivetrain() != null) { + subsystems.getDrivetrain().brakeMotors(); } if (subsystems.climbPivotSubsystem != null) { subsystems.climbPivotSubsystem.brakeMotors(); diff --git a/src/main/java/frc/robot/Subsystems.java b/src/main/java/frc/robot/Subsystems.java index 76a24f44..cc757115 100644 --- a/src/main/java/frc/robot/Subsystems.java +++ b/src/main/java/frc/robot/Subsystems.java @@ -42,7 +42,7 @@ public static class SubsystemConstants { // Subsystems go here public final DrivebaseWrapper drivebaseWrapper; - public final CommandSwerveDrivetrain drivebaseSubsystem; + private final CommandSwerveDrivetrain drivebaseSubsystem; public final VisionSubsystem visionSubsystem; public final ElevatorSubsystem elevatorSubsystem; public final ArmPivot armPivotSubsystem; @@ -124,4 +124,8 @@ public Subsystems(Sensors sensors) { elevatorLEDSubsystem = null; } } + + public CommandSwerveDrivetrain getDrivetrain() { + return drivebaseSubsystem; + } } diff --git a/src/main/java/frc/robot/generated/CompTunerConstants.java b/src/main/java/frc/robot/generated/CompTunerConstants.java index f55ad969..c2494dbb 100644 --- a/src/main/java/frc/robot/generated/CompTunerConstants.java +++ b/src/main/java/frc/robot/generated/CompTunerConstants.java @@ -55,7 +55,7 @@ public class CompTunerConstants { // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(80.0); + private static final Current kSlipCurrent = Amps.of(60.0); // 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. diff --git a/src/main/java/frc/robot/sensors/ArmSensor.java b/src/main/java/frc/robot/sensors/ArmSensor.java index 9592321b..a3e3c227 100644 --- a/src/main/java/frc/robot/sensors/ArmSensor.java +++ b/src/main/java/frc/robot/sensors/ArmSensor.java @@ -16,8 +16,8 @@ public class ArmSensor { private final LaserCan mainSensor; // VALUES ARE IN METERS - private static final double TROUGH_LOWER_LIMIT = 0.10; - private static final double TROUGH_UPPER_LIMIT = 0.20; + private static final double TROUGH_LOWER_LIMIT = 0.12; + private static final double TROUGH_UPPER_LIMIT = 0.22; private static final double CLAW_LOWER_LIMIT = 0.01; private static final double CLAW_UPPER_LIMIT = 0.09; diff --git a/src/main/java/frc/robot/sensors/ElevatorLight.java b/src/main/java/frc/robot/sensors/ElevatorLight.java index f6139f74..9a6372c5 100644 --- a/src/main/java/frc/robot/sensors/ElevatorLight.java +++ b/src/main/java/frc/robot/sensors/ElevatorLight.java @@ -17,6 +17,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Hardware; import frc.robot.util.SoloScoringMode; +import frc.robot.util.ScoringMode; import java.util.function.DoubleSupplier; import java.util.function.Supplier; @@ -82,7 +83,7 @@ public Command animate(LEDPattern animation, String name) { .withName("Animate" + name); } - public Command showScoringMode(Supplier scoringMode) { + public Command showScoringMode(Supplier scoringMode, ScoringMode intakingMode) { return run(() -> { SoloScoringMode currentMode = scoringMode.get(); if (currentMode == SoloScoringMode.ALGAE_IN_CLAW) { @@ -90,7 +91,11 @@ public Command showScoringMode(Supplier scoringMode) { } else if (currentMode == SoloScoringMode.CORAL_IN_CLAW) { updateLEDs(LEDPattern.solid(Color.kWhite)); } else if (currentMode == SoloScoringMode.NO_GAME_PIECE) { - updateLEDs(LEDPattern.solid(Color.kDeepPink)); + if (intakingMode == ScoringMode.CORAL) { + updateLEDs(LEDPattern.solid(Color.kDarkOliveGreen)); + } else if(intakingMode == ScoringMode.ALGAE) { + updateLEDs(LEDPattern.solid(Color.kChocolate)); + } } }) .withName("Animate Scoring Mode"); diff --git a/src/main/java/frc/robot/subsystems/ArmPivot.java b/src/main/java/frc/robot/subsystems/ArmPivot.java index 880a9f82..3aa2b614 100644 --- a/src/main/java/frc/robot/subsystems/ArmPivot.java +++ b/src/main/java/frc/robot/subsystems/ArmPivot.java @@ -32,7 +32,7 @@ public class ArmPivot extends SubsystemBase { // Presets - private final double ARMPIVOT_KP = 50; // previously 38.5 + private final double ARMPIVOT_KP = 38.5; // previously 50 private final double ARMPIVOT_KI = 0; private final double ARMPIVOT_KD = 0; private final double ARMPIVOT_KS = 0.1; @@ -195,7 +195,7 @@ public void factoryDefaults() { talonFXConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake; // enabling current limits - talonFXConfiguration.CurrentLimits.StatorCurrentLimit = 80; + talonFXConfiguration.CurrentLimits.StatorCurrentLimit = 40; talonFXConfiguration.CurrentLimits.StatorCurrentLimitEnable = true; talonFXConfiguration.CurrentLimits.SupplyCurrentLimit = 20; talonFXConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true; diff --git a/src/main/java/frc/robot/subsystems/GroundArm.java b/src/main/java/frc/robot/subsystems/GroundArm.java index 1e69da24..56f1d3e1 100644 --- a/src/main/java/frc/robot/subsystems/GroundArm.java +++ b/src/main/java/frc/robot/subsystems/GroundArm.java @@ -25,7 +25,7 @@ public class GroundArm extends SubsystemBase { private final double ARMPIVOT_KV = 4; private final double ARMPIVOT_KG = 0.048; private final double ARMPIVOT_KA = 0; - public static final double STOWED_POSITION = 0.45; + public static final double STOWED_POSITION = 0.46; public static final double UP_POSITION = 0.27; // untested - should be somewhere in between stowed and ground public static final double GROUND_POSITION = -0.050; diff --git a/src/main/java/frc/robot/subsystems/GroundSpinny.java b/src/main/java/frc/robot/subsystems/GroundSpinny.java index 96182264..9398d571 100644 --- a/src/main/java/frc/robot/subsystems/GroundSpinny.java +++ b/src/main/java/frc/robot/subsystems/GroundSpinny.java @@ -48,7 +48,7 @@ public void configMotors() { configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake; cfg.apply(configuration); // enabling stator current limits - currentLimits.StatorCurrentLimit = 40; // subject to change + currentLimits.StatorCurrentLimit = 100; // subject to change currentLimits.StatorCurrentLimitEnable = true; currentLimits.SupplyCurrentLimit = 20; // subject to change currentLimits.SupplyCurrentLimitEnable = true; diff --git a/src/main/java/frc/robot/subsystems/SuperStructure.java b/src/main/java/frc/robot/subsystems/SuperStructure.java index d37b680b..a968d280 100644 --- a/src/main/java/frc/robot/subsystems/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/SuperStructure.java @@ -414,18 +414,28 @@ public Command algaeProcessorScore(BooleanSupplier score) { // scores algae in p .withName("Algae Processor Score"); } - public Command algaeNetScore(BooleanSupplier score) { + public Command algaeNetPrescore() { + return Commands.parallel( + elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_TWO_THREE), + armPivot.moveToPosition(ArmPivot.CORAL_PRESET_UP)) + .withName("Algae Net Prescore"); + } + + public Command algaeNetScore() { return Commands.sequence( Commands.parallel( elevator.setLevel(ElevatorSubsystem.ALGAE_NET_SCORE), - armPivot.moveToPosition(ArmPivot.ALGAE_NET_SCORE), - spinnyClaw.algaeIntakePower()), - Commands.waitUntil(score), - spinnyClaw.algaeHoldExtakePower().withTimeout(0.7), - Commands.waitSeconds(0.7), + Commands.sequence( + spinnyClaw.algaeIntakePower(), + Commands.waitSeconds(0.25), + Commands.parallel( + armPivot.moveToPosition(ArmPivot.ALGAE_NET_SCORE), + Commands.sequence( + Commands.waitSeconds(0.05), spinnyClaw.algaeExtakePower())))), + Commands.waitSeconds(0.2), armPivot.moveToPosition( ArmPivot.CORAL_PRESET_UP), // added to prevent hitting the barge after scoring net - elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_THREE_FOUR)) + elevator.setLevel(ElevatorSubsystem.ALGAE_LEVEL_TWO_THREE)) .withName("Algae Net Score"); } diff --git a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java index 2c77f3f0..52f2c2b5 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoAlign.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoAlign.java @@ -49,20 +49,20 @@ public static boolean readyToScore() { } public static boolean isStationary() { - var speeds = AutoLogic.s.drivebaseSubsystem.getState().Speeds; + var speeds = AutoLogic.s.getDrivetrain().getState().Speeds; return MathUtil.isNear(0, speeds.vxMetersPerSecond, 0.01) && MathUtil.isNear(0, speeds.vyMetersPerSecond, 0.01) && MathUtil.isNear(0, speeds.omegaRadiansPerSecond, Units.degreesToRadians(2)); } public static boolean isLevel() { - var rotation = AutoLogic.s.drivebaseSubsystem.getRotation3d(); + var rotation = AutoLogic.s.getDrivetrain().getRotation3d(); return MathUtil.isNear(0, rotation.getX(), Units.degreesToRadians(2)) && MathUtil.isNear(0, rotation.getY(), Units.degreesToRadians(2)); } public static boolean isCloseEnough() { - var currentPose = AutoLogic.s.drivebaseSubsystem.getState().Pose; + var currentPose = AutoLogic.s.getDrivetrain().getState().Pose; var branchPose = AutoAlignCommand.getNearestBranch(currentPose); return currentPose.getTranslation().getDistance(branchPose.getTranslation()) < 0.05; } @@ -231,7 +231,8 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - // Create a swerve request to stop all motion by setting velocities and rotational rate to 0 + // Create a swerve request to stop all motion by setting velocities and + // rotational rate to 0 SwerveRequest stop = driveRequest.withVelocityX(0).withVelocityY(0).withRotationalRate(0); // Set the drive control with the stop request to halt all movement drive.setControl(stop); diff --git a/src/main/java/frc/robot/subsystems/auto/AutoBuilderConfig.java b/src/main/java/frc/robot/subsystems/auto/AutoBuilderConfig.java index 02f94323..3e13a4cc 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoBuilderConfig.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoBuilderConfig.java @@ -39,7 +39,8 @@ public static void buildAuto(CommandSwerveDrivetrain drivebase) { ), RobotConfig.fromGUISettings(), // The robot configuration () -> { - // Boolean supplier that controls when the path will be mirrored for the red alliance + // 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 @@ -49,7 +50,7 @@ public static void buildAuto(CommandSwerveDrivetrain drivebase) { } return false; }, - s.drivebaseSubsystem // Reference to this subsystem to set requirements + s.getDrivetrain() // Reference to this subsystem to set requirements ); } catch (IOException | ParseException e) { diff --git a/src/main/java/frc/robot/subsystems/auto/AutoLogic.java b/src/main/java/frc/robot/subsystems/auto/AutoLogic.java index cee62d7f..7f66f038 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoLogic.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoLogic.java @@ -111,7 +111,9 @@ public static enum StartPosition { new AutoPath("MRSF_G-F_WithWait", "MRSF_G-F_WithWait"), new AutoPath("MRSF_G-H", "MRSF_G-H"), new AutoPath("MLSF_H-K_Cooking", "MLSF_H-K_Cooking"), - new AutoPath("MLSF_H-G", "MLSF_H-G")); + new AutoPath("MLSF_H-G", "MLSF_H-G"), + new AutoPath("OSWCounterAlgaeEF", "OSWCounterAlgaeEF"), + new AutoPath("OSMCounterAlgaeEF", "OSMCounterAlgaeEF")); private static List threePiecePaths = List.of( @@ -206,7 +208,8 @@ public static Command getAutoCommand(String pathName) // Load the path you want to follow using its name in the GUI PathPlannerPath path = PathPlannerPath.fromPathFile(pathName); - // Create a path following command using AutoBuilder. This will also trigger event markers. + // Create a path following command using AutoBuilder. This will also trigger + // event markers. return AutoBuilder.followPath(path); } @@ -289,7 +292,7 @@ public static Command scoreCommand() { if (r.superStructure != null) { return new ConditionalCommand( // If true: - AutoAlign.autoAlign(s.drivebaseSubsystem, controls) + AutoAlign.autoAlign(s.getDrivetrain(), controls) .repeatedly() .withDeadline(r.superStructure.coralLevelFour(() -> AutoAlign.readyToScore())) .withName("scoreCommand"), @@ -298,13 +301,13 @@ public static Command scoreCommand() { // Condition: () -> ARMSENSOR_ENABLED && r.sensors.armSensor.booleanInClaw()); } - return AutoAlign.autoAlign(s.drivebaseSubsystem, controls) + return AutoAlign.autoAlign(s.getDrivetrain(), controls) .withName("scoreCommand-noSuperstructure"); } public static Command algaeCommand23() { if (r.superStructure != null) { - return AlgaeAlign.algaeAlign(s.drivebaseSubsystem, controls) + return AlgaeAlign.algaeAlign(s.getDrivetrain(), controls) .repeatedly() .withDeadline(r.superStructure.algaeLevelTwoThreeIntake()) .withName("algaeCommand23"); @@ -314,7 +317,7 @@ public static Command algaeCommand23() { public static Command algaeCommand34() { if (r.superStructure != null) { - return AlgaeAlign.algaeAlign(s.drivebaseSubsystem, controls) + return AlgaeAlign.algaeAlign(s.getDrivetrain(), controls) .repeatedly() .withDeadline(r.superStructure.algaeLevelThreeFourIntake()) .withName("algaeCommand34"); @@ -325,7 +328,13 @@ public static Command algaeCommand34() { public static Command netCommand() { if (r.superStructure != null) { return BargeAlign.bargeScore( - s.drivebaseSubsystem, r.superStructure, () -> 0, () -> 0, () -> 0, () -> false) + s.getDrivetrain(), + r.superStructure, + () -> 0, + () -> 0, + () -> 0, + () -> false, + s.drivebaseWrapper) .withName("net"); } return Commands.none().withName("net"); diff --git a/src/main/java/frc/robot/subsystems/auto/BargeAlign.java b/src/main/java/frc/robot/subsystems/auto/BargeAlign.java index 0e87d818..48810d4d 100644 --- a/src/main/java/frc/robot/subsystems/auto/BargeAlign.java +++ b/src/main/java/frc/robot/subsystems/auto/BargeAlign.java @@ -10,6 +10,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.DrivebaseWrapper; import frc.robot.subsystems.SuperStructure; import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain; import java.util.function.BooleanSupplier; @@ -27,17 +28,23 @@ public class BargeAlign extends Command { .withDriveRequestType(DriveRequestType.OpenLoopVoltage) .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); - public static boolean atScoringXPosition(CommandSwerveDrivetrain drivebasesubsystem) { - double robotX = drivebasesubsystem.getState().Pose.getX(); - return blueBargeLineX < robotX && robotX < redBargeLineX; + public static boolean atScoringXPosition(DrivebaseWrapper drivebaseWrapper) { + if (drivebaseWrapper != null) { + double robotX = drivebaseWrapper.getEstimatedPosition().getX(); + return blueBargeLineX < robotX && robotX < redBargeLineX; + } else { + return false; + } } private static Command driveToBlackLine( CommandSwerveDrivetrain drivebaseSubsystem, DoubleSupplier manualXSpeed, DoubleSupplier manualYSpeed, - DoubleSupplier manualRotateSpeed) { - return new BargeAlign(drivebaseSubsystem, manualXSpeed, manualYSpeed, manualRotateSpeed) + DoubleSupplier manualRotateSpeed, + DrivebaseWrapper drivebaseWrapper) { + return new BargeAlign( + drivebaseSubsystem, manualXSpeed, manualYSpeed, manualRotateSpeed, drivebaseWrapper) .withName("Drive to Black Line"); } @@ -56,29 +63,27 @@ public static Command bargeScore( DoubleSupplier manualXSpeed, DoubleSupplier manualYSpeed, DoubleSupplier manualRotateSpeed, - BooleanSupplier manualScore) { + BooleanSupplier manualScore, + DrivebaseWrapper drivebaseWrapper) { return Commands.sequence( - BargeAlign.driveToBlackLine( - drivebaseSubsystem, manualXSpeed, manualYSpeed, manualRotateSpeed) - .asProxy(), - BargeAlign.driveToBarge(drivebaseSubsystem, manualXSpeed, manualYSpeed, manualRotateSpeed) - .asProxy() - .withDeadline( - superStructure.algaeNetScore( - () -> - manualScore.getAsBoolean() - || BargeAlign.atScoringXPosition(drivebaseSubsystem)))); + Commands.parallel( + superStructure.algaeNetPrescore(), + BargeAlign.driveToBlackLine( + drivebaseSubsystem, manualXSpeed, manualYSpeed, manualRotateSpeed) + .asProxy()), + superStructure.algaeNetScore()); } private static Command driveToBarge( CommandSwerveDrivetrain drivebaseSubsystem, DoubleSupplier manualXSpeed, DoubleSupplier manualYSpeed, - DoubleSupplier manualRotateSpeed) { + DoubleSupplier manualRotateSpeed, + DrivebaseWrapper drivebaseWrapper) { return drivebaseSubsystem .applyRequest( () -> { - boolean onRedSide = drivebaseSubsystem.getState().Pose.getX() > fieldLength / 2; + boolean onRedSide = drivebaseWrapper.getEstimatedPosition().getX() > fieldLength / 2; double xSpeed = onRedSide ? -xBargeDriveSpeed : xBargeDriveSpeed; double manualX = manualXSpeed.getAsDouble(); if (manualX != 0) { @@ -89,7 +94,7 @@ private static Command driveToBarge( .withVelocityY(manualYSpeed.getAsDouble()) .withRotationalRate(manualRotateSpeed.getAsDouble()); }) - .until(() -> BargeAlign.atScoringXPosition(drivebaseSubsystem)) + .until(() -> BargeAlign.atScoringXPosition(drivebaseWrapper)) .finallyDo( () -> drivebaseSubsystem.setControl( @@ -104,13 +109,16 @@ private static Command driveToBarge( private DoubleSupplier manualXSpeed; private DoubleSupplier manualYSpeed; private DoubleSupplier manualRotateSpeed; + private DrivebaseWrapper drivebaseWrapper; private BargeAlign( CommandSwerveDrivetrain drive, DoubleSupplier manualXSpeed, DoubleSupplier manualYSpeed, - DoubleSupplier manualRotateSpeed) { + DoubleSupplier manualRotateSpeed, + DrivebaseWrapper drivebaseWrapper) { this.drive = drive; + this.drivebaseWrapper = drivebaseWrapper; this.manualXSpeed = manualXSpeed; this.manualYSpeed = manualYSpeed; this.manualRotateSpeed = manualRotateSpeed; @@ -121,7 +129,7 @@ private BargeAlign( @Override public void initialize() { - boolean onRedSide = drive.getState().Pose.getX() > fieldLength / 2; + boolean onRedSide = drivebaseWrapper.getEstimatedPosition().getX() > fieldLength / 2; double targetX = onRedSide ? redBlacklineX : blueBlacklineX; Rotation2d targetAngle = onRedSide ? Rotation2d.k180deg : Rotation2d.kZero; pidX.setSetpoint(targetX); @@ -130,7 +138,7 @@ public void initialize() { @Override public void execute() { - Pose2d currentPose = drive.getState().Pose; + Pose2d currentPose = drivebaseWrapper.getEstimatedPosition(); // Calculate the power for X direction and clamp it between -2 and 2 double powerX = pidX.calculate(currentPose.getX()); powerX = MathUtil.clamp(powerX, -2, 2);