Updated swervelib

swervelib/parser/PIDFConfig.java

@@ -0,0 +1,109 @@
+package frc.robot.subsystems.swervedrive2.swervelib.parser;
+
+import edu.wpi.first.math.controller.PIDController;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.deserializer.PIDFRange;
+
+/**
+ * Hold the PIDF and Integral Zone values for a PID.
+ */
+public class PIDFConfig
+{
+
+  /**
+   * Proportional Gain for PID.
+   */
+  public double p;
+  /**
+   * Integral Gain for PID.
+   */
+  public double i;
+  /**
+   * Derivative Gain for PID.
+   */
+  public double d;
+  /**
+   * Feedforward value for PID.
+   */
+  public double f;
+  /**
+   * Integral zone of the PID.
+   */
+  public double iz;
+
+  /**
+   * The PIDF output range.
+   */
+  public PIDFRange output = new PIDFRange();
+
+  /**
+   * Used when parsing PIDF values from JSON.
+   */
+  public PIDFConfig()
+  {
+  }
+
+  /**
+   * PIDF Config constructor to contain the values.
+   *
+   * @param p  P gain.
+   * @param i  I gain.
+   * @param d  D gain.
+   * @param f  F gain.
+   * @param iz Intergral zone.
+   */
+  public PIDFConfig(double p, double i, double d, double f, double iz)
+  {
+    this.p = p;
+    this.i = i;
+    this.d = d;
+    this.f = f;
+    this.iz = iz;
+  }
+
+  /**
+   * PIDF Config constructor to contain the values.
+   *
+   * @param p P gain.
+   * @param i I gain.
+   * @param d D gain.
+   * @param f F gain.
+   */
+  public PIDFConfig(double p, double i, double d, double f)
+  {
+    this(p, i, d, f, 0);
+  }
+
+  /**
+   * PIDF Config constructor to contain the values.
+   *
+   * @param p P gain.
+   * @param i I gain.
+   * @param d D gain.
+   */
+  public PIDFConfig(double p, double i, double d)
+  {
+    this(p, i, d, 0, 0);
+  }
+
+  /**
+   * PIDF Config constructor to contain the values.
+   *
+   * @param p P gain.
+   * @param d D gain.
+   */
+  public PIDFConfig(double p, double d)
+  {
+    this(p, 0, d, 0, 0);
+  }
+
+  /**
+   * Create a PIDController from the PID values.
+   *
+   * @return PIDController.
+   */
+  public PIDController createPIDController()
+  {
+    return new PIDController(p, i, d);
+  }
+}
+

swervelib/parser/SwerveControllerConfiguration.java

@@ -0,0 +1,59 @@
+package frc.robot.subsystems.swervedrive2.swervelib.parser;
+
+import static frc.robot.subsystems.swervedrive2.swervelib.math.SwerveMath.calculateMaxAngularVelocity;
+
+/**
+ * Swerve Controller configuration class which is used to configure
+ * {@link frc.robot.subsystems.swervedrive2.swervelib.SwerveController}.
+ */
+public class SwerveControllerConfiguration
+{
+
+  /**
+   * Maximum robot speed in meters per second.
+   */
+  public final double     maxSpeed;
+  /**
+   * Maximum angular velocity in rad/s
+   */
+  public final double     maxAngularVelocity;
+  /**
+   * PIDF for the heading of the robot.
+   */
+  public final PIDFConfig headingPIDF;
+  /**
+   * hypotenuse deadband for the robot angle control joystick.
+   */
+  public final double     angleJoyStickRadiusDeadband; // Deadband for the minimum hypot for the heading joystick.
+
+  /**
+   * Construct the swerve controller configuration.
+   *
+   * @param driveCfg                    Drive configuration.
+   * @param headingPIDF                 Heading PIDF configuration.
+   * @param angleJoyStickRadiusDeadband Deadband on radius of angle joystick.
+   */
+  public SwerveControllerConfiguration(SwerveDriveConfiguration driveCfg, PIDFConfig headingPIDF,
+                                       double angleJoyStickRadiusDeadband)
+  {
+    this.maxSpeed = driveCfg.maxSpeed;
+    this.maxAngularVelocity = calculateMaxAngularVelocity(driveCfg.maxSpeed,
+                                                          Math.abs(driveCfg.moduleLocationsMeters[0].getX()),
+                                                          Math.abs(driveCfg.moduleLocationsMeters[0].getY()));
+    this.headingPIDF = headingPIDF;
+    this.angleJoyStickRadiusDeadband = angleJoyStickRadiusDeadband;
+  }
+
+  /**
+   * Construct the swerve controller configuration. Assumes hypotenuse deadband of 0.5 (minimum radius for angle to be
+   * set on angle joystick is .5 of the controller).
+   *
+   * @param driveCfg    Drive configuration.
+   * @param headingPIDF Heading PIDF configuration.
+   */
+  public SwerveControllerConfiguration(SwerveDriveConfiguration driveCfg, PIDFConfig headingPIDF)
+  {
+    this(driveCfg, headingPIDF, 0.5);
+  }
+
+}

