Updated swervelib

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));
+  }
+}