Addressing issue #7 by reading CANCoder values until successful with 10ms delay between readings. Fall back to reading relative encoder.

swervelib/SwerveController.java

@@ -72,11 +72,14 @@ public class SwerveController
    * @param currentHeadingAngleRadians The current robot heading in radians.
    * @return {@link ChassisSpeeds} which can be sent to th Swerve Drive.
    */
-  public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double angle, double currentHeadingAngleRadians)
+  public ChassisSpeeds getTargetSpeeds(
+      double xInput, double yInput, double angle, double currentHeadingAngleRadians)
   {
-    // Calculates an angular rate using a PIDController and the commanded angle.  This is then scaled by
+    // Calculates an angular rate using a PIDController and the commanded angle.  This is then
+    // scaled by
     // the drivebase's maximum angular velocity.
-    double omega = thetaController.calculate(currentHeadingAngleRadians, angle) * config.maxAngularVelocity;
+    double omega =
+        thetaController.calculate(currentHeadingAngleRadians, angle) * config.maxAngularVelocity;
     // Convert joystick inputs to m/s by scaling by max linear speed.  Also uses a cubic function
     // to allow for precise control and fast movement.
     double x = Math.pow(xInput, 3) * config.maxSpeed;
@@ -96,13 +99,20 @@ public class SwerveController
    * @param currentHeadingAngleRadians The current robot heading in radians.
    * @return {@link ChassisSpeeds} which can be sent to th Swerve Drive.
    */
-  public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headingX, double headingY,
+  public ChassisSpeeds getTargetSpeeds(
-                                       double currentHeadingAngleRadians)
+      double xInput,
+      double yInput,
+      double headingX,
+      double headingY,
+      double currentHeadingAngleRadians)
   {
-    // Converts the horizontal and vertical components to the commanded angle, in radians, unless the joystick is near
+    // Converts the horizontal and vertical components to the commanded angle, in radians, unless
-    // the center (i. e. has been released), in which case the angle is held at the last valid joystick input (hold
+    // the joystick is near
+    // the center (i. e. has been released), in which case the angle is held at the last valid
+    // joystick input (hold
     // position when stick released).
-    double        angle  = withinHypotDeadband(headingX, headingY) ? lastAngle : Math.atan2(headingX, headingY);
+    double angle =
+        withinHypotDeadband(headingX, headingY) ? lastAngle : Math.atan2(headingX, headingY);
     ChassisSpeeds speeds = getTargetSpeeds(xInput, yInput, angle, currentHeadingAngleRadians);
 
     // Used for the position hold feature

swervelib/SwerveDrive.java

@@ -11,10 +11,10 @@ import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import frc.robot.Robot;
 import java.util.ArrayList;
 import java.util.List;
 import swervelib.imu.SwerveIMU;
@@ -68,8 +68,8 @@ public class SwerveDrive
   private       int                      moduleSynchronizationCounter = 0;
 
   /**
-   * Creates a new swerve drivebase subsystem.  Robot is controlled via the {@link SwerveDrive#drive} method, or via the
+   * Creates a new swerve drivebase subsystem. Robot is controlled via the {@link SwerveDrive#drive} method, or via the
-   * {@link SwerveDrive#setModuleStates} method.  The {@link SwerveDrive#drive} method incorporates kinematics— it takes
+   * {@link SwerveDrive#setModuleStates} method. The {@link SwerveDrive#drive} method incorporates kinematics-- it takes
    * a translation and rotation, as well as parameters for field-centric and closed-loop velocity control.
    * {@link SwerveDrive#setModuleStates} takes a list of SwerveModuleStates and directly passes them to the modules.
    * This subsystem also handles odometry.
@@ -78,7 +78,8 @@ public class SwerveDrive
    * @param controllerConfig The {@link SwerveControllerConfiguration} to use when creating the
    *                         {@link SwerveController}.
    */
-  public SwerveDrive(SwerveDriveConfiguration config, SwerveControllerConfiguration controllerConfig)
+  public SwerveDrive(
+      SwerveDriveConfiguration config, SwerveControllerConfiguration controllerConfig)
   {
     swerveDriveConfiguration = config;
     swerveController = new SwerveController(controllerConfig);
@@ -87,7 +88,7 @@ public class SwerveDrive
 
     // Create an integrator for angle if the robot is being simulated to emulate an IMU
     // If the robot is real, instantiate the IMU instead.
-    if (!Robot.isReal())
+    if (RobotBase.isSimulation())
     {
       simIMU = new SwerveIMUSimulation();
     } else
@@ -98,44 +99,48 @@ public class SwerveDrive
 
     this.swerveModules = config.modules;
 
-//    odometry = new SwerveDriveOdometry(kinematics, getYaw(), getModulePositions());
+    //    odometry = new SwerveDriveOdometry(kinematics, getYaw(), getModulePositions());
-    swerveDrivePoseEstimator = new SwerveDrivePoseEstimator(
+    swerveDrivePoseEstimator =
-        kinematics,
+        new SwerveDrivePoseEstimator(
-        getYaw(),
+            kinematics,
-        getModulePositions(),
+            getYaw(),
-        new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(0)),
+            getModulePositions(),
-        VecBuilder.fill(0.1, 0.1, 0.1), // x,y,heading in radians; state std dev, higher=less weight
+            new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(0)),
-        VecBuilder.fill(0.9, 0.9, 0.9)); // x,y,heading in radians; Vision measurement std dev, higher=less weight
+            VecBuilder.fill(
+                0.1, 0.1, 0.1), // x,y,heading in radians; state std dev, higher=less weight
+            VecBuilder.fill(
+                0.9, 0.9,
+                0.9)); // x,y,heading in radians; Vision measurement std dev, higher=less weight
 
     zeroGyro();
     SmartDashboard.putData("Field", field);
   }
 
   /**
-   * The primary method for controlling the drivebase.  Takes a Translation2d and a rotation rate, and calculates and
+   * The primary method for controlling the drivebase. Takes a Translation2d and a rotation rate, and calculates and
-   * commands module states accordingly.  Can use either open-loop or closed-loop velocity control for the wheel
+   * commands module states accordingly. Can use either open-loop or closed-loop velocity control for the wheel
-   * velocities.  Also has field- and robot-relative modes, which affect how the translation vector is used.
+   * velocities. Also has field- and robot-relative modes, which affect how the translation vector is used.
    *
    * @param translation   {@link Translation2d} that is the commanded linear velocity of the robot, in meters per
    *                      second. In robot-relative mode, positive x is torwards the bow (front) and positive y is
-   *                      torwards port (left).  In field-relative mode, positive x is away from the alliance wall
+   *                      torwards port (left). In field-relative mode, positive x is away from the alliance wall (field
-   *                      (field North) and positive y is torwards the left wall when looking through the driver station
+   *                      North) and positive y is torwards the left wall when looking through the driver station glass
-   *                      glass (field West).
+   *                      (field West).
-   * @param rotation      Robot angular rate, in radians per second. CCW positive.  Unaffected by field/robot
+   * @param rotation      Robot angular rate, in radians per second. CCW positive. Unaffected by field/robot
    *                      relativity.
-   * @param fieldRelative Drive mode.  True for field-relative, false for robot-relative.
+   * @param fieldRelative Drive mode. True for field-relative, false for robot-relative.
-   * @param isOpenLoop    Whether to use closed-loop velocity control.  Set to true to disable closed-loop.
+   * @param isOpenLoop    Whether to use closed-loop velocity control. Set to true to disable closed-loop.
    */
-  public void drive(Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop)
+  public void drive(
+      Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop)
   {
-    // Creates a robot-relative ChassisSpeeds object, converting from field-relative speeds if necessary.
+    // Creates a robot-relative ChassisSpeeds object, converting from field-relative speeds if
-    ChassisSpeeds velocity = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(translation.getX(),
+    // necessary.
-                                                                                   translation.getY(),
+    ChassisSpeeds velocity =
-                                                                                   rotation,
+        fieldRelative
-                                                                                   getYaw()) : new ChassisSpeeds(
+        ? ChassisSpeeds.fromFieldRelativeSpeeds(
-        translation.getX(),
+            translation.getX(), translation.getY(), rotation, getYaw())
-        translation.getY(),
+        : new ChassisSpeeds(translation.getX(), translation.getY(), rotation);
-        rotation);
 
     // Display commanded speed for testing
     SmartDashboard.putString("RobotVelocity", velocity.toString());
@@ -147,10 +152,10 @@ public class SwerveDrive
   }
 
   /**
-   * Set the module states (azimuth and velocity) directly.  Used primarily for auto pathing.
+   * Set the module states (azimuth and velocity) directly. Used primarily for auto pathing.
    *
    * @param desiredStates A list of SwerveModuleStates to send to the modules.
-   * @param isOpenLoop    Whether to use closed-loop velocity control.  Set to true to disable closed-loop.
+   * @param isOpenLoop    Whether to use closed-loop velocity control. Set to true to disable closed-loop.
    */
   public void setModuleStates(SwerveModuleState2[] desiredStates, boolean isOpenLoop)
   {
@@ -161,10 +166,12 @@ public class SwerveDrive
     for (SwerveModule module : swerveModules)
     {
       module.setDesiredState(desiredStates[module.moduleNumber], isOpenLoop);
-      SmartDashboard.putNumber("Module " + module.moduleNumber + " Speed Setpoint: ",
+      SmartDashboard.putNumber(
-                               desiredStates[module.moduleNumber].speedMetersPerSecond);
+          "Module " + module.moduleNumber + " Speed Setpoint: ",
-      SmartDashboard.putNumber("Module " + module.moduleNumber + " Angle Setpoint: ",
+          desiredStates[module.moduleNumber].speedMetersPerSecond);
-                               desiredStates[module.moduleNumber].angle.getDegrees());
+      SmartDashboard.putNumber(
+          "Module " + module.moduleNumber + " Angle Setpoint: ",
+          desiredStates[module.moduleNumber].angle.getDegrees());
     }
   }
 