swervelib/parser/SwerveDriveConfiguration.java

@@ -0,0 +1,76 @@
+package frc.robot.subsystems.swervedrive2.swervelib.parser;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import frc.robot.subsystems.swervedrive2.swervelib.SwerveModule;
+import frc.robot.subsystems.swervedrive2.swervelib.imu.SwerveIMU;
+
+/**
+ * Swerve drive configurations used during SwerveDrive construction.
+ */
+public class SwerveDriveConfiguration
+{
+
+  /**
+   * Swerve Module locations.
+   */
+  public Translation2d[] moduleLocationsMeters;
+  /**
+   * Swerve IMU
+   */
+  public SwerveIMU       imu;
+  /**
+   * Invert the imu measurements.
+   */
+  public boolean         invertedIMU = false;
+  /**
+   * Max speed in meters per second.
+   */
+  public double          maxSpeed;
+  /**
+   * Number of modules on the robot.
+   */
+  public int             moduleCount;
+  /**
+   * Swerve Modules.
+   */
+  public SwerveModule[]  modules;
+
+  /**
+   * Create swerve drive configuration.
+   *
+   * @param moduleConfigs Module configuration.
+   * @param swerveIMU     Swerve IMU.
+   * @param maxSpeed      Max speed of the robot in meters per second.
+   * @param invertedIMU   Invert the IMU.
+   */
+  public SwerveDriveConfiguration(SwerveModuleConfiguration[] moduleConfigs, SwerveIMU swerveIMU, double maxSpeed,
+                                  boolean invertedIMU)
+  {
+    this.moduleCount = moduleConfigs.length;
+    this.imu = swerveIMU;
+    this.maxSpeed = maxSpeed;
+    this.invertedIMU = invertedIMU;
+    this.modules = createModules(moduleConfigs);
+    this.moduleLocationsMeters = new Translation2d[moduleConfigs.length];
+    for (SwerveModule module : modules)
+    {
+      this.moduleLocationsMeters[module.moduleNumber] = module.configuration.moduleLocation;
+    }
+  }
+
+  /**
+   * Create modules based off of the SwerveModuleConfiguration.
+   *
+   * @param swerves Swerve constants.
+   * @return Swerve Modules.
+   */
+  public SwerveModule[] createModules(SwerveModuleConfiguration[] swerves)
+  {
+    SwerveModule[] modArr = new SwerveModule[swerves.length];
+    for (int i = 0; i < swerves.length; i++)
+    {
+      modArr[i] = new SwerveModule(i, swerves[i]);
+    }
+    return modArr;
+  }
+}

swervelib/parser/SwerveModuleConfiguration.java

@@ -0,0 +1,179 @@
+package frc.robot.subsystems.swervedrive2.swervelib.parser;
+
+import static frc.robot.subsystems.swervedrive2.swervelib.math.SwerveMath.calculateAngleKV;
+import static frc.robot.subsystems.swervedrive2.swervelib.math.SwerveMath.calculateDegreesPerSteeringRotation;
+import static frc.robot.subsystems.swervedrive2.swervelib.math.SwerveMath.calculateMaxAcceleration;
+import static frc.robot.subsystems.swervedrive2.swervelib.math.SwerveMath.calculateMetersPerRotation;
+
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Translation2d;
+import frc.robot.subsystems.swervedrive2.swervelib.encoders.SwerveAbsoluteEncoder;
+import frc.robot.subsystems.swervedrive2.swervelib.motors.SwerveMotor;
+
+/**
+ * Swerve Module configuration class which is used to configure
+ * {@link frc.robot.subsystems.swervedrive2.swervelib.SwerveModule}.
+ */
+public class SwerveModuleConfiguration
+{
+
+  /**
+   * Angle offset in degrees for the Swerve Module.
+   */
+  public final double                              angleOffset;
+  /**
+   * Whether the absolute encoder is inverted.
+   */
+  public final boolean                             absoluteEncoderInverted;
+  /**
+   * State of inversion of the drive motor.
+   */
+  public final boolean                             driveMotorInverted;
+  /**
+   * State of inversion of the angle motor.
+   */
+  public final boolean                             angleMotorInverted;
+  /**
+   * Maximum robot speed in meters per second.
+   */
+  public final double                              maxSpeed;
+  /**
+   * PIDF configuration options for the angle motor closed-loop PID controller.
+   */
+  public       PIDFConfig                          anglePIDF;
+  /**
+   * PIDF configuration options for the drive motor closed-loop PID controller.
+   */
+  public       PIDFConfig                          velocityPIDF;
+  /**
+   * Angle volt-meter-per-second.
+   */
+  public       double                              angleKV;
+  /**
+   * Swerve module location relative to the robot.
+   */
+  public       Translation2d                       moduleLocation;
+  /**
+   * Physical characteristics of the swerve module.
+   */
+  public       SwerveModulePhysicalCharacteristics physicalCharacteristics;
+  /**
+   * The drive motor and angle motor of this swerve module.
+   */
+  public       SwerveMotor                         driveMotor, angleMotor;
+  /**
+   * The Absolute Encoder for the swerve module.
+   */
+  public SwerveAbsoluteEncoder absoluteEncoder;
+
+
+  /**
+   * Construct a configuration object for swerve modules.
+   *
+   * @param driveMotor              Drive {@link SwerveMotor}.
+   * @param angleMotor              Angle {@link SwerveMotor}
+   * @param absoluteEncoder         Absolute encoder {@link SwerveAbsoluteEncoder}.
+   * @param angleOffset             Absolute angle offset to 0.
+   * @param absoluteEncoderInverted Absolute encoder inverted.
+   * @param angleMotorInverted      State of inversion of the angle motor.
+   * @param driveMotorInverted      Drive motor inverted.
+   * @param xMeters                 Module location in meters from the center horizontally.
+   * @param yMeters                 Module location in meters from center vertically.
+   * @param anglePIDF               Angle PIDF configuration.
+   * @param velocityPIDF            Velocity PIDF configuration.
+   * @param maxSpeed                Maximum speed in meters per second.
+   * @param physicalCharacteristics Physical characteristics of the swerve module.
+   */
+  public SwerveModuleConfiguration(SwerveMotor driveMotor, SwerveMotor angleMotor,
+                                   SwerveAbsoluteEncoder absoluteEncoder, double angleOffset,
+                                   double xMeters,
+                                   double yMeters, PIDFConfig anglePIDF, PIDFConfig velocityPIDF, double maxSpeed,
+                                   SwerveModulePhysicalCharacteristics physicalCharacteristics,
+                                   boolean absoluteEncoderInverted, boolean driveMotorInverted,
+                                   boolean angleMotorInverted)
+  {
+    this.driveMotor = driveMotor;
+    this.angleMotor = angleMotor;
+    this.absoluteEncoder = absoluteEncoder;
+    this.angleOffset = angleOffset;
+    this.absoluteEncoderInverted = absoluteEncoderInverted;
+    this.driveMotorInverted = driveMotorInverted;
+    this.angleMotorInverted = angleMotorInverted;
+    this.moduleLocation = new Translation2d(xMeters, yMeters);
+    this.anglePIDF = anglePIDF;
+    this.velocityPIDF = velocityPIDF;
+    this.maxSpeed = maxSpeed;
+    this.angleKV = calculateAngleKV(physicalCharacteristics.optimalVoltage,
+                                    physicalCharacteristics.angleMotorFreeSpeedRPM,
+                                    physicalCharacteristics.angleGearRatio);
+    this.physicalCharacteristics = physicalCharacteristics;
+
+  }
+
+  /**
+   * Construct a configuration object for swerve modules. Assumes the absolute encoder and drive motor are not
+   * inverted.
+   *
+   * @param driveMotor              Drive {@link SwerveMotor}.
+   * @param angleMotor              Angle {@link SwerveMotor}
+   * @param absoluteEncoder         Absolute encoder {@link SwerveAbsoluteEncoder}.
+   * @param angleOffset             Absolute angle offset to 0.
+   * @param xMeters                 Module location in meters from the center horizontally.
+   * @param yMeters                 Module location in meters from center vertically.
+   * @param anglePIDF               Angle PIDF configuration.
+   * @param velocityPIDF            Velocity PIDF configuration.
+   * @param maxSpeed                Maximum robot speed in meters per second.
+   * @param physicalCharacteristics Physical characteristics of the swerve module.
+   */
+  public SwerveModuleConfiguration(SwerveMotor driveMotor, SwerveMotor angleMotor,
+                                   SwerveAbsoluteEncoder absoluteEncoder, double angleOffset,
+                                   double xMeters, double yMeters, PIDFConfig anglePIDF, PIDFConfig velocityPIDF,
+                                   double maxSpeed, SwerveModulePhysicalCharacteristics physicalCharacteristics)
+  {
+    this(driveMotor,
+         angleMotor,
+         absoluteEncoder,
+         angleOffset,
+         xMeters,
+         yMeters,
+         anglePIDF,
+         velocityPIDF,
+         maxSpeed,
+         physicalCharacteristics,
+         false,
+         false,
+         false);
+  }
+
+
+  /**
+   * Create the drive feedforward for swerve modules.
+   *
+   * @return Drive feedforward for drive motor on a swerve module.
+   */
+  public SimpleMotorFeedforward createDriveFeedforward()
+  {
+    double kv = physicalCharacteristics.optimalVoltage / maxSpeed;
+    ///^ Volt-seconds per meter (max voltage divided by max speed)
+    double ka = physicalCharacteristics.optimalVoltage /
+                calculateMaxAcceleration(physicalCharacteristics.wheelGripCoefficientOfFriction);
+    ///^ Volt-seconds^2 per meter (max voltage divided by max accel)
+    return new SimpleMotorFeedforward(0, kv, ka);
+  }
+
+  /**
+   * Get the encoder conversion for position encoders.
+   *
+   * @param isDriveMotor For the drive motor.
+   * @return Position encoder conversion factor.
+   */
+  public double getPositionEncoderConversion(boolean isDriveMotor)
+  {
+    return isDriveMotor ? calculateMetersPerRotation(physicalCharacteristics.wheelDiameter,
+                                                     physicalCharacteristics.driveGearRatio,
+                                                     physicalCharacteristics.driveEncoderPulsePerRotation)
+                        : calculateDegreesPerSteeringRotation(
+                            angleMotor.isAttachedAbsoluteEncoder() ? 1 : physicalCharacteristics.angleGearRatio,
+                            physicalCharacteristics.angleEncoderPulsePerRotation);
+  }
+}

swervelib/parser/SwerveModulePhysicalCharacteristics.java

@@ -0,0 +1,117 @@
+package frc.robot.subsystems.swervedrive2.swervelib.parser;
+
+/**
+ * Configuration class which stores physical characteristics shared between every swerve module.
+ */
+public class SwerveModulePhysicalCharacteristics
+{
+
+  /**
+   * Wheel diameter in meters.
+   */
+  public final double wheelDiameter;
+  /**
+   * Drive gear ratio. How many times the motor has to spin before the wheel makes a complete rotation.
+   */
+  public final double driveGearRatio;
+  /**
+   * Angle gear ratio. How many times the motor has to spin before the wheel makes a full rotation.
+   */
+  public final double angleGearRatio;
+  /**
+   * Drive motor encoder pulse per rotation. 1 if integrated encoder.
+   */
+  public final int    driveEncoderPulsePerRotation;
+  /**
+   * Angle motor encoder pulse per rotation. 1 if integrated encoder.
+   */
+  public final int    angleEncoderPulsePerRotation;
+  /**
+   * Optimal voltage of the robot.
+   */
+  public final double optimalVoltage;
+  /**
+   * Current limits for the Swerve Module.
+   */
+  public final int    driveMotorCurrentLimit, angleMotorCurrentLimit;
+  /**
+   * The time it takes for the motor to go from 0 to full throttle in seconds.
+   */
+  public final double driveMotorRampRate, angleMotorRampRate;
+  /**
+   * Wheel grip tape coefficient of friction on carpet, as described by the vendor.
+   */
+  public final double wheelGripCoefficientOfFriction;
+  /**
+   * Free speed rotations per minute of the motor, as described by the vendor.
+   */
+  public final double angleMotorFreeSpeedRPM;
+
+  /**
+   * Construct the swerve module physical characteristics.
+   *
+   * @param driveGearRatio                 Gear ratio of the drive motor. Number of motor rotations to rotate the
+   *                                       wheel.
+   * @param angleGearRatio                 Gear ratio of the angle motor. Number of motor rotations to spin the wheel.
+   * @param angleMotorFreeSpeedRPM         Motor free speed rotation per minute.
+   * @param wheelDiameter                  Wheel diameter in meters.
+   * @param wheelGripCoefficientOfFriction Wheel grip coefficient of friction on carpet given by manufacturer.
+   * @param optimalVoltage                 Optimal robot voltage.
+   * @param driveMotorCurrentLimit         Current limit for the drive motor.
+   * @param angleMotorCurrentLimit         Current limit for the angle motor.
+   * @param driveMotorRampRate             The time in seconds to go from 0 to full throttle on the motor. (Prevents
+   *                                       over drawing power from battery)
+   * @param angleMotorRampRate             The time in seconds to go from 0 to full throttle on the motor. (Prevents
+   *                                       overdrawing power and power loss).
+   * @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,
+                                             double angleMotorFreeSpeedRPM,
+                                             double wheelDiameter, double wheelGripCoefficientOfFriction,
+                                             double optimalVoltage, int driveMotorCurrentLimit,
+                                             int angleMotorCurrentLimit, double driveMotorRampRate,
+                                             double angleMotorRampRate, int driveEncoderPulsePerRotation,
+                                             int angleEncoderPulsePerRotation)
+  {
+    this.wheelGripCoefficientOfFriction = wheelGripCoefficientOfFriction;
+    this.optimalVoltage = optimalVoltage;
+
+    this.angleMotorFreeSpeedRPM = angleMotorFreeSpeedRPM;
+    this.angleGearRatio = angleGearRatio;
+    this.driveGearRatio = driveGearRatio;
+    this.angleEncoderPulsePerRotation = angleEncoderPulsePerRotation;
+    this.driveEncoderPulsePerRotation = driveEncoderPulsePerRotation;
+    this.wheelDiameter = wheelDiameter;
+
+    this.driveMotorCurrentLimit = driveMotorCurrentLimit;
+    this.angleMotorCurrentLimit = angleMotorCurrentLimit;
+    this.driveMotorRampRate = driveMotorRampRate;
+    this.angleMotorRampRate = angleMotorRampRate;
+  }
+
+  /**
+   * Construct the swerve module physical characteristics. Assume coefficient of friction is 1.19 (taken from blue
+   * nitrile on carpet from Studica) and optimal voltage is 12v. Assumes the drive motor current limit is 40A, and the
+   * angle motor current limit is 20A.
+   *
+   * @param driveGearRatio               Gear ratio of the drive motor. Number of motor rotations to rotate the wheel.
+   * @param angleGearRatio               Gear ratio of the angle motor. Number of motor rotations to spin the wheel.
+   * @param angleMotorFreeSpeedRPM       Motor free speed rotation per minute.
+   * @param wheelDiameter                Wheel diameter in meters.
+   * @param driveMotorRampRate           The time in seconds to go from 0 to full throttle on the motor. (Prevents over
+   *                                     drawing power from battery)
+   * @param angleMotorRampRate           The time in seconds to go from 0 to full throttle on the motor. (Prevents
+   *                                     overdrawing power and power loss).
+   * @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,
+                                             double angleMotorFreeSpeedRPM,
+                                             double wheelDiameter, double driveMotorRampRate, double angleMotorRampRate,
+                                             int driveEncoderPulsePerRotation, int angleEncoderPulsePerRotation)
+  {
+    this(driveGearRatio, angleGearRatio, angleMotorFreeSpeedRPM, wheelDiameter, 1.19, 12,
+         40, 20, driveMotorRampRate, angleMotorRampRate, driveEncoderPulsePerRotation, angleEncoderPulsePerRotation);
+  }
+}

swervelib/parser/SwerveParser.java

@@ -0,0 +1,140 @@
+package frc.robot.subsystems.swervedrive2.swervelib.parser;
+
+import com.fasterxml.jackson.databind.JsonNode;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import edu.wpi.first.math.util.Units;
+import frc.robot.subsystems.swervedrive2.swervelib.SwerveDrive;
+import frc.robot.subsystems.swervedrive2.swervelib.SwerveModule;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.json.ControllerPropertiesJson;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.json.ModuleJson;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.json.PIDFPropertiesJson;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.json.PhysicalPropertiesJson;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.json.SwerveDriveJson;
+import java.io.File;
+import java.io.IOException;
+import java.util.HashMap;
+
+/**
+ * Helper class used to parse the JSON directory with specified configuration options.
+ */
+public class SwerveParser
+{
+
+  private static final HashMap<String, Integer> moduleConfigs = new HashMap<>();
+  /**
+   * Parsed swervedrive.json
+   */
+  public static        SwerveDriveJson          swerveDriveJson;
+  /**
+   * Parsed controllerproperties.json
+   */
+  public static        ControllerPropertiesJson controllerPropertiesJson;
+  /**
+   * Parsed modules/pidfproperties.json
+   */
+  public static        PIDFPropertiesJson       pidfPropertiesJson;
+  /**
+   * Parsed modules/physicalproperties.json
+   */
+  public static        PhysicalPropertiesJson   physicalPropertiesJson;
+  /**
+   * Array holding the module jsons given in {@link SwerveDriveJson}.
+   */
+  public static        ModuleJson[]             moduleJsons;
+
+
+  /**
+   * Construct a swerve parser. Will throw an error if there is a missing file.
+   *
+   * @param directory Directory with swerve configurations.
+   */
+  public SwerveParser(File directory) throws IOException
+  {
+    checkDirectory(directory);
+    swerveDriveJson = new ObjectMapper().readValue(new File(directory, "swervedrive.json"), SwerveDriveJson.class);
+    controllerPropertiesJson = new ObjectMapper().readValue(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++)
+    {
+      moduleConfigs.put(swerveDriveJson.modules[i], i);
+      File moduleFile = new File(directory, "modules/" + swerveDriveJson.modules[i]);
+      assert moduleFile.exists();
+      moduleJsons[i] = new ObjectMapper().readValue(moduleFile, ModuleJson.class);
+    }
+  }
+
+  /**
+   * Get the swerve module by the json name.
+   *
+   * @param name               JSON name.
+   * @param driveConfiguration {@link SwerveDriveConfiguration} to pull from.
+   * @return {@link SwerveModuleConfiguration} based on the file.
+   */
+  public static SwerveModule getModuleConfigurationByName(String name,
+                                                          SwerveDriveConfiguration driveConfiguration)
+  {
+    return driveConfiguration.modules[moduleConfigs.get(name + ".json")];
+  }
+
+  /**
+   * Open JSON file.
+   *
+   * @param file JSON File to open.
+   * @return JsonNode of file.
+   */
+  private 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 void checkDirectory(File directory)
+  {
+    assert new File(directory, "swervedrive.json").exists();
+    assert new File(directory, "controllerproperties.json").exists();
+    assert new File(directory, "modules").exists() && new File(directory, "modules").isDirectory();
+    assert new File(directory, "modules/pidfproperties.json").exists();
+    assert new File(directory, "modules/physicalproperties.json").exists();
+  }
+
+  /**
+   * Create {@link SwerveDrive} from JSON configuration directory.
+   *
+   * @return {@link SwerveDrive} instance.
+   */
+  public SwerveDrive createSwerveDrive()
+  {
+    double                      maxSpeedMPS          = Units.feetToMeters(swerveDriveJson.maxSpeed);
+    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,
+                                                                 maxSpeedMPS,
+                                                                 physicalPropertiesJson.createPhysicalProperties(
+                                                                     swerveDriveJson.optimalVoltage));
+    }
+    SwerveDriveConfiguration swerveDriveConfiguration = new SwerveDriveConfiguration(moduleConfigurations,
+                                                                                     swerveDriveJson.imu.createIMU(),
+                                                                                     maxSpeedMPS,
+                                                                                     swerveDriveJson.invertedIMU);
+
+    return new SwerveDrive(swerveDriveConfiguration,
+                           controllerPropertiesJson.createControllerConfiguration(swerveDriveConfiguration));
+  }
+}

