diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 7311e2b1f..a74806e22 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -53,6 +53,26 @@ public class SwerveModule * An {@link Alert} for if there is no Absolute Encoder on the module. */ private final Alert noEncoderWarning; + /** + * NT3 Raw Absolute Angle publisher for the absolute encoder. + */ + private final String rawAbsoluteAngleName; + /** + * NT3 Adjusted Absolute angle publisher for the absolute encoder. + */ + private final String adjAbsoluteAngleName; + /** + * NT3 Absolute encoder read issue. + */ + private final String absoluteEncoderIssueName; + /** + * NT3 raw angle motor. + */ + private final String rawAngleName; + /** + * NT3 Raw drive motor. + */ + private final String rawDriveName; /** * Module number for kinematics, usually 0 to 3. front left -> front right -> back left -> back right. */ @@ -176,6 +196,12 @@ public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfigurat "Pushing the Absolute Encoder offset to the encoder failed on module #" + moduleNumber, Alert.AlertType.WARNING); + + rawAbsoluteAngleName = "Module[" + configuration.name + "] Raw Absolute Encoder"; + adjAbsoluteAngleName = "Module[" + configuration.name + "] Adjusted Absolute Encoder"; + absoluteEncoderIssueName = "Module[" + configuration.name + "] Absolute Encoder Read Issue"; + rawAngleName = "Module[" + configuration.name + "] Raw Angle Encoder"; + rawDriveName = "Module[" + configuration.name + "] Raw Drive Encoder"; } /** @@ -293,7 +319,8 @@ private double getCosineCompensatedVelocity(SwerveModuleState desiredState) /* If error is close to 0 rotations, we're already there, so apply full power */ /* If the error is close to 0.25 rotations, then we're 90 degrees, so movement doesn't help us at all */ cosineScalar = Rotation2d.fromDegrees(desiredState.angle.getDegrees()) - .minus(Rotation2d.fromDegrees(getAbsolutePosition())).getCos(); // TODO: Investigate angle modulus by 180. + .minus(Rotation2d.fromDegrees(getAbsolutePosition())) + .getCos(); // TODO: Investigate angle modulus by 180. /* Make sure we don't invert our drive, even though we shouldn't ever target over 90 degrees anyway */ if (cosineScalar < 0.0) { @@ -526,13 +553,11 @@ public void updateTelemetry() { if (absoluteEncoder != null) { - SmartDashboard.putNumber("Module[" + configuration.name + "] Raw Absolute Encoder", - absoluteEncoder.getAbsolutePosition()); + SmartDashboard.putNumber(rawAbsoluteAngleName, absoluteEncoder.getAbsolutePosition()); } - SmartDashboard.putNumber("Module[" + configuration.name + "] Raw Angle Encoder", angleMotor.getPosition()); - SmartDashboard.putNumber("Module[" + configuration.name + "] Raw Drive Encoder", driveMotor.getPosition()); - SmartDashboard.putNumber("Module[" + configuration.name + "] Adjusted Absolute Encoder", getAbsolutePosition()); - SmartDashboard.putNumber("Module[" + configuration.name + "] Absolute Encoder Read Issue", - getAbsoluteEncoderReadIssue() ? 1 : 0); + SmartDashboard.putNumber(rawAngleName, angleMotor.getPosition()); + SmartDashboard.putNumber(rawDriveName, driveMotor.getPosition()); + SmartDashboard.putNumber(adjAbsoluteAngleName, getAbsolutePosition()); + SmartDashboard.putNumber(absoluteEncoderIssueName, getAbsoluteEncoderReadIssue() ? 1 : 0); } } diff --git a/src/main/java/swervelib/imu/Pigeon2Swerve.java b/src/main/java/swervelib/imu/Pigeon2Swerve.java index 8a3340295..bc89264c3 100644 --- a/src/main/java/swervelib/imu/Pigeon2Swerve.java +++ b/src/main/java/swervelib/imu/Pigeon2Swerve.java @@ -1,11 +1,9 @@ package swervelib.imu; -import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.configs.Pigeon2Configurator; import com.ctre.phoenix6.hardware.Pigeon2; -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.smartdashboard.SmartDashboard; @@ -20,7 +18,7 @@ public class Pigeon2Swerve extends SwerveIMU /** * Wait time for status frames to show up. */ - public static double STATUS_TIMEOUT_SECONDS = 0.04; + public static double STATUS_TIMEOUT_SECONDS = 0.04; /** * Pigeon2 IMU device. */ @@ -28,11 +26,11 @@ public class Pigeon2Swerve extends SwerveIMU /** * Offset for the Pigeon 2. */ - private Rotation3d offset = new Rotation3d(); + private Rotation3d offset = new Rotation3d(); /** * Inversion for the gyro */ - private boolean invertedIMU = false; + private boolean invertedIMU = false; /** * Generate the SwerveIMU for pigeon. diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java index b2d2eec7d..213f19941 100644 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -17,6 +17,10 @@ public class TalonFXSwerve extends SwerveMotor { + /** + * Wait time for status frames to show up. + */ + public static double STATUS_TIMEOUT_SECONDS = 0.02; /** * Factory default already occurred. */ @@ -33,10 +37,6 @@ public class TalonFXSwerve extends SwerveMotor * Velocity voltage setter for controlling drive motor. */ private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0); - /** - * Wait time for status frames to show up. - */ - public static double STATUS_TIMEOUT_SECONDS = 0.02; /** * TalonFX motor controller. */ @@ -44,11 +44,11 @@ public class TalonFXSwerve extends SwerveMotor /** * Conversion factor for the motor. */ - private double conversionFactor; + private double conversionFactor; /** * Current TalonFX configuration. */ - private TalonFXConfiguration configuration = new TalonFXConfiguration(); + private TalonFXConfiguration configuration = new TalonFXConfiguration(); /** diff --git a/src/main/java/swervelib/parser/json/DeviceJson.java b/src/main/java/swervelib/parser/json/DeviceJson.java index 2bdadfb25..5239c449e 100644 --- a/src/main/java/swervelib/parser/json/DeviceJson.java +++ b/src/main/java/swervelib/parser/json/DeviceJson.java @@ -1,5 +1,9 @@ package swervelib.parser.json; +import static swervelib.telemetry.SwerveDriveTelemetry.canIdWarning; +import static swervelib.telemetry.SwerveDriveTelemetry.i2cLockupWarning; +import static swervelib.telemetry.SwerveDriveTelemetry.serialCommsIssueWarning; + import com.revrobotics.SparkRelativeEncoder.Type; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.I2C; @@ -26,8 +30,6 @@ import swervelib.motors.SwerveMotor; import swervelib.motors.TalonFXSwerve; import swervelib.motors.TalonSRXSwerve; -import swervelib.telemetry.Alert; -import swervelib.telemetry.Alert.AlertType; /** * Device JSON parsed class. Used to access the JSON data. @@ -35,36 +37,18 @@ public class DeviceJson { - /** - * An {@link Alert} for if the CAN ID is greater than 40. - */ - private final 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 final Alert i2cLockupWarning = new Alert("IMU", - "I2C lockup issue detected on roboRIO. Check console for more information.", - Alert.AlertType.WARNING); - /** - * NavX serial comm issue. - */ - private final Alert serialCommsIssueWarning = new Alert("IMU", - "Serial comms is interrupted with USB and other serial traffic and causes intermittent connected/disconnection issues. Please consider another protocol or be mindful of this.", - AlertType.WARNING); /** * The device type, e.g. pigeon/pigeon2/sparkmax/talonfx/navx */ - public String type; + public String type; /** * The CAN ID or pin ID of the device. */ - public int id; + public int id; /** * The CAN bus name which the device resides on if using CAN. */ - public String canbus = ""; + public String canbus = ""; /** * Create a {@link SwerveAbsoluteEncoder} from the current configuration. diff --git a/src/main/java/swervelib/telemetry/Alert.java b/src/main/java/swervelib/telemetry/Alert.java index f3c6af33c..40f9af78a 100644 --- a/src/main/java/swervelib/telemetry/Alert.java +++ b/src/main/java/swervelib/telemetry/Alert.java @@ -34,11 +34,9 @@ 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. @@ -111,24 +109,7 @@ 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; - } + printAlert(text); } this.active = active; } @@ -142,28 +123,39 @@ 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; - } + printAlert(text); } this.text = text; } + + /** + * Print the alert message. + * + * @param text Text to print. + */ + private void printAlert(String 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; + } + } + /** * Represents an alert's level of urgency. */ @@ -219,14 +211,16 @@ private static class SendableAlerts implements Sendable */ 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); + List alertStrings = new ArrayList<>(); + for (Alert alert : alerts) + { + if (alert.type == type && alert.active) + { + alertStrings.add(alert.text); + } + } + // alertStrings.sort((a1, a2) -> (int) (a2.activeStartTime - a1.activeStartTime)); + return alertStrings.toArray(new String[alertStrings.size()]); } @Override diff --git a/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java b/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java index 7c1f54619..8a300d969 100644 --- a/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java +++ b/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java @@ -2,6 +2,7 @@ import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import swervelib.telemetry.Alert.AlertType; /** * Telemetry to describe the {@link swervelib.SwerveDrive} following frc-web-components. (Which follows AdvantageKit) @@ -9,69 +10,87 @@ public class SwerveDriveTelemetry { + /** + * An {@link Alert} for if the CAN ID is greater than 40. + */ + public static final 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. + */ + public static final Alert i2cLockupWarning = new Alert("IMU", + "I2C lockup issue detected on roboRIO. Check console for more information.", + Alert.AlertType.WARNING); + /** + * NavX serial comm issue. + */ + public static final Alert serialCommsIssueWarning = new Alert("IMU", + "Serial comms is interrupted with USB and other serial traffic and causes intermittent connected/disconnection issues. Please consider another protocol or be mindful of this.", + AlertType.WARNING); /** * The current telemetry verbosity level. */ - public static TelemetryVerbosity verbosity = TelemetryVerbosity.MACHINE; + public static TelemetryVerbosity verbosity = TelemetryVerbosity.MACHINE; /** * State of simulation of the Robot, used to optimize retrieval. */ - public static boolean isSimulation = RobotBase.isSimulation(); + public static boolean isSimulation = RobotBase.isSimulation(); /** * The number of swerve modules */ - public static int moduleCount; + public static int moduleCount; /** * The number of swerve modules */ - public static double[] wheelLocations; + public static double[] wheelLocations; /** * An array of rotation and velocity values describing the measured state of each swerve module */ - public static double[] measuredStates; + public static double[] measuredStates; /** * An array of rotation and velocity values describing the desired state of each swerve module */ - public static double[] desiredStates; + public static double[] desiredStates; /** * The robot's current rotation based on odometry or gyro readings */ - public static double robotRotation = 0; + public static double robotRotation = 0; /** * The maximum achievable speed of the modules, used to adjust the size of the vectors. */ - public static double maxSpeed; + public static double maxSpeed; /** * The units of the module rotations and robot rotation */ - public static String rotationUnit = "degrees"; + public static String rotationUnit = "degrees"; /** * The distance between the left and right modules. */ - public static double sizeLeftRight; + public static double sizeLeftRight; /** * The distance between the front and back modules. */ - public static double sizeFrontBack; + public static double sizeFrontBack; /** * The direction the robot should be facing when the "Robot Rotation" is zero or blank. This option is often useful to * align with odometry data or match videos. 'up', 'right', 'down' or 'left' */ - public static String forwardDirection = "up"; + public static String forwardDirection = "up"; /** * The maximum achievable angular velocity of the robot. This is used to visualize the angular velocity from the * chassis speeds properties. */ - public static double maxAngularVelocity; + public static double maxAngularVelocity; /** * The maximum achievable angular velocity of the robot. This is used to visualize the angular velocity from the * chassis speeds properties. */ - public static double[] measuredChassisSpeeds = new double[3]; + public static double[] measuredChassisSpeeds = new double[3]; /** * Describes the desired forward, sideways and angular velocity of the robot. */ - public static double[] desiredChassisSpeeds = new double[3]; + public static double[] desiredChassisSpeeds = new double[3]; /** * Upload data to smartdashboard