@@ -175,8 +182,7 @@ public class SwerveDrive
    */
   public void setChassisSpeeds(ChassisSpeeds chassisSpeeds)
   {
-    setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds),
+    setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds), false);
-                    false);
   }
 
   /**
@@ -197,9 +203,11 @@ public class SwerveDrive
   public ChassisSpeeds getFieldVelocity()
   {
     // ChassisSpeeds has a method to convert from field-relative to robot-relative speeds,
-    // but not the reverse.  However, because this transform is a simple rotation, negating the angle
+    // but not the reverse.  However, because this transform is a simple rotation, negating the
+    // angle
     // given as the robot angle reverses the direction of rotation, and the conversion is reversed.
-    return ChassisSpeeds.fromFieldRelativeSpeeds(kinematics.toChassisSpeeds(getStates()), getYaw().unaryMinus());
+    return ChassisSpeeds.fromFieldRelativeSpeeds(
+        kinematics.toChassisSpeeds(getStates()), getYaw().unaryMinus());
   }
 
   /**
@@ -212,10 +220,9 @@ public class SwerveDrive
     return kinematics.toChassisSpeeds(getStates());
   }
 
-
   /**
    * Resets odometry to the given pose. Gyro angle and module positions do not need to be reset when calling this
-   * method.  However, if either gyro angle or module position is reset, this must be called in order for odometry to
+   * method. However, if either gyro angle or module position is reset, this must be called in order for odometry to
    * keep working.
    *
    * @param pose The pose to set the odometry to
@@ -257,7 +264,8 @@ public class SwerveDrive
    */
   public SwerveModulePosition[] getModulePositions()
   {
-    SwerveModulePosition[] positions = new SwerveModulePosition[swerveDriveConfiguration.moduleCount];
+    SwerveModulePosition[] positions =
+        new SwerveModulePosition[swerveDriveConfiguration.moduleCount];
     for (SwerveModule module : swerveModules)
     {
       positions[module.moduleNumber] = module.getPosition();
@@ -270,8 +278,9 @@ public class SwerveDrive
    */
   public void zeroGyro()
   {
-    // Resets the real gyro or the angle accumulator, depending on whether the robot is being simulated
+    // Resets the real gyro or the angle accumulator, depending on whether the robot is being
-    if (Robot.isReal())
+    // simulated
+    if (!RobotBase.isSimulation())
     {
       imu.setYaw(0);
     } else
@@ -283,14 +292,14 @@ public class SwerveDrive
   }
 
   /**
-   * Gets the current yaw angle of the robot, as reported by the imu.  CCW positive, not wrapped.
+   * Gets the current yaw angle of the robot, as reported by the imu. CCW positive, not wrapped.
    *
    * @return The yaw as a {@link Rotation2d} angle
    */
   public Rotation2d getYaw()
   {
     // Read the imu if the robot is real or the accumulator if the robot is simulated.
-    if (Robot.isReal())
+    if (!RobotBase.isSimulation())
     {
       double[] ypr = new double[3];
       imu.getYawPitchRoll(ypr);
@@ -309,7 +318,7 @@ public class SwerveDrive
   public Rotation2d getPitch()
   {
     // Read the imu if the robot is real or the accumulator if the robot is simulated.
-    if (Robot.isReal())
+    if (!RobotBase.isSimulation())
     {
       double[] ypr = new double[3];
       imu.getYawPitchRoll(ypr);
@@ -328,7 +337,7 @@ public class SwerveDrive
   public Rotation2d getRoll()
   {
     // Read the imu if the robot is real or the accumulator if the robot is simulated.
-    if (Robot.isReal())
+    if (!RobotBase.isSimulation())
     {
       double[] ypr = new double[3];
       imu.getYawPitchRoll(ypr);
@@ -347,7 +356,7 @@ public class SwerveDrive
   public Rotation3d getGyroRotation3d()
   {
     // Read the imu if the robot is real or the accumulator if the robot is simulated.
-    if (Robot.isReal())
+    if (!RobotBase.isSimulation())
     {
       double[] ypr = new double[3];
       imu.getYawPitchRoll(ypr);
@@ -382,9 +391,8 @@ public class SwerveDrive
   {
     for (SwerveModule swerveModule : swerveModules)
     {
-      swerveModule.setDesiredState(new SwerveModuleState2(0,
+      swerveModule.setDesiredState(
-                                                          swerveModule.configuration.moduleLocation.getAngle(),
+          new SwerveModuleState2(0, swerveModule.configuration.moduleLocation.getAngle(), 0), true);
-                                                          0), true);
     }
   }
 
@@ -400,7 +408,9 @@ public class SwerveDrive
     List<Pose2d> poses   = new ArrayList<>();
     for (SwerveModule module : swerveModules)
     {
-      poses.add(robotPose.plus(new Transform2d(module.configuration.moduleLocation, module.getState().angle)));
+      poses.add(
+          robotPose.plus(
+              new Transform2d(module.configuration.moduleLocation, module.getState().angle)));
     }
     return poses.toArray(poseArr);
   }
@@ -429,10 +439,13 @@ public class SwerveDrive
     swerveDrivePoseEstimator.update(getYaw(), getModulePositions());
 
     // Update angle accumulator if the robot is simulated
-    if (!Robot.isReal())
+    if (RobotBase.isSimulation())
     {
-      simIMU.updateOdometry(kinematics, getStates(),
+      simIMU.updateOdometry(
-                            getSwerveModulePoses(swerveDrivePoseEstimator.getEstimatedPosition()), field);
+          kinematics,
+          getStates(),
+          getSwerveModulePoses(swerveDrivePoseEstimator.getEstimatedPosition()),
+          field);
     }
 
     field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition());
@@ -446,19 +459,21 @@ public class SwerveDrive
       moduleStates[module.moduleNumber + 1] = moduleState.speedMetersPerSecond;
       sumOmega += Math.abs(moduleState.omegaRadPerSecond);
 
-      SmartDashboard.putNumber("Module" + module.moduleNumber + "Relative Encoder", module.getRelativePosition());
+      SmartDashboard.putNumber(
-      SmartDashboard.putNumber("Module" + module.moduleNumber + "Absolute Encoder", module.getAbsolutePosition());
+          "Module" + module.moduleNumber + "Relative Encoder", module.getRelativePosition());
+      SmartDashboard.putNumber(
+          "Module" + module.moduleNumber + "Absolute Encoder", module.getAbsolutePosition());
     }
     SmartDashboard.putNumberArray("moduleStates", moduleStates);
 
-    // If the robot isn't moving synchronize the encoders every 100ms (Inspired by democrat's SDS lib)
+    // If the robot isn't moving synchronize the encoders every 100ms (Inspired by democrat's SDS
+    // lib)
     // To ensure that everytime we initialize it works.
     if (sumOmega <= .01 && ++moduleSynchronizationCounter > 5)
     {
       synchronizeModuleEncoders();
       moduleSynchronizationCounter = 0;
     }
-
   }
 
   /**
@@ -492,10 +507,11 @@ public class SwerveDrive
       swerveDrivePoseEstimator.addVisionMeasurement(robotPose, timestamp);
     } else
     {
-      swerveDrivePoseEstimator.resetPosition(robotPose.getRotation(), getModulePositions(), robotPose);
+      swerveDrivePoseEstimator.resetPosition(
+          robotPose.getRotation(), getModulePositions(), robotPose);
     }
 
-    if (Robot.isReal())
+    if (!RobotBase.isSimulation())
     {
       imu.setYaw(swerveDrivePoseEstimator.getEstimatedPosition().getRotation().getDegrees());
       // Yaw reset recommended by Team 1622

swervelib/SwerveModule.java

@@ -4,8 +4,8 @@ import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 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.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import frc.robot.Robot;
 import swervelib.encoders.SwerveAbsoluteEncoder;
 import swervelib.math.SwerveModuleState2;
 import swervelib.motors.SwerveMotor;
@@ -59,10 +59,10 @@ public class SwerveModule
    */
   public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfiguration)
   {
-//    angle = 0;
+    //    angle = 0;
-//    speed = 0;
+    //    speed = 0;
-//    omega = 0;
+    //    omega = 0;
-//    fakePos = 0;
+    //    fakePos = 0;
     this.moduleNumber = moduleNumber;
     configuration = moduleConfiguration;
     angleOffset = moduleConfiguration.angleOffset;
@@ -90,7 +90,8 @@ public class SwerveModule
     {
       absoluteEncoder.factoryDefault();
       absoluteEncoder.configure(moduleConfiguration.absoluteEncoderInverted);
-      angleMotor.configureIntegratedEncoder(moduleConfiguration.getPositionEncoderConversion(false));
+      angleMotor.configureIntegratedEncoder(
+          moduleConfiguration.getPositionEncoderConversion(false));
       angleMotor.setPosition(absoluteEncoder.getAbsolutePosition() - angleOffset);
     }
 
@@ -109,7 +110,7 @@ public class SwerveModule
     driveMotor.burnFlash();
     angleMotor.burnFlash();
 
-    if (!Robot.isReal())
+    if (RobotBase.isSimulation())
     {
       simModule = new SwerveModuleSimulation();
     }
@@ -124,7 +125,7 @@ public class SwerveModule
   {
     if (absoluteEncoder != null)
     {
-      angleMotor.setPosition(absoluteEncoder.getAbsolutePosition() - angleOffset);
+      angleMotor.setPosition(getAbsolutePosition() - angleOffset);
     }
   }
 
@@ -136,14 +137,19 @@ public class SwerveModule
    */
   public void setDesiredState(SwerveModuleState2 desiredState, boolean isOpenLoop)
   {
-    SwerveModuleState simpleState = new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle);
+    SwerveModuleState simpleState =
+        new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle);
     simpleState = SwerveModuleState.optimize(simpleState, getState().angle);
-    desiredState = new SwerveModuleState2(simpleState.speedMetersPerSecond, simpleState.angle,
+    desiredState =
-                                          desiredState.omegaRadPerSecond);
+        new SwerveModuleState2(
+            simpleState.speedMetersPerSecond, simpleState.angle, desiredState.omegaRadPerSecond);
 
-    SmartDashboard.putNumber("Optimized " + moduleNumber + " Speed Setpoint: ", desiredState.speedMetersPerSecond);
+    SmartDashboard.putNumber(
-    SmartDashboard.putNumber("Optimized " + moduleNumber + " Angle Setpoint: ", desiredState.angle.getDegrees());
+        "Optimized " + moduleNumber + " Speed Setpoint: ", desiredState.speedMetersPerSecond);
-    SmartDashboard.putNumber("Module " + moduleNumber + " Omega: ", Math.toDegrees(desiredState.omegaRadPerSecond));
+    SmartDashboard.putNumber(
+        "Optimized " + moduleNumber + " Angle Setpoint: ", desiredState.angle.getDegrees());
+    SmartDashboard.putNumber(
+        "Module " + moduleNumber + " Omega: ", Math.toDegrees(desiredState.omegaRadPerSecond));
 
     if (isOpenLoop)
     {
@@ -156,17 +162,18 @@ public class SwerveModule
     }
 
     // Prevents module rotation if speed is less than 1%
-    double angle = (Math.abs(desiredState.speedMetersPerSecond) <= (configuration.maxSpeed * 0.01) ?
+    double angle =
-                    lastAngle :
+        (Math.abs(desiredState.speedMetersPerSecond) <= (configuration.maxSpeed * 0.01)
-                    desiredState.angle.getDegrees());
+         ? lastAngle
-    angleMotor.setReference(angle, Math.toDegrees(desiredState.omegaRadPerSecond) * configuration.angleKV);
+         : desiredState.angle.getDegrees());
+    angleMotor.setReference(
+        angle, Math.toDegrees(desiredState.omegaRadPerSecond) * configuration.angleKV);
     lastAngle = angle;
 
-    if (!Robot.isReal())
+    if (RobotBase.isSimulation())
     {
       simModule.updateStateAndPosition(desiredState);
     }
-
   }
 
   /**
@@ -190,7 +197,7 @@ public class SwerveModule
     double     velocity;
     Rotation2d azimuth;
     double     omega;
-    if (Robot.isReal())
+    if (!RobotBase.isSimulation())
     {
       velocity = driveMotor.getVelocity();
       azimuth = Rotation2d.fromDegrees(angleMotor.getPosition());
@@ -211,7 +218,7 @@ public class SwerveModule
   {
     double     position;
     Rotation2d azimuth;
-    if (Robot.isReal())
+    if (!RobotBase.isSimulation())
     {
       position = driveMotor.getPosition();
       azimuth = Rotation2d.fromDegrees(angleMotor.getPosition());
@@ -230,7 +237,13 @@ public class SwerveModule
    */
   public double getAbsolutePosition()
   {
-    return absoluteEncoder.getAbsolutePosition();
+    double angle = absoluteEncoder.getAbsolutePosition();
+    if (absoluteEncoder.readingError)
+    {
+      angle = getRelativePosition();
+    }
+
+    return angle;
   }
 
   /**

swervelib/encoders/CANCoderSwerve.java

@@ -1,5 +1,6 @@
 package swervelib.encoders;
 
+import com.ctre.phoenix.ErrorCode;
 import com.ctre.phoenix.sensors.AbsoluteSensorRange;
 import com.ctre.phoenix.sensors.CANCoderConfiguration;
 import com.ctre.phoenix.sensors.MagnetFieldStrength;
@@ -69,24 +70,61 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
     CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
     canCoderConfiguration.absoluteSensorRange = AbsoluteSensorRange.Unsigned_0_to_360;
     canCoderConfiguration.sensorDirection = inverted;
-    canCoderConfiguration.initializationStrategy = SensorInitializationStrategy.BootToAbsolutePosition;
+    canCoderConfiguration.initializationStrategy =
+        SensorInitializationStrategy.BootToAbsolutePosition;
     canCoderConfiguration.sensorTimeBase = SensorTimeBase.PerSecond;
     encoder.configAllSettings(canCoderConfiguration);
   }
 
   /**
-   * Get the absolute position of the encoder.
+   * Get the absolute position of the encoder. Sets {@link SwerveAbsoluteEncoder#readingError} on erroneous readings.
    *
    * @return Absolute position in degrees from [0, 360).
    */
   @Override
   public double getAbsolutePosition()
   {
-    if (encoder.getMagnetFieldStrength() != MagnetFieldStrength.Good_GreenLED)
+    readingError = false;
+    MagnetFieldStrength strength = encoder.getMagnetFieldStrength();
+
+    if (strength != MagnetFieldStrength.Good_GreenLED)
     {
-      DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.\n", false);
+      DriverStation.reportWarning(
+          "CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.\n", false);
     }
-    return encoder.getAbsolutePosition();
+    if (strength == MagnetFieldStrength.Invalid_Unknown || strength == MagnetFieldStrength.BadRange_RedLED)
+    {
+      readingError = true;
+      return 0;
+    }
+    double angle = encoder.getAbsolutePosition();
+
+    // Taken from democat's library.
+    // Source: https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/CanCoderFactoryBuilder.java#L51-L74
+    ErrorCode code     = encoder.getLastError();
+    int       ATTEMPTS = 3;
+    for (int i = 0; i < ATTEMPTS; i++)
+    {
+      if (code == ErrorCode.OK)
+      {
+        break;
+      }
+      try
+      {
+        Thread.sleep(10);
+      } catch (InterruptedException e)
+      {
+      }
+      angle = encoder.getAbsolutePosition();
+      code = encoder.getLastError();
+    }
+    if (code != ErrorCode.OK)
+    {
+      readingError = true;
+      DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " reading was faulty, ignoring.\n", false);
+    }
+
+    return angle;
   }
 
   /**

swervelib/encoders/SwerveAbsoluteEncoder.java

@@ -6,6 +6,11 @@ package swervelib.encoders;
 public abstract class SwerveAbsoluteEncoder
 {
 
+  /**
+   * Last angle reading was faulty.
+   */
+  public boolean readingError = false;
+
   /**
    * Reset the encoder to factory defaults.
    */

swervelib/encoders/package-info.java

@@ -1,4 +1,4 @@
 /**
  * Absolute encoders for the swerve drive, all implement {@link swervelib.encoders.SwerveAbsoluteEncoder}.
  */
-package swervelib.encoders;
+package swervelib.encoders;

swervelib/imu/ADIS16448Swerve.java

@@ -81,4 +81,3 @@ public class ADIS16448Swerve extends SwerveIMU
     return imu;
   }
 }
-

swervelib/imu/ADXRS450Swerve.java

@@ -80,5 +80,4 @@ public class ADXRS450Swerve extends SwerveIMU
   {
     return imu;
   }
-
 }

