diff --git a/swervelib/SwerveDrive.java b/swervelib/SwerveDrive.java index 0915b08..aeef17b 100644 --- a/swervelib/SwerveDrive.java +++ b/swervelib/SwerveDrive.java @@ -11,7 +11,6 @@ import edu.wpi.first.math.geometry.Rotation3d; 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; @@ -67,6 +66,10 @@ public class SwerveDrive * Odometry lock to ensure thread safety. */ private final Lock odometryLock = new ReentrantLock(); + /** + * Deadband for speeds in heading correction. + */ + private final double HEADING_CORRECTION_DEADBAND = 0.01; /** * Field object. */ @@ -100,14 +103,14 @@ public class SwerveDrive * correction. */ public boolean chassisVelocityCorrection = true; - /** - * Whether heading correction PID is currently active. - */ - private boolean correctionEnabled = false; /** * Whether to correct heading when driving translationally. Set to true to enable. */ public boolean headingCorrection = false; + /** + * Whether heading correction PID is currently active. + */ + private boolean correctionEnabled = false; /** * Swerve IMU device for sensing the heading of the robot. */ @@ -120,10 +123,6 @@ public class SwerveDrive * Counter to synchronize the modules relative encoder with absolute encoder when not moving. */ private int moduleSynchronizationCounter = 0; - /** - * Deadband for speeds in heading correction. - */ - private final double HEADING_CORRECTION_DEADBAND = 0.01; /** * The last heading set in radians. */ @@ -413,10 +412,10 @@ public class SwerveDrive if (headingCorrection) { if (Math.abs(velocity.omegaRadiansPerSecond) < HEADING_CORRECTION_DEADBAND - && (Math.abs(velocity.vxMetersPerSecond) > HEADING_CORRECTION_DEADBAND - || Math.abs(velocity.vyMetersPerSecond) > HEADING_CORRECTION_DEADBAND)) + && (Math.abs(velocity.vxMetersPerSecond) > HEADING_CORRECTION_DEADBAND + || Math.abs(velocity.vyMetersPerSecond) > HEADING_CORRECTION_DEADBAND)) { - if (!correctionEnabled) + if (!correctionEnabled) { lastHeadingRadians = getYaw().getRadians(); correctionEnabled = true; diff --git a/swervelib/SwerveModule.java b/swervelib/SwerveModule.java index 82b3ca9..fe3989a 100644 --- a/swervelib/SwerveModule.java +++ b/swervelib/SwerveModule.java @@ -5,13 +5,13 @@ import edu.wpi.first.math.geometry.Rotation2d; 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; @@ -33,6 +33,14 @@ public class SwerveModule * Absolute encoder for swerve drive. */ private final SwerveAbsoluteEncoder absoluteEncoder; + /** + * An {@link Alert} for if pushing the Absolute Encoder offset to the encoder fails. + */ + private final Alert encoderOffsetWarning; + /** + * An {@link Alert} for if there is no Absolute Encoder on the module. + */ + private final Alert noEncoderWarning; /** * Module number for kinematics, usually 0 to 3. front left -> front right -> back left -> back right. */ @@ -129,6 +137,15 @@ public class SwerveModule } lastState = getState(); + + noEncoderWarning = new Alert("Motors", + "There is no Absolute Encoder on module #" + + moduleNumber, + Alert.AlertType.WARNING); + encoderOffsetWarning = new Alert("Motors", + "Pushing the Absolute Encoder offset to the encoder failed on module #" + + moduleNumber, + Alert.AlertType.WARNING); } /** @@ -399,12 +416,11 @@ public class SwerveModule angleOffset = 0; } else { - DriverStation.reportWarning( - "Pushing the Absolute Encoder offset to the encoder failed on module #" + moduleNumber, false); + encoderOffsetWarning.set(true); } } else { - DriverStation.reportWarning("There is no Absolute Encoder on module #" + moduleNumber, false); + noEncoderWarning.set(true); } } diff --git a/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java b/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java index 1b830b1..bcf4df9 100644 --- a/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java +++ b/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,14 @@ 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; + /** + * An {@link Alert} detailing how the analog absolute encoder may not report accurate velocities. + */ + private Alert inaccurateVelocities; /** * Construct the Thrifty Encoder as a Swerve Absolute Encoder. @@ -28,6 +36,14 @@ public class AnalogAbsoluteEncoderSwerve extends SwerveAbsoluteEncoder public AnalogAbsoluteEncoderSwerve(AnalogInput encoder) { this.encoder = encoder; + cannotSetOffset = new Alert( + "Encoders", + "Cannot Set Absolute Encoder Offset of Analog Encoders Channel #" + encoder.getChannel(), + Alert.AlertType.WARNING); + inaccurateVelocities = new Alert( + "Encoders", + "The Analog Absolute encoder may not report accurate velocities!", + Alert.AlertType.WARNING_TRACE); } /** @@ -101,8 +117,7 @@ public class AnalogAbsoluteEncoderSwerve extends SwerveAbsoluteEncoder 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 +129,7 @@ public class AnalogAbsoluteEncoderSwerve extends SwerveAbsoluteEncoder @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/swervelib/encoders/CANCoderSwerve.java b/swervelib/encoders/CANCoderSwerve.java index 5d8c328..011560c 100644 --- a/swervelib/encoders/CANCoderSwerve.java +++ b/swervelib/encoders/CANCoderSwerve.java @@ -9,7 +9,7 @@ import com.ctre.phoenix6.hardware.CANcoder; 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. @@ -20,7 +20,23 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder /** * CANCoder with WPILib sendable and support. */ - public CANcoder encoder; + public CANcoder encoder; + /** + * An {@link Alert} for if the CANCoder magnet field is less than ideal. + */ + private Alert magnetFieldLessThanIdeal; + /** + * An {@link Alert} for if the CANCoder reading is faulty. + */ + private Alert readingFaulty; + /** + * An {@link Alert} for if the CANCoder reading is faulty and the reading is ignored. + */ + private Alert readingIgnored; + /** + * An {@link Alert} for if the absolute encoder offset cannot be set. + */ + private Alert cannotSetOffset; /** * Initialize the CANCoder on the standard CANBus. @@ -41,6 +57,24 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder public CANCoderSwerve(int id, String canbus) { encoder = new CANcoder(id, canbus); + magnetFieldLessThanIdeal = new Alert( + "Encoders", + "CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.", + Alert.AlertType.WARNING); + readingFaulty = new Alert( + "Encoders", + "CANCoder " + encoder.getDeviceID() + " reading was faulty.", + Alert.AlertType.WARNING); + readingIgnored = new Alert( + "Encoders", + "CANCoder " + encoder.getDeviceID() + " reading was faulty, ignoring.", + Alert.AlertType.WARNING); + cannotSetOffset = new Alert( + "Encoders", + "Failure to set CANCoder " + + encoder.getDeviceID() + + " Absolute Encoder Offset", + Alert.AlertType.WARNING); } /** @@ -89,17 +123,17 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder readingError = false; MagnetHealthValue strength = encoder.getMagnetHealth().getValue(); - if (strength != MagnetHealthValue.Magnet_Green) - { - DriverStation.reportWarning( - "CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.\n", false); - } + magnetFieldLessThanIdeal.set(strength != MagnetHealthValue.Magnet_Green); 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,7 +149,10 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder 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 +186,17 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder return false; } error = cfg.apply(magCfg.withMagnetOffset(offset / 360)); + 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/swervelib/encoders/CanAndCoderSwerve.java b/swervelib/encoders/CanAndCoderSwerve.java index 9ab0098..1c6b7b1 100644 --- a/swervelib/encoders/CanAndCoderSwerve.java +++ b/swervelib/encoders/CanAndCoderSwerve.java @@ -25,7 +25,7 @@ public class CanAndCoderSwerve extends SwerveAbsoluteEncoder /** * Reset the encoder to factory defaults. - * + *

* This will not clear the stored zero offset. */ @Override diff --git a/swervelib/encoders/PWMDutyCycleEncoderSwerve.java b/swervelib/encoders/PWMDutyCycleEncoderSwerve.java index 0361aec..24e15d6 100644 --- a/swervelib/encoders/PWMDutyCycleEncoderSwerve.java +++ b/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,10 @@ public class PWMDutyCycleEncoderSwerve extends SwerveAbsoluteEncoder * Inversion state. */ private boolean isInverted; + /** + * An {@link Alert} for if the encoder cannot report accurate velocities. + */ + private Alert inaccurateVelocities; /** * Constructor for the PWM duty cycle encoder. @@ -31,6 +35,11 @@ public class PWMDutyCycleEncoderSwerve extends SwerveAbsoluteEncoder public PWMDutyCycleEncoderSwerve(int pin) { encoder = new DutyCycleEncoder(pin); + inaccurateVelocities = new Alert( + "Encoders", + "The PWM Duty Cycle encoder may not report accurate velocities!", + Alert.AlertType.WARNING_TRACE); + } /** @@ -74,7 +83,7 @@ public class PWMDutyCycleEncoderSwerve extends SwerveAbsoluteEncoder @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/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java b/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java index 0508bca..3024787 100644 --- a/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java +++ b/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java @@ -4,9 +4,9 @@ import com.revrobotics.CANSparkMax; 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. @@ -17,7 +17,16 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder /** * The {@link SparkAnalogSensor} representing the duty cycle encoder attached to the SparkMax analog port. */ - public SparkAnalogSensor encoder; + public SparkAnalogSensor encoder; + /** + * An {@link Alert} for if there is a failure configuring the encoder. + */ + private Alert failureConfiguring; + /** + * An {@link Alert} for if the absolute encoder does not support integrated offsets. + */ + private Alert doesNotSupportIntegratedOffsets; + /** * Create the {@link SparkMaxAnalogEncoderSwerve} object as a analog sensor from the {@link CANSparkMax} motor data @@ -34,6 +43,15 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder { throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax"); } + failureConfiguring = new Alert( + "Encoders", + "Failure configuring SparkMax Analog Encoder", + Alert.AlertType.WARNING_TRACE); + doesNotSupportIntegratedOffsets = new Alert( + "Encoders", + "SparkMax Analog Sensors do not support integrated offsets", + Alert.AlertType.WARNING_TRACE); + } /** @@ -50,7 +68,7 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder return; } } - DriverStation.reportWarning("Failure configuring encoder", true); + failureConfiguring.set(true); } /** @@ -113,7 +131,7 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder @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/swervelib/encoders/SparkMaxEncoderSwerve.java b/swervelib/encoders/SparkMaxEncoderSwerve.java index f95d36d..dd81ada 100644 --- a/swervelib/encoders/SparkMaxEncoderSwerve.java +++ b/swervelib/encoders/SparkMaxEncoderSwerve.java @@ -4,9 +4,9 @@ import com.revrobotics.AbsoluteEncoder; 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. @@ -17,7 +17,15 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder /** * The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkMax. */ - public AbsoluteEncoder encoder; + public AbsoluteEncoder encoder; + /** + * An {@link Alert} for if there is a failure configuring the encoder. + */ + private Alert failureConfiguring; + /** + * An {@link Alert} for if there is a failure configuring the encoder offset. + */ + private Alert offsetFailure; /** * Create the {@link SparkMaxEncoderSwerve} object as a duty cycle from the {@link CANSparkMax} motor. @@ -36,6 +44,14 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder { throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax"); } + failureConfiguring = new Alert( + "Encoders", + "Failure configuring SparkMax Analog Encoder", + Alert.AlertType.WARNING_TRACE); + offsetFailure = new Alert( + "Encoders", + "Failure to set Absolute Encoder Offset", + Alert.AlertType.WARNING_TRACE); } /** @@ -52,7 +68,7 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder return; } } - DriverStation.reportWarning("Failure configuring encoder", true); + failureConfiguring.set(true); } /** @@ -124,7 +140,8 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder 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/swervelib/encoders/SwerveAbsoluteEncoder.java b/swervelib/encoders/SwerveAbsoluteEncoder.java index a01e4c4..c992b81 100644 --- a/swervelib/encoders/SwerveAbsoluteEncoder.java +++ b/swervelib/encoders/SwerveAbsoluteEncoder.java @@ -56,6 +56,7 @@ public abstract class SwerveAbsoluteEncoder /** * Get the velocity in degrees/sec. + * * @return velocity in degrees/sec. */ public abstract double getVelocity(); diff --git a/swervelib/imu/ADIS16470Swerve.java b/swervelib/imu/ADIS16470Swerve.java index 5a102bf..562bbd1 100644 --- a/swervelib/imu/ADIS16470Swerve.java +++ b/swervelib/imu/ADIS16470Swerve.java @@ -2,8 +2,8 @@ package swervelib.imu; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis; import edu.wpi.first.wpilibj.ADIS16470_IMU; +import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.Optional; diff --git a/swervelib/imu/NavXSwerve.java b/swervelib/imu/NavXSwerve.java index 272efaf..339c0e9 100644 --- a/swervelib/imu/NavXSwerve.java +++ b/swervelib/imu/NavXSwerve.java @@ -1,15 +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 java.util.Optional; +import swervelib.telemetry.Alert; /** * Communicates with the NavX as the IMU. @@ -25,6 +24,10 @@ 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; /** * Constructor for the NavX swerve. @@ -33,6 +36,7 @@ public class NavXSwerve extends SwerveIMU */ public NavXSwerve(SerialPort.Port port) { + navXError = new Alert("IMU", "Error instantiating NavX.", Alert.AlertType.ERROR_TRACE); try { /* Communicate w/navX-MXP via the MXP SPI Bus. */ @@ -43,7 +47,8 @@ public class NavXSwerve extends SwerveIMU 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 +69,8 @@ public class NavXSwerve extends SwerveIMU 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 +91,8 @@ public class NavXSwerve extends SwerveIMU 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/swervelib/imu/Pigeon2Swerve.java b/swervelib/imu/Pigeon2Swerve.java index 61fb6f8..25a41ea 100644 --- a/swervelib/imu/Pigeon2Swerve.java +++ b/swervelib/imu/Pigeon2Swerve.java @@ -88,11 +88,14 @@ public class Pigeon2Swerve extends SwerveIMU public Rotation3d getRawRotation3d() { // TODO: Switch to suppliers. - StatusSignal w = imu.getQuatW(); - StatusSignal x = imu.getQuatX(); - StatusSignal y = imu.getQuatY(); - StatusSignal z = imu.getQuatZ(); - return new Rotation3d(new Quaternion(w.refresh().getValue(), x.refresh().getValue(), y.refresh().getValue(), z.refresh().getValue())); + StatusSignal w = imu.getQuatW(); + StatusSignal x = imu.getQuatX(); + StatusSignal y = imu.getQuatY(); + StatusSignal z = imu.getQuatZ(); + return new Rotation3d(new Quaternion(w.refresh().getValue(), + x.refresh().getValue(), + y.refresh().getValue(), + z.refresh().getValue())); } /** @@ -120,7 +123,9 @@ public class Pigeon2Swerve extends SwerveIMU StatusSignal yAcc = imu.getAccelerationX(); StatusSignal zAcc = imu.getAccelerationX(); - return Optional.of(new Translation3d(xAcc.refresh().getValue(), yAcc.refresh().getValue(), zAcc.refresh().getValue()).times(9.81 / 16384.0)); + return Optional.of(new Translation3d(xAcc.refresh().getValue(), + yAcc.refresh().getValue(), + zAcc.refresh().getValue()).times(9.81 / 16384.0)); } /** diff --git a/swervelib/math/SwerveMath.java b/swervelib/math/SwerveMath.java index 0b4fdd5..5de06d5 100644 --- a/swervelib/math/SwerveMath.java +++ b/swervelib/math/SwerveMath.java @@ -13,8 +13,6 @@ import swervelib.SwerveController; 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/swervelib/motors/SparkFlexSwerve.java b/swervelib/motors/SparkFlexSwerve.java index a5aef96..27c9c4a 100644 --- a/swervelib/motors/SparkFlexSwerve.java +++ b/swervelib/motors/SparkFlexSwerve.java @@ -1,21 +1,21 @@ package swervelib.motors; import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.CANSparkFlex; -import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkLowLevel.PeriodicFrame; +import com.revrobotics.CANSparkMax; import com.revrobotics.MotorFeedbackSensor; import com.revrobotics.REVLibError; 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; /** @@ -27,11 +27,11 @@ public class SparkFlexSwerve extends SwerveMotor /** * SparkMAX Instance. */ - public CANSparkFlex motor; + public CANSparkFlex motor; /** * Integrated encoder. */ - public RelativeEncoder encoder; + public RelativeEncoder encoder; /** * Absolute encoder attached to the SparkMax (if exists) */ @@ -39,11 +39,19 @@ public class SparkFlexSwerve extends SwerveMotor /** * Closed-loop PID controller. */ - public SparkPIDController pid; + public SparkPIDController pid; /** * Factory default already occurred. */ private boolean factoryDefaultOccurred = false; + /** + * An {@link Alert} for if there is an error configuring the motor. + */ + private Alert failureConfiguring; + /** + * An {@link Alert} for if the absolute encoder's offset is set in the json instead of the hardware client. + */ + private Alert absoluteEncoderOffsetWarning; /** * Initialize the swerve motor. @@ -65,6 +73,15 @@ public class SparkFlexSwerve extends SwerveMotor // Spin off configurations in a different thread. // configureSparkMax(() -> motor.setCANTimeout(0)); // Commented out because it prevents feedback. + failureConfiguring = new Alert("Motors", + "Failure configuring motor " + + motor.getDeviceId(), + Alert.AlertType.WARNING_TRACE); + 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); + } /** @@ -92,7 +109,7 @@ public class SparkFlexSwerve extends SwerveMotor return; } } - DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true); + failureConfiguring.set(true); } /** @@ -185,10 +202,7 @@ public class SparkFlexSwerve extends SwerveMotor { 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())); } @@ -357,20 +371,20 @@ public class SparkFlexSwerve extends SwerveMotor if (isDriveMotor) { configureSparkFlex(() -> - pid.setReference( - setpoint, - ControlType.kVelocity, - pidSlot, - feedforward)); + pid.setReference( + setpoint, + ControlType.kVelocity, + pidSlot, + feedforward)); } else { configureSparkFlex(() -> - pid.setReference( - setpoint, - ControlType.kPosition, - pidSlot, - feedforward)); - if(SwerveDriveTelemetry.isSimulation) + pid.setReference( + setpoint, + ControlType.kPosition, + pidSlot, + feedforward)); + if (SwerveDriveTelemetry.isSimulation) { encoder.setPosition(setpoint); } diff --git a/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/swervelib/motors/SparkMaxBrushedMotorSwerve.java index fc7d94e..3ec6536 100644 --- a/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -1,20 +1,20 @@ package swervelib.motors; import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkLowLevel.PeriodicFrame; +import com.revrobotics.CANSparkMax; 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; /** * Brushed motor control with SparkMax. @@ -30,11 +30,11 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor /** * Absolute encoder attached to the SparkMax (if exists) */ - public AbsoluteEncoder absoluteEncoder; + public AbsoluteEncoder absoluteEncoder; /** * Integrated encoder. */ - public RelativeEncoder encoder; + public RelativeEncoder encoder; /** * Closed-loop PID controller. */ @@ -42,7 +42,19 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor /** * Factory default already occurred. */ - private boolean factoryDefaultOccurred = false; + private boolean factoryDefaultOccurred = false; + /** + * An {@link Alert} for if the motor has no encoder. + */ + private Alert noEncoderAlert; + /** + * An {@link Alert} for if there is an error configuring the motor. + */ + private Alert failureConfiguringAlert; + /** + * An {@link Alert} for if the motor has no encoder defined. + */ + private Alert noEncoderDefinedAlert; /** * Initialize the swerve motor. @@ -59,7 +71,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor // 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."); } @@ -90,6 +102,16 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor } // Spin off configurations in a different thread. // configureSparkMax(() -> motor.setCANTimeout(0)); // Commented it out because it prevents feedback. + + noEncoderAlert = new Alert("Motors", + "Cannot use motor without encoder.", + Alert.AlertType.ERROR_TRACE); + failureConfiguringAlert = new Alert("Motors", + "Failure configuring motor " + motor.getDeviceId(), + Alert.AlertType.WARNING_TRACE); + noEncoderDefinedAlert = new Alert("Motors", + "An encoder MUST be defined to work with a SparkMAX", + Alert.AlertType.ERROR_TRACE); } /** @@ -122,7 +144,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor return; } } - DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true); + failureConfiguringAlert.set(true); } /** @@ -220,7 +242,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor } 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/swervelib/motors/TalonFXSwerve.java b/swervelib/motors/TalonFXSwerve.java index af29e21..fab3a01 100644 --- a/swervelib/motors/TalonFXSwerve.java +++ b/swervelib/motors/TalonFXSwerve.java @@ -20,31 +20,31 @@ public class TalonFXSwerve extends SwerveMotor /** * Factory default already occurred. */ - private final boolean factoryDefaultOccurred = false; - /** - * Current TalonFX configuration. - */ - private TalonFXConfiguration configuration = new TalonFXConfiguration(); + private final boolean factoryDefaultOccurred = false; /** * Whether the absolute encoder is integrated. */ - private final boolean absoluteEncoder = false; + private final boolean absoluteEncoder = false; /** * Motion magic angle voltage setter. */ - private final MotionMagicVoltage m_angleVoltageSetter = new MotionMagicVoltage(0); + private final MotionMagicVoltage m_angleVoltageSetter = new MotionMagicVoltage(0); + /** + * Velocity voltage setter for controlling drive motor. + */ + private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0); // /** // * Motion Magic exponential voltage setters. // */ // private final MotionMagicExpoVoltage m_angleVoltageExpoSetter = new MotionMagicExpoVoltage(0); - /** - * Velocity voltage setter for controlling drive motor. - */ - private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0); /** * TalonFX motor controller. */ TalonFX motor; + /** + * Current TalonFX configuration. + */ + private TalonFXConfiguration configuration = new TalonFXConfiguration(); /** * Constructor for TalonFX swerve motor. diff --git a/swervelib/parser/SwerveDriveConfiguration.java b/swervelib/parser/SwerveDriveConfiguration.java index f0df58a..06ec164 100644 --- a/swervelib/parser/SwerveDriveConfiguration.java +++ b/swervelib/parser/SwerveDriveConfiguration.java @@ -89,7 +89,22 @@ public class SwerveDriveConfiguration */ public double getDriveBaseRadiusMeters() { - Translation2d furthestModule = moduleLocationsMeters[0]; - return Math.abs(Math.sqrt(Math.pow(furthestModule.getX(), 2) + Math.pow(furthestModule.getY(), 2))); + //Find Center of Robot by adding all module offsets together. Should be zero, but incase it isn't + Translation2d centerOfModules = moduleLocationsMeters[0].plus(moduleLocationsMeters[1]) + .plus(moduleLocationsMeters[2]) + .plus(moduleLocationsMeters[3]); + + //Find Largest Radius by checking the distance to the center point + double largestRadius = centerOfModules.getDistance(moduleLocationsMeters[0]); + for (int i = 1; i < moduleLocationsMeters.length; i++) + { + if (largestRadius < centerOfModules.getDistance(moduleLocationsMeters[i])) + { + largestRadius = centerOfModules.getDistance(moduleLocationsMeters[i]); + } + } + + //Return Largest Radius + return largestRadius; } } diff --git a/swervelib/parser/json/DeviceJson.java b/swervelib/parser/json/DeviceJson.java index 9b3a8aa..59e1205 100644 --- a/swervelib/parser/json/DeviceJson.java +++ b/swervelib/parser/json/DeviceJson.java @@ -26,6 +26,7 @@ import swervelib.motors.SparkMaxSwerve; 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. @@ -33,18 +34,30 @@ import swervelib.motors.TalonSRXSwerve; 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); /** * 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. @@ -57,8 +70,7 @@ public class DeviceJson { 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 +111,7 @@ public class DeviceJson { 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 +132,7 @@ public class DeviceJson "\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 +157,7 @@ public class DeviceJson { 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/swervelib/telemetry/Alert.java b/swervelib/telemetry/Alert.java new file mode 100644 index 0000000..1718c82 --- /dev/null +++ b/swervelib/telemetry/Alert.java @@ -0,0 +1,213 @@ +// 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; + } + + /** + * 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 + } + + 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); + } + } +} \ No newline at end of file