Config File and Feature Flags

src/main/deploy/config-common.jsonc

@@ -0,0 +1,118 @@
+{
+  "FEATURE_FLAGS": {
+		"ENABLE_DRIVE": true
+	},
+	"DRIVE_INFO": {
+    "STATUS_SIGNAL_FREQUENCY": 200,
+    "PIGEON_ID": 1,
+		"FRONT_LEFT_MODULE_DRIVE_MOTOR": 2,
+    "FRONT_LEFT_MODULE_STEER_MOTOR": 3,
+    "FRONT_LEFT_MODULE_STEER_ENCODER": 4,
+    "FRONT_LEFT_MODULE_STEER_OFFSET": 52.053,
+    "FRONT_LEFT_DRIVE_MOTOR_CONFIG": {
+      "KA": 0,
+      "KD": 0,
+      "KI": 0,
+      "KP": 0.38,
+      "KS": 0.24,
+      "KV": 0.123,
+      "VOLTAGE_CLOSED_LOOP_RAMP_PERIOD": 0,
+      "MOTOR_ROTATION_DIRECTION": "Clockwise_Positive"
+    },
+    "FRONT_LEFT_STEER_MOTOR_CONFIG": {
+      "KA": 0,
+      "KD": 0,
+      "KS": 0.5,
+      "KI": 0,
+      "KP": 4,
+      "KV": 0.15,
+      "VOLTAGE_CLOSED_LOOP_RAMP_PERIOD": 0,
+      "MOTOR_ROTATION_DIRECTION": "Clockwise_Positive"
+    },
+		"FRONT_RIGHT_MODULE_DRIVE_MOTOR": 8,
+    "FRONT_RIGHT_MODULE_STEER_MOTOR": 9,
+    "FRONT_RIGHT_MODULE_STEER_ENCODER": 10,
+    "FRONT_RIGHT_MODULE_STEER_OFFSET": -16.963,
+    "FRONT_RIGHT_DRIVE_MOTOR_CONFIG": {
+      "KA": 0,
+      "KD": 0,
+      "KI": 0,
+      "KP": 0.38,
+      "KS": 0.24,
+      "KV": 0.123,
+      "VOLTAGE_CLOSED_LOOP_RAMP_PERIOD": 0,
+      "MOTOR_ROTATION_DIRECTION": "Clockwise_Positive"
+    },
+    "FRONT_RIGHT_STEER_MOTOR_CONFIG": {
+      "KA": 0,
+      "KD": 0,
+      "KS": 0.5,
+      "KI": 0,
+      "KP": 4,
+      "KV": 0.15,
+      "VOLTAGE_CLOSED_LOOP_RAMP_PERIOD": 0,
+      "MOTOR_ROTATION_DIRECTION": "Clockwise_Positive"
+    },
+    "BACK_LEFT_MODULE_DRIVE_MOTOR": 5,
+    "BACK_LEFT_MODULE_STEER_MOTOR": 6,
+    "BACK_LEFT_MODULE_STEER_ENCODER": 7,
+    "BACK_LEFT_MODULE_STEER_OFFSET": 104.678,
+    "BACK_LEFT_DRIVE_MOTOR_CONFIG": {
+      "KA": 0,
+      "KD": 0,
+      "KI": 0,
+      "KP": 0.38,
+      "KS": 0.24,
+      "KV": 0.123,
+      "VOLTAGE_CLOSED_LOOP_RAMP_PERIOD": 0,
+      "MOTOR_ROTATION_DIRECTION": "CounterClockwise_Positive"
+    },
+    "BACK_LEFT_STEER_MOTOR_CONFIG": {
+      "KA": 0,
+      "KD": 0,
+      "KS": 0.5,
+      "KI": 0,
+      "KP": 4,
+      "KV": 0.15,
+      "VOLTAGE_CLOSED_LOOP_RAMP_PERIOD": 0,
+      "MOTOR_ROTATION_DIRECTION": "Clockwise_Positive"
+    },
+    "BACK_RIGHT_MODULE_DRIVE_MOTOR": 11,
+    "BACK_RIGHT_MODULE_STEER_MOTOR": 12,
+    "BACK_RIGHT_MODULE_STEER_ENCODER": 13,
+    "BACK_RIGHT_MODULE_STEER_OFFSET": -48.779,
+    "BACK_RIGHT_DRIVE_MOTOR_CONFIG": {
+      "KA": 0,
+      "KD": 0,
+      "KI": 0,
+      "KP": 0.38,
+      "KS": 0.24,
+      "KV": 0.123,
+      "VOLTAGE_CLOSED_LOOP_RAMP_PERIOD": 0,
+      "MOTOR_ROTATION_DIRECTION": "CounterClockwise_Positive"
+    },
+    "BACK_RIGHT_STEER_MOTOR_CONFIG": {
+      "KA": 0,
+      "KD": 0,
+      "KS": 0.5,
+      "KI": 0,
+      "KP": 4,
+      "KV": 0.15,
+      "VOLTAGE_CLOSED_LOOP_RAMP_PERIOD": 0,
+      "MOTOR_ROTATION_DIRECTION": "Clockwise_Positive"
+    },
+    // 1/6.98
+    // https://docs.wcproducts.com/welcome/gearboxes/wcp-swerve-x2/general-info/ratio-options
+    // X1 with 11 tooth pinion
+    "DRIVE_GEAR_RATIO": 0.1432664756446991,
+		"ANGLE_GEAR_RATIO": 0.08264462809917356,
+    "WHEEL_DIAMETER_METERS": 0.0975325866,
+    "TRACKWIDTH_METERS": 0.635,
+    "WHEELBASE_METERS": 0.4826,
+    "FULL_WIDTH_METERS": 0.9,
+    "FULL_LENGTH_METERS": 0.9,
+    "BUMPER_HEIGHT_METERS": 0.1,
+    "MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND": 7.33038286, // 420 deg/s measured 22. March
+    "MAX_LINEAR_VELOCITY_METERS_PER_SECOND": 4.0 // Estimated March 28
+	}
+}

src/main/deploy/config-simulation.jsonc

@@ -0,0 +1,4 @@
+// Overrides for simulation go here.
+{
+  "ROBORIO_CANBUS": "simulation"
+}

src/main/java/com/team973/frc2025/RobotConfig.java

@@ -0,0 +1,107 @@
+package com.team973.frc2025;
+
+import com.fasterxml.jackson.core.JsonParser;
+import com.fasterxml.jackson.core.json.JsonReadFeature;
+import com.fasterxml.jackson.databind.DeserializationFeature;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import com.team973.frc2025.shared.RobotInfo;
+import edu.wpi.first.wpilibj.Filesystem;
+import edu.wpi.first.wpilibj.RobotBase;
+import java.io.File;
+import java.io.IOException;
+import java.nio.file.Files;
+import java.nio.file.Paths;
+import java.util.ArrayList;
+import java.util.List;
+
+public class RobotConfig {
+
+  private static boolean m_initialized;
+  private static RobotInfo m_robotInfo;
+
+  // The following are set once during initialization so that we can log them
+  private static String m_configProfile;
+
+  public static RobotInfo get() {
+    maybeInitialize();
+    return m_robotInfo;
+  }
+
+  public static String getConfigProfile() {
+    maybeInitialize();
+    return m_configProfile;
+  }
+
+  private static synchronized void maybeInitialize() {
+    if (m_initialized) {
+      return;
+    }
+    List<String> configPaths = getConfigPaths();
+    System.err.printf("Using configSources: %s\n", configPaths);
+    m_robotInfo = getMergedConfig(configPaths);
+    m_initialized = true;
+  }
+
+  public static String readConfigProfile() throws IOException {
+    String path = Filesystem.getOperatingDirectory().toPath().resolve("profile").toString();
+    return Files.readString(Paths.get(path));
+  }
+
+  public static RobotInfo getMergedConfig(List<String> configPaths) {
+    RobotInfo res = new RobotInfo();
+
+    for (String path : configPaths) {
+
+      ObjectMapper mapper = new ObjectMapper();
+      mapper.configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, true);
+      mapper.configure(JsonReadFeature.ALLOW_TRAILING_COMMA.mappedFeature(), true);
+      mapper.setDefaultMergeable(true);
+      mapper.configure(JsonParser.Feature.ALLOW_COMMENTS, true);
+      mapper.configOverride(List.class).setMergeable(false);
+      try {
+        mapper.readerForUpdating(res).readValue(new File(path));
+      } catch (com.fasterxml.jackson.core.JsonParseException e) {
+        System.err.printf("JSON parsing error in %s: %s\n", path, e.getMessage());
+        throw new RuntimeException(e);
+      } catch (IOException e) {
+        System.err.println("Could not read file: " + e.getMessage());
+        throw new RuntimeException(e);
+      }
+    }
+
+    return res;
+  }
+
+  /** configSuffix is something like "common", "simulation", */
+  public static String absPathFromConfigSuffix(String configSuffix) {
+    return Filesystem.getDeployDirectory()
+        .toPath()
+        .resolve("config-" + configSuffix.trim() + ".jsonc")
+        .toString();
+  }
+
+  public static List<String> getConfigPaths() {
+    List<String> res = new ArrayList<>();
+
+    // Every robot starts out with the base config (config-common.yaml)
+    res.add(absPathFromConfigSuffix("common"));
+
+    if (RobotBase.isSimulation()) {
+      m_configProfile = "simulation";
+      res.add(absPathFromConfigSuffix("simulation"));
+    } else {
+      try {
+        m_configProfile = readConfigProfile();
+        res.add(absPathFromConfigSuffix(m_configProfile));
+      } catch (IOException e) {
+        // On a competition robot this isn't really a big deal because the common
+        // config should be complete, so we can continue on here. Setting the
+        // config profile to ERROR will signal that something failed during setup.
+        System.err.printf("Failed to read config profile: %s\n", e.getMessage());
+        m_configProfile = "ERROR";
+      }
+    }
+
+    return res;
+  }
+}