swervelib/imu/AnalogGyroSwerve.java

@@ -27,7 +27,8 @@ public class AnalogGyroSwerve extends SwerveIMU
   {
     if (!(channel == 0 || channel == 1))
     {
-      throw new RuntimeException("Analog Gyroscope must be attached to port 0 or 1 on the roboRIO.\n");
+      throw new RuntimeException(
+          "Analog Gyroscope must be attached to port 0 or 1 on the roboRIO.\n");
     }
     gyro = new AnalogGyro(channel);
     SmartDashboard.putData(gyro);

swervelib/imu/Pigeon2Swerve.java

@@ -15,7 +15,6 @@ public class Pigeon2Swerve extends SwerveIMU
    */
   WPI_Pigeon2 imu;
 
-
   /**
    * Generate the SwerveIMU for pigeon.
    *

swervelib/imu/package-info.java

@@ -1,4 +1,4 @@
 /**
  * IMUs used for controlling the robot heading. All implement {@link swervelib.imu.SwerveIMU}.
  */
-package swervelib.imu;
+package swervelib.imu;

swervelib/math/SwerveKinematics2.java

@@ -76,8 +76,10 @@ public class SwerveKinematics2 extends SwerveDriveKinematics
     {
       m_inverseKinematics.setRow(i * 2, 0, /* Start Data */ 1, 0, -m_modules[i].getY());
       m_inverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, +m_modules[i].getX());
-      bigInverseKinematics.setRow(i * 2, 0, /* Start Data */ 1, 0, -m_modules[i].getX(), -m_modules[i].getY());
+      bigInverseKinematics.setRow(
-      bigInverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, -m_modules[i].getY(), +m_modules[i].getX());
+          i * 2, 0, /* Start Data */ 1, 0, -m_modules[i].getX(), -m_modules[i].getY());
+      bigInverseKinematics.setRow(
+          i * 2 + 1, 0, /* Start Data */ 0, 1, -m_modules[i].getY(), +m_modules[i].getX());
     }
     m_forwardKinematics = m_inverseKinematics.pseudoInverse();
 
