From a63d2384b731b5ca1b625fc717727910dee40d02 Mon Sep 17 00:00:00 2001 From: T Grinch <10247070+thenetworkgrinch@users.noreply.github.com> Date: Tue, 16 Jan 2024 21:21:18 -0600 Subject: [PATCH] Updated formatting. Signed-off-by: thenetworkgrinch --- src/main/java/swervelib/SwerveDrive.java | 22 +- src/main/java/swervelib/SwerveModule.java | 16 +- .../encoders/AnalogAbsoluteEncoderSwerve.java | 14 +- .../swervelib/encoders/CANCoderSwerve.java | 51 +-- .../swervelib/encoders/CanAndCoderSwerve.java | 2 +- .../encoders/PWMDutyCycleEncoderSwerve.java | 6 +- .../encoders/SparkMaxAnalogEncoderSwerve.java | 15 +- .../encoders/SparkMaxEncoderSwerve.java | 11 +- .../encoders/SwerveAbsoluteEncoder.java | 1 + .../java/swervelib/imu/ADIS16470Swerve.java | 2 +- src/main/java/swervelib/imu/NavXSwerve.java | 9 +- .../java/swervelib/imu/Pigeon2Swerve.java | 17 +- .../swervelib/motors/SparkFlexSwerve.java | 56 ++-- .../motors/SparkMaxBrushedMotorSwerve.java | 40 ++- .../java/swervelib/motors/TalonFXSwerve.java | 16 +- .../parser/SwerveDriveConfiguration.java | 15 +- .../swervelib/parser/json/DeviceJson.java | 14 +- src/main/java/swervelib/telemetry/Alert.java | 294 ++++++++++-------- 18 files changed, 341 insertions(+), 260 deletions(-) diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index b13f72029..aeef17b66 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -66,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. */ @@ -99,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. */ @@ -119,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. */ @@ -412,10 +412,10 @@ public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d cent 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/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 5827c99ff..61d2b5903 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -64,11 +64,17 @@ public class SwerveModule /** * 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); + 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); + 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. @@ -405,10 +411,12 @@ public void pushOffsetsToControllers() if (absoluteEncoder.setAbsoluteEncoderOffset(angleOffset)) { angleOffset = 0; - } else { + } else + { encoderOffsetWarning.set(true); } - } else { + } else + { noEncoderWarning.set(true); } } diff --git a/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java b/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java index 33cfd76d4..f0af1a03a 100644 --- a/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java @@ -18,14 +18,18 @@ 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( + 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( + /** + * 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); diff --git a/src/main/java/swervelib/encoders/CANCoderSwerve.java b/src/main/java/swervelib/encoders/CANCoderSwerve.java index 22f97bd4b..7a71875e7 100644 --- a/src/main/java/swervelib/encoders/CANCoderSwerve.java +++ b/src/main/java/swervelib/encoders/CANCoderSwerve.java @@ -20,28 +20,36 @@ 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( + 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( + /** + * 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( + /** + * 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( + /** + * 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", + + encoder.getDeviceID() + + " Absolute Encoder Offset", Alert.AlertType.WARNING); /** @@ -111,16 +119,13 @@ public double getAbsolutePosition() readingError = false; MagnetHealthValue strength = encoder.getMagnetHealth().getValue(); - if (strength != MagnetHealthValue.Magnet_Green) - { - magnetFieldLessThanIdeal.set(true); - } else magnetFieldLessThanIdeal.set(false); + magnetFieldLessThanIdeal.set(strength != MagnetHealthValue.Magnet_Green); if (strength == MagnetHealthValue.Magnet_Invalid || strength == MagnetHealthValue.Magnet_Red) { readingError = true; readingFaulty.set(true); return 0; - }else + } else { readingFaulty.set(false); } @@ -141,7 +146,10 @@ public double getAbsolutePosition() { readingError = true; readingIgnored.set(true); - } else readingIgnored.set(false); + } else + { + readingIgnored.set(false); + } return angle.getValue() * 360; } @@ -176,10 +184,11 @@ public boolean setAbsoluteEncoderOffset(double offset) error = cfg.apply(magCfg.withMagnetOffset(offset / 360)); cannotSetOffset.setText( "Failure to set CANCoder " - + encoder.getDeviceID() - + " Absolute Encoder Offset Error: " - + error); - if (error == StatusCode.OK) { + + encoder.getDeviceID() + + " Absolute Encoder Offset Error: " + + error); + if (error == StatusCode.OK) + { cannotSetOffset.set(false); return true; } diff --git a/src/main/java/swervelib/encoders/CanAndCoderSwerve.java b/src/main/java/swervelib/encoders/CanAndCoderSwerve.java index 9ab00987f..1c6b7b13a 100644 --- a/src/main/java/swervelib/encoders/CanAndCoderSwerve.java +++ b/src/main/java/swervelib/encoders/CanAndCoderSwerve.java @@ -25,7 +25,7 @@ public CanAndCoderSwerve(int canid) /** * Reset the encoder to factory defaults. - * + *

* This will not clear the stored zero offset. */ @Override diff --git a/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java b/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java index 4f83aa3ed..d43c80ed5 100644 --- a/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java @@ -22,8 +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 = new Alert( + /** + * 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); diff --git a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java index b8b040044..22084d323 100644 --- a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java @@ -4,7 +4,6 @@ import com.revrobotics.REVLibError; import com.revrobotics.SparkAnalogSensor; import com.revrobotics.SparkAnalogSensor.Mode; - import java.util.function.Supplier; import swervelib.motors.SwerveMotor; import swervelib.telemetry.Alert; @@ -18,14 +17,18 @@ 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( + 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( + /** + * 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); diff --git a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java index da773d9cc..4d8ed8e27 100644 --- a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java @@ -4,7 +4,6 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.REVLibError; import com.revrobotics.SparkAbsoluteEncoder.Type; - import java.util.function.Supplier; import swervelib.motors.SwerveMotor; import swervelib.telemetry.Alert; @@ -18,13 +17,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( + 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( + private Alert offsetFailure = new Alert( "Encoders", "Failure to set Absolute Encoder Offset", Alert.AlertType.WARNING_TRACE); diff --git a/src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java b/src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java index a01e4c472..c992b81a5 100644 --- a/src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java +++ b/src/main/java/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/src/main/java/swervelib/imu/ADIS16470Swerve.java b/src/main/java/swervelib/imu/ADIS16470Swerve.java index 5a102bf06..562bbd1ee 100644 --- a/src/main/java/swervelib/imu/ADIS16470Swerve.java +++ b/src/main/java/swervelib/imu/ADIS16470Swerve.java @@ -2,8 +2,8 @@ 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/src/main/java/swervelib/imu/NavXSwerve.java b/src/main/java/swervelib/imu/NavXSwerve.java index 621fd2d2a..8fa09fdb2 100644 --- a/src/main/java/swervelib/imu/NavXSwerve.java +++ b/src/main/java/swervelib/imu/NavXSwerve.java @@ -7,9 +7,8 @@ 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; +import swervelib.telemetry.Alert; /** * Communicates with the NavX as the IMU. @@ -24,8 +23,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 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); /** diff --git a/src/main/java/swervelib/imu/Pigeon2Swerve.java b/src/main/java/swervelib/imu/Pigeon2Swerve.java index 61fb6f834..25a41ea20 100644 --- a/src/main/java/swervelib/imu/Pigeon2Swerve.java +++ b/src/main/java/swervelib/imu/Pigeon2Swerve.java @@ -88,11 +88,14 @@ public void setOffset(Rotation3d offset) 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 Optional getAccel() 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/src/main/java/swervelib/motors/SparkFlexSwerve.java b/src/main/java/swervelib/motors/SparkFlexSwerve.java index a8b7e159a..22216fe4c 100644 --- a/src/main/java/swervelib/motors/SparkFlexSwerve.java +++ b/src/main/java/swervelib/motors/SparkFlexSwerve.java @@ -1,18 +1,17 @@ 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 java.util.function.Supplier; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; @@ -28,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) */ @@ -40,18 +39,25 @@ 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 = 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); + 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. @@ -362,20 +368,20 @@ public void setReference(double setpoint, double feedforward) 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/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java index 53604739c..6d4615940 100644 --- a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -1,17 +1,21 @@ package swervelib.motors; -import com.revrobotics.*; +import com.revrobotics.AbsoluteEncoder; 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 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. */ @@ -26,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. */ @@ -38,13 +42,25 @@ 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); + 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. diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java index af29e21ab..2deefb358 100644 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -21,10 +21,6 @@ public class TalonFXSwerve extends SwerveMotor * Factory default already occurred. */ private final boolean factoryDefaultOccurred = false; - /** - * Current TalonFX configuration. - */ - private TalonFXConfiguration configuration = new TalonFXConfiguration(); /** * Whether the absolute encoder is integrated. */ @@ -33,18 +29,22 @@ public class TalonFXSwerve extends SwerveMotor * Motion magic angle voltage setter. */ private final MotionMagicVoltage m_angleVoltageSetter = new MotionMagicVoltage(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); +// /** +// * Motion Magic exponential voltage setters. +// */ +// private final MotionMagicExpoVoltage m_angleVoltageExpoSetter = new MotionMagicExpoVoltage(0); /** * TalonFX motor controller. */ TalonFX motor; + /** + * Current TalonFX configuration. + */ + private TalonFXConfiguration configuration = new TalonFXConfiguration(); /** * Constructor for TalonFX swerve motor. diff --git a/src/main/java/swervelib/parser/SwerveDriveConfiguration.java b/src/main/java/swervelib/parser/SwerveDriveConfiguration.java index 3e416783f..06ec164d1 100644 --- a/src/main/java/swervelib/parser/SwerveDriveConfiguration.java +++ b/src/main/java/swervelib/parser/SwerveDriveConfiguration.java @@ -91,16 +91,19 @@ public double getDriveBaseRadiusMeters() { //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]); + .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 groups = new HashMap(); +/** + * Class for managing persistent alerts to be sent over NetworkTables. + */ +public class Alert +{ - private final AlertType type; - private boolean active = false; - private double activeStartTime = 0.0; - private String text; + private static Map groups = new HashMap(); - /** - * 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); + 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 + { /** - * 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. + * 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. */ - 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); - } + 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, /** - * Sets whether the alert should currently be displayed. When activated, the alert text will also - * be sent to the console. + * 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. */ - 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; - } + 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 + } - /** 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<>(); - 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); - } + 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); } - /** 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 + @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