diff --git a/.vscode/settings.json b/.vscode/settings.json
index 8fa1620..c5f4d52 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -114,7 +114,10 @@
     "xtr1common": "cpp",
     "xtree": "cpp",
     "xutility": "cpp",
-    "queue": "cpp"
+    "queue": "cpp",
+    "expected": "cpp",
+    "source_location": "cpp",
+    "version": "cpp"
   },
   "wpilib.useWindbgX": true,
   "C_Cpp.errorSquiggles": "enabled",
diff --git a/README.md b/README.md
index ac52f3e..967dd78 100644
--- a/README.md
+++ b/README.md
@@ -43,8 +43,8 @@ Commands set targets to these subsystems. Some commands are like actions, such a
 
 #### WPILib Tools
 
-- Download WPILib tools and WPILib VSCode. Attached is the link for version 2025.1.1.
-  - [2025.1.1 WINDOWS](https://packages.wpilib.workers.dev/installer/v2025.1.1/Win64/WPILib_Windows-2025.1.1.iso)
+- Download WPILib tools and WPILib VSCode. Attached is the link for version 2025.2.1.
+  - [2025.2.1 WINDOWS](https://packages.wpilib.workers.dev/installer/v2025.2.1/Win64/WPILib_Windows-2025.2.1.iso)
 - Once downloaded, double click on the file. By default, it will be saved in the downloads folder.
 - Run the .exe file inside of it. It should be called "WPILib Installer".
 - Go through the default setup process.
@@ -106,7 +106,7 @@ Select x64 or arm depending on your chip. If using M1/M2/M3 mac, select arm.
 
 Ignore the LLVM section.
 
-- [WPILib 2025.1.1](https://github.com/wpilibsuite/allwpilib/releases/tag/v2025.1.1).
+- [WPILib 2025.2.1](https://github.com/wpilibsuite/allwpilib/releases/tag/v2025.2.1).
 - [git-scm](https://git-scm.com/download/mac).
 - Clang-format: `brew install clang-format`
 - CppCheck: `brew install cppcheck`
@@ -173,9 +173,6 @@ To undo the going back:
 
 ## CppCheck Warnings
 ```
-src/frc846/cpp/frc846/control/calculators/CurrentTorqueCalculator.cc:49:30: warning: Variable 'duty_cycle_original' is assigned a value that is never used. [unreadVariable]
-src/frc846/cpp/frc846/robot/swerve/drivetrain.cc:160: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/build.gradle b/build.gradle
index c6074be..1baf879 100644
--- a/build.gradle
+++ b/build.gradle
@@ -135,7 +135,7 @@ spotless {
 }
 
 if (!project.hasProperty('fromCI')) {
-    check.dependsOn spotlessApply
+    spotlessApply
 }
 spotlessCppCheck.onlyIf { !project.hasProperty('fromCI') || project.hasProperty('runningSpotlessCpp') }
 
@@ -204,4 +204,4 @@ runCppcheck.onlyIf { !project.hasProperty('fromCI') }
 if (!project.hasProperty('fromCI')) {
     check.dependsOn runCppcheck
     runCppcheck.dependsOn spotlessApply
-}
\ No newline at end of file
+}
diff --git a/networktables.json b/networktables.json
index 1db10e2..df0b6b3 100644
--- a/networktables.json
+++ b/networktables.json
@@ -231,6 +231,342 @@
       "persistent": true
     }
   },
+  {
+    "name": "/Preferences/MotorMonkey/MotorMonkey/voltage_min (V)",
+    "type": "double",
+    "value": 7.5,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/MotorMonkey/MotorMonkey/recal_voltage_thresh (V)",
+    "type": "double",
+    "value": 9.5,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/MotorMonkey/MotorMonkey/default_max_draw (A)",
+    "type": "double",
+    "value": 150.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/MotorMonkey/MotorMonkey/battery_cc (A)",
+    "type": "double",
+    "value": 600.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/MotorMonkey/MotorMonkey/min_max_draw (A)",
+    "type": "double",
+    "value": 40.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/MotorMonkey/MotorMonkey/max_max_draw (A)",
+    "type": "double",
+    "value": 300.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/DrivetrainConstructor/drive_motor_current_limit (A)",
+    "type": "double",
+    "value": 120.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/DrivetrainConstructor/steer_motor_current_limit (A)",
+    "type": "double",
+    "value": 120.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/DrivetrainConstructor/drive_motor_smart_current_limit (A)",
+    "type": "double",
+    "value": 80.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/DrivetrainConstructor/steer_motor_smart_current_limit (A)",
+    "type": "double",
+    "value": 80.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/DrivetrainConstructor/drive_motor_voltage_compensation (V)",
+    "type": "double",
+    "value": 16.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/DrivetrainConstructor/steer_motor_voltage_compensation (V)",
+    "type": "double",
+    "value": 12.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/FR/cancoder_offset_ (deg)",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/FL/cancoder_offset_ (deg)",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/BL/cancoder_offset_ (deg)",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/BR/cancoder_offset_ (deg)",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/max_speed (fps)",
+    "type": "double",
+    "value": 15.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/max_omega (deg_per_s)",
+    "type": "double",
+    "value": 180.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/steer_lag (s)",
+    "type": "double",
+    "value": 0.05,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/MotorMonkey/voltage_min (V)",
+    "type": "double",
+    "value": 8.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/MotorMonkey/recal_voltage_thresh (V)",
+    "type": "double",
+    "value": 10.5,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/MotorMonkey/default_max_draw (A)",
+    "type": "double",
+    "value": 150.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/MotorMonkey/min_max_draw (A)",
+    "type": "double",
+    "value": 60.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/MotorMonkey/max_max_draw (A)",
+    "type": "double",
+    "value": 250.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/MotorMonkey/battery_cc (A)",
+    "type": "double",
+    "value": 700.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/MotorMonkey/brownout_perm_loops",
+    "type": "int",
+    "value": 500,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/bearing_gains/_kP",
+    "type": "double",
+    "value": 0.5,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/bearing_gains/_kI",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/bearing_gains/_kD",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/bearing_gains/deadband (deg_per_s)",
+    "type": "double",
+    "value": 3.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/lock_gains/_kP",
+    "type": "double",
+    "value": 0.5,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/lock_gains/_kI",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/lock_gains/_kD",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/lock_gains/deadband (in)",
+    "type": "double",
+    "value": 2.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/drive_to_subtract (in)",
+    "type": "double",
+    "value": 5.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/max_omega_cut (deg_per_s)",
+    "type": "double",
+    "value": 40.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/Robot/Robot/permissible_current_draw (A)",
+    "type": "double",
+    "value": 300.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/steer_gains/_kP",
+    "type": "double",
+    "value": 2.0,
+    "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/SwerveDrivetrain/odom_fudge_factor",
+    "type": "double",
+    "value": 0.875,
+    "properties": {
+      "persistent": true
+    }
+  },
   {
     "name": "/Preferences/coral_wrist/coral_wrist/coral_wrist/coral_wrist_tolerance_ (in)",
     "type": "double",
diff --git a/networktables.json.bck b/networktables.json.bck
index d2f84d8..40443cf 100644
--- a/networktables.json.bck
+++ b/networktables.json.bck
@@ -231,6 +231,310 @@
       "persistent": true
     }
   },
+  {
+    "name": "/Preferences/MotorMonkey/MotorMonkey/voltage_min (V)",
+    "type": "double",
+    "value": 7.5,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/MotorMonkey/MotorMonkey/recal_voltage_thresh (V)",
+    "type": "double",
+    "value": 9.5,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/MotorMonkey/MotorMonkey/default_max_draw (A)",
+    "type": "double",
+    "value": 150.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/MotorMonkey/MotorMonkey/battery_cc (A)",
+    "type": "double",
+    "value": 600.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/MotorMonkey/MotorMonkey/min_max_draw (A)",
+    "type": "double",
+    "value": 40.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/MotorMonkey/MotorMonkey/max_max_draw (A)",
+    "type": "double",
+    "value": 300.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/DrivetrainConstructor/drive_motor_current_limit (A)",
+    "type": "double",
+    "value": 120.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/DrivetrainConstructor/steer_motor_current_limit (A)",
+    "type": "double",
+    "value": 120.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/DrivetrainConstructor/drive_motor_smart_current_limit (A)",
+    "type": "double",
+    "value": 80.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/DrivetrainConstructor/steer_motor_smart_current_limit (A)",
+    "type": "double",
+    "value": 80.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/DrivetrainConstructor/drive_motor_voltage_compensation (V)",
+    "type": "double",
+    "value": 16.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/DrivetrainConstructor/steer_motor_voltage_compensation (V)",
+    "type": "double",
+    "value": 12.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/FR/cancoder_offset_ (deg)",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/FL/cancoder_offset_ (deg)",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/BL/cancoder_offset_ (deg)",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/BR/cancoder_offset_ (deg)",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/max_speed (fps)",
+    "type": "double",
+    "value": 15.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/max_omega (deg_per_s)",
+    "type": "double",
+    "value": 180.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/steer_lag (s)",
+    "type": "double",
+    "value": 0.05,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/MotorMonkey/voltage_min (V)",
+    "type": "double",
+    "value": 8.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/MotorMonkey/recal_voltage_thresh (V)",
+    "type": "double",
+    "value": 10.5,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/MotorMonkey/default_max_draw (A)",
+    "type": "double",
+    "value": 150.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/MotorMonkey/min_max_draw (A)",
+    "type": "double",
+    "value": 60.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/MotorMonkey/max_max_draw (A)",
+    "type": "double",
+    "value": 250.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/MotorMonkey/battery_cc (A)",
+    "type": "double",
+    "value": 700.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/MotorMonkey/brownout_perm_loops",
+    "type": "int",
+    "value": 500,
+    "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
+    }
+  },
+  {
+    "name": "/Preferences/Robot/Robot/permissible_current_draw (A)",
+    "type": "double",
+    "value": 300.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/steer_gains/_kP",
+    "type": "double",
+    "value": 2.0,
+    "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/SwerveDrivetrain/odom_fudge_factor",
+    "type": "double",
+    "value": 0.875,
+    "properties": {
+      "persistent": true
+    }
+  },
   {
     "name": "/Preferences/coral_wrist/coral_wrist/coral_wrist/coral_wrist_tolerance_ (in)",
     "type": "double",
diff --git a/simgui-ds.json b/simgui-ds.json
index 23ded49..981b520 100644
--- a/simgui-ds.json
+++ b/simgui-ds.json
@@ -1,4 +1,14 @@
 {
+  "FMS": {
+    "window": {
+      "visible": false
+    }
+  },
+  "Keyboard 0 Settings": {
+    "window": {
+      "visible": true
+    }
+  },
   "FMS": {
     "window": {
       "visible": false
diff --git a/simgui.json b/simgui.json
index 5aa8702..cb7cb15 100644
--- a/simgui.json
+++ b/simgui.json
@@ -33,6 +33,31 @@
       "/SmartDashboard/zero_bearing": "Command"
     }
   },
+  "NetworkTables": {
+    "transitory": {
+      "Shuffleboard": {
+        ".metadata": {
+          "open": true
+        },
+        "open": true
+      },
+      "SmartDashboard": {
+        "open": true,
+        "verify_hardware": {
+          "open": true
+        },
+        "zero_bearing": {
+          "open": true
+        }
+      }
+    }
+  },
+  "NetworkTables Info": {
+    "Connections": {
+      "open": true
+    },
+    "Elastic@1": {
+      "Publishers": {
   "NetworkTables": {
     "transitory": {
       "LiveWindow": {
@@ -91,6 +116,9 @@
         "prune_prefs": {
           "open": true
         },
+    "Server": {
+      "open": true
+    },
         "zero_bearing": {
           "open": true
         }
diff --git a/src/frc846/cpp/frc846/base/FunkyLogSystem.cc b/src/frc846/cpp/frc846/base/FunkyLogSystem.cc
index ac08848..16ba8d5 100644
--- a/src/frc846/cpp/frc846/base/FunkyLogSystem.cc
+++ b/src/frc846/cpp/frc846/base/FunkyLogSystem.cc
@@ -1,9 +1,8 @@
 #include "frc846/base/FunkyLogSystem.h"
 
 #include <fstream>
-#include <functional>
 #include <future>
-#include <iostream>
+#include <string>
 #include <thread>
 
 #include "frc846/base/compression.h"
diff --git a/src/frc846/cpp/frc846/base/Loggable.cc b/src/frc846/cpp/frc846/base/Loggable.cc
index 7848e21..79d4d29 100644
--- a/src/frc846/cpp/frc846/base/Loggable.cc
+++ b/src/frc846/cpp/frc846/base/Loggable.cc
@@ -4,13 +4,15 @@
 
 namespace frc846::base {
 
-std::string Loggable::Join(std::string p, std::string n) { return p + "/" + n; }
+std::string_view Loggable::Join(std::string p, std::string n) {
+  return p + "/" + n;
+}
 
 unsigned int Loggable::GetWarnCount() { return warn_count_; }
 
 unsigned int Loggable::GetErrorCount() { return error_count_; }
 
-std::unordered_set<std::string> Loggable::used_preferences_{};
+std::unordered_set<std::string_view> Loggable::used_preferences_{};
 
 unsigned int Loggable::warn_count_ = 0;
 unsigned int Loggable::error_count_ = 0;
diff --git a/src/frc846/cpp/frc846/base/compression.cc b/src/frc846/cpp/frc846/base/compression.cc
index 257c0d7..60f31b9 100644
--- a/src/frc846/cpp/frc846/base/compression.cc
+++ b/src/frc846/cpp/frc846/base/compression.cc
@@ -2,8 +2,6 @@
 
 #include <cctype>
 #include <cstring>
-#include <iostream>
-#include <string>
 
 namespace frc846::base {
 
@@ -13,11 +11,11 @@ std::pair<uint8_t, bool> Compression::char_conv(char x) {
 
   x = std::tolower(x);
 
-  const std::string ENCODING =
+  const std::string_view ENCODING =
       "\nabcdefghijklmnopqrstuvwxyz0123456789!@#$%^&*()[]<>|;:',./?~_- ";
 
   size_t pos = ENCODING.find(x);
-  if (pos != std::string::npos) {
+  if (pos != std::string_view::npos) {
     val = pos;
   } else {
     val = ENCODING.size() - 1;
@@ -47,7 +45,7 @@ std::vector<uint8_t> Compression::pack_bytes(std::vector<uint8_t> conv) {
   return output;
 }
 
-std::vector<uint8_t> Compression::compress(const std::string& data) {
+std::vector<uint8_t> Compression::compress(const std::string_view& data) {
   std::vector<uint8_t> conv;
   for (char c : data) {
     auto [val, is_upper] = char_conv(c);
diff --git a/src/frc846/cpp/frc846/base/fserver.cc b/src/frc846/cpp/frc846/base/fserver.cc
new file mode 100644
index 0000000..cf55045
--- /dev/null
+++ b/src/frc846/cpp/frc846/base/fserver.cc
@@ -0,0 +1,106 @@
+#include "frc846/base/fserver.h"
+
+#include <string.h>
+
+#include <algorithm>
+#include <iostream>
+#include <thread>
+
+namespace frc846::base {
+
+LoggingServer::LoggingServer() : messages{}, clients{} {
+#ifdef _WIN32
+  /*
+  Configuring Windows Socket API.
+  */
+
+  WSADATA wsaData;
+  int result = WSAStartup(MAKEWORD(2, 2), &wsaData);
+  if (result != 0) {
+    std::cerr << "WSAStartup failed: " << result << std::endl;
+    exit(EXIT_FAILURE);
+  }
+#endif
+}
+
+LoggingServer::~LoggingServer() {
+#ifdef _WIN32
+  WSACleanup();
+#endif
+}
+
+void LoggingServer::Start(int port) {
+  len = sizeof(cliaddr);
+
+  sockfd = socket(AF_INET, SOCK_DGRAM, 0);
+  if (sockfd < 0) throw std::runtime_error("Socket creation failed");
+
+  memset(&servaddr, 0, sizeof(servaddr));
+  servaddr.sin_family = AF_INET;
+  servaddr.sin_addr.s_addr = INADDR_ANY;
+  servaddr.sin_port = htons(port);
+
+  if (bind(sockfd, reinterpret_cast<const struct sockaddr *>(&servaddr),
+          sizeof(servaddr)) < 0)
+    throw std::runtime_error("Socket bind failed");
+
+  std::thread sender([&]() {
+    while (true) {
+      do {
+        std::this_thread::sleep_for(std::chrono::milliseconds(50));
+      } while (messages.empty() || clients.empty());
+
+      msg_mtx.lock();
+      auto msg = messages.front();
+      messages.pop();
+      msg_mtx.unlock();
+
+      cli_mtx.lock();
+      for (const auto &client : clients) {
+        sendto(sockfd, reinterpret_cast<const char *>(msg.data()), msg.size(),
+            0, reinterpret_cast<const struct sockaddr *>(&(client.addr)),
+            sizeof(client.addr));
+      }
+      cli_mtx.unlock();
+    }
+  });
+
+  std::thread receiver([&]() {
+    char buffer[1024];
+    std::chrono::milliseconds lastPruneTime{getTime()};
+    for (std::chrono::milliseconds t{lastPruneTime};; t = getTime()) {
+      if (recvfrom(sockfd, buffer, sizeof(buffer), 0,
+              reinterpret_cast<struct sockaddr *>(&cliaddr), &len) > 0) {
+        cli_mtx.lock();
+        auto it = std::find_if(
+            clients.begin(), clients.end(), [&](const LoggingClient &client) {
+              return client.addr.sin_addr.s_addr == cliaddr.sin_addr.s_addr;
+            });
+
+        if (it != clients.end()) {
+          it->addr.sin_port = cliaddr.sin_port;
+          it->lastKeepAlive = getTime();
+        } else {
+          clients.push_back({cliaddr, getTime()});
+        }
+        cli_mtx.unlock();
+      }
+      if (t - lastPruneTime > std::chrono::milliseconds(500)) {
+        cli_mtx.lock();
+        for (size_t i = 0; i < clients.size(); i++) {
+          if (getTime() - clients[i].lastKeepAlive >
+              std::chrono::milliseconds(5000)) {
+            clients.erase(clients.begin() + i);
+          }
+        }
+        cli_mtx.unlock();
+        lastPruneTime = t;
+      }
+    }
+  });
+
+  sender.detach();
+  receiver.detach();
+}
+
+}  // namespace frc846::base
\ No newline at end of file
diff --git a/src/frc846/cpp/frc846/control/HigherMotorController.cc b/src/frc846/cpp/frc846/control/HigherMotorController.cc
index 14622e6..97ac46d 100644
--- a/src/frc846/cpp/frc846/control/HigherMotorController.cc
+++ b/src/frc846/cpp/frc846/control/HigherMotorController.cc
@@ -140,4 +140,10 @@ void HigherMotorController::EnableStatusFrames(
   frc846::control::MotorMonkey::EnableStatusFrames(slot_id_, frames);
 }
 
+void HigherMotorController::OverrideStatusFramePeriod(
+    frc846::control::config::StatusFrame frame, units::millisecond_t period) {
+  frc846::control::MotorMonkey::OverrideStatusFramePeriod(
+      slot_id_, frame, period);
+}
+
 }  // namespace frc846::control
diff --git a/src/frc846/cpp/frc846/control/MotorMonkey.cc b/src/frc846/cpp/frc846/control/MotorMonkey.cc
index 9b2be43..f8ef099 100644
--- a/src/frc846/cpp/frc846/control/MotorMonkey.cc
+++ b/src/frc846/cpp/frc846/control/MotorMonkey.cc
@@ -5,7 +5,8 @@
 #include <rev/SparkFlex.h>
 #include <rev/SparkMax.h>
 
-#include <string>
+#include <iostream>
+#include <string_view>
 
 #include "frc846/control/hardware/SparkMXFX_interm.h"
 #include "frc846/control/hardware/TalonFX_interm.h"
@@ -13,8 +14,6 @@
 #include "frc846/control/hardware/simulation/SIMLEVEL.h"
 #include "frc846/math/collection.h"
 
-// TODO: Add dynamic can/power management
-
 namespace frc846::control {
 
 #define CHECK_SLOT_ID()                                                      \
@@ -74,15 +73,99 @@ frc846::wpilib::unit_ohm
 
 units::volt_t MotorMonkey::battery_voltage{0_V};
 
+units::volt_t MotorMonkey::last_disabled_voltage{0_V};
+
+frc846::math::DoubleSyncBuffer MotorMonkey::sync_buffer{100U, 35};
+
+units::ampere_t MotorMonkey::max_draw_{0.0_A};
+
 std::queue<MotorMonkey::MotorMessage> MotorMonkey::control_messages{};
 
-void MotorMonkey::Tick(units::ampere_t max_draw) {
+int MotorMonkey::num_loops_last_brown = 4000;
+
+void MotorMonkey::Setup() {
+  loggable_.RegisterPreference("voltage_min", 8.0_V);
+  loggable_.RegisterPreference("recal_voltage_thresh", 10.5_V);
+  loggable_.RegisterPreference("default_max_draw", 150.0_A);
+  loggable_.RegisterPreference("min_max_draw", 60_A);
+  loggable_.RegisterPreference("max_max_draw", 250_A);
+  loggable_.RegisterPreference("battery_cc", 700_A);
+  loggable_.RegisterPreference("brownout_perm_loops", 500);
+
+  max_draw_ = loggable_.GetPreferenceValue_unit_type<units::ampere_t>(
+      "default_max_draw");
+}
+
+void MotorMonkey::RecalculateMaxDraw() {
+  if (!sync_buffer.IsValid()) return;
+
+  if (frc::RobotController::IsBrownedOut()) num_loops_last_brown = 0;
+  if (num_loops_last_brown <
+      loggable_.GetPreferenceValue_int("brownout_perm_loops")) {
+    max_draw_ =
+        loggable_.GetPreferenceValue_unit_type<units::ampere_t>("min_max_draw");
+    return;
+  }
+
+  sync_buffer.Sync();
+  loggable_.Graph("sync_diff_", sync_buffer.GetSyncDiff());
+
+  auto sy_trough = sync_buffer.GetTrough();
+  units::ampere_t cc_current{sy_trough.first};
+
+  loggable_.Graph("cc_current", cc_current);
+
+  units::ampere_t current_draw =
+      loggable_.GetPreferenceValue_unit_type<units::ampere_t>("battery_cc") -
+      cc_current;
+  units::volt_t voltage{sy_trough.second};
+
+  units::volt_t voltage_min =
+      loggable_.GetPreferenceValue_unit_type<units::volt_t>("voltage_min");
+  units::volt_t recal_voltage_thresh =
+      loggable_.GetPreferenceValue_unit_type<units::volt_t>(
+          "recal_voltage_thresh");
+
+  if (voltage > recal_voltage_thresh) return;
+
+  units::ampere_t temp_max_draw_ = current_draw *
+                                   (last_disabled_voltage - voltage_min) /
+                                   (last_disabled_voltage - voltage);
+
+  if (temp_max_draw_ <
+      loggable_.GetPreferenceValue_unit_type<units::ampere_t>("min_max_draw")) {
+    return;
+  } else if (temp_max_draw_ >
+             loggable_.GetPreferenceValue_unit_type<units::ampere_t>(
+                 "max_max_draw")) {
+    return;
+  } else {
+    max_draw_ = temp_max_draw_;
+  }
+}
+
+void MotorMonkey::Tick(bool disabled) {
   battery_voltage = frc::RobotController::GetBatteryVoltage();
-  loggable_.Graph("battery_voltage", battery_voltage.to<double>());
+  loggable_.Graph("battery_voltage", battery_voltage);
+  loggable_.Graph("last_disabled_voltage", last_disabled_voltage);
+
+  if (disabled) {
+    last_disabled_voltage = battery_voltage;
+    return;
+  }
+
+  units::ampere_t total_pred_draw = WriteMessages(max_draw_);
 
-  WriteMessages(max_draw);
+  loggable_.Graph("total_pred_draw", total_pred_draw);
 
-  // TODO: Improve battery voltage estimation for simulation
+  units::ampere_t cc_current =
+      loggable_.GetPreferenceValue_unit_type<units::ampere_t>("battery_cc") -
+      total_pred_draw;
+  sync_buffer.Add(cc_current.to<double>(), battery_voltage.to<double>());
+
+  RecalculateMaxDraw();
+
+  loggable_.Graph("max_draw", max_draw_);
 
   for (size_t i = 0; i < CONTROLLER_REGISTRY_SIZE; i++) {
     if (controller_registry[i] != nullptr) {
@@ -92,13 +175,16 @@ void MotorMonkey::Tick(units::ampere_t max_draw) {
             dynamic_cast<simulation::MCSimulator*>(controller_registry[i]);
         sim->SetBatteryVoltage(battery_voltage);
         sim->SetLoad(load_registry[i]);
+        // sim->Tick();
+        // TODO: fix sim
       }
     }
   }
 }
 
-void MotorMonkey::WriteMessages(units::ampere_t max_draw) {
+units::ampere_t MotorMonkey::WriteMessages(units::ampere_t max_draw) {
   units::ampere_t total_current = 0.0_A;
+  units::ampere_t second_total_current = 0.0_A;
   std::queue<MotorMessage> temp_messages{control_messages};
 
   double scale_factor = 1.0;
@@ -112,8 +198,6 @@ void MotorMonkey::WriteMessages(units::ampere_t max_draw) {
     units::radians_per_second_t velocity =
         controller_registry[msg.slot_id]->GetVelocity();
 
-    frc846::control::base::MotorGains gains = gains_registry[msg.slot_id];
-
     double duty_cycle = 0.0;
 
     switch (msg.type) {
@@ -121,36 +205,32 @@ void MotorMonkey::WriteMessages(units::ampere_t max_draw) {
       duty_cycle = std::get<double>(msg.value);
       break;
     case MotorMessage::Type::Position: {
-      duty_cycle =
-          gains.calculate((std::get<units::radian_t>(msg.value) -
-                              controller_registry[msg.slot_id]->GetPosition())
-                              .to<double>(),
-              0.0, velocity.to<double>(), 0.0);
-      duty_cycle = std::clamp(duty_cycle, -1.0, 1.0);
+      // Onboard control should only be used with low-current draw systems
+      duty_cycle = 0.0;
       break;
     }
     case MotorMessage::Type::Velocity: {
-      duty_cycle = gains.calculate(
-          (std::get<units::radians_per_second_t>(msg.value) - velocity)
-              .to<double>(),
-          0.0, 0.0, 0.0);
-      duty_cycle +=
-          gains.kFF *
-          (std::get<units::radians_per_second_t>(msg.value) /
-              frc846::control::base::MotorSpecificationPresets::get(motor_type)
-                  .free_speed)
-              .to<double>();
-      duty_cycle = std::clamp(duty_cycle, -1.0, 1.0);
+      // Onboard control should only be used with low-current draw systems
+      duty_cycle = 0.0;
       break;
     }
     default:
       throw std::runtime_error(
           "Unsupported MotorMessage type in MotorMonkey WriteMessages");
     }
-    total_current +=
-        units::math::abs(frc846::control::calculators::CurrentTorqueCalculator::
-                predict_current_draw(duty_cycle, velocity, battery_voltage,
-                    circuit_resistance_registry[msg.slot_id], motor_type));
+    units::ampere_t pred_draw =
+        frc846::control::calculators::CurrentTorqueCalculator::
+            predict_current_draw(duty_cycle, velocity, battery_voltage,
+                circuit_resistance_registry[msg.slot_id], motor_type);
+
+    if (velocity > 0_rad_per_s && pred_draw < 0_A) {
+      (void)pred_draw;  // Regen braking mode
+    } else if (velocity < 0_rad_per_s && pred_draw > 0_A) {
+      (void)pred_draw;  // Regen braking mode
+    } else {
+      second_total_current += units::math::abs(pred_draw);
+    }
+    total_current += units::math::abs(pred_draw);
     temp_messages.pop();
   }
 
@@ -215,6 +295,7 @@ void MotorMonkey::WriteMessages(units::ampere_t max_draw) {
 
     control_messages.pop();
   }
+  return second_total_current;
 }
 
 bool MotorMonkey::VerifyConnected() {
@@ -298,6 +379,16 @@ void MotorMonkey::EnableStatusFrames(
   LOG_IF_ERROR("EnableStatusFrames");
 }
 
+void MotorMonkey::OverrideStatusFramePeriod(size_t slot_id,
+    frc846::control::config::StatusFrame frame, units::millisecond_t period) {
+  CHECK_SLOT_ID();
+
+  SMART_RETRY(
+      controller_registry[slot_id]->OverrideStatusFramePeriod(frame, period),
+      "OverrideStatusFramePeriod");
+  LOG_IF_ERROR("OverrideStatusFramePeriod");
+}
+
 units::volt_t MotorMonkey::GetBatteryVoltage() { return battery_voltage; }
 
 void MotorMonkey::SetLoad(size_t slot_id, units::newton_meter_t load) {
@@ -416,7 +507,7 @@ void MotorMonkey::SetSoftLimits(size_t slot_id, units::radian_t forward_limit,
   LOG_IF_ERROR("SetSoftLimits");
 }
 
-std::string MotorMonkey::parseError(
+std::string_view MotorMonkey::parseError(
     frc846::control::hardware::ControllerErrorCodes err) {
   switch (err) {
   case frc846::control::hardware::ControllerErrorCodes::kAllOK: return "All OK";
diff --git a/src/frc846/cpp/frc846/control/calculators/CurrentTorqueCalculator.cc b/src/frc846/cpp/frc846/control/calculators/CurrentTorqueCalculator.cc
index c5afa01..4e80de3 100644
--- a/src/frc846/cpp/frc846/control/calculators/CurrentTorqueCalculator.cc
+++ b/src/frc846/cpp/frc846/control/calculators/CurrentTorqueCalculator.cc
@@ -46,7 +46,6 @@ double CurrentTorqueCalculator::scale_current_draw(double scale_factor,
 double CurrentTorqueCalculator::limit_current_draw(double duty_cycle,
     units::ampere_t current_limit, units::revolutions_per_minute_t rpm,
     units::volt_t v_supply, unit_ohm circuit_resistance, MotorSpecs specs) {
-  double duty_cycle_original = duty_cycle;
   units::ampere_t current_draw = predict_current_draw(
       duty_cycle, rpm, v_supply, circuit_resistance, specs);
   if (units::math::abs(current_draw) > current_limit) {
diff --git a/src/frc846/cpp/frc846/control/hardware/SparkMXFX_interm.cc b/src/frc846/cpp/frc846/control/hardware/SparkMXFX_interm.cc
index 6a76924..7478c60 100644
--- a/src/frc846/cpp/frc846/control/hardware/SparkMXFX_interm.cc
+++ b/src/frc846/cpp/frc846/control/hardware/SparkMXFX_interm.cc
@@ -121,7 +121,7 @@ void SparkMXFX_interm::EnableStatusFrames(
 
   if (vector_has(frames, config::StatusFrame::kVelocityFrame) ||
       vector_has(frames, config::StatusFrame::kCurrentFrame)) {
-    configs.signals.PrimaryEncoderVelocityPeriodMs(20);
+    configs.signals.PrimaryEncoderVelocityPeriodMs(25);
     configs.signals.PrimaryEncoderVelocityAlwaysOn(true);
   } else {
     configs.signals.PrimaryEncoderVelocityPeriodMs(32767);
@@ -147,6 +147,23 @@ void SparkMXFX_interm::EnableStatusFrames(
   APPLY_CONFIG_NO_RESET();
 }
 
+void SparkMXFX_interm::OverrideStatusFramePeriod(
+    frc846::control::config::StatusFrame frame, units::millisecond_t period) {
+  if (frame == config::StatusFrame::kFaultFrame ||
+      frame == config::StatusFrame::kLeader) {
+    configs.signals.FaultsPeriodMs(period.to<int>());
+    configs.signals.FaultsAlwaysOn(true);
+  } else if (frame == config::StatusFrame::kVelocityFrame) {
+    configs.signals.PrimaryEncoderVelocityPeriodMs(period.to<int>());
+  } else if (frame == config::StatusFrame::kPositionFrame) {
+    configs.signals.PrimaryEncoderPositionPeriodMs(period.to<int>());
+  } else if (frame == config::StatusFrame::kSensorFrame) {
+    configs.signals.AnalogPositionPeriodMs(period.to<int>());
+  }
+
+  APPLY_CONFIG_NO_RESET();
+}
+
 bool SparkMXFX_interm::IsDuplicateControlMessage(double duty_cycle) {
   if (double* dc = std::get_if<double>(&last_command_)) {
     return *dc == duty_cycle;
diff --git a/src/frc846/cpp/frc846/control/hardware/TalonFX_interm.cc b/src/frc846/cpp/frc846/control/hardware/TalonFX_interm.cc
index db97819..28249bb 100644
--- a/src/frc846/cpp/frc846/control/hardware/TalonFX_interm.cc
+++ b/src/frc846/cpp/frc846/control/hardware/TalonFX_interm.cc
@@ -5,7 +5,7 @@ namespace frc846::control::hardware {
 bool TalonFX_interm::VerifyConnected() { return talon_.IsAlive(); }
 
 TalonFX_interm::TalonFX_interm(
-    int can_id, std::string bus, units::millisecond_t max_wait_time)
+    int can_id, std::string_view bus, units::millisecond_t max_wait_time)
     : talon_(can_id, bus), max_wait_time_(max_wait_time) {}
 
 void TalonFX_interm::Tick() {
@@ -90,19 +90,59 @@ void TalonFX_interm::EnableStatusFrames(
   if (last_error_ != ControllerErrorCodes::kAllOK) { return; }
   for (auto frame : frames) {
     ctre::phoenix::StatusCode last_status_code = ctre::phoenix::StatusCode::OK;
-    if (frame == frc846::control::config::StatusFrame::kCurrentFrame) {
-      last_status_code = talon_.GetSupplyCurrent().SetUpdateFrequency(10_Hz);
-    } else if (frame == frc846::control::config::StatusFrame::kPositionFrame) {
+    switch (frame) {
+    case frc846::control::config::StatusFrame::kCurrentFrame:
+      last_status_code =
+          talon_.GetSupplyCurrent().SetUpdateFrequency(10_Hz, max_wait_time_);
+      break;
+    case frc846::control::config::StatusFrame::kPositionFrame:
       last_status_code =
           talon_.GetPosition().SetUpdateFrequency(50_Hz, max_wait_time_);
-    } else if (frame == frc846::control::config::StatusFrame::kVelocityFrame) {
-      last_status_code = talon_.GetVelocity().SetUpdateFrequency(50_Hz);
+      if (last_status_code != ctre::phoenix::StatusCode::OK) break;
+
+      [[fallthrough]];  // Latency compensation requires vel, acc frames
+    case frc846::control::config::StatusFrame::kVelocityFrame:
+      last_status_code =
+          talon_.GetVelocity().SetUpdateFrequency(40_Hz, max_wait_time_);
+      if (last_status_code != ctre::phoenix::StatusCode::OK) break;
+      last_status_code =
+          talon_.GetAcceleration().SetUpdateFrequency(40_Hz, max_wait_time_);
+      break;
+
+    default: break;
     }
+
     last_error_ = getErrorCode(last_status_code);
     if (last_error_ != ControllerErrorCodes::kAllOK) return;
   }
 }
 
+void TalonFX_interm::OverrideStatusFramePeriod(
+    frc846::control::config::StatusFrame frame, units::millisecond_t period) {
+  ctre::phoenix::StatusCode last_status_code = ctre::phoenix::StatusCode::OK;
+  switch (frame) {
+  case frc846::control::config::StatusFrame::kCurrentFrame:
+    last_status_code = talon_.GetSupplyCurrent().SetUpdateFrequency(
+        1 / period, max_wait_time_);
+    break;
+  case frc846::control::config::StatusFrame::kPositionFrame:
+    last_status_code =
+        talon_.GetPosition().SetUpdateFrequency(1 / period, max_wait_time_);
+    if (last_status_code != ctre::phoenix::StatusCode::OK) break;
+
+    [[fallthrough]];  // Latency compensation requires vel, acc frames
+  case frc846::control::config::StatusFrame::kVelocityFrame:
+    last_status_code =
+        talon_.GetVelocity().SetUpdateFrequency(1 / period, max_wait_time_);
+    if (last_status_code != ctre::phoenix::StatusCode::OK) break;
+    last_status_code =
+        talon_.GetAcceleration().SetUpdateFrequency(1 / period, max_wait_time_);
+    break;
+  default: break;
+  }
+  last_error_ = getErrorCode(last_status_code);
+}
+
 bool TalonFX_interm::IsDuplicateControlMessage(double duty_cycle) {
   if (double* dc = std::get_if<double>(&last_command_)) {
     return *dc == duty_cycle;
@@ -129,10 +169,12 @@ void TalonFX_interm::ZeroEncoder(units::radian_t position) {
 }
 
 units::radians_per_second_t TalonFX_interm::GetVelocity() {
-  return talon_.GetVelocity().GetValue();
+  return ctre::phoenix6::BaseStatusSignal::GetLatencyCompensatedValue(
+      talon_.GetVelocity(), talon_.GetAcceleration());
 }
 units::radian_t TalonFX_interm::GetPosition() {
-  return talon_.GetPosition().GetValue();
+  return ctre::phoenix6::BaseStatusSignal::GetLatencyCompensatedValue(
+      talon_.GetPosition(), talon_.GetVelocity());
 }
 units::ampere_t TalonFX_interm::GetCurrent() {
   return talon_.GetSupplyCurrent().GetValue();
diff --git a/src/frc846/cpp/frc846/control/hardware/simulation/MCSimulator.cc b/src/frc846/cpp/frc846/control/hardware/simulation/MCSimulator.cc
index cdc0f10..9a44e7e 100644
--- a/src/frc846/cpp/frc846/control/hardware/simulation/MCSimulator.cc
+++ b/src/frc846/cpp/frc846/control/hardware/simulation/MCSimulator.cc
@@ -29,16 +29,20 @@ void MCSimulator::Tick() {
     duty_cycle = gains.calculate(
         pos->to<double>(), 0.0, velocity_.to<double>(), load_.to<double>());
   }
+  duty_cycle = std::clamp(duty_cycle, -1.0, 1.0);
+
   pred_current_ = frc846::control::calculators::CurrentTorqueCalculator::
       predict_current_draw(
           duty_cycle, velocity_, battery_voltage_, circuit_resistance_, specs);
   units::newton_meter_t torque_output =
       frc846::control::calculators::CurrentTorqueCalculator::current_to_torque(
           pred_current_, specs);
+  units::newton_meter_t friction_torque =
+      specs.stall_torque * specs.friction_loss *
+      (velocity_ / units::math::abs(velocity_));
+  if (units::math::abs(velocity_) < 0.01_rad_per_s) { friction_torque = 0_Nm; }
   torque_output -= load_;
 
-  if (inverted) torque_output = -torque_output;
-
   std::chrono::microseconds current_time =
       std::chrono::duration_cast<std::chrono::microseconds>(
           std::chrono::system_clock::now().time_since_epoch());
@@ -48,6 +52,9 @@ void MCSimulator::Tick() {
   units::radians_per_second_t new_velocity =
       frc846::control::calculators::VelocityPositionEstimator::predict_velocity(
           velocity_, torque_output, loop_time, rotational_inertia_);
+  new_velocity =
+      units::radians_per_second_t{std::clamp(new_velocity.to<double>(),
+          (-specs.free_speed).to<double>(), specs.free_speed.to<double>())};
   units::radians_per_second_t avg_velocity = (velocity_ + new_velocity) / 2.0;
 
   position_ =
@@ -62,21 +69,31 @@ void MCSimulator::SetInverted(bool inverted) {
   position_ = -position_;
 }
 
-void MCSimulator::WriteDC(double duty_cycle) { control_message = duty_cycle; }
+void MCSimulator::WriteDC(double duty_cycle) {
+  if (inverted) duty_cycle = -duty_cycle;
+  control_message = duty_cycle;
+}
 void MCSimulator::WriteVelocity(units::radians_per_second_t velocity) {
+  if (inverted) velocity = -velocity;
   control_message = velocity;
 }
 void MCSimulator::WritePosition(units::radian_t position) {
+  if (inverted) position_ = -position_;
   control_message = position;
 }
 
 units::ampere_t MCSimulator::GetCurrent() {
   return units::math::abs(pred_current_);
 }
-units::radian_t MCSimulator::GetPosition() { return position_; }
-units::radians_per_second_t MCSimulator::GetVelocity() { return velocity_; }
+units::radian_t MCSimulator::GetPosition() {
+  return inverted ? -position_ : position_;
+}
+units::radians_per_second_t MCSimulator::GetVelocity() {
+  return inverted ? -velocity_ : velocity_;
+}
 
 void MCSimulator::ZeroEncoder(units::radian_t position) {
+  if (inverted) position = -position;
   this->position_ = position;
 }
 
@@ -103,15 +120,18 @@ void MCSimulator::EnableStatusFrames(
 }
 
 bool MCSimulator::IsDuplicateControlMessage(double duty_cycle) {
+  if (inverted) duty_cycle = -duty_cycle;
   return std::holds_alternative<double>(control_message) &&
          std::get<double>(control_message) == duty_cycle;
 }
 bool MCSimulator::IsDuplicateControlMessage(
     units::radians_per_second_t velocity) {
+  if (inverted) velocity = -velocity;
   return std::holds_alternative<units::radians_per_second_t>(control_message) &&
          std::get<units::radians_per_second_t>(control_message) == velocity;
 }
 bool MCSimulator::IsDuplicateControlMessage(units::radian_t position) {
+  if (inverted) position = -position;
   return std::holds_alternative<units::radian_t>(control_message) &&
          std::get<units::radian_t>(control_message) == position;
 }
@@ -120,7 +140,10 @@ void MCSimulator::SetGains(frc846::control::base::MotorGains gains) {
   this->gains = gains;
 }
 
-void MCSimulator::SetLoad(units::newton_meter_t load) { this->load_ = load; }
+void MCSimulator::SetLoad(units::newton_meter_t load) {
+  if (inverted) load = -load;
+  this->load_ = load;
+}
 void MCSimulator::SetBatteryVoltage(units::volt_t voltage) {
   this->battery_voltage_ = voltage;
 }
diff --git a/src/frc846/cpp/frc846/math/Differentiator.cc b/src/frc846/cpp/frc846/math/Differentiator.cc
new file mode 100644
index 0000000..73463dd
--- /dev/null
+++ b/src/frc846/cpp/frc846/math/Differentiator.cc
@@ -0,0 +1,41 @@
+#include "frc846/math/Differentiator.h"
+
+namespace frc846::math {
+
+Differentiator::Differentiator()
+    : last_value_(0.0), last_timestamp_(0.0), rate_(0.0) {}
+
+void Differentiator::Reset() {
+  last_value_ = 0.0;
+  last_timestamp_ = 0.0;
+  rate_ = 0.0;
+}
+
+double Differentiator::Calculate(double value) {
+  double current_timestamp =
+      std::chrono::duration_cast<std::chrono::milliseconds>(
+          std::chrono::system_clock::now().time_since_epoch())
+          .count() /
+      1000.0;
+
+  if (last_timestamp_ == 0.0) {
+    last_value_ = value;
+    last_timestamp_ = current_timestamp;
+    return 0.0;
+  }
+
+  double dt = current_timestamp - last_timestamp_;
+
+  if (dt <= 0.0) { return rate_; }
+
+  rate_ = (value - last_value_) / dt;
+
+  last_value_ = value;
+  last_timestamp_ = current_timestamp;
+
+  return rate_;
+}
+
+double Differentiator::GetRate() const { return rate_; }
+
+}  // namespace frc846::math
\ No newline at end of file
diff --git a/src/frc846/cpp/frc846/math/DoubleSyncBuffer.cc b/src/frc846/cpp/frc846/math/DoubleSyncBuffer.cc
new file mode 100644
index 0000000..4289067
--- /dev/null
+++ b/src/frc846/cpp/frc846/math/DoubleSyncBuffer.cc
@@ -0,0 +1,70 @@
+#include "frc846/math/DoubleSyncBuffer.h"
+
+#include <stdexcept>
+
+namespace frc846::math {
+
+DoubleSyncBuffer::DoubleSyncBuffer(size_t sz, int max_sync_diff)
+    : def_size{sz}, max_sync_diff_{max_sync_diff} {
+  m_buffer_1.reserve(def_size);
+  m_buffer_2.reserve(def_size);
+}
+
+void DoubleSyncBuffer::Add(double val1, double val2) {
+  m_buffer_1.push_back(val1);
+  m_buffer_2.push_back(val2);
+
+  if (m_buffer_1.size() > def_size) {
+    m_buffer_1.erase(m_buffer_1.begin());
+    m_buffer_2.erase(m_buffer_2.begin());
+  }
+}
+
+bool DoubleSyncBuffer::IsValid() {
+  if (m_buffer_1.size() != m_buffer_2.size())
+    throw std::runtime_error(
+        "DoubleSyncBuffer contained buffer sizes are inequal");
+
+  return m_buffer_1.size() == def_size;
+}
+
+void DoubleSyncBuffer::Sync() {
+  if (!IsValid())
+    throw std::runtime_error(
+        "DoubleSyncBuffer::Sync() called on invalid buffer");
+
+  sync_diff_ = 0;
+
+  double max_correlation = -1.0;
+
+  for (int shift = 0; shift < max_sync_diff_; shift++) {
+    double correlation = 0.0;
+    size_t sz = m_buffer_2.size() - shift;
+    for (size_t i = 0; i < sz; i++) {
+      correlation += m_buffer_1[i] * m_buffer_2[i + shift];
+    }
+    correlation /= sz;
+    if (correlation > max_correlation) {
+      max_correlation = correlation;
+      sync_diff_ = shift;
+    }
+  }
+}
+
+std::pair<double, double> DoubleSyncBuffer::GetTrough() {
+  if (!IsValid())
+    throw std::runtime_error(
+        "DoubleSyncBuffer::GetTrough() called on invalid buffer");
+
+  double min1 = 100000000.0;
+  double min2 = 100000000.0;
+
+  for (size_t i = 1; i < m_buffer_2.size() - sync_diff_; i++) {
+    if (m_buffer_2[i + sync_diff_] < min2) min2 = m_buffer_2[i + sync_diff_];
+    if (m_buffer_1[i] < min1) min1 = m_buffer_1[i];
+  }
+
+  return std::make_pair(min1, min2);
+}
+
+}  // namespace frc846::math
\ No newline at end of file
diff --git a/src/frc846/cpp/frc846/math/RampRateLimiter.cc b/src/frc846/cpp/frc846/math/RampRateLimiter.cc
new file mode 100644
index 0000000..4ddff41
--- /dev/null
+++ b/src/frc846/cpp/frc846/math/RampRateLimiter.cc
@@ -0,0 +1,31 @@
+#include "frc846/math/RampRateLimiter.h"
+
+#include <cmath>
+
+namespace frc846::math {
+
+RampRateLimiter::RampRateLimiter() {
+  m_lastTime = std::chrono::duration_cast<std::chrono::milliseconds>(
+      std::chrono::system_clock::now().time_since_epoch());
+}
+
+double RampRateLimiter::limit(double value, double rateLimit) {
+  auto currentTime = std::chrono::duration_cast<std::chrono::milliseconds>(
+      std::chrono::system_clock::now().time_since_epoch());
+  auto elapsedTime = currentTime - m_lastTime;
+  m_lastTime = currentTime;
+
+  double maxChange = std::abs(rateLimit * elapsedTime.count() / 1000.0);
+  double change = value - m_lastValue;
+
+  if (change > maxChange) {
+    value = m_lastValue + maxChange;
+  } else if (change < -maxChange) {
+    value = m_lastValue - maxChange;
+  }
+
+  m_lastValue = value;
+  return value;
+}
+
+}  // namespace frc846::math
\ No newline at end of file
diff --git a/src/frc846/cpp/frc846/robot/GenericRobot.cc b/src/frc846/cpp/frc846/robot/GenericRobot.cc
index 8ced2ca..84e655b 100644
--- a/src/frc846/cpp/frc846/robot/GenericRobot.cc
+++ b/src/frc846/cpp/frc846/robot/GenericRobot.cc
@@ -31,8 +31,6 @@ GenericRobot::GenericRobot(GenericRobotContainer* container)
   FRC_CheckErrorStatus(status, "{}", "InitializeNotifier");
 
   HAL_SetNotifierName(notifier_, "Robot", &status);
-
-  RegisterPreference("permissible_current_draw", 300_A);
 }
 
 GenericRobot::~GenericRobot() {
@@ -56,6 +54,9 @@ void GenericRobot::StartCompetition() {
 
   frc846::base::FunkyLogSystem::Start(20000);
 
+  // Setup MotorMonkey
+  frc846::control::MotorMonkey::Setup();
+
   // Add dashboard buttons
 
   frc::SmartDashboard::PutData("verify_hardware",
@@ -130,7 +131,7 @@ void GenericRobot::StartCompetition() {
         loop.Clear();
       } else if (mode == Mode::kAutonomous) {
         // Get and run selected auto command
-        std::string option_name = auto_chooser_.GetSelected();
+        std::string_view option_name = auto_chooser_.GetSelected();
         auto_command_ = autos_[option_name];
 
         if (auto_command_ != nullptr) {
@@ -177,9 +178,7 @@ void GenericRobot::StartCompetition() {
     generic_robot_container_->UpdateHardware();
 
     // Tick MotorMonkey
-    frc846::control::MotorMonkey::Tick(
-        GetPreferenceValue_unit_type<units::ampere_t>(
-            "permissible_current_draw"));
+    frc846::control::MotorMonkey::Tick(mode == Mode::kDisabled);
 
     // Update dashboards
     frc::SmartDashboard::UpdateValues();
@@ -223,11 +222,11 @@ void GenericRobot::VerifyHardware() {
   generic_robot_container_->VerifyHardware();
 }
 
-void GenericRobot::AddAuto(std::string name, frc2::Command* command) {
+void GenericRobot::AddAuto(std::string_view name, frc2::Command* command) {
   auto_chooser_.AddOption(name, name);
   autos_[name] = command;
   frc::SmartDashboard::PutData(&auto_chooser_);
   frc::SmartDashboard::UpdateValues();
 }
 
-}  // namespace frc846::robot
\ No newline at end of file
+}  // namespace frc846::robot
diff --git a/src/frc846/cpp/frc846/robot/swerve/aim_command.cc b/src/frc846/cpp/frc846/robot/swerve/aim_command.cc
new file mode 100644
index 0000000..1d16971
--- /dev/null
+++ b/src/frc846/cpp/frc846/robot/swerve/aim_command.cc
@@ -0,0 +1,27 @@
+#include "frc846/robot/swerve/aim_command.h"
+
+namespace frc846::robot::swerve {
+
+AimCommand::AimCommand(DrivetrainSubsystem* drivetrain, units::degree_t target)
+    : Loggable("AimCommand"), drivetrain_{drivetrain}, target_(target) {
+  SetName("AimCommand");
+  AddRequirements({drivetrain_});
+}
+
+void AimCommand::Initialize() {
+  Log("AimCommand initialized with target {}", target_);
+}
+
+void AimCommand::Execute() {
+  drivetrain_->SetTarget(DrivetrainOLControlTarget{
+      {0_fps, 0_fps}, drivetrain_->ApplyBearingPID(target_)});
+}
+
+void AimCommand::End(bool interrupted) {
+  Log("AimCommand ended with interruption status {}", interrupted);
+  drivetrain_->SetTargetZero();
+}
+
+bool AimCommand::IsFinished() { return false; }
+
+}  // namespace frc846::robot::swerve
\ No newline at end of file
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
index d092d7e..2fe9b01 100644
--- a/src/frc846/cpp/frc846/robot/swerve/control/swerve_ol_calculator.cc
+++ b/src/frc846/cpp/frc846/robot/swerve/control/swerve_ol_calculator.cc
@@ -17,34 +17,70 @@ SwerveOpenLoopCalculatorOutput SwerveOpenLoopCalculator::calculate(
   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};
+  auto calculate_module_targets = [&](units::radians_per_second_t rotation) {
+    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) - 90_deg;
+      units::degree_t rot_direction = location.angle(false) - 90_deg;
 
-    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};
+      frc846::math::VectorND<units::feet_per_second, 2> rotation_vector{
+          rotation * units::math::cos(rot_direction) * radius / 1_rad,
+          rotation * units::math::sin(rot_direction) * radius / 1_rad};
 
-    module_targets[i] =
-        inputs.translation_target.rotate(-inputs.bearing, true) + rotation;
-  }
+      module_targets[i] =
+          inputs.translation_target.rotate(-inputs.bearing, true) +
+          rotation_vector;
+    }
+  };
 
-  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();
+  auto get_max_mag = [&]() {
+    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();
+      }
     }
-  }
+    return max_mag;
+  };
 
-  if (max_mag > inputs.max_speed) {
+  auto rescale_outputs = [&](units::feet_per_second_t max_mag) {
     for (int i = 0; i < 4; i++) {
       module_targets[i] *= inputs.max_speed / max_mag;
     }
+  };
+
+  calculate_module_targets(inputs.rotation_target);
+
+  if (inputs.cut_excess_steering) {
+    units::radians_per_second_t rotation_target = inputs.rotation_target;
+    units::radians_per_second_t abs_rotation_target =
+        units::math::abs(inputs.rotation_target);
+    units::feet_per_second_t max_mag;
+
+    do {
+      calculate_module_targets(rotation_target);
+
+      max_mag = get_max_mag();
+
+      if (max_mag > inputs.max_speed) {
+        abs_rotation_target -= constants_.rotation_iter_dec;
+        rotation_target =
+            units::math::copysign(abs_rotation_target, rotation_target);
+        if (abs_rotation_target < 0_rad_per_s) {
+          calculate_module_targets(0_rad_per_s);
+          max_mag = get_max_mag();
+          if (max_mag > inputs.max_speed) { rescale_outputs(get_max_mag()); }
+          break;
+        }
+      } else {
+        break;
+      }
+    } while (max_mag > inputs.max_speed);
+  } else {
+    units::feet_per_second_t max_mag = get_max_mag();
+    if (max_mag > inputs.max_speed) { rescale_outputs(max_mag); }
   }
 
   SwerveOpenLoopCalculatorOutput output{};
diff --git a/src/frc846/cpp/frc846/robot/swerve/drive_to_point_command.cc b/src/frc846/cpp/frc846/robot/swerve/drive_to_point_command.cc
new file mode 100644
index 0000000..8f4cc26
--- /dev/null
+++ b/src/frc846/cpp/frc846/robot/swerve/drive_to_point_command.cc
@@ -0,0 +1,79 @@
+#include "frc846/robot/swerve/drive_to_point_command.h"
+
+namespace frc846::robot::swerve {
+
+DriveToPointCommand::DriveToPointCommand(DrivetrainSubsystem* drivetrain,
+    frc846::math::FieldPoint target, units::feet_per_second_t max_speed,
+    units::feet_per_second_squared_t max_acceleration,
+    units::feet_per_second_squared_t max_deceleration)
+    : Loggable("DriveToPointCommand"),
+      drivetrain_(drivetrain),
+      max_speed_(max_speed),
+      max_acceleration_(max_acceleration),
+      max_deceleration_(max_deceleration),
+      target_(target) {
+  SetName("DriveToPointCommand");
+  AddRequirements({drivetrain_});
+}
+
+void DriveToPointCommand::Initialize() {
+  Log("DriveToPointCommand initialized");
+  start_point_ = drivetrain_->GetReadings().pose.position;
+}
+
+void DriveToPointCommand::Execute() {
+  DrivetrainReadings dt_readings{drivetrain_->GetReadings()};
+
+  DrivetrainAccelerationControlTarget dt_target{
+      .linear_acceleration = max_acceleration_,
+      .accel_dir = (target_.point - start_point_).angle(true),
+      .angular_velocity = 0_deg_per_s,
+  };
+
+  units::second_t t_decel =
+      ((dt_readings.pose.velocity.magnitude() - target_.velocity) /
+          max_deceleration_);
+  units::foot_t stopping_distance =
+      (dt_readings.pose.velocity.magnitude() * t_decel) -
+      ((max_deceleration_ * t_decel * t_decel) / 2.0);
+
+  if ((target_.point - start_point_).magnitude() <=
+      stopping_distance -
+          drivetrain_->GetPreferenceValue_unit_type<units::inch_t>(
+              "drive_to_subtract")) {
+    if (dt_readings.pose.velocity.magnitude() < 4_fps &&
+        target_.velocity > 1_fps) {
+      dt_target.accel_dir = dt_readings.pose.velocity.angle(true) + 180_deg;
+    } else {
+      dt_target.accel_dir =
+          (dt_readings.pose.position - start_point_).angle(true);
+    }
+    dt_target.linear_acceleration = max_deceleration_;
+  } else if (dt_readings.pose.velocity.magnitude() < max_speed_) {
+    dt_target.linear_acceleration = max_acceleration_;
+    dt_target.accel_dir =
+        (target_.point - dt_readings.pose.position).angle(true);
+  } else {
+    dt_target.linear_acceleration = 0_fps_sq;
+    dt_target.accel_dir =
+        (target_.point - dt_readings.pose.position).angle(true);
+  }
+
+  dt_target.angular_velocity = drivetrain_->ApplyBearingPID(target_.bearing);
+
+  drivetrain_->SetTarget(dt_target);
+}
+
+void DriveToPointCommand::End(bool interrupted) {
+  Log("DriveToPointCommand ended with interruption status {}", interrupted);
+  drivetrain_->SetTargetZero();
+}
+
+bool DriveToPointCommand::IsFinished() {
+  auto current_point = drivetrain_->GetReadings().pose.position;
+  return (current_point - start_point_).magnitude() >=
+         (target_.point - start_point_).magnitude();
+  // TODO: add bump sensor
+}
+
+}  // namespace frc846::robot::swerve
\ 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
index 9a3125a..2b33dd0 100644
--- a/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc
+++ b/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc
@@ -1,5 +1,7 @@
 #include "frc846/robot/swerve/drivetrain.h"
 
+#include <thread>
+
 #include "frc846/robot/swerve/control/swerve_ol_calculator.h"
 #include "frc846/robot/swerve/swerve_module.h"
 
@@ -9,22 +11,40 @@ DrivetrainSubsystem::DrivetrainSubsystem(DrivetrainConfigs configs)
     : GenericSubsystem{"SwerveDrivetrain"},
       configs_{configs},
       modules_{},
-      navX_{configs.navX_connection_mode} {
+      navX_{configs.navX_connection_mode, studica::AHRS::k200Hz} {
   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", 2.0);
+  RegisterPreference("steer_gains/_kP", 4.0);
   RegisterPreference("steer_gains/_kI", 0.0);
   RegisterPreference("steer_gains/_kD", 0.0);
   RegisterPreference("steer_gains/_kF", 0.0);
 
+  RegisterPreference("bearing_gains/_kP", 0.5);
+  RegisterPreference("bearing_gains/_kI", 0.0);
+  RegisterPreference("bearing_gains/_kD", 0.0);
+  RegisterPreference("bearing_gains/deadband", 3.0_deg_per_s);
+
+  RegisterPreference("lock_gains/_kP", 0.5);
+  RegisterPreference("lock_gains/_kI", 0.0);
+  RegisterPreference("lock_gains/_kD", 0.0);
+  RegisterPreference("lock_gains/deadband", 2_in);
+  RegisterPreference("lock_adj_rate", 0.05_in);
+
+  RegisterPreference("drive_to_subtract", 5_in);
+
   RegisterPreference("max_speed", 15_fps);
   RegisterPreference("max_omega", units::degrees_per_second_t{180});
+  RegisterPreference("max_omega_cut", units::degrees_per_second_t{40});
 
   RegisterPreference("odom_fudge_factor", 0.875);
 
+  RegisterPreference("steer_lag", 0.05_s);
+
+  RegisterPreference("rc_control_speed", 2.5_fps);
+
   odometry_.setConstants({});
   ol_calculator_.setConstants({
       .wheelbase_horizontal_dim = configs.wheelbase_horizontal_dim,
@@ -42,8 +62,8 @@ void DrivetrainSubsystem::Setup() {
     module->InitByParent();
     module->Setup();
     module->SetSteerGains(steer_gains);
-    module->ZeroWithCANcoder();
   }
+  ZeroBearing();
 }
 
 DrivetrainTarget DrivetrainSubsystem::ZeroTarget() const {
@@ -89,10 +109,49 @@ void DrivetrainSubsystem::SetCANCoderOffsets() {
   }
 }
 
+units::degrees_per_second_t DrivetrainSubsystem::ApplyBearingPID(
+    units::degree_t target_bearing) {
+  units::degree_t bearing = GetReadings().pose.bearing;
+  units::degrees_per_second_t yaw_rate = GetReadings().yaw_rate;
+
+  units::degree_t error =
+      frc846::math::CoterminalDifference(target_bearing, bearing);
+
+  Graph("bearing_pid/bearing", bearing);
+  Graph("bearing_pid/target", target_bearing);
+  Graph("bearing_pid/error", error);
+  Graph("bearing_pid/yaw_rate", yaw_rate);
+
+  frc846::control::base::MotorGains gains{
+      GetPreferenceValue_double("bearing_gains/_kP"),
+      GetPreferenceValue_double("bearing_gains/_kI"),
+      GetPreferenceValue_double("bearing_gains/_kD"), 0.0};
+
+  double raw_output =
+      gains.calculate(error.to<double>(), 0.0, yaw_rate.to<double>(), 0.0);
+
+  Graph("bearing_pid/raw_output (c.dps)", raw_output);
+
+  units::degrees_per_second_t output =
+      1_deg_per_s *
+      frc846::math::HorizontalDeadband(raw_output,
+          GetPreferenceValue_unit_type<units::degrees_per_second_t>(
+              "bearing_gains/deadband")
+              .to<double>(),
+          GetPreferenceValue_unit_type<units::degrees_per_second_t>("max_omega")
+              .to<double>());
+
+  Graph("bearing_pid/output", output);
+
+  return output;
+}
+
 DrivetrainReadings DrivetrainSubsystem::ReadFromHardware() {
   units::degree_t bearing = navX_.GetAngle() * 1_deg;
+  units::degrees_per_second_t yaw_rate = navX_.GetRate() * 1_deg_per_s;
 
   Graph("readings/bearing", bearing);
+  Graph("readings/yaw_rate", yaw_rate);
 
   frc846::math::VectorND<units::inch, 4> drive_positions{
       0_in, 0_in, 0_in, 0_in};
@@ -130,37 +189,86 @@ DrivetrainReadings DrivetrainSubsystem::ReadFromHardware() {
   Graph("readings/position_x", new_pose.position[0]);
   Graph("readings/position_y", new_pose.position[1]);
 
-  return {new_pose};
+  return {new_pose, yaw_rate};
+}
+
+frc846::math::VectorND<units::feet_per_second, 2>
+DrivetrainSubsystem::compensateForSteerLag(
+    frc846::math::VectorND<units::feet_per_second, 2> uncompensated) {
+  // units::degree_t steer_lag_compensation =
+  //     -GetPreferenceValue_unit_type<units::second_t>("steer_lag") *
+  //     GetReadings().yaw_rate;
+
+  // Graph("target/steer_lag_compensation", steer_lag_compensation);
+
+  // return uncompensated.rotate(steer_lag_compensation, true);
+  return uncompensated;
+}
+
+void DrivetrainSubsystem::WriteVelocitiesHelper(
+    frc846::math::VectorND<units::feet_per_second, 2> velocity,
+    units::degrees_per_second_t angular_velocity, bool cut_excess_steering,
+    units::feet_per_second_t speed_limit) {
+  units::degree_t bearing = GetReadings().pose.bearing;
+  auto velocity_compensated = compensateForSteerLag(velocity);
+
+  auto ol_calc_outputs = ol_calculator_.calculate({velocity_compensated,
+      angular_velocity, bearing, speed_limit, cut_excess_steering});
+
+  for (int i = 0; i < 4; i++) {
+    modules_[i]->SetTarget(SwerveModuleOLControlTarget{
+        ol_calc_outputs.drive_outputs[i], ol_calc_outputs.steer_outputs[i]});
+  }
 }
 
 void DrivetrainSubsystem::WriteToHardware(DrivetrainTarget target) {
+  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")});
+  }
+
   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, bearing,
-        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]};
-      modules_[i]->SetTarget(module_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);
+
+    WriteVelocitiesHelper(ol_target->velocity, ol_target->angular_velocity,
+        false,
+        GetPreferenceValue_unit_type<units::feet_per_second_t>("max_speed"));
   } 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
+    Graph(
+        "target/accel/linear_acceleration", accel_target->linear_acceleration);
+    Graph("target/accel/accel_dir", accel_target->accel_dir);
+    Graph("target/angular_velocity", accel_target->angular_velocity);
+
+    auto motor_specs = frc846::control::base::MotorSpecificationPresets::get(
+        configs_.module_common_config.motor_types);
+
+    units::feet_per_second_t true_max_speed =
+        motor_specs.free_speed * configs_.module_common_config.drive_reduction;
+    units::feet_per_second_t accel_buffer =
+        accel_target->linear_acceleration / configs_.max_accel * true_max_speed;
+
+    auto vel_new_target = GetReadings().pose.velocity +
+                          frc846::math::VectorND<units::feet_per_second, 2>{
+                              accel_buffer, accel_target->accel_dir, true};
+
+    units::degrees_per_second_t angular_vel = units::math::min(
+        units::math::max(accel_target->angular_velocity,
+            -GetPreferenceValue_unit_type<units::degrees_per_second_t>(
+                "max_omega_cut")),
+        GetPreferenceValue_unit_type<units::degrees_per_second_t>(
+            "max_omega_cut"));
+
+    units::feet_per_second_t speed_limit =
+        GetPreferenceValue_unit_type<units::feet_per_second_t>("max_speed");
+    if (accel_target->speed_limit >= 1_fps)
+      speed_limit = accel_target->speed_limit;
+    WriteVelocitiesHelper(vel_new_target, angular_vel, true, speed_limit);
   }
 
   for (int i = 0; i < 4; i++)
diff --git a/src/frc846/cpp/frc846/robot/swerve/swerve_module.cc b/src/frc846/cpp/frc846/robot/swerve/swerve_module.cc
index c026d9e..1217005 100644
--- a/src/frc846/cpp/frc846/robot/swerve/swerve_module.cc
+++ b/src/frc846/cpp/frc846/robot/swerve/swerve_module.cc
@@ -2,6 +2,8 @@
 
 #include <units/math.h>
 
+#include <thread>
+
 #include "frc846/math/collection.h"
 
 namespace frc846::robot::swerve {
@@ -59,11 +61,17 @@ void SwerveModuleSubsystem::Setup() {
   drive_.EnableStatusFrames(
       {frc846::control::config::StatusFrame::kPositionFrame,
           frc846::control::config::StatusFrame::kVelocityFrame});
+  drive_.OverrideStatusFramePeriod(
+      frc846::control::config::StatusFrame::kPositionFrame, 5_ms);
+  drive_.OverrideStatusFramePeriod(
+      frc846::control::config::StatusFrame::kVelocityFrame, 5_ms);
   drive_helper_.SetPosition(0_ft);
 
   steer_.Setup();
   steer_.EnableStatusFrames(
       {frc846::control::config::StatusFrame::kPositionFrame});
+  steer_.OverrideStatusFramePeriod(
+      frc846::control::config::StatusFrame::kPositionFrame, 5_ms);
 
   ZeroWithCANcoder();
 }
@@ -90,24 +98,32 @@ void SwerveModuleSubsystem::ZeroWithCANcoder() {
   constexpr int kMaxAttempts = 5;
   constexpr int kSleepTimeMs = 500;
 
+  last_rezero = 0;
+
   for (int attempts = 1; attempts <= kMaxAttempts; ++attempts) {
     Log("CANCoder zero attempt {}/{}", attempts, kMaxAttempts);
     auto position = cancoder_.GetAbsolutePosition();
 
+    units::degree_t position_zero =
+        -position.GetValue() +
+        GetPreferenceValue_unit_type<units::degree_t>("cancoder_offset_");
+
     if (position.IsAllGood()) {
-      units::degree_t position_zero =
-          -position.GetValue() +
-          GetPreferenceValue_unit_type<units::degree_t>("cancoder_offset_");
       steer_helper_.SetPosition(position_zero);
       Log("Zeroed to {}!", position_zero);
       return;
+    } else if (attempts == kMaxAttempts) {
+      Error("Unable to zero normally after {} attempts - attempting anyways",
+          kMaxAttempts);
+      steer_helper_.SetPosition(position_zero);
+      Warn("Unreliably zeroed to {}!", position_zero);
+      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() {
@@ -127,36 +143,27 @@ SwerveModuleReadings SwerveModuleSubsystem::ReadFromHardware() {
 }
 
 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);
+  Graph("target/ol_drive_target", target.drive);
+  Graph("target/ol_steer_target", target.steer);
 
-    auto [steer_dir, invert_drive] =
-        calculateSteerPosition(ol_target->steer, GetReadings().steer_pos);
+  auto [steer_dir, invert_drive] =
+      calculateSteerPosition(target.steer, GetReadings().steer_pos);
 
-    Graph("target/steer_dir", steer_dir);
-    Graph("target/invert_drive", invert_drive);
+  Graph("target/steer_dir", steer_dir);
+  Graph("target/invert_drive", invert_drive);
 
-    units::dimensionless::scalar_t cosine_comp =
-        units::math::cos(ol_target->steer - GetReadings().steer_pos);
+  units::dimensionless::scalar_t cosine_comp =
+      units::math::cos(target.steer - GetReadings().steer_pos);
 
-    Graph("target/cosine_comp", cosine_comp.to<double>());
+  Graph("target/cosine_comp", cosine_comp.to<double>());
 
-    double drive_duty_cycle = ol_target->drive / max_speed_;
+  double drive_duty_cycle = target.drive / max_speed_;
 
-    drive_helper_.WriteDC(cosine_comp * drive_duty_cycle);
+  drive_helper_.WriteDC(cosine_comp * drive_duty_cycle);
 
-    if (std::abs(drive_duty_cycle) > 0.002) {
-      steer_helper_.WritePositionOnController(steer_dir);
-    }
-  } 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");
+  if (std::abs(drive_duty_cycle) > 0.002 || last_rezero < 50) {
+    steer_helper_.WritePositionOnController(steer_dir);
+    last_rezero += 1;
   }
 }
 
diff --git a/src/frc846/cpp/frc846/robot/xbox.cc b/src/frc846/cpp/frc846/robot/xbox.cc
index 4d44f74..eed07e9 100644
--- a/src/frc846/cpp/frc846/robot/xbox.cc
+++ b/src/frc846/cpp/frc846/robot/xbox.cc
@@ -9,8 +9,8 @@ XboxReadings::XboxReadings(frc::XboxController& xbox, double trigger_threshold)
       right_stick_y(-xbox.GetRightY()),  // negated so + is up
       left_trigger(xbox.GetLeftTriggerAxis() >= trigger_threshold),
       right_trigger(xbox.GetRightTriggerAxis() >= trigger_threshold),
-      left_bumper(xbox.GetLeftBumper()),
-      right_bumper(xbox.GetRightBumper()),
+      left_bumper(xbox.GetLeftBumperButton()),
+      right_bumper(xbox.GetRightBumperButton()),
       back_button(xbox.GetBackButton()),
       start_button(xbox.GetStartButton()),
       rsb(xbox.GetRightStickButton()),
diff --git a/src/frc846/include/frc846/base/FunkyLogSystem.h b/src/frc846/include/frc846/base/FunkyLogSystem.h
index 9c9fde4..ec3ffb0 100644
--- a/src/frc846/include/frc846/base/FunkyLogSystem.h
+++ b/src/frc846/include/frc846/base/FunkyLogSystem.h
@@ -11,7 +11,6 @@
 #include <filesystem>
 #include <fstream>
 #include <initializer_list>
-#include <iostream>
 #include <mutex>
 #include <queue>
 #include <type_traits>
@@ -122,7 +121,7 @@ class FunkyLogger {
   }
 
 public:
-  FunkyLogger(std::string pname) : pname_{pname} {};
+  FunkyLogger(std::string_view pname) : pname_{pname} {};
 
   template <typename... T>
   void Log(fmt::format_string<T...> fmt, T&&... args) const {
@@ -140,4 +139,4 @@ class FunkyLogger {
   }
 };
 
-}  // namespace frc846::base
+}  // namespace frc846::base
\ No newline at end of file
diff --git a/src/frc846/include/frc846/base/Loggable.h b/src/frc846/include/frc846/base/Loggable.h
index 4c071ee..55424c4 100644
--- a/src/frc846/include/frc846/base/Loggable.h
+++ b/src/frc846/include/frc846/base/Loggable.h
@@ -8,7 +8,6 @@
 #include <filesystem>
 #include <fstream>
 #include <initializer_list>
-#include <iostream>
 #include <type_traits>
 #include <unordered_set>
 #include <variant>
@@ -64,16 +63,16 @@ class Loggable {
   template <typename U> void Graph(std::string key, U value) const {
     static_assert(units::traits::is_unit_t<U>(), "must be a unit type");
 
-    std::string fullkey = fmt::format("{} ({})", name_ + "/" + key,
-        units::abbreviation(units::make_unit<U>(0)));
-    Graph(fullkey, value.template to<double>());
+    std::string modkey = fmt::format(
+        "{} ({})", key, units::abbreviation(units::make_unit<U>(0)));
+    Graph(modkey, value.template to<double>());
   }
 
   // Creates a unit-type preference.
   template <typename U> void RegisterPreference(std::string key, U fallback) {
-    std::string fullkey = fmt::format("{} ({})", name_ + "/" + key,
-        units::abbreviation(units::make_unit<U>(0)));
-    RegisterPreference(fullkey, fallback.template to<double>());
+    std::string modkey = fmt::format(
+        "{} ({})", key, units::abbreviation(units::make_unit<U>(0)));
+    RegisterPreference(modkey, fallback.template to<double>());
   }
 
   // Creates a double preference.
@@ -90,9 +89,9 @@ class Loggable {
 
   // Returns the value of the preference for a unit-type.
   template <typename U> U GetPreferenceValue_unit_type(std::string key) {
-    std::string fullkey = fmt::format("{} ({})", name_ + "/" + key,
-        units::abbreviation(units::make_unit<U>(0)));
-    return units::make_unit<U>(GetPreferenceValue_double(fullkey));
+    std::string modkey = fmt::format(
+        "{} ({})", key, units::abbreviation(units::make_unit<U>(0)));
+    return units::make_unit<U>(GetPreferenceValue_double(modkey));
   }
 
   // Returns the value of the preference for a double.
@@ -131,7 +130,7 @@ class Loggable {
   static unsigned int GetWarnCount();
   static unsigned int GetErrorCount();
 
-  static std::string Join(std::string p, std::string n);
+  static std::string_view Join(std::string p, std::string n);
 
   static std::vector<std::string> ListKeysToPrune();
 
@@ -142,7 +141,7 @@ class Loggable {
 
   const std::string name_;
 
-  static std::unordered_set<std::string> used_preferences_;
+  static std::unordered_set<std::string_view> used_preferences_;
 
   static unsigned int warn_count_;
   static unsigned int error_count_;
@@ -150,4 +149,4 @@ class Loggable {
   frc846::base::FunkyLogger logger;
 };
 
-}  // namespace frc846::base
+}  // namespace frc846::base
\ No newline at end of file
diff --git a/src/frc846/include/frc846/base/compression.h b/src/frc846/include/frc846/base/compression.h
index e570d99..169fed4 100644
--- a/src/frc846/include/frc846/base/compression.h
+++ b/src/frc846/include/frc846/base/compression.h
@@ -1,6 +1,6 @@
 #pragma once
 
-#include <string>
+#include <string_view>
 #include <vector>
 
 namespace frc846::base {
@@ -25,6 +25,6 @@ class Compression {
   @param data input string
   @return compressed data, 6-bit binary packed into bytes.
   */
-  static std::vector<uint8_t> compress(const std::string& data);
+  static std::vector<uint8_t> compress(const std::string_view& data);
 };
 }  // namespace frc846::base
\ No newline at end of file
diff --git a/src/frc846/include/frc846/base/fserver.h b/src/frc846/include/frc846/base/fserver.h
index 3c8d6ce..54d78a7 100644
--- a/src/frc846/include/frc846/base/fserver.h
+++ b/src/frc846/include/frc846/base/fserver.h
@@ -2,11 +2,8 @@
 
 #include <chrono>
 #include <cstdint>
-#include <iostream>
 #include <mutex>
 #include <queue>
-#include <thread>
-#include <unordered_set>
 #include <vector>
 
 #ifdef _WIN32
@@ -24,7 +21,6 @@ typedef int socklen_t;
 #else
 
 #include <arpa/inet.h>
-#include <string.h>
 #include <sys/socket.h>
 #include <unistd.h>
 
@@ -45,26 +41,8 @@ class LoggingServer {
   }
 
 public:
-  LoggingServer() : messages{}, clients{} {
-#ifdef _WIN32
-    /*
-    Configuring Windows Socket API.
-    */
-
-    WSADATA wsaData;
-    int result = WSAStartup(MAKEWORD(2, 2), &wsaData);
-    if (result != 0) {
-      std::cerr << "WSAStartup failed: " << result << std::endl;
-      exit(EXIT_FAILURE);
-    }
-#endif
-  }
-
-  ~LoggingServer() {
-#ifdef _WIN32
-    WSACleanup();
-#endif
-  }
+  LoggingServer();
+  ~LoggingServer();
 
   /*
   Starts a UDP logging server on the specified port. Make sure port is FRC legal
@@ -72,80 +50,7 @@ class LoggingServer {
   watching clients.
   @param port port to start the server on
   */
-  void Start(int port) {
-    len = sizeof(cliaddr);
-
-    sockfd = socket(AF_INET, SOCK_DGRAM, 0);
-    std::cout << sockfd << std::endl;
-    if (sockfd < 0) return;
-
-    memset(&servaddr, 0, sizeof(servaddr));
-    servaddr.sin_family = AF_INET;
-    servaddr.sin_addr.s_addr = INADDR_ANY;
-    servaddr.sin_port = htons(port);
-
-    if (bind(sockfd, (const struct sockaddr *)&servaddr, sizeof(servaddr)) < 0)
-      return;
-
-    std::thread sender([&]() {
-      while (true) {
-        do {
-          std::this_thread::sleep_for(std::chrono::milliseconds(50));
-        } while (messages.empty() || clients.empty());
-
-        msg_mtx.lock();
-        auto msg = messages.front();
-        messages.pop();
-        msg_mtx.unlock();
-
-        cli_mtx.lock();
-        for (const auto &client : clients) {
-          sendto(sockfd, reinterpret_cast<const char *>(msg.data()), msg.size(),
-              0, reinterpret_cast<const struct sockaddr *>(&(client.addr)),
-              sizeof(client.addr));
-        }
-        cli_mtx.unlock();
-      }
-    });
-
-    std::thread receiver([&]() {
-      char buffer[1024];
-      std::chrono::milliseconds lastPruneTime{getTime()};
-      for (std::chrono::milliseconds t{lastPruneTime};; t = getTime()) {
-        if (recvfrom(sockfd, buffer, sizeof(buffer), 0,
-                (struct sockaddr *)&cliaddr, &len) > 0) {
-          bool exists = false;
-          cli_mtx.lock();
-          for (LoggingClient &client : clients) {
-            if (client.addr.sin_addr.s_addr == cliaddr.sin_addr.s_addr) {
-              if (client.addr.sin_port != cliaddr.sin_port) {
-                client.addr.sin_port = cliaddr.sin_port;
-              }
-              client.lastKeepAlive = getTime();
-              exists = true;
-              break;
-            }
-          }
-          if (!exists) { clients.push_back({cliaddr, getTime()}); }
-          cli_mtx.unlock();
-        }
-        if (t - lastPruneTime > std::chrono::milliseconds(500)) {
-          cli_mtx.lock();
-          for (size_t i = 0; i < clients.size(); i++) {
-            if (getTime() - clients[i].lastKeepAlive >
-                std::chrono::milliseconds(5000)) {
-              clients.erase(clients.begin() + i);
-            }
-          }
-          cli_mtx.unlock();
-          lastPruneTime = t;
-        }
-      }
-    });
-
-    sender.detach();
-    receiver.detach();
-  }
+  void Start(int port);
 
   /*
   Adds message to the sending queue. Messages will not be sent out unless server
diff --git a/src/frc846/include/frc846/control/HMCHelper.h b/src/frc846/include/frc846/control/HMCHelper.h
index 76f022f..2f1a050 100644
--- a/src/frc846/include/frc846/control/HMCHelper.h
+++ b/src/frc846/include/frc846/control/HMCHelper.h
@@ -41,7 +41,7 @@ template <typename T> class HMCHelper {
 
   void WritePosition(pos_unit position) {
     CHECK_HMC();
-    hmc_->WritePosition(position / conv_);
+    hmc_->WritePosition(position / conv_ - mark_offset);
   }
 
   void WriteVelocityOnController(vel_unit velocity) {
@@ -51,7 +51,7 @@ template <typename T> class HMCHelper {
 
   void WritePositionOnController(pos_unit position) {
     CHECK_HMC();
-    hmc_->WritePositionOnController(position / conv_);
+    hmc_->WritePositionOnController(position / conv_ - mark_offset);
   }
 
   vel_unit GetVelocity() {
@@ -61,7 +61,7 @@ template <typename T> class HMCHelper {
 
   pos_unit GetPosition() {
     CHECK_HMC();
-    return hmc_->GetPosition() * conv_;
+    return hmc_->GetPosition() * conv_ + mark_offset;
   }
 
   units::current::ampere_t GetCurrent() {
@@ -77,12 +77,13 @@ template <typename T> class HMCHelper {
   /*
   SetControllerSoftLimits()
 
-  Set software limits for forward and reverse motion. Will be managed onboard
-  the motor controller.
+  Set software limits for forward and reverse motion. Will be managed
+  onboard the motor controller.
   */
   void SetControllerSoftLimits(pos_unit forward_limit, pos_unit reverse_limit) {
     CHECK_HMC();
-    hmc_->SetControllerSoftLimits(forward_limit * conv_, reverse_limit * conv_);
+    hmc_->SetControllerSoftLimits(forward_limit * conv_ - mark_offset,
+        reverse_limit * conv_ - mark_offset);
   }
 
   /*
@@ -96,14 +97,17 @@ template <typename T> class HMCHelper {
       pos_unit forward_reduce = pos_unit(0),
       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));
+        using_limits, forward_limit - mark_offset, reverse_limit - mark_offset,
+        forward_reduce - mark_offset, reverse_reduce - mark_offset,
+        reduce_max_dc));
   }
 
 private:
   HigherMotorController* hmc_;
 
   conv_unit conv_;
+
+  pos_unit mark_offset{0};
 };
 
 }  // namespace frc846::control
\ No newline at end of file
diff --git a/src/frc846/include/frc846/control/HigherMotorController.h b/src/frc846/include/frc846/control/HigherMotorController.h
index aef5250..ca914c7 100644
--- a/src/frc846/include/frc846/control/HigherMotorController.h
+++ b/src/frc846/include/frc846/control/HigherMotorController.h
@@ -100,6 +100,19 @@ class HigherMotorController {
 
   units::volt_t GetAnalogDeviceOutput();
 
+  // void ConfigForwardLimitSwitch(
+  //     bool stop_motor, frc846::control::base::LimitSwitchDefaultState type);
+  // void ConfigReverseLimitSwitch(
+  //     bool stop_motor, frc846::control::base::LimitSwitchDefaultState type);
+
+  // bool GetForwardLimitSwitchState();
+  // bool GetReverseLimitSwitchState();
+
+  // units::volt_t GetAnalogDeviceOutput();
+
+  void OverrideStatusFramePeriod(
+      config::StatusFrame frame, units::millisecond_t period);
+
   // Verifies if the speed controller is connected and accessible
   bool VerifyConnected();
 
diff --git a/src/frc846/include/frc846/control/MotorMonkey.h b/src/frc846/include/frc846/control/MotorMonkey.h
index 8529e5e..333eb49 100644
--- a/src/frc846/include/frc846/control/MotorMonkey.h
+++ b/src/frc846/include/frc846/control/MotorMonkey.h
@@ -14,6 +14,7 @@
 #include "frc846/control/config/construction_params.h"
 #include "frc846/control/config/status_frames.h"
 #include "frc846/control/hardware/TalonFX_interm.h"
+#include "frc846/math/DoubleSyncBuffer.h"
 
 #define CONTROLLER_REGISTRY_SIZE 64
 
@@ -27,20 +28,28 @@ CAN utilization as well as power.
 */
 class MotorMonkey {
 public:
+  /*
+  Setup()
+
+  Sets up the preferences used by MotorMonkey.
+  */
+  static void Setup();
+
   /*
   Tick()
 
   Updates all motor controllers. Should be called each loop.
   */
-  static void Tick(units::ampere_t max_draw);
+  static void Tick(bool disabled);
 
   /*
   WriteMessages()
 
   Writes all messages in the message queue to the motor controllers. Also,
   dynamically manages current draw and drops redundant messages.
+  Returns predicted total current draw.
   */
-  static void WriteMessages(units::ampere_t max_draw);
+  static units::ampere_t WriteMessages(units::ampere_t max_draw);
 
   /*
   ConstructController()
@@ -59,6 +68,9 @@ class MotorMonkey {
   static void EnableStatusFrames(
       size_t slot_id, std::vector<frc846::control::config::StatusFrame> frames);
 
+  static void OverrideStatusFramePeriod(size_t slot_id,
+      frc846::control::config::StatusFrame frame, units::millisecond_t period);
+
   /*
   GetBatteryVoltage()
 
@@ -116,12 +128,14 @@ class MotorMonkey {
   static void SetSoftLimits(size_t slot_id, units::radian_t forward_limit,
       units::radian_t reverse_limit);
 
-  static std::string parseError(
+  static std::string_view parseError(
       frc846::control::hardware::ControllerErrorCodes err);
 
   static bool VerifyConnected();
 
 private:
+  static void RecalculateMaxDraw();
+
   static frc846::base::Loggable loggable_;
 
   static size_t slot_counter_;
@@ -140,6 +154,11 @@ class MotorMonkey {
       circuit_resistance_registry[CONTROLLER_REGISTRY_SIZE];
 
   static units::volt_t battery_voltage;
+  static units::volt_t last_disabled_voltage;
+
+  static frc846::math::DoubleSyncBuffer sync_buffer;
+
+  static units::ampere_t max_draw_;
 
   struct MotorMessage {
     enum class Type { DC, Position, Velocity };
@@ -149,6 +168,8 @@ class MotorMonkey {
   };
 
   static std::queue<MotorMessage> control_messages;
+
+  static int num_loops_last_brown;
 };
 
 }  // namespace frc846::control
\ No newline at end of file
diff --git a/src/frc846/include/frc846/control/base/motor_specs.h b/src/frc846/include/frc846/control/base/motor_specs.h
index 34f3271..0e4e8f8 100644
--- a/src/frc846/include/frc846/control/base/motor_specs.h
+++ b/src/frc846/include/frc846/control/base/motor_specs.h
@@ -25,6 +25,7 @@ struct MotorSpecs {
   units::ampere_t stall_current;
   units::ampere_t free_current;
   units::newton_meter_t stall_torque;
+  double friction_loss = 0.01;
 };
 
 /*
diff --git a/src/frc846/include/frc846/control/config/construction_params.h b/src/frc846/include/frc846/control/config/construction_params.h
index ddb8656..44a07c7 100644
--- a/src/frc846/include/frc846/control/config/construction_params.h
+++ b/src/frc846/include/frc846/control/config/construction_params.h
@@ -44,7 +44,7 @@ struct MotorConstructionParameters {
 
   frc846::wpilib::unit_kg_m_sq rotational_inertia;
 
-  std::string bus = "";
+  std::string_view bus = "";
 
   units::millisecond_t max_wait_time = 20_ms;
 };
diff --git a/src/frc846/include/frc846/control/hardware/IntermediateController.h b/src/frc846/include/frc846/control/hardware/IntermediateController.h
index 060773e..25467e3 100644
--- a/src/frc846/include/frc846/control/hardware/IntermediateController.h
+++ b/src/frc846/include/frc846/control/hardware/IntermediateController.h
@@ -52,6 +52,9 @@ class IntermediateController {
 
   virtual void EnableStatusFrames(
       std::vector<frc846::control::config::StatusFrame> frames) = 0;
+  virtual void OverrideStatusFramePeriod(
+      frc846::control::config::StatusFrame frame,
+      units::millisecond_t period) = 0;
 
   virtual bool IsDuplicateControlMessage(double duty_cycle) = 0;
   virtual bool IsDuplicateControlMessage(
@@ -77,7 +80,7 @@ class IntermediateController {
 
   virtual ControllerErrorCodes GetLastErrorCode() = 0;
 
-  virtual bool VerifyConnected() = 0;  // changed
+  virtual bool VerifyConnected() = 0;
 };
 
 }  // namespace frc846::control::hardware
\ No newline at end of file
diff --git a/src/frc846/include/frc846/control/hardware/SparkMXFX_interm.h b/src/frc846/include/frc846/control/hardware/SparkMXFX_interm.h
index ea27fc3..1e96ee9 100644
--- a/src/frc846/include/frc846/control/hardware/SparkMXFX_interm.h
+++ b/src/frc846/include/frc846/control/hardware/SparkMXFX_interm.h
@@ -3,7 +3,6 @@
 #include <rev/SparkFlex.h>
 #include <rev/SparkMax.h>
 
-#include <algorithm>
 #include <variant>
 
 #include "IntermediateController.h"
@@ -115,11 +114,14 @@ class SparkMXFX_interm : public IntermediateController {
 
   Enables specified status frames which are periodically updated
 
-  @param frames A the status frames wanted to be enabled.
+  @param frames the frames to be enabled
   */
   void EnableStatusFrames(
       std::vector<frc846::control::config::StatusFrame> frames) override;
 
+  void OverrideStatusFramePeriod(frc846::control::config::StatusFrame frame,
+      units::millisecond_t period) override;
+
   /*
   IsDuplicateControlMessage()
 
diff --git a/src/frc846/include/frc846/control/hardware/TalonFX_interm.h b/src/frc846/include/frc846/control/hardware/TalonFX_interm.h
index 06afe4f..72b1f18 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 = "",
+  TalonFX_interm(int can_id, std::string_view bus = "",
       units::millisecond_t max_wait_time = 20_ms);
   /*
   Tick()
@@ -112,11 +112,14 @@ class TalonFX_interm : public IntermediateController {
 
   Enables specified status frames which are periodically updated
 
-  @param frames A the status frames wanted to be enabled.
+  @param frames the frames to be enabled
   */
   void EnableStatusFrames(
       std::vector<frc846::control::config::StatusFrame> frames) override;
 
+  void OverrideStatusFramePeriod(frc846::control::config::StatusFrame frame,
+      units::millisecond_t period) override;
+
   /*
   IsDuplicateControlMessage()
 
diff --git a/src/frc846/include/frc846/control/hardware/simulation/MCSimulator.h b/src/frc846/include/frc846/control/hardware/simulation/MCSimulator.h
index 2a68c4e..baea102 100644
--- a/src/frc846/include/frc846/control/hardware/simulation/MCSimulator.h
+++ b/src/frc846/include/frc846/control/hardware/simulation/MCSimulator.h
@@ -95,6 +95,11 @@ class MCSimulator : public frc846::control::hardware::IntermediateController {
 
   void EnableStatusFrames(
       std::vector<frc846::control::config::StatusFrame> frames) override;
+  void OverrideStatusFramePeriod(frc846::control::config::StatusFrame frame,
+      units::millisecond_t period) override {
+    (void)frame;
+    (void)period;
+  };
 
   bool IsDuplicateControlMessage(double duty_cycle) override;
   bool IsDuplicateControlMessage(units::radians_per_second_t velocity) override;
diff --git a/src/frc846/include/frc846/math/Differentiator.h b/src/frc846/include/frc846/math/Differentiator.h
new file mode 100644
index 0000000..b1dac81
--- /dev/null
+++ b/src/frc846/include/frc846/math/Differentiator.h
@@ -0,0 +1,28 @@
+#pragma once
+
+#include <chrono>
+
+namespace frc846::math {
+
+/*
+Differentiator class
+
+A class that differentiates a given value with respect to time
+*/
+
+class Differentiator {
+public:
+  Differentiator();
+
+  void Reset();
+
+  double Calculate(double value);
+
+  double GetRate() const;
+
+private:
+  double last_value_;
+  double last_timestamp_;
+  double rate_;
+};
+}  // namespace frc846::math
diff --git a/src/frc846/include/frc846/math/DoubleSyncBuffer.h b/src/frc846/include/frc846/math/DoubleSyncBuffer.h
new file mode 100644
index 0000000..c70a07f
--- /dev/null
+++ b/src/frc846/include/frc846/math/DoubleSyncBuffer.h
@@ -0,0 +1,52 @@
+#pragma once
+
+#include <stddef.h>
+
+#include <vector>
+
+namespace frc846::math {
+
+/*
+DoubleSyncBuffer
+
+A class that contains two rolling buffers of doubles and syncs them.
+*/
+class DoubleSyncBuffer {
+public:
+  /*
+  DoubleSyncBuffer()
+
+  Constructs a DoubleSyncBuffer with the given size.
+  */
+  DoubleSyncBuffer(size_t sz = 50U, int max_sync_diff = 15);
+
+  /*
+  Add()
+
+  Adds a value to each contained buffer. The values may be unsynced. Sync() will
+  be called automatically.
+  The second signal may be time delayed.
+  */
+  void Add(double val1, double val2);
+
+  // Computes the sync difference between the two buffers
+  void Sync();
+
+  bool IsValid();
+
+  // Returns the computed sync difference between each container buffer
+  int GetSyncDiff() { return sync_diff_; }
+
+  std::pair<double, double> GetTrough();
+
+private:
+  std::vector<double> m_buffer_1;
+  std::vector<double> m_buffer_2;
+
+  size_t def_size;
+  int max_sync_diff_;
+
+  int sync_diff_;
+};
+
+}  // namespace frc846::math
\ No newline at end of file
diff --git a/src/frc846/include/frc846/math/RampRateLimiter.h b/src/frc846/include/frc846/math/RampRateLimiter.h
new file mode 100644
index 0000000..503453b
--- /dev/null
+++ b/src/frc846/include/frc846/math/RampRateLimiter.h
@@ -0,0 +1,23 @@
+#pragma once
+
+#include <chrono>
+
+namespace frc846::math {
+
+/*
+RampRateLimiter
+
+A class that limits the rate of change of a value.
+*/
+class RampRateLimiter {
+public:
+  RampRateLimiter();
+
+  double limit(double value, double rateLimit);
+
+private:
+  double m_lastValue = 0.0;
+  std::chrono::milliseconds m_lastTime;
+};
+
+}
\ No newline at end of file
diff --git a/src/frc846/include/frc846/math/fieldpoints.h b/src/frc846/include/frc846/math/fieldpoints.h
index 59d8aeb..0b9befe 100644
--- a/src/frc846/include/frc846/math/fieldpoints.h
+++ b/src/frc846/include/frc846/math/fieldpoints.h
@@ -12,33 +12,41 @@
 namespace frc846::math {
 
 struct FieldPoint {
-  VectorND<units::foot, 2> point;
+  Vector2D point;
   units::degree_t bearing;
 
-  VectorND<units::feet_per_second, 2> velocity;
+  units::feet_per_second_t velocity;
 
   // Returns a FieldPoint that is 'mirrored' across the field
   FieldPoint mirror(bool shouldMirror = true) const {
     if (shouldMirror) {
       return FieldPoint{{field_size_x - point[0], field_size_y - point[1]},
-          180_deg + bearing, {velocity[0], velocity[1]}};
+          180_deg + bearing, velocity};
     }
-    return FieldPoint{
-        {point[0], point[1]}, bearing, {velocity[0], velocity[1]}};
+    return FieldPoint{{point[0], point[1]}, bearing, velocity};
   }
 
-  // Returns a FieldPoint that is 'mirrored' across the centerline of the field
+  // Returns a FieldPoint that is mirrored across the centerline of the field
   FieldPoint mirrorOnlyY(bool shouldMirror = true) const {
     if (shouldMirror) {
-      return FieldPoint{{point[0], field_size_y - point[1]}, 180_deg - bearing,
-          {velocity[0], velocity[1]}};
+      return FieldPoint{
+          {point[0], field_size_y - point[1]}, 180_deg - bearing, velocity};
+    }
+    return mirror(false);
+  }
+
+  // Returns a FieldPoint that is mirrored along the length-axis of the field
+  FieldPoint mirrorOnlyX(bool shouldMirror = true) const {
+    if (shouldMirror) {
+      return FieldPoint{
+          {field_size_x - point[0], point[1]}, -bearing, velocity};
     }
     return mirror(false);
   }
 
 private:
-  static constexpr units::inch_t field_size_y = 651.25_in;
-  static constexpr units::inch_t field_size_x = 315.5_in;
+  static constexpr units::inch_t field_size_y = 690.875_in;
+  static constexpr units::inch_t field_size_x = 317_in;
 };
 
 }  // namespace frc846::math
\ No newline at end of file
diff --git a/src/frc846/include/frc846/math/vectors.h b/src/frc846/include/frc846/math/vectors.h
index 416c455..f89b15d 100644
--- a/src/frc846/include/frc846/math/vectors.h
+++ b/src/frc846/include/frc846/math/vectors.h
@@ -147,18 +147,22 @@ template <typename UT, size_t N> class VectorND {
         data[0] * units::math::sin(angle) + data[1] * units::math::cos(angle)};
   }
 
-  // Returns the dot product of this vector and another
-  T dot(const VectorND<UT, N>& other) const {
-    T result = T{};
+  // Returns the dot product of this vector and another with the units of the
+  // other vector
+  template <typename UT2>
+  units::unit_t<UT2> dot(const VectorND<UT2, N>& other) const {
+    units::unit_t<UT2> result{};
     for (size_t i = 0; i < N; ++i) {
-      result += data[i] * other[i].template to<double>();
+      result += data[i].template to<double>() * other[i];
     }
     return result;
   }
 
   // Returns the cross product of this vector and another
   // Cross product is only defined for 3D vectors
-  VectorND<UT, N> cross(const VectorND<UT, N>& other) const {
+  template <typename UT2>
+  VectorND<units::compound_unit<UT, UT2>, N> cross(
+      const VectorND<UT2, 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],
@@ -180,13 +184,18 @@ template <typename UT, size_t N> class VectorND {
   }
 
   // Projects this vector onto another and returns
-  VectorND<UT, N> projectOntoAnother(const VectorND<UT, N>& other) const {
+  template <typename UT2>
+  VectorND<UT, N> projectOntoAnother(const VectorND<UT2, N>& other) const {
     return other.projectOntoThis(*this);
   }
 
   // Projects another vector onto this and returns
-  VectorND<UT, N> projectOntoThis(const VectorND<UT, N>& other) const {
-    return unit() * dot(other).template to<double>();
+  template <typename UT2>
+  VectorND<UT2, N> projectOntoThis(const VectorND<UT2, N>& other) const {
+    assert(N == 2 && "Projection is only defined for 2D vectors.");
+    double unit_x = unit()[0].template to<double>();
+    double unit_y = unit()[1].template to<double>();
+    return {unit_x * dot(other), unit_y * dot(other)};
   }
 
   // Returns the angle of this vector
@@ -197,12 +206,16 @@ template <typename UT, size_t N> class VectorND {
     if (angleIsBearing) { return units::math::atan2(data[0], data[1]); }
     try {
       return units::math::atan2(data[1], data[0]);
-    } catch (std::exception& exc) { return 0_deg; }
+    } catch (std::exception& exc) {
+      (void)exc;
+      return 0_deg;
+    }
   }
 
   // Returns the angle between this vector and another
+  template <typename UT2>
   units::degree_t angleTo(
-      const VectorND<UT, N>& other, bool angleIsBearing = false) const {
+      const VectorND<UT2, N>& other, bool angleIsBearing = false) const {
     return other.angle(angleIsBearing) - angle(angleIsBearing);
   }
 
diff --git a/src/frc846/include/frc846/robot/GenericCommand.h b/src/frc846/include/frc846/robot/GenericCommand.h
index c3b0ab8..b7f8960 100644
--- a/src/frc846/include/frc846/robot/GenericCommand.h
+++ b/src/frc846/include/frc846/robot/GenericCommand.h
@@ -55,9 +55,9 @@ class GenericCommand : public frc2::CommandHelper<frc2::Command, Subclass>,
 
     const auto elapsed_time = end_time - start_time;
 
-    if (elapsed_time > 1000_us) {
+    if (elapsed_time > 3000_us) {
       Warn("Command {} periodic overrun. Took {} ms.", name(),
-          elapsed_time / 1000_us);
+          elapsed_time / 3000_us);
     }
 
     avg_periodic_time_ =
diff --git a/src/frc846/include/frc846/robot/GenericRobot.h b/src/frc846/include/frc846/robot/GenericRobot.h
index bd93a01..581d828 100644
--- a/src/frc846/include/frc846/robot/GenericRobot.h
+++ b/src/frc846/include/frc846/robot/GenericRobot.h
@@ -35,7 +35,7 @@ class GenericRobot : public frc::RobotBase, public frc846::base::Loggable {
 
   void VerifyHardware();
 
-  void AddAuto(std::string name, frc2::Command* command);
+  void AddAuto(std::string_view name, frc2::Command* command);
 
 private:
   hal::Handle<HAL_NotifierHandle> notifier_;
@@ -47,8 +47,8 @@ class GenericRobot : public frc::RobotBase, public frc846::base::Loggable {
   GenericRobotContainer* generic_robot_container_;
 
   frc2::Command* auto_command_ = nullptr;
-  frc::SendableChooser<std::string> auto_chooser_;
-  std::unordered_map<std::string, frc2::Command*> autos_;
+  frc::SendableChooser<std::string_view> auto_chooser_;
+  std::unordered_map<std::string_view, frc2::Command*> autos_;
 };
 
 }  // namespace frc846::robot
diff --git a/src/frc846/include/frc846/robot/GenericRobotContainer.h b/src/frc846/include/frc846/robot/GenericRobotContainer.h
index e5b22d9..b5df8f3 100644
--- a/src/frc846/include/frc846/robot/GenericRobotContainer.h
+++ b/src/frc846/include/frc846/robot/GenericRobotContainer.h
@@ -8,21 +8,67 @@ class GenericRobotContainer : public frc846::base::Loggable {
 public:
   GenericRobotContainer() : frc846::base::Loggable{"robot_container"} {}
 
-  void RegisterSubsystems(
-      std::vector<frc846::robot::SubsystemBase*> subsystems) {
-    all_subsystems_ = subsystems;
+  void RegisterSubsystemGroupA(
+      std::initializer_list<std::pair<frc846::robot::SubsystemBase*, bool>>
+          subsystems) {
+    for (auto& [subsystem, init] : subsystems) {
+      if (init) {
+        subsystem->Init();
+        group_a_subsystems_.push_back(subsystem);
+        all_subsystems_.push_back(subsystem);
+      }
+    }
+  }
+
+  void RegisterSubsystemGroupB(
+      std::initializer_list<std::pair<frc846::robot::SubsystemBase*, bool>>
+          subsystems) {
+    for (auto& [subsystem, init] : subsystems) {
+      if (init) {
+        subsystem->Init();
+        group_b_subsystems_.push_back(subsystem);
+        all_subsystems_.push_back(subsystem);
+      }
+    }
+  }
+
+  void RegisterSubsystemGroupAB(
+      std::initializer_list<std::pair<frc846::robot::SubsystemBase*, bool>>
+          subsystems) {
+    for (auto& [subsystem, init] : subsystems) {
+      if (init) {
+        subsystem->Init();
+        group_a_subsystems_.push_back(subsystem);
+        group_b_subsystems_.push_back(subsystem);
+        all_subsystems_.push_back(subsystem);
+      }
+    }
   }
 
   void UpdateReadings() {
-    for (auto subsystem : all_subsystems_) {
-      subsystem->UpdateReadings();
+    if (read_counter % 2 == 0) {
+      for (auto subsystem : group_a_subsystems_) {
+        subsystem->UpdateReadings();
+      }
+    } else {
+      for (auto subsystem : group_b_subsystems_) {
+        subsystem->UpdateReadings();
+      }
     }
+    read_counter++;
   }
 
   void UpdateHardware() {
-    for (auto subsystem : all_subsystems_) {
-      subsystem->UpdateHardware();
+    if (write_counter % 2 == 0) {
+      for (auto subsystem : group_a_subsystems_) {
+        subsystem->UpdateHardware();
+      }
+    } else {
+      for (auto subsystem : group_b_subsystems_) {
+        subsystem->UpdateHardware();
+      }
     }
+    write_counter++;
   }
 
   void Setup() {
@@ -45,6 +91,11 @@ class GenericRobotContainer : public frc846::base::Loggable {
 
 private:
   std::vector<frc846::robot::SubsystemBase*> all_subsystems_{};
+  std::vector<frc846::robot::SubsystemBase*> group_a_subsystems_{};
+  std::vector<frc846::robot::SubsystemBase*> group_b_subsystems_{};
+
+  unsigned int read_counter;
+  unsigned int write_counter;
 };
 
 }  // namespace frc846::robot
diff --git a/src/frc846/include/frc846/robot/GenericSubsystem.h b/src/frc846/include/frc846/robot/GenericSubsystem.h
index 302cb14..8d617c6 100644
--- a/src/frc846/include/frc846/robot/GenericSubsystem.h
+++ b/src/frc846/include/frc846/robot/GenericSubsystem.h
@@ -21,6 +21,8 @@ class SubsystemBase : public frc846::base::Loggable {
 
   virtual ~SubsystemBase() = default;
 
+  virtual void Init() = 0;
+
   virtual void Setup() = 0;
 
   virtual void UpdateReadings() = 0;
@@ -49,7 +51,7 @@ class GenericSubsystem : public frc2::SubsystemBase, public SubsystemBase {
   virtual ~GenericSubsystem() { Warn("Destroying subsystem"); };
 
   // Initializer function for RobotContainer use only.
-  void Init() {
+  void Init() override final {
     SetName(name());
     Log("Initializing subsystem");
     init_ = true;
diff --git a/src/frc846/include/frc846/robot/swerve/aim_command.h b/src/frc846/include/frc846/robot/swerve/aim_command.h
new file mode 100644
index 0000000..a00436b
--- /dev/null
+++ b/src/frc846/include/frc846/robot/swerve/aim_command.h
@@ -0,0 +1,31 @@
+#pragma once
+
+#include <frc2/command/Command.h>
+#include <frc2/command/CommandHelper.h>
+
+#include "frc846/base/Loggable.h"
+#include "frc846/robot/swerve/drivetrain.h"
+
+namespace frc846::robot::swerve {
+
+class AimCommand : public frc2::CommandHelper<frc2::Command, AimCommand>,
+                   public frc846::base::Loggable {
+public:
+  AimCommand(frc846::robot::swerve::DrivetrainSubsystem* drivetrain,
+      units::degree_t target);
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  void End(bool interrupted) override;
+
+  bool IsFinished() override;
+
+private:
+  frc846::robot::swerve::DrivetrainSubsystem* drivetrain_;
+
+  units::degree_t target_;
+};
+
+}  // namespace frc846::robot::swerve
\ No newline at end of file
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
index 29c490a..97c767e 100644
--- a/src/frc846/include/frc846/robot/swerve/control/swerve_ol_calculator.h
+++ b/src/frc846/include/frc846/robot/swerve/control/swerve_ol_calculator.h
@@ -15,6 +15,8 @@ namespace frc846::robot::swerve::control {
 struct SwerveOpenLoopCalculatorConstants {
   units::inch_t wheelbase_horizontal_dim;
   units::inch_t wheelbase_forward_dim;
+
+  units::radians_per_second_t rotation_iter_dec = 6_rad_per_s;
 };
 
 struct SwerveOpenLoopCalculatorInputs {
@@ -22,6 +24,7 @@ struct SwerveOpenLoopCalculatorInputs {
   units::degrees_per_second_t rotation_target;
   units::degree_t bearing;
   units::feet_per_second_t max_speed;
+  bool cut_excess_steering;
 };
 
 struct SwerveOpenLoopCalculatorOutput {
diff --git a/src/frc846/include/frc846/robot/swerve/drive_to_point_command.h b/src/frc846/include/frc846/robot/swerve/drive_to_point_command.h
new file mode 100644
index 0000000..8a536ce
--- /dev/null
+++ b/src/frc846/include/frc846/robot/swerve/drive_to_point_command.h
@@ -0,0 +1,40 @@
+#pragma once
+
+#include <frc2/command/Command.h>
+#include <frc2/command/CommandHelper.h>
+
+#include "frc846/base/Loggable.h"
+#include "frc846/math/fieldpoints.h"
+#include "frc846/robot/swerve/drivetrain.h"
+
+namespace frc846::robot::swerve {
+
+class DriveToPointCommand
+    : public frc2::CommandHelper<frc2::Command, DriveToPointCommand>,
+      public frc846::base::Loggable {
+public:
+  DriveToPointCommand(frc846::robot::swerve::DrivetrainSubsystem* drivetrain,
+      frc846::math::FieldPoint target, units::feet_per_second_t max_speed,
+      units::feet_per_second_squared_t max_acceleration,
+      units::feet_per_second_squared_t max_deceleration);
+
+  void Initialize() override;
+
+  void Execute() override;
+
+  void End(bool interrupted) override;
+
+  bool IsFinished() override;
+
+private:
+  frc846::robot::swerve::DrivetrainSubsystem* drivetrain_;
+
+  units::feet_per_second_t max_speed_;
+  units::feet_per_second_squared_t max_acceleration_;
+  units::feet_per_second_squared_t max_deceleration_;
+
+  frc846::math::FieldPoint target_;
+  frc846::math::Vector2D start_point_;
+};
+
+}  // namespace frc846::robot::swerve
\ No newline at end of file
diff --git a/src/frc846/include/frc846/robot/swerve/drivetrain.h b/src/frc846/include/frc846/robot/swerve/drivetrain.h
index 1449ac0..a4541ca 100644
--- a/src/frc846/include/frc846/robot/swerve/drivetrain.h
+++ b/src/frc846/include/frc846/robot/swerve/drivetrain.h
@@ -26,11 +26,12 @@ struct DrivetrainConfigs {
   units::inch_t wheelbase_horizontal_dim;
   units::inch_t wheelbase_forward_dim;
 
-  units::feet_per_second_t max_speed;
+  units::feet_per_second_squared_t max_accel;
 };
 
 struct DrivetrainReadings {
   frc846::robot::swerve::odometry::SwervePose pose;
+  units::degrees_per_second_t yaw_rate;
 };
 
 // Open-loop control, for use during teleop
@@ -41,8 +42,10 @@ struct DrivetrainOLControlTarget {
 
 // 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;
+  units::feet_per_second_squared_t linear_acceleration;
+  units::degree_t accel_dir;
+  units::degrees_per_second_t angular_velocity;
+  units::feet_per_second_t speed_limit = -1_fps;
 };
 
 using DrivetrainTarget = std::variant<DrivetrainOLControlTarget,
@@ -69,9 +72,18 @@ class DrivetrainSubsystem
 
   void SetCANCoderOffsets();
 
+  units::degrees_per_second_t ApplyBearingPID(units::degree_t target_bearing);
+
 private:
   DrivetrainReadings ReadFromHardware() override;
 
+  frc846::math::VectorND<units::feet_per_second, 2> compensateForSteerLag(
+      frc846::math::VectorND<units::feet_per_second, 2> uncompensated);
+
+  void WriteVelocitiesHelper(
+      frc846::math::VectorND<units::feet_per_second, 2> velocity,
+      units::degrees_per_second_t angular_velocity, bool cut_excess_steering,
+      units::feet_per_second_t speed_limit);
   void WriteToHardware(DrivetrainTarget target) override;
 
   DrivetrainConfigs configs_;
diff --git a/src/frc846/include/frc846/robot/swerve/swerve_module.h b/src/frc846/include/frc846/robot/swerve/swerve_module.h
index 1125343..5143ec6 100644
--- a/src/frc846/include/frc846/robot/swerve/swerve_module.h
+++ b/src/frc846/include/frc846/robot/swerve/swerve_module.h
@@ -20,20 +20,13 @@ struct SwerveModuleReadings {
   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>;
+using SwerveModuleTarget = SwerveModuleOLControlTarget;
 
 struct SwerveModuleUniqueConfig {
   std::string loc;
@@ -57,7 +50,7 @@ struct SwerveModuleCommonConfig {
   steer_conv_unit steer_reduction;
   drive_conv_unit drive_reduction;
 
-  std::string bus = "";
+  std::string_view bus = "";
 };
 
 /*
@@ -101,6 +94,8 @@ class SwerveModuleSubsystem
   void SetSteerGains(frc846::control::base::MotorGains gains);
 
 private:
+  int last_rezero = 101;
+
   /*
   getMotorParams()
 
diff --git a/src/y2025/cpp/FunkyRobot.cc b/src/y2025/cpp/FunkyRobot.cc
index a49574b..e4472cb 100644
--- a/src/y2025/cpp/FunkyRobot.cc
+++ b/src/y2025/cpp/FunkyRobot.cc
@@ -10,6 +10,7 @@
 #include <frc2/command/button/Trigger.h>
 #include <hal/Notifier.h>
 
+#include "calculators/AntiTippingCalculator.h"
 #include "commands/teleop/drive_command.h"
 #include "commands/teleop/leds_command.h"
 #include "control_triggers.h"
@@ -56,7 +57,16 @@ void FunkyRobot::InitTeleop() {
   ControlTriggerInitializer::InitTeleopTriggers(container_);
 }
 
-void FunkyRobot::OnPeriodic() {}
+void FunkyRobot::OnPeriodic() {
+  // TODO: plug real heights into AntiTippingCalculator
+  AntiTippingCalculator::SetTelescopeHeight(36_in);
+  AntiTippingCalculator::SetElevatorHeight(45_in);
+
+  auto cg = AntiTippingCalculator::CalculateRobotCG();
+  Graph("robot_cg_x", cg[0]);
+  Graph("robot_cg_y", cg[1]);
+  Graph("robot_cg_z", cg[2]);
+}
 
 void FunkyRobot::InitTest() {}
 
diff --git a/src/y2025/cpp/autos/ActionMaker.cc b/src/y2025/cpp/autos/ActionMaker.cc
index 67caca6..a15fe2d 100644
--- a/src/y2025/cpp/autos/ActionMaker.cc
+++ b/src/y2025/cpp/autos/ActionMaker.cc
@@ -1,7 +1,7 @@
 #include "autos/ActionMaker.h"
 
 std::unique_ptr<frc2::Command> ActionMaker::GetAction(
-    std::string name, RobotContainer& container) {
+    std::string_view name, RobotContainer& container) {
   // TODO: Create
   return nullptr;
 }
\ No newline at end of file
diff --git a/src/y2025/cpp/calculators/AntiTippingCalculator.cc b/src/y2025/cpp/calculators/AntiTippingCalculator.cc
new file mode 100644
index 0000000..919b04f
--- /dev/null
+++ b/src/y2025/cpp/calculators/AntiTippingCalculator.cc
@@ -0,0 +1,102 @@
+#include "calculators/AntiTippingCalculator.h"
+
+#include <units/force.h>
+#include <units/torque.h>
+
+#include "frc846/math/constants.h"
+#include "subsystems/robot_constants.h"
+
+frc846::math::Vector3D AntiTippingCalculator::elev_cg_position_{
+    robot_constants::elevator::pos_x, robot_constants::elevator::pos_y,
+    robot_constants::base::height};
+frc846::math::Vector3D AntiTippingCalculator::tele_cg_position_{
+    robot_constants::telescope::pos_x, robot_constants::telescope::pos_y,
+    robot_constants::base::height};
+
+void AntiTippingCalculator::SetElevatorHeight(units::inch_t height) {
+  if (height > robot_constants::elevator::min_height_off_base) {
+    elev_cg_position_[2] = height / 2.0;
+  } else {
+    elev_cg_position_[2] =
+        ((robot_constants::elevator::elevator_weight *
+             robot_constants::elevator::min_height_off_base / 2.0) +
+            (robot_constants::elevator::end_effector_weight * height)) /
+        (robot_constants::elevator::elevator_weight +
+            robot_constants::elevator::end_effector_weight);
+  }
+}
+
+void AntiTippingCalculator::SetTelescopeHeight(units::inch_t height) {
+  tele_cg_position_[2] = height / 2.0;
+}
+
+frc846::math::Vector3D AntiTippingCalculator::CalculateRobotCG() {
+  double total_weight_scalar = robot_constants::total_weight.to<double>();
+
+  frc846::math::Vector3D weighted_sum =
+      (elev_cg_position_ *
+              robot_constants::elevator::total_weight.to<double>() +
+          tele_cg_position_ *
+              robot_constants::telescope::total_weight.to<double>() +
+          frc846::math::Vector3D{0_in, 0_in, robot_constants::base::height} *
+              robot_constants::base::weight.to<double>());
+
+  return weighted_sum / total_weight_scalar;
+}
+
+frc846::math::VectorND<units::feet_per_second_squared, 2>
+AntiTippingCalculator::LimitAcceleration(
+    frc846::math::VectorND<units::feet_per_second, 2> vel_vec,
+    units::degree_t bearing) {
+  if (units::math::abs(vel_vec[0]) < 0.05_fps) vel_vec[0] = 0.05_fps;
+  if (units::math::abs(vel_vec[1]) < 0.05_fps) vel_vec[1] = 0.05_fps;
+
+  frc846::math::VectorND<units::feet_per_second, 2> v_neg_dir =
+      vel_vec.rotate(180_deg, true).unit();
+
+  frc846::math::Vector3D robot_cg = CalculateRobotCG();
+
+  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
+  };
+
+  frc846::math::Vector2D wheel_vecs[4];
+
+  for (size_t i = 0; i < 4; i++) {
+    wheel_vecs[i] =
+        frc846::math::Vector2D{
+            kModuleLocationSigns[i].first * robot_constants::base::wheelbase_x,
+            kModuleLocationSigns[i].second * robot_constants::base::wheelbase_y}
+            .rotate(bearing, true);
+  }
+
+  size_t closest_wheel_vec = 0U;
+  units::degree_t closest_angle = 360_deg;
+
+  for (size_t i = 0; i < 4; i++) {
+    units::degree_t angle = units::math::abs(v_neg_dir.angleTo(wheel_vecs[i]));
+    if (angle < closest_angle) {
+      closest_angle = angle;
+      closest_wheel_vec = i;
+    }
+  }
+
+  frc846::math::Vector2D closest_wheel = wheel_vecs[closest_wheel_vec];
+
+  frc846::math::Vector2D effective_wheel_vec =
+      v_neg_dir.projectOntoThis<units::inch>(closest_wheel);
+  frc846::math::Vector3D effective_wheel_vec_3d{
+      effective_wheel_vec[0], effective_wheel_vec[1], 0_in};
+
+  frc846::math::Vector3D r_vec = robot_cg - effective_wheel_vec_3d;
+
+  units::feet_per_second_squared_t perm_accel_x =
+      frc846::math::constants::physics::g * r_vec[0] / r_vec[2];
+  units::feet_per_second_squared_t perm_accel_y =
+      frc846::math::constants::physics::g * r_vec[1] / r_vec[2];
+
+  return {perm_accel_x, perm_accel_y};
+}
\ 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
index 980ff7f..e157f81 100644
--- a/src/y2025/cpp/commands/teleop/drive_command.cc
+++ b/src/y2025/cpp/commands/teleop/drive_command.cc
@@ -1,5 +1,7 @@
 #include "commands/teleop/drive_command.h"
 
+#include "calculators/AntiTippingCalculator.h"
+
 DriveCommand::DriveCommand(RobotContainer &container)
     : frc846::robot::GenericCommand<RobotContainer, DriveCommand>{
           container, "drive_command"} {
@@ -16,7 +18,7 @@ void DriveCommand::Periodic() {
   container_.drivetrain_.SetTarget({target});
 
   double translate_x = frc846::math::HorizontalDeadband(
-      container_.control_input_.GetReadings().translate_x,
+      ci_readings_.translate_x,
       container_.control_input_.GetPreferenceValue_double(
           "translation_deadband"),
       1,
@@ -24,15 +26,14 @@ void DriveCommand::Periodic() {
       1);
 
   double translate_y = frc846::math::HorizontalDeadband(
-      container_.control_input_.GetReadings().translate_y,
+      ci_readings_.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,
+  double rotation = frc846::math::HorizontalDeadband(ci_readings_.rotation,
       container_.control_input_.GetPreferenceValue_double("rotation_deadband"),
       1, container_.control_input_.GetPreferenceValue_int("rotation_exponent"),
       1);
@@ -46,6 +47,53 @@ void DriveCommand::Periodic() {
               "max_omega");
 
   target.velocity = {translate_x * max_speed, translate_y * max_speed};
+
+  auto delta_dir = (frc846::math::VectorND<units::feet_per_second, 2>{
+                        target.velocity[0], target.velocity[1]} -
+                    container_.drivetrain_.GetReadings().pose.velocity);
+
+  Graph("delta_dir_x", delta_dir[0]);
+  Graph("delta_dir_y", delta_dir[1]);
+
+  auto accel_limited = AntiTippingCalculator::LimitAcceleration(
+      delta_dir, container_.drivetrain_.GetReadings().pose.bearing);
+
+  Graph("limited_accel_x", accel_limited[0]);
+  Graph("limited_accel_y", accel_limited[1]);
+
+  target.velocity[0] =
+      1_fps * rampRateLimiter_x_.limit(target.velocity[0].to<double>(),
+                  accel_limited[0].to<double>());
+  target.velocity[1] =
+      1_fps * rampRateLimiter_y_.limit(target.velocity[1].to<double>(),
+                  accel_limited[1].to<double>());
+
+  if (ci_readings_.rc_control) {
+    frc846::math::VectorND<units::feet_per_second, 2> vel_rc{0_fps, 0_fps};
+
+    units::feet_per_second_t rc_speed =
+        container_.drivetrain_
+            .GetPreferenceValue_unit_type<units::feet_per_second_t>(
+                "rc_control_speed");
+
+    if (ci_readings_.rc_p_x) {
+      vel_rc[0] = rc_speed;
+      vel_rc[1] = 0_fps;
+    } else if (ci_readings_.rc_n_x) {
+      vel_rc[0] = -rc_speed;
+      vel_rc[1] = 0_fps;
+    } else if (ci_readings_.rc_p_y) {
+      vel_rc[0] = 0_fps;
+      vel_rc[1] = rc_speed;
+    } else if (ci_readings_.rc_n_y) {
+      vel_rc[0] = 0_fps;
+      vel_rc[1] = -rc_speed;
+    }
+
+    target.velocity =
+        vel_rc.rotate(container_.drivetrain_.GetReadings().pose.bearing, true);
+  }
+
   target.angular_velocity = rotation * max_omega;
 
   container_.drivetrain_.SetTarget({target});
diff --git a/src/y2025/cpp/commands/teleop/lock_to_reef_command.cc b/src/y2025/cpp/commands/teleop/lock_to_reef_command.cc
new file mode 100644
index 0000000..1fe44f8
--- /dev/null
+++ b/src/y2025/cpp/commands/teleop/lock_to_reef_command.cc
@@ -0,0 +1,73 @@
+#include "commands/teleop/lock_to_reef_command.h"
+
+#include "reef.h"
+
+LockToReefCommand::LockToReefCommand(RobotContainer& container, bool is_left)
+    : frc846::robot::GenericCommand<RobotContainer,
+          LockToReefCommand>{container, "LockToReefCommand"},
+      is_left_{is_left} {
+  AddRequirements({&container_.drivetrain_});
+}
+
+void LockToReefCommand::OnInit() { Log("LockToReefCommand initialized"); }
+
+void LockToReefCommand::Periodic() {
+  auto pos = container_.drivetrain_.GetReadings().pose.position;
+  int reef_target_pos = ReefProvider::getClosestReefSide(pos);
+
+  auto ci_readings_ = container_.control_input_.GetReadings();
+  units::inch_t adj_rate =
+      container_.drivetrain_.GetPreferenceValue_unit_type<units::inch_t>(
+          "lock_adj_rate");
+  if (ci_readings_.rc_n_x) {
+    base_adj[0] -= adj_rate;
+  } else if (ci_readings_.rc_p_x) {
+    base_adj[0] += adj_rate;
+  } else if (ci_readings_.rc_n_y) {
+    base_adj[1] -= adj_rate;
+  } else if (ci_readings_.rc_p_y) {
+    base_adj[1] += adj_rate;
+  }
+
+  auto target_pos =
+      ReefProvider::getReefScoringLocations()[2 * reef_target_pos +
+                                              (is_left_ ? 0 : 1)];
+
+  target_pos.point +=
+      base_adj.rotate(container_.drivetrain_.GetReadings().pose.bearing);
+
+  frc846 ::math::Vector2D r_vec = target_pos.point - pos;
+  Graph("lock_to_point/x_err", r_vec[0]);
+  Graph("lock_to_point/y_err", r_vec[1]);
+  if (r_vec.magnitude() <
+      container_.drivetrain_.GetPreferenceValue_unit_type<units::inch_t>(
+          "lock_gains/deadband")) {
+    container_.drivetrain_.SetTarget(
+        frc846::robot::swerve::DrivetrainOLControlTarget{{0_fps, 0_fps},
+            container_.drivetrain_.ApplyBearingPID(target_pos.bearing)});
+  } else {
+    frc846::control::base::MotorGains lock_gains{
+        container_.drivetrain_.GetPreferenceValue_double("lock_gains/_kP"),
+        container_.drivetrain_.GetPreferenceValue_double("lock_gains/_kI"),
+        container_.drivetrain_.GetPreferenceValue_double("lock_gains/_kD"),
+        0.0};
+    units::feet_per_second_t speed_target =
+        1_fps * lock_gains.calculate(r_vec.magnitude().to<double>(), 0.0,
+                    container_.drivetrain_.GetReadings()
+                        .pose.velocity.magnitude()
+                        .to<double>(),
+                    0.0);
+
+    container_.drivetrain_.SetTarget(
+        frc846::robot::swerve::DrivetrainOLControlTarget{
+            frc846::math::VectorND<units::feet_per_second, 2>{
+                speed_target, r_vec.angle(true) + 180_deg, true},
+            container_.drivetrain_.ApplyBearingPID(target_pos.bearing)});
+  }
+}
+
+void LockToReefCommand::OnEnd(bool interrupted) {
+  container_.drivetrain_.SetTargetZero();
+}
+
+bool LockToReefCommand::IsFinished() { return false; }
\ No newline at end of file
diff --git a/src/y2025/cpp/control_triggers.cc b/src/y2025/cpp/control_triggers.cc
index 8ad3c48..4b0b59b 100644
--- a/src/y2025/cpp/control_triggers.cc
+++ b/src/y2025/cpp/control_triggers.cc
@@ -4,12 +4,41 @@
 #include <frc2/command/WaitCommand.h>
 #include <frc2/command/button/Trigger.h>
 
+#include "commands/teleop/lock_to_reef_command.h"
+#include "frc846/robot/swerve/aim_command.h"
+#include "frc846/robot/swerve/drive_to_point_command.h"
+#include "reef.h"
+
 void ControlTriggerInitializer::InitTeleopTriggers(RobotContainer& container) {
   frc2::Trigger drivetrain_zero_bearing_trigger{[&] {
     return container.control_input_.GetReadings().zero_bearing;
   }};
-
   drivetrain_zero_bearing_trigger.WhileTrue(frc2::InstantCommand([&] {
     container.drivetrain_.ZeroBearing();
   }).ToPtr());
-}
\ No newline at end of file
+
+  // FAKE, TODO: remove
+
+  frc2::Trigger test_move_10_ft_trigger{[&] {
+    return container.control_input_.GetReadings().test_move_10_ft;
+  }};
+  test_move_10_ft_trigger.WhileTrue(
+      frc846::robot::swerve::DriveToPointCommand{&container.drivetrain_,
+          {{0_ft, 9_ft}, 0_deg, 0_fps}, 15_fps, 35_fps_sq, 15_fps_sq}
+          .ToPtr());
+
+  frc2::Trigger test_bearing_pid_trigger{[&] {
+    return container.control_input_.GetReadings().test_bearing_pid;
+  }};
+  test_bearing_pid_trigger.WhileTrue(
+      frc846::robot::swerve::AimCommand{&container.drivetrain_, 0_deg}.ToPtr());
+
+  // END FAKE
+
+  frc2::Trigger{[&] {
+    return container.control_input_.GetReadings().lock_left_reef;
+  }}.WhileTrue(LockToReefCommand{container, true}.ToPtr());
+  frc2::Trigger{[&] {
+    return container.control_input_.GetReadings().lock_right_reef;
+  }}.WhileTrue(LockToReefCommand{container, false}.ToPtr());
+}
diff --git a/src/y2025/cpp/field.cc b/src/y2025/cpp/field.cc
index 48348de..1bdd5f1 100644
--- a/src/y2025/cpp/field.cc
+++ b/src/y2025/cpp/field.cc
@@ -12,7 +12,7 @@ frc846::math::FieldPoint Field_nonstatic::getPoint(std::string name) {
   }
 
   Warn("Unable to access fieldpoint: {}.", name);
-  return frc846::math::FieldPoint{{0_in, 0_in}, 0_deg, {0_fps, 0_fps}};
+  return frc846::math::FieldPoint{{0_in, 0_in}, 0_deg, 0_fps};
 }
 
 std::vector<frc846::math::FieldPoint> Field_nonstatic::getPath(
@@ -90,7 +90,7 @@ std::string Field_nonstatic::getFileDirectory() {
 }
 
 frc846::math::FieldPoint Field_nonstatic::parsePoint(std::string line) {
-  frc846::math::FieldPoint pt{{0_in, 0_in}, 0_deg, {0_fps, 0_fps}};
+  frc846::math::FieldPoint pt{{0_in, 0_in}, 0_deg, 0_fps};
 
   auto tokens = split(line, ',');
   if (tokens.size() != 0) {
@@ -99,13 +99,13 @@ frc846::math::FieldPoint Field_nonstatic::parsePoint(std::string line) {
       if (tokens.size() == 6) v = std::stod(tokens[5]) * 1_fps;
 
       pt = {{std::stod(tokens[2]) * 1_in, std::stod(tokens[3]) * 1_in},
-          std::stod(tokens[4]) * 1_deg, {v, 0_fps}};
+          std::stod(tokens[4]) * 1_deg, v};
     } else if (tokens[0] == "P") {
       auto v = 0_fps;
       if (tokens.size() == 5) v = std::stod(tokens[4]) * 1_fps;
 
       pt = {{std::stod(tokens[1]) * 1_in, std::stod(tokens[2]) * 1_in},
-          std::stod(tokens[3]) * 1_deg, {v, 0_fps}};
+          std::stod(tokens[3]) * 1_deg, v};
     } else if (tokens[0] == "F") {
       if (tokens.size() == 2) { pt = getPoint(tokens[1]); }
     } else {
diff --git a/src/y2025/cpp/reef.cc b/src/y2025/cpp/reef.cc
new file mode 100644
index 0000000..a794b54
--- /dev/null
+++ b/src/y2025/cpp/reef.cc
@@ -0,0 +1,51 @@
+#include "reef.h"
+
+#include <iostream>
+
+std::vector<frc846::math::FieldPoint> ReefProvider::getReefScoringLocations() {
+  std::vector<frc846::math::FieldPoint> reefScoringLocations;
+
+  // TODO: add real values
+  // TODO: add reef center
+  frc846::math::Vector2D left_reef_displacement =
+      frc846::math::Vector2D{8.0_in, 40.0_in};
+
+  frc846::math::Vector2D right_reef_displacement =
+      frc846::math::Vector2D{-8.0_in, 40.0_in};
+
+  for (int i = 0; i < 6; i++) {
+    reefScoringLocations.push_back(frc846::math::FieldPoint{
+        left_reef_displacement.rotate(60_deg * i, true), 60_deg * i + 180_deg,
+        0_fps});
+    reefScoringLocations.push_back(frc846::math::FieldPoint{
+        right_reef_displacement.rotate(60_deg * i, true), 60_deg * i + 180_deg,
+        0_fps});
+  }
+
+  return reefScoringLocations;
+}
+
+int ReefProvider::getClosestReefSide(frc846::math::Vector2D current_pos) {
+  // TODO: include center reef, perhaps hysteresis
+
+  units::degree_t angle = current_pos.angle(true);
+  while (angle < 0_deg)
+    angle += 360_deg;
+  while (angle > 360_deg)
+    angle -= 360_deg;
+
+  if (angle < 30_deg)
+    return 0;
+  else if (angle < 90_deg)
+    return 1;
+  else if (angle < 150_deg)
+    return 2;
+  else if (angle < 210_deg)
+    return 3;
+  else if (angle < 270_deg)
+    return 4;
+  else if (angle < 330_deg)
+    return 5;
+
+  return 0;
+}
\ 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 87e6c4b..29b2df5 100644
--- a/src/y2025/cpp/subsystems/abstract/control_input.cc
+++ b/src/y2025/cpp/subsystems/abstract/control_input.cc
@@ -31,6 +31,15 @@ ControlInputReadings ControlInputSubsystem::ReadFromHardware() {
         readings.zero_bearing ? 1 : 0);
   }
 
+  if (readings.lock_left_reef != previous_readings_.lock_left_reef) {
+    Log("ControlInput [Lock Left Reef] state changed to {}",
+        readings.lock_left_reef ? 1 : 0);
+  }
+  if (readings.lock_right_reef != previous_readings_.lock_right_reef) {
+    Log("ControlInput [Lock Right Reef] state changed to {}",
+        readings.lock_right_reef ? 1 : 0);
+  }
+
   previous_readings_ = readings;
 
   return readings;
@@ -53,7 +62,21 @@ ControlInputReadings ControlInputSubsystem::UpdateWithInput() {
   ci_readings_.zero_bearing = dr_readings.back_button;
   ci_readings_.translate_x = dr_readings.left_stick_x;
   ci_readings_.translate_y = dr_readings.left_stick_y;
+
+  ci_readings_.rc_p_y = (int)dr_readings.pov == 0;
+  ci_readings_.rc_p_x = (int)dr_readings.pov == 90;
+  ci_readings_.rc_n_y = (int)dr_readings.pov == 180;
+  ci_readings_.rc_n_x = (int)dr_readings.pov == 270;
+  ci_readings_.rc_control = ci_readings_.rc_p_y || ci_readings_.rc_n_y ||
+                            ci_readings_.rc_p_x || ci_readings_.rc_n_x;
+
   ci_readings_.rotation = dr_readings.right_stick_x;
 
+  ci_readings_.test_move_10_ft = dr_readings.x_button;
+  ci_readings_.test_bearing_pid = dr_readings.y_button;
+
+  ci_readings_.lock_left_reef = dr_readings.left_bumper;
+  ci_readings_.lock_right_reef = dr_readings.right_bumper;
+
   return ci_readings_;
 }
\ No newline at end of file
diff --git a/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc b/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc
index 8cedd0b..d6dd10a 100644
--- a/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc
+++ b/src/y2025/cpp/subsystems/hardware/DrivetrainConstructor.cc
@@ -3,6 +3,7 @@
 #include "frc846/control/calculators/CircuitResistanceCalculator.h"
 #include "frc846/math/constants.h"
 #include "ports.h"
+#include "subsystems/robot_constants.h"
 
 DrivetrainConstructor::DrivetrainConstructor()
     : Loggable{"DrivetrainConstructor"} {
@@ -17,6 +18,9 @@ DrivetrainConstructor::DrivetrainConstructor()
 
 frc846::robot::swerve::DrivetrainConfigs
 DrivetrainConstructor::getDrivetrainConfigs() {
+  frc846::control::base::MotorMonkeyType mmtype =
+      frc846::control::base::MotorMonkeyType::TALON_FX_KRAKENX60;
+
   frc846::robot::swerve::DrivetrainConfigs configs;
 
   /*
@@ -38,17 +42,19 @@ DrivetrainConstructor::getDrivetrainConfigs() {
   unsigned int num_connectors_BL = 3;
   unsigned int num_connectors_BR = 3;
 
+  double drive_gear_ratio = 6.12;
   frc846::robot::swerve::drive_conv_unit drive_reduction =
-      (frc846::math::constants::geometry::pi * wheel_diameter) / 6.12_tr;
+      (frc846::math::constants::geometry::pi * wheel_diameter) /
+      (drive_gear_ratio * 1_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;
+  configs.wheelbase_forward_dim = robot_constants::base::wheelbase_y;
+  configs.wheelbase_horizontal_dim = robot_constants::base::wheelbase_x;
 
   units::pound_t wheel_approx_weight = 2_lb;
   units::inch_t wheel_weight_radius = 1_in;
 
-  units::pound_t robot_weight = 120_lb;
+  units::pound_t robot_weight = robot_constants::total_weight;
 
   // (Mass wheel) * (wheel_r)^2 * (steer reduction)^2
   frc846::wpilib::unit_kg_m_sq relative_steer_inertia{
@@ -62,6 +68,15 @@ DrivetrainConstructor::getDrivetrainConfigs() {
 
   /* END SETTABLES */
 
+  // Max accel = (4 * [Max force per wheel]) / (robot_weight)
+  units::meter_t effective_torque_radius =
+      (wheel_diameter / 2.0) / drive_gear_ratio;
+  units::newton_t max_force_per_wheel{
+      frc846::control::base::MotorSpecificationPresets::get(mmtype)
+          .stall_torque /
+      effective_torque_radius};
+  configs.max_accel = (4.0 * max_force_per_wheel) / robot_weight;
+
   frc846::wpilib::unit_ohm wire_resistance_FR{
       frc846::control::calculators::CircuitResistanceCalculator::calculate(
           wire_length_FR, frc846::control::calculators::fourteen_gauge,
@@ -124,9 +139,7 @@ DrivetrainConstructor::getDrivetrainConfigs() {
 
   configs.module_common_config =
       frc846::robot::swerve::SwerveModuleCommonConfig{drive_params,
-          steer_params,
-          frc846::control::base::MotorMonkeyType::TALON_FX_KRAKENX60,
-          steer_reduction, drive_reduction, ""};
+          steer_params, mmtype, steer_reduction, drive_reduction, ""};
   configs.module_unique_configs = {FR_config, FL_config, BL_config, BR_config};
 
   return configs;
diff --git a/src/y2025/cpp/subsystems/hardware/coral_telescope.cc b/src/y2025/cpp/subsystems/hardware/coral_telescope.cc
index a223316..f5a11bf 100644
--- a/src/y2025/cpp/subsystems/hardware/coral_telescope.cc
+++ b/src/y2025/cpp/subsystems/hardware/coral_telescope.cc
@@ -60,5 +60,10 @@ TelescopeReadings TelescopeSubsystem::ReadFromHardware() {
 
 void TelescopeSubsystem::WriteToHardware(TelescopeTarget target) {
   telescope_.SetGains(GET_PIDF_GAINS("coral_telescope/coral_telescope_gains_"));
-  motor_helper_.WritePosition(target.position);
+  // //motor_helper_.WritePosition(target.position);
+  // motor_helper_.WriteDC(arm_calculator_.calculate({motor_helper_.GetPosition(),
+  //     target.position, motor_helper_.GetVelocity(),
+  //     GET_PIDF_GAINS("algae_pivot/algae_pivot_gains_")}));
+
+  telescope_.SetLoad(1_Nm);
 }
diff --git a/src/y2025/cpp/subsystems/hardware/elevator.cc b/src/y2025/cpp/subsystems/hardware/elevator.cc
index 75cc550..6e7af0d 100644
--- a/src/y2025/cpp/subsystems/hardware/elevator.cc
+++ b/src/y2025/cpp/subsystems/hardware/elevator.cc
@@ -58,5 +58,6 @@ ElevatorReadings ElevatorSubsystem::ReadFromHardware() {
 
 void ElevatorSubsystem::WriteToHardware(ElevatorTarget target) {
   elevator_.SetGains(GET_PIDF_GAINS("elevator/elevator_gains_"));
-  motor_helper_.WritePosition(target.height);
+  // motor_helper_.WritePosition(target.height);
+  elevator_.SetLoad(1_Nm);
 }
\ No newline at end of file
diff --git a/src/y2025/include/autos/ActionMaker.h b/src/y2025/include/autos/ActionMaker.h
index 976827d..26a50fd 100644
--- a/src/y2025/include/autos/ActionMaker.h
+++ b/src/y2025/include/autos/ActionMaker.h
@@ -2,7 +2,7 @@
 
 #include <frc2/command/Command.h>
 
-#include <string>
+#include <string_view>
 #include <unordered_map>
 
 #include "subsystems/robot_container.h"
@@ -10,5 +10,5 @@
 class ActionMaker {
 public:
   static std::unique_ptr<frc2::Command> GetAction(
-      std::string name, RobotContainer& container);
+      std::string_view name, RobotContainer& container);
 };
\ No newline at end of file
diff --git a/src/y2025/include/calculators/AntiTippingCalculator.h b/src/y2025/include/calculators/AntiTippingCalculator.h
new file mode 100644
index 0000000..47b88ce
--- /dev/null
+++ b/src/y2025/include/calculators/AntiTippingCalculator.h
@@ -0,0 +1,56 @@
+#pragma once
+
+#include <time.h>
+#include <units/acceleration.h>
+#include <units/angular_velocity.h>
+#include <units/length.h>
+#include <units/mass.h>
+#include <units/velocity.h>
+
+#include "frc846/math/calculator.h"
+#include "frc846/math/constants.h"
+#include "frc846/math/vectors.h"
+
+/*
+AntiTippingCalculator
+
+Class that provides static methods to aid in preventing the robot from tipping.
+*/
+class AntiTippingCalculator {
+public:
+  /*
+  SetElevatorHeight()
+
+  Sets the position of the end effector from the ground. Location of the CG
+  of the elevator is heuristically determined.
+  */
+  static void SetElevatorHeight(units::inch_t height);
+
+  /*
+  SetElevatorHeight()
+
+  Sets the position of the end effector from the ground. Location of the CG
+  of the elevator is heuristically determined.
+  */
+  static void SetTelescopeHeight(units::inch_t height);
+
+  /*
+  CalculateRobotCG()
+
+  Finds the location of the center of gravity of the robot.
+  */
+  static frc846::math::Vector3D CalculateRobotCG();
+
+  /*
+  LimitAcceleration()
+
+  Limits the acceleration of the robot to prevent tipping.
+  */
+  static frc846::math::VectorND<units::feet_per_second_squared, 2>
+  LimitAcceleration(frc846::math::VectorND<units::feet_per_second, 2> vel_vec,
+      units::degree_t bearing);
+
+private:
+  static frc846::math::Vector3D elev_cg_position_;
+  static frc846::math::Vector3D tele_cg_position_;
+};
\ 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
index 432354f..a543663 100644
--- a/src/y2025/include/commands/teleop/drive_command.h
+++ b/src/y2025/include/commands/teleop/drive_command.h
@@ -1,5 +1,6 @@
 #pragma once
 
+#include "frc846/math/RampRateLimiter.h"
 #include "frc846/robot/GenericCommand.h"
 #include "subsystems/robot_container.h"
 
@@ -15,4 +16,8 @@ class DriveCommand
   void OnEnd(bool interrupted) override;
 
   bool IsFinished() override;
+
+private:
+  frc846::math::RampRateLimiter rampRateLimiter_x_;
+  frc846::math::RampRateLimiter rampRateLimiter_y_;
 };
\ No newline at end of file
diff --git a/src/y2025/include/commands/teleop/lock_to_reef_command.h b/src/y2025/include/commands/teleop/lock_to_reef_command.h
new file mode 100644
index 0000000..505196e
--- /dev/null
+++ b/src/y2025/include/commands/teleop/lock_to_reef_command.h
@@ -0,0 +1,29 @@
+#pragma once
+
+#include <frc2/command/Command.h>
+#include <frc2/command/CommandHelper.h>
+
+#include "frc846/base/Loggable.h"
+#include "frc846/math/fieldpoints.h"
+#include "frc846/robot/GenericCommand.h"
+#include "frc846/robot/swerve/drivetrain.h"
+#include "subsystems/robot_container.h"
+
+class LockToReefCommand
+    : public frc846::robot::GenericCommand<RobotContainer, LockToReefCommand> {
+public:
+  LockToReefCommand(RobotContainer& container, bool is_left);
+
+  void OnInit() override;
+
+  void Periodic() override;
+
+  void OnEnd(bool interrupted) override;
+
+  bool IsFinished() override;
+
+private:
+  bool is_left_;
+
+  frc846::math::Vector2D base_adj{0_in, 0_in};
+};
diff --git a/src/y2025/include/reef.h b/src/y2025/include/reef.h
new file mode 100644
index 0000000..4800a7b
--- /dev/null
+++ b/src/y2025/include/reef.h
@@ -0,0 +1,24 @@
+#pragma once
+
+#include "frc846/base/Loggable.h"
+#include "frc846/math/fieldpoints.h"
+
+class ReefProvider {
+public:
+  /*
+  getReefScoringLocations()
+
+  Returns a vector of FieldPoints representing the locations of the reef scoring
+  locations on the field. Order: LRLR(etc.). Starts furthest from alliance wall,
+  then moves CW.
+  */
+  static std::vector<frc846::math::FieldPoint> getReefScoringLocations();
+
+  /*
+  getClosestReefSide()
+
+  Returns the side (0, 1, 2, 3, 4, 5) of the closest reef scoring location to
+  the robot. Starts furthest from alliance wall, then moves CW.
+  */
+  static int getClosestReefSide(frc846::math::Vector2D current_pos);
+};
\ No newline at end of file
diff --git a/src/y2025/include/rsighandler.h b/src/y2025/include/rsighandler.h
index b421818..4d31c2f 100644
--- a/src/y2025/include/rsighandler.h
+++ b/src/y2025/include/rsighandler.h
@@ -14,7 +14,7 @@
 #include <map>
 
 void handler(int sig) {
-  std::map<int, std::string> sigErrors;
+  std::map<int, std::string_view> sigErrors;
 
   sigErrors[SIGFPE] = "FATAL ERROR >> Arithmetic Error, SIGFPE";
   sigErrors[SIGILL] = "FATAL ERROR >> Illegal Instruction, SIGILL";
diff --git a/src/y2025/include/subsystems/abstract/control_input.h b/src/y2025/include/subsystems/abstract/control_input.h
index b215034..308a58d 100644
--- a/src/y2025/include/subsystems/abstract/control_input.h
+++ b/src/y2025/include/subsystems/abstract/control_input.h
@@ -6,8 +6,20 @@
 struct ControlInputReadings {
   double translate_x;
   double translate_y;
+
+  bool rc_p_y;
+  bool rc_p_x;
+  bool rc_n_y;
+  bool rc_n_x;
+  bool rc_control;
+
   double rotation;
 
+  bool test_move_10_ft;  // TODO: remove when not needed
+  bool test_bearing_pid;
+  bool lock_left_reef;
+  bool lock_right_reef;
+
   bool zero_bearing;
 };
 
diff --git a/src/y2025/include/subsystems/robot_constants.h b/src/y2025/include/subsystems/robot_constants.h
new file mode 100644
index 0000000..10fccec
--- /dev/null
+++ b/src/y2025/include/subsystems/robot_constants.h
@@ -0,0 +1,39 @@
+#pragma once
+
+#include <units/length.h>
+
+#include "frc846/math/vectors.h"
+
+struct robot_constants {
+  struct base {
+    static constexpr units::inch_t wheelbase_x = 28_in;
+    static constexpr units::inch_t wheelbase_y = 28_in;
+    static constexpr units::pound_t weight = 75_lb;
+
+    static constexpr units::inch_t height = 6_in;
+  };
+
+  struct elevator {
+    static constexpr units::pound_t elevator_weight = 20_lb;
+    static constexpr units::inch_t min_height_off_base = 38_in;
+
+    static constexpr units::pound_t end_effector_weight = 10_lb;
+
+    static constexpr units::pound_t total_weight =
+        elevator_weight + end_effector_weight;
+
+    static constexpr units::inch_t pos_x = -5_in;
+    static constexpr units::inch_t pos_y = -7_in;
+  };
+
+  struct telescope {
+    static constexpr units::pound_t total_weight = 20_lb;
+
+    static constexpr units::inch_t pos_x = 5_in;
+    static constexpr units::inch_t pos_y = -7_in;
+  };
+
+  static constexpr units::pound_t total_weight =
+      base::weight + elevator::elevator_weight + elevator::end_effector_weight +
+      telescope::total_weight;
+};
\ 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 53428ae..0a7b368 100644
--- a/src/y2025/include/subsystems/robot_container.h
+++ b/src/y2025/include/subsystems/robot_container.h
@@ -38,17 +38,29 @@ class RobotContainer : public frc846::robot::GenericRobotContainer {
     RegisterPreference("init_ramp", true);
 
     control_input_.Init();
-    if (GetPreferenceValue_bool("init_drivetrain")) drivetrain_.Init();
-    if (GetPreferenceValue_bool("init_leds")) leds_.Init();
-    if (GetPreferenceValue_bool("init_algae_pivot")) algae_pivot_.Init();
-    if (GetPreferenceValue_bool("init_climber")) climber_.Init();
-    if (GetPreferenceValue_bool("init_telescope")) telescope_.Init();
-    if (GetPreferenceValue_bool("init_coral_wrist")) coral_wrist_.Init();
-    if (GetPreferenceValue_bool("init_elevator")) elevator_.Init();
-    if (GetPreferenceValue_bool("init_ramp")) ramp_.Init();
-
-    RegisterSubsystems({&control_input_, &drivetrain_, &leds_, &algae_pivot_,
-        &telescope_, &coral_wrist_, &elevator_, &ramp_, &climber_});
+
+    bool drivetrain_init = (GetPreferenceValue_bool("init_drivetrain"));
+    bool algae_pivot_init = (GetPreferenceValue_bool("init_algae_pivot"));
+    bool leds_init = (GetPreferenceValue_bool("init_leds"));
+    bool climber_init = (GetPreferenceValue_bool("init_climber"));
+    bool telescope_init = (GetPreferenceValue_bool("init_telescope"));
+    bool coral_wrist_init = (GetPreferenceValue_bool("init_coral_wrist"));
+    bool elevator_init = (GetPreferenceValue_bool("init_elevator"));
+    bool ramp_init = (GetPreferenceValue_bool("init_ramp"));
+
+    RegisterSubsystemGroupA({{&control_input_, true}});
+    RegisterSubsystemGroupB({{&leds_, leds_init}});
+
+    RegisterSubsystemGroupA({{&ramp_, &ramp_init}});
+    RegisterSubsystemGroupB({{&climber_, &climber_init}});
+
+    RegisterSubsystemGroupA({{&algae_pivot_, &algae_pivot_init}});
+    RegisterSubsystemGroupA({{&elevator_, &elevator_init}});
+
+    RegisterSubsystemGroupB({{&telescope_, &telescope_init}});
+    RegisterSubsystemGroupB({{&coral_wrist_, &coral_wrist_init}});
+
+    RegisterSubsystemGroupAB({{&drivetrain_, drivetrain_init}});
   }
 };
 
diff --git a/vendordeps/Phoenix6-frc2025-latest.json b/vendordeps/Phoenix6-25.2.1.json
similarity index 85%
rename from vendordeps/Phoenix6-frc2025-latest.json
rename to vendordeps/Phoenix6-25.2.1.json
index 7f4bd2e..1397da1 100644
--- a/vendordeps/Phoenix6-frc2025-latest.json
+++ b/vendordeps/Phoenix6-25.2.1.json
@@ -1,7 +1,7 @@
 {
-    "fileName": "Phoenix6-frc2025-latest.json",
+    "fileName": "Phoenix6-25.2.1.json",
     "name": "CTRE-Phoenix (v6)",
-    "version": "25.1.0",
+    "version": "25.2.1",
     "frcYear": "2025",
     "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
     "mavenUrls": [
@@ -19,14 +19,14 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-java",
-            "version": "25.1.0"
+            "version": "25.2.1"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "api-cpp",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -40,7 +40,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -54,7 +54,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "api-cpp-sim",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -68,7 +68,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -82,7 +82,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -96,7 +96,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -110,7 +110,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -124,7 +124,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -138,7 +138,21 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.1.0",
+            "version": "25.2.1",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProTalonFXS",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -152,7 +166,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -166,7 +180,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -180,7 +194,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -196,7 +210,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-cpp",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_Phoenix6_WPI",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -212,7 +226,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_PhoenixTools",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -228,7 +242,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "wpiapi-cpp-sim",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_Phoenix6_WPISim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -244,7 +258,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_PhoenixTools_Sim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -260,7 +274,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimTalonSRX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -276,7 +290,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimVictorSPX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -292,7 +306,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimPigeonIMU",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -308,7 +322,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimCANCoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -324,7 +338,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimProTalonFX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -337,10 +351,26 @@
             ],
             "simMode": "swsim"
         },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProTalonFXS",
+            "version": "25.2.1",
+            "libName": "CTRE_SimProTalonFXS",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimProCANcoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -356,7 +386,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimProPigeon2",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -372,7 +402,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.1.0",
+            "version": "25.2.1",
             "libName": "CTRE_SimProCANrange",
             "headerClassifier": "headers",
             "sharedLibrary": true,
diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib-2025.0.2.json
similarity index 86%
rename from vendordeps/REVLib.json
rename to vendordeps/REVLib-2025.0.2.json
index 2c39628..c29aefa 100644
--- a/vendordeps/REVLib.json
+++ b/vendordeps/REVLib-2025.0.2.json
@@ -1,7 +1,7 @@
 {
-    "fileName": "REVLib.json",
+    "fileName": "REVLib-2025.0.2.json",
     "name": "REVLib",
-    "version": "2025.0.0",
+    "version": "2025.0.2",
     "frcYear": "2025",
     "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
     "mavenUrls": [
@@ -12,19 +12,18 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-java",
-            "version": "2025.0.0"
+            "version": "2025.0.2"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2025.0.0",
+            "version": "2025.0.2",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
                 "windowsx86-64",
-                "windowsx86",
                 "linuxarm64",
                 "linuxx86-64",
                 "linuxathena",
@@ -37,14 +36,13 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-cpp",
-            "version": "2025.0.0",
+            "version": "2025.0.2",
             "libName": "REVLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
             "binaryPlatforms": [
                 "windowsx86-64",
-                "windowsx86",
                 "linuxarm64",
                 "linuxx86-64",
                 "linuxathena",
@@ -55,14 +53,13 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2025.0.0",
+            "version": "2025.0.2",
             "libName": "REVLibDriver",
             "headerClassifier": "headers",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
             "binaryPlatforms": [
                 "windowsx86-64",
-                "windowsx86",
                 "linuxarm64",
                 "linuxx86-64",
                 "linuxathena",
diff --git a/vendordeps/Studica-2025.0.0.json b/vendordeps/Studica-2025.0.1.json
similarity index 89%
rename from vendordeps/Studica-2025.0.0.json
rename to vendordeps/Studica-2025.0.1.json
index ddb0e44..5010be0 100644
--- a/vendordeps/Studica-2025.0.0.json
+++ b/vendordeps/Studica-2025.0.1.json
@@ -1,13 +1,13 @@
 {
-    "fileName": "Studica-2025.0.0.json",
+    "fileName": "Studica-2025.0.1.json",
     "name": "Studica",
-    "version": "2025.0.0",
+    "version": "2025.0.1",
     "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
     "frcYear": "2025",
     "mavenUrls": [
         "https://dev.studica.com/maven/release/2025/"
     ],
-    "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.0.json",
+    "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.1.json",
     "cppDependencies": [
         {
             "artifactId": "Studica-cpp",
@@ -24,7 +24,7 @@
             "libName": "Studica",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
-            "version": "2025.0.0"
+            "version": "2025.0.1"
         },
         {
             "artifactId": "Studica-driver",
@@ -41,14 +41,14 @@
             "libName": "StudicaDriver",
             "sharedLibrary": false,
             "skipInvalidPlatforms": true,
-            "version": "2025.0.0"
+            "version": "2025.0.1"
         }
     ],
     "javaDependencies": [
         {
             "artifactId": "Studica-java",
             "groupId": "com.studica.frc",
-            "version": "2025.0.0"
+            "version": "2025.0.1"
         }
     ],
     "jniDependencies": [
@@ -65,7 +65,7 @@
                 "osxuniversal",
                 "windowsx86-64"
             ],
-            "version": "2025.0.0"
+            "version": "2025.0.1"
         }
     ]
 }
\ No newline at end of file