@@ -197,11 +199,7 @@ public class SwerveKinematics2 extends SwerveDriveKinematics
       for (int i = 0; i < m_numModules; i++)
       {
         m_inverseKinematics.setRow(
-            i * 2,
+            i * 2, 0, /* Start Data */ 1, 0, -m_modules[i].getY() + centerOfRotationMeters.getY());
-            0, /* Start Data */
-            1,
-            0,
-            -m_modules[i].getY() + centerOfRotationMeters.getY());
         m_inverseKinematics.setRow(
             i * 2 + 1,
             0, /* Start Data */
@@ -237,14 +235,7 @@ public class SwerveKinematics2 extends SwerveDriveKinematics
     var moduleVelocityStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
 
     var accelerationVector = new SimpleMatrix(4, 1);
-    accelerationVector.setColumn(
+    accelerationVector.setColumn(0, 0, 0, 0, Math.pow(chassisSpeeds.omegaRadiansPerSecond, 2), 0);
-        0,
-        0,
-        0,
-        0,
-        Math.pow(chassisSpeeds.omegaRadiansPerSecond, 2),
-        0
-                                );
 
     var moduleAccelerationStatesMatrix = bigInverseKinematics.mult(accelerationVector);
 
@@ -260,26 +251,11 @@ public class SwerveKinematics2 extends SwerveDriveKinematics
       Rotation2d angle = new Rotation2d(x, y);
 
       var trigThetaAngle = new SimpleMatrix(2, 2);
-      trigThetaAngle.setColumn(
+      trigThetaAngle.setColumn(0, 0, angle.getCos(), -angle.getSin());
-          0,
+      trigThetaAngle.setColumn(1, 0, angle.getSin(), angle.getCos());
-          0,
-          angle.getCos(),
-          -angle.getSin()
-                              );
-      trigThetaAngle.setColumn(
-          1,
-          0,
-          angle.getSin(),
-          angle.getCos()
-                              );
 
       var accelVector = new SimpleMatrix(2, 1);
-      accelVector.setColumn(
+      accelVector.setColumn(0, 0, ax, ay);
-          0,
-          0,
-          ax,
-          ay
-                           );
 
       var omegaVector = trigThetaAngle.mult(accelVector);
 

swervelib/math/SwerveMath.java

@@ -19,14 +19,15 @@ public class SwerveMath
 
   /**
    * Calculate the angle kV which will be multiplied by the radians per second for the feedforward. Volt * seconds /
-   * degree <=> (maxVolts) / (maxSpeed)
+   * degree == (maxVolts) / (maxSpeed)
    *
    * @param optimalVoltage    Optimal voltage to use when calculating the angle kV.
    * @param motorFreeSpeedRPM Motor free speed in Rotations per Minute.
    * @param angleGearRatio    Angle gear ratio, the amount of times the motor as to turn for the wheel rotation.
    * @return angle kV for feedforward.
    */
-  public static double calculateAngleKV(double optimalVoltage, double motorFreeSpeedRPM, double angleGearRatio)
+  public static double calculateAngleKV(
+      double optimalVoltage, double motorFreeSpeedRPM, double angleGearRatio)
   {
     double maxAngularVelocity = 360 * (motorFreeSpeedRPM / angleGearRatio) / 60; // deg/s
     return optimalVoltage / maxAngularVelocity;
@@ -41,7 +42,8 @@ public class SwerveMath
    * @param pulsePerRotation The number of encoder pulses per rotation. 1 if using an integrated encoder.
    * @return Meters per rotation for the drive motor.
    */
-  public static double calculateMetersPerRotation(double wheelDiameter, double driveGearRatio, double pulsePerRotation)
+  public static double calculateMetersPerRotation(
+      double wheelDiameter, double driveGearRatio, double pulsePerRotation)
   {
     return (Math.PI * wheelDiameter) / (driveGearRatio * pulsePerRotation);
   }
@@ -69,18 +71,21 @@ public class SwerveMath
   public static double applyDeadband(double value, boolean scaled, double deadband)
   {
     value = Math.abs(value) > deadband ? value : 0;
-    return scaled ? ((1 / (1 - deadband)) * (Math.abs(value) - deadband)) * Math.signum(value) : value;
+    return scaled
+           ? ((1 / (1 - deadband)) * (Math.abs(value) - deadband)) * Math.signum(value)
+           : value;
   }
 
   /**
-   * Calculate the degrees per steering rotation for the integrated encoder. Encoder conversion values.  Drive converts
+   * Calculate the degrees per steering rotation for the integrated encoder. Encoder conversion values. Drive converts
    * motor rotations to linear wheel distance and steering converts motor rotations to module azimuth.
    *
    * @param angleGearRatio   The gear ratio of the steering motor.
    * @param pulsePerRotation The number of pulses in a complete rotation for the encoder, 1 if integrated.
    * @return Degrees per steering rotation for the angle motor.
    */
-  public static double calculateDegreesPerSteeringRotation(double angleGearRatio, double pulsePerRotation)
+  public static double calculateDegreesPerSteeringRotation(
+      double angleGearRatio, double pulsePerRotation)
   {
     return 360 / (angleGearRatio * pulsePerRotation);
   }
@@ -93,7 +98,8 @@ public class SwerveMath
    * @param furthestModuleY Y of the furthest module in meters.
    * @return Maximum angular velocity in rad/s.
    */
-  public static double calculateMaxAngularVelocity(double maxSpeed, double furthestModuleX, double furthestModuleY)
+  public static double calculateMaxAngularVelocity(
+      double maxSpeed, double furthestModuleX, double furthestModuleY)
   {
     return maxSpeed / Math.hypot(furthestModuleX, furthestModuleY);
   }
@@ -119,8 +125,12 @@ public class SwerveMath
    * @param robotMass     Mass of the robot in kg.
    * @return Theoretical maximum acceleration in m/s/s.
    */
-  public static double calculateMaxAcceleration(double stallTorqueNm, double gearRatio, double moduleCount,
+  public static double calculateMaxAcceleration(
-                                                double wheelDiameter, double robotMass)
+      double stallTorqueNm,
+      double gearRatio,
+      double moduleCount,
+      double wheelDiameter,
+      double robotMass)
   {
     return (stallTorqueNm * gearRatio * moduleCount) / ((wheelDiameter / 2) * robotMass);
   }
@@ -137,50 +147,60 @@ public class SwerveMath
    * @param config                 The swerve drive configuration.
    * @return Maximum acceleration allowed in the robot direction.
    */
-  private static double calcMaxAccel(Rotation2d angle, double chassisMass, double robotMass,
+  private static double calcMaxAccel(
-                                     Translation3d chassisCenterOfGravity, SwerveDriveConfiguration config)
+      Rotation2d angle,
+      double chassisMass,
+      double robotMass,
+      Translation3d chassisCenterOfGravity,
+      SwerveDriveConfiguration config)
   {
     double xMoment = (chassisCenterOfGravity.getX() * chassisMass);
     double yMoment = (chassisCenterOfGravity.getY() * chassisMass);
-    // Calculate the vertical mass moment using the floor as the datum.  This will be used later to calculate max
+    // Calculate the vertical mass moment using the floor as the datum.  This will be used later to
+    // calculate max
     // acceleration
     double        zMoment      = (chassisCenterOfGravity.getZ() * (chassisMass));
     Translation3d robotCG      = new Translation3d(xMoment, yMoment, zMoment).div(robotMass);
     Translation2d horizontalCG = robotCG.toTranslation2d();
 
-    Translation2d projectedHorizontalCg = new Translation2d(
+    Translation2d projectedHorizontalCg =
-        (angle.getSin() * angle.getCos() * horizontalCG.getY()) + (Math.pow(angle.getCos(), 2) * horizontalCG.getX()),
+        new Translation2d(
-        (angle.getSin() * angle.getCos() * horizontalCG.getX()) + (Math.pow(angle.getSin(), 2) * horizontalCG.getY())
+            (angle.getSin() * angle.getCos() * horizontalCG.getY())
-    );
+            + (Math.pow(angle.getCos(), 2) * horizontalCG.getX()),
+            (angle.getSin() * angle.getCos() * horizontalCG.getX())
+            + (Math.pow(angle.getSin(), 2) * horizontalCG.getY()));
 
-    // Projects the edge of the wheelbase onto the direction line.  Assumes the wheelbase is rectangular.
+    // Projects the edge of the wheelbase onto the direction line.  Assumes the wheelbase is
-    // Because a line is being projected, rather than a point, one of the coordinates of the projected point is
+    // rectangular.
+    // Because a line is being projected, rather than a point, one of the coordinates of the
+    // projected point is
     // already known.
     Translation2d projectedWheelbaseEdge;
     double        angDeg = angle.getDegrees();
     if (angDeg <= 45 && angDeg >= -45)
     {
       SwerveModuleConfiguration conf = getSwerveModule(config.modules, true, true);
-      projectedWheelbaseEdge = new Translation2d(conf.moduleLocation.getX(),
+      projectedWheelbaseEdge =
-                                                 conf.moduleLocation.getX() * angle.getTan());
+          new Translation2d(
+              conf.moduleLocation.getX(), conf.moduleLocation.getX() * angle.getTan());
     } else if (135 >= angDeg && angDeg > 45)
     {
       SwerveModuleConfiguration conf = getSwerveModule(config.modules, true, true);
-      projectedWheelbaseEdge = new Translation2d(
+      projectedWheelbaseEdge =
-          conf.moduleLocation.getY() / angle.getTan(),
+          new Translation2d(
-          conf.moduleLocation.getY());
+              conf.moduleLocation.getY() / angle.getTan(), conf.moduleLocation.getY());
     } else if (-135 <= angDeg && angDeg < -45)
     {
       SwerveModuleConfiguration conf = getSwerveModule(config.modules, true, false);
-      projectedWheelbaseEdge = new Translation2d(
+      projectedWheelbaseEdge =
-          conf.moduleLocation.getY() / angle.getTan(),
+          new Translation2d(
-          conf.moduleLocation.getY());
+              conf.moduleLocation.getY() / angle.getTan(), conf.moduleLocation.getY());
     } else
     {
       SwerveModuleConfiguration conf = getSwerveModule(config.modules, false, true);
-      projectedWheelbaseEdge = new Translation2d(
+      projectedWheelbaseEdge =
-          conf.moduleLocation.getX(),
+          new Translation2d(
-          conf.moduleLocation.getX() * angle.getTan());
+              conf.moduleLocation.getX(), conf.moduleLocation.getX() * angle.getTan());
     }
 
     double horizontalDistance = projectedHorizontalCg.plus(projectedWheelbaseEdge).getNorm();
@@ -192,7 +212,7 @@ public class SwerveMath
 
   /**
    * Limits a commanded velocity to prevent exceeding the maximum acceleration given by
-   * {@link SwerveMath#calcMaxAccel(Rotation2d, double, double, Translation3d, SwerveDriveConfiguration)}.  Note that
+   * {@link SwerveMath#calcMaxAccel(Rotation2d, double, double, Translation3d, SwerveDriveConfiguration)}. Note that
    * this takes and returns field-relative velocities.
    *
    * @param commandedVelocity      The desired velocity
@@ -204,12 +224,18 @@ public class SwerveMath
    * @param robotMass              The weight of the robot in kg. (Including manipulators, etc).
    * @param chassisCenterOfGravity Chassis center of gravity.
    * @param config                 The swerve drive configuration.
-   * @return The limited velocity.  This is either the commanded velocity, if attainable, or the closest attainable
+   * @return The limited velocity. This is either the commanded velocity, if attainable, or the closest attainable
    * velocity.
    */
-  public static Translation2d limitVelocity(Translation2d commandedVelocity, ChassisSpeeds fieldVelocity,
+  public static Translation2d limitVelocity(
-                                            Pose2d robotPose, double loopTime, double chassisMass, double robotMass,
+      Translation2d commandedVelocity,
-                                            Translation3d chassisCenterOfGravity, SwerveDriveConfiguration config)
+      ChassisSpeeds fieldVelocity,
+      Pose2d robotPose,
+      double loopTime,
+      double chassisMass,
+      double robotMass,
+      Translation3d chassisCenterOfGravity,
+      SwerveDriveConfiguration config)
   {
     // Get the robot's current field-relative velocity
     Translation2d currentVelocity = SwerveController.getTranslation2d(fieldVelocity);
@@ -222,12 +248,18 @@ public class SwerveMath
 
     // Creates an acceleration vector with the direction of delta V and a magnitude
     // of the maximum allowed acceleration in that direction
-    Translation2d maxAccel = new Translation2d(
+    Translation2d maxAccel =
-        calcMaxAccel(deltaV
+        new Translation2d(
-                         // Rotates the velocity vector to convert from field-relative to robot-relative
+            calcMaxAccel(
-                         .rotateBy(robotPose.getRotation().unaryMinus())
+                deltaV
-                         .getAngle(), chassisMass, robotMass, chassisCenterOfGravity, config),
+                    // Rotates the velocity vector to convert from field-relative to robot-relative
-        deltaV.getAngle());
+                    .rotateBy(robotPose.getRotation().unaryMinus())
+                    .getAngle(),
+                chassisMass,
+                robotMass,
+                chassisCenterOfGravity,
+                config),
+            deltaV.getAngle());
 
     // Calculate the maximum achievable velocity by the next loop cycle.
     // delta V = Vf - Vi = at
@@ -251,17 +283,22 @@ public class SwerveMath
    * @param left    True = furthest left, False = furthest right.
    * @return Module location which is the furthest from center and abides by parameters.
    */
-  public static SwerveModuleConfiguration getSwerveModule(SwerveModule[] modules, boolean front, boolean left)
+  public static SwerveModuleConfiguration getSwerveModule(
+      SwerveModule[] modules, boolean front, boolean left)
   {
     Translation2d             target        = modules[0].configuration.moduleLocation, current, temp;
     SwerveModuleConfiguration configuration = modules[0].configuration;
     for (SwerveModule module : modules)
     {
       current = module.configuration.moduleLocation;
-      temp = front ? (target.getY() >= current.getY() ? current : target)
+      temp =
-                   : (target.getY() <= current.getY() ? current : target);
+          front
-      target = left ? (target.getX() >= temp.getX() ? temp : target)
+          ? (target.getY() >= current.getY() ? current : target)
-                    : (target.getX() <= temp.getX() ? temp : target);
+          : (target.getY() <= current.getY() ? current : target);
+      target =
+          left
+          ? (target.getX() >= temp.getX() ? temp : target)
+          : (target.getX() <= temp.getX() ? temp : target);
       configuration = current.equals(target) ? module.configuration : configuration;
     }
     return configuration;

swervelib/math/SwerveModuleState2.java

@@ -36,7 +36,8 @@ public class SwerveModuleState2 extends SwerveModuleState
    * @param angle                The angle of the module.
    * @param omegaRadPerSecond    The angular velocity of the module.
    */
-  public SwerveModuleState2(double speedMetersPerSecond, Rotation2d angle, double omegaRadPerSecond)
+  public SwerveModuleState2(
+      double speedMetersPerSecond, Rotation2d angle, double omegaRadPerSecond)
   {
     this.speedMetersPerSecond = speedMetersPerSecond;
     this.angle = angle;

swervelib/math/package-info.java

@@ -1,4 +1,4 @@
 /**
  * Mathematics for swerve drives.
  */
-package swervelib.math;
+package swervelib.math;

swervelib/motors/SparkMaxSwerve.java

@@ -38,7 +38,6 @@ public class SparkMaxSwerve extends SwerveMotor
    */
   private boolean               factoryDefaultOccurred = false;
 
-
   /**
    * Initialize the swerve motor.
    *
@@ -54,7 +53,8 @@ public class SparkMaxSwerve extends SwerveMotor
 
     encoder = motor.getEncoder();
     pid = motor.getPIDController();
-    pid.setFeedbackDevice(encoder); // Configure feedback of the PID controller as the integrated encoder.
+    pid.setFeedbackDevice(
+        encoder); // Configure feedback of the PID controller as the integrated encoder.
 
     motor.setCANTimeout(0); // Spin off configurations in a different thread.
   }
@@ -179,12 +179,17 @@ public class SparkMaxSwerve extends SwerveMotor
       encoder.setPositionConversionFactor(positionConversionFactor);
       encoder.setVelocityConversionFactor(positionConversionFactor / 60);
 
-      // Taken from https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67
+      // Taken from
+      // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67
       configureCANStatusFrames(10, 20, 20, 500, 500);
     } else
     {
       absoluteEncoder.setPositionConversionFactor(positionConversionFactor);
       absoluteEncoder.setVelocityConversionFactor(positionConversionFactor / 60);
+      if (!isAttachedAbsoluteEncoder())
+      {
+        configureCANStatusFrames(10, 20, 20, 500, 500);
+      }
     }
   }
 
@@ -196,7 +201,8 @@ public class SparkMaxSwerve extends SwerveMotor
   @Override
   public void configurePIDF(PIDFConfig config)
   {
-    int pidSlot = isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
+    int pidSlot =
+        isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
     pid.setP(config.p, pidSlot);
     pid.setI(config.i, pidSlot);
     pid.setD(config.d, pidSlot);
@@ -228,7 +234,8 @@ public class SparkMaxSwerve extends SwerveMotor
    * @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position
    * @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position
    */
-  public void configureCANStatusFrames(int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4)
+  public void configureCANStatusFrames(
+      int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4)
   {
     motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0);
     motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1);
@@ -290,8 +297,13 @@ public class SparkMaxSwerve extends SwerveMotor
   @Override
   public void setReference(double setpoint, double feedforward)
   {
-    int pidSlot = isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
+    int pidSlot =
-    pid.setReference(setpoint, isDriveMotor ? ControlType.kVelocity : ControlType.kPosition, pidSlot, feedforward);
+        isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
+    pid.setReference(
+        setpoint,
+        isDriveMotor ? ControlType.kVelocity : ControlType.kPosition,
+        pidSlot,
+        feedforward);
   }
 
   /**

swervelib/motors/TalonFXSwerve.java

@@ -5,7 +5,7 @@ import com.ctre.phoenix.motorcontrol.DemandType;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
-import frc.robot.Robot;
+import edu.wpi.first.wpilibj.RobotBase;
 import swervelib.encoders.SwerveAbsoluteEncoder;
 import swervelib.parser.PIDFConfig;
 import swervelib.simulation.ctre.PhysicsSim;
@@ -55,7 +55,7 @@ public class TalonFXSwerve extends SwerveMotor
     factoryDefaults();
     clearStickyFaults();
 
-    if (!Robot.isReal())
+    if (RobotBase.isSimulation())
     {
       PhysicsSim.getInstance().addTalonFX(motor, .25, 6800);
     }
@@ -106,7 +106,6 @@ public class TalonFXSwerve extends SwerveMotor
     motor.clearStickyFaults();
   }
 
-
   /**
    * Set the absolute encoder to be a compatible absolute encoder.
    *
@@ -122,15 +121,17 @@ public class TalonFXSwerve extends SwerveMotor
   /**
    * Configure the integrated encoder for the swerve module. Sets the conversion factors for position and velocity.
    *
-   * @param positionConversionFactor The conversion factor to apply for position. <p><br /> Degrees: <br />
+   * @param positionConversionFactor The conversion factor to apply for position.
+   *                                 <p><br>
+   *                                 Degrees: <br>
    *                                 <code>
    *                                 360 / (angleGearRatio * encoderTicksPerRotation)
-   *                                 </code><br /></p>
+   *                                 </code><br>
-   *                                 <p><br />Meters:<br />
+   *                                 <p><br>
+   *                                 Meters:<br>
    *                                 <code>
    *                                 (Math.PI * wheelDiameter) / (driveGearRatio * encoderTicksPerRotation)
    *                                 </code>
-   *                                 </p>
    */
   @Override
   public void configureIntegratedEncoder(double positionConversionFactor)
@@ -265,9 +266,8 @@ public class TalonFXSwerve extends SwerveMotor
    */
   public double convertToNativeSensorUnits(double setpoint)
   {
-    setpoint = isDriveMotor ?
+    setpoint =
-               setpoint * .1 :
+        isDriveMotor ? setpoint * .1 : placeInAppropriate0To360Scope(getRawPosition(), setpoint);
-               placeInAppropriate0To360Scope(getRawPosition(), setpoint);
     return setpoint / positionConversionFactor;
   }
 
@@ -280,17 +280,18 @@ public class TalonFXSwerve extends SwerveMotor
   @Override
   public void setReference(double setpoint, double feedforward)
   {
-    if (!Robot.isReal())
+    if (RobotBase.isSimulation())
     {
       PhysicsSim.getInstance().run();
     }
 
     burnFlash();
 
-    motor.set(isDriveMotor ? ControlMode.Velocity : ControlMode.Position,
+    motor.set(
-              convertToNativeSensorUnits(setpoint),
+        isDriveMotor ? ControlMode.Velocity : ControlMode.Position,
-              DemandType.ArbitraryFeedForward,
+        convertToNativeSensorUnits(setpoint),
-              feedforward * -0.3);
+        DemandType.ArbitraryFeedForward,
+        feedforward * -0.3);
     // Credit to Team 3181 for the -0.3, I'm not sure why it works, but it does.
   }
 
@@ -334,7 +335,7 @@ public class TalonFXSwerve extends SwerveMotor
   @Override
   public void setPosition(double position)
   {
-    if (!absoluteEncoder && Robot.isReal())
+    if (!absoluteEncoder && !RobotBase.isSimulation())
     {
       motor.setSelectedSensorPosition(convertToNativeSensorUnits(position));
     }
@@ -400,5 +401,4 @@ public class TalonFXSwerve extends SwerveMotor
   {
     return absoluteEncoder;
   }
-
 }

swervelib/motors/TalonSRXSwerve.java

@@ -6,7 +6,7 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
-import frc.robot.Robot;
+import edu.wpi.first.wpilibj.RobotBase;
 import swervelib.encoders.SwerveAbsoluteEncoder;
 import swervelib.parser.PIDFConfig;
 
@@ -20,18 +20,18 @@ public class TalonSRXSwerve extends SwerveMotor
    * Factory default already occurred.
    */
   private final boolean factoryDefaultOccurred = false;
+  /**
+   * Whether the absolute encoder is integrated.
+   */
+  private final boolean absoluteEncoder        = false;
   /**
    * TalonSRX motor controller.
    */
   WPI_TalonSRX motor;
-  /**
-   * Whether the absolute encoder is integrated.
-   */
-  private final boolean absoluteEncoder          = false;
   /**
    * The position conversion factor.
    */
-  private       double  positionConversionFactor = 1;
+  private double positionConversionFactor = 1;
 
   /**
    * Constructor for TalonSRX swerve motor.
@@ -82,7 +82,6 @@ public class TalonSRXSwerve extends SwerveMotor
     motor.clearStickyFaults();
   }
 
-
   /**
    * Set the absolute encoder to be a compatible absolute encoder.
    *
@@ -187,10 +186,11 @@ public class TalonSRXSwerve extends SwerveMotor
   {
     burnFlash();
 
-    motor.set(isDriveMotor ? ControlMode.Velocity : ControlMode.Position,
+    motor.set(
-              convertToNativeSensorUnits(setpoint),
+        isDriveMotor ? ControlMode.Velocity : ControlMode.Position,
-              DemandType.ArbitraryFeedForward,
+        convertToNativeSensorUnits(setpoint),
-              feedforward * -0.3);
+        DemandType.ArbitraryFeedForward,
+        feedforward * -0.3);
     // Credit to Team 3181 for the -0.3, I'm not sure why it works, but it does.
   }
 
@@ -234,7 +234,7 @@ public class TalonSRXSwerve extends SwerveMotor
   @Override
   public void setPosition(double position)
   {
-    if (!absoluteEncoder && Robot.isReal())
+    if (!absoluteEncoder && !RobotBase.isSimulation())
     {
       motor.setSelectedSensorPosition(convertToNativeSensorUnits(position));
     }
@@ -354,9 +354,8 @@ public class TalonSRXSwerve extends SwerveMotor
    */
   public double convertToNativeSensorUnits(double setpoint)
   {
-    setpoint = isDriveMotor ?
+    setpoint =
-               setpoint * .1 :
+        isDriveMotor ? setpoint * .1 : placeInAppropriate0To360Scope(getRawPosition(), setpoint);
-               placeInAppropriate0To360Scope(getRawPosition(), setpoint);
     return setpoint / positionConversionFactor;
   }
 }

swervelib/motors/package-info.java

@@ -1,4 +1,4 @@
 /**
  * Swerve motor controller wrappers which implement {@link swervelib.motors.SwerveMotor}.
  */
-package swervelib.motors;
+package swervelib.motors;

swervelib/package-info.java

@@ -3,4 +3,4 @@
  *
  * @version 1.0.0
  */
-package swervelib;
+package swervelib;

swervelib/parser/PIDFConfig.java

@@ -106,4 +106,3 @@ public class PIDFConfig
     return new PIDController(p, i, d);
   }
 }
-

swervelib/parser/SwerveControllerConfiguration.java

@@ -23,7 +23,8 @@ public class SwerveControllerConfiguration
   /**
    * hypotenuse deadband for the robot angle control joystick.
    */
-  public final double     angleJoyStickRadiusDeadband; // Deadband for the minimum hypot for the heading joystick.
+  public final double
+                          angleJoyStickRadiusDeadband; // Deadband for the minimum hypot for the heading joystick.
 
   /**
    * Construct the swerve controller configuration.
@@ -32,13 +33,17 @@ public class SwerveControllerConfiguration
    * @param headingPIDF                 Heading PIDF configuration.
    * @param angleJoyStickRadiusDeadband Deadband on radius of angle joystick.
    */
-  public SwerveControllerConfiguration(SwerveDriveConfiguration driveCfg, PIDFConfig headingPIDF,
+  public SwerveControllerConfiguration(
-                                       double angleJoyStickRadiusDeadband)
+      SwerveDriveConfiguration driveCfg,
+      PIDFConfig headingPIDF,
+      double angleJoyStickRadiusDeadband)
   {
     this.maxSpeed = driveCfg.maxSpeed;
-    this.maxAngularVelocity = calculateMaxAngularVelocity(driveCfg.maxSpeed,
+    this.maxAngularVelocity =
-                                                          Math.abs(driveCfg.moduleLocationsMeters[0].getX()),
+        calculateMaxAngularVelocity(
-                                                          Math.abs(driveCfg.moduleLocationsMeters[0].getY()));
+            driveCfg.maxSpeed,
+            Math.abs(driveCfg.moduleLocationsMeters[0].getX()),
+            Math.abs(driveCfg.moduleLocationsMeters[0].getY()));
     this.headingPIDF = headingPIDF;
     this.angleJoyStickRadiusDeadband = angleJoyStickRadiusDeadband;
   }
@@ -54,5 +59,4 @@ public class SwerveControllerConfiguration
   {
     this(driveCfg, headingPIDF, 0.5);
   }
-
 }

swervelib/parser/SwerveDriveConfiguration.java

@@ -43,8 +43,11 @@ public class SwerveDriveConfiguration
    * @param maxSpeed      Max speed of the robot in meters per second.
    * @param invertedIMU   Invert the IMU.
    */
-  public SwerveDriveConfiguration(SwerveModuleConfiguration[] moduleConfigs, SwerveIMU swerveIMU, double maxSpeed,
+  public SwerveDriveConfiguration(
-                                  boolean invertedIMU)
+      SwerveModuleConfiguration[] moduleConfigs,
+      SwerveIMU swerveIMU,
+      double maxSpeed,
+      boolean invertedIMU)
   {
     this.moduleCount = moduleConfigs.length;
     this.imu = swerveIMU;

swervelib/parser/SwerveModuleConfiguration.java

@@ -65,7 +65,6 @@ public class SwerveModuleConfiguration
    */
   public SwerveAbsoluteEncoder absoluteEncoder;
 
-
   /**
    * Construct a configuration object for swerve modules.
    *
@@ -83,13 +82,20 @@ public class SwerveModuleConfiguration
    * @param maxSpeed                Maximum speed in meters per second.
    * @param physicalCharacteristics Physical characteristics of the swerve module.
    */
-  public SwerveModuleConfiguration(SwerveMotor driveMotor, SwerveMotor angleMotor,
+  public SwerveModuleConfiguration(
-                                   SwerveAbsoluteEncoder absoluteEncoder, double angleOffset,
+      SwerveMotor driveMotor,
-                                   double xMeters,
+      SwerveMotor angleMotor,
-                                   double yMeters, PIDFConfig anglePIDF, PIDFConfig velocityPIDF, double maxSpeed,
+      SwerveAbsoluteEncoder absoluteEncoder,
-                                   SwerveModulePhysicalCharacteristics physicalCharacteristics,
+      double angleOffset,
-                                   boolean absoluteEncoderInverted, boolean driveMotorInverted,
+      double xMeters,
-                                   boolean angleMotorInverted)
+      double yMeters,
+      PIDFConfig anglePIDF,
+      PIDFConfig velocityPIDF,
+      double maxSpeed,
+      SwerveModulePhysicalCharacteristics physicalCharacteristics,
+      boolean absoluteEncoderInverted,
+      boolean driveMotorInverted,
+      boolean angleMotorInverted)
   {
     this.driveMotor = driveMotor;
     this.angleMotor = angleMotor;
@@ -102,11 +108,12 @@ public class SwerveModuleConfiguration
     this.anglePIDF = anglePIDF;
     this.velocityPIDF = velocityPIDF;
     this.maxSpeed = maxSpeed;
-    this.angleKV = calculateAngleKV(physicalCharacteristics.optimalVoltage,
+    this.angleKV =
-                                    physicalCharacteristics.angleMotorFreeSpeedRPM,
+        calculateAngleKV(
-                                    physicalCharacteristics.angleGearRatio);
+            physicalCharacteristics.optimalVoltage,
+            physicalCharacteristics.angleMotorFreeSpeedRPM,
+            physicalCharacteristics.angleGearRatio);
     this.physicalCharacteristics = physicalCharacteristics;
-
   }
 
   /**
@@ -124,27 +131,34 @@ public class SwerveModuleConfiguration
    * @param maxSpeed                Maximum robot speed in meters per second.
    * @param physicalCharacteristics Physical characteristics of the swerve module.
    */
-  public SwerveModuleConfiguration(SwerveMotor driveMotor, SwerveMotor angleMotor,
+  public SwerveModuleConfiguration(
-                                   SwerveAbsoluteEncoder absoluteEncoder, double angleOffset,
+      SwerveMotor driveMotor,
-                                   double xMeters, double yMeters, PIDFConfig anglePIDF, PIDFConfig velocityPIDF,
+      SwerveMotor angleMotor,
-                                   double maxSpeed, SwerveModulePhysicalCharacteristics physicalCharacteristics)
+      SwerveAbsoluteEncoder absoluteEncoder,
+      double angleOffset,
+      double xMeters,
+      double yMeters,
+      PIDFConfig anglePIDF,
+      PIDFConfig velocityPIDF,
+      double maxSpeed,
+      SwerveModulePhysicalCharacteristics physicalCharacteristics)
   {
-    this(driveMotor,
+    this(
-         angleMotor,
+        driveMotor,
-         absoluteEncoder,
+        angleMotor,
-         angleOffset,
+        absoluteEncoder,
-         xMeters,
+        angleOffset,
-         yMeters,
+        xMeters,
-         anglePIDF,
+        yMeters,
-         velocityPIDF,
+        anglePIDF,
-         maxSpeed,
+        velocityPIDF,
-         physicalCharacteristics,
+        maxSpeed,
-         false,
+        physicalCharacteristics,
-         false,
+        false,
-         false);
+        false,
+        false);
   }
 
-
   /**
    * Create the drive feedforward for swerve modules.
    *
@@ -153,10 +167,11 @@ public class SwerveModuleConfiguration
   public SimpleMotorFeedforward createDriveFeedforward()
   {
     double kv = physicalCharacteristics.optimalVoltage / maxSpeed;
-    ///^ Volt-seconds per meter (max voltage divided by max speed)
+    /// ^ Volt-seconds per meter (max voltage divided by max speed)
-    double ka = physicalCharacteristics.optimalVoltage /
+    double ka =
-                calculateMaxAcceleration(physicalCharacteristics.wheelGripCoefficientOfFriction);
+        physicalCharacteristics.optimalVoltage
-    ///^ Volt-seconds^2 per meter (max voltage divided by max accel)
+        / calculateMaxAcceleration(physicalCharacteristics.wheelGripCoefficientOfFriction);
+    /// ^ Volt-seconds^2 per meter (max voltage divided by max accel)
     return new SimpleMotorFeedforward(0, kv, ka);
   }
 
@@ -168,11 +183,13 @@ public class SwerveModuleConfiguration
    */
   public double getPositionEncoderConversion(boolean isDriveMotor)
   {
-    return isDriveMotor ? calculateMetersPerRotation(physicalCharacteristics.wheelDiameter,
+    return isDriveMotor
-                                                     physicalCharacteristics.driveGearRatio,
+           ? calculateMetersPerRotation(
-                                                     physicalCharacteristics.driveEncoderPulsePerRotation)
+        physicalCharacteristics.wheelDiameter,
-                        : calculateDegreesPerSteeringRotation(
+        physicalCharacteristics.driveGearRatio,
-                            physicalCharacteristics.angleGearRatio,
+        physicalCharacteristics.driveEncoderPulsePerRotation)
-                            physicalCharacteristics.angleEncoderPulsePerRotation);
+           : calculateDegreesPerSteeringRotation(
+               physicalCharacteristics.angleGearRatio,
+               physicalCharacteristics.angleEncoderPulsePerRotation);
   }
 }

swervelib/parser/SwerveModulePhysicalCharacteristics.java

@@ -66,13 +66,19 @@ public class SwerveModulePhysicalCharacteristics
    * @param driveEncoderPulsePerRotation   The number of encoder pulses per motor rotation, 1 for integrated encoders.
    * @param angleEncoderPulsePerRotation   The number of encoder pulses per motor rotation, 1 for integrated encoders.
    */
-  public SwerveModulePhysicalCharacteristics(double driveGearRatio, double angleGearRatio,
+  public SwerveModulePhysicalCharacteristics(
-                                             double angleMotorFreeSpeedRPM,
+      double driveGearRatio,
-                                             double wheelDiameter, double wheelGripCoefficientOfFriction,
+      double angleGearRatio,
-                                             double optimalVoltage, int driveMotorCurrentLimit,
+      double angleMotorFreeSpeedRPM,
-                                             int angleMotorCurrentLimit, double driveMotorRampRate,
+      double wheelDiameter,
-                                             double angleMotorRampRate, int driveEncoderPulsePerRotation,
+      double wheelGripCoefficientOfFriction,
-                                             int angleEncoderPulsePerRotation)
+      double optimalVoltage,
+      int driveMotorCurrentLimit,
+      int angleMotorCurrentLimit,
+      double driveMotorRampRate,
+      double angleMotorRampRate,
+      int driveEncoderPulsePerRotation,
+      int angleEncoderPulsePerRotation)
   {
     this.wheelGripCoefficientOfFriction = wheelGripCoefficientOfFriction;
     this.optimalVoltage = optimalVoltage;
@@ -106,12 +112,28 @@ public class SwerveModulePhysicalCharacteristics
    * @param driveEncoderPulsePerRotation The number of encoder pulses per motor rotation, 1 for integrated encoders.
    * @param angleEncoderPulsePerRotation The number of encoder pulses per motor rotation, 1 for integrated encoders.
    */
-  public SwerveModulePhysicalCharacteristics(double driveGearRatio, double angleGearRatio,
+  public SwerveModulePhysicalCharacteristics(
-                                             double angleMotorFreeSpeedRPM,
+      double driveGearRatio,
-                                             double wheelDiameter, double driveMotorRampRate, double angleMotorRampRate,
+      double angleGearRatio,
-                                             int driveEncoderPulsePerRotation, int angleEncoderPulsePerRotation)
+      double angleMotorFreeSpeedRPM,
+      double wheelDiameter,
+      double driveMotorRampRate,
+      double angleMotorRampRate,
+      int driveEncoderPulsePerRotation,
+      int angleEncoderPulsePerRotation)
   {
-    this(driveGearRatio, angleGearRatio, angleMotorFreeSpeedRPM, wheelDiameter, 1.19, 12,
+    this(
-         40, 20, driveMotorRampRate, angleMotorRampRate, driveEncoderPulsePerRotation, angleEncoderPulsePerRotation);
+        driveGearRatio,
+        angleGearRatio,
+        angleMotorFreeSpeedRPM,
+        wheelDiameter,
+        1.19,
+        12,
+        40,
+        20,
+        driveMotorRampRate,
+        angleMotorRampRate,
+        driveEncoderPulsePerRotation,
+        angleEncoderPulsePerRotation);
   }
 }

swervelib/parser/SwerveParser.java

@@ -45,7 +45,6 @@ public class SwerveParser
    */
   public static        ModuleJson[]             moduleJsons;
 
-
   /**
    * Construct a swerve parser. Will throw an error if there is a missing file.
    *
@@ -55,13 +54,22 @@ public class SwerveParser
   public SwerveParser(File directory) throws IOException
   {
     checkDirectory(directory);
-    swerveDriveJson = new ObjectMapper().readValue(new File(directory, "swervedrive.json"), SwerveDriveJson.class);
+    swerveDriveJson =
-    controllerPropertiesJson = new ObjectMapper().readValue(new File(directory, "controllerproperties.json"),
+        new ObjectMapper()
-                                                            ControllerPropertiesJson.class);
+            .readValue(new File(directory, "swervedrive.json"), SwerveDriveJson.class);
-    pidfPropertiesJson = new ObjectMapper().readValue(new File(directory, "modules/pidfproperties.json"),
+    controllerPropertiesJson =
-                                                      PIDFPropertiesJson.class);
+        new ObjectMapper()
-    physicalPropertiesJson = new ObjectMapper().readValue(new File(directory, "modules/physicalproperties.json"),
+            .readValue(
-                                                          PhysicalPropertiesJson.class);
+                new File(directory, "controllerproperties.json"), ControllerPropertiesJson.class);
+    pidfPropertiesJson =
+        new ObjectMapper()
+            .readValue(
+                new File(directory, "modules/pidfproperties.json"), PIDFPropertiesJson.class);
+    physicalPropertiesJson =
+        new ObjectMapper()
+            .readValue(
+                new File(directory, "modules/physicalproperties.json"),
+                PhysicalPropertiesJson.class);
     moduleJsons = new ModuleJson[swerveDriveJson.modules.length];
     for (int i = 0; i < moduleJsons.length; i++)
     {
@@ -79,8 +87,8 @@ public class SwerveParser
    * @param driveConfiguration {@link SwerveDriveConfiguration} to pull from.
    * @return {@link SwerveModuleConfiguration} based on the file.
    */
-  public static SwerveModule getModuleConfigurationByName(String name,
+  public static SwerveModule getModuleConfigurationByName(
-                                                          SwerveDriveConfiguration driveConfiguration)
+      String name, SwerveDriveConfiguration driveConfiguration)
   {
     return driveConfiguration.modules[moduleConfigs.get(name + ".json")];
   }
@@ -123,22 +131,28 @@ public class SwerveParser
    */
   public SwerveDrive createSwerveDrive()
   {
-    double                      maxSpeedMPS          = Units.feetToMeters(swerveDriveJson.maxSpeed);
+    double maxSpeedMPS = Units.feetToMeters(swerveDriveJson.maxSpeed);
-    SwerveModuleConfiguration[] moduleConfigurations = new SwerveModuleConfiguration[moduleJsons.length];
+    SwerveModuleConfiguration[] moduleConfigurations =
+        new SwerveModuleConfiguration[moduleJsons.length];
     for (int i = 0; i < moduleConfigurations.length; i++)
     {
       ModuleJson module = moduleJsons[i];
-      moduleConfigurations[i] = module.createModuleConfiguration(pidfPropertiesJson.angle, pidfPropertiesJson.drive,
+      moduleConfigurations[i] =
-                                                                 maxSpeedMPS,
+          module.createModuleConfiguration(
-                                                                 physicalPropertiesJson.createPhysicalProperties(
+              pidfPropertiesJson.angle,
-                                                                     swerveDriveJson.optimalVoltage));
+              pidfPropertiesJson.drive,
+              maxSpeedMPS,
+              physicalPropertiesJson.createPhysicalProperties(swerveDriveJson.optimalVoltage));
     }
-    SwerveDriveConfiguration swerveDriveConfiguration = new SwerveDriveConfiguration(moduleConfigurations,
+    SwerveDriveConfiguration swerveDriveConfiguration =
-                                                                                     swerveDriveJson.imu.createIMU(),
+        new SwerveDriveConfiguration(
-                                                                                     maxSpeedMPS,
+            moduleConfigurations,
-                                                                                     swerveDriveJson.invertedIMU);
+            swerveDriveJson.imu.createIMU(),
+            maxSpeedMPS,
+            swerveDriveJson.invertedIMU);
 
-    return new SwerveDrive(swerveDriveConfiguration,
+    return new SwerveDrive(
-                           controllerPropertiesJson.createControllerConfiguration(swerveDriveConfiguration));
+        swerveDriveConfiguration,
+        controllerPropertiesJson.createControllerConfiguration(swerveDriveConfiguration));
   }
 }

