diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9bd888d..3f63495 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,47 +4,44 @@ package frc.robot; -import com.pathplanner.lib.auto.AutoBuilder; - -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Timer; 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.InstantCommand; import edu.wpi.first.wpilibj2.command.PrintCommand; -import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.constants.AutoConstants; import frc.robot.constants.OIConstants; import frc.robot.subsystems.Drivetrain; +import frc.robot.utilities.Elastic; import frc.robot.utilities.Utilities; public class RobotContainer { - - Boolean isRed; - private Drivetrain drivetrain; - private Utilities utilities; - private CommandXboxController driver; private SendableChooser autoChooser; + private Timer shiftTimer; + public RobotContainer() { drivetrain = new Drivetrain(); driver = new CommandXboxController(OIConstants.kDriverControllerPort); + shiftTimer = new Timer(); + shiftTimer.reset(); + configureBindings(); } - - - - private void configureBindings() { drivetrain.setDefaultCommand( drivetrain.drive( @@ -55,15 +52,9 @@ public class RobotContainer { ) ); - ShuffleboardTab tab = Shuffleboard.getTab("SideFirst"); - - if (Utilities.ShiftFirst() == Alliance.Red) { - tab.add("IsRed", isRed); -} - + configureShiftDisplay(); } - public Command getAutonomousCommand() { if(AutoConstants.kAutoConfigOk) { return autoChooser.getSelected(); @@ -71,4 +62,55 @@ public class RobotContainer { return new PrintCommand("Robot Config loading failed, autonomous disabled"); } } + + private void configureShiftDisplay() { + SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay); + + RobotModeTriggers.autonomous().onTrue(new InstantCommand(() -> { + shiftTimer.stop(); + SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay); + })); + + RobotModeTriggers.teleop().onTrue(new InstantCommand(() -> { + Elastic.selectTab(OIConstants.kTeleopTab); + shiftTimer.reset(); + shiftTimer.start(); + })); + + new Trigger(() -> shiftTimer.get() <= 10).onTrue(new InstantCommand(() -> { + SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay); + })); + + new Trigger(() -> shiftTimer.get() > 10 && shiftTimer.get() <= 35).onTrue(new InstantCommand(() -> { + SmartDashboard.putStringArray( + OIConstants.kCurrentActiveHub, + Utilities.ShiftFirst() == Alliance.Red ? OIConstants.kRedDisplay : OIConstants.kBlueDisplay + ); + })); + + new Trigger(() -> shiftTimer.get() > 35 && shiftTimer.get() <= 60).onTrue(new InstantCommand(() -> { + SmartDashboard.putStringArray( + OIConstants.kCurrentActiveHub, + Utilities.ShiftFirst() == Alliance.Red ? OIConstants.kBlueDisplay : OIConstants.kRedDisplay + ); + })); + + new Trigger(() -> shiftTimer.get() > 60 && shiftTimer.get() <= 85).onTrue(new InstantCommand(() -> { + SmartDashboard.putStringArray( + OIConstants.kCurrentActiveHub, + Utilities.ShiftFirst() == Alliance.Red ? OIConstants.kRedDisplay : OIConstants.kBlueDisplay + ); + })); + + new Trigger(() -> shiftTimer.get() > 85 && shiftTimer.get() <= 110).onTrue(new InstantCommand(() -> { + SmartDashboard.putStringArray( + OIConstants.kCurrentActiveHub, + Utilities.ShiftFirst() == Alliance.Red ? OIConstants.kBlueDisplay : OIConstants.kRedDisplay + ); + })); + + new Trigger(() -> shiftTimer.get() > 110).onTrue(new InstantCommand(() -> { + SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay); + })); + } } diff --git a/src/main/java/frc/robot/constants/OIConstants.java b/src/main/java/frc/robot/constants/OIConstants.java index cb8392f..19b5612 100644 --- a/src/main/java/frc/robot/constants/OIConstants.java +++ b/src/main/java/frc/robot/constants/OIConstants.java @@ -1,5 +1,7 @@ package frc.robot.constants; +import edu.wpi.first.wpilibj.util.Color; + public class OIConstants { public static final int kDriverControllerPort = 0; public static final int kOperatorControllerPort = 1; @@ -7,4 +9,21 @@ public class OIConstants { public static final double kDriveDeadband = .01; public static final double kJoystickExponential = 3; + + public static final String kTeleopTab = "Teleoperated"; + + public static final String kCurrentActiveHub = "Alliance Hub Currently Active"; + + public static final String[] kRedBlueDisplay = { + Color.kRed.toHexString(), + Color.kBlue.toHexString() + }; + + public static final String[] kRedDisplay = { + Color.kRed.toHexString() + }; + + public static final String[] kBlueDisplay = { + Color.kBlue.toHexString() + }; } diff --git a/src/main/java/frc/robot/utilities/Elastic.java b/src/main/java/frc/robot/utilities/Elastic.java new file mode 100644 index 0000000..7c16896 --- /dev/null +++ b/src/main/java/frc/robot/utilities/Elastic.java @@ -0,0 +1,390 @@ +// Copyright (c) 2023-2026 Gold87 and other Elastic contributors +// This software can be modified and/or shared under the terms +// defined by the Elastic license: +// https://github.com/Gold872/elastic_dashboard/blob/main/LICENSE + +package frc.robot.utilities; + +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.PubSubOption; +import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.networktables.StringTopic; + +public final class Elastic { + private static final StringTopic notificationTopic = + NetworkTableInstance.getDefault().getStringTopic("/Elastic/RobotNotifications"); + private static final StringPublisher notificationPublisher = + notificationTopic.publish(PubSubOption.sendAll(true), PubSubOption.keepDuplicates(true)); + private static final StringTopic selectedTabTopic = + NetworkTableInstance.getDefault().getStringTopic("/Elastic/SelectedTab"); + private static final StringPublisher selectedTabPublisher = + selectedTabTopic.publish(PubSubOption.keepDuplicates(true)); + private static final ObjectMapper objectMapper = new ObjectMapper(); + + /** + * Represents the possible levels of notifications for the Elastic dashboard. These levels are + * used to indicate the severity or type of notification. + */ + public enum NotificationLevel { + /** Informational Message */ + INFO, + /** Warning message */ + WARNING, + /** Error message */ + ERROR + } + + /** + * Sends an notification to the Elastic dashboard. The notification is serialized as a JSON string + * before being published. + * + * @param notification the {@link Notification} object containing notification details + */ + public static void sendNotification(Notification notification) { + try { + notificationPublisher.set(objectMapper.writeValueAsString(notification)); + } catch (JsonProcessingException e) { + e.printStackTrace(); + } + } + + /** + * Selects the tab of the dashboard with the given name. If no tab matches the name, this will + * have no effect on the widgets or tabs in view. + * + *

If the given name is a number, Elastic will select the tab whose index equals the number + * provided. + * + * @param tabName the name of the tab to select + */ + public static void selectTab(String tabName) { + selectedTabPublisher.set(tabName); + } + + /** + * Selects the tab of the dashboard at the given index. If this index is greater than or equal to + * the number of tabs, this will have no effect. + * + * @param tabIndex the index of the tab to select. + */ + public static void selectTab(int tabIndex) { + selectTab(Integer.toString(tabIndex)); + } + + /** + * Represents an notification object to be sent to the Elastic dashboard. This object holds + * properties such as level, title, description, display time, and dimensions to control how the + * notification is displayed on the dashboard. + */ + public static class Notification { + @JsonProperty("level") + private NotificationLevel level; + + @JsonProperty("title") + private String title; + + @JsonProperty("description") + private String description; + + @JsonProperty("displayTime") + private int displayTimeMillis; + + @JsonProperty("width") + private double width; + + @JsonProperty("height") + private double height; + + /** + * Creates a new Notification with all default parameters. This constructor is intended to be + * used with the chainable decorator methods + * + *

Title and description fields are empty. + */ + public Notification() { + this(NotificationLevel.INFO, "", ""); + } + + /** + * Creates a new Notification with all properties specified. + * + * @param level the level of the notification (e.g., INFO, WARNING, ERROR) + * @param title the title text of the notification + * @param description the descriptive text of the notification + * @param displayTimeMillis the time in milliseconds for which the notification is displayed + * @param width the width of the notification display area + * @param height the height of the notification display area, inferred if below zero + */ + public Notification( + NotificationLevel level, + String title, + String description, + int displayTimeMillis, + double width, + double height) { + this.level = level; + this.title = title; + this.displayTimeMillis = displayTimeMillis; + this.description = description; + this.height = height; + this.width = width; + } + + /** + * Creates a new Notification with default display time and dimensions. + * + * @param level the level of the notification + * @param title the title text of the notification + * @param description the descriptive text of the notification + */ + public Notification(NotificationLevel level, String title, String description) { + this(level, title, description, 3000, 350, -1); + } + + /** + * Creates a new Notification with a specified display time and default dimensions. + * + * @param level the level of the notification + * @param title the title text of the notification + * @param description the descriptive text of the notification + * @param displayTimeMillis the display time in milliseconds + */ + public Notification( + NotificationLevel level, String title, String description, int displayTimeMillis) { + this(level, title, description, displayTimeMillis, 350, -1); + } + + /** + * Creates a new Notification with specified dimensions and default display time. If the height + * is below zero, it is automatically inferred based on screen size. + * + * @param level the level of the notification + * @param title the title text of the notification + * @param description the descriptive text of the notification + * @param width the width of the notification display area + * @param height the height of the notification display area, inferred if below zero + */ + public Notification( + NotificationLevel level, String title, String description, double width, double height) { + this(level, title, description, 3000, width, height); + } + + /** + * Updates the level of this notification + * + * @param level the level to set the notification to + */ + public void setLevel(NotificationLevel level) { + this.level = level; + } + + /** + * @return the level of this notification + */ + public NotificationLevel getLevel() { + return level; + } + + /** + * Updates the title of this notification + * + * @param title the title to set the notification to + */ + public void setTitle(String title) { + this.title = title; + } + + /** + * Gets the title of this notification + * + * @return the title of this notification + */ + public String getTitle() { + return title; + } + + /** + * Updates the description of this notification + * + * @param description the description to set the notification to + */ + public void setDescription(String description) { + this.description = description; + } + + public String getDescription() { + return description; + } + + /** + * Updates the display time of the notification + * + * @param seconds the number of seconds to display the notification for + */ + public void setDisplayTimeSeconds(double seconds) { + setDisplayTimeMillis((int) Math.round(seconds * 1000)); + } + + /** + * Updates the display time of the notification in milliseconds + * + * @param displayTimeMillis the number of milliseconds to display the notification for + */ + public void setDisplayTimeMillis(int displayTimeMillis) { + this.displayTimeMillis = displayTimeMillis; + } + + /** + * Gets the display time of the notification in milliseconds + * + * @return the number of milliseconds the notification is displayed for + */ + public int getDisplayTimeMillis() { + return displayTimeMillis; + } + + /** + * Updates the width of the notification + * + * @param width the width to set the notification to + */ + public void setWidth(double width) { + this.width = width; + } + + /** + * Gets the width of the notification + * + * @return the width of the notification + */ + public double getWidth() { + return width; + } + + /** + * Updates the height of the notification + * + *

If the height is set to -1, the height will be determined automatically by the dashboard + * + * @param height the height to set the notification to + */ + public void setHeight(double height) { + this.height = height; + } + + /** + * Gets the height of the notification + * + * @return the height of the notification + */ + public double getHeight() { + return height; + } + + /** + * Modifies the notification's level and returns itself to allow for method chaining + * + * @param level the level to set the notification to + * @return the current notification + */ + public Notification withLevel(NotificationLevel level) { + this.level = level; + return this; + } + + /** + * Modifies the notification's title and returns itself to allow for method chaining + * + * @param title the title to set the notification to + * @return the current notification + */ + public Notification withTitle(String title) { + setTitle(title); + return this; + } + + /** + * Modifies the notification's description and returns itself to allow for method chaining + * + * @param description the description to set the notification to + * @return the current notification + */ + public Notification withDescription(String description) { + setDescription(description); + return this; + } + + /** + * Modifies the notification's display time and returns itself to allow for method chaining + * + * @param seconds the number of seconds to display the notification for + * @return the current notification + */ + public Notification withDisplaySeconds(double seconds) { + return withDisplayMilliseconds((int) Math.round(seconds * 1000)); + } + + /** + * Modifies the notification's display time and returns itself to allow for method chaining + * + * @param displayTimeMillis the number of milliseconds to display the notification for + * @return the current notification + */ + public Notification withDisplayMilliseconds(int displayTimeMillis) { + setDisplayTimeMillis(displayTimeMillis); + return this; + } + + /** + * Modifies the notification's width and returns itself to allow for method chaining + * + * @param width the width to set the notification to + * @return the current notification + */ + public Notification withWidth(double width) { + setWidth(width); + return this; + } + + /** + * Modifies the notification's height and returns itself to allow for method chaining + * + * @param height the height to set the notification to + * @return the current notification + */ + public Notification withHeight(double height) { + setHeight(height); + return this; + } + + /** + * Modifies the notification's height and returns itself to allow for method chaining + * + *

This will set the height to -1 to have it automatically determined by the dashboard + * + * @return the current notification + */ + public Notification withAutomaticHeight() { + setHeight(-1); + return this; + } + + /** + * Modifies the notification to disable the auto dismiss behavior + * + *

This sets the display time to 0 milliseconds + * + *

The auto dismiss behavior can be re-enabled by setting the display time to a number + * greater than 0 + * + * @return the current notification + */ + public Notification withNoAutoDismiss() { + setDisplayTimeMillis(0); + return this; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/utilities/PhotonVision.java b/src/main/java/frc/robot/utilities/PhotonVision.java index 46e4ae4..7e6d048 100644 --- a/src/main/java/frc/robot/utilities/PhotonVision.java +++ b/src/main/java/frc/robot/utilities/PhotonVision.java @@ -45,6 +45,8 @@ public class PhotonVision implements IAprilTagProvider,IVisualPoseProvider { this.latestResult = null; } + // TODO This periodic method has to be called from somewhere, even though the cameras + // could be used in multiple other subsystems public void periodic() { // TODO Do we care about missed results? Probably not, if we're taking long enough to miss results something else is wrong List results = camera.getAllUnreadResults(); diff --git a/src/main/java/frc/robot/utilities/Utilities.java b/src/main/java/frc/robot/utilities/Utilities.java index 58e6b25..9ec0a8e 100644 --- a/src/main/java/frc/robot/utilities/Utilities.java +++ b/src/main/java/frc/robot/utilities/Utilities.java @@ -1,7 +1,5 @@ package frc.robot.utilities; -import com.pathplanner.lib.auto.AutoBuilder; - import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance;