diff --git a/src/main/deploy/elastic-layout.json b/src/main/deploy/elastic-layout.json new file mode 100644 index 0000000..c2d387c --- /dev/null +++ b/src/main/deploy/elastic-layout.json @@ -0,0 +1,950 @@ +{ + "version": 1.0, + "grid_size": 32, + "tabs": [ + { + "name": "Autonomous", + "grid_layout": { + "layouts": [ + { + "title": "List Layout", + "x": 1568.0, + "y": 0.0, + "width": 352.0, + "height": 448.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "Auto Delay", + "x": 1984.0, + "y": 416.0, + "width": 288.0, + "height": 128.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/SmartDashboard/Auto Delay", + "period": 0.06, + "sort_options": false + } + }, + { + "title": "Start Position", + "x": 1984.0, + "y": 480.0, + "width": 288.0, + "height": 128.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/SmartDashboard/Start Position", + "period": 0.06, + "sort_options": false + } + }, + { + "title": "Alliance Fuel Source", + "x": 1952.0, + "y": 256.0, + "width": 288.0, + "height": 128.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/SmartDashboard/Alliance Fuel Source", + "period": 0.06, + "sort_options": false + } + }, + { + "title": "Neutral Zone", + "x": 1664.0, + "y": 320.0, + "width": 128.0, + "height": 128.0, + "type": "Toggle Switch", + "properties": { + "topic": "/SmartDashboard/Neutral Zone", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Climb", + "x": 1760.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Toggle Switch", + "properties": { + "topic": "/SmartDashboard/Climb", + "period": 0.06, + "data_type": "boolean" + } + } + ] + } + ], + "containers": [ + { + "title": "Alerts", + "x": 1568.0, + "y": 448.0, + "width": 352.0, + "height": 320.0, + "type": "Alerts", + "properties": { + "topic": "/SmartDashboard/Alerts", + "period": 0.02 + } + }, + { + "title": "Autonomous Mode", + "x": 0.0, + "y": 0.0, + "width": 1568.0, + "height": 768.0, + "type": "Field", + "properties": { + "topic": "/SmartDashboard/Autonomous Mode", + "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, + "show_robot_outside_widget": true + } + } + ] + } + }, + { + "name": "Teleoperated", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Alerts", + "x": 1664.0, + "y": 0.0, + "width": 256.0, + "height": 768.0, + "type": "Alerts", + "properties": { + "topic": "/SmartDashboard/Alerts", + "period": 0.02 + } + }, + { + "title": "Match Time", + "x": 0.0, + "y": 0.0, + "width": 1152.0, + "height": 768.0, + "type": "Match Time", + "properties": { + "topic": "Dashboard/Robot Values/Match Time", + "period": 0.02, + "data_type": "double", + "time_display_mode": "Minutes and Seconds", + "red_start_time": 15, + "yellow_start_time": 30 + } + }, + { + "title": "Hub Active", + "x": 1152.0, + "y": 0.0, + "width": 512.0, + "height": 192.0, + "type": "Boolean Box", + "properties": { + "topic": "Dashboard/Robot Values/Hub Active", + "period": 0.02, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Turret Has Target", + "x": 1152.0, + "y": 576.0, + "width": 256.0, + "height": 192.0, + "type": "Boolean Box", + "properties": { + "topic": "Dashboard/Robot Values/Turret Has Target", + "period": 0.02, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Hub State Timer", + "x": 1152.0, + "y": 192.0, + "width": 512.0, + "height": 192.0, + "type": "Text Display", + "properties": { + "topic": "Dashboard/Robot Values/Hub State Time Remaining", + "period": 0.02, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Shooter Mode", + "x": 1152.0, + "y": 384.0, + "width": 512.0, + "height": 192.0, + "type": "Boolean Box", + "properties": { + "topic": "Dashboard/Robot Values/Shooter Is Shoot Mode", + "period": 0.02, + "data_type": "boolean", + "true_color": 4278190335, + "false_color": 4294934272, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Turret Lined Up", + "x": 1408.0, + "y": 576.0, + "width": 256.0, + "height": 192.0, + "type": "Boolean Box", + "properties": { + "topic": "Dashboard/Robot Values/Turret Lined Up", + "period": 0.02, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + } + ] + } + }, + { + "name": "Programmer", + "grid_layout": { + "layouts": [ + { + "title": "Hood PID", + "x": 0.0, + "y": 0.0, + "width": 256.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "Hood PID", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 128.0, + "type": "PIDController", + "properties": { + "topic": "/SmartDashboard/Hood PID", + "period": 0.05 + } + } + ] + }, + { + "title": "Auto Driving", + "x": 256.0, + "y": 0.0, + "width": 256.0, + "height": 736.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "Auto X PID", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 128.0, + "type": "PIDController", + "properties": { + "topic": "/SmartDashboard/Auto X PID", + "period": 0.05 + } + }, + { + "title": "Auto Y PID", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 128.0, + "type": "PIDController", + "properties": { + "topic": "/SmartDashboard/Auto Y PID", + "period": 0.05 + } + }, + { + "title": "Auto Heading PID", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 128.0, + "type": "PIDController", + "properties": { + "topic": "/SmartDashboard/Auto Heading PID", + "period": 0.05 + } + } + ] + }, + { + "title": "Intake", + "x": 512.0, + "y": 0.0, + "width": 256.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "RIGHT" + }, + "children": [ + { + "title": "Forward Voltage", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Intake Forward Voltage", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Reverse Voltage", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Intake Reverse Voltage", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Extension Max Position", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Intake Extension Max Position", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Extend Voltage", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Intake Extend Voltage", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Retract Voltage", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Intake Retract Voltage", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + } + ] + }, + { + "title": "Hood", + "x": 768.0, + "y": 0.0, + "width": 256.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "RIGHT" + }, + "children": [ + { + "title": "Min Angle", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Hood Min Angle", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Max Angle", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Hood Max Angle", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Shoot Angle", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Hood Shoot Angle", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Pass Angle", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Hood Pass Angle", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Tolerance", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Hood Tolerance", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + } + ] + }, + { + "title": "Turret", + "x": 1024.0, + "y": 0.0, + "width": 256.0, + "height": 640.0, + "type": "List Layout", + "properties": { + "label_position": "RIGHT" + }, + "children": [ + { + "title": "kP", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Turret kP", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "kI", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Turret kI", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "kD", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Turret kD", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Test Setpoint (deg)", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Turret Test Setpoint", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Min Angle", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Turret Min Angle", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Max Angle", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Turret Max Angle", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Home Angle", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Turret Home Angle", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Tolerance", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Turret Tolerance", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Pass Angle", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Turret Pass Angle", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + } + ] + }, + { + "title": "Flywheel", + "x": 1280.0, + "y": 0.0, + "width": 256.0, + "height": 512.0, + "type": "List Layout", + "properties": { + "label_position": "RIGHT" + }, + "children": [ + { + "title": "kP", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Flywheel kP", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "kD", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Flywheel kD", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "kS (V)", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Flywheel kS", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "kV (V/RPM)", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Flywheel kV", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "kA (V/rps²)", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Flywheel kA", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Tolerance", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Flywheel Tolerance", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Pass Velocity", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Flywheel Pass Velocity", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + } + ] + }, + { + "title": "Feeder", + "x": 1280.0, + "y": 512.0, + "width": 256.0, + "height": 224.0, + "type": "List Layout", + "properties": { + "label_position": "RIGHT" + }, + "children": [ + { + "title": "Feed Voltage", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Feeder Voltage", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + } + ] + }, + { + "title": "Vision", + "x": 512.0, + "y": 256.0, + "width": 256.0, + "height": 256.0, + "type": "List Layout", + "properties": { + "label_position": "RIGHT" + }, + "children": [ + { + "title": "Max Angular Rate", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Vision Max Angular Rate", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Max Tilt", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Vision Max Tilt", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + } + ] + }, + { + "title": "Climber", + "x": 1536.0, + "y": 0.0, + "width": 256.0, + "height": 736.0, + "type": "List Layout", + "properties": { + "label_position": "RIGHT" + }, + "children": [ + { + "title": "L1 Rotation", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Climber L1 Rotation", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "L3 Rotation", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Climber L3 Rotation", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Extension Threshold", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Climber Extension Threshold", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Retraction Threshold", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Climber Retraction Threshold", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Extend Voltage", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Climber Extend Voltage", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Retract Voltage", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Climber Retract Voltage", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Rotate Voltage", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Climber Rotate Voltage", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Tolerance", + "x": 0.0, + "y": 0.0, + "width": 121.0, + "height": 64.0, + "type": "Text Display", + "properties": { + "topic": "/Preferences/Dashboard/Dashboard Settings/Climber Tolerance", + "period": 0.02, + "data_type": "double", + "show_submit_button": true + } + } + ] + } + ], + "containers": [ + { + "title": "Turret Test Mode", + "x": 768.0, + "y": 256.0, + "width": 256.0, + "height": 192.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Turret Test Mode", + "period": 0.05, + "show_type": true, + "maximize_button_space": true + } + } + ] + } + } + ] +} diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index ca6af30..1b33dfb 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -2,18 +2,48 @@ import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue; + +import java.util.stream.IntStream; + import com.ctre.phoenix6.swerve.SwerveRequest; import choreo.auto.AutoFactory; import choreo.trajectory.SwerveSample; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.Drive; public class Autos { + private enum StartPosition + { + LeftTrench("Left Trench"), LeftBump("Left Bump"), Hub("Hub"), RightBump("Right Bump"), RightTrench("Right Trench"); + + private final String displayName; + + private StartPosition(String displayName) + { + this.displayName = displayName; + } + } + + private enum AllianceFuelSourceSelection + { + None("None"), Depot("Depot"), Outpost("Outpost"), DepotToOutpost("Depot to Outpost"), OutpostToDepot("Outpost to Depot"); + + private final String displayName; + + private AllianceFuelSourceSelection(String displayName) + { + this.displayName = displayName; + } + } + private final AutoFactory _autoFactory; private final Drive _driveSubsystem; private final SwerveRequest.FieldCentric _autoFollowingRequest = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); @@ -21,6 +51,14 @@ public class Autos private final PIDController _yController = new PIDController(0.0, 0.0, 0.0); private final PIDController _headingController = new PIDController(0.0, 0.0, 0.0); + // Dashboard + private final Field2d _field; + private final SendableChooser _autoDelayChooser; + private final SendableChooser _startPositionChooser; + private final SendableChooser _allianceFuelSourceSelectionChooser; + private final String _neutralZoneNTKey; + private final String _climbNTKey; + public Autos(Drive driveSubsystem) { _driveSubsystem = driveSubsystem; @@ -35,6 +73,45 @@ public Autos(Drive driveSubsystem) driveSubsystem ); // @formatter:on + + // Dashboard + _field = new Field2d(); + + _autoDelayChooser = new SendableChooser<>(); + _startPositionChooser = new SendableChooser<>(); + _allianceFuelSourceSelectionChooser = new SendableChooser<>(); + + _neutralZoneNTKey = "Neutral Zone"; + _climbNTKey = "Climb"; + + _autoDelayChooser.setDefaultOption("0", 0); + IntStream.range(1, 6).forEach(n -> _autoDelayChooser.addOption(String.valueOf(n), n)); + + var startPositions = StartPosition.values(); + var fuelSources = AllianceFuelSourceSelection.values(); + + _startPositionChooser.setDefaultOption(startPositions[0].displayName, startPositions[0]); + _allianceFuelSourceSelectionChooser.setDefaultOption(fuelSources[0].displayName, fuelSources[0]); + + for (int i = 1; i < startPositions.length; i++) + { + _startPositionChooser.addOption(startPositions[i].displayName, startPositions[i]); + } + + for (int i = 1; i < fuelSources.length; i++) + { + _allianceFuelSourceSelectionChooser.addOption(fuelSources[i].displayName, fuelSources[i]); + } + + SmartDashboard.putData("Autonomous Mode", _field); + SmartDashboard.putData("Auto Delay", _autoDelayChooser); + SmartDashboard.putData("Start Position", _startPositionChooser); + SmartDashboard.putData("Alliance Fuel Source", _allianceFuelSourceSelectionChooser); + SmartDashboard.putData("Auto X PID", _xController); + SmartDashboard.putData("Auto Y PID", _yController); + SmartDashboard.putData("Auto Heading PID", _headingController); + SmartDashboard.putBoolean(_neutralZoneNTKey, false); + SmartDashboard.putBoolean(_climbNTKey, false); } private void followTrajectory(SwerveSample sample) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7a28db4..76caad8 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -9,6 +9,8 @@ import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.net.WebServer; +import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.simulation.RoboRioSim; import edu.wpi.first.wpilibj2.command.Command; @@ -28,6 +30,10 @@ public Robot() { m_robotContainer = new RobotContainer(); Epilogue.bind(this); + + // Start a webserver in the deploy directory so Elastic can remotely download + // layouts + WebServer.start(5800, Filesystem.getDeployDirectory().getPath()); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 388592a..b6b4860 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -24,6 +24,7 @@ import frc.robot.Constants.DriveConstants; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.Drive; +import frc.robot.subsystems.dashboard.Dashboard; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.Turret; @@ -44,8 +45,9 @@ public class RobotContainer private final Drive _drivetrain = TunerConstants.createDrivetrain(); private final Intake _intake = new Intake(); private final Shooter _shooter = new Shooter(); - // private final Turret _turret = new Turret(_drivetrain::getState); - private final Autos _autos = new Autos(_drivetrain); + private final Turret _turret = new Turret(_drivetrain::getState); + private final Autos _autos = new Autos(_drivetrain); + private final Dashboard _dashboard = new Dashboard(_intake, _shooter, _turret); public RobotContainer() { diff --git a/src/main/java/frc/robot/subsystems/dashboard/Dashboard.java b/src/main/java/frc/robot/subsystems/dashboard/Dashboard.java new file mode 100644 index 0000000..373e1ea --- /dev/null +++ b/src/main/java/frc/robot/subsystems/dashboard/Dashboard.java @@ -0,0 +1,142 @@ +package frc.robot.subsystems.dashboard; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.DegreesPerSecond; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Value; +import static edu.wpi.first.units.Units.Volts; + +import edu.wpi.first.networktables.BooleanPublisher; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Preferences; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.ClimberConstants; +import frc.robot.Constants.IntakeConstants; +import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.VisionConstants; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.shooter.Hood.HoodPosition; +import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.Turret; +import frc.robot.util.Utilities; + +public class Dashboard extends SubsystemBase +{ + private final Intake _intake; + private final Shooter _shooter; + private final Turret _turret; + private final DoublePublisher _matchTimePublisher; + private final BooleanPublisher _hubActivePublisher; + private final DoublePublisher _hubStateTimeRemainingPublisher; + private final BooleanPublisher _turretHasTargetPublisher; + private final BooleanPublisher _turretLinedUpPublisher; + private final BooleanPublisher _shooterShootModePublisher; + private final StringPublisher _shooterModePublisher; + + public Dashboard(Intake intake, Shooter shooter, Turret turret) + { + _intake = intake; + _shooter = shooter; + _turret = turret; + + var dashboardTable = NetworkTableInstance.getDefault().getTable("Dashboard"); + var valuesTable = dashboardTable.getSubTable("Robot Values"); + + _matchTimePublisher = valuesTable.getDoubleTopic("Match Time").publish(); + _hubActivePublisher = valuesTable.getBooleanTopic("Hub Active").publish(); + _hubStateTimeRemainingPublisher = valuesTable.getDoubleTopic("Hub State Time Remaining").publish(); + _turretHasTargetPublisher = valuesTable.getBooleanTopic("Turret Has Target").publish(); + _turretLinedUpPublisher = valuesTable.getBooleanTopic("Turret Lined Up").publish(); + _shooterShootModePublisher = valuesTable.getBooleanTopic("Shooter Is Shoot Mode").publish(); + _shooterModePublisher = valuesTable.getStringTopic("Shooter Mode").publish(); + + initializeTuningPreferences(); + } + + @Override + public void periodic() + { + _matchTimePublisher.set(Math.max(DriverStation.getMatchTime(), 0.0)); + + boolean hubActive = Utilities.isHubActive(); + _hubActivePublisher.set(hubActive); + _hubStateTimeRemainingPublisher.set(Math.max(Utilities.getHubStateTimeRemaining().in(Seconds), 0.0)); + + if (_turret != null) + { + _turretHasTargetPublisher.set(_turret.hasTarget()); + _turretLinedUpPublisher.set(_turret.isLinedUp()); + } + else + { + _turretHasTargetPublisher.set(false); + _turretLinedUpPublisher.set(false); + } + + HoodPosition shooterMode = _shooter.getShooterMode(); + _shooterShootModePublisher.set(shooterMode != HoodPosition.Pass); + _shooterModePublisher.set(shooterMode.name()); + } + + private static void initPreference(String key, double value) + { + Preferences.initDouble(key, value); + } + + private void initializeTuningPreferences() + { + String prefix = "Dashboard/Dashboard Settings/"; + + initPreference(prefix + "Intake Forward Voltage", IntakeConstants.INTAKE_VOLTS.in(Volts)); + initPreference(prefix + "Intake Reverse Voltage", IntakeConstants.REVERSE_VOLTS.in(Volts)); + initPreference(prefix + "Intake Extension Max Position", IntakeConstants.EXTENSION_MAX_POSITION.in(Inches)); + initPreference(prefix + "Intake Extend Voltage", IntakeConstants.EXTEND_VOLTS.in(Volts)); + initPreference(prefix + "Intake Retract Voltage", IntakeConstants.RETRACT_VOLTS.in(Volts)); + + initPreference(prefix + "Flywheel kP", ShooterConstants.FLYWHEEL_KP); + initPreference(prefix + "Flywheel kD", ShooterConstants.FLYWHEEL_KD); + initPreference(prefix + "Flywheel kS", ShooterConstants.FLYWHEEL_KS.in(Volts)); + initPreference(prefix + "Flywheel kV", ShooterConstants.FLYWHEEL_KV.in(Volts.per(RPM))); + initPreference(prefix + "Flywheel kA", ShooterConstants.FLYWHEEL_KA.in(Volts.per(RotationsPerSecondPerSecond))); + initPreference(prefix + "Flywheel Tolerance", ShooterConstants.FLYWHEEL_TOLERANCE.in(Value)); + initPreference(prefix + "Flywheel Pass Velocity", ShooterConstants.PASS_FLYWHEEL_VELOCITY.in(RPM)); + + initPreference(prefix + "Hood Min Angle", ShooterConstants.HOOD_MIN_ANGLE.in(Degrees)); + initPreference(prefix + "Hood Max Angle", ShooterConstants.HOOD_MAX_ANGLE.in(Degrees)); + initPreference(prefix + "Hood Shoot Angle", ShooterConstants.HOOD_SHOOT_ANGLE.in(Degrees)); + initPreference(prefix + "Hood Pass Angle", ShooterConstants.HOOD_PASS_ANGLE.in(Degrees)); + initPreference(prefix + "Hood Tolerance", ShooterConstants.HOOD_TOLERANCE.in(Degrees)); + + initPreference(prefix + "Turret kP", ShooterConstants.TURRET_KP); + initPreference(prefix + "Turret kI", ShooterConstants.TURRET_KI); + initPreference(prefix + "Turret kD", ShooterConstants.TURRET_KD); + initPreference(prefix + "Turret Test Setpoint", 0.0); + initPreference(prefix + "Turret Min Angle", ShooterConstants.TURRET_MIN_ANGLE.in(Degrees)); + initPreference(prefix + "Turret Max Angle", ShooterConstants.TURRET_MAX_ANGLE.in(Degrees)); + initPreference(prefix + "Turret Home Angle", ShooterConstants.TURRET_HOME_ANGLE.in(Degrees)); + initPreference(prefix + "Turret Pass Angle", ShooterConstants.TURRET_PASS_TARGET.in(Degrees)); + initPreference(prefix + "Turret Tolerance", ShooterConstants.TURRET_TOLERANCE.in(Degrees)); + + initPreference(prefix + "Feeder Voltage", ShooterConstants.FEEDER_VOLTAGE.in(Volts)); + + initPreference(prefix + "Vision Max Angular Rate", VisionConstants.MAX_ANGULAR_RATE_FOR_VISION.in(DegreesPerSecond)); + initPreference(prefix + "Vision Max Tilt", VisionConstants.MAX_TILT_FOR_VISION.in(Degrees)); + + initPreference(prefix + "Climber L1 Rotation", ClimberConstants.L1_ROTATION.in(Degrees)); + initPreference(prefix + "Climber L3 Rotation", ClimberConstants.L3_ROTATION.in(Degrees)); + initPreference(prefix + "Climber Extension Threshold", ClimberConstants.EXTENSION_THRESHOLD.in(Inches)); + initPreference(prefix + "Climber Retraction Threshold", ClimberConstants.RETRACTION_THRESHOLD.in(Inches)); + initPreference(prefix + "Climber Extend Voltage", ClimberConstants.EXTEND_OUTPUT.in(Volts)); + initPreference(prefix + "Climber Retract Voltage", ClimberConstants.RETRACT_VOLTAGE.in(Volts)); + initPreference(prefix + "Climber Rotate Voltage", ClimberConstants.ROTATE_OUTPUT.in(Volts)); + initPreference(prefix + "Climber Tolerance", ClimberConstants.ROTATION_TOLERANCE.in(Degrees)); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/Flywheel.java b/src/main/java/frc/robot/subsystems/shooter/Flywheel.java index 5f6dccc..e3dcb9c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/shooter/Flywheel.java @@ -24,6 +24,7 @@ import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.Preferences; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.simulation.FlywheelSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; @@ -47,6 +48,11 @@ public class Flywheel private AngularVelocity _targetVelocity = RPM.zero(); @Logged private Voltage _flywheelVoltage = Volts.zero(); + private double _lastKP; + private double _lastKD; + private double _lastKS; + private double _lastKV; + private double _lastKA; public Flywheel() { @@ -71,6 +77,12 @@ public Flywheel() _closedLoopController = _leadMotor.getClosedLoopController(); _flywheelEncoder = _leadMotor.getEncoder(); + _lastKP = ShooterConstants.FLYWHEEL_KP; + _lastKD = ShooterConstants.FLYWHEEL_KD; + _lastKS = ShooterConstants.FLYWHEEL_KS.in(Volts); + _lastKV = ShooterConstants.FLYWHEEL_KV.in(Volts.per(RPM)); + _lastKA = ShooterConstants.FLYWHEEL_KA.in(Volts.per(RotationsPerSecondPerSecond)); + if (RobotBase.isReal()) { _leadMotor.setCANTimeout(0); @@ -90,6 +102,24 @@ public void periodic() { _velocity = RPM.of(_flywheelEncoder.getVelocity()); _flywheelVoltage = Volts.of(_leadMotor.getAppliedOutput() * _leadMotor.getBusVoltage()); + + double newP = Preferences.getDouble("Dashboard/Dashboard Settings/Flywheel kP", ShooterConstants.FLYWHEEL_KP); + double newD = Preferences.getDouble("Dashboard/Dashboard Settings/Flywheel kD", ShooterConstants.FLYWHEEL_KD); + double newS = Preferences.getDouble("Dashboard/Dashboard Settings/Flywheel kS", ShooterConstants.FLYWHEEL_KS.in(Volts)); + double newV = Preferences.getDouble("Dashboard/Dashboard Settings/Flywheel kV", ShooterConstants.FLYWHEEL_KV.in(Volts.per(RPM))); + double newA = Preferences.getDouble("Dashboard/Dashboard Settings/Flywheel kA", ShooterConstants.FLYWHEEL_KA.in(Volts.per(RotationsPerSecondPerSecond))); + if (newP != _lastKP || newD != _lastKD || newS != _lastKS || newV != _lastKV || newA != _lastKA) + { + var config = new SparkFlexConfig(); + config.closedLoop.p(newP).d(newD); + config.closedLoop.feedForward.kS(newS).kV(newV).kA(newA); + _leadMotor.configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + _lastKP = newP; + _lastKD = newD; + _lastKS = newS; + _lastKV = newV; + _lastKA = newA; + } } public void simulationPeriodic() diff --git a/src/main/java/frc/robot/subsystems/shooter/Hood.java b/src/main/java/frc/robot/subsystems/shooter/Hood.java index 7338ce2..b8c0e98 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Hood.java +++ b/src/main/java/frc/robot/subsystems/shooter/Hood.java @@ -8,11 +8,14 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.networktables.DoubleEntry; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.simulation.DutyCycleEncoderSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.epilogue.Logged; import frc.robot.Constants.GeneralConstants; @@ -32,9 +35,9 @@ public enum HoodPosition public final Angle targetAngle; - private HoodPosition(Angle angle) + private HoodPosition(Angle targetAngle) { - targetAngle = angle; + this.targetAngle = targetAngle; } } @@ -55,6 +58,13 @@ private HoodPosition(Angle angle) @Logged private boolean _hasSetpoint; + // Dashboard + private final DoubleEntry _minAngleEntry; + private final DoubleEntry _maxAngleEntry; + private final DoubleEntry _shootAngleEntry; + private final DoubleEntry _passAngleEntry; + private final DoubleEntry _toleranceEntry; + public Hood() { _hoodMotor = new WPI_VictorSPX(CANConstants.HOOD_MOTOR); @@ -69,6 +79,18 @@ public Hood() _pidController.setTolerance(ShooterConstants.HOOD_TOLERANCE.in(Degrees)); + // Dashboard + SmartDashboard.putData("Hood PID", _pidController); + + var table = NetworkTableInstance.getDefault().getTable("Shooter").getSubTable("Hood"); + + _minAngleEntry = table.getDoubleTopic("Min Angle").getEntry(ShooterConstants.HOOD_MIN_ANGLE.in(Degrees)); + _maxAngleEntry = table.getDoubleTopic("Max Angle").getEntry(ShooterConstants.HOOD_MAX_ANGLE.in(Degrees)); + _shootAngleEntry = table.getDoubleTopic("Shoot Angle").getEntry(ShooterConstants.HOOD_SHOOT_ANGLE.in(Degrees)); + _passAngleEntry = table.getDoubleTopic("Pass Angle").getEntry(ShooterConstants.HOOD_PASS_ANGLE.in(Degrees)); + _toleranceEntry = table.getDoubleTopic("Tolerance").getEntry(ShooterConstants.HOOD_TOLERANCE.in(Degrees)); + + // Real vs. Simulation if (RobotBase.isReal()) { _hoodMotor.configFactoryDefault(); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index f32c092..cd7cd9e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -153,4 +153,14 @@ public void pass() _flywheel.setVelocity(ShooterConstants.PASS_FLYWHEEL_VELOCITY); _state = ShooterState.Priming; } + + public ShooterState getShooterState() + { + return _state; + } + + public HoodPosition getShooterMode() + { + return _hood.getHoodPosition(); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Turret.java b/src/main/java/frc/robot/subsystems/shooter/Turret.java index 3af1c93..7f472eb 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Turret.java +++ b/src/main/java/frc/robot/subsystems/shooter/Turret.java @@ -30,10 +30,14 @@ import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.AnalogPotentiometer; +import edu.wpi.first.wpilibj.Preferences; import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.simulation.AnalogInputSim; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.AIOConstants; import frc.robot.Constants.CANConstants; @@ -60,6 +64,10 @@ public enum TurretState private Supplier _swerveStateSupplier; private SwerveDriveState _swerveDriveState; private PositionVoltage _positionRequest = new PositionVoltage(0).withSlot(0); + private double _lastKP; + private double _lastKI; + private double _lastKD; + private boolean _testModeActive; private List _cachedTagFilter; @Logged private Angle _fieldTurretAngle; @@ -113,6 +121,25 @@ public Turret(Supplier swerveStateSupplier) _swerveDriveState = new SwerveDriveState(); _cachedTagFilter = List.of(); + _lastKP = ShooterConstants.TURRET_KP; + _lastKI = ShooterConstants.TURRET_KI; + _lastKD = ShooterConstants.TURRET_KD; + + _testModeActive = false; + Command testModeCommand = Commands.startRun(() -> _testModeActive = true, () -> + { + Angle setpoint = Degrees.of(Preferences.getDouble("Dashboard/Dashboard Settings/Turret Test Setpoint", 0.0)); + _turretSetpoint = MeasureUtil.clamp(setpoint, ShooterConstants.TURRET_MIN_ANGLE, ShooterConstants.TURRET_MAX_ANGLE); + _hasSetpoint = true; + _turretMotor.setControl(_positionRequest.withPosition(_turretSetpoint.times(ShooterConstants.TURRET_GEAR_RATIO).in(Rotations))); + }, this).finallyDo(() -> + { + _testModeActive = false; + _hasSetpoint = false; + _turretMotor.setVoltage(0.0); + }); + SmartDashboard.putData("Turret Test Mode", testModeCommand); + if (RobotBase.isReal()) { _turretMotorSim = null; @@ -131,6 +158,21 @@ public Turret(Supplier swerveStateSupplier) @Override public void periodic() { + double newP = Preferences.getDouble("Dashboard/Dashboard Settings/Turret kP", ShooterConstants.TURRET_KP); + double newI = Preferences.getDouble("Dashboard/Dashboard Settings/Turret kI", ShooterConstants.TURRET_KI); + double newD = Preferences.getDouble("Dashboard/Dashboard Settings/Turret kD", ShooterConstants.TURRET_KD); + if (newP != _lastKP || newI != _lastKI || newD != _lastKD) + { + var slot0 = new Slot0Configs(); + slot0.kP = newP; + slot0.kI = newI; + slot0.kD = newD; + _turretMotor.getConfigurator().apply(slot0); + _lastKP = newP; + _lastKI = newI; + _lastKD = newD; + } + _robotTurretAngle = Degrees.of(_turretSensor.get()); SwerveDriveState state = _swerveStateSupplier.get(); @@ -155,6 +197,8 @@ public void periodic() _targetHorizontalOffset = Degrees.of(targetData.getHorizontalOffset()); } + if (_testModeActive) return; + Angle requestedSetpoint = switch (_turretState) { case Idle -> null; @@ -164,11 +208,12 @@ public void periodic() if (requestedSetpoint == null) { - _turretSetpoint = null; + _hasSetpoint = false; _turretMotor.setVoltage(0.0); return; } + _hasSetpoint = true; _turretSetpoint = MeasureUtil.clamp(requestedSetpoint, ShooterConstants.TURRET_MIN_ANGLE, ShooterConstants.TURRET_MAX_ANGLE); var target = _turretSetpoint.times(ShooterConstants.TURRET_GEAR_RATIO); _turretMotor.setControl(_positionRequest.withPosition(target.in(Rotations))); @@ -212,7 +257,7 @@ public boolean hasTarget() public boolean isLinedUp() { - if (_turretSetpoint == null) return _turretState == TurretState.Idle; + if (!_hasSetpoint) return _turretState == TurretState.Idle; if (_turretState == TurretState.Track && !_hasTarget) return false; return _robotTurretAngle.isNear(_turretSetpoint, ShooterConstants.TURRET_TOLERANCE); diff --git a/src/main/java/frc/robot/util/Utilities.java b/src/main/java/frc/robot/util/Utilities.java index 5fe8a74..4bd381d 100644 --- a/src/main/java/frc/robot/util/Utilities.java +++ b/src/main/java/frc/robot/util/Utilities.java @@ -65,6 +65,35 @@ public static boolean isHubActive() } + public static Time getHubStateTimeRemaining() + { + var timeRemaining = Seconds.of(DriverStation.getMatchTime()); + if (timeRemaining.lt(Seconds.zero())) + { + return Seconds.zero(); + } + + if (DriverStation.isAutonomous()) + { + return timeRemaining; + } + + if (!DriverStation.isTeleop()) + { + return Seconds.zero(); + } + + return switch (getAllianceShift(timeRemaining)) + { + case 0 -> timeRemaining.minus(kTransitionShiftEndTimeSecs); + case 1 -> timeRemaining.minus(kShift1EndTimeSecs); + case 2 -> timeRemaining.minus(kShift2EndTimeSecs); + case 3 -> timeRemaining.minus(kShift3EndTimeSecs); + case 4 -> timeRemaining.minus(kShift4EndTimeSecs); + default -> timeRemaining; + }; + } + /** * Check if we are on the Blue alliance. */