swervelib/parser/deserializer/package-info.java

@@ -1,4 +1,4 @@
 /**
  * Deserialize specific variables for outside the parser.
  */
-package swervelib.parser.deserializer;
+package swervelib.parser.deserializer;

swervelib/parser/json/ControllerPropertiesJson.java

@@ -25,8 +25,10 @@ public class ControllerPropertiesJson
    * @param driveConfiguration {@link SwerveDriveConfiguration} parsed configuration.
    * @return {@link SwerveControllerConfiguration} object based on parsed data.
    */
-  public SwerveControllerConfiguration createControllerConfiguration(SwerveDriveConfiguration driveConfiguration)
+  public SwerveControllerConfiguration createControllerConfiguration(
+      SwerveDriveConfiguration driveConfiguration)
   {
-    return new SwerveControllerConfiguration(driveConfiguration, heading, angleJoystickRadiusDeadband);
+    return new SwerveControllerConfiguration(
+        driveConfiguration, heading, angleJoystickRadiusDeadband);
   }
 }

swervelib/parser/json/DeviceJson.java

@@ -102,7 +102,6 @@ public class DeviceJson
         return new TalonSRXSwerve(id, isDriveMotor);
       default:
         throw new RuntimeException(type + " is not a recognized absolute encoder type.");
