README updated

README.md

@@ -11,6 +11,9 @@
 SwerveDrive swerveDrive = SwerveParser.fromJSONDirectory(new File(Filesystem.getDeployDirectory(), "swerve"));
 ```
 
+# Library Information
+* The library is located in [swervelib/](./swervelib) with documentation in [docs/](./docs) and example JSON in [swerve-deploy](./swerve-deploy).
+
 # Images
 ![Field Shuffleboard](./imgaes/field.png)
 ![Simulation](./imgaes/simulation.png)

swervelib/SwerveDrive.java

@@ -0,0 +1,709 @@
+package frc.robot.subsystems.swervedrive.swerve;
+
+import com.ctre.phoenix.sensors.CANCoder;
+import com.ctre.phoenix.sensors.WPI_Pigeon2;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.drive.RobotDriveBase;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.Robot;
+import frc.robot.subsystems.swervedrive.swerve.SwerveModule.SwerveModuleLocation;
+import frc.robot.subsystems.swervedrive.swerve.SwerveModule.Verbosity;
+import frc.robot.subsystems.swervedrive.swerve.SwerveMotor.ModuleMotorType;
+import frc.robot.subsystems.swervedrive.swerve.kinematics.SwerveDriveKinematics2;
+import frc.robot.subsystems.swervedrive.swerve.kinematics.SwerveModuleState2;
+import java.io.Closeable;
+
+/**
+ * SwerveDrive base which is meant to be platform agnostic. This implementation expect second order kinematics because
+ * second order kinematics prevents the drift that builds up when using first order kinematics. As per this <a
+ * href="https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964">ChiefDelphi
+ * post</a>
+ */
+public class SwerveDrive extends RobotDriveBase implements Sendable, AutoCloseable
+{
+
+
+  /**
+   * Count of SwerveModule instances created.
+   */
+  private static int instances;
+
+  /**
+   * Front left swerve drive
+   */
+  public final SwerveModule<?, ?, ?> m_frontLeft;
+  /**
+   * Back left swerve drive
+   */
+  public final SwerveModule<?, ?, ?> m_backLeft;
+  /**
+   * Front right swerve drive
+   */
+  public final SwerveModule<?, ?, ?> m_frontRight;
+  /**
+   * Back right swerve drive
+   */
+  public final SwerveModule<?, ?, ?> m_backRight;
+  /**
+   * Maximum speed in meters per second.
+   */
+  public final double                m_driverMaxSpeedMPS, m_driverMaxAngularVelocity, m_physicalMaxSpeedMPS;
+  /**
+   * Swerve drive kinematics.
+   */
+  private final SwerveDriveKinematics2   m_swerveKinematics;
+  /**
+   * Swerve drive pose estimator for attempting to figure out our current position.
+   */
+  private final SwerveDrivePoseEstimator m_swervePoseEstimator;
+  /**
+   * Pigeon 2.0 centered on the robot.
+   */
+  private final WPI_Pigeon2              m_pigeonIMU;
+  /**
+   * Field2d displayed on shuffleboard with current position.
+   */
+  private final Field2d                  m_field = new Field2d();
+  /**
+   * The slew rate limiters to make control smooth.
+   */
+  private final SlewRateLimiter          m_xLimiter, m_yLimiter, m_turningLimiter;
+  private final Timer         m_driveTimer;
+  /**
+   * Invert the gyro reading.
+   */
+  private       boolean       m_gyroInverted;
+  private       double        m_angle;
+  private       ChassisSpeeds m_prevChassisSpeed = new ChassisSpeeds(0, 0, 0);
+  private       double        m_timerPrev;
+
+  /**
+   * Constructor for Swerve Drive assuming modules have been created and configured with PIDF and conversions.
+   *
+   * @param frontLeft                                    Front left swerve module configured.
+   * @param backLeft                                     Back left swerve module.
+   * @param frontRight                                   Front right swerve module.
+   * @param backRight                                    Back right swerve module
+   * @param pigeon                                       Pigeon IMU.
+   * @param driverMaxSpeedMetersPerSecond                Maximum speed for all modules to follow when in teleop.
+   * @param driverMaxAngularVelocityRadiansPerSecond     Maximum angular velocity for turning when using the drive
+   *                                                     function.
+   * @param driverMaxDriveAccelerationMetersPerSecond    Maximum acceleration in meters per second for the drive motors
+   *                                                     when in teleop for the slew rate limiter.
+   * @param driverMaxAngularAccelerationRadiansPerSecond Maximum angular acceleration in meters per second for the
+   *                                                     steering motors when in teleop for the slew rate limiters.
+   * @param physicalMaxSpeedMPS                          Maximum speed a module can go physically, used to desaturate
+   *                                                     wheel speeds.
+   * @param gyroInverted                                 Invert the gyroscope for the robot.
+   * @param start                                        The starting pose on the field.
+   */
+  public SwerveDrive(SwerveModule<?, ?, ?> frontLeft,
+                     SwerveModule<?, ?, ?> frontRight,
+                     SwerveModule<?, ?, ?> backLeft,
+                     SwerveModule<?, ?, ?> backRight, WPI_Pigeon2 pigeon,
+                     double driverMaxSpeedMetersPerSecond, double driverMaxAngularVelocityRadiansPerSecond,
+                     double driverMaxDriveAccelerationMetersPerSecond,
+                     double driverMaxAngularAccelerationRadiansPerSecond,
+                     double physicalMaxSpeedMPS,
+                     boolean gyroInverted, Pose2d start)
+  {
+    instances++;
+    if (instances > 1)
+    {
+      throw new RuntimeException("Cannot use more than one swerve drive instance at a time.");
+    }
+    m_driveTimer = new Timer();
+    m_driveTimer.reset();
+    m_driveTimer.start();
+    m_timerPrev = m_driveTimer.get();
+
+    m_frontLeft = frontLeft;
+    m_backRight = backRight;
+    m_backLeft = backLeft;
+    m_frontRight = frontRight;
+    m_gyroInverted = gyroInverted;
+
+    m_driverMaxSpeedMPS = driverMaxSpeedMetersPerSecond;
+    m_driverMaxAngularVelocity = driverMaxAngularVelocityRadiansPerSecond;
+    m_xLimiter = new SlewRateLimiter(driverMaxDriveAccelerationMetersPerSecond);
+    m_yLimiter = new SlewRateLimiter(driverMaxDriveAccelerationMetersPerSecond);
+    m_turningLimiter = new SlewRateLimiter(driverMaxAngularAccelerationRadiansPerSecond);
+    m_physicalMaxSpeedMPS = physicalMaxSpeedMPS;
+
+    m_pigeonIMU = pigeon;
+    configurePigeonIMU(); // Reset pigeon to 0 and default settings.
+
+    m_swerveKinematics = new SwerveDriveKinematics2(frontLeft.swerveModuleLocation,
+                                                    frontRight.swerveModuleLocation,
+                                                    backLeft.swerveModuleLocation,
+                                                    backRight.swerveModuleLocation);
+    m_swervePoseEstimator = new SwerveDrivePoseEstimator(
+        m_swerveKinematics,
+        getRotation(),
+        getPositions(),
+        start,
+        VecBuilder.fill(0.1, 0.1, 0.1), // x,y,heading in radians; state std dev, higher=less weight
+        VecBuilder.fill(0.9, 1.0, 0.9)); // x,y,heading in radians; Vision measurement std dev, higher=less weight
+
+    // Inspired by https://github.com/Team364/BaseFalconSwerve/blob/main/src/main/java/frc/robot/subsystems/Swerve.java
+    SmartDashboard.putData(m_field);
+    SmartDashboard.putData(m_pigeonIMU);
+
+    setVoltageCompensation();
+    zeroModules(); // Set all modules to 0.
+  }
+
+  /**
+   * Enable voltage compensation to the current battery voltage on all modules.
+   */
+  public void setVoltageCompensation()
+  {
+    double currentVoltage = RobotController.getBatteryVoltage();
+    m_frontLeft.setVoltageCompensation(currentVoltage);
+    m_frontRight.setVoltageCompensation(currentVoltage);
+    m_backLeft.setVoltageCompensation(currentVoltage);
+    m_backRight.setVoltageCompensation(currentVoltage);
+  }
+
+
+  /**
+   * Create swerve drive modules
+   *
+   * @param driveGearRatio            Drive gear ratio in form of (rotation:1 AKA rotations/1) to get the encoder ticks
+   *                                  per rotation.
+   * @param steerGearRatio            Steering motor gear ratio (usually 12.8:1 for MK4 in form of rotations:1 or
+   *                                  rotations/1), only applied if using Neo's.
+   * @param wheelDiameterMeters       The wheel diameter of the swerve drive module in meters.
+   * @param wheelBaseMeters           The Distance between front and back wheels of the robot in meters.
+   * @param driveTrainWidthMeters     The Distance between centers of right and left wheels in meters.
+   * @param steeringMotorFreeSpeedRPM The RPM free speed of the steering motor.
+   * @param maxSpeedMPS               The maximum drive speed in meters per second.
+   * @param maxDriveAcceleration      The maximum drive acceleration in meters^2 per second.
+   * @param steeringMotorInverted     The steering motor is inverted.
+   * @param drivingMotorInverted      The driving motor is inverted.
+   * @param configs                   The swerve module configuration classes for the swerve drive given.
+   * @return Array of swerve modules in the order of front left, front right, back left, back right.
+   */
+  public static SwerveModule<?, ?, ?>[] createModules(
+      double driveGearRatio, double steerGearRatio, double wheelDiameterMeters, double wheelBaseMeters,
+      double driveTrainWidthMeters, double steeringMotorFreeSpeedRPM, double maxSpeedMPS,
+      double maxDriveAcceleration, boolean steeringMotorInverted, boolean drivingMotorInverted,
+      SwerveModuleConfig<?, ?, ?>[] configs)
+  {
+    SwerveModule<?, ?, ?>[] modules = new SwerveModule[configs.length];
+    for (int i = 0; i < configs.length; i++)
+    {
+      int loc;
+      switch (configs[i].loc)
+      {
+        case FrontLeft:
+          loc = 0;
+          break;
+        case BackLeft:
+          loc = 2;
+          break;
+        case FrontRight:
+          loc = 1;
+          break;
+        case BackRight:
+          loc = 3;
+          break;
+        default:
+          loc = i;
+      }
+
+      modules[loc] = configs[i].createModule(driveGearRatio, steerGearRatio, wheelDiameterMeters, wheelBaseMeters,
+                                             driveTrainWidthMeters, steeringMotorFreeSpeedRPM, maxSpeedMPS,
+                                             maxDriveAcceleration, steeringMotorInverted, drivingMotorInverted);
+    }
+    return modules;
+  }
+
+  /**
+   * Sets the speed to 0 and angle to 0 for all the swerve drive modules.
+   */
+  public void zeroModules()
+  {
+    // feedWatchdog();
+    // m_frontLeft.setAngle(0, 0);
+    // m_backLeft.setAngle(0, 0);
+    // m_frontRight.setAngle(0, 0);
+    // m_backRight.setAngle(0, 0);
+    m_frontRight.set(0);
+    m_backLeft.set(0);
+    m_frontLeft.set(0);
+    m_backRight.set(0);
+    set(0, 0, 0, false);
+  }
+
+  /**
+   * Configure the PigeonIMU with factory default settings and a zeroed gyroscope.
+   */
+  public void configurePigeonIMU()
+  {
+    m_pigeonIMU.configFactoryDefault();
+    zeroGyro();
+  }
+
+  /**
+   * Update the swerve drive odometry.
+   *
+   * @return Swerve drive odometry.
+   */
+  public SwerveDrivePoseEstimator update()
+  {
+    Pose2d pos = m_swervePoseEstimator.update(getRotation(), getPositions());
+    if (!Robot.isReal())
+    {
+      m_pigeonIMU.setYaw(pos.getRotation().getDegrees());
+    }
+    return m_swervePoseEstimator;
+  }
+
+  /**
+   * Drive swerve based off controller input.
+   *
+   * @param forward       The speed (-1 to 1) in which the Y axis should go.
+   * @param strafe        The speed (-1 to 1) in which the X axis should go.
+   * @param turn          The speed (-1 to 1) in which the robot should turn.
+   * @param fieldRelative Whether or not to use field relative controls.
+   */
+  public void drive(double forward, double strafe, double turn, boolean fieldRelative)
+  {
+    // 2. Apply deadband/Dead-Zone
+    forward = Math.abs(forward) > m_deadband ? forward : 0.0;
+    strafe = Math.abs(strafe) > m_deadband ? strafe : 0.0;
+    turn = Math.abs(turn) > m_deadband ? turn : 0.0;
+
+    // If nothing is asked of us we do nothing.
+    if ((Math.abs(forward) + Math.abs(strafe) + Math.abs(turn)) <= m_deadband)
+    {
+      zeroModules();
+      return;
+    }
+
+    // 3. Make the driving smoother
+    forward = m_xLimiter.calculate(forward) * m_driverMaxSpeedMPS;
+    strafe = m_yLimiter.calculate(strafe) * m_driverMaxSpeedMPS;
+    turn = m_turningLimiter.calculate(turn) * m_driverMaxAngularVelocity;
+
+    set(forward, strafe, turn, fieldRelative);
+  }
+
+
+  /**
+   * Swerve drive function
+   *
+   * @param forward         forward meters per second, strafe facing left from alliance wall
+   * @param strafe          strafe meters per second, forward away from alliance wall
+   * @param radianPerSecond radians per second
+   * @param fieldRelative   field relative
+   */
+  public void set(double forward, double strafe, double radianPerSecond, boolean fieldRelative)
+  {
+    ChassisSpeeds node = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(forward, strafe, radianPerSecond,
+                                                                               getRotation())
+                                       : new ChassisSpeeds(forward, strafe, radianPerSecond);
+    set(node);
+  }
+
+  /**
+   * Set the swerve drive module states.
+   *
+   * @param node Chassis speeds to set the swerve module too.
+   */
+  public void set(ChassisSpeeds node)
+  {
+
+    // Taken from https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5?u=nstrike
+    double timerDt = (m_driveTimer.get() - m_timerPrev);
+    Pose2d robot_pose_vel = new Pose2d(node.vxMetersPerSecond * timerDt,
+                                       node.vyMetersPerSecond * timerDt,
+                                       Rotation2d.fromRadians(node.omegaRadiansPerSecond * timerDt));
+    Twist2d twist_vel = new Pose2d(0, 0, new Rotation2d(0)).log(robot_pose_vel);
+    ChassisSpeeds updated_chassis_speeds = new ChassisSpeeds(
+        twist_vel.dx / timerDt, twist_vel.dy / timerDt, twist_vel.dtheta / timerDt);
+
+    SwerveModuleState2[] moduleStates = m_swerveKinematics.toSwerveModuleStates(updated_chassis_speeds);
+//        new Translation2d((m_frontLeft.swerveModuleLocation.getX() + m_frontRight.swerveModuleLocation.getX()) / 2,
+//                          (m_frontLeft.swerveModuleLocation.getY() + m_backLeft.swerveModuleLocation.getY()) / 2));
+
+    setModuleStates(moduleStates);
+    m_prevChassisSpeed = updated_chassis_speeds;
+    m_timerPrev = m_driveTimer.get();
+
+    try
+    {
+      if (!Robot.isReal())
+      {
+        m_angle += updated_chassis_speeds.omegaRadiansPerSecond * 0.02;
+      }
+      this.update();
+      m_field.setRobotPose(m_swervePoseEstimator.getEstimatedPosition());
+    } catch (Exception e)
+    {
+      System.err.println("Cannot update SwerveDrive Odometry!");
+    }
+  }
+
+  /**
+   * Set the swerve module states given an array of states. Normalize the wheel speeds to abide by maximum supplied
+   *
+   * @param states Module states in a specified order. [front left, front right, back left, back right]
+   * @throws RuntimeException If the CANCoder is inaccessible or not configured.
+   */
+  public void setModuleStates(SwerveModuleState2[] states)
+  {
+    feedWatchdog(); // Required
+    SwerveDriveKinematics2.desaturateWheelSpeeds(states, m_physicalMaxSpeedMPS);
+    m_frontLeft.setState(states[0]);
+    m_frontRight.setState(states[1]);
+    m_backLeft.setState(states[2]);
+    m_backRight.setState(states[3]);
+  }
+
+  /**
+   * Sets the wheels into an X formation to prevent movement.
+   */
+  public void setX()
+  {
+    setModuleStates(new SwerveModuleState2[]{new SwerveModuleState2(0, Rotation2d.fromDegrees(45), 0),
+                                             new SwerveModuleState2(0, Rotation2d.fromDegrees(-45), 0),
+                                             new SwerveModuleState2(0, Rotation2d.fromDegrees(-45), 0),
+                                             new SwerveModuleState2(0, Rotation2d.fromDegrees(45), 0)});
+  }
+
+  /**
+   * Invert the gyroscope reading.
+   *
+   * @param isInverted Inversion of the gryoscope, true is inverted.
+   */
+  public void setGyroInverted(boolean isInverted)
+  {
+    m_gyroInverted = isInverted;
+  }
+
+  /**
+   * Get the current robot rotation.
+   *
+   * @return {@link Rotation2d} of the robot.
+   */
+  public Rotation2d getRotation()
+  {
+    if (Robot.isReal())
+    {
+      return m_gyroInverted ? Rotation2d.fromDegrees(360 - m_pigeonIMU.getYaw()) : Rotation2d.fromDegrees(
+          m_pigeonIMU.getYaw());
+    } else
+    {
+      return new Rotation2d(m_angle);
+    }
+  }
+
+  /**
+   * Get the current Pose, used in autonomous.
+   *
+   * @return Current pose based off odometry.
+   */
+  public Pose2d getPose()
+  {
+    return m_swervePoseEstimator.getEstimatedPosition();
+  }
+
+  /**
+   * Get current swerve module positions in order.
+   *
+   * @return Swerve module positions array.
+   */
+  public SwerveModulePosition[] getPositions()
+  {
+    return new SwerveModulePosition[]{m_frontLeft.getPosition(), m_frontRight.getPosition(),
+                                      m_backLeft.getPosition(), m_backRight.getPosition()};
+  }
+
+  /**
+   * Get current swerve module states in order.
+   *
+   * @return Swerve module states array.
+   */
+  public SwerveModuleState2[] getStates()
+  {
+    return new SwerveModuleState2[]{m_frontLeft.getState(), m_frontRight.getState(),
+                                    m_backLeft.getState(), m_backRight.getState()};
+  }
+
+  /**
+   * Reset the odometry given the position and using current rotation from the PigeonIMU 2.
+   *
+   * @param pose Current position on the field.
+   */
+  public void resetOdometry(Pose2d pose)
+  {
+    m_swervePoseEstimator.resetPosition(getRotation(), getPositions(), pose);
+  }
+
+  /**
+   * Resets the drive encoders to currently read a position of 0.
+   */
+  public void resetEncoders()
+  {
+    m_frontLeft.resetEncoders();
+    m_backLeft.resetEncoders();
+    m_frontRight.resetEncoders();
+    m_backRight.resetEncoders();
+  }
+
+  /**
+   * Set the current rotation of the gyroscope (pigeonIMU 2) to 0.
+   */
+  public void zeroGyro()
+  {
+    m_pigeonIMU.setYaw(0);
+  }
+
+  /**
+   * Stop all running and turning motors.
+   */
+  @Override
+  public void stopMotor()
+  {
+    feedWatchdog();
+    m_frontLeft.stopMotor();
+    m_frontRight.stopMotor();
+    m_backLeft.stopMotor();
+    m_backRight.stopMotor();
+  }
+
+  /**
+   * Update swerve module parameters based on data in the dashboard.
+   */
+  public void subscribe()
+  {
+    m_frontRight.subscribe();
+    m_frontLeft.subscribe();
+    m_backLeft.subscribe();
+    m_backRight.subscribe();
+  }
+
+  /**
+   * Publish data from the Swerve Modules to the dashboard.
+   *
+   * @param level Verbosity level in terms of CAN utilization.
+   */
+  public void publish(Verbosity level)
+  {
+    m_frontRight.publish(level);
+    m_frontLeft.publish(level);
+    m_backRight.publish(level);
+    m_backLeft.publish(level);
+  }
+
+  /**
+   * Set the PIDF coefficients for the closed loop PID onboard the motor controller. Tuning the PID
+   * <p>
+   * <b>P</b> = .5 and increase it by .1 until oscillations occur, then decrease by .05 then .005 until oscillations
+   * stop and angle is perfect or near perfect.
+   * </p>
+   * <p>
+   * <b>I</b> = 0 tune this if your PID never quite reaches the target, after tuning <b>D</b>. Increase this by
+   * <b>P</b>*.01 each time and adjust accordingly.
+   * </p>
+   * <p>
+   * <b>D</b> = 0 tune this if the PID accelerates too fast, it will smooth the motion. Increase this by <b>P</b>*10
+   * and adjust accordingly.
+   * </p>
+   * <p>
+   * <b>F</b> = 0 tune this if the PID is being used for velocity, the <b>F</b> is multiplied by the target and added
+   * to the voltage output. Increase this by 0.01 until the PIDF reaches the desired state in a fast enough manner.
+   * </p>
+   * Documentation for this is best described by CTRE <a
+   * href="https://docs.ctre-phoenix.com/en/stable/ch16_ClosedLoop.html#position-closed-loop-control-mode">here</a>.
+   *
+   * @param p               Proportional gain for closed loop. This is multiplied by closed loop error in sensor units.
+   * @param i               Integral gain for closed loop. This is multiplied by closed loop error in sensor units every
+   *                        PID Loop.
+   * @param d               Derivative gain for closed loop. This is multiplied by derivative error (sensor units per
+   *                        PID loop).
+   * @param f               Feed Fwd gain for Closed loop.
+   * @param integralZone    Integral Zone can be used to auto clear the integral accumulator if the sensor pos is too
+   *                        far from the target. This prevents unstable oscillation if the kI is too large. Value is in
+   *                        sensor units.
+   * @param moduleMotorType Swerve drive motor type.
+   */
+  public void setPIDF(double p, double i, double d, double f, double integralZone,
+                      ModuleMotorType moduleMotorType)
+  {
+    m_frontRight.setPIDF(p, i, d, f, integralZone, moduleMotorType);
+    m_frontLeft.setPIDF(p, i, d, f, integralZone, moduleMotorType);
+    m_backRight.setPIDF(p, i, d, f, integralZone, moduleMotorType);
+    m_backLeft.setPIDF(p, i, d, f, integralZone, moduleMotorType);
+  }
+
+  /**
+   * Synchronize internal steering encoders with the absolute encoder.
+   */
+  public void synchronizeEncoders()
+  {
+    m_backRight.synchronizeSteeringEncoder();
+    m_backLeft.synchronizeSteeringEncoder();
+    m_frontRight.synchronizeSteeringEncoder();
+    m_frontLeft.synchronizeSteeringEncoder();
+  }
+
+  /**
+   * Get the description of the robot drive base.
+   *
+   * @return string of the RobotDriveBase
+   */
+  @Override
+  public String getDescription()
+  {
+    return "Swerve Drive Base";
+  }
+
+  /**
+   * Initializes this {@link Sendable} object.
+   *
+   * @param builder sendable builder
+   */
+  @Override
+  public void initSendable(SendableBuilder builder)
+  {
+//    builder.setSmartDashboardType("SwerveDrive");
+//    SendableRegistry.addChild(this, m_frontLeft);
+//    SendableRegistry.addChild(this, m_frontRight);
+//    SendableRegistry.addChild(this, m_backLeft);
+//    SendableRegistry.addChild(this, m_backRight);
+//    SendableRegistry.addChild(this, m_field);
+//    SendableRegistry.addChild(this, m_pigeonIMU);
+  }
+
+  /**
+   * Closes this resource, relinquishing any underlying resources. This method is invoked automatically on objects
+   * managed by the {@code try}-with-resources statement.
+   *
+   * <p>While this interface method is declared to throw {@code
+   * Exception}, implementers are <em>strongly</em> encouraged to declare concrete implementations of the {@code close}
+   * method to throw more specific exceptions, or to throw no exception at all if the close operation cannot fail.
+   *
+   * <p> Cases where the close operation may fail require careful
+   * attention by implementers. It is strongly advised to relinquish the underlying resources and to internally
+   * <em>mark</em> the resource as closed, prior to throwing the exception. The {@code close} method is unlikely to be
+   * invoked more than once and so this ensures that the resources are released in a timely manner. Furthermore it
+   * reduces problems that could arise when the resource wraps, or is wrapped, by another resource.
+   *
+   * <p><em>Implementers of this interface are also strongly advised
+   * to not have the {@code close} method throw {@link InterruptedException}.</em>
+   * <p>
+   * This exception interacts with a thread's interrupted status, and runtime misbehavior is likely to occur if an
+   * {@code InterruptedException} is {@linkplain Throwable#addSuppressed suppressed}.
+   * <p>
+   * More generally, if it would cause problems for an exception to be suppressed, the {@code AutoCloseable.close}
+   * method should not throw it.
+   *
+   * <p>Note that unlike the {@link Closeable#close close}
+   * method of {@link Closeable}, this {@code close} method is <em>not</em> required to be idempotent.  In other words,
+   * calling this {@code close} method more than once may have some visible side effect, unlike {@code Closeable.close}
+   * which is required to have no effect if called more than once.
+   * <p>
+   * However, implementers of this interface are strongly encouraged to make their {@code close} methods idempotent.
+   *
+   * @throws Exception if this resource cannot be closed
+   */
+  @Override
+  public void close() throws Exception
+  {
+    SendableRegistry.remove(this);
+  }
+
+  /**
+   * Helper class for easier swerve module creation
+   *
+   * @param <DriveMotorType>    The motor type for the drive motor on the swerve moduule.
+   * @param <SteeringMotorType> The motor type for the steering motor on the module.
+   * @param <AbsoluteEncoder>   The absolute encoder type.
+   */
+  public static class SwerveModuleConfig<DriveMotorType extends MotorController,
+      SteeringMotorType extends MotorController,
+      AbsoluteEncoder extends CANCoder>
+  {
+
+    public DriveMotorType       drive;
+    public SteeringMotorType    steering;
+    public AbsoluteEncoder      encoder;
+    public double               angleOffset;
+    public SwerveModuleLocation loc;
+
+    /**
+     * Swerve Module configuration class to define the motor CAN IDs and absolute encoder offset of a swerve module.
+     *
+     * @param driveMotor      Drive motor for the swerve module.
+     * @param steerMotor      Steer motor for the swerve module.
+     * @param encoderSteering CANCoder for the steering motor on the swerve module.
+     * @param offset          Absolute encoder offset.
+     * @param location        Swerve Moduule location on the chassis.
+     */
+    public SwerveModuleConfig(DriveMotorType driveMotor, SteeringMotorType steerMotor, AbsoluteEncoder encoderSteering,
+                              double offset,
+                              SwerveModuleLocation location)
+    {
+      drive = driveMotor;
+      steering = steerMotor;
+      angleOffset = offset;
+      encoder = encoderSteering;
+      loc = location;
+    }
+
+    /**
+     * Create the swerve module from the configuration.
+     *
+     * @param driveGearRatio            Drive gear ratio in form of (rotation:1 AKA rotations/1) to get the encoder
+     *                                  ticks per rotation.
+     * @param steerGearRatio            Steering motor gear ratio (usually 12.8:1 for MK4 in form of rotations:1 or
+     *                                  rotations/1), only applied if using Neo's.
+     * @param wheelDiameterMeters       The wheel diameter of the swerve drive module in meters.
+     * @param wheelBaseMeters           The Distance between front and back wheels of the robot in meters.
+     * @param driveTrainWidthMeters     The Distance between centers of right and left wheels in meters.
+     * @param steeringMotorFreeSpeedRPM The RPM free speed of the steering motor.
+     * @param maxSpeedMPS               The maximum drive speed in meters per second.
+     * @param maxDriveAcceleration      The maximum drive acceleration in meters^2 per second.
+     * @param steeringInverted          The steering motor is inverted.
+     * @param drivingInverted           The driving motor is inverted.
+     * @return The Swerve Module.
+     */
+    public SwerveModule<DriveMotorType, SteeringMotorType, AbsoluteEncoder> createModule(double driveGearRatio,
+                                                                                         double steerGearRatio,
+                                                                                         double wheelDiameterMeters,
+                                                                                         double wheelBaseMeters,
+                                                                                         double driveTrainWidthMeters,
+                                                                                         double steeringMotorFreeSpeedRPM,
+                                                                                         double maxSpeedMPS,
+                                                                                         double maxDriveAcceleration,
+                                                                                         boolean steeringInverted,
+                                                                                         boolean drivingInverted)
+    {
+      return new SwerveModule<>(drive, steering, encoder, loc,
+                                driveGearRatio, steerGearRatio,
+                                angleOffset, wheelDiameterMeters,
+                                wheelBaseMeters,
+                                driveTrainWidthMeters,
+                                steeringMotorFreeSpeedRPM,
+                                maxSpeedMPS, maxDriveAcceleration, .6, .4, steeringInverted, drivingInverted);
+    }
+  }
+
+}

swervelib/SwerveEncoder.java

@@ -0,0 +1,102 @@
+package frc.robot.subsystems.swervedrive.swerve;
+
+import com.ctre.phoenix.sensors.CANCoder;
+import com.ctre.phoenix.sensors.MagnetFieldStrength;
+import com.revrobotics.AbsoluteEncoder;
+import edu.wpi.first.wpilibj.AnalogEncoder;
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
+import frc.robot.subsystems.swervedrive.swerve.encoders.CTRECANCoder;
+import frc.robot.subsystems.swervedrive.swerve.encoders.PWMAnalogEncoder;
+import frc.robot.subsystems.swervedrive.swerve.encoders.PWMDutyCycleEncoder;
+import frc.robot.subsystems.swervedrive.swerve.encoders.REVAbsoluteEncoder;
+
+/**
+ * Swerve Encoder class definition for common interfaces.
+ */
+public abstract class SwerveEncoder<AbsoluteEncoderType>
+{
+
+  /**
+   * The encoder can be directly referenced for configuration purposes of onboard PIDs.
+   */
+  public AbsoluteEncoderType m_encoder;
+
+  /**
+   * Get the SwerveEncoder class from the given encoder types
+   *
+   * @param encoder               Encoder
+   * @param <AbsoluteEncoderType> One of DutyCycleEncoder, AnalogEncoder, AbsoluteEncoder, and CANCoder
+   * @return SwerveEncoder subclass
+   */
+  public static <AbsoluteEncoderType> SwerveEncoder<?> fromEncoder(AbsoluteEncoderType encoder)
+  {
+    if (encoder instanceof CANCoder)
+    {
+      return new CTRECANCoder((CANCoder) encoder);
+    } else if (encoder instanceof DutyCycleEncoder)
+    {
+      return new PWMDutyCycleEncoder((DutyCycleEncoder) encoder);
+    } else if (encoder instanceof AnalogEncoder)
+    {
+      return new PWMAnalogEncoder((AnalogEncoder) encoder);
+    } else if (encoder instanceof AbsoluteEncoder)
+    {
+      return new REVAbsoluteEncoder((AbsoluteEncoder) encoder);
+    }
+
+    return null;
+  }
+
+  /**
+   * Configure the absolute encoder if possible.
+   */
+  public abstract void configure();
+
+  /**
+   * Reset encoder to factory default settings, if possible.
+   */
+  public abstract void factoryDefault();
+
+  /**
+   * Get the magnetic field strength, if available.
+   *
+   * @return CTRE MagneticFieldStrength Enum.
+   */
+  public abstract MagnetFieldStrength getMagnetFieldStrength();
+
+  /**
+   * Get the absolute position in degrees.
+   *
+   * @return Absolute position (0, 360]
+   */
+  public abstract double getAbsolutePosition();
+
+  /**
+   * Get the velocity of the absolute encoder in degrees per second.
+   *
+   * @return Velocity in degrees per second.
+   */
+  public abstract double getVelocity();
+
+  /**
+   * Configure the magnetic offset for the AbsoluteEncoder.
+   *
+   * @param offset Offset in degrees.
+   */
+  public abstract void setOffset(double offset);
+
+  /**
+   * Is the encoder reachable?
+   *
+   * @return True if reachable, false otherwise.
+   */
+  public abstract boolean reachable();
+
+  /**
+   * Configure the sensor direction
+   *
+   * @param isInverted Inverted or not.
+   */
+  public abstract void setInverted(boolean isInverted);
+
+}

swervelib/SwerveModule.java

@@ -0,0 +1,938 @@
+package frc.robot.subsystems.swervedrive.swerve;
+
+import static java.util.Objects.requireNonNull;
+
+import com.ctre.phoenix.motorcontrol.can.TalonFX;
+import com.ctre.phoenix.sensors.CANCoder;
+import com.ctre.phoenix.sensors.MagnetFieldStrength;
+import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.CANSparkMax;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import edu.wpi.first.wpilibj.AnalogEncoder;
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj.shuffleboard.SimpleWidget;
+import frc.robot.Robot;
+import frc.robot.subsystems.swervedrive.swerve.SwerveMotor.ModuleMotorType;
+import frc.robot.subsystems.swervedrive.swerve.kinematics.SwerveModuleState2;
+import java.io.Closeable;
+import java.util.HashMap;
+import java.util.Map;
+
+/**
+ * Swerve module for representing a single swerve module of the robot.
+ *
+ * @param <DriveMotorType>      Main motor type that drives the wheel.
+ * @param <AngleMotorType>      Motor that controls the angle of the wheel.
+ * @param <AbsoluteEncoderType> Absolute encoder for the swerve drive module.
+ */
+public class SwerveModule<DriveMotorType extends MotorController, AngleMotorType extends MotorController,
+    AbsoluteEncoderType>
+    implements MotorController, Sendable, AutoCloseable
+{
+
+  /**
+   * Swerve Module location object relative to the center of the robot.
+   */
+  public final  Translation2d                 swerveModuleLocation;
+  /**
+   * Motor Controllers for drive motor of the swerve module.
+   */
+  public final  SwerveMotor                   driveMotor;
+  /***
+   * Motor Controller for the turning motor of the swerve drive module.
+   */
+  public final  SwerveMotor                   turningMotor;
+  /**
+   * Enum representing the swerve module's location on the robot, assuming square.
+   */
+  private final SwerveModuleLocation          swerveLocation;
+  /**
+   * Absolute encoder for the swerve module.
+   */
+  private final SwerveEncoder<?>              absoluteEncoder;
+  /**
+   * The Distance between centers of right and left wheels in meters.
+   */
+  private final double                        driveTrainWidth;
+  /**
+   * The Distance between front and back wheels of the robot in meters.
+   */
+  private final double                        wheelBase;
+  /**
+   * Drive feedforward for PID when driving by velocity.
+   */
+  private final SimpleMotorFeedforward        driveFeedforward;
+  /**
+   * kV for steering feedforward.
+   */
+  private final double                        steeringKV;
+  private final ShuffleboardTab               moduleTab;
+  private final HashMap<String, SimpleWidget> NT4Entries               = new HashMap<>();
+  /**
+   * Angle offset of the CANCoder at initialization.
+   */
+  public        double                        angleOffset              = 0;
+  /**
+   * Maximum speed in meters per second, used to eliminate unnecessary movement of the module.
+   */
+  public        double                        maxDriveSpeedMPS;
+  /**
+   * Inverted drive motor.
+   */
+  private       boolean                       invertedDrive            = false;
+  /**
+   * Inverted turning motor.
+   */
+  private       boolean                       invertedTurn             = false;
+  /**
+   * Power to drive motor from -1 to 1.
+   */
+  private       double                        drivePower               = 0;
+  /**
+   * Store the last angle for optimization.
+   */
+  private       double                        targetAngle;
+  /**
+   * Angular velocity in radians per second.
+   */
+  private       double                        targetAngularVelocityRPS = 0;
+  /**
+   * Target velocity for the swerve module.
+   */
+  private       double                        targetVelocity           = 0;
+
+  ///////////////////////////// CONFIGURATION FUNCTIONS SECTION ///////////////////////////////////////////////////
+
+  /**
+   * Swerve module constructor. Both motors <b>MUST</b> be a {@link MotorController} class. It is recommended to create
+   * a command to reset the encoders when triggered and
+   *
+   * @param mainMotor                 Main drive motor. Must be a {@link MotorController} type.
+   * @param angleMotor                Angle motor for controlling the angle of the swerve module.
+   * @param encoder                   Absolute encoder for the swerve module.
+   * @param driveGearRatio            Drive gear ratio in form of (rotation:1 AKA rotations/1) to get the encoder ticks
+   *                                  per rotation.
+   * @param steerGearRatio            Steering motor gear ratio (usually 12.8:1 for MK4 in form of rotations:1 or
+   *                                  rotations/1), only applied if using Neo's.
+   * @param swervePosition            Swerve Module position on the robot.
+   * @param steeringOffsetDegrees     The current offset of the absolute encoder from 0 in degrees.
+   * @param wheelDiameterMeters       The wheel diameter of the swerve drive module in meters.
+   * @param wheelBaseMeters           The Distance between front and back wheels of the robot in meters.
+   * @param driveTrainWidthMeters     The Distance between centers of right and left wheels in meters.
+   * @param steeringMotorFreeSpeedRPM The RPM free speed of the steering motor.
+   * @param maxSpeedMPS               The maximum drive speed in meters per second.
+   * @param maxDriveAcceleration      The maximum drive acceleration in meters^2 per second.
+   * @param drivingPowerLimit         The power limit for the closed loop PID of the driver motor.
+   * @param steeringPowerLimit        The power limit for the closed loop PID of the steering motor.
+   * @param steeringInverted          The steering motor is inverted.
+   * @param drivingInverted           The driving motor is inverted.
+   * @throws RuntimeException if an assertion fails or invalid swerve module location is given.
+   */
+  public SwerveModule(DriveMotorType mainMotor, AngleMotorType angleMotor, AbsoluteEncoderType encoder,
+                      SwerveModuleLocation swervePosition, double driveGearRatio, double steerGearRatio,
+                      double steeringOffsetDegrees, double wheelDiameterMeters, double wheelBaseMeters,
+                      double driveTrainWidthMeters, double steeringMotorFreeSpeedRPM, double maxSpeedMPS,
+                      double maxDriveAcceleration, double drivingPowerLimit, double steeringPowerLimit,
+                      boolean steeringInverted, boolean drivingInverted)
+  {
+    // Steps to configure swerve drive are as follows
+    // 1.  Set Current limit of turning motor to 20 amps
+    // 2.  Enable voltage compensation with optimal battery voltage
+    // 3.  Configure CANCoder
+    // 4.  Set inverted motors.
+    // 5.  Configure status frames
+    // 6.  Set all motors to brake mode.
+    // 7.  Set velocity and position conversion factors on drive motor encoder.
+    // 8.  Set PIDF with integral zone on drive motor controller PID.
+    // 9.  Set velocity and position conversion factors on turning motor controller. (Usually P=0.01,I=0,D=0,F=0,IZ=1)
+    // 10. Set PIDF with integral zone on turning motor controller.
+    // 11. Check all CAN devices are active.
+    // 12. Reset the angle on the internal encoder to the absolute encoder.
+    requireNonNull(mainMotor);
+    requireNonNull(angleMotor);
+    requireNonNull(encoder);
+
+    this.wheelBase = wheelBaseMeters;
+    this.driveTrainWidth = driveTrainWidthMeters;
+
+    assert mainMotor instanceof CANSparkMax || mainMotor instanceof TalonFX;
+    assert angleMotor instanceof CANSparkMax || angleMotor instanceof TalonFX;
+    assert encoder instanceof DutyCycleEncoder || encoder instanceof AnalogEncoder || encoder instanceof CANCoder ||
+           encoder instanceof AbsoluteEncoder;
+
+    absoluteEncoder = SwerveEncoder.fromEncoder(encoder);
+    assert absoluteEncoder != null;
+
+    absoluteEncoder.factoryDefault();
+
+    driveMotor = SwerveMotor.fromMotor(mainMotor,
+                                       absoluteEncoder,
+                                       ModuleMotorType.DRIVE,
+                                       driveGearRatio,
+                                       wheelDiameterMeters,
+                                       0,
+                                       drivingPowerLimit);
+
+    turningMotor = SwerveMotor.fromMotor(angleMotor,
+                                         absoluteEncoder,
+                                         ModuleMotorType.TURNING,
+                                         steerGearRatio,
+                                         wheelDiameterMeters,
+                                         steeringMotorFreeSpeedRPM,
+                                         steeringPowerLimit);
+
+    swerveLocation = swervePosition;
+    swerveModuleLocation = getSwerveModulePosition(swervePosition);
+
+    assert driveMotor != null;
+    assert turningMotor != null;
+
+    // Set the maximum speed for each swerve module for use when trying to optimize movements.
+    // Drive feedforward gains
+    //        public static final double KS = 0;
+    //        public static final double KV = 12 / MAX_SPEED; // Volt-seconds per meter (max voltage divided by max
+    //        speed)
+    //        public static final double KA = 12 / MAX_ACCELERATION; // Volt-seconds^2 per meter (max voltage divided
+    //        by max accel)
+    maxDriveSpeedMPS = maxSpeedMPS;
+    driveFeedforward = new SimpleMotorFeedforward(0, 12 / maxDriveSpeedMPS, 12 / maxDriveAcceleration);
+    steeringKV = (12 * 60) / (steeringMotorFreeSpeedRPM * Math.toRadians(360 / steerGearRatio));
+
+    driveMotor.setInverted(drivingInverted);
+    invertedDrive = drivingInverted;
+    turningMotor.setInverted(steeringInverted);
+    invertedTurn = steeringInverted;
+
+    absoluteEncoder.configure();
+
+    setAngleOffset(steeringOffsetDegrees);
+    resetEncoders();
+
+    // Convert CANCoder to read data as unsigned 0 to 360 for synchronization purposes.
+    absoluteEncoder.configure();
+
+    assert activeCAN();
+
+    resetEncoders();
+    synchronizeSteeringEncoder();
+
+    driveMotor.saveConfig();
+    turningMotor.saveConfig();
+
+    // targetAngle = getState().angle.getDegrees();
+    targetAngle = 0;
+
+    if (!remoteIntegratedEncoder() && Robot.isReal())
+    {
+      Robot.getInstance().addPeriodic(this::synchronizeSteeringEncoder, 0.5);
+    }
+
+    // Shuffleboard Data
+    moduleTab = Shuffleboard.getTab(SwerveModule.SwerveModuleLocationToString(swerveLocation));
+    publish(Verbosity.SETUP);
+
+  }
+
+  /**
+   * Convert {@link SwerveModuleLocation} to {@link String} representation.
+   *
+   * @param swerveLocation Swerve position to convert.
+   * @return {@link String} name of the {@link SwerveModuleLocation} enum.
+   */
+  public static String SwerveModuleLocationToString(SwerveModuleLocation swerveLocation)
+  {
+    switch (swerveLocation)
+    {
+      case FrontLeft:
+        return "Front Left Module";
+      case BackLeft:
+        return "Back Left Module";
+      case FrontRight:
+        return "Front Right Module";
+      case BackRight:
+        return "Back Right Module";
+      default:
+        return "Unknown";
+    }
+  }
+
+  /**
+   * Reset the REV encoders onboard the SparkMax's to 0, and set's the drive motor to position to 0 and synchronizes the
+   * internal steering encoders with the absolute encoder.
+   */
+  public void resetEncoders()
+  {
+    driveMotor.setEncoder(0);
+
+    if (!remoteIntegratedEncoder())
+    {
+      turningMotor.setEncoder(0);
+      synchronizeSteeringEncoder();
+    }
+  }
+
+  /**
+   * Synchronizes the internal encoder for the steering motor with the value from the absolute encoder.
+   */
+  public void synchronizeSteeringEncoder()
+  {
+    if (!remoteIntegratedEncoder())
+    {
+      if (absoluteEncoder.getMagnetFieldStrength() != MagnetFieldStrength.Good_GreenLED)
+      {
+        System.err.println("CANCoder magnetic field strength is unacceptable, will not synchronize encoders.");
+        return;
+      }
+      turningMotor.setEncoder(absoluteEncoder.getAbsolutePosition() - angleOffset);
+    }
+  }
+
+  /////////////////////// END OF CONFIGURATION FUNCTIONS SECTION //////////////////////////
+
+  ////////////////////////////// STATUS FUNCTIONS SECTION //////////////////////////////////////////////////////
+
+  /**
+   * Set the PIDF coefficients for the closed loop PID onboard the motor controller. Tuning the PID
+   * <p>
+   * <b>P</b> = .5 and increase it by .1 until oscillations occur, then decrease by .05 then .005 until oscillations
+   * stop and angle is perfect or near perfect.
+   * </p>
+   * <p>
+   * <b>I</b> = 0 tune this if your PID never quite reaches the target, after tuning <b>D</b>. Increase this by
+   * <b>P</b>*.01 each time and adjust accordingly.
+   * </p>
+   * <p>
+   * <b>D</b> = 0 tune this if the PID accelerates too fast, it will smooth the motion. Increase this by <b>P</b>*10
+   * and adjust accordingly.
+   * </p>
+   * <p>
+   * <b>F</b> = 0 tune this if the PID is being used for velocity, the <b>F</b> is multiplied by the target and added
+   * to the voltage output. Increase this by 0.01 until the PIDF reaches the desired state in a fast enough manner.
+   * </p>
+   * Documentation for this is best described by CTRE <a
+   * href="https://docs.ctre-phoenix.com/en/stable/ch16_ClosedLoop.html#position-closed-loop-control-mode">here</a>.
+   *
+   * @param p               Proportional gain for closed loop. This is multiplied by closed loop error in sensor units.
+   * @param i               Integral gain for closed loop. This is multiplied by closed loop error in sensor units every
+   *                        PID Loop.
+   * @param d               Derivative gain for closed loop. This is multiplied by derivative error (sensor units per
+   *                        PID loop).
+   * @param f               Feed Fwd gain for Closed loop.
+   * @param integralZone    Integral Zone can be used to auto clear the integral accumulator if the sensor pos is too
+   *                        far from the target. This prevents unstable oscillation if the kI is too large. Value is in
+   *                        sensor units.
+   * @param moduleMotorType Swerve drive motor type.
+   */
+  public void setPIDF(double p, double i, double d, double f, double integralZone,
+                      ModuleMotorType moduleMotorType)
+  {
+    if ((moduleMotorType == ModuleMotorType.DRIVE))
+    {
+      driveMotor.setPIDF(p, i, d, f, integralZone);
+    } else
+    {
+      turningMotor.setPIDF(p, i, d, f, integralZone);
+    }
+  }
+
+  //////////////////////////// END OF STATUS FUNCTIONS SECTION ////////////////////////////////////////////////
+
+  //////////////////////////// ODOMETRY AND STATE FUNCTIONS SECTION ///////////////////////////////////////////
+
+  /**
+   * Configure the magnetic offset in the CANCoder.
+   *
+   * @param offset Magnetic offset in degrees.
+   */
+  public void setAngleOffset(double offset)
+  {
+    // System.out.println(offset);
+    angleOffset = offset;
+    absoluteEncoder.setOffset(0);
+  }
+
+  /**
+   * Check that the link is good on the swerve module and CAN bus is able to retrieve data.
+   *
+   * @return true on all devices are accessible over CAN.
+   */
+  public boolean activeCAN()
+  {
+    boolean drive = driveMotor.reachable(), turn = turningMotor.reachable(), encoder = absoluteEncoder.reachable();
+
+    return drive && turn && encoder;
+  }
+
+  /**
+   * Checks if the absolute encoder is able to be read directly by the motor controller instead of using the
+   * synchronizeEncoder() function periodically.
+   *
+   * @return If the absolute sensor in the steering motor is directly accessed by the motor controller.
+   */
+  public boolean remoteIntegratedEncoder()
+  {
+    return turningMotor.remoteIntegratedEncoder();
+  }
+
+  /**
+   * Set the angle of the swerve module.
+   *
+   * @param angle       Angle in degrees.
+   * @param feedforward The feedforward for the PID.
+   */
+  public void setAngle(double angle, double feedforward)
+  {
+    turningMotor.setTarget(Math.round(angle) % 180, feedforward);
+  }
+
+  /**
+   * Set the angle of the swerve module without feedforward.
+   *
+   * @param angle Angle in degrees.
+   */
+  private void setAngle(double angle)
+  {
+    turningMotor.setTarget(angle, 0);
+  }
+
+  /**
+   * Set the drive motor velocity in MPS.
+   *
+   * @param velocity Velocity in MPS.
+   */
+  public void setVelocity(double velocity)
+  {
+    targetVelocity = velocity;
+    driveMotor.setTarget(velocity, driveFeedforward.calculate(velocity));
+  }
+
+  /**
+   * Get the module state.
+   *
+   * @return SwerveModuleState with the encoder inputs.
+   * @throws RuntimeException Exception if CANCoder doesnt exist
+   */
+  public SwerveModuleState2 getState()
+  {
+    double     mps = driveMotor.get();
+    double     angularVelocityRPS;
+    Rotation2d angle;
+    angle = Rotation2d.fromDegrees(Math.round(
+        Robot.isReal() ? absoluteEncoder.getAbsolutePosition() - angleOffset : targetAngle));
+    angularVelocityRPS = Robot.isReal() ? Math.toRadians(absoluteEncoder.getVelocity()) : targetAngularVelocityRPS;
+    //^ Convert degrees per second to radians per second.
+    return new SwerveModuleState2(mps, angle, angularVelocityRPS);
+  }
+
+  /////////////////// END OF ODOMETRY AND STATE FUNCTIONS SECTION ////////////////////////////////////////
+
+  /////////////////// DIAGNOSTIC AND TUNING FUNCTIONS SECTION ////////////////////////////////////////////
+
+  /**
+   * Set the module speed and angle based off the module state.
+   *
+   * @param state Module state.
+   */
+  public void setState(SwerveModuleState2 state)
+  {
+    // state.angle = state.angle.minus(Rotation2d.fromDegrees(angleOffset));
+    // inspired by https://github.com/first95/FRC2022/blob/1f57d6837e04d8c8a89f4d83d71b5d2172f41a0e/SwervyBot/src/main/java/frc/robot/SwerveModule.java#L22
+    Rotation2d currentAngle = getState().angle;
+//    SwerveModuleState optimizedState = SwerveModuleState.optimize(new SwerveModuleState(state.speedMetersPerSecond,
+//                                                                                        state.angle), currentAngle);
+    SwerveModuleState2 optimizedState = SwerveModuleState2.optimize(state, currentAngle);
+    double             angle          = optimizedState.angle.getDegrees(); // getDegrees returns in the range of -180 to 180 we want 0 to
+    // 360.
+    double velocity = (Math.abs(optimizedState.speedMetersPerSecond) <= (maxDriveSpeedMPS * 0.01)) ? 0
+                                                                                                   :
+                      optimizedState.speedMetersPerSecond;
+    // turn motor code
+    // Prevent rotating module if speed is less then 1%. Prevents Jittering.
+    angle = (Math.abs(optimizedState.speedMetersPerSecond) <= (maxDriveSpeedMPS * 0.01)) ? 0 : angle;
+    setAngle(angle, optimizedState.angularVelocityRadPerSecond * steeringKV);
+    setVelocity(velocity);
+    targetAngle = angle;
+    targetAngularVelocityRPS = optimizedState.angularVelocityRadPerSecond;
+  }
+
+  /**
+   * Get the swerve module position based off the sensors.
+   *
+   * @return Swerve Module position, with the angle as what the angle is supposed to be, not what it actually is.
+   */
+  public SwerveModulePosition getPosition()
+  {
+    return new SwerveModulePosition(driveMotor.getPosition(), Rotation2d.fromDegrees(targetAngle));
+  }
+
+  //////////////////////////// END OF DIAGNOSTIC AND TUNING FUNCTIONS SECTION /////////////////////////
+
+  /**
+   * Subscribe from data within smart dashboard to make changes to the swerve module.
+   */
+  public void subscribe()
+  {
+
+  }
+
+  //////////////////////////// ENUMS SECTION //////////////////////////////////////////////////////////
+
+  /**
+   * Create a widget and add the entry to the hashmap of entries for network tables.
+   *
+   * @param name         Key to display on shuffleboard.
+   * @param defaultValue Default value.
+   * @return Widget that can be modified.
+   */
+  private SimpleWidget addEntry(String name, Object defaultValue)
+  {
+    if (NT4Entries.containsKey(name))
+    {
+      GenericEntry entry = NT4Entries.get(name).getEntry();
+      if (defaultValue instanceof Integer)
+      {
+        entry.setInteger((int) defaultValue);
+      } else if (defaultValue instanceof Double)
+      {
+        entry.setDouble((double) defaultValue);
+      } else if (defaultValue instanceof Boolean)
+      {
+        entry.setBoolean((boolean) defaultValue);
+      } else if (defaultValue instanceof Double[])
+      {
+        entry.setDoubleArray((Double[]) defaultValue);
+      } else
+      {
+        throw new RuntimeException("Cannot publish value for " + name);
+      }
+      return NT4Entries.get(name);
+    }
+    try
+    {
+      SimpleWidget widget = moduleTab.add(name, defaultValue);
+      NT4Entries.put(name, widget);
+      return widget;
+    } catch (Exception e)
+    {
+      System.err.println(e);
+    }
+    throw new RuntimeException("Error creating entry");
+  }
+
+  /**
+   * Publish data to the smart dashboard relating to this swerve module.
+   *
+   * @param level Verbosity level, affects the CAN utilization, on HIGH it will enable the update button.
+   */
+  public void publish(Verbosity level)
+  {
+    boolean goodLaptop = false;
+    switch (level)
+    {
+      case SETUP:
+        // PID
+        addEntry("drive/pid/kP", driveMotor.kP);
+        addEntry("drive/pid/kI", driveMotor.kI);
+        addEntry("drive/pid/kD", driveMotor.kD);
+        addEntry("drive/pid/kF", driveMotor.kF);
+        addEntry("drive/pid/kIZ", driveMotor.kIZ);
+
+        addEntry("steer/pid/kP", turningMotor.kP);
+        addEntry("steer/pid/kI", turningMotor.kI);
+        addEntry("steer/pid/kD", turningMotor.kD);
+        addEntry("steer/pid/kF", turningMotor.kF);
+        addEntry("steer/pid/kIZ", turningMotor.kIZ);
+
+        // Pretty Widget Setup
+        if (goodLaptop)
+        {
+          addEntry("Angle (Degrees)",
+                   new Double[]{targetAngle, turningMotor.get(), absoluteEncoder.getAbsolutePosition()})
+              .withPosition(0, 1)
+              .withSize(4, 4)
+              .withProperties(Map.of("visible time", 5))
+              .withWidget(BuiltInWidgets.kGraph);
+          addEntry("Absolute Angle", absoluteEncoder.getAbsolutePosition())
+              .withPosition(0, 0)
+              .withSize(2, 1);
+          addEntry("Integrated Angle", turningMotor.get())
+              .withPosition(2, 0)
+              .withSize(2, 1);
+          addEntry("Target Angle", targetAngle)
+              .withPosition(8, 4)
+              .withSize(2, 2);
+
+          addEntry("Velocity (Meters Per Second)", new Double[]{targetVelocity, driveMotor.get()})
+              .withPosition(4, 1)
+              .withSize(4, 4)
+              .withProperties(Map.of("visible time", 5))
+              .withWidget(BuiltInWidgets.kGraph);
+          addEntry("Motor Velocity", driveMotor.get())
+              .withPosition(4, 0)
+              .withSize(4, 1);
+          addEntry("Target Velocity", targetVelocity)
+              .withPosition(8, 2)
+              .withSize(2, 2);
+
+          // Inverted Motors.
+          addEntry("Steer Motor Inverted", invertedTurn)
+              .withPosition(0, 5)
+              .withSize(4, 1);
+          addEntry("Drive Motor inverted", invertedDrive)
+              .withPosition(4, 5)
+              .withSize(4, 1);
+
+          addEntry("Absolute Encoder Magnetic Field", absoluteEncoder.getMagnetFieldStrength().value)
+              .withPosition(10, 2)
+              .withSize(2, 1);
+          addEntry("Drive Motor Current", driveMotor.getAmps())
+              .withPosition(10, 5)
+              .withSize(2, 1);
+
+          // Angle Constants
+          addEntry("Absolute Encoder Offset", angleOffset)
+              .withProperties(Map.of("min", -180, "max", 180))
+              .withPosition(10, 3)
+              .withSize(2, 2)
+              .withWidget(BuiltInWidgets.kDial);
+        } else
+        {
+          addEntry("Target Angle", targetAngle)
+              .withProperties(Map.of("min", -180, "max", 180))
+              .withWidget(BuiltInWidgets.kDial)
+              .withPosition(0, 0)
+              .withSize(4, 2);
+          addEntry("Integrated Angle", turningMotor.get())
+              .withProperties(Map.of("min", 0, "max", 360))
+              .withWidget(BuiltInWidgets.kDial)
+              .withPosition(0, 2)
+              .withSize(2, 2);
+          addEntry("Absolute Angle", absoluteEncoder.getAbsolutePosition())
+              .withProperties(Map.of("min", 0, "max", 360))
+              .withWidget(BuiltInWidgets.kDial)
+              .withPosition(2, 2)
+              .withSize(2, 2);
+
+          addEntry("Target Velocity", targetVelocity)
+              .withPosition(4, 0)
+              .withProperties(Map.of("min", -maxDriveSpeedMPS, "max", maxDriveSpeedMPS))
+              .withSize(4, 2)
+              .withWidget(BuiltInWidgets.kNumberBar);
+          addEntry("Motor Velocity", driveMotor.get())
+              .withPosition(4, 2)
+              .withProperties(Map.of("min", -maxDriveSpeedMPS, "max", maxDriveSpeedMPS))
+              .withSize(4, 2)
+              .withWidget(BuiltInWidgets.kNumberBar);
+
+          // Inverted Motors.
+          addEntry("Steer Motor Inverted", invertedTurn)
+              .withPosition(0, 4)
+              .withSize(4, 1);
+          addEntry("Drive Motor inverted", invertedDrive)
+              .withPosition(4, 4)
+              .withSize(4, 1);
+
+          addEntry("Absolute Encoder Magnetic Field", absoluteEncoder.getMagnetFieldStrength().value)
+              .withPosition(8, 2)
+              .withSize(2, 1);
+
+          addEntry("Drive Motor Current", driveMotor.getAmps())
+              .withPosition(10, 2)
+              .withSize(2, 1);
+
+          // Angle Constants
+          addEntry("Absolute Encoder Offset", angleOffset)
+              .withProperties(Map.of("min", -180, "max", 180))
+              .withPosition(8, 3)
+              .withSize(2, 2)
+              .withWidget(BuiltInWidgets.kDial);
+        }
+
+        addEntry("CAN Connection", activeCAN())
+            .withPosition(8, 0)
+            .withSize(4, 2);
+
+      case HIGH:
+        // Update if button is set.
+        // The higher the better, 2 and 3 are what we want.
+        addEntry("Absolute Encoder Magnetic Field", absoluteEncoder.getMagnetFieldStrength().value);
+      case NORMAL:
+        // Steering Encoder Values
+        if (goodLaptop)
+        {
+          addEntry("Angle (Degrees)",
+                   new Double[]{targetAngle, turningMotor.get(), absoluteEncoder.getAbsolutePosition()});
+        }
+        addEntry("Absolute Angle", absoluteEncoder.getAbsolutePosition());
+        addEntry("Integrated Angle", turningMotor.get());
+
+        // Driving Encoder Values
+        if (goodLaptop)
+        {
+          addEntry("Velocity (Meters Per Second)", new Double[]{targetVelocity, driveMotor.get()});
+        }
+        addEntry("Motor Velocity", driveMotor.get());
+        addEntry("Drive Motor Current", driveMotor.getAmps());
+
+        // CAN Bus is accessible
+        addEntry("CAN Connection", activeCAN());
+
+      case LOW:
+        // PID
+        addEntry("Target Velocity", targetVelocity);
+        addEntry("Target Angle", targetAngle);
+
+    }
+  }
+
+  /**
+   * Initializes this {@link Sendable} object.
+   *
+   * @param builder sendable builder
+   */
+  @Override
+  public void initSendable(SendableBuilder builder)
+  {
+    builder.setSmartDashboardType(SwerveModuleLocationToString(swerveLocation) + " SwerveDriveModule");
+    builder.setActuator(true);
+    builder.setSafeState(this::stopMotor);
+  }
+
+  //////////////////////////////////// END OF ENUMS SECTION //////////////////////////////////////////////
+
+  ////////////////////////////////// OVERRIDES SECTION ///////////////////////////////////////////////////
+
+  /**
+   * Closes this resource, relinquishing any underlying resources. This method is invoked automatically on objects
+   * managed by the {@code try}-with-resources statement.
+   *
+   * <p>While this interface method is declared to throw {@code
+   * Exception}, implementers are <em>strongly</em> encouraged to declare concrete implementations of the {@code close}
+   * method to throw more specific exceptions, or to throw no exception at all if the close operation cannot fail.
+   *
+   * <p> Cases where the close operation may fail require careful
+   * attention by implementers. It is strongly advised to relinquish the underlying resources and to internally
+   * <em>mark</em> the resource as closed, prior to throwing the exception. The {@code close} method is unlikely to be
+   * invoked more than once and so this ensures that the resources are released in a timely manner. Furthermore it
+   * reduces problems that could arise when the resource wraps, or is wrapped, by another resource.
+   *
+   * <p><em>Implementers of this interface are also strongly advised
+   * to not have the {@code close} method throw {@link InterruptedException}.</em>
+   * <p>
+   * This exception interacts with a thread's interrupted status, and runtime misbehavior is likely to occur if an
+   * {@code InterruptedException} is {@linkplain Throwable#addSuppressed suppressed}.
+   * <p>
+   * More generally, if it would cause problems for an exception to be suppressed, the {@code AutoCloseable.close}
+   * method should not throw it.
+   *
+   * <p>Note that unlike the {@link Closeable#close close}
+   * method of {@link Closeable}, this {@code close} method is <em>not</em> required to be idempotent.  In other words,
+   * calling this {@code close} method more than once may have some visible side effect, unlike {@code Closeable.close}
+   * which is required to have no effect if called more than once.
+   * <p>
+   * However, implementers of this interface are strongly encouraged to make their {@code close} methods idempotent.
+   */
+  @Override
+  public void close()
+  {
+    SendableRegistry.remove(this);
+  }
+
+  /**
+   * Disable the motor controller.
+   */
+  @Override
+  public void disable()
+  {
+    stopMotor();
+  }
+
+  /**
+   * Stops motor movement. Motor can be moved again by calling set without having to re-enable the motor.
+   */
+  @Override
+  public void stopMotor()
+  {
+//    turningMotor.stop();
+    driveMotor.stop();
+  }
+
+  /**
+   * Common interface for setting the speed of a motor controller.
+   *
+   * @param speed The speed to set. Value should be between -1.0 and 1.0.
+   */
+  @Override
+  public void set(double speed)
+  {
+    drivePower = speed;
+    driveMotor.set(speed);
+  }
+
+  /**
+   * Common interface for getting the current set speed of a motor controller.
+   *
+   * @return The current set speed. Value is between -1.0 and 1.0.
+   */
+  @Override
+  public double get()
+  {
+    return drivePower;
+  }
+
+  /**
+   * Get the swerve module position in {@link Translation2d} from the enum passed.
+   *
+   * @param swerveLocation Swerve module location enum.
+   * @return Location as {@link Translation2d}.
+   * @throws RuntimeException If Enum value is not defined.
+   */
+  private Translation2d getSwerveModulePosition(SwerveModuleLocation swerveLocation)
+  {
+    // Modeling off of https://github.com/Stampede3630/2022-Code/blob/master/src/main/java/frc/robot/SwerveDrive.java
+    switch (swerveLocation)
+    {
+      case FrontLeft:
+        return new Translation2d(wheelBase / 2, driveTrainWidth / 2);
+      case BackLeft:
+        return new Translation2d(-wheelBase / 2, driveTrainWidth / 2);
+      case FrontRight:
+        return new Translation2d(wheelBase / 2, -driveTrainWidth / 2);
+      case BackRight:
+        return new Translation2d(-wheelBase / 2, -driveTrainWidth / 2);
+      default:
+        throw new RuntimeException("Invalid location given");
+    }
+  }
+
+  /**
+   * Set the voltage compensation for the swerve module motor.
+   *
+   * @param nominalVoltage Nominal voltage for operation to output to.
+   */
+  public void setVoltageCompensation(double nominalVoltage)
+  {
+    driveMotor.setVoltageCompensation(nominalVoltage);
+    turningMotor.setVoltageCompensation(nominalVoltage);
+  }
+
+  //////////////////////////////////// END OF OVERRIDES SECTION ////////////////////////////////////////////
+
+  //////////////////////////////////// STATIC FUNCTIONS SECTION ////////////////////////////////////////////
+
+  /**
+   * Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with
+   * voltage compensation. This is useful to protect the motor from current spikes.
+   *
+   * @param currentLimit Current limit in AMPS at free speed.
+   * @param type         Swerve Drive Motor type to configure.
+   */
+  public void setCurrentLimit(int currentLimit, ModuleMotorType type)
+  {
+    if (type == ModuleMotorType.DRIVE)
+    {
+      driveMotor.setCurrentLimit(currentLimit);
+    } else
+    {
+      turningMotor.setCurrentLimit(currentLimit);
+    }
+  }
+
+  /////////////////////////////////// END OF STATIC FUNCTIONS SECTION //////////////////////////////////////////
+
+  /////////////////////////////////// EXTRA FUNCTIONS THAT PROBABLY WONT MATTER ////////////////////////////////
+
+  /**
+   * Set the steering motor to be inverted.
+   *
+   * @param isInverted The state of inversion, true is inverted.
+   */
+  public void setInvertedTurn(boolean isInverted)
+  {
+    invertedTurn = isInverted;
+    turningMotor.setInverted(isInverted);
+  }
+
+  /**
+   * Invert the absolute encoder.
+   *
+   * @param isInverted The state of inversion, true is inverted.
+   */
+  public void setInvertedAbsoluteEncoder(boolean isInverted)
+  {
+    absoluteEncoder.setInverted(isInverted);
+  }
+
+  ////////////// CUSTOM INVERSION FUNCTIONS SECTION //////////////////////////
+
+  /**
+   * Common interface for returning if a motor controller is in the inverted state or not.
+   *
+   * @return isInverted The state of the inversion true is inverted.
+   */
+  @Override
+  public boolean getInverted()
+  {
+    return invertedDrive;
+  }
+
+  /**
+   * Common interface for inverting direction of a motor controller.
+   *
+   * @param isInverted The state of inversion, true is inverted.
+   */
+  @Override
+  public void setInverted(boolean isInverted)
+  {
+    invertedDrive = isInverted;
+    driveMotor.setInverted(isInverted);
+  }
+
+  /**
+   * Swerve Module location on the robot.
+   */
+  public enum SwerveModuleLocation
+  {
+    FrontLeft,
+    BackLeft,
+    FrontRight,
+    BackRight
+  }
+
+  /**
+   * Verbosity levels for data publishing,
+   */
+  public enum Verbosity
+  {
+    /**
+     * The bare minimum and not utilize the CAN bus when reporting data. Only posts data from attributes.
+     */
+    LOW,
+    /**
+     * Utilize the CAN bus minimally.
+     */
+    NORMAL,
+    /**
+     * Extensively use the CAN bus to fetch data and report back.
+     */
+    HIGH,
+    /**
+     * Creates every field for the module.
+     */
+    SETUP
+  }
+
+  ////////////////////////////// END OF CUSTOM INVERSION FUNCTIONS SECTION /////////////////////////////////////
+
+}

swervelib/SwerveMotor.java

@@ -0,0 +1,204 @@
+package frc.robot.subsystems.swervedrive.swerve;
+
+import com.ctre.phoenix.motorcontrol.can.TalonFX;
+import com.revrobotics.CANSparkMax;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import frc.robot.subsystems.swervedrive.swerve.motors.CTRESwerveMotor;
+import frc.robot.subsystems.swervedrive.swerve.motors.REVSwerveMotor;
+
+public abstract class SwerveMotor
+{
+
+  /**
+   * Module motor type.
+   */
+  public ModuleMotorType m_motorType;
+  /**
+   * PIDF Values.
+   */
+  public double          kP, kI, kD, kF, kIZ;
+  /**
+   * Target value for PID.
+   */
+  public double target;
+
+  /**
+   * Constructor for Swerve Motor, expecting CANSparkMax or TalonFX. Clears sticky faults and restores factory
+   * defaults.
+   *
+   * @param motor           Motor controller.
+   * @param absoluteEncoder The absolute encoder used for the module, if the motor is a turning motor and the encoder is
+   *                        compatible it will set the encoder as the remote integrated encoder and does not need
+   *                        periodic synchronization.
+   * @param type            Swerve module motor type.
+   * @param gearRatio       Gear ratio.
+   * @param wheelDiameter   Wheel diameter in meters.
+   * @param freeSpeedRPM    Free speed RPM of the motor.
+   * @param powerLimit      Power limit for the motor.
+   * @param <MotorType>     Motor type to use.
+   */
+  public static <MotorType extends MotorController> SwerveMotor fromMotor(MotorType motor,
+                                                                          SwerveEncoder<?> absoluteEncoder,
+                                                                          ModuleMotorType type,
+                                                                          double gearRatio,
+                                                                          double wheelDiameter,
+                                                                          double freeSpeedRPM, double powerLimit)
+  {
+    if (motor instanceof CANSparkMax)
+    {
+      return new REVSwerveMotor(((CANSparkMax) motor),
+                                absoluteEncoder,
+                                type,
+                                gearRatio,
+                                wheelDiameter,
+                                freeSpeedRPM,
+                                powerLimit);
+    }
+    if (motor instanceof TalonFX)
+    {
+      return new CTRESwerveMotor((TalonFX) motor,
+                                 absoluteEncoder,
+                                 type,
+                                 gearRatio,
+                                 wheelDiameter,
+                                 freeSpeedRPM,
+                                 powerLimit);
+    }
+    return null;
+  }
+
+
+  /**
+   * Configure the maximum power (-1 to 1) the PID can output. This helps manage voltage pull for the drive base.
+   *
+   * @param min Minimum output.
+   * @param max Maximum output.
+   */
+  public abstract void setPIDOutputRange(double min, double max);
+
+  /**
+   * Set the PIDF coefficients for the closed loop PID onboard the SparkMax.
+   *
+   * @param P            Proportional gain for closed loop. This is multiplied by closed loop error in sensor units.
+   *                     Default is 1.0
+   * @param I            Integral gain for closed loop. This is multiplied by closed loop error in sensor units every
+   *                     PID Loop.
+   * @param D            Derivative gain for closed loop. This is multiplied by derivative error (sensor units per PID
+   *                     loop). Default is 0.1
+   * @param F            Feed Fwd gain for Closed loop.
+   * @param integralZone Integral Zone can be used to auto clear the integral accumulator if the sensor pos is too far
+   *                     from the target. This prevents unstable oscillation if the kI is too large. Value is in sensor
+   *                     units.
+   */
+  public abstract void setPIDF(double P, double I, double D, double F, double integralZone);
+
+  /**
+   * Configures the conversion factor based upon which motor.
+   *
+   * @param conversionFactor Conversion from RPM to MPS for drive motor, and rotations to degrees for the turning
+   *                         motor.
+   */
+  public abstract void setConversionFactor(double conversionFactor);
+
+  /**
+   * Set the target for the PID loop.
+   *
+   * @param target      The PID target to aim for.
+   * @param feedforward The feedforward for this target.
+   */
+  public abstract void setTarget(double target, double feedforward);
+
+  /**
+   * Stop the motor by sending 0 volts to it.
+   */
+  public abstract void stop();
+
+  /**
+   * Set the speed of the drive motor from -1 to 1.
+   *
+   * @param speed Speed from -1 to 1.
+   */
+  public abstract void set(double speed);
+
+  /**
+   * Get the current value of the encoder corresponding to the PID.
+   *
+   * @return Current value of the encoder either in velocity or position..
+   */
+  public abstract double get();
+
+  /**
+   * Get the current output.
+   *
+   * @return Output amps.
+   */
+  public abstract double getAmps();
+
+  /**
+   * Get the current value of the encoder.
+   *
+   * @return Current value of the encoder.
+   */
+  public abstract double getPosition();
+
+  /**
+   * Set the voltage compensation for the swerve module motor.
+   *
+   * @param nominalVoltage Nominal voltage for operation to output to.
+   */
+  public abstract void setVoltageCompensation(double nominalVoltage);
+
+  /**
+   * Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with
+   * voltage compensation. This is useful to protect the motor from current spikes.
+   *
+   * @param currentLimit Current limit in AMPS at free speed.
+   */
+  public abstract void setCurrentLimit(int currentLimit);
+
+  /**
+   * Set the encoder value.
+   *
+   * @param value Value to set the encoder to.
+   */
+  public abstract void setEncoder(double value);
+
+  /**
+   * Check that the link is good on the swerve module and CAN bus is able to retrieve data.
+   *
+   * @return true on all devices are accessible over CAN.
+   */
+  public abstract boolean reachable();
+
+  /**
+   * Check if the absolute encoder is used inplace of the integrated encoder.
+   *
+   * @return true, if the absolute encoder is being used as the integrated controller.
+   */
+  public abstract boolean remoteIntegratedEncoder();
+
+  /**
+   * Optimize the CAN status frames to reduce utilization.
+   */
+  public abstract void optimizeCANFrames();
+
+  /**
+   * Save configuration data to the motor controller so it is persistent on reboot.
+   */
+  public abstract void saveConfig();
+
+  /**
+   * Invert the motor.
+   *
+   * @param inverted Set the motor as inverted.
+   */
+  public abstract void setInverted(boolean inverted);
+
+  /**
+   * Motor type for the swerve drive module
+   */
+  public enum ModuleMotorType
+  {
+    DRIVE, TURNING
+  }
+}

swervelib/SwerveParser.java

@@ -0,0 +1,303 @@
+package frc.robot.subsystems.swervedrive.swerve;
+
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+import com.ctre.phoenix.sensors.CANCoder;
+import com.ctre.phoenix.sensors.WPI_Pigeon2;
+import com.fasterxml.jackson.databind.JsonNode;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.AnalogEncoder;
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import frc.robot.subsystems.swervedrive.swerve.SwerveModule.SwerveModuleLocation;
+import java.io.File;
+import java.io.IOException;
+
+/**
+ * Swerve Drive JSON parser.
+ */
+public class SwerveParser
+{
+
+  /**
+   * Open JSON file.
+   *
+   * @param file JSON File to open.
+   * @return JsonNode of file.
+   */
+  private static JsonNode openJson(File file)
+  {
+    try
+    {
+      return new ObjectMapper().readTree(file);
+    } catch (IOException e)
+    {
+      throw new RuntimeException(e);
+    }
+  }
+
+  /**
+   * Check directory structure.
+   *
+   * @param directory JSON Configuration Directory
+   */
+  private static void checkDirectory(File directory)
+  {
+    assert new File(directory, "swerve.json").exists();
+    assert new File(directory, "modules").exists() && new File(directory, "modules").isDirectory();
+    assert new File(directory, "modules/back").exists() && new File(directory, "modules/back").isDirectory();
+    assert new File(directory, "modules/front").exists() && new File(directory, "modules/front").isDirectory();
+    assert new File(directory, "modules/back/left.json").exists();
+    assert new File(directory, "modules/back/right.json").exists();
+    assert new File(directory, "modules/front/left.json").exists();
+    assert new File(directory, "modules/front/right.json").exists();
+  }
+
+  /**
+   * Create the motor controller based off the config.
+   *
+   * @param motorConfig JSON Motor config object.
+   * @return Motor Controller
+   */
+  private static MotorController createMotor(JsonNode motorConfig)
+  {
+    int motorID = motorConfig.get("ID").asInt();
+    switch (motorConfig.get("Type").asText())
+    {
+      case "Neo":
+        return new CANSparkMax(motorID, MotorType.kBrushless);
+      case "Falcon":
+        return motorConfig.has("CANBus") ? new WPI_TalonFX(motorID, motorConfig.get("CANBus").asText())
+                                         : new WPI_TalonFX(motorID);
+    }
+    return null;
+  }
+
+  /**
+   * Create SwerveEncoder from JSON config.
+   *
+   * @param encoderConfig Swerve Encoder JSON config.
+   * @return SwerveEncoder
+   */
+  private static Object createEncoder(JsonNode encoderConfig)
+  {
+    int encoderID = encoderConfig.get("ID").asInt();
+    switch (encoderConfig.get("Type").asText())
+    {
+      case "CANCoder":
+        return encoderConfig.has("CANBus") ? new CANCoder(encoderID, encoderConfig.get("CANBus").asText())
+                                           : new CANCoder(encoderID);
+      case "ThriftyBot":
+      case "AnalogEncoder":
+        return new AnalogEncoder(encoderID);
+      case "SRXMagEncoder":
+      case "REVThroughBore":
+        return new DutyCycleEncoder(encoderID); // This probably doesn't work
+    }
+    return null;
+  }
+
+  /**
+   * Create the gyro object based off the configuration.
+   *
+   * @param gyroConfig Gyro configuration.
+   * @return Gyro object.
+   */
+  private static WPI_Pigeon2 createGyro(JsonNode gyroConfig)
+  {
+    int gyroID = gyroConfig.get("ID").asInt();
+
+    if (gyroConfig.get("Type").asText().equals("Pigeon2"))
+    {
+      return gyroConfig.has("CANBus") ? new WPI_Pigeon2(gyroID, gyroConfig.get("CANBus").asText())
+                                      : new WPI_Pigeon2(gyroID);
+//      case "Pigeon":
+//        return new WPI_PigeonIMU(gyroID);
+    }
+    return null;
+  }
+
+  /**
+   * Check module JSON config for existing values.
+   *
+   * @param moduleJson Module JSON object.
+   */
+  private static void checkModule(JsonNode moduleJson)
+  {
+    assert moduleJson.has("Motor");
+    assert moduleJson.get("Motor").has("Drive");
+    assert moduleJson.get("Motor").get("Drive").has("Type") && moduleJson.get("Motor").get("Drive").has("ID");
+    assert moduleJson.get("Motor").has("Steer");
+    assert moduleJson.get("Motor").get("Steer").has("Type") && moduleJson.get("Motor").get("Steer").has("ID") &&
+           moduleJson.get("Motor").get("Steer").has("FreeSpeedRPM");
+    assert moduleJson.has("AbsoluteEncoder");
+    assert moduleJson.get("AbsoluteEncoder").has("Type") &&
+           moduleJson.get("AbsoluteEncoder").has("ID") &&
+           moduleJson.get("AbsoluteEncoder").has("Offset");
+  }
+
+  /**
+   * Check the main json for correct attributes.
+   *
+   * @param mainJson Main JSON.
+   */
+  private static void checkMain(JsonNode mainJson)
+  {
+    assert mainJson.has("WheelBase");
+    assert mainJson.has("TrackWidth");
+    assert mainJson.has("WheelDiameter");
+    assert mainJson.has("Speed");
+    assert mainJson.get("Speed").has("MetersPerSecond") && mainJson.get("Speed").has("RadianPerSecond") &&
+           mainJson.get("Speed").has("PhysicalMetersPerSecond");
+    assert mainJson.has("Acceleration");
+    assert mainJson.get("Acceleration").has("MetersPerSecond") && mainJson.get("Acceleration").has("RadianPerSecond");
+    assert mainJson.has("Drive");
+    assert mainJson.get("Drive").has("Inverted") && mainJson.get("Drive").has("GearRatio") &&
+           mainJson.get("Drive").has("MaxPower");
+    assert mainJson.has("Steer");
+    assert mainJson.get("Steer").has("Inverted") && mainJson.get("Steer").has("GearRatio") &&
+           mainJson.get("Steer").has("MaxPower");
+    assert mainJson.has("Gyro");
+    assert mainJson.get("Gyro").has("Inverted") && mainJson.get("Gyro").has("Type") && mainJson.get("Gyro").has("ID");
+    assert mainJson.has("Initial Pose");
+    assert mainJson.get("Initial Pose").has("X") && mainJson.get("Initial Pose").has("Y") &&
+           mainJson.get("Initial Pose").has("Rotation");
+  }
+
+  /**
+   * Create SwerveModule from JSON configuration file.
+   *
+   * @param mainJson       Main JSON to pull from.
+   * @param moduleFile     Module specific JSON data to pull from.
+   * @param moduleLocation Swerve module location that is being created.
+   * @return SwerveModule
+   */
+  private static SwerveModule<?, ?, ?> createModule(JsonNode mainJson, File moduleFile,
+                                                    SwerveModuleLocation moduleLocation)
+  {
+    JsonNode moduleJson = openJson(moduleFile);
+    checkModule(moduleJson);
+    MotorController driveMotor = createMotor(moduleJson.get("Motor").get("Drive")),
+        steerMotor = createMotor(moduleJson.get("Motor").get("Steer"));
+    Object encoder = createEncoder(moduleJson.get("AbsoluteEncoder"));
+
+    SwerveModule<?, ?, ?> module = new SwerveModule<>(driveMotor,
+                                                      steerMotor,
+                                                      encoder,
+                                                      moduleLocation,
+                                                      mainJson.get("Drive").get("GearRatio").asDouble(),
+                                                      mainJson.get("Steer").get("GearRatio").asDouble(),
+                                                      moduleJson.get("AbsoluteEncoder").get("Offset").asDouble(),
+                                                      Units.inchesToMeters(mainJson.get("WheelDiameter").asDouble()),
+                                                      Units.inchesToMeters(mainJson.get("WheelBase").asDouble()),
+                                                      Units.inchesToMeters(mainJson.get("TrackWidth").asDouble()),
+                                                      moduleJson.get("Motor").get("Steer").get("FreeSpeedRPM")
+                                                                .asDouble(),
+                                                      mainJson.get("Speed").get("MetersPerSecond").asDouble(),
+                                                      mainJson.get("Acceleration").get("MetersPerSecond").asDouble(),
+                                                      mainJson.get("Drive").get("MaxPower").asDouble(),
+                                                      mainJson.get("Steer").get("MaxPower").asDouble(),
+                                                      mainJson.get("Steer").get("Inverted").asBoolean(),
+                                                      mainJson.get("Drive").get("Inverted").asBoolean());
+
+    if (moduleJson.get("Motor").get("Steer").has("PID"))
+    {
+      JsonNode jsonPIDF = moduleJson.get("Motor").get("Steer").get("PID");
+      setModulePIDF(module.turningMotor, jsonPIDF);
+    } else if (mainJson.get("Steer").has("PID"))
+    {
+      JsonNode jsonPIDF = mainJson.get("Steer").get("PID");
+      setModulePIDF(module.turningMotor, jsonPIDF);
+    }
+
+    if (moduleJson.get("Motor").get("Drive").has("PID"))
+    {
+      JsonNode jsonPIDF = moduleJson.get("Motor").get("Drive").get("PID");
+      setModulePIDF(module.driveMotor, jsonPIDF);
+    } else if (mainJson.get("Steer").has("PID"))
+    {
+      JsonNode jsonPIDF = mainJson.get("Drive").get("PID");
+      setModulePIDF(module.turningMotor, jsonPIDF);
+    }
+
+    if (moduleJson.get("Motor").get("Steer").has("CurrentLimit"))
+    {
+      module.turningMotor.setCurrentLimit(moduleJson.get("Motor").get("Steer").get("CurrentLimit")
+                                                    .asInt());
+    } else if (mainJson.get("Steer").has("CurrentLimit"))
+    {
+      module.turningMotor.setCurrentLimit(mainJson.get("Steer").get("CurrentLimit").asInt());
+    }
+
+    if (moduleJson.get("Motor").get("Drive").has("CurrentLimit"))
+    {
+      module.driveMotor.setCurrentLimit(moduleJson.get("Motor").get("Drive").get("CurrentLimit")
+                                                  .asInt());
+    } else if (mainJson.get("Drive").has("CurrentLimit"))
+    {
+      module.turningMotor.setCurrentLimit(mainJson.get("Drive").get("CurrentLimit").asInt());
+    }
+
+    return module;
+  }
+
+  /**
+   * Set the PIDF for the module if the given JSON has the paramters.
+   *
+   * @param motor    Motor to configure.
+   * @param jsonPIDF JSON ndoe with values.
+   */
+  private static void setModulePIDF(SwerveMotor motor, JsonNode jsonPIDF)
+  {
+    if (jsonPIDF.has("P") && jsonPIDF.has("I") && jsonPIDF.has("D") &&
+        jsonPIDF.has("F") && jsonPIDF.has("IntegralZone"))
+    {
+      motor.setPIDF(jsonPIDF.get("P").asDouble(),
+                    jsonPIDF.get("I").asDouble(),
+                    jsonPIDF.get("D").asDouble(),
+                    jsonPIDF.get("F").asDouble(),
+                    jsonPIDF.get("IntegralZone").asDouble());
+    }
+  }
+
+  /**
+   * Create SwerveDrive from JSON configuration directory.
+   *
+   * @param directory JSON Configuration directory.
+   * @return SwerveDrive
+   */
+  public static SwerveDrive fromJSONDirectory(File directory)
+  {
+    checkDirectory(directory);
+    JsonNode swerveJson = openJson(new File(directory, "swerve.json"));
+    checkMain(swerveJson);
+    return new SwerveDrive(createModule(swerveJson,
+                                        new File(directory, "modules/front/left.json"),
+                                        SwerveModuleLocation.FrontLeft),
+                           createModule(swerveJson,
+                                        new File(directory, "modules/front/right.json"),
+                                        SwerveModuleLocation.FrontRight),
+                           createModule(swerveJson,
+                                        new File(directory, "modules/back/left.json"),
+                                        SwerveModuleLocation.BackLeft),
+                           createModule(swerveJson,
+                                        new File(directory, "modules/back/right.json"),
+                                        SwerveModuleLocation.BackRight),
+                           createGyro(swerveJson.get("Gyro")),
+                           swerveJson.get("Speed").get("MetersPerSecond").asDouble(),
+                           swerveJson.get("Speed").get("RadianPerSecond").asDouble() * Math.PI,
+                           swerveJson.get("Acceleration").get("MetersPerSecond").asDouble(),
+                           swerveJson.get("Acceleration").get("RadianPerSecond").asDouble() * Math.PI,
+                           swerveJson.get("Speed").get("PhysicalMetersPerSecond").asDouble(),
+                           swerveJson.get("Gyro").get("Inverted").asBoolean(),
+                           new Pose2d(Units.inchesToMeters(swerveJson.get("Initial Pose").get("X").asDouble()),
+                                      Units.inchesToMeters(swerveJson.get("Initial Pose").get("Y").asDouble()),
+                                      Rotation2d.fromDegrees(swerveJson.get("Initial Pose").get("Rotation")
+                                                                       .asDouble())));
+
+  }
+}