swervelib/parser/deserializer/PIDFRange.java

@@ -0,0 +1,17 @@
+package frc.robot.subsystems.swervedrive2.swervelib.parser.deserializer;
+
+/**
+ * Class to hold the minimum and maximum input or output of the PIDF.
+ */
+public class PIDFRange
+{
+
+  /**
+   * Minimum value.
+   */
+  public double min = -1;
+  /**
+   * Maximum value.
+   */
+  public double max = 1;
+}

swervelib/parser/json/ControllerPropertiesJson.java

@@ -0,0 +1,32 @@
+package frc.robot.subsystems.swervedrive2.swervelib.parser.json;
+
+import frc.robot.subsystems.swervedrive2.swervelib.parser.PIDFConfig;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.SwerveControllerConfiguration;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.SwerveDriveConfiguration;
+
+/**
+ * {@link frc.robot.subsystems.swervedrive2.swervelib.SwerveController} parsed class. Used to access the JSON data.
+ */
+public class ControllerPropertiesJson
+{
+
+  /**
+   * The minimum radius of the angle control joystick to allow for heading adjustment of the robot.
+   */
+  public double     angleJoystickRadiusDeadband;
+  /**
+   * The PID used to control the robot heading.
+   */
+  public PIDFConfig heading;
+
+  /**
+   * Create the {@link SwerveControllerConfiguration} based on parsed and given data.
+   *
+   * @param driveConfiguration {@link SwerveDriveConfiguration} parsed configuration.
+   * @return {@link SwerveControllerConfiguration} object based on parsed data.
+   */
+  public SwerveControllerConfiguration createControllerConfiguration(SwerveDriveConfiguration driveConfiguration)
+  {
+    return new SwerveControllerConfiguration(driveConfiguration, heading, angleJoystickRadiusDeadband);
+  }
+}