-
     }
   }
 
@@ -121,6 +120,7 @@ public class DeviceJson
       case "none":
         return null;
     }
-    throw new RuntimeException("Could not create absolute encoder from data port of " + type + " id " + id);
+    throw new RuntimeException(
+        "Could not create absolute encoder from data port of " + type + " id " + id);
   }
 }

swervelib/parser/json/ModuleJson.java

@@ -53,9 +53,11 @@ public class ModuleJson
    * @param physicalCharacteristics Physical characteristics of the swerve module.
    * @return {@link SwerveModuleConfiguration} based on the provided data and parsed data.
    */
-  public SwerveModuleConfiguration createModuleConfiguration(PIDFConfig anglePIDF, PIDFConfig velocityPIDF,
+  public SwerveModuleConfiguration createModuleConfiguration(
-                                                             double maxSpeed,
+      PIDFConfig anglePIDF,
-                                                             SwerveModulePhysicalCharacteristics physicalCharacteristics)
+      PIDFConfig velocityPIDF,
+      double maxSpeed,
+      SwerveModulePhysicalCharacteristics physicalCharacteristics)
   {
     SwerveMotor           angleMotor = angle.createMotor(false);
     SwerveAbsoluteEncoder absEncoder = encoder.createEncoder();
@@ -67,10 +69,19 @@ public class ModuleJson
       angleMotor.setAbsoluteEncoder(absEncoder);
     }
 
