diff --git a/.github/workflows/compilation_check.yaml b/.github/workflows/compilation_check.yaml
index 16cbea9..6ae6b1c 100644
--- a/.github/workflows/compilation_check.yaml
+++ b/.github/workflows/compilation_check.yaml
@@ -82,5 +82,8 @@ jobs:
       - name: Install Roborio Toolchain
         run: ./gradlew installRoborioToolchain
 
+      - name: Clear build directory
+        run: ./gradlew clean
+
       - name: Compile code and run tests
-        run: ./gradlew build -PfromCI -PrunningSpotlessCpp
+        run: ./gradlew build -PfromCI --refresh-dependencies
diff --git a/.github/workflows/formatting_check.yaml b/.github/workflows/formatting_check.yaml
index 0d8d00a..9ab47a7 100644
--- a/.github/workflows/formatting_check.yaml
+++ b/.github/workflows/formatting_check.yaml
@@ -34,4 +34,4 @@ jobs:
         run: ./gradlew installRoborioToolchain
 
       - name: Spotless Check
-        run: ./gradlew spotlessCheck -PfromCI
+        run: ./gradlew spotlessCheck -PfromCI  -PrunningSpotlessCpp
diff --git a/README.md b/README.md
index 5726a77..77cf5aa 100644
--- a/README.md
+++ b/README.md
@@ -75,7 +75,7 @@ If unfamiliar with GitHub, go through tutorials in the [Using Git](#using-git) s
 
 #### CppCheck
 
-- Download [CppCheck](https://sourceforge.net/projects/cppcheck/files/1.86/cppcheck-1.86-x64-Setup.msi/download).
+- Download [CppCheck](https://github.com/danmar/cppcheck/releases/download/2.16.0/cppcheck-2.16.0-x64-Setup.msi).
 - If "Add CppCheck to Path" is an option during setup process, select YES.
   - If not, add `C:\Program Files\Cppcheck` to PATH
   - Read [this](https://stackoverflow.com/questions/44272416/how-to-add-a-folder-to-path-environment-variable-in-windows-10-with-screensho) for more information on adding files to PATH.
@@ -178,9 +178,9 @@ To undo the going back:
 
 
 ## CppCheck Warnings
+
 ```
-src\frc846\cpp\frc846\math\collection.cc:11:0: warning: The function 'HorizontalDeadband' is never used. [unusedFunction]
+src\frc846\cpp\frc846\robot\swerve\drivetrain.cc:152:64: warning: Variable 'accel_target' is assigned a value that is never used. [unreadVariable]
 src\frc846\cpp\frc846\math\collection.cc:25:0: warning: The function 'VerticalDeadband' is never used. [unusedFunction]
-src\frc846\cpp\frc846\math\collection.cc:39:0: warning: The function 'CoterminalDifference' is never used. [unusedFunction]
 src\frc846\cpp\frc846\math\collection.cc:52:0: warning: The function 'CoterminalSum' is never used. [unusedFunction]
 ```
\ No newline at end of file
diff --git a/__metrics.py b/__metrics.py
new file mode 100644
index 0000000..88d6f83
--- /dev/null
+++ b/__metrics.py
@@ -0,0 +1,26 @@
+# Outputs metrics such as [linecount] for this codebase
+
+import os
+
+INCL = ['*.py', '*.cc', '*.cpp', '.h', '.hpp', '.gradle', '.bat', '.sh', '.md', '.standard', '.json', '.lst']
+FORCE_INCL_DIR = ["src\\deploy", "src/deploy"]
+
+if __name__ == "__main__":
+
+    cwd = os.getcwd()
+
+    files = []
+    for root, dirs, filenames in os.walk(cwd):
+        for filename in filenames:
+            if any([root.__contains__(d) for d in FORCE_INCL_DIR]):
+                files.append(os.path.join(root, filename))
+                continue
+            for ext in INCL:
+                if filename.endswith(ext):
+                    files.append(os.path.join(root, filename))
+
+    linecount = 0
+    for file in files:
+        linecount += sum(1 for line in open(file))
+
+    print(linecount)
\ No newline at end of file
diff --git a/build.gradle b/build.gradle
index d44efbb..28e8734 100644
--- a/build.gradle
+++ b/build.gradle
@@ -36,7 +36,7 @@ def deployArtifact = deploy.targets.roborio.artifacts.frcCpp
 def includeDesktopSupport = false
 
 // Set to true to run simulation in debug mode
-wpi.cpp.debugSimulation = false
+wpi.cpp.debugSimulation = true
 
 // Default enable simgui
 wpi.sim.addGui().defaultEnabled = false
@@ -182,15 +182,21 @@ task runCppcheck(type: Exec) {
             }
         }
 
-        def cppcheckSection = """## CppCheck Warnings\n```\n${reportContent.trim()}\n```\n"""
+        def cppcheckSection = """
+## CppCheck Warnings
+
+```
+${reportContent.trim()}
+```
+"""
         def readmeFile = file('README.md')
-        def readmeContent = readmeFile.text
+        def readmeContent = readmeFile.text.trim()
 
         if (readmeContent.contains("## CppCheck Warnings")) {
             readmeContent = readmeContent.substring(0, readmeContent.indexOf("## CppCheck Warnings"))
         }
 
-        readmeContent += "\n" + cppcheckSection.trim()
+        readmeContent += cppcheckSection.trim()
         readmeFile.text = readmeContent
     }
 }
diff --git a/networktables.json b/networktables.json
new file mode 100644
index 0000000..c15bf00
--- /dev/null
+++ b/networktables.json
@@ -0,0 +1,186 @@
+[
+  {
+    "name": "/Preferences/DrivetrainConstructor/DrivetrainConstructor/drive_motor_current_limit (A)",
+    "type": "double",
+    "value": 120.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/DrivetrainConstructor/DrivetrainConstructor/steer_motor_current_limit (A)",
+    "type": "double",
+    "value": 120.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/DrivetrainConstructor/DrivetrainConstructor/drive_motor_smart_current_limit (A)",
+    "type": "double",
+    "value": 80.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/DrivetrainConstructor/DrivetrainConstructor/steer_motor_smart_current_limit (A)",
+    "type": "double",
+    "value": 80.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/DrivetrainConstructor/DrivetrainConstructor/drive_motor_voltage_compensation (V)",
+    "type": "double",
+    "value": 16.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/DrivetrainConstructor/DrivetrainConstructor/steer_motor_voltage_compensation (V)",
+    "type": "double",
+    "value": 12.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/FR/SwerveDrivetrain/FR/cancoder_offset_ (deg)",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/FL/SwerveDrivetrain/FL/cancoder_offset_ (deg)",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/BR/SwerveDrivetrain/BR/cancoder_offset_ (deg)",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/BL/SwerveDrivetrain/BL/cancoder_offset_ (deg)",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/steer_gains/kP",
+    "type": "double",
+    "value": 0.3,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/steer_gains/kI",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/steer_gains/kD",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/steer_gains/kF",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/robot_container/init_drivetrain",
+    "type": "boolean",
+    "value": true,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/robot_container/init_leds",
+    "type": "boolean",
+    "value": true,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/control_input/trigger_threshold",
+    "type": "double",
+    "value": 0.3,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/SwerveDrivetrain/max_speed (fps)",
+    "type": "double",
+    "value": 15.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/SwerveDrivetrain/max_omega (deg_per_s)",
+    "type": "double",
+    "value": 180.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/control_input/translation_deadband",
+    "type": "double",
+    "value": 0.07,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/control_input/translation_exponent",
+    "type": "int",
+    "value": 2,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/control_input/rotation_deadband",
+    "type": "double",
+    "value": 0.07,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/control_input/rotation_exponent",
+    "type": "int",
+    "value": 2,
+    "properties": {
+      "persistent": true
+    }
+  }
+]
diff --git a/networktables.json.bck b/networktables.json.bck
new file mode 100644
index 0000000..903bd45
--- /dev/null
+++ b/networktables.json.bck
@@ -0,0 +1,138 @@
+[
+  {
+    "name": "/Preferences/DrivetrainConstructor/DrivetrainConstructor/drive_motor_current_limit (A)",
+    "type": "double",
+    "value": 120.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/DrivetrainConstructor/DrivetrainConstructor/steer_motor_current_limit (A)",
+    "type": "double",
+    "value": 120.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/DrivetrainConstructor/DrivetrainConstructor/drive_motor_smart_current_limit (A)",
+    "type": "double",
+    "value": 80.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/DrivetrainConstructor/DrivetrainConstructor/steer_motor_smart_current_limit (A)",
+    "type": "double",
+    "value": 80.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/DrivetrainConstructor/DrivetrainConstructor/drive_motor_voltage_compensation (V)",
+    "type": "double",
+    "value": 16.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/DrivetrainConstructor/DrivetrainConstructor/steer_motor_voltage_compensation (V)",
+    "type": "double",
+    "value": 12.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/FR/SwerveDrivetrain/FR/cancoder_offset_ (deg)",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/FL/SwerveDrivetrain/FL/cancoder_offset_ (deg)",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/BR/SwerveDrivetrain/BR/cancoder_offset_ (deg)",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/BL/SwerveDrivetrain/BL/cancoder_offset_ (deg)",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/steer_gains/kP",
+    "type": "double",
+    "value": 0.3,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/steer_gains/kI",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/steer_gains/kD",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/steer_gains/kF",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/robot_container/init_drivetrain",
+    "type": "boolean",
+    "value": true,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/robot_container/init_leds",
+    "type": "boolean",
+    "value": true,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/control_input/trigger_threshold",
+    "type": "double",
+    "value": 0.3,
+    "properties": {
+      "persistent": true
+    }
+  }
+]
diff --git a/simgui-ds.json b/simgui-ds.json
new file mode 100644
index 0000000..73cc713
--- /dev/null
+++ b/simgui-ds.json
@@ -0,0 +1,92 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ]
+}
diff --git a/simgui.json b/simgui.json
new file mode 100644
index 0000000..2708274
--- /dev/null
+++ b/simgui.json
@@ -0,0 +1,24 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/get_prune_list": "Command",
+      "/SmartDashboard/prune_prefs": "Command",
+      "/SmartDashboard/set_cancoder_offsets": "Command",
+      "/SmartDashboard/verify_hardware": "Command",
+      "/SmartDashboard/zero_bearing": "Command"
+    }
+  },
+  "NetworkTables Info": {
+    "Connections": {
+      "open": true
+    },
+    "Server": {
+      "Subscribers": {
+        "open": true
+      },
+      "open": true
+    },
+    "visible": true
+  }
+}
diff --git a/src/frc846/cpp/frc846/control/HigherMotorController.cc b/src/frc846/cpp/frc846/control/HigherMotorController.cc
index 6c35454..9cc972c 100644
--- a/src/frc846/cpp/frc846/control/HigherMotorController.cc
+++ b/src/frc846/cpp/frc846/control/HigherMotorController.cc
@@ -106,4 +106,9 @@ void HigherMotorController::SetSoftLimits(
   soft_limits_ = soft_limits;
 }
 
+void HigherMotorController::EnableStatusFrames(
+    std::vector<frc846::control::config::StatusFrame> frames) {
+  frc846::control::MotorMonkey::EnableStatusFrames(slot_id_, frames);
+}
+
 }  // namespace frc846::control