swervelib/parser/json/DeviceJson.java

@@ -0,0 +1,100 @@
+package frc.robot.subsystems.swervedrive2.swervelib.parser.json;
+
+import frc.robot.subsystems.swervedrive2.swervelib.encoders.CANCoderSwerve;
+import frc.robot.subsystems.swervedrive2.swervelib.encoders.SparkMaxEncoderSwerve;
+import frc.robot.subsystems.swervedrive2.swervelib.encoders.SwerveAbsoluteEncoder;
+import frc.robot.subsystems.swervedrive2.swervelib.imu.NavXSwerve;
+import frc.robot.subsystems.swervedrive2.swervelib.imu.Pigeon2Swerve;
+import frc.robot.subsystems.swervedrive2.swervelib.imu.PigeonSwerve;
+import frc.robot.subsystems.swervedrive2.swervelib.imu.SwerveIMU;
+import frc.robot.subsystems.swervedrive2.swervelib.motors.SparkMaxSwerve;
+import frc.robot.subsystems.swervedrive2.swervelib.motors.SwerveMotor;
+
+/**
+ * Device JSON parsed class. Used to access the JSON data.
+ */
+public class DeviceJson
+{
+
+  /**
+   * The device type, e.g. pigeon/pigeon2/sparkmax/talonfx/navx
+   */
+  public String type;
+  /**
+   * The CAN ID or pin ID of the device.
+   */
+  public int    id;
+  /**
+   * The CAN bus name which the device resides on if using CAN.
+   */
+  public String canbus = "";
+
+  /**
+   * Create a {@link SwerveAbsoluteEncoder} from the current configuration.
+   *
+   * @return {@link SwerveAbsoluteEncoder} given.
+   */
+  public SwerveAbsoluteEncoder createEncoder()
+  {
+    switch (type)
+    {
+      case "integrated":
+      case "attached":
+        return null;
+      case "cancoder":
+        return new CANCoderSwerve(id, canbus != null ? canbus : "");
+      default:
+        throw new RuntimeException(type + " is not a recognized absolute encoder type.");
+    }
+  }
+
+  /**
+   * Create a {@link SwerveIMU} from the given configuration.
+   *
+   * @return {@link SwerveIMU} given.
+   */
+  public SwerveIMU createIMU()
+  {
+    switch (type)
+    {
+      case "navx":
+        return new NavXSwerve();
+      case "pigeon":
+        return new PigeonSwerve(id);
+      case "pigeon2":
+        return new Pigeon2Swerve(id, canbus != null ? canbus : "");
+      default:
+        throw new RuntimeException(type + " is not a recognized absolute encoder type.");
+    }
+  }
+
+  /**
+   * Create a {@link SwerveMotor} from the given configuration.
+   *
+   * @param isDriveMotor If the motor being generated is a drive motor.
+   * @return {@link SwerveMotor} given.
+   */
+  public SwerveMotor createMotor(boolean isDriveMotor)
+  {
+    if (type.equals("sparkmax"))
+    {
+      return new SparkMaxSwerve(id, isDriveMotor);
+    }
+    throw new RuntimeException(type + " is not a recognized absolute encoder type.");
+  }
+
+  /**
+   * Create a {@link SwerveAbsoluteEncoder} from the data port on the motor controller.
+   *
+   * @param motor The motor to create the absolute encoder from.
+   * @return {@link SwerveAbsoluteEncoder} from the motor controller.
+   */
+  public SwerveAbsoluteEncoder createIntegratedEncoder(SwerveMotor motor)
+  {
+    if (type.equals("sparkmax"))
+    {
+      return new SparkMaxEncoderSwerve(motor);
+    }
+    throw new RuntimeException("Could not create absolute encoder from data port of " + type + " id " + id);
+  }
+}

