From dba98491e81060055fbdc998c3985d98534f56c3 Mon Sep 17 00:00:00 2001 From: gaming <48131223+TheGamer1002@users.noreply.github.com> Date: Tue, 16 Jan 2024 16:54:53 -0500 Subject: [PATCH] Implement NetworkAlerts --- .../drivebase/AbsoluteDriveAdv.java | 1 - src/main/java/swervelib/SwerveDrive.java | 1 - src/main/java/swervelib/SwerveModule.java | 21 +- .../encoders/AnalogAbsoluteEncoderSwerve.java | 17 +- .../swervelib/encoders/CANCoderSwerve.java | 49 +++-- .../encoders/PWMDutyCycleEncoderSwerve.java | 10 +- .../encoders/SparkMaxAnalogEncoderSwerve.java | 18 +- .../encoders/SparkMaxEncoderSwerve.java | 17 +- src/main/java/swervelib/imu/NavXSwerve.java | 15 +- src/main/java/swervelib/math/SwerveMath.java | 2 - .../swervelib/motors/SparkFlexSwerve.java | 17 +- .../motors/SparkMaxBrushedMotorSwerve.java | 24 ++- .../swervelib/parser/json/DeviceJson.java | 19 +- src/main/java/swervelib/telemetry/Alert.java | 195 ++++++++++++++++++ 14 files changed, 342 insertions(+), 64 deletions(-) create mode 100644 src/main/java/swervelib/telemetry/Alert.java diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java index 1c7254587..7a97907ce 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java @@ -7,7 +7,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.networktables.BooleanSubscriber; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 0915b08dd..b13f72029 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -11,7 +11,6 @@ import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 82b3ca928..5827c99ff 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -5,13 +5,13 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.math.SwerveMath; import swervelib.motors.SwerveMotor; import swervelib.parser.SwerveModuleConfiguration; import swervelib.simulation.SwerveModuleSimulation; +import swervelib.telemetry.Alert; import swervelib.telemetry.SwerveDriveTelemetry; import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; @@ -61,6 +61,14 @@ public class SwerveModule * Encoder synchronization queued. */ private boolean synchronizeEncoderQueued = false; + /** + * An {@link Alert} for if pushing the Absolute Encoder offset to the encoder fails. + */ + private Alert encoderOffsetWarning = new Alert("Motors", "Pushing the Absolute Encoder offset to the encoder failed on module #" + moduleNumber, Alert.AlertType.WARNING); + /** + * An {@link Alert} for if there is no Absolute Encoder on the module. + */ + private Alert noEncoderWarning = new Alert("Motors", "There is no Absolute Encoder on module #" + moduleNumber, Alert.AlertType.WARNING); /** * Construct the swerve module and initialize the swerve module motors and absolute encoder. @@ -397,14 +405,11 @@ public void pushOffsetsToControllers() if (absoluteEncoder.setAbsoluteEncoderOffset(angleOffset)) { angleOffset = 0; - } else - { - DriverStation.reportWarning( - "Pushing the Absolute Encoder offset to the encoder failed on module #" + moduleNumber, false); + } else { + encoderOffsetWarning.set(true); } - } else - { - DriverStation.reportWarning("There is no Absolute Encoder on module #" + moduleNumber, false); + } else { + noEncoderWarning.set(true); } } diff --git a/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java b/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java index 1b830b1b3..33cfd76d4 100644 --- a/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java @@ -1,8 +1,8 @@ package swervelib.encoders; import edu.wpi.first.wpilibj.AnalogInput; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; +import swervelib.telemetry.Alert; /** * Swerve Absolute Encoder for Thrifty Encoders and other analog encoders. @@ -19,6 +19,16 @@ public class AnalogAbsoluteEncoderSwerve extends SwerveAbsoluteEncoder * Inversion state of the encoder. */ private boolean inverted = false; + /** An {@link Alert} for if the absolute encoder offset cannot be set. */ + private Alert cannotSetOffset = new Alert( + "Encoders", + "Cannot Set Absolute Encoder Offset of Analog Encoders Channel #" + encoder.getChannel(), + Alert.AlertType.WARNING); + /** An {@link Alert} detailing how the analog absolute encoder may not report accurate velocities. */ + private Alert inaccurateVelocities = new Alert( + "Encoders", + "The Analog Absolute encoder may not report accurate velocities!", + Alert.AlertType.WARNING_TRACE); /** * Construct the Thrifty Encoder as a Swerve Absolute Encoder. @@ -101,8 +111,7 @@ public Object getAbsoluteEncoder() public boolean setAbsoluteEncoderOffset(double offset) { //Do Nothing - DriverStation.reportWarning( - "Cannot Set Absolute Encoder Offset of Analog Encoders Channel #" + encoder.getChannel(), false); + cannotSetOffset.set(true); return false; } @@ -114,7 +123,7 @@ public boolean setAbsoluteEncoderOffset(double offset) @Override public double getVelocity() { - DriverStation.reportWarning("The Analog Absolute encoder may not report accurate velocities!", true); + inaccurateVelocities.set(true); return encoder.getValue(); } } diff --git a/src/main/java/swervelib/encoders/CANCoderSwerve.java b/src/main/java/swervelib/encoders/CANCoderSwerve.java index 5d8c32835..2871ff950 100644 --- a/src/main/java/swervelib/encoders/CANCoderSwerve.java +++ b/src/main/java/swervelib/encoders/CANCoderSwerve.java @@ -9,7 +9,7 @@ import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; import com.ctre.phoenix6.signals.MagnetHealthValue; import com.ctre.phoenix6.signals.SensorDirectionValue; -import edu.wpi.first.wpilibj.DriverStation; +import swervelib.telemetry.Alert; /** * Swerve Absolute Encoder for CTRE CANCoders. @@ -21,6 +21,28 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder * CANCoder with WPILib sendable and support. */ public CANcoder encoder; + /** An {@link Alert} for if the CANCoder magnet field is less than ideal. */ + private Alert magnetFieldLessThanIdeal = new Alert( + "Encoders", + "CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.", + Alert.AlertType.WARNING); + /** An {@link Alert} for if the CANCoder reading is faulty. */ + private Alert readingFaulty = new Alert( + "Encoders", + "CANCoder " + encoder.getDeviceID() + " reading was faulty.", + Alert.AlertType.WARNING); + /** An {@link Alert} for if the CANCoder reading is faulty and the reading is ignored. */ + private Alert readingIgnored = new Alert( + "Encoders", + "CANCoder " + encoder.getDeviceID() + " reading was faulty, ignoring.", + Alert.AlertType.WARNING); + /** An {@link Alert} for if the absolute encoder offset cannot be set. */ + private Alert cannotSetOffset = new Alert( + "Encoders", + "Failure to set CANCoder " + + encoder.getDeviceID() + + " Absolute Encoder Offset", + Alert.AlertType.WARNING); /** * Initialize the CANCoder on the standard CANBus. @@ -91,15 +113,14 @@ public double getAbsolutePosition() if (strength != MagnetHealthValue.Magnet_Green) { - DriverStation.reportWarning( - "CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.\n", false); - } + magnetFieldLessThanIdeal.set(true); + } else magnetFieldLessThanIdeal.set(false); if (strength == MagnetHealthValue.Magnet_Invalid || strength == MagnetHealthValue.Magnet_Red) { readingError = true; - DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " reading was faulty.\n", false); + readingFaulty.set(true); return 0; - } + }else readingFaulty.set(false); StatusSignal angle = encoder.getAbsolutePosition().refresh(); // Taken from democat's library. @@ -115,8 +136,8 @@ public double getAbsolutePosition() if (angle.getStatus() != StatusCode.OK) { readingError = true; - DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " reading was faulty, ignoring.\n", false); - } + readingIgnored.set(true); + } else readingIgnored.set(false); return angle.getValue() * 360; } @@ -149,12 +170,16 @@ public boolean setAbsoluteEncoderOffset(double offset) return false; } error = cfg.apply(magCfg.withMagnetOffset(offset / 360)); - if (error == StatusCode.OK) - { + cannotSetOffset.setText( + "Failure to set CANCoder " + + encoder.getDeviceID() + + " Absolute Encoder Offset Error: " + + error); + if (error == StatusCode.OK) { + cannotSetOffset.set(false); return true; } - DriverStation.reportWarning( - "Failure to set CANCoder " + encoder.getDeviceID() + " Absolute Encoder Offset Error: " + error, false); + cannotSetOffset.set(true); return false; } diff --git a/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java b/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java index 0361aec81..4f83aa3ed 100644 --- a/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java @@ -1,7 +1,7 @@ package swervelib.encoders; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DutyCycleEncoder; +import swervelib.telemetry.Alert; /** * DutyCycle encoders such as "US Digital MA3 with PWM Output, the CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag @@ -22,6 +22,12 @@ public class PWMDutyCycleEncoderSwerve extends SwerveAbsoluteEncoder * Inversion state. */ private boolean isInverted; + /** An {@link Alert} for if the encoder cannot report accurate velocities. */ + private Alert inaccurateVelocities = new Alert( + "Encoders", + "The PWM Duty Cycle encoder may not report accurate velocities!", + Alert.AlertType.WARNING_TRACE); + /** * Constructor for the PWM duty cycle encoder. @@ -74,7 +80,7 @@ public Object getAbsoluteEncoder() @Override public double getVelocity() { - DriverStation.reportWarning("The PWM Duty Cycle encoder may not report accurate velocities!", true); + inaccurateVelocities.set(true); return encoder.get(); } diff --git a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java index 0508bca8b..b8b040044 100644 --- a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java @@ -4,9 +4,10 @@ import com.revrobotics.REVLibError; import com.revrobotics.SparkAnalogSensor; import com.revrobotics.SparkAnalogSensor.Mode; -import edu.wpi.first.wpilibj.DriverStation; + import java.util.function.Supplier; import swervelib.motors.SwerveMotor; +import swervelib.telemetry.Alert; /** * SparkMax absolute encoder, attached through the data port analog pin. @@ -18,6 +19,17 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder * The {@link SparkAnalogSensor} representing the duty cycle encoder attached to the SparkMax analog port. */ public SparkAnalogSensor encoder; + /** An {@link Alert} for if there is a failure configuring the encoder. */ + private Alert failureConfiguring = new Alert( + "Encoders", + "Failure configuring SparkMax Analog Encoder", + Alert.AlertType.WARNING_TRACE); + /** An {@link Alert} for if the absolute encoder does not support integrated offsets. */ + private Alert doesNotSupportIntegratedOffsets = new Alert( + "Encoders", + "SparkMax Analog Sensors do not support integrated offsets", + Alert.AlertType.WARNING_TRACE); + /** * Create the {@link SparkMaxAnalogEncoderSwerve} object as a analog sensor from the {@link CANSparkMax} motor data @@ -50,7 +62,7 @@ private void configureSparkMax(Supplier config) return; } } - DriverStation.reportWarning("Failure configuring encoder", true); + failureConfiguring.set(true); } /** @@ -113,7 +125,7 @@ public Object getAbsoluteEncoder() @Override public boolean setAbsoluteEncoderOffset(double offset) { - DriverStation.reportWarning("SparkMax Analog Sensor's do not support integrated offsets", true); + doesNotSupportIntegratedOffsets.set(true); return false; } diff --git a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java index f95d36d24..da773d9cc 100644 --- a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java @@ -4,9 +4,10 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.REVLibError; import com.revrobotics.SparkAbsoluteEncoder.Type; -import edu.wpi.first.wpilibj.DriverStation; + import java.util.function.Supplier; import swervelib.motors.SwerveMotor; +import swervelib.telemetry.Alert; /** * SparkMax absolute encoder, attached through the data port. @@ -18,6 +19,15 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder * The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkMax. */ public AbsoluteEncoder encoder; + /** An {@link Alert} for if there is a failure configuring the encoder. */ + private Alert failureConfiguring = new Alert( + "Encoders", + "Failure configuring SparkMax Analog Encoder", + Alert.AlertType.WARNING_TRACE); + private Alert offsetFailure = new Alert( + "Encoders", + "Failure to set Absolute Encoder Offset", + Alert.AlertType.WARNING_TRACE); /** * Create the {@link SparkMaxEncoderSwerve} object as a duty cycle from the {@link CANSparkMax} motor. @@ -52,7 +62,7 @@ private void configureSparkMax(Supplier config) return; } } - DriverStation.reportWarning("Failure configuring encoder", true); + failureConfiguring.set(true); } /** @@ -124,7 +134,8 @@ public boolean setAbsoluteEncoderOffset(double offset) return true; } } - DriverStation.reportWarning("Failure to set Absolute Encoder Offset Error: " + error, false); + offsetFailure.setText("Failure to set Absolute Encoder Offset Error: " + error); + offsetFailure.set(true); return false; } diff --git a/src/main/java/swervelib/imu/NavXSwerve.java b/src/main/java/swervelib/imu/NavXSwerve.java index 272efafa4..621fd2d2a 100644 --- a/src/main/java/swervelib/imu/NavXSwerve.java +++ b/src/main/java/swervelib/imu/NavXSwerve.java @@ -1,14 +1,14 @@ package swervelib.imu; import com.kauailabs.navx.frc.AHRS; -import edu.wpi.first.math.geometry.Quaternion; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.I2C; import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.SerialPort; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import swervelib.telemetry.Alert; + import java.util.Optional; /** @@ -25,6 +25,8 @@ public class NavXSwerve extends SwerveIMU * Offset for the NavX. */ private Rotation3d offset = new Rotation3d(); + /** An {@link Alert} for if there is an error instantiating the NavX. */ + private Alert navXError = new Alert("IMU", "Error instantiating NavX.", Alert.AlertType.ERROR_TRACE); /** * Constructor for the NavX swerve. @@ -43,7 +45,8 @@ public NavXSwerve(SerialPort.Port port) SmartDashboard.putData(gyro); } catch (RuntimeException ex) { - DriverStation.reportError("Error instantiating navX: " + ex.getMessage(), true); + navXError.setText("Error instantiating NavX: " + ex.getMessage()); + navXError.set(true); } } @@ -64,7 +67,8 @@ public NavXSwerve(SPI.Port port) SmartDashboard.putData(gyro); } catch (RuntimeException ex) { - DriverStation.reportError("Error instantiating navX: " + ex.getMessage(), true); + navXError.setText("Error instantiating NavX: " + ex.getMessage()); + navXError.set(true); } } @@ -85,7 +89,8 @@ public NavXSwerve(I2C.Port port) SmartDashboard.putData(gyro); } catch (RuntimeException ex) { - DriverStation.reportError("Error instantiating navX: " + ex.getMessage(), true); + navXError.setText("Error instantiating NavX: " + ex.getMessage()); + navXError.set(true); } } diff --git a/src/main/java/swervelib/math/SwerveMath.java b/src/main/java/swervelib/math/SwerveMath.java index 0b4fdd5e4..5de06d599 100644 --- a/src/main/java/swervelib/math/SwerveMath.java +++ b/src/main/java/swervelib/math/SwerveMath.java @@ -13,8 +13,6 @@ import swervelib.SwerveModule; import swervelib.parser.SwerveDriveConfiguration; import swervelib.parser.SwerveModuleConfiguration; -import swervelib.telemetry.SwerveDriveTelemetry; -import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; /** * Mathematical functions which pertain to swerve drive. diff --git a/src/main/java/swervelib/motors/SparkFlexSwerve.java b/src/main/java/swervelib/motors/SparkFlexSwerve.java index a5aef9685..a8b7e159a 100644 --- a/src/main/java/swervelib/motors/SparkFlexSwerve.java +++ b/src/main/java/swervelib/motors/SparkFlexSwerve.java @@ -12,10 +12,11 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkAnalogSensor; import com.revrobotics.SparkPIDController; -import edu.wpi.first.wpilibj.DriverStation; + import java.util.function.Supplier; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; +import swervelib.telemetry.Alert; import swervelib.telemetry.SwerveDriveTelemetry; /** @@ -44,6 +45,13 @@ public class SparkFlexSwerve extends SwerveMotor * Factory default already occurred. */ private boolean factoryDefaultOccurred = false; + /** An {@link Alert} for if there is an error configuring the motor. */ + private Alert failureConfiguring = new Alert("Motors","Failure configuring motor " + motor.getDeviceId(), Alert.AlertType.WARNING_TRACE); + /** An {@link Alert} for if the absolute encoder's offset is set in the json instead of the hardware client. */ + private Alert absoluteEncoderOffsetWarning = new Alert("Motors", + "IF possible configure the duty cycle encoder offset in the REV Hardware Client instead of using the " + + "absoluteEncoderOffset in the Swerve Module JSON!", + Alert.AlertType.WARNING); /** * Initialize the swerve motor. @@ -92,7 +100,7 @@ private void configureSparkFlex(Supplier config) return; } } - DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true); + failureConfiguring.set(true); } /** @@ -185,10 +193,7 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) { if (encoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) { - DriverStation.reportWarning( - "IF possible configure the duty cycle encoder offset in the REV Hardware Client instead of using the" + - " absoluteEncoderOffset in the Swerve Module JSON!", - false); + absoluteEncoderOffsetWarning.set(true); absoluteEncoder = encoder; configureSparkFlex(() -> pid.setFeedbackDevice((MotorFeedbackSensor) absoluteEncoder.getAbsoluteEncoder())); } diff --git a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java index fc7d94edc..53604739c 100644 --- a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -1,20 +1,16 @@ package swervelib.motors; -import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.CANSparkMax; +import com.revrobotics.*; import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkLowLevel.PeriodicFrame; -import com.revrobotics.REVLibError; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkMaxAlternateEncoder; -import com.revrobotics.SparkPIDController; import com.revrobotics.SparkRelativeEncoder.Type; -import edu.wpi.first.wpilibj.DriverStation; -import java.util.function.Supplier; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; +import swervelib.telemetry.Alert; + +import java.util.function.Supplier; /** * Brushed motor control with SparkMax. @@ -43,6 +39,12 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor * Factory default already occurred. */ private boolean factoryDefaultOccurred = false; + /** An {@link Alert} for if the motor has no encoder. */ + private Alert noEncoderAlert = new Alert("Motors", "Cannot use motor without encoder.", Alert.AlertType.ERROR_TRACE); + /** An {@link Alert} for if there is an error configuring the motor. */ + private Alert failureConfiguringAlert = new Alert("Motors","Failure configuring motor " + motor.getDeviceId(), Alert.AlertType.WARNING_TRACE); + /** An {@link Alert} for if the motor has no encoder defined. */ + private Alert noEncoderDefinedAlert = new Alert("Motors","An encoder MUST be defined to work with a SparkMAX", Alert.AlertType.ERROR_TRACE); /** * Initialize the swerve motor. @@ -59,7 +61,7 @@ public SparkMaxBrushedMotorSwerve(CANSparkMax motor, boolean isDriveMotor, Type // Drive motors **MUST** have an encoder attached. if (isDriveMotor && encoderType == Type.kNoSensor) { - DriverStation.reportError("Cannot use motor without encoder.", true); + noEncoderAlert.set(true); throw new RuntimeException("Cannot use SparkMAX as a drive motor without an encoder attached."); } @@ -122,7 +124,7 @@ private void configureSparkMax(Supplier config) return; } } - DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true); + failureConfiguringAlert.set(true); } /** @@ -220,7 +222,7 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) } if (absoluteEncoder == null && this.encoder == null) { - DriverStation.reportError("An encoder MUST be defined to work with a SparkMAX", true); + noEncoderDefinedAlert.set(true); throw new RuntimeException("An encoder MUST be defined to work with a SparkMAX"); } return this; diff --git a/src/main/java/swervelib/parser/json/DeviceJson.java b/src/main/java/swervelib/parser/json/DeviceJson.java index 9b3a8aacf..6ee795a53 100644 --- a/src/main/java/swervelib/parser/json/DeviceJson.java +++ b/src/main/java/swervelib/parser/json/DeviceJson.java @@ -26,6 +26,7 @@ import swervelib.motors.SwerveMotor; import swervelib.motors.TalonFXSwerve; import swervelib.motors.TalonSRXSwerve; +import swervelib.telemetry.Alert; /** * Device JSON parsed class. Used to access the JSON data. @@ -45,6 +46,14 @@ public class DeviceJson * The CAN bus name which the device resides on if using CAN. */ public String canbus = ""; + /** + * An {@link Alert} for if the CAN ID is greater than 40. + */ + private Alert canIdWarning = new Alert("JSON", "CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!", Alert.AlertType.WARNING); + /** + * An {@link Alert} for if there is an I2C lockup issue on the roboRIO. + */ + private Alert i2cLockupWarning = new Alert("IMU", "I2C lockup issue detected on roboRIO. Check console for more information.", Alert.AlertType.WARNING); /** * Create a {@link SwerveAbsoluteEncoder} from the current configuration. @@ -57,8 +66,7 @@ public SwerveAbsoluteEncoder createEncoder(SwerveMotor motor) { if (id > 40) { - DriverStation.reportWarning("CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!", - false); + canIdWarning.set(true); } switch (type) { @@ -99,8 +107,7 @@ public SwerveIMU createIMU() { if (id > 40) { - DriverStation.reportWarning("CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!", - false); + canIdWarning.set(true); } switch (type) { @@ -121,6 +128,7 @@ public SwerveIMU createIMU() "\nhttps://docs.wpilib.org/en/stable/docs/yearly-overview/known-issues" + ".html#onboard-i2c-causing-system-lockups", false); + i2cLockupWarning.set(true); return new NavXSwerve(I2C.Port.kMXP); case "navx_usb": return new NavXSwerve(Port.kUSB); @@ -145,8 +153,7 @@ public SwerveMotor createMotor(boolean isDriveMotor) { if (id > 40) { - DriverStation.reportWarning("CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!", - false); + canIdWarning.set(true); } switch (type) { diff --git a/src/main/java/swervelib/telemetry/Alert.java b/src/main/java/swervelib/telemetry/Alert.java new file mode 100644 index 000000000..56f0c5da8 --- /dev/null +++ b/src/main/java/swervelib/telemetry/Alert.java @@ -0,0 +1,195 @@ +// Copyright (c) 2023 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found below. + +// MIT License +// +// Copyright (c) 2023 FRC 6328 +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +package swervelib.telemetry; + +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.util.ArrayList; +import java.util.Comparator; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import java.util.function.Predicate; + +/** Class for managing persistent alerts to be sent over NetworkTables. */ +public class Alert { + private static Map groups = new HashMap(); + + private final AlertType type; + private boolean active = false; + private double activeStartTime = 0.0; + private String text; + + /** + * Creates a new Alert in the default group - "Alerts". If this is the first to be instantiated, + * the appropriate entries will be added to NetworkTables. + * + * @param text Text to be displayed when the alert is active. + * @param type Alert level specifying urgency. + */ + public Alert(String text, AlertType type) { + this("Alerts", text, type); + } + + /** + * Creates a new Alert. If this is the first to be instantiated in its group, the appropriate + * entries will be added to NetworkTables. + * + * @param group Group identifier, also used as NetworkTables title + * @param text Text to be displayed when the alert is active. + * @param type Alert level specifying urgency. + */ + public Alert(String group, String text, AlertType type) { + if (!groups.containsKey(group)) { + groups.put(group, new SendableAlerts()); + SmartDashboard.putData(group, groups.get(group)); + } + + this.text = text; + this.type = type; + groups.get(group).alerts.add(this); + } + + /** + * Sets whether the alert should currently be displayed. When activated, the alert text will also + * be sent to the console. + */ + public void set(boolean active) { + if (active && !this.active) { + activeStartTime = Timer.getFPGATimestamp(); + switch (type) { + case ERROR: + DriverStation.reportError(text, false); + break; + case ERROR_TRACE: + DriverStation.reportError(text, true); + break; + case WARNING: + DriverStation.reportWarning(text, false); + break; + case WARNING_TRACE: + DriverStation.reportWarning(text, true); + break; + case INFO: + System.out.println(text); + break; + } + } + this.active = active; + } + + /** Updates current alert text. */ + public void setText(String text) { + if (active && !text.equals(this.text)) { + switch (type) { + case ERROR: + DriverStation.reportError(text, false); + break; + case ERROR_TRACE: + DriverStation.reportError(text, true); + break; + case WARNING: + DriverStation.reportWarning(text, false); + break; + case WARNING_TRACE: + DriverStation.reportWarning(text, true); + break; + case INFO: + System.out.println(text); + break; + } + } + this.text = text; + } + + private static class SendableAlerts implements Sendable { + public final List alerts = new ArrayList<>(); + + public String[] getStrings(AlertType type) { + Predicate activeFilter = (Alert x) -> x.type == type && x.active; + Comparator timeSorter = + (Alert a1, Alert a2) -> (int) (a2.activeStartTime - a1.activeStartTime); + return alerts.stream() + .filter(activeFilter) + .sorted(timeSorter) + .map((Alert a) -> a.text) + .toArray(String[]::new); + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Alerts"); + builder.addStringArrayProperty("errors", () -> getStrings(AlertType.ERROR), null); + builder.addStringArrayProperty("errors", () -> getStrings(AlertType.ERROR_TRACE), null); + builder.addStringArrayProperty("warnings", () -> getStrings(AlertType.WARNING), null); + builder.addStringArrayProperty("warnings", () -> getStrings(AlertType.WARNING_TRACE), null); + builder.addStringArrayProperty("infos", () -> getStrings(AlertType.INFO), null); + } + } + + /** Represents an alert's level of urgency. */ + public static enum AlertType { + /** + * High priority alert - displayed first on the dashboard with a red "X" symbol. Use this type + * for problems which will seriously affect the robot's functionality and thus require immediate + * attention. + */ + ERROR, + /** + * High priority alert - displayed first on the dashboard with a red "X" symbol. Use this type + * for problems which will seriously affect the robot's functionality and thus require immediate + * attention. + * Trace printed to driver station console. + */ + ERROR_TRACE, + + /** + * Medium priority alert - displayed second on the dashboard with a yellow "!" symbol. Use this + * type for problems which could affect the robot's functionality but do not necessarily require + * immediate attention. + */ + WARNING, + /** + * Medium priority alert - displayed second on the dashboard with a yellow "!" symbol. Use this + * type for problems which could affect the robot's functionality but do not necessarily require + * immediate attention. + * Trace printed to driver station console. + */ + WARNING_TRACE, + /** + * Low priority alert - displayed last on the dashboard with a green "i" symbol. Use this type + * for problems which are unlikely to affect the robot's functionality, or any other alerts + * which do not fall under "ERROR" or "WARNING". + */ + INFO + } +} \ No newline at end of file