-    return new SwerveModuleConfiguration(drive.createMotor(true), angleMotor, absEncoder,
+    return new SwerveModuleConfiguration(
-                                         absoluteEncoderOffset, Units.inchesToMeters(location.x),
+        drive.createMotor(true),
-                                         Units.inchesToMeters(location.y), anglePIDF, velocityPIDF, maxSpeed,
+        angleMotor,
-                                         physicalCharacteristics, absoluteEncoderInverted, inverted.drive,
+        absEncoder,
-                                         inverted.angle);
+        absoluteEncoderOffset,
+        Units.inchesToMeters(location.x),
+        Units.inchesToMeters(location.y),
+        anglePIDF,
+        velocityPIDF,
+        maxSpeed,
+        physicalCharacteristics,
+        absoluteEncoderInverted,
+        inverted.drive,
+        inverted.angle);
   }
 }

swervelib/parser/json/PhysicalPropertiesJson.java

@@ -46,12 +46,19 @@ public class PhysicalPropertiesJson
    */
   public SwerveModulePhysicalCharacteristics createPhysicalProperties(double optimalVoltage)
   {
-    return new SwerveModulePhysicalCharacteristics(gearRatio.drive, gearRatio.angle, angleMotorFreeSpeedRPM,
+    return new SwerveModulePhysicalCharacteristics(
-                                                   Units.inchesToMeters(wheelDiameter), wheelGripCoefficientOfFriction,
+        gearRatio.drive,
-                                                   optimalVoltage,
+        gearRatio.angle,
-                                                   currentLimit.drive, currentLimit.angle, rampRate.drive,
+        angleMotorFreeSpeedRPM,
-                                                   rampRate.angle, encoderPulsePerRotation.drive,
+        Units.inchesToMeters(wheelDiameter),
-                                                   encoderPulsePerRotation.angle);
+        wheelGripCoefficientOfFriction,
+        optimalVoltage,
+        currentLimit.drive,
+        currentLimit.angle,
+        rampRate.drive,
+        rampRate.angle,
+        encoderPulsePerRotation.drive,
+        encoderPulsePerRotation.angle);
   }
 }
 