swervelib/parser/json/ModuleJson.java

@@ -0,0 +1,76 @@
+package frc.robot.subsystems.swervedrive2.swervelib.parser.json;
+
+import edu.wpi.first.math.util.Units;
+import frc.robot.subsystems.swervedrive2.swervelib.encoders.SwerveAbsoluteEncoder;
+import frc.robot.subsystems.swervedrive2.swervelib.motors.SwerveMotor;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.PIDFConfig;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.SwerveModuleConfiguration;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.SwerveModulePhysicalCharacteristics;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.json.modules.BoolMotorJson;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.json.modules.LocationJson;
+
+/**
+ * {@link frc.robot.subsystems.swervedrive2.swervelib.SwerveModule} JSON parsed class. Used to access the JSON data.
+ */
+public class ModuleJson
+{
+
+  /**
+   * Drive motor device configuration.
+   */
+  public DeviceJson    drive;
+  /**
+   * Angle motor device configuration.
+   */
+  public DeviceJson    angle;
+  /**
+   * Absolute encoder device configuration.
+   */
+  public DeviceJson    encoder;
+  /**
+   * Defines which motors are inverted.
+   */
+  public BoolMotorJson inverted;
+  /**
+   * Absolute encoder offset from 0 in degrees.
+   */
+  public double        absoluteEncoderOffset;
+  /**
+   * Absolute encoder inversion state.
+   */
+  public boolean       absoluteEncoderInverted = false;
+  /**
+   * The location of the swerve module from the center of the robot in inches.
+   */
+  public LocationJson  location;
+
+  /**
+   * Create the swerve module configuration based off of parsed data.
+   *
+   * @param anglePIDF               The PIDF values for the angle motor.
+   * @param velocityPIDF            The velocity PIDF values for the drive motor.
+   * @param maxSpeed                The maximum speed of the robot in meters per second.
+   * @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,
+                                                             double maxSpeed,
+                                                             SwerveModulePhysicalCharacteristics physicalCharacteristics)
+  {
+    SwerveMotor           angleMotor = angle.createMotor(false);
+    SwerveAbsoluteEncoder absEncoder = encoder.createEncoder();
+
+    // If the absolute encoder is attached.
+    if (absEncoder == null)
+    {
+      absEncoder = angle.createIntegratedEncoder(angleMotor);
+      angleMotor.setAbsoluteEncoder(absEncoder);
+    }
+
+    return new SwerveModuleConfiguration(drive.createMotor(true), angleMotor, absEncoder,
+                                         absoluteEncoderOffset, Units.inchesToMeters(location.x),
+                                         Units.inchesToMeters(location.y), anglePIDF, velocityPIDF, maxSpeed,
+                                         physicalCharacteristics, absoluteEncoderInverted, inverted.drive,
+                                         inverted.angle);
+  }
+}

swervelib/parser/json/PIDFPropertiesJson.java

@@ -0,0 +1,20 @@
+package frc.robot.subsystems.swervedrive2.swervelib.parser.json;
+
+import frc.robot.subsystems.swervedrive2.swervelib.parser.PIDFConfig;
+
+/**
+ * {@link frc.robot.subsystems.swervedrive2.swervelib.SwerveModule} PID with Feedforward for the drive motor and angle
+ * motor.
+ */
+public class PIDFPropertiesJson
+{
+
+  /**
+   * The PIDF with Integral Zone used for the drive motor.
+   */
+  public PIDFConfig drive;
+  /**
+   * The PIDF with Integral Zone used for the angle motor.
+   */
+  public PIDFConfig angle;
+}

swervelib/parser/json/PhysicalPropertiesJson.java

@@ -0,0 +1,128 @@
+package frc.robot.subsystems.swervedrive2.swervelib.parser.json;
+
+import edu.wpi.first.math.util.Units;
+import frc.robot.subsystems.swervedrive2.swervelib.parser.SwerveModulePhysicalCharacteristics;
+
+/**
+ * {@link frc.robot.subsystems.swervedrive2.swervelib.parser.SwerveModulePhysicalCharacteristics} parsed data. Used to
+ * configure the SwerveModule.
+ */
+public class PhysicalPropertiesJson
+{
+
+  /**
+   * Wheel diameter in inches.
+   */
+  public double            wheelDiameter;
+  /**
+   * Gear ratio for the motors, number of times the motor has to spin before the wheel rotates a single time.
+   */
+  public MotorConfigDouble gearRatio;
+  /**
+   * Encoder pulse per rotation for non-integrated encoders. 1 for integrated encoders.
+   */
+  public MotorConfigInt    encoderPulsePerRotation        = new MotorConfigInt(1, 1);
+  /**
+   * The current limit in AMPs to apply to the motors.
+   */
+  public MotorConfigInt    currentLimit                   = new MotorConfigInt(40, 20);
+  /**
+   * The minimum number of seconds to take for the motor to go from 0 to full throttle.
+   */
+  public MotorConfigDouble rampRate                       = new MotorConfigDouble(0.25, 0.25);
+  /**
+   * The grip tape coefficient of friction on carpet. Used to calculate the practical maximum acceleration.
+   */
+  public double            wheelGripCoefficientOfFriction = 1.19;
+  /**
+   * Angle motor free speed rotations per minute.
+   */
+  public double            angleMotorFreeSpeedRPM;
+
+  /**
+   * Create the physical characteristics based off the parsed data.
+   *
+   * @param optimalVoltage Optimal voltage to compensate for and use to calculate drive motor feedforward.
+   * @return {@link SwerveModulePhysicalCharacteristics} based on parsed data.
+   */
+  public SwerveModulePhysicalCharacteristics createPhysicalProperties(double optimalVoltage)
+  {
+    return new SwerveModulePhysicalCharacteristics(gearRatio.drive, gearRatio.angle, angleMotorFreeSpeedRPM,
+                                                   Units.inchesToMeters(wheelDiameter), wheelGripCoefficientOfFriction,
+                                                   optimalVoltage,
+                                                   currentLimit.drive, currentLimit.angle, rampRate.drive,
+                                                   rampRate.angle, encoderPulsePerRotation.drive,
+                                                   encoderPulsePerRotation.angle);
+  }
+}
+
+/**
+ * Used to store doubles for motor configuration.
+ */
+class MotorConfigDouble
+{
+
+  /**
+   * Drive motor.
+   */
+  public double drive;
+  /**
+   * Angle motor.
+   */
+  public double angle;
+
+  /**
+   * Default constructor.
+   */
+  public MotorConfigDouble()
+  {
+  }
+
+  /**
+   * Default constructor.
+   *
+   * @param angle Angle data.
+   * @param drive Drive data.
+   */
+  public MotorConfigDouble(double angle, double drive)
+  {
+    this.angle = angle;
+    this.drive = drive;
+  }
+
+}
+
+/**
+ * Used to store ints for motor configuration.
+ */
+class MotorConfigInt
+{
+
+  /**
+   * Drive motor.
+   */
+  public int drive;
+  /**
+   * Angle motor.
+   */
+  public int angle;
+
+  /**
+   * Default constructor.
+   */
+  public MotorConfigInt()
+  {
+  }
+
+  /**
+   * Default constructor with values.
+   *
+   * @param drive Drive data.
+   * @param angle Angle data.
+   */
+  public MotorConfigInt(int drive, int angle)
+  {
+    this.angle = angle;
+    this.drive = drive;
+  }
+}

swervelib/parser/json/SwerveDriveJson.java

@@ -0,0 +1,30 @@
+package frc.robot.subsystems.swervedrive2.swervelib.parser.json;
+
+/**
+ * {@link frc.robot.subsystems.swervedrive2.swervelib.SwerveDrive} JSON parsed class. Used to access parsed data from
+ * the swervedrive.json file.
+ */
+public class SwerveDriveJson
+{
+
+  /**
+   * Maximum robot speed in feet per second.
+   */
+  public double     maxSpeed;
+  /**
+   * Optimal voltage to compensate to and base feedforward calculations off of.
+   */
+  public double     optimalVoltage;
+  /**
+   * Robot IMU used to determine heading of the robot.
+   */
+  public DeviceJson imu;
+  /**
+   * Invert the IMU of the robot.
+   */
+  public boolean    invertedIMU;
+  /**
+   * Module JSONs in order clockwise order starting from front left.
+   */
+  public String[]   modules;
+}

swervelib/parser/json/modules/BoolMotorJson.java

@@ -0,0 +1,17 @@
+package frc.robot.subsystems.swervedrive2.swervelib.parser.json.modules;
+
+/**
+ * Inverted motor JSON parsed class. Used to access the JSON data.
+ */
+public class BoolMotorJson
+{
+
+  /**
+   * Drive motor inversion state.
+   */
+  public boolean drive;
+  /**
+   * Angle motor inversion state.
+   */
+  public boolean angle;
+}

swervelib/parser/json/modules/LocationJson.java

@@ -0,0 +1,19 @@
+package frc.robot.subsystems.swervedrive2.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
+{
+
+  /**
+   * Location of the swerve module in inches from the center of the robot horizontally.
+   */
+  public double x;
+  /**
+   * Location of the swerve module in inches from the center of the robot vertically.
+   */
+  public double y;
+}