diff --git a/src/frc846/cpp/frc846/control/MotorMonkey.cc b/src/frc846/cpp/frc846/control/MotorMonkey.cc
index ac24f73..5171dc7 100644
--- a/src/frc846/cpp/frc846/control/MotorMonkey.cc
+++ b/src/frc846/cpp/frc846/control/MotorMonkey.cc
@@ -17,10 +17,11 @@
 
 namespace frc846::control {
 
-#define CHECK_SLOT_ID()                        \
-  if (slot_id >= CONTROLLER_REGISTRY_SIZE ||   \
-      controller_registry[slot_id] == nullptr) \
-  throw std::runtime_error("Invalid MotorMonkey slot ID")
+#define CHECK_SLOT_ID()                                                      \
+  if (controller_registry[slot_id] == nullptr)                               \
+    throw std::runtime_error(                                                \
+        "Invalid MotorMonkey slot ID: " + std::to_string(slot_id) + " in " + \
+        "[" + __func__ + "]");
 
 #define LOG_IF_ERROR(action_name)                                          \
   {                                                                        \
@@ -35,20 +36,22 @@ namespace frc846::control {
 #define NUM_RETRIES 5
 #define INITIAL_RETRY_DELAY_MS 10
 
-#define SMART_RETRY(action, action_name)                                  \
-  for (int i = 0; i < NUM_RETRIES; i++) {                                 \
-    action;                                                               \
-    hardware::ControllerErrorCodes err =                                  \
-        controller_registry[slot_id]->GetLastErrorCode();                 \
-    if (err == hardware::ControllerErrorCodes::kAllOK)                    \
-      break;                                                              \
-    else {                                                                \
-      loggable_.Warn(                                                     \
-          "Error [{}] while attempting [{}] for slot ID {}. Retrying...", \
-          parseError(err), action_name, slot_id);                         \
-      std::this_thread::sleep_for(                                        \
-          std::chrono::milliseconds(INITIAL_RETRY_DELAY_MS * (1 << i)));  \
-    }                                                                     \
+#define SMART_RETRY(action, action_name)                                    \
+  for (int i = 0; i < NUM_RETRIES; i++) {                                   \
+    action;                                                                 \
+    hardware::ControllerErrorCodes err =                                    \
+        controller_registry[slot_id]->GetLastErrorCode();                   \
+    if (err == hardware::ControllerErrorCodes::kAllOK)                      \
+      break;                                                                \
+    else if (i == NUM_RETRIES - 1) {                                        \
+      loggable_.Error("Failed [{}] for slot ID {}.", action_name, slot_id); \
+    } else {                                                                \
+      loggable_.Warn(                                                       \
+          "Error [{}] while attempting [{}] for slot ID {}. Retrying...",   \
+          parseError(err), action_name, slot_id);                           \
+      std::this_thread::sleep_for(                                          \
+          std::chrono::milliseconds(INITIAL_RETRY_DELAY_MS * (1 << i)));    \
+    }                                                                       \
   }
 
 frc846::base::Loggable MotorMonkey::loggable_{"MotorMonkey"};
@@ -106,7 +109,10 @@ size_t MotorMonkey::ConstructController(
   frc846::control::hardware::IntermediateController* this_controller = nullptr;
 
   if (frc::RobotBase::IsSimulation() &&
-      MOTOR_SIM_LEVEL == MOTOR_SIM_LEVEL_SIM_HARDWARE) {
+      MOTOR_SIM_LEVEL == MOTOR_SIM_LEVEL_SIM_PHYSICS) {
+    std::cout << "Constructing simulation controller" << std::endl;
+    loggable_.Log(
+        "Constructing physics simulation controller for slot ID {}.", slot_id);
     slot_id_to_sim_[slot_id] = true;
     this_controller = controller_registry[slot_id] =
         new frc846::control::simulation::MCSimulator{
@@ -126,9 +132,13 @@ size_t MotorMonkey::ConstructController(
     this_controller = controller_registry[slot_id] =
         new frc846::control::hardware::SparkFLEX_interm{
             params.can_id, params.max_wait_time};
+  } else {
+    throw std::runtime_error("Invalid MotorMonkeyType [" +
+                             std::to_string((int)type) +
+                             "]: not constructing controller");
   }
 
-  if (this_controller == nullptr) return slot_id;
+  if (this_controller == nullptr) { return slot_id; }
 
   SMART_RETRY(this_controller->SetInverted(params.inverted), "SetInverted");
   LOG_IF_ERROR("SetInverted");
@@ -149,6 +159,15 @@ size_t MotorMonkey::ConstructController(
   return slot_id;
 }
 
+void MotorMonkey::EnableStatusFrames(
+    size_t slot_id, std::vector<frc846::control::config::StatusFrame> frames) {
+  CHECK_SLOT_ID();
+
+  SMART_RETRY(controller_registry[slot_id]->EnableStatusFrames(frames),
+      "EnableStatusFrames");
+  LOG_IF_ERROR("EnableStatusFrames");
+}
+
 units::volt_t MotorMonkey::GetBatteryVoltage() { return battery_voltage; }
 
 void MotorMonkey::SetLoad(size_t slot_id, units::newton_meter_t load) {
diff --git a/src/frc846/cpp/frc846/control/config/SoftLimitsHelper.cc b/src/frc846/cpp/frc846/control/config/soft_limits.cc
similarity index 97%
rename from src/frc846/cpp/frc846/control/config/SoftLimitsHelper.cc
rename to src/frc846/cpp/frc846/control/config/soft_limits.cc
index c559823..e0b8f32 100644
--- a/src/frc846/cpp/frc846/control/config/SoftLimitsHelper.cc
+++ b/src/frc846/cpp/frc846/control/config/soft_limits.cc
@@ -1,4 +1,4 @@
-#include "frc846/control/config/SoftLimitsHelper.h"
+#include "frc846/control/config/soft_limits.h"
 
 namespace frc846::control::config {
 
diff --git a/src/frc846/cpp/frc846/control/hardware/SparkMXFX_interm.cc b/src/frc846/cpp/frc846/control/hardware/SparkMXFX_interm.cc
index bc86728..9371058 100644
--- a/src/frc846/cpp/frc846/control/hardware/SparkMXFX_interm.cc
+++ b/src/frc846/cpp/frc846/control/hardware/SparkMXFX_interm.cc
@@ -1,4 +1,5 @@
 #include "frc846/control/hardware/SparkMXFX_interm.h"
+
 #include "frc846/wpilib/util.h"
 
 namespace frc846::control::hardware {
@@ -9,6 +10,7 @@ namespace frc846::control::hardware {
   if (last_error_ != ControllerErrorCodes::kAllOK) return
 
 bool SparkMXFX_interm::VerifyConnected() {
+  if (esc_ == nullptr) return false;
   esc_->GetFirmwareVersion();
   return esc_->GetFirmwareVersion() != 0;
 }
diff --git a/src/frc846/cpp/frc846/math/collection.cc b/src/frc846/cpp/frc846/math/collection.cc
index 8ea0ee3..27da0bc 100644
--- a/src/frc846/cpp/frc846/math/collection.cc
+++ b/src/frc846/cpp/frc846/math/collection.cc
@@ -51,14 +51,14 @@ units::degree_t CoterminalDifference(
 
 units::degree_t CoterminalSum(
     units::degree_t angle, units::degree_t other_angle) {
-  const units::angle::degree_t difference =
+  const units::angle::degree_t sum =
       units::math::fmod(angle, 1_tr) + units::math::fmod(other_angle, 1_tr);
-  if (difference > 0.5_tr) {
-    return difference - 1_tr;
-  } else if (difference < -0.5_tr) {
-    return difference + 1_tr;
+  if (sum > 0.5_tr) {
+    return sum - 1_tr;
+  } else if (sum < -0.5_tr) {
+    return sum + 1_tr;
   } else {
-    return difference;
+    return sum;
   }
 }
 
diff --git a/src/frc846/cpp/frc846/robot/GenericRobot.cc b/src/frc846/cpp/frc846/robot/GenericRobot.cc
index fa97ece..0161209 100644
--- a/src/frc846/cpp/frc846/robot/GenericRobot.cc
+++ b/src/frc846/cpp/frc846/robot/GenericRobot.cc
@@ -69,13 +69,13 @@ void GenericRobot::StartCompetition() {
       new frc846::wpilib::NTAction(
           [this] { frc846::base::Loggable::PrunePreferences(this); }));
 
-  // Verify robot hardware
-  VerifyHardware();
-
   // Setup all subsystems and set initial targets to zero.
   generic_robot_container_->Setup();
   generic_robot_container_->ZeroTargets();
 
+  // Verify robot hardware
+  VerifyHardware();
+
   OnInitialize();
 
   // Report to driver station that robot is ready
diff --git a/src/frc846/cpp/frc846/robot/swerve/control/swerve_ol_calculator.cc b/src/frc846/cpp/frc846/robot/swerve/control/swerve_ol_calculator.cc
new file mode 100644
index 0000000..183cc02
--- /dev/null
+++ b/src/frc846/cpp/frc846/robot/swerve/control/swerve_ol_calculator.cc
@@ -0,0 +1,57 @@
+#include "frc846/robot/swerve/control/swerve_ol_calculator.h"
+
+namespace frc846::robot::swerve::control {
+
+SwerveOpenLoopCalculatorOutput SwerveOpenLoopCalculator::calculate(
+    SwerveOpenLoopCalculatorInputs inputs) {
+  std::pair<double, double> kModuleLocationSigns[4] = {
+      {+0.5, +0.5},  // FR
+      {-0.5, +0.5},  // FL
+      {-0.5, -0.5},  // BL
+      {+0.5, -0.5},  // BR
+  };
+
+  std::array<frc846::math::VectorND<units::feet_per_second, 2>, 4>
+      module_targets;
+
+  units::inch_t radius = units::math::hypot(
+      constants_.wheelbase_horizontal_dim, constants_.wheelbase_forward_dim);
+
+  for (int i = 0; i < 4; i++) {
+    frc846::math::VectorND<units::inch, 2> location{
+        kModuleLocationSigns[i].first * constants_.wheelbase_horizontal_dim,
+        kModuleLocationSigns[i].second * constants_.wheelbase_forward_dim};
+
+    units::degree_t rot_direction = location.angle(false);
+
+    frc846::math::VectorND<units::feet_per_second, 2> rotation{
+        inputs.rotation_target * units::math::cos(rot_direction) * radius /
+            1_rad,
+        inputs.rotation_target * units::math::sin(rot_direction) * radius /
+            1_rad};
+
+    module_targets[i] = inputs.translation_target + rotation;
+  }
+
+  units::feet_per_second_t max_mag = 0_fps;
+  for (int i = 0; i < 4; i++) {
+    if (module_targets[i].magnitude() > max_mag) {
+      max_mag = module_targets[i].magnitude();
+    }
+  }
+
+  if (max_mag > inputs.max_speed) {
+    for (int i = 0; i < 4; i++) {
+      module_targets[i] *= inputs.max_speed / max_mag;
+    }
+  }
+
+  SwerveOpenLoopCalculatorOutput output{};
+  for (int i = 0; i < 4; i++) {
+    output.drive_outputs[i] = module_targets[i].magnitude();
+    output.steer_outputs[i] = module_targets[i].angle(true);
+  }
+  return output;
+}
+
+}  // namespace frc846::robot::swerve::control
\ No newline at end of file
diff --git a/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc b/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc
new file mode 100644
index 0000000..aa5587c
--- /dev/null
+++ b/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc
@@ -0,0 +1,166 @@
+#include "frc846/robot/swerve/drivetrain.h"
+
+#include "frc846/robot/swerve/control/swerve_ol_calculator.h"
+#include "frc846/robot/swerve/swerve_module.h"
+
+namespace frc846::robot::swerve {
+
+DrivetrainSubsystem::DrivetrainSubsystem(DrivetrainConfigs configs)
+    : GenericSubsystem{"SwerveDrivetrain"},
+      configs_{configs},
+      modules_{},
+      navX_{frc::SerialPort::kMXP} {
+  for (int i = 0; i < 4; i++) {
+    modules_[i] = new SwerveModuleSubsystem{*this,
+        configs_.module_unique_configs[i], configs_.module_common_config};
+  }
+
+  RegisterPreference("steer_gains/kP", 0.3);
+  RegisterPreference("steer_gains/kI", 0.0);
+  RegisterPreference("steer_gains/kD", 0.0);
+  RegisterPreference("steer_gains/kF", 0.0);
+
+  RegisterPreference("max_speed", 15_fps);
+  RegisterPreference("max_omega", units::degrees_per_second_t{180});
+
+  odometry_.setConstants({});
+  ol_calculator_.setConstants({
+      .wheelbase_horizontal_dim = configs.wheelbase_horizontal_dim,
+      .wheelbase_forward_dim = configs.wheelbase_forward_dim,
+  });
+}
+
+void DrivetrainSubsystem::Setup() {
+  frc846::control::base::MotorGains steer_gains{
+      GetPreferenceValue_double("steer_gains/kP"),
+      GetPreferenceValue_double("steer_gains/kI"),
+      GetPreferenceValue_double("steer_gains/kD"),
+      GetPreferenceValue_double("steer_gains/kF")};
+  for (SwerveModuleSubsystem* module : modules_) {
+    module->InitByParent();
+    module->Setup();
+    module->SetSteerGains(steer_gains);
+    module->ZeroWithCANcoder();
+  }
+}
+
+DrivetrainTarget DrivetrainSubsystem::ZeroTarget() const {
+  return DrivetrainOLControlTarget{{0_fps, 0_fps}, 0_deg_per_s};
+}
+
+bool DrivetrainSubsystem::VerifyHardware() {
+  bool ok = true;
+  for (SwerveModuleSubsystem* module : modules_) {
+    ok &= module->VerifyHardware();
+  }
+  FRC846_VERIFY(ok, ok, "At least one module failed verification");
+  return ok;
+}
+
+void DrivetrainSubsystem::ZeroBearing() {
+  if (!is_initialized()) return;
+
+  constexpr int kMaxAttempts = 5;
+  constexpr int kSleepTimeMs = 500;
+  for (int attempts = 1; attempts <= kMaxAttempts; ++attempts) {
+    Log("Gyro zero attempt {}/{}", attempts, kMaxAttempts);
+    if (navX_.IsConnected() && !navX_.IsCalibrating()) {
+      navX_.ZeroYaw();
+      Log("Zeroed bearing");
+
+      for (SwerveModuleSubsystem* module : modules_) {
+        module->ZeroWithCANcoder();
+      }
+      return;
+    }
+
+    Warn("Attempt to zero failed, sleeping {} ms...", kSleepTimeMs);
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(kSleepTimeMs));
+  }
+  Error("Unable to zero after {} attempts", kMaxAttempts);
+}
+
+void DrivetrainSubsystem::SetCANCoderOffsets() {
+  for (SwerveModuleSubsystem* module : modules_) {
+    module->SetCANCoderOffset();
+  }
+}
+
+DrivetrainReadings DrivetrainSubsystem::ReadFromHardware() {
+  units::degree_t bearing = navX_.GetAngle() * 1_deg;
+
+  Graph("readings/bearing", bearing);
+
+  frc846::math::VectorND<units::inch, 4> drive_positions{
+      0_in, 0_in, 0_in, 0_in};
+  std::array<units::degree_t, 4> steer_positions{};
+
+  frc846::math::VectorND<units::feet_per_second, 2> velocity{0_fps, 0_fps};
+
+  for (int i = 0; i < 4; i++) {
+    modules_[i]->UpdateReadings();
+    SwerveModuleReadings r = modules_[i]->GetReadings();
+
+    drive_positions[i] = r.drive_pos;
+    steer_positions[i] = r.steer_pos;
+
+    velocity += (frc846::math::VectorND<units::feet_per_second, 2>{
+        r.vel, r.steer_pos + bearing, true});
+  }
+
+  velocity /= 4.0;
+
+  Graph("readings/velocity_x", velocity[0]);
+  Graph("readings/velocity_y", velocity[1]);
+
+  frc846::robot::swerve::odometry::SwervePose new_pose{
+      .position =
+          odometry_.calculate({bearing, steer_positions, drive_positions})
+              .position,
+      .bearing = bearing,
+      .velocity = velocity};
+
+  // TODO: consider bearing simulation
+
+  Graph("readings/position_x", new_pose.position[0]);
+  Graph("readings/position_y", new_pose.position[1]);
+
+  return {new_pose};
+}
+
+void DrivetrainSubsystem::WriteToHardware(DrivetrainTarget target) {
+  if (DrivetrainOLControlTarget* ol_target =
+          std::get_if<DrivetrainOLControlTarget>(&target)) {
+    Graph("target/ol_velocity_x", ol_target->velocity[0]);
+    Graph("target/ol_velocity_y", ol_target->velocity[1]);
+    Graph("target/ol_angular_velocity", ol_target->angular_velocity);
+
+    units::degree_t bearing = navX_.GetAngle() * 1_deg;
+
+    auto ol_calc_outputs = ol_calculator_.calculate({ol_target->velocity,
+        ol_target->angular_velocity,
+        GetPreferenceValue_unit_type<units::feet_per_second_t>("max_speed")});
+
+    for (int i = 0; i < 4; i++) {
+      modules_[i]->SetSteerGains({GetPreferenceValue_double("steer_gains/kP"),
+          GetPreferenceValue_double("steer_gains/kI"),
+          GetPreferenceValue_double("steer_gains/kD"),
+          GetPreferenceValue_double("steer_gains/kF")});
+
+      SwerveModuleOLControlTarget module_target{
+          .drive = ol_calc_outputs.drive_outputs[i],
+          .steer = ol_calc_outputs.steer_outputs[i] - bearing};
+      modules_[i]->SetTarget(module_target);
+    }
+  } else if (DrivetrainAccelerationControlTarget* accel_target =
+                 std::get_if<DrivetrainAccelerationControlTarget>(&target)) {
+    throw std::runtime_error("Acceleration control not yet implemented");
+    // TODO: implement acceleration control for drivetrain
+  }
+
+  for (int i = 0; i < 4; i++)
+    modules_[i]->UpdateHardware();
+}
+
+}  // namespace frc846::robot::swerve
\ No newline at end of file
diff --git a/src/frc846/cpp/frc846/robot/swerve/odometry/swerve_odometry_calculator.cc b/src/frc846/cpp/frc846/robot/swerve/odometry/swerve_odometry_calculator.cc
new file mode 100644
index 0000000..0bb4074
--- /dev/null
+++ b/src/frc846/cpp/frc846/robot/swerve/odometry/swerve_odometry_calculator.cc
@@ -0,0 +1,25 @@
+#include "frc846/robot/swerve/odometry/swerve_odometry_calculator.h"
+
+namespace frc846::robot::swerve::odometry {
+
+SwerveOdometryCalculator::SwerveOdometryCalculator() {}
+
+SwerveOdometryOutput SwerveOdometryCalculator::calculate(
+    SwerveOdometryInputs inputs) {
+  auto module_diffs = inputs.drive_pos - previous_module_positions_;
+  previous_module_positions_ = inputs.drive_pos;
+
+  frc846::math::Vector2D displacement{0_ft, 0_ft};
+
+  for (int i = 0; i < 4; i++) {
+    displacement +=
+        frc846::math::Vector2D{inputs.drive_pos[i], inputs.steer_pos[i], true};
+  }
+  displacement.rotate(inputs.bearing, true);
+
+  last_position_ += displacement;
+
+  return {last_position_};
+}
+
+}  // namespace frc846::robot::swerve::odometry
\ No newline at end of file
diff --git a/src/frc846/cpp/frc846/robot/swerve/odometry/swerve_pose.cc b/src/frc846/cpp/frc846/robot/swerve/odometry/swerve_pose.cc
new file mode 100644
index 0000000..669db38
--- /dev/null
+++ b/src/frc846/cpp/frc846/robot/swerve/odometry/swerve_pose.cc
@@ -0,0 +1,30 @@
+#include "frc846/robot/swerve/odometry/swerve_pose.h"
+
+namespace frc846::robot::swerve::odometry {
+
+SwervePose SwervePose::rotate(units::degree_t angle) const {
+  return SwervePose{
+      position.rotate(angle), bearing + angle, velocity.rotate(angle)};
+}
+
+SwervePose SwervePose::translate(frc846::math::Vector2D translation) const {
+  return SwervePose{position + translation, bearing, velocity};
+}
+
+SwervePose SwervePose::extrapolate(units::second_t time) const {
+  return SwervePose{
+      position + frc846::math::Vector2D{velocity[0] * time, velocity[1] * time},
+      bearing, velocity};
+}
+
+SwervePose SwervePose::operator+(const SwervePose& other) const {
+  return SwervePose{position + other.position, bearing + other.bearing,
+      velocity + other.velocity};
+}
+
+SwervePose SwervePose::operator-(const SwervePose& other) const {
+  return SwervePose{position - other.position, bearing - other.bearing,
+      velocity - other.velocity};
+}
+
+}  // namespace frc846::robot::swerve::odometry
diff --git a/src/frc846/cpp/frc846/robot/swerve/swerve_module.cc b/src/frc846/cpp/frc846/robot/swerve/swerve_module.cc
new file mode 100644
index 0000000..1844156
--- /dev/null
+++ b/src/frc846/cpp/frc846/robot/swerve/swerve_module.cc
@@ -0,0 +1,183 @@
+#include "frc846/robot/swerve/swerve_module.h"
+
+#include <units/math.h>
+
+#include "frc846/math/collection.h"
+
+namespace frc846::robot::swerve {
+
+SwerveModuleSubsystem::SwerveModuleSubsystem(Loggable& parent,
+    SwerveModuleUniqueConfig unique_config,
+    SwerveModuleCommonConfig common_config)
+    : frc846::robot::GenericSubsystem<SwerveModuleReadings, SwerveModuleTarget>(
+          parent, unique_config.loc),
+      drive_{common_config.motor_types,
+          getMotorParams(unique_config, common_config).first},
+      steer_{common_config.motor_types,
+          getMotorParams(unique_config, common_config).second},
+      cancoder_{unique_config.cancoder_id, common_config.bus} {
+  drive_helper_.SetConversion(common_config.drive_reduction);
+  steer_helper_.SetConversion(common_config.steer_reduction);
+
+  drive_helper_.bind(&drive_);
+  steer_helper_.bind(&steer_);
+
+  cancoder_.OptimizeBusUtilization();
+  cancoder_.GetAbsolutePosition().SetUpdateFrequency(20_Hz);
+
+  RegisterPreference("cancoder_offset_", 0.0_deg);
+
+  max_speed_ = frc846::control::base::MotorSpecificationPresets::get(
+                   common_config.motor_types)
+                   .free_speed *
+               common_config.drive_reduction;
+}
+
+std::pair<frc846::control::config::MotorConstructionParameters,
+    frc846::control::config::MotorConstructionParameters>
+SwerveModuleSubsystem::getMotorParams(SwerveModuleUniqueConfig unique_config,
+    SwerveModuleCommonConfig common_config) {
+  frc846::control::config::MotorConstructionParameters drive_params =
+      common_config.drive_params;
+  frc846::control::config::MotorConstructionParameters steer_params =
+      common_config.steer_params;
+
+  drive_params.can_id = unique_config.drive_id;
+  steer_params.can_id = unique_config.steer_id;
+
+  drive_params.bus = common_config.bus;
+  steer_params.bus = common_config.bus;
+
+  drive_params.circuit_resistance = unique_config.circuit_resistance;
+  steer_params.circuit_resistance = unique_config.circuit_resistance;
+
+  return {drive_params, steer_params};
+}
+
+void SwerveModuleSubsystem::Setup() {
+  drive_.Setup();
+  drive_.EnableStatusFrames(
+      {frc846::control::config::StatusFrame::kPositionFrame,
+          frc846::control::config::StatusFrame::kVelocityFrame});
+  drive_helper_.SetPosition(0_ft);
+
+  steer_.Setup();
+  steer_.EnableStatusFrames(
+      {frc846::control::config::StatusFrame::kPositionFrame});
+
+  ZeroWithCANcoder();
+}
+
+SwerveModuleTarget SwerveModuleSubsystem::ZeroTarget() const {
+  return SwerveModuleOLControlTarget{0.0_fps, 0_deg};
+}
+
+bool SwerveModuleSubsystem::VerifyHardware() {
+  bool ok = true;
+  FRC846_VERIFY(drive_.VerifyConnected(), ok, "Could not verify drive motor");
+  FRC846_VERIFY(steer_.VerifyConnected(), ok, "Could not verify steer motor");
+  return ok;
+}
+
+void SwerveModuleSubsystem::SetCANCoderOffset() {
+  SetCANCoderOffset(cancoder_.GetAbsolutePosition().GetValue());
+}
+void SwerveModuleSubsystem::SetCANCoderOffset(units::degree_t offset) {
+  SetPreferenceValue("cancoder_offset_", offset);
+}
+
+void SwerveModuleSubsystem::ZeroWithCANcoder() {
+  constexpr int kMaxAttempts = 5;
+  constexpr int kSleepTimeMs = 500;
+
+  for (int attempts = 1; attempts <= kMaxAttempts; ++attempts) {
+    Log("CANCoder zero attempt {}/{}", attempts, kMaxAttempts);
+    auto position = cancoder_.GetAbsolutePosition();
+
+    if (position.IsAllGood()) {
+      steer_helper_.SetPosition(-position.GetValue());
+      Log("Zeroed to {}!", -position.GetValue());
+      return;
+    }
+
+    Warn("Attempt to zero failed, sleeping {} ms...", kSleepTimeMs);
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(kSleepTimeMs));
+  }
+  Error("Unable to zero after {} attempts", kMaxAttempts);
+}
+
+SwerveModuleReadings SwerveModuleSubsystem::ReadFromHardware() {
+  SwerveModuleReadings readings;
+  readings.vel = drive_helper_.GetVelocity();
+  readings.drive_pos = drive_helper_.GetPosition();
+  readings.steer_pos = steer_helper_.GetPosition();
+
+  Graph("readings/drive_motor_vel", readings.vel);
+  Graph("readings/drive_motor_pos", readings.drive_pos);
+  Graph("readings/steer_motor_pos", readings.steer_pos);
+
+  Graph("readings/cancoder_pos",
+      units::degree_t(cancoder_.GetAbsolutePosition().GetValue()));
+
+  return readings;
+}
+
+void SwerveModuleSubsystem::WriteToHardware(SwerveModuleTarget target) {
+  if (SwerveModuleOLControlTarget* ol_target =
+          std::get_if<SwerveModuleOLControlTarget>(&target)) {
+    Graph("target/ol_drive_target", ol_target->drive);
+    Graph("target/ol_steer_target", ol_target->steer);
+
+    auto [steer_dir, invert_drive] =
+        calculateSteerPosition(GetReadings().steer_pos, ol_target->steer);
+
+    Graph("target/steer_dir", steer_dir);
+    Graph("target/invert_drive", invert_drive);
+
+    units::dimensionless::scalar_t cosine_comp =
+        units::math::cos(steer_dir - GetReadings().steer_pos);
+
+    Graph("target/cosine_comp", cosine_comp.to<double>());
+
+    double drive_duty_cycle = ol_target->drive / max_speed_;
+
+    drive_helper_.WriteDC(cosine_comp * drive_duty_cycle);
+
+    if (std::abs(drive_duty_cycle) > 0.002) {
+      steer_helper_.WritePositionOnController(ol_target->steer);
+    }
+  } else if (SwerveModuleTorqueControlTarget* torque_target =
+                 std::get_if<SwerveModuleTorqueControlTarget>(&target)) {
+    // TODO: finish torque control for drivetrain
+    Graph("target/torque_drive_target", torque_target->drive);
+    Graph("target/torque_steer_target", torque_target->steer);
+  } else {
+    throw std::runtime_error("SwerveModuleTarget was not of a valid type");
+  }
+}
+
+std::pair<units::degree_t, bool> SwerveModuleSubsystem::calculateSteerPosition(
+    units::degree_t target_norm, units::degree_t current) {
+  bool reverse = false;
+
+  units::degree_t diff =
+      frc846::math::CoterminalDifference(target_norm, current);
+
+  if (diff > 90_deg) {
+    diff -= 180_deg;
+    reverse = true;
+  } else if (diff < -90_deg) {
+    diff += 180_deg;
+    reverse = true;
+  }
+
+  return {current + diff, reverse};
+}
+
+void SwerveModuleSubsystem::SetSteerGains(
+    frc846::control::base::MotorGains gains) {
+  steer_.SetGains(gains);
+}
+
+}  // namespace frc846::robot::swerve
\ No newline at end of file
diff --git a/src/frc846/include/frc846/control/HMCHelper.h b/src/frc846/include/frc846/control/HMCHelper.h
index 5b9c6eb..76f022f 100644
--- a/src/frc846/include/frc846/control/HMCHelper.h
+++ b/src/frc846/include/frc846/control/HMCHelper.h
@@ -94,7 +94,7 @@ template <typename T> class HMCHelper {
   void SetSoftLimits(bool using_limits, pos_unit forward_limit = pos_unit(0),
       pos_unit reverse_limit = pos_unit(0),
       pos_unit forward_reduce = pos_unit(0),
-      pos_unit reverse_reduce = pos_unit(0), double reduce_max_dc = double) {
+      pos_unit reverse_reduce = pos_unit(0), double reduce_max_dc = 0.0) {
     hmc_->SetSoftLimits(config::SoftLimitsHelper<T>::CreateSoftLimits(conv_,
         using_limits, forward_limit, reverse_limit, forward_reduce,
         reverse_reduce, reduce_max_dc));
diff --git a/src/frc846/include/frc846/control/HigherMotorController.h b/src/frc846/include/frc846/control/HigherMotorController.h
index 05e833c..7308263 100644
--- a/src/frc846/include/frc846/control/HigherMotorController.h
+++ b/src/frc846/include/frc846/control/HigherMotorController.h
@@ -2,14 +2,15 @@
 
 #include <units/torque.h>
 
+#include <optional>
+#include <vector>
+
 #include "frc846/control/base/motor_control_base.h"
 #include "frc846/control/base/motor_gains.h"
 #include "frc846/control/base/motor_specs.h"
 #include "frc846/control/config/construction_params.h"
-
-#include "frc846/control/config/SoftLimitsHelper.h"
-
-#include <optional>
+#include "frc846/control/config/soft_limits.h"
+#include "frc846/control/config/status_frames.h"
 
 namespace frc846::control {
 
@@ -86,7 +87,10 @@ class HigherMotorController {
   // Custom soft limits maintained by HigherMotorController
   void SetSoftLimits(config::SoftLimits soft_limits);
 
-  // verify's if the hardware is connected
+  // Enables specific status frames and disables all others
+  void EnableStatusFrames(std::vector<config::StatusFrame> frames);
+
+  // Verifies if the speed controller is connected and accessible
   bool VerifyConnected();
 
 private:
diff --git a/src/frc846/include/frc846/control/MotorMonkey.h b/src/frc846/include/frc846/control/MotorMonkey.h
index 14ef3eb..c7535ce 100644
--- a/src/frc846/include/frc846/control/MotorMonkey.h
+++ b/src/frc846/include/frc846/control/MotorMonkey.h
@@ -4,12 +4,14 @@
 #include <rev/CANSparkMax.h>
 
 #include <ctre/phoenix6/TalonFX.hpp>
+#include <vector>
 
 #include "frc846/base/Loggable.h"
 #include "frc846/control/base/motor_control_base.h"
 #include "frc846/control/base/motor_gains.h"
 #include "frc846/control/base/motor_specs.h"
 #include "frc846/control/config/construction_params.h"
+#include "frc846/control/config/status_frames.h"
 #include "frc846/control/hardware/TalonFX_interm.h"
 
 #define CONTROLLER_REGISTRY_SIZE 64
@@ -40,6 +42,14 @@ class MotorMonkey {
   static size_t ConstructController(frc846::control::base::MotorMonkeyType type,
       frc846::control::config::MotorConstructionParameters params);
 
+  /*
+  EnableStatusFrames()
+
+  Enables specific status frames for a motor controller. Disables all others.
+  */
+  static void EnableStatusFrames(
+      size_t slot_id, std::vector<frc846::control::config::StatusFrame> frames);
+
   /*
   GetBatteryVoltage()
 
diff --git a/src/frc846/include/frc846/control/calculators/CircuitResistanceCalculator.h b/src/frc846/include/frc846/control/calculators/CircuitResistanceCalculator.h
index 66d7111..e8a0ee2 100644
--- a/src/frc846/include/frc846/control/calculators/CircuitResistanceCalculator.h
+++ b/src/frc846/include/frc846/control/calculators/CircuitResistanceCalculator.h
@@ -2,7 +2,7 @@
 
 #include <units/length.h>
 
-#include "frc846/wpilib/846_units.h"
+#include "frc846/wpilib/units.h"
 
 namespace frc846::control::calculators {
 
diff --git a/src/frc846/include/frc846/control/calculators/CurrentTorqueCalculator.h b/src/frc846/include/frc846/control/calculators/CurrentTorqueCalculator.h
index df247e0..3ce74e2 100644
--- a/src/frc846/include/frc846/control/calculators/CurrentTorqueCalculator.h
+++ b/src/frc846/include/frc846/control/calculators/CurrentTorqueCalculator.h
@@ -2,7 +2,7 @@
 
 #include "frc846/control/base/motor_control_base.h"
 #include "frc846/control/base/motor_specs.h"
-#include "frc846/wpilib/846_units.h"
+#include "frc846/wpilib/units.h"
 
 namespace frc846::control::calculators {
 
diff --git a/src/frc846/include/frc846/control/calculators/VelocityPositionEstimator.h b/src/frc846/include/frc846/control/calculators/VelocityPositionEstimator.h
index 0f8af5d..a66b7d7 100644
--- a/src/frc846/include/frc846/control/calculators/VelocityPositionEstimator.h
+++ b/src/frc846/include/frc846/control/calculators/VelocityPositionEstimator.h
@@ -4,7 +4,7 @@
 #include <units/mass.h>
 #include <units/torque.h>
 
-#include "frc846/wpilib/846_units.h"
+#include "frc846/wpilib/units.h"
 
 namespace frc846::control::calculators {
 
diff --git a/src/frc846/include/frc846/control/config/construction_params.h b/src/frc846/include/frc846/control/config/construction_params.h
index aff213d..ddb8656 100644
--- a/src/frc846/include/frc846/control/config/construction_params.h
+++ b/src/frc846/include/frc846/control/config/construction_params.h
@@ -4,7 +4,7 @@
 #include <units/time.h>
 #include <units/voltage.h>
 
-#include "frc846/wpilib/846_units.h"
+#include "frc846/wpilib/units.h"
 
 namespace frc846::control::config {
 
@@ -17,8 +17,6 @@ Contains all parameters necessary to construct a motor controller.
 @param inverted: Whether the motor controller is inverted.
 @param brake_mode: Whether the motor controller is in brake mode.
 @param motor_current_limit: The current limit maintained onboard the ESC.
-@param threshold_time: The time threshold for the current limit specified
-earlier.
 @param smart_current_limit: The smart current limit for the motor controller.
 @param voltage_compensation: The voltage compensation of the motor controller.
 @param circuit_resistance: The circuit resistance leading upto motor controller.
@@ -46,7 +44,7 @@ struct MotorConstructionParameters {
 
   frc846::wpilib::unit_kg_m_sq rotational_inertia;
 
-  std::string bus = "rio";
+  std::string bus = "";
 
   units::millisecond_t max_wait_time = 20_ms;
 };
diff --git a/src/frc846/include/frc846/control/config/SoftLimitsHelper.h b/src/frc846/include/frc846/control/config/soft_limits.h
similarity index 99%
rename from src/frc846/include/frc846/control/config/SoftLimitsHelper.h
rename to src/frc846/include/frc846/control/config/soft_limits.h
index c182eb1..7eaa357 100644
--- a/src/frc846/include/frc846/control/config/SoftLimitsHelper.h
+++ b/src/frc846/include/frc846/control/config/soft_limits.h
@@ -1,6 +1,7 @@
 #pragma once
 
 #include <units/angular_velocity.h>
+
 #include <cassert>
 
 namespace frc846::control::config {
diff --git a/src/frc846/include/frc846/control/hardware/SparkMXFX_interm.h b/src/frc846/include/frc846/control/hardware/SparkMXFX_interm.h
index da840c9..923f790 100644
--- a/src/frc846/include/frc846/control/hardware/SparkMXFX_interm.h
+++ b/src/frc846/include/frc846/control/hardware/SparkMXFX_interm.h
@@ -2,8 +2,9 @@
 
 #include <rev/CANSparkFlex.h>
 #include <rev/CANSparkMax.h>
-#include <variant>
+
 #include <algorithm>
+#include <variant>
 
 #include "IntermediateController.h"
 
diff --git a/src/frc846/include/frc846/control/hardware/TalonFX_interm.h b/src/frc846/include/frc846/control/hardware/TalonFX_interm.h
index f7b099b..1ecfc12 100644
--- a/src/frc846/include/frc846/control/hardware/TalonFX_interm.h
+++ b/src/frc846/include/frc846/control/hardware/TalonFX_interm.h
@@ -15,7 +15,7 @@ TalonFX hardware.
 */
 class TalonFX_interm : public IntermediateController {
 public:
-  TalonFX_interm(int can_id, std::string bus = "rio",
+  TalonFX_interm(int can_id, std::string bus = "",
       units::millisecond_t max_wait_time = 20_ms);
   /*
   Tick()
diff --git a/src/frc846/include/frc846/control/hardware/simulation/MCSimulator.h b/src/frc846/include/frc846/control/hardware/simulation/MCSimulator.h
index 108b7cb..b21e073 100644
--- a/src/frc846/include/frc846/control/hardware/simulation/MCSimulator.h
+++ b/src/frc846/include/frc846/control/hardware/simulation/MCSimulator.h
@@ -11,7 +11,7 @@
 #include "frc846/control/base/motor_gains.h"
 #include "frc846/control/base/motor_specs.h"
 #include "frc846/control/hardware/IntermediateController.h"
-#include "frc846/wpilib/846_units.h"
+#include "frc846/wpilib/units.h"
 
 namespace frc846::control::simulation {
 
diff --git a/src/frc846/include/frc846/control/hardware/simulation/SIMLEVEL.h b/src/frc846/include/frc846/control/hardware/simulation/SIMLEVEL.h
index c42ef26..04ec905 100644
--- a/src/frc846/include/frc846/control/hardware/simulation/SIMLEVEL.h
+++ b/src/frc846/include/frc846/control/hardware/simulation/SIMLEVEL.h
@@ -5,6 +5,6 @@ namespace frc846::control::simulation {
 #define MOTOR_SIM_LEVEL_SIM_HARDWARE 1
 #define MOTOR_SIM_LEVEL_SIM_PHYSICS 2
 
-#define MOTOR_SIM_LEVEL MOTOR_SIM_LEVEL_SIM_HARDWARE
+#define MOTOR_SIM_LEVEL MOTOR_SIM_LEVEL_SIM_PHYSICS
 
 }  // namespace frc846::control::simulation
\ No newline at end of file
diff --git a/src/frc846/include/frc846/math/collection.h b/src/frc846/include/frc846/math/collection.h
index 6913885..63ba163 100644
--- a/src/frc846/include/frc846/math/collection.h
+++ b/src/frc846/include/frc846/math/collection.h
@@ -21,11 +21,11 @@ double HorizontalDeadband(double input, double x_intercept, double max,
 double VerticalDeadband(double input, double y_intercept, double max,
     double exponent = 1, double sensitivity = 1);
 
-// Returns the smallest difference between two angles
+// Returns the smallest difference between angle to other_angle.
 units::degree_t CoterminalDifference(
     units::degree_t angle, units::degree_t other_angle);
 
-// Returns the smallest sum between two angles.
+// Returns the smallest sum of two angles.
 units::degree_t CoterminalSum(
     units::degree_t angle, units::degree_t other_angle);
 
diff --git a/src/frc846/include/frc846/math/constants.h b/src/frc846/include/frc846/math/constants.h
index da72d7f..49ebb07 100644
--- a/src/frc846/include/frc846/math/constants.h
+++ b/src/frc846/include/frc846/math/constants.h
@@ -9,7 +9,7 @@ struct constants {
     static constexpr units::feet_per_second_squared_t g{32.17405};
   };
   struct geometry {
-    static constexpr double pi = 3.14159265;
+    static constexpr double pi = 3.14159265358979323846;
   };
 };
 
diff --git a/src/frc846/include/frc846/math/fieldpoints.h b/src/frc846/include/frc846/math/fieldpoints.h
index 3239933..59d8aeb 100644
--- a/src/frc846/include/frc846/math/fieldpoints.h
+++ b/src/frc846/include/frc846/math/fieldpoints.h
@@ -12,10 +12,10 @@
 namespace frc846::math {
 
 struct FieldPoint {
-  VectorND<units::foot_t, 2> point;
+  VectorND<units::foot, 2> point;
   units::degree_t bearing;
 
-  VectorND<units::feet_per_second_t, 2> velocity;
+  VectorND<units::feet_per_second, 2> velocity;
 
   // Returns a FieldPoint that is 'mirrored' across the field
   FieldPoint mirror(bool shouldMirror = true) const {
diff --git a/src/frc846/include/frc846/math/vectors.h b/src/frc846/include/frc846/math/vectors.h
index 3dd207a..416c455 100644
--- a/src/frc846/include/frc846/math/vectors.h
+++ b/src/frc846/include/frc846/math/vectors.h
@@ -12,11 +12,13 @@
 #include "frc846/math/collection.h"
 
 namespace frc846::math {
-template <typename T, size_t N> class VectorND {
+template <typename UT, size_t N> class VectorND {
   static_assert(
       N > 0, "VectorND can not be created with less than one dimension.");
-  static_assert(units::traits::is_unit_t<T>(),
-      "VectorND can only be created with unit types.");
+  static_assert(units::traits::is_unit<UT>(),
+      "VectorND can only be created with unit types. Unit_t is invalid.");
+
+  using T = units::unit_t<UT>;
 
 private:
   std::vector<T> data;
@@ -50,86 +52,86 @@ template <typename T, size_t N> class VectorND {
   }
 
   // Copy constructor for VectorND
-  VectorND(const VectorND<T, N>& other) : VectorND() {
+  VectorND(const VectorND<UT, N>& other) : VectorND() {
     for (size_t i = 0; i < N; ++i) {
       data[i] = other[i];
     }
   }
 
-  // Adds VectorND<T, N> to this and returns result
-  VectorND<T, N> operator+(const VectorND<T, N>& other) const {
-    VectorND<T, N> result;
+  // Adds VectorND<UT, N> to this and returns result
+  VectorND<UT, N> operator+(const VectorND<UT, N>& other) const {
+    VectorND<UT, N> result;
     for (size_t i = 0; i < N; ++i) {
       result[i] = data[i] + other[i];
     }
     return result;
   }
 
-  // Subtracts VectorND<T, N> from this and returns result
-  VectorND<T, N> operator-(const VectorND<T, N>& other) const {
-    VectorND<T, N> result;
+  // Subtracts VectorND<UT, N> from this and returns result
+  VectorND<UT, N> operator-(const VectorND<UT, N>& other) const {
+    VectorND<UT, N> result;
     for (size_t i = 0; i < N; ++i) {
       result[i] = data[i] - other[i];
     }
     return result;
   }
 
-  VectorND<T, N> operator*(const double scalar) const {
-    VectorND<T, N> result;
+  VectorND<UT, N> operator*(const double scalar) const {
+    VectorND<UT, N> result;
     for (size_t i = 0; i < N; ++i) {
       result[i] = data[i] * scalar;
     }
     return result;
   }
 
-  friend double operator*(double lhs, const VectorND<T, N>& rhs) {
+  friend double operator*(double lhs, const VectorND<UT, N>& rhs) {
     return rhs * lhs;
   }
 
-  VectorND<T, N> operator/(const double scalar) const {
-    VectorND<T, N> result;
+  VectorND<UT, N> operator/(const double scalar) const {
+    VectorND<UT, N> result;
     for (size_t i = 0; i < N; ++i) {
       result[i] = data[i] / scalar;
     }
     return result;
   }
 
-  VectorND<T, N>& operator+=(const VectorND<T, N>& other) {
+  VectorND<UT, N>& operator+=(const VectorND<UT, N>& other) {
     for (size_t i = 0; i < N; ++i) {
       data[i] += other[i];
     }
     return *this;
   }
 
-  VectorND<T, N>& operator-=(const VectorND<T, N>& other) {
+  VectorND<UT, N>& operator-=(const VectorND<UT, N>& other) {
     for (size_t i = 0; i < N; ++i) {
       data[i] -= other[i];
     }
     return *this;
   }
 
-  VectorND<T, N>& operator*=(const double scalar) {
+  VectorND<UT, N>& operator*=(const double scalar) {
     for (size_t i = 0; i < N; ++i) {
       data[i] *= scalar;
     }
     return *this;
   }
 
-  VectorND<T, N>& operator/=(const double scalar) {
+  VectorND<UT, N>& operator/=(const double scalar) {
     for (size_t i = 0; i < N; ++i) {
       data[i] /= scalar;
     }
     return *this;
   }
 
-  void operator=(const VectorND<T, N>& other) {
+  void operator=(const VectorND<UT, N>& other) {
     for (size_t i = 0; i < N; ++i) {
       data[i] = other[i];
     }
   }
 
   // Uses 'safe' double comparison
-  bool operator==(const VectorND<T, N>& other) const {
+  bool operator==(const VectorND<UT, N>& other) const {
     for (size_t i = 0; i < N; ++i) {
       if (!frc846::math::DEquals(data[i], other[i])) { return false; }
     }
@@ -137,7 +139,7 @@ template <typename T, size_t N> class VectorND {
   }
 
   // Returns a vector rotated by a given angle. Default is clockwise rotation.
-  VectorND<T, N> rotate(units::degree_t angle, bool clockwise = true) const {
+  VectorND<UT, N> rotate(units::degree_t angle, bool clockwise = true) const {
     static_assert(N == 2, "Rotation is only defined for 2D vectors.");
     if (clockwise) { angle = -angle; }
     return {
@@ -146,7 +148,7 @@ template <typename T, size_t N> class VectorND {
   }
 
   // Returns the dot product of this vector and another
-  T dot(const VectorND<T, N>& other) const {
+  T dot(const VectorND<UT, N>& other) const {
     T result = T{};
     for (size_t i = 0; i < N; ++i) {
       result += data[i] * other[i].template to<double>();
@@ -156,7 +158,7 @@ template <typename T, size_t N> class VectorND {
 
   // Returns the cross product of this vector and another
   // Cross product is only defined for 3D vectors
-  VectorND<T, N> cross(const VectorND<T, N>& other) const {
+  VectorND<UT, N> cross(const VectorND<UT, N>& other) const {
     static_assert(N == 3, "Cross product is only defined for 3D vectors.");
     return {data[1] * other[2] - data[2] * other[1],
         data[2] * other[0] - data[0] * other[2],
@@ -173,17 +175,17 @@ template <typename T, size_t N> class VectorND {
   }
 
   // Returns the unit vector of this vector
-  VectorND<T, N> unit() const {
+  VectorND<UT, N> unit() const {
     return *this / magnitude().template to<double>();
   }
 
   // Projects this vector onto another and returns
-  VectorND<T, N> projectOntoAnother(const VectorND<T, N>& other) const {
+  VectorND<UT, N> projectOntoAnother(const VectorND<UT, N>& other) const {
     return other.projectOntoThis(*this);
   }
 
   // Projects another vector onto this and returns
-  VectorND<T, N> projectOntoThis(const VectorND<T, N>& other) const {
+  VectorND<UT, N> projectOntoThis(const VectorND<UT, N>& other) const {
     return unit() * dot(other).template to<double>();
   }
 
@@ -193,12 +195,14 @@ template <typename T, size_t N> class VectorND {
   units::degree_t angle(bool angleIsBearing = false) const {
     assert(N == 2 && "Angle can only be calculated for 2D vectors.");
     if (angleIsBearing) { return units::math::atan2(data[0], data[1]); }
-    return units::math::atan2(data[1], data[0]);
+    try {
+      return units::math::atan2(data[1], data[0]);
+    } catch (std::exception& exc) { return 0_deg; }
   }
 
   // Returns the angle between this vector and another
   units::degree_t angleTo(
-      const VectorND<T, N>& other, bool angleIsBearing = false) const {
+      const VectorND<UT, N>& other, bool angleIsBearing = false) const {
     return other.angle(angleIsBearing) - angle(angleIsBearing);
   }
 
@@ -227,10 +231,10 @@ template <typename T, size_t N> class VectorND {
 // Commonly used vector types
 
 // 1D vector, units::inch_t
-using Vector1D = VectorND<units::inch_t, 1>;
+using Vector1D = VectorND<units::inch, 1>;
 // 2D vector, units::inch_t
-using Vector2D = VectorND<units::inch_t, 2>;
+using Vector2D = VectorND<units::inch, 2>;
 // 3D vector, units::inch_t
-using Vector3D = VectorND<units::inch_t, 3>;
+using Vector3D = VectorND<units::inch, 3>;
 
 }  // namespace frc846::math
\ No newline at end of file
diff --git a/src/frc846/include/frc846/robot/GenericSubsystem.h b/src/frc846/include/frc846/robot/GenericSubsystem.h
index 2fb88c7..302cb14 100644
--- a/src/frc846/include/frc846/robot/GenericSubsystem.h
+++ b/src/frc846/include/frc846/robot/GenericSubsystem.h
@@ -7,13 +7,11 @@
 
 namespace frc846::robot {
 
-#define FRC846_VERIFY(expr, ok, fail_msg)         \
-  do {                                            \
-    if (!(expr)) {                                \
-      ok = false;                                 \
-      Error("Verification failed: {}", fail_msg); \
-    }                                             \
-  } while (0)
+#define FRC846_VERIFY(expr, ok, fail_msg)       \
+  if (!(expr)) {                                \
+    ok = false;                                 \
+    Error("Verification failed: {}", fail_msg); \
+  }
 
 // Non-templated subsystem base class.
 class SubsystemBase : public frc846::base::Loggable {
@@ -57,6 +55,18 @@ class GenericSubsystem : public frc2::SubsystemBase, public SubsystemBase {
     init_ = true;
   }
 
+  /*
+  InitByParent()
+
+  Initializer function to be called by a parent subsystem only. Will not
+  register with WPILib.
+  */
+  void InitByParent() {
+    SetName(name());
+    Log("Initializing subsystem (by parent)");
+    init_ = true;
+  }
+
 private:
   bool init_;
 
diff --git a/src/frc846/include/frc846/robot/calculators/VerticalArmCalculator.h b/src/frc846/include/frc846/robot/calculators/VerticalArmCalculator.h
index 54837a0..38fab9b 100644
--- a/src/frc846/include/frc846/robot/calculators/VerticalArmCalculator.h
+++ b/src/frc846/include/frc846/robot/calculators/VerticalArmCalculator.h
@@ -8,7 +8,7 @@
 #include "frc846/control/base/motor_gains.h"
 #include "frc846/math/calculator.h"
 #include "frc846/math/constants.h"
-#include "frc846/wpilib/846_units.h"
+#include "frc846/wpilib/units.h"
 
 namespace frc846::robot::calculators {
 
diff --git a/src/frc846/include/frc846/robot/swerve/control/swerve_ol_calculator.h b/src/frc846/include/frc846/robot/swerve/control/swerve_ol_calculator.h
new file mode 100644
index 0000000..1be0927
--- /dev/null
+++ b/src/frc846/include/frc846/robot/swerve/control/swerve_ol_calculator.h
@@ -0,0 +1,45 @@
+#pragma once
+
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+#include <units/length.h>
+#include <units/velocity.h>
+
+#include <array>
+
+#include "frc846/math/calculator.h"
+#include "frc846/math/vectors.h"
+
+namespace frc846::robot::swerve::control {
+
+struct SwerveOpenLoopCalculatorConstants {
+  units::inch_t wheelbase_horizontal_dim;
+  units::inch_t wheelbase_forward_dim;
+};
+
+struct SwerveOpenLoopCalculatorInputs {
+  frc846::math::VectorND<units::feet_per_second, 2> translation_target;
+  units::degrees_per_second_t rotation_target;
+  units::feet_per_second_t max_speed;
+};
+
+struct SwerveOpenLoopCalculatorOutput {
+  std::array<units::feet_per_second_t, 4> drive_outputs;
+  std::array<units::degree_t, 4> steer_outputs;
+};
+
+/*
+SwerveOpenLoopCalculator
+
+Calculates the open-loop control targets for each swerve module, given target
+translational and rotational velocities.
+*/
+class SwerveOpenLoopCalculator
+    : public frc846::math::Calculator<SwerveOpenLoopCalculatorInputs,
+          SwerveOpenLoopCalculatorOutput, SwerveOpenLoopCalculatorConstants> {
+public:
+  SwerveOpenLoopCalculatorOutput calculate(
+      SwerveOpenLoopCalculatorInputs inputs) override;
+};
+
+}  // namespace frc846::robot::swerve::odometry
diff --git a/src/frc846/include/frc846/robot/swerve/drivetrain.h b/src/frc846/include/frc846/robot/swerve/drivetrain.h
new file mode 100644
index 0000000..fc43de4
--- /dev/null
+++ b/src/frc846/include/frc846/robot/swerve/drivetrain.h
@@ -0,0 +1,91 @@
+#pragma once
+
+#include <AHRS.h>
+#include <units/acceleration.h>
+#include <units/angular_acceleration.h>
+
+#include "frc846/robot/GenericSubsystem.h"
+#include "frc846/robot/swerve/control/swerve_ol_calculator.h"
+#include "frc846/robot/swerve/odometry/swerve_odometry_calculator.h"
+#include "frc846/robot/swerve/odometry/swerve_pose.h"
+#include "frc846/robot/swerve/swerve_module.h"
+
+namespace frc846::robot::swerve {
+
+enum NavX_connection_type {
+  kSPI,
+  kSerial,
+};
+
+/*
+DrivetrainConfigs
+
+Contains all configs related to the specific drivetrain in use.
+*/
+struct DrivetrainConfigs {
+  NavX_connection_type navX_connection_mode;
+
+  SwerveModuleCommonConfig module_common_config;
+  std::array<SwerveModuleUniqueConfig, 4> module_unique_configs;
+
+  units::inch_t wheelbase_horizontal_dim;
+  units::inch_t wheelbase_forward_dim;
+
+  units::feet_per_second_t max_speed;
+};
+
+struct DrivetrainReadings {
+  frc846::robot::swerve::odometry::SwervePose pose;
+};
+
+// Open-loop control, for use during teleop
+struct DrivetrainOLControlTarget {
+  frc846::math::VectorND<units::feet_per_second, 2> velocity;
+  units::degrees_per_second_t angular_velocity;
+};
+
+// Allows for acceleration-based control of the drivetrain
+struct DrivetrainAccelerationControlTarget {
+  frc846::math::VectorND<units::feet_per_second_squared, 2> linear_acceleration;
+  units::degrees_per_second_squared_t angular_acceleration;
+};
+
+using DrivetrainTarget = std::variant<DrivetrainOLControlTarget,
+    DrivetrainAccelerationControlTarget>;
+
+/*
+DrivetrainSubsystem
+
+A generic class to control a 4-module Kraken x60 swerve drive with CANCoders.
+*/
+class DrivetrainSubsystem
+    : public frc846::robot::GenericSubsystem<DrivetrainReadings,
+          DrivetrainTarget> {
+public:
+  DrivetrainSubsystem(DrivetrainConfigs configs);
+
+  void Setup() override;
+
+  DrivetrainTarget ZeroTarget() const override;
+
+  bool VerifyHardware() override;
+
+  void ZeroBearing();
+
+  void SetCANCoderOffsets();
+
+private:
+  DrivetrainReadings ReadFromHardware() override;
+
+  void WriteToHardware(DrivetrainTarget target) override;
+
+  DrivetrainConfigs configs_;
+  std::array<SwerveModuleSubsystem*, 4> modules_;
+
+  AHRS navX_;
+
+  frc846::robot::swerve::odometry::SwerveOdometryCalculator odometry_;
+  frc846::robot::swerve::control::SwerveOpenLoopCalculator ol_calculator_;
+};
+
+}  // namespace frc846::robot::swerve
\ No newline at end of file
diff --git a/src/frc846/include/frc846/robot/swerve/odometry/swerve_odometry_calculator.h b/src/frc846/include/frc846/robot/swerve/odometry/swerve_odometry_calculator.h
new file mode 100644
index 0000000..52b36ed
--- /dev/null
+++ b/src/frc846/include/frc846/robot/swerve/odometry/swerve_odometry_calculator.h
@@ -0,0 +1,39 @@
+#pragma once
+
+#include <units/angle.h>
+#include <units/length.h>
+
+#include <array>
+
+#include "frc846/math/calculator.h"
+#include "frc846/math/vectors.h"
+
+namespace frc846::robot::swerve::odometry {
+
+struct SwerveOdometryConstants {};
+
+struct SwerveOdometryInputs {
+  units::degree_t bearing;
+  std::array<units::degree_t, 4> steer_pos;
+  frc846::math::VectorND<units::inch, 4> drive_pos;
+};
+
+struct SwerveOdometryOutput {
+  frc846::math::Vector2D position;
+};
+
+class SwerveOdometryCalculator
+    : public frc846::math::Calculator<SwerveOdometryInputs,
+          SwerveOdometryOutput, SwerveOdometryConstants> {
+public:
+  SwerveOdometryCalculator();
+
+  SwerveOdometryOutput calculate(SwerveOdometryInputs inputs) override;
+
+private:
+  frc846::math::VectorND<units::inch, 4> previous_module_positions_;
+
+  frc846::math::Vector2D last_position_;
+};
+
+}  // namespace frc846::robot::swerve::odometry
diff --git a/src/frc846/include/frc846/robot/swerve/odometry/swerve_pose.h b/src/frc846/include/frc846/robot/swerve/odometry/swerve_pose.h
new file mode 100644
index 0000000..1a5318a
--- /dev/null
+++ b/src/frc846/include/frc846/robot/swerve/odometry/swerve_pose.h
@@ -0,0 +1,34 @@
+#pragma once
+
+#include "frc846/math/vectors.h"
+
+namespace frc846::robot::swerve::odometry {
+
+/*
+SwervePose
+
+A class representing the pose of a swerve drive robot. Contains Vector2D (check
+math/vectors.h) position and velocity, and a bearing (degrees).
+*/
+struct SwervePose {
+  frc846::math::Vector2D position;
+  units::degree_t bearing;
+  frc846::math::VectorND<units::feet_per_second, 2> velocity;
+
+  SwervePose rotate(units::degree_t angle) const;
+  SwervePose translate(frc846::math::Vector2D translation) const;
+
+  /*
+  Extrapolates the pose of the robot by the given time, knowing the velocity.
+  Can be used for latency compensation.
+
+  @note Can be innacurate with large time intervals, especially if the robot is
+  moving at a non-constant velocity.
+  */
+  SwervePose extrapolate(units::second_t time) const;
+
+  SwervePose operator+(const SwervePose& other) const;
+  SwervePose operator-(const SwervePose& other) const;
+};
+
+}  // namespace frc846::robot::swerve::odometry
\ No newline at end of file
diff --git a/src/frc846/include/frc846/robot/swerve/swerve_module.h b/src/frc846/include/frc846/robot/swerve/swerve_module.h
new file mode 100644
index 0000000..1125343
--- /dev/null
+++ b/src/frc846/include/frc846/robot/swerve/swerve_module.h
@@ -0,0 +1,139 @@
+#pragma once
+
+#include <units/angle.h>
+#include <units/base.h>
+#include <units/torque.h>
+#include <units/velocity.h>
+
+#include <ctre/phoenix6/CANcoder.hpp>
+#include <variant>
+
+#include "frc846/control/HMCHelper.h"
+#include "frc846/control/HigherMotorController.h"
+#include "frc846/robot/GenericSubsystem.h"
+
+namespace frc846::robot::swerve {
+
+struct SwerveModuleReadings {
+  units::feet_per_second_t vel;
+  units::foot_t drive_pos;
+  units::degree_t steer_pos;
+};
+
+// Allows for torque-based control by DrivetrainSubsystem
+struct SwerveModuleTorqueControlTarget {
+  units::newton_meter_t drive;
+  units::newton_meter_t steer;
+};
+
+// For open-loop control by DrivetrainSubsystem
+struct SwerveModuleOLControlTarget {
+  units::feet_per_second_t drive;
+  units::degree_t steer;
+};
+
+using SwerveModuleTarget =
+    std::variant<SwerveModuleTorqueControlTarget, SwerveModuleOLControlTarget>;
+
+struct SwerveModuleUniqueConfig {
+  std::string loc;
+
+  int cancoder_id;
+  int drive_id;
+  int steer_id;
+
+  frc846::wpilib::unit_ohm circuit_resistance;
+};
+
+using steer_conv_unit = units::dimensionless::scalar_t;
+using drive_conv_unit = units::unit_t<
+    units::compound_unit<units::foot, units::inverse<units::turn>>>;
+
+struct SwerveModuleCommonConfig {
+  frc846::control::config::MotorConstructionParameters drive_params;
+  frc846::control::config::MotorConstructionParameters steer_params;
+
+  frc846::control::base::MotorMonkeyType motor_types;
+  steer_conv_unit steer_reduction;
+  drive_conv_unit drive_reduction;
+
+  std::string bus = "";
+};
+
+/*
+SwerveModuleSubsystem
+
+A class representing a single swerve module. Controls a drive and steer motor
+and a CANcoder. Meant to be constructed as a child subsystem of
+DrivetrainSubsystem.
+*/
+class SwerveModuleSubsystem
+    : public frc846::robot::GenericSubsystem<SwerveModuleReadings,
+          SwerveModuleTarget> {
+public:
+  /*
+  SwerveModuleSubsystem()
+
+  Constructs a SwerveModuleSubsystem object with the given parameters. For use
+  by DrivetrainSubsystem.
+  */
+  SwerveModuleSubsystem(Loggable& parent,
+      SwerveModuleUniqueConfig unique_config,
+      SwerveModuleCommonConfig common_config);
+
+  void Setup() override;
+
+  SwerveModuleTarget ZeroTarget() const override;
+
+  bool VerifyHardware() override;
+
+  void SetCANCoderOffset();
+  void SetCANCoderOffset(units::degree_t offset);
+
+  void ZeroWithCANcoder();
+
+  /*
+  SetSteerGains()
+
+  Sets the gains for the steer motor controller. Should be called after
+  SwerveModuleSubsystem Setup, in DrivetrainSubsystem Setup.
+  */
+  void SetSteerGains(frc846::control::base::MotorGains gains);
+
+private:
+  /*
+  getMotorParams()
+
+  Static helper function modifies the drive and steer motor parameters provided
+  in the common configuration using the unique configuration.
+  */
+  static std::pair<frc846::control::config::MotorConstructionParameters,
+      frc846::control::config::MotorConstructionParameters>
+  getMotorParams(SwerveModuleUniqueConfig unique_config,
+      SwerveModuleCommonConfig common_config);
+
+  SwerveModuleReadings ReadFromHardware() override;
+
+  void WriteToHardware(SwerveModuleTarget target) override;
+
+  /*
+  calculateSteerPosition()
+
+  Calculates the direction for the steer motor, based on a normalized target.
+  Also returns a boolean that represents the inversion of the drive motor.
+  */
+  std::pair<units::degree_t, bool> calculateSteerPosition(
+      units::degree_t target_norm, units::degree_t current);
+
+  frc846::control::HigherMotorController drive_;
+  frc846::control::HigherMotorController steer_;
+
+  frc846::control::HMCHelper<units::foot> drive_helper_;
+  frc846::control::HMCHelper<units::degree> steer_helper_;
+
+  ctre::phoenix6::hardware::CANcoder cancoder_;
+
+  units::feet_per_second_t max_speed_;
+};
+
+}  // namespace frc846::robot::swerve
\ No newline at end of file
diff --git a/src/frc846/include/frc846/wpilib/846_units.h b/src/frc846/include/frc846/wpilib/units.h
similarity index 100%
rename from src/frc846/include/frc846/wpilib/846_units.h
rename to src/frc846/include/frc846/wpilib/units.h
diff --git a/src/frc846/include/frc846/wpilib/util.h b/src/frc846/include/frc846/wpilib/util.h
index 0c90856..50f8fcd 100644
--- a/src/frc846/include/frc846/wpilib/util.h
+++ b/src/frc846/include/frc846/wpilib/util.h
@@ -1,5 +1,5 @@
-#include <vector>
 #include <algorithm>
+#include <vector>
 
 namespace frc846::wpilib {
 
diff --git a/src/y2025/cpp/FunkyRobot.cc b/src/y2025/cpp/FunkyRobot.cc
index 079a629..a49574b 100644
--- a/src/y2025/cpp/FunkyRobot.cc
+++ b/src/y2025/cpp/FunkyRobot.cc
@@ -10,7 +10,7 @@
 #include <frc2/command/button/Trigger.h>
 #include <hal/Notifier.h>
 
-// #include "commands/teleop/drive_command.h"
+#include "commands/teleop/drive_command.h"
 #include "commands/teleop/leds_command.h"
 #include "control_triggers.h"
 #include "field.h"
@@ -32,13 +32,12 @@ void FunkyRobot::OnInitialize() {
   // }
 
   // // Add dashboard buttons
-  // frc::SmartDashboard::PutData(
-  //     "zero_modules", new frc846::ntinf::NTAction(
-  //                         [this] { container_.drivetrain_.ZeroModules(); }));
-  // frc::SmartDashboard::PutData("zero_bearing",
-  //                              new frc846::ntinf::NTAction([this] {
-  //                                container_.drivetrain_.SetBearing(0_deg);
-  //                              }));
+  frc::SmartDashboard::PutData("set_cancoder_offsets",
+      new frc846::wpilib::NTAction(
+          [this] { container_.drivetrain_.SetCANCoderOffsets(); }));
+  frc::SmartDashboard::PutData(
+      "zero_bearing", new frc846::wpilib::NTAction(
+                          [this] { container_.drivetrain_.ZeroBearing(); }));
 
   // frc::SmartDashboard::PutData(
   //     "zero_odometry", new frc846::ntinf::NTAction(
@@ -51,7 +50,7 @@ void FunkyRobot::OnDisable() {
 }
 
 void FunkyRobot::InitTeleop() {
-  // container_.drivetrain_.SetDefaultCommand(DriveCommand{container_});
+  container_.drivetrain_.SetDefaultCommand(DriveCommand{container_});
   container_.leds_.SetDefaultCommand(LEDsCommand{container_});
 
   ControlTriggerInitializer::InitTeleopTriggers(container_);
@@ -63,7 +62,7 @@ void FunkyRobot::InitTest() {}
 
 #ifndef RUNNING_FRC_TESTS
 int main() {
-  // configureSignalHandlers();
+  if (frc::RobotBase::IsSimulation()) configureSignalHandlers();
   return frc::StartRobot<FunkyRobot>();
 }
 #endif
\ No newline at end of file
diff --git a/src/y2025/cpp/commands/teleop/drive_command.cc b/src/y2025/cpp/commands/teleop/drive_command.cc
new file mode 100644
index 0000000..980ff7f
--- /dev/null
+++ b/src/y2025/cpp/commands/teleop/drive_command.cc
@@ -0,0 +1,56 @@
+#include "commands/teleop/drive_command.h"
+
+DriveCommand::DriveCommand(RobotContainer &container)
+    : frc846::robot::GenericCommand<RobotContainer, DriveCommand>{
+          container, "drive_command"} {
+  AddRequirements({&container_.drivetrain_});
+}
+
+void DriveCommand::OnInit() {}
+
+void DriveCommand::Periodic() {
+  ControlInputReadings ci_readings_{container_.control_input_.GetReadings()};
+
+  frc846::robot::swerve::DrivetrainOLControlTarget target{};
+
+  container_.drivetrain_.SetTarget({target});
+
+  double translate_x = frc846::math::HorizontalDeadband(
+      container_.control_input_.GetReadings().translate_x,
+      container_.control_input_.GetPreferenceValue_double(
+          "translation_deadband"),
+      1,
+      container_.control_input_.GetPreferenceValue_int("translation_exponent"),
+      1);
+
+  double translate_y = frc846::math::HorizontalDeadband(
+      container_.control_input_.GetReadings().translate_y,
+      container_.control_input_.GetPreferenceValue_double(
+          "translation_deadband"),
+      1,
+      container_.control_input_.GetPreferenceValue_int("translation_exponent"),
+      1);
+
+  double rotation = frc846::math::HorizontalDeadband(
+      container_.control_input_.GetReadings().rotation,
+      container_.control_input_.GetPreferenceValue_double("rotation_deadband"),
+      1, container_.control_input_.GetPreferenceValue_int("rotation_exponent"),
+      1);
+
+  units::feet_per_second_t max_speed =
+      container_.drivetrain_
+          .GetPreferenceValue_unit_type<units::feet_per_second_t>("max_speed");
+  units::degrees_per_second_t max_omega =
+      container_.drivetrain_
+          .GetPreferenceValue_unit_type<units::degrees_per_second_t>(
+              "max_omega");
+
+  target.velocity = {translate_x * max_speed, translate_y * max_speed};
+  target.angular_velocity = rotation * max_omega;
+
+  container_.drivetrain_.SetTarget({target});
+}
+
+void DriveCommand::OnEnd(bool interrupted) {}
+
+bool DriveCommand::IsFinished() { return false; }
\ No newline at end of file
diff --git a/src/y2025/cpp/commands/teleop/leds_command.cc b/src/y2025/cpp/commands/teleop/leds_command.cc
index 6b0e799..4b8b908 100644
--- a/src/y2025/cpp/commands/teleop/leds_command.cc
+++ b/src/y2025/cpp/commands/teleop/leds_command.cc
@@ -9,8 +9,6 @@ LEDsCommand::LEDsCommand(RobotContainer &container)
 void LEDsCommand::OnInit() {}
 
 void LEDsCommand::Periodic() {
-  ControlInputReadings ci_readings_{container_.control_input_.GetReadings()};
-
   LEDsState lstate{};
 
   container_.leds_.SetTarget({lstate});
diff --git a/src/y2025/cpp/control_triggers.cc b/src/y2025/cpp/control_triggers.cc
index 3375e2b..8ad3c48 100644
--- a/src/y2025/cpp/control_triggers.cc
+++ b/src/y2025/cpp/control_triggers.cc
@@ -5,13 +5,11 @@
 #include <frc2/command/button/Trigger.h>
 
 void ControlTriggerInitializer::InitTeleopTriggers(RobotContainer& container) {
-  // frc2::Trigger drivetrain_zero_bearing_trigger{
-  //     [&] { return container.control_input_.GetReadings().zero_bearing; }};
+  frc2::Trigger drivetrain_zero_bearing_trigger{[&] {
+    return container.control_input_.GetReadings().zero_bearing;
+  }};
 
-  // drivetrain_zero_bearing_trigger.WhileTrue(
-  //     frc2::InstantCommand([&] {
-  //       container.drivetrain_.SetBearing(
-  //           frc846::util::ShareTables::GetBoolean("is_red_side") ? 0_deg
-  //                                                                : 180_deg);
-  //     }).ToPtr());
+  drivetrain_zero_bearing_trigger.WhileTrue(frc2::InstantCommand([&] {
+    container.drivetrain_.ZeroBearing();
+  }).ToPtr());
 }
\ No newline at end of file
diff --git a/src/y2025/cpp/subsystems/abstract/control_input.cc b/src/y2025/cpp/subsystems/abstract/control_input.cc
index 5665669..87e6c4b 100644
--- a/src/y2025/cpp/subsystems/abstract/control_input.cc
+++ b/src/y2025/cpp/subsystems/abstract/control_input.cc
@@ -6,6 +6,12 @@ ControlInputSubsystem::ControlInputSubsystem()
 
 void ControlInputSubsystem::Setup() {
   RegisterPreference("trigger_threshold", 0.3);
+
+  RegisterPreference("translation_deadband", 0.07);
+  RegisterPreference("translation_exponent", 2);
+
+  RegisterPreference("rotation_deadband", 0.07);
+  RegisterPreference("rotation_exponent", 2);
 }
 
 ControlInputTarget ControlInputSubsystem::ZeroTarget() const {
diff --git a/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc b/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc
new file mode 100644
index 0000000..6e4b910
--- /dev/null
+++ b/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc
@@ -0,0 +1,134 @@
+#include "subsystems/hardware/DrivetrainConstructor.h"
+
+#include "frc846/control/calculators/CircuitResistanceCalculator.h"
+#include "frc846/math/constants.h"
+#include "ports.h"
+
+DrivetrainConstructor::DrivetrainConstructor()
+    : Loggable{"DrivetrainConstructor"} {
+  RegisterPreference("drive_motor_current_limit", 120_A);
+  RegisterPreference("steer_motor_current_limit", 120_A);
+  RegisterPreference("drive_motor_smart_current_limit", 80_A);
+  RegisterPreference("steer_motor_smart_current_limit", 80_A);
+
+  RegisterPreference("drive_motor_voltage_compensation", 16_V);
+  RegisterPreference("steer_motor_voltage_compensation", 12_V);
+}
+
+frc846::robot::swerve::DrivetrainConfigs
+DrivetrainConstructor::getDrivetrainConfigs() {
+  frc846::robot::swerve::DrivetrainConfigs configs;
+
+  /*
+  START SETTABLES
+  TODO: Set these values during season
+  */
+
+  configs.navX_connection_mode =
+      frc846::robot::swerve::NavX_connection_type::kSerial;
+
+  units::inch_t wheel_diameter = 4_in;
+
+  units::inch_t wire_length_FL = 14_in;
+  units::inch_t wire_length_FR = 14_in;
+  units::inch_t wire_length_BL = 14_in;
+  units::inch_t wire_length_BR = 14_in;
+
+  unsigned int num_connectors_FR = 3;
+  unsigned int num_connectors_FL = 3;
+  unsigned int num_connectors_BL = 3;
+  unsigned int num_connectors_BR = 3;
+
+  frc846::robot::swerve::drive_conv_unit drive_reduction =
+      (frc846::math::constants::geometry::pi * wheel_diameter) / 6.12_tr;
+  frc846::robot::swerve::steer_conv_unit steer_reduction = 7_tr / 150_tr;
+
+  configs.wheelbase_forward_dim = 26_in;
+  configs.wheelbase_horizontal_dim = 26_in;
+
+  units::pound_t wheel_approx_weight = 2_lb;
+  units::inch_t wheel_weight_radius = 1_in;
+
+  units::pound_t robot_weight = 120_lb;
+
+  // (Mass wheel) * (wheel_r)^2 * (steer reduction)^2
+  frc846::wpilib::unit_kg_m_sq relative_steer_inertia{
+      wheel_approx_weight * (wheel_weight_radius * wheel_weight_radius) *
+      (steer_reduction * steer_reduction).to<double>()};
+
+  // (Mass robot) * [(wheel_d)/2]^2 * (drive reduction)^2
+  frc846::wpilib::unit_kg_m_sq relative_drive_inertia{
+      robot_weight * (wheel_diameter * wheel_diameter) *
+      (drive_reduction * drive_reduction).to<double>()};
+
+  /* END SETTABLES */
+
+  frc846::wpilib::unit_ohm wire_resistance_FR{
+      frc846::control::calculators::CircuitResistanceCalculator::calculate(
+          wire_length_FR, frc846::control::calculators::fourteen_gauge,
+          num_connectors_FR)};
+  frc846::wpilib::unit_ohm wire_resistance_FL{
+      frc846::control::calculators::CircuitResistanceCalculator::calculate(
+          wire_length_FL, frc846::control::calculators::fourteen_gauge,
+          num_connectors_FL)};
+
+  frc846::wpilib::unit_ohm wire_resistance_BL{
+      frc846::control::calculators::CircuitResistanceCalculator::calculate(
+          wire_length_BL, frc846::control::calculators::fourteen_gauge,
+          num_connectors_BL)};
+  frc846::wpilib::unit_ohm wire_resistance_BR{
+      frc846::control::calculators::CircuitResistanceCalculator::calculate(
+          wire_length_BR, frc846::control::calculators::fourteen_gauge,
+          num_connectors_BR)};
+
+  frc846::robot::swerve::SwerveModuleUniqueConfig FR_config{"FR",
+      ports::drivetrain_::kFRDrive_CANID, ports::drivetrain_::kFRSteer_CANID,
+      ports::drivetrain_::kFRCANCoder_CANID, wire_resistance_FR};
+  frc846::robot::swerve::SwerveModuleUniqueConfig FL_config{"FL",
+      ports::drivetrain_::kFLDrive_CANID, ports::drivetrain_::kFLSteer_CANID,
+      ports::drivetrain_::kFLCANCoder_CANID, wire_resistance_FL};
+  frc846::robot::swerve::SwerveModuleUniqueConfig BL_config{"BL",
+      ports::drivetrain_::kBLDrive_CANID, ports::drivetrain_::kBLSteer_CANID,
+      ports::drivetrain_::kBLCANCoder_CANID, wire_resistance_BL};
+  frc846::robot::swerve::SwerveModuleUniqueConfig BR_config{"BR",
+      ports::drivetrain_::kBRDrive_CANID, ports::drivetrain_::kBRSteer_CANID,
+      ports::drivetrain_::kBRCANCoder_CANID, wire_resistance_BR};
+
+  frc846::control::config::MotorConstructionParameters drive_params{
+      .can_id = 999,  // overriden by unique config
+      .inverted = false,
+      .brake_mode = true,
+      .motor_current_limit = GetPreferenceValue_unit_type<units::ampere_t>(
+          "drive_motor_current_limit"),
+      .smart_current_limit = GetPreferenceValue_unit_type<units::ampere_t>(
+          "drive_motor_smart_current_limit"),
+      .voltage_compensation = GetPreferenceValue_unit_type<units::volt_t>(
+          "drive_motor_voltage_compensation"),
+      .circuit_resistance = 999_Ohm,  // overriden by unique config
+      .rotational_inertia = relative_drive_inertia,
+      .bus = "",
+  };
+  frc846::control::config::MotorConstructionParameters steer_params{
+      .can_id = 999,  // overriden by unique config
+      .inverted = false,
+      .brake_mode = false,
+      .motor_current_limit = GetPreferenceValue_unit_type<units::ampere_t>(
+          "steer_motor_current_limit"),
+      .smart_current_limit = GetPreferenceValue_unit_type<units::ampere_t>(
+          "steer_motor_smart_current_limit"),
+      .voltage_compensation = GetPreferenceValue_unit_type<units::volt_t>(
+          "steer_motor_voltage_compensation"),
+      .circuit_resistance = 999_Ohm,  // overriden by unique config
+      .rotational_inertia = relative_steer_inertia,
+      .bus = "",
+  };
+
+  configs.module_common_config =
+      frc846::robot::swerve::SwerveModuleCommonConfig{drive_params,
+          steer_params,
+          frc846::control::base::MotorMonkeyType::TALON_FX_KRAKENX60,
+          steer_reduction, drive_reduction, ""};
+  configs.module_unique_configs = {FR_config, FL_config, BR_config, BL_config};
+
+  return configs;
+}
\ No newline at end of file
diff --git a/src/y2025/include/commands/teleop/drive_command.h b/src/y2025/include/commands/teleop/drive_command.h
new file mode 100644
index 0000000..432354f
--- /dev/null
+++ b/src/y2025/include/commands/teleop/drive_command.h
@@ -0,0 +1,18 @@
+#pragma once
+
+#include "frc846/robot/GenericCommand.h"
+#include "subsystems/robot_container.h"
+
+class DriveCommand
+    : public frc846::robot::GenericCommand<RobotContainer, DriveCommand> {
+public:
+  DriveCommand(RobotContainer& container);
+
+  void OnInit() override;
+
+  void Periodic() override;
+
+  void OnEnd(bool interrupted) override;
+
+  bool IsFinished() override;
+};
\ No newline at end of file
diff --git a/src/y2025/include/rsighandler.h b/src/y2025/include/rsighandler.h
index 77d82f1..b421818 100644
--- a/src/y2025/include/rsighandler.h
+++ b/src/y2025/include/rsighandler.h
@@ -45,7 +45,7 @@ void handler(int sig) {
   } else {
     std::cerr << "? Unknown Exception Occured" << std::endl;
   }
-  // exit(1);
+  exit(1);
 }
 #endif
 
diff --git a/src/y2025/include/subsystems/hardware/DrivetrainConstructor.h b/src/y2025/include/subsystems/hardware/DrivetrainConstructor.h
new file mode 100644
index 0000000..75c98a7
--- /dev/null
+++ b/src/y2025/include/subsystems/hardware/DrivetrainConstructor.h
@@ -0,0 +1,17 @@
+#pragma once
+
+#include "frc846/base/Loggable.h"
+#include "frc846/robot/swerve/drivetrain.h"
+
+/*
+DrivetrainConstructor
+
+A class providing methods to aid construction of a DrivetrainSubsystem
+object.
+*/
+class DrivetrainConstructor : public frc846::base::Loggable {
+public:
+  DrivetrainConstructor();
+
+  frc846::robot::swerve::DrivetrainConfigs getDrivetrainConfigs();
+};
\ No newline at end of file
diff --git a/src/y2025/include/subsystems/robot_container.h b/src/y2025/include/subsystems/robot_container.h
index e05a7c5..5a96742 100644
--- a/src/y2025/include/subsystems/robot_container.h
+++ b/src/y2025/include/subsystems/robot_container.h
@@ -2,6 +2,7 @@
 
 #include "frc846/robot/GenericRobotContainer.h"
 #include "subsystems/abstract/control_input.h"
+#include "subsystems/hardware/DrivetrainConstructor.h"
 #include "subsystems/hardware/leds.h"
 
 class RobotContainer : public frc846::robot::GenericRobotContainer {
@@ -9,13 +10,18 @@ class RobotContainer : public frc846::robot::GenericRobotContainer {
   ControlInputSubsystem control_input_{};
   LEDsSubsystem leds_{};
 
+  DrivetrainConstructor drivetrain_constructor_{};
+  frc846::robot::swerve::DrivetrainSubsystem drivetrain_{
+      drivetrain_constructor_.getDrivetrainConfigs()};
+
   RobotContainer() {
     RegisterPreference("init_drivetrain", true);
     RegisterPreference("init_leds", true);
 
     control_input_.Init();
+    if (GetPreferenceValue_bool("init_drivetrain")) drivetrain_.Init();
     if (GetPreferenceValue_bool("init_leds")) leds_.Init();
 
-    RegisterSubsystems({&control_input_, &leds_});
+    RegisterSubsystems({&control_input_, &drivetrain_, &leds_});
   }
 };
diff --git a/style.standard b/style.standard
index 13b525c..77b0c6b 100644
--- a/style.standard
+++ b/style.standard
@@ -1 +1 @@
-{   BasedOnStyle: Google,   AccessModifierOffset: -2,   AlignAfterOpenBracket: DontAlign,   AlignConsecutiveAssignments: false,   AlignConsecutiveDeclarations: false,   AlignEscapedNewlines: Left,   AlignOperands: true,   AlignTrailingComments: true,   AllowAllParametersOfDeclarationOnNextLine: true,   AllowShortBlocksOnASingleLine: true,   AllowShortCaseLabelsOnASingleLine: true,   AllowShortEnumsOnASingleLine: true,   AllowShortFunctionsOnASingleLine: true,   AllowShortIfStatementsOnASingleLine: true,   AllowShortLambdasOnASingleLine: Inline,   AllowShortLoopsOnASingleLine: false,   AlwaysBreakAfterReturnType: None,   AlwaysBreakBeforeMultilineStrings: false,   AlwaysBreakTemplateDeclarations: false,   BinPackArguments: true,   BinPackParameters: true,   BreakAfterJavaFieldAnnotations: false,   BreakBeforeBinaryOperators: None,   BreakBeforeBraces: Attach,   BreakBeforeConceptDeclarations: false,   BreakBeforeTernaryOperators: true,   BreakConstructorInitializersBeforeComma: false,   ColumnLimit: 80,   CompactNamespaces: false,   ConstructorInitializerAllOnOneLineOrOnePerLine: true,   ConstructorInitializerIndentWidth: 4,   ContinuationIndentWidth: 4,   Cpp11BracedListStyle: true,   DeriveLineEnding: true,   DerivePointerAlignment: true,   DisableFormat: false,   FixNamespaceComments: false,   IndentAccessModifiers: false,   IndentCaseLabels: false,   IndentExternBlock: AfterExternBlock,   IndentPPDirectives: None,   IndentWidth: 2,   InsertTrailingCommas: None,   KeepEmptyLinesAtTheStartOfBlocks: false,   MacroBlockBegin: "",   MacroBlockEnd: "",   MaxEmptyLinesToKeep: 1,   NamespaceIndentation: None,   PenaltyBreakAssignment: 2,   PenaltyBreakBeforeFirstCallParameter: 19,   PenaltyBreakComment: 300,   PenaltyBreakFirstLessLess: 120,   PenaltyBreakString: 1000,   PenaltyExcessCharacter: 1000000,   PenaltyReturnTypeOnItsOwnLine: 60,   PointerAlignment: Left,   ReflowComments: true,   SortIncludes: false,   SortUsingDeclarations: true,   SpaceAfterCStyleCast: false,   SpaceAfterLogicalNot: false,   SpaceAfterTemplateKeyword: true,   SpaceAroundPointerQualifiers: Default,   SpaceBeforeAssignmentOperators: true,   SpaceBeforeCaseColon: false,   SpaceBeforeCpp11BracedList: false,   SpaceBeforeCtorInitializerColon: true,   SpaceBeforeInheritanceColon: true,   SpaceBeforeParens: ControlStatements,   SpaceBeforeRangeBasedForLoopColon: true,   SpaceBeforeSquareBrackets: false,   SpaceInEmptyBlock: false,   SpaceInEmptyParentheses: false,   SpacesBeforeTrailingComments: 2,   SpacesInAngles: false,   SpacesInCStyleCastParentheses: false,   SpacesInContainerLiterals: false,   SpacesInParentheses: false,   SpacesInSquareBrackets: false,   Standard: Auto,   TabWidth: 2,   UseCRLF: false,   UseTab: Never }
\ No newline at end of file
+{   BasedOnStyle: Google,   AccessModifierOffset: -2,   AlignAfterOpenBracket: DontAlign,   AlignConsecutiveAssignments: false,   AlignConsecutiveDeclarations: false,   AlignEscapedNewlines: Left,   AlignOperands: true,   AlignTrailingComments: true,   AllowAllParametersOfDeclarationOnNextLine: true,   AllowShortBlocksOnASingleLine: true,   AllowShortCaseLabelsOnASingleLine: true,   AllowShortEnumsOnASingleLine: true,   AllowShortFunctionsOnASingleLine: true,   AllowShortIfStatementsOnASingleLine: true,   AllowShortLambdasOnASingleLine: Inline,   AllowShortLoopsOnASingleLine: false,   AlwaysBreakAfterReturnType: None,   AlwaysBreakBeforeMultilineStrings: false,   AlwaysBreakTemplateDeclarations: false,   BinPackArguments: true,   BinPackParameters: true,   BreakAfterJavaFieldAnnotations: false,   BreakBeforeBinaryOperators: None,   BreakBeforeBraces: Attach,   BreakBeforeConceptDeclarations: false,   BreakBeforeTernaryOperators: true,   BreakConstructorInitializersBeforeComma: false,   ColumnLimit: 80,   CompactNamespaces: false,   ConstructorInitializerAllOnOneLineOrOnePerLine: true,   ConstructorInitializerIndentWidth: 4,   ContinuationIndentWidth: 4,   Cpp11BracedListStyle: true,   DeriveLineEnding: true,   DerivePointerAlignment: true,   DisableFormat: false,   FixNamespaceComments: false,   IndentAccessModifiers: false,   IndentCaseLabels: false,   IndentExternBlock: AfterExternBlock,   IndentPPDirectives: None,   IndentWidth: 2,   InsertTrailingCommas: None,   KeepEmptyLinesAtTheStartOfBlocks: false,   MacroBlockBegin: "",   MacroBlockEnd: "",   MaxEmptyLinesToKeep: 1,   NamespaceIndentation: None,   PenaltyBreakAssignment: 2,   PenaltyBreakBeforeFirstCallParameter: 19,   PenaltyBreakComment: 300,   PenaltyBreakFirstLessLess: 120,   PenaltyBreakString: 1000,   PenaltyExcessCharacter: 1000000,   PenaltyReturnTypeOnItsOwnLine: 60,   PointerAlignment: Left,   ReflowComments: true,   SortIncludes: true,   SortUsingDeclarations: true,   SpaceAfterCStyleCast: false,   SpaceAfterLogicalNot: false,   SpaceAfterTemplateKeyword: true,   SpaceAroundPointerQualifiers: Default,   SpaceBeforeAssignmentOperators: true,   SpaceBeforeCaseColon: false,   SpaceBeforeCpp11BracedList: false,   SpaceBeforeCtorInitializerColon: true,   SpaceBeforeInheritanceColon: true,   SpaceBeforeParens: ControlStatements,   SpaceBeforeRangeBasedForLoopColon: true,   SpaceBeforeSquareBrackets: false,   SpaceInEmptyBlock: false,   SpaceInEmptyParentheses: false,   SpacesBeforeTrailingComments: 2,   SpacesInAngles: false,   SpacesInCStyleCastParentheses: false,   SpacesInContainerLiterals: false,   SpacesInParentheses: false,   SpacesInSquareBrackets: false,   Standard: Auto,   TabWidth: 2,   UseCRLF: false,   UseTab: Never   }
\ No newline at end of file