@@ -88,7 +95,6 @@ class MotorConfigDouble
     this.angle = angle;
     this.drive = drive;
   }
-
 }
 
 /**
@@ -124,4 +130,4 @@ class MotorConfigInt
     this.angle = angle;
     this.drive = drive;
   }
-}
+}

swervelib/parser/json/modules/LocationJson.java

@@ -4,7 +4,6 @@ package swervelib.parser.json.modules;
  * Location JSON parsed class. Used to access the JSON data. Module locations, in inches, as distances to the center of
  * the robot. Positive x is torwards the robot front, and * +y is torwards robot left.
  */
-
 public class LocationJson
 {
 

swervelib/parser/json/modules/package-info.java

@@ -1,4 +1,4 @@
 /**
  * JSON Mapped Configuration types for modules.
  */
-package swervelib.parser.json.modules;
+package swervelib.parser.json.modules;

swervelib/parser/json/package-info.java

@@ -1,4 +1,4 @@
 /**
  * JSON Mapped classes for parsing configuration files.
  */
-package swervelib.parser.json;
+package swervelib.parser.json;

swervelib/parser/package-info.java

@@ -1,4 +1,4 @@
 /**
  * JSON Parser for YAGSL configurations.
  */
-package swervelib.parser;
+package swervelib.parser;

swervelib/simulation/SwerveIMUSimulation.java

@@ -86,8 +86,11 @@ public class SwerveIMUSimulation
    * @param modulePoses {@link Pose2d} representing the swerve modules.
    * @param field       {@link Field2d} to update.
    */
-  public void updateOdometry(SwerveKinematics2 kinematics, SwerveModuleState2[] states, Pose2d[] modulePoses,
+  public void updateOdometry(
-                             Field2d field)
+      SwerveKinematics2 kinematics,
+      SwerveModuleState2[] states,
+      Pose2d[] modulePoses,
+      Field2d field)
   {
     angle += kinematics.toChassisSpeeds(states).omegaRadiansPerSecond * (timer.get() - lastTime);
     lastTime = timer.get();

swervelib/simulation/SwerveModuleSimulation.java

@@ -73,7 +73,8 @@ public class SwerveModuleSimulation
    */
   public SwerveModulePosition getPosition()
   {
-    return new SwerveModulePosition(fakePos, state.angle.plus(new Rotation2d(state.omegaRadPerSecond * dt)));
+    return new SwerveModulePosition(
+        fakePos, state.angle.plus(new Rotation2d(state.omegaRadPerSecond * dt)));
   }
 
   /**

swervelib/simulation/ctre/PhysicsSim.java

@@ -16,6 +16,8 @@ public class PhysicsSim
 
   /**
    * Gets the robot simulator instance.
+   *
+   * @return {@link PhysicsSim} instance.
    */
   public static PhysicsSim getInstance()
   {
@@ -25,7 +27,8 @@ public class PhysicsSim
   /* scales a random domain of [0, 2pi] to [min, max] while prioritizing the peaks */
   static double random(double min, double max)
   {
-    return (max - min) / 2 * Math.sin(Math.IEEEremainder(Math.random(), 2 * 3.14159)) + (max + min) / 2;
+    return (max - min) / 2 * Math.sin(Math.IEEEremainder(Math.random(), 2 * 3.14159))
+           + (max + min) / 2;
   }
 
   static double random(double max)
@@ -53,11 +56,16 @@ public class PhysicsSim
    * @param fullVel         The maximum motor velocity, in ticks per 100ms
    * @param sensorPhase     The phase of the TalonSRX sensors
    */
-  public void addTalonSRX(TalonSRX talon, final double accelToFullTime, final double fullVel, final boolean sensorPhase)
+  public void addTalonSRX(
+      TalonSRX talon,
+      final double accelToFullTime,
+      final double fullVel,
+      final boolean sensorPhase)
   {
     if (talon != null)
     {
-      TalonSRXSimProfile simTalon = new TalonSRXSimProfile(talon, accelToFullTime, fullVel, sensorPhase);
+      TalonSRXSimProfile simTalon =
+          new TalonSRXSimProfile(talon, accelToFullTime, fullVel, sensorPhase);
       _simProfiles.add(simTalon);
     }
   }
@@ -82,11 +90,16 @@ public class PhysicsSim
    * @param fullVel         The maximum motor velocity, in ticks per 100ms
    * @param sensorPhase     The phase of the TalonFX sensors
    */
-  public void addTalonFX(TalonFX falcon, final double accelToFullTime, final double fullVel, final boolean sensorPhase)
+  public void addTalonFX(
+      TalonFX falcon,
+      final double accelToFullTime,
+      final double fullVel,
+      final boolean sensorPhase)
   {
     if (falcon != null)
     {
-      TalonFXSimProfile simFalcon = new TalonFXSimProfile(falcon, accelToFullTime, fullVel, sensorPhase);
+      TalonFXSimProfile simFalcon =
+          new TalonFXSimProfile(falcon, accelToFullTime, fullVel, sensorPhase);
       _simProfiles.add(simFalcon);
     }
   }
@@ -152,4 +165,4 @@ public class PhysicsSim
       return period;
     }
   }
-}
+}

swervelib/simulation/ctre/TalonFXSimProfile.java

@@ -31,8 +31,11 @@ class TalonFXSimProfile extends SimProfile
    * @param fullVel         The maximum motor velocity, in ticks per 100ms
    * @param sensorPhase     The phase of the TalonFX sensors
    */
-  public TalonFXSimProfile(final TalonFX falcon, final double accelToFullTime, final double fullVel,
+  public TalonFXSimProfile(
-                           final boolean sensorPhase)
+      final TalonFX falcon,
+      final double accelToFullTime,
+      final double fullVel,
+      final boolean sensorPhase)
   {
     this._falcon = falcon;
     this._accelToFullTime = accelToFullTime;
@@ -42,9 +45,10 @@ class TalonFXSimProfile extends SimProfile
 
   /**
    * Runs the simulation profile.
-   * <p>
+   *
-   * This uses very rudimentary physics simulation and exists to allow users to test features of our products in
+   * <p>This uses very rudimentary physics simulation and exists to allow users to test features of
-   * simulation using our examples out of the box. Users may modify this to utilize more accurate physics simulation.
+   * our products in simulation using our examples out of the box. Users may modify this to utilize more accurate
+   * physics simulation.
    */
   public void run()
   {
@@ -85,4 +89,4 @@ class TalonFXSimProfile extends SimProfile
 
     _falcon.getSimCollection().setBusVoltage(12 - outPerc * outPerc * 3 / 4 * random(0.95, 1.05));
   }
-}
+}

swervelib/simulation/ctre/TalonSRXSimProfile.java

@@ -31,8 +31,11 @@ class TalonSRXSimProfile extends SimProfile
    * @param fullVel         The maximum motor velocity, in ticks per 100ms
    * @param sensorPhase     The phase of the TalonSRX sensors
    */
-  public TalonSRXSimProfile(final TalonSRX talon, final double accelToFullTime, final double fullVel,
+  public TalonSRXSimProfile(
-                            final boolean sensorPhase)
+      final TalonSRX talon,
+      final double accelToFullTime,
+      final double fullVel,
+      final boolean sensorPhase)
   {
     this._talon = talon;
     this._accelToFullTime = accelToFullTime;
@@ -42,9 +45,10 @@ class TalonSRXSimProfile extends SimProfile
 
   /**
    * Runs the simulation profile.
-   * <p>
+   *
-   * This uses very rudimentary physics simulation and exists to allow users to test features of our products in
+   * <p>This uses very rudimentary physics simulation and exists to allow users to test features of
-   * simulation using our examples out of the box. Users may modify this to utilize more accurate physics simulation.
+   * our products in simulation using our examples out of the box. Users may modify this to utilize more accurate
+   * physics simulation.
    */
   public void run()
   {

swervelib/simulation/ctre/VictorSPXSimProfile.java

@@ -1,6 +1,5 @@
 package swervelib.simulation.ctre;
 
-
 import static swervelib.simulation.ctre.PhysicsSim.random;
 
 import com.ctre.phoenix.motorcontrol.can.VictorSPX;
@@ -26,9 +25,10 @@ class VictorSPXSimProfile extends SimProfile
 
   /**
    * Runs the simulation profile.
-   * <p>
+   *
-   * This uses very rudimentary physics simulation and exists to allow users to test features of our products in
+   * <p>This uses very rudimentary physics simulation and exists to allow users to test features of
-   * simulation using our examples out of the box. Users may modify this to utilize more accurate physics simulation.
+   * our products in simulation using our examples out of the box. Users may modify this to utilize more accurate
+   * physics simulation.
    */
   public void run()
   {

swervelib/simulation/ctre/package-info.java

@@ -1,4 +1,4 @@
 /**
  * CTRE Physics Simulator.
  */
-package swervelib.simulation.ctre;
+package swervelib.simulation.ctre;

swervelib/simulation/package-info.java

@@ -1,4 +1,4 @@
 /**
  * Classes used to simulate the swerve drive.
  */
-package swervelib.simulation;
+package swervelib.simulation;