From 68ecbe929be07cfcb0bcf6fcb3737bd9fa32d8ab Mon Sep 17 00:00:00 2001 From: T Grinch <10247070+thenetworkgrinch@users.noreply.github.com> Date: Tue, 12 Dec 2023 10:48:16 -0600 Subject: [PATCH] Reformatted code, generated javadocs. Signed-off-by: thenetworkgrinch --- src/main/java/swervelib/SwerveDrive.java | 39 +++++++------- src/main/java/swervelib/SwerveModule.java | 51 ++++++++++--------- .../encoders/AnalogAbsoluteEncoderSwerve.java | 13 +++-- .../swervelib/encoders/CANCoderSwerve.java | 12 ++--- .../swervelib/encoders/CanAndCoderSwerve.java | 9 ++-- .../encoders/PWMDutyCycleEncoderSwerve.java | 3 +- .../encoders/SparkMaxEncoderSwerve.java | 12 ++--- .../encoders/SwerveAbsoluteEncoder.java | 6 +-- .../java/swervelib/motors/SparkMaxSwerve.java | 12 +++-- .../SwerveModulePhysicalCharacteristics.java | 2 +- .../parser/json/PhysicalPropertiesJson.java | 2 +- 11 files changed, 84 insertions(+), 77 deletions(-) diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index f270e2c6f..d47d2f61a 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -66,11 +66,11 @@ public class SwerveDrive /** * Odometry lock to ensure thread safety. */ - private final Lock odometryLock = new ReentrantLock(); + private final Lock odometryLock = new ReentrantLock(); /** * Field object. */ - public Field2d field = new Field2d(); + public Field2d field = new Field2d(); /** * Swerve controller for controlling heading of the robot. */ @@ -79,31 +79,31 @@ public class SwerveDrive * Standard deviation of encoders and gyroscopes, usually should not change. (meters of position and degrees of * rotation) */ - public Matrix stateStdDevs = VecBuilder.fill(0.1, - 0.1, - 0.1); + public Matrix stateStdDevs = VecBuilder.fill(0.1, + 0.1, + 0.1); /** * The standard deviation of the vision measurement, for best accuracy calculate the standard deviation at 2 or more * points and fit a line to it and modify this using {@link SwerveDrive#addVisionMeasurement(Pose2d, double, Matrix)} * with the calculated optimal standard deviation. (Units should be meters per pixel). By optimizing this you can get * vision accurate to inches instead of feet. */ - public Matrix visionMeasurementStdDevs = VecBuilder.fill(0.9, - 0.9, - 0.9); + public Matrix visionMeasurementStdDevs = VecBuilder.fill(0.9, + 0.9, + 0.9); /** * Invert odometry readings of drive motor positions, used as a patch for debugging currently. */ - public boolean invertOdometry = false; + public boolean invertOdometry = false; /** * Correct chassis velocity in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)} using 254's * correction. */ - public boolean chassisVelocityCorrection = true; + public boolean chassisVelocityCorrection = true; /** * Whether to correct heading when driving translationally. Set to true to enable. */ - public boolean headingCorrection = false; + public boolean headingCorrection = false; /** * Swerve IMU device for sensing the heading of the robot. */ @@ -115,23 +115,23 @@ public class SwerveDrive /** * Counter to synchronize the modules relative encoder with absolute encoder when not moving. */ - private int moduleSynchronizationCounter = 0; + private int moduleSynchronizationCounter = 0; /** * The last heading set in radians. */ - private double lastHeadingRadians = 0; + private double lastHeadingRadians = 0; /** * The absolute max speed that your robot can reach while translating in meters per second. */ - private double attainableMaxTranslationalSpeedMetersPerSecond = 0; + private double attainableMaxTranslationalSpeedMetersPerSecond = 0; /** * The absolute max speed the robot can reach while rotating radians per second. */ - private double attainableMaxRotationalVelocityRadiansPerSecond = 0; + private double attainableMaxRotationalVelocityRadiansPerSecond = 0; /** * Maximum speed of the robot in meters per second. */ - private double maxSpeedMPS; + private double maxSpeedMPS; /** * Creates a new swerve drivebase subsystem. Robot is controlled via the {@link SwerveDrive#drive} method, or via the @@ -902,7 +902,8 @@ public void updateOdometry() SmartDashboard.putNumber( "Module[" + module.configuration.name + "] Absolute Encoder", module.getAbsolutePosition()); SmartDashboard.putNumber( - "Module[" + module.configuration.name + "] Absolute Encoder Read Issue", module.getAbsoluteEncoderReadIssue() ? 1 : 0); + "Module[" + module.configuration.name + "] Absolute Encoder Read Issue", + module.getAbsoluteEncoderReadIssue() ? 1 : 0); } if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) { @@ -1072,8 +1073,8 @@ public void setModuleStateOptimization(boolean optimizationEnabled) } /** - * Pushes the Absolute Encoder offsets to the Encoder or Motor Controller, depending on type. - * Also removes the internal offsets to prevent double offsetting. + * Pushes the Absolute Encoder offsets to the Encoder or Motor Controller, depending on type. Also removes the + * internal offsets to prevent double offsetting. */ public void pushOffsetsToControllers() { diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index bcc9a39a9..999ccc161 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -24,10 +24,6 @@ public class SwerveModule * Swerve module configuration options. */ public final SwerveModuleConfiguration configuration; - /** - * Angle offset from the absolute encoder. - */ - private double angleOffset; /** * Swerve Motors. */ @@ -39,32 +35,36 @@ public class SwerveModule /** * Module number for kinematics, usually 0 to 3. front left -> front right -> back left -> back right. */ - public int moduleNumber; + public int moduleNumber; /** * Feedforward for drive motor during closed loop control. */ - public SimpleMotorFeedforward feedforward; + public SimpleMotorFeedforward feedforward; /** * Maximum speed of the drive motors in meters per second. */ - public double maxSpeed; + public double maxSpeed; /** * Last swerve module state applied. */ - public SwerveModuleState lastState; + public SwerveModuleState lastState; /** * Enable {@link SwerveModuleState} optimizations so the angle is reversed and speed is reversed to ensure the module * never turns more than 90deg. */ - public boolean moduleStateOptimization = true; + public boolean moduleStateOptimization = true; + /** + * Angle offset from the absolute encoder. + */ + private double angleOffset; /** * Simulated swerve module. */ - private SwerveModuleSimulation simModule; + private SwerveModuleSimulation simModule; /** * Encoder synchronization queued. */ - private boolean synchronizeEncoderQueued = false; + private boolean synchronizeEncoderQueued = false; /** * Construct the swerve module and initialize the swerve module motors and absolute encoder. @@ -390,22 +390,23 @@ public SwerveModuleConfiguration getConfiguration() } /** - * Push absolute encoder offset in the memory of the encoder or controller. - * Also removes the internal angle offset. + * Push absolute encoder offset in the memory of the encoder or controller. Also removes the internal angle offset. */ public void pushOffsetsToControllers() { - if(absoluteEncoder != null) + if (absoluteEncoder != null) { - if(absoluteEncoder.setAbsoluteEncoderOffset(angleOffset)){ + if (absoluteEncoder.setAbsoluteEncoderOffset(angleOffset)) + { angleOffset = 0; + } else + { + DriverStation.reportWarning( + "Pushing the Absolute Encoder offset to the encoder failed on module #" + moduleNumber, false); } - else{ - DriverStation.reportWarning("Pushing the Absolute Encoder offset to the encoder failed on module #"+moduleNumber, false); - } - } - else{ - DriverStation.reportWarning("There is no Absolute Encoder on module #"+moduleNumber, false); + } else + { + DriverStation.reportWarning("There is no Absolute Encoder on module #" + moduleNumber, false); } } @@ -417,6 +418,7 @@ public void restoreInternalOffset() absoluteEncoder.setAbsoluteEncoderOffset(0); angleOffset = configuration.angleOffset; } + /** * Get if the last Absolute Encoder had a read issue, such as it does not exist. * @@ -424,9 +426,12 @@ public void restoreInternalOffset() */ public boolean getAbsoluteEncoderReadIssue() { - if(absoluteEncoder == null) + if (absoluteEncoder == null) + { return true; - else + } else + { return absoluteEncoder.readingError; + } } } diff --git a/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java b/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java index 600e137b5..1b830b1b3 100644 --- a/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java @@ -3,7 +3,6 @@ import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.DriverStation; /** * Swerve Absolute Encoder for Thrifty Encoders and other analog encoders. @@ -94,19 +93,19 @@ public Object getAbsoluteEncoder() /** * Cannot Set the offset of an Analog Absolute Encoder. - * + * * @param offset the offset the Absolute Encoder uses as the zero point. - * * @return Will always be false as setting the offset is unsupported of an Analog absolute encoder. */ @Override public boolean setAbsoluteEncoderOffset(double offset) { //Do Nothing - DriverStation.reportWarning("Cannot Set Absolute Encoder Offset of Analog Encoders Channel #"+encoder.getChannel(), false); + DriverStation.reportWarning( + "Cannot Set Absolute Encoder Offset of Analog Encoders Channel #" + encoder.getChannel(), false); return false; - } - + } + /** * Get the velocity in degrees/sec. * @@ -115,7 +114,7 @@ public boolean setAbsoluteEncoderOffset(double offset) @Override public double getVelocity() { - DriverStation.reportWarning("The Analog Absolute encoder may not report accurate velocities!",true); + DriverStation.reportWarning("The Analog Absolute encoder may not report accurate velocities!", true); return encoder.getValue(); } } diff --git a/src/main/java/swervelib/encoders/CANCoderSwerve.java b/src/main/java/swervelib/encoders/CANCoderSwerve.java index 4c7637de5..87c93cc09 100644 --- a/src/main/java/swervelib/encoders/CANCoderSwerve.java +++ b/src/main/java/swervelib/encoders/CANCoderSwerve.java @@ -140,23 +140,23 @@ public Object getAbsoluteEncoder() /** * Sets the Absolute Encoder Offset inside of the CANcoder's Memory. - * + * * @param offset the offset the Absolute Encoder uses as the zero point. - * * @return if setting Absolute Encoder Offset was successful or not. */ @Override public boolean setAbsoluteEncoderOffset(double offset) { ErrorCode error = encoder.configMagnetOffset(offset); - if(error == ErrorCode.OK) + if (error == ErrorCode.OK) { return true; } - DriverStation.reportWarning("Failure to set CANCoder " + encoder.getDeviceID() + " Absolute Encoder Offset Error: "+error, false); + DriverStation.reportWarning( + "Failure to set CANCoder " + encoder.getDeviceID() + " Absolute Encoder Offset Error: " + error, false); return false; - } - + } + /** * Get the velocity in degrees/sec. * diff --git a/src/main/java/swervelib/encoders/CanAndCoderSwerve.java b/src/main/java/swervelib/encoders/CanAndCoderSwerve.java index 64ea4719d..7e7c19dcf 100644 --- a/src/main/java/swervelib/encoders/CanAndCoderSwerve.java +++ b/src/main/java/swervelib/encoders/CanAndCoderSwerve.java @@ -1,7 +1,6 @@ package swervelib.encoders; import com.reduxrobotics.sensors.canandcoder.CANandcoder; - import edu.wpi.first.wpilibj.DriverStation; /** @@ -82,19 +81,19 @@ public Object getAbsoluteEncoder() /** * Cannot set the offset of the CanAndCoder. - * + * * @param offset the offset the Absolute Encoder uses as the zero point. - * * @return always false due to CanAndCoder not supporting offset changing. */ @Override public boolean setAbsoluteEncoderOffset(double offset) { //CanAndCoder does not support Absolute Offset Changing - DriverStation.reportWarning("Cannot Set Absolute Encoder Offset of CanAndCoders ID: "+encoder.getAddress(), false); + DriverStation.reportWarning("Cannot Set Absolute Encoder Offset of CanAndCoders ID: " + encoder.getAddress(), + false); return false; } - + /** * Get the velocity in degrees/sec. * diff --git a/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java b/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java index e85ba8164..0361aec81 100644 --- a/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java @@ -98,9 +98,8 @@ public void clearStickyFaults() /** * Sets the offset of the Encoder in the WPILib Encoder Library. - * + * * @param offset the offset the Absolute Encoder uses as the zero point. - * * @return Always true due to no external device commands. */ @Override diff --git a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java index cc178adea..87f978228 100644 --- a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java @@ -108,27 +108,27 @@ public Object getAbsoluteEncoder() /** * Sets the Absolute Encoder Offset inside of the SparkMax's Memory. - * + * * @param offset the offset the Absolute Encoder uses as the zero point. - * * @return if setting Absolute Encoder Offset was successful or not. */ @Override public boolean setAbsoluteEncoderOffset(double offset) { REVLibError error = null; - for(int i=0; i < maximumRetries; i++) + for (int i = 0; i < maximumRetries; i++) { error = encoder.setZeroOffset(offset); - if(error == REVLibError.kOk) + if (error == REVLibError.kOk) { return true; } } - DriverStation.reportWarning("Failure to set Absolute Encoder Offset Error: "+ error, false); + DriverStation.reportWarning("Failure to set Absolute Encoder Offset Error: " + error, false); return false; } - /** + + /** * Get the velocity in degrees/sec. * * @return velocity in degrees/sec. diff --git a/src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java b/src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java index 387c6e91d..a01e4c472 100644 --- a/src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java +++ b/src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java @@ -48,13 +48,13 @@ public abstract class SwerveAbsoluteEncoder /** * Sets the Absolute Encoder offset at the Encoder Level. - * + * * @param offset the offset the Absolute Encoder uses as the zero point. - * * @return if setting Absolute Encoder Offset was successful or not. */ public abstract boolean setAbsoluteEncoderOffset(double offset); - /* + + /** * Get the velocity in degrees/sec. * @return velocity in degrees/sec. */ diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java index 21859863c..7d97cae97 100644 --- a/src/main/java/swervelib/motors/SparkMaxSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java @@ -214,19 +214,23 @@ public void configureIntegratedEncoder(double positionConversionFactor) configureSparkMax(() -> { if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) { - return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor(positionConversionFactor); + return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor( + positionConversionFactor); } else { - return ((SparkMaxAnalogSensor)absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor(positionConversionFactor); + return ((SparkMaxAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor( + positionConversionFactor); } }); configureSparkMax(() -> { if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) { - return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor(positionConversionFactor / 60); + return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor( + positionConversionFactor / 60); } else { - return ((SparkMaxAnalogSensor)absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor(positionConversionFactor / 60); + return ((SparkMaxAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor( + positionConversionFactor / 60); } }); } diff --git a/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java b/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java index 87e13cf51..99360ce1b 100644 --- a/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java +++ b/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java @@ -11,7 +11,7 @@ public class SwerveModulePhysicalCharacteristics /** * Current limits for the Swerve Module. */ - public final int driveMotorCurrentLimit, angleMotorCurrentLimit; + public final int driveMotorCurrentLimit, angleMotorCurrentLimit; /** * The time it takes for the motor to go from 0 to full throttle in seconds. */ diff --git a/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java b/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java index 779f84fb9..4ede4025f 100644 --- a/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java +++ b/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java @@ -14,7 +14,7 @@ public class PhysicalPropertiesJson * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)} for angle motors or * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} for drive motors. */ - public MotorConfigDouble conversionFactor = new MotorConfigDouble(0, 0); + public MotorConfigDouble conversionFactor = new MotorConfigDouble(0, 0); /** * The current limit in AMPs to apply to the motors. */