diff --git a/README.md b/README.md
index 4c147c4..77cf5aa 100644
--- a/README.md
+++ b/README.md
@@ -180,9 +180,7 @@ To undo the going back:
 ## CppCheck Warnings
 
 ```
-src\frc846\cpp\frc846\robot\swerve\drivetrain.cc:94:44: warning: Variable 'ol_target' is assigned a value that is never used. [unreadVariable]
-src\frc846\cpp\frc846\robot\swerve\drivetrain.cc:108:64: warning: Variable 'accel_target' is assigned a value that is never used. [unreadVariable]
-src\frc846\cpp\frc846\math\collection.cc:11:0: warning: The function 'HorizontalDeadband' is never used. [unusedFunction]
+src\frc846\cpp\frc846\robot\swerve\drivetrain.cc:152:64: warning: Variable 'accel_target' is assigned a value that is never used. [unreadVariable]
 src\frc846\cpp\frc846\math\collection.cc:25:0: warning: The function 'VerticalDeadband' is never used. [unusedFunction]
 src\frc846\cpp\frc846\math\collection.cc:52:0: warning: The function 'CoterminalSum' is never used. [unusedFunction]
 ```
\ No newline at end of file
diff --git a/networktables.json b/networktables.json
index 903bd45..c15bf00 100644
--- a/networktables.json
+++ b/networktables.json
@@ -134,5 +134,53 @@
     "properties": {
       "persistent": true
     }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/SwerveDrivetrain/max_speed (fps)",
+    "type": "double",
+    "value": 15.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/SwerveDrivetrain/max_omega (deg_per_s)",
+    "type": "double",
+    "value": 180.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/control_input/translation_deadband",
+    "type": "double",
+    "value": 0.07,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/control_input/translation_exponent",
+    "type": "int",
+    "value": 2,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/control_input/rotation_deadband",
+    "type": "double",
+    "value": 0.07,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/control_input/rotation_exponent",
+    "type": "int",
+    "value": 2,
+    "properties": {
+      "persistent": true
+    }
   }
 ]
diff --git a/networktables.json.bck b/networktables.json.bck
index c07936a..903bd45 100644
--- a/networktables.json.bck
+++ b/networktables.json.bck
@@ -54,5 +54,85 @@
     "properties": {
       "persistent": true
     }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/FL/SwerveDrivetrain/FL/cancoder_offset_ (deg)",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/BR/SwerveDrivetrain/BR/cancoder_offset_ (deg)",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/BL/SwerveDrivetrain/BL/cancoder_offset_ (deg)",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/steer_gains/kP",
+    "type": "double",
+    "value": 0.3,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/steer_gains/kI",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/steer_gains/kD",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/SwerveDrivetrain/steer_gains/kF",
+    "type": "double",
+    "value": 0.0,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/robot_container/init_drivetrain",
+    "type": "boolean",
+    "value": true,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/robot_container/init_leds",
+    "type": "boolean",
+    "value": true,
+    "properties": {
+      "persistent": true
+    }
+  },
+  {
+    "name": "/Preferences/control_input/trigger_threshold",
+    "type": "double",
+    "value": 0.3,
+    "properties": {
+      "persistent": true
+    }
   }
 ]
diff --git a/simgui.json b/simgui.json
index 272f1ce..2708274 100644
--- a/simgui.json
+++ b/simgui.json
@@ -4,10 +4,21 @@
       "/FMSInfo": "FMSInfo",
       "/SmartDashboard/get_prune_list": "Command",
       "/SmartDashboard/prune_prefs": "Command",
-      "/SmartDashboard/verify_hardware": "Command"
+      "/SmartDashboard/set_cancoder_offsets": "Command",
+      "/SmartDashboard/verify_hardware": "Command",
+      "/SmartDashboard/zero_bearing": "Command"
     }
   },
   "NetworkTables Info": {
+    "Connections": {
+      "open": true
+    },
+    "Server": {
+      "Subscribers": {
+        "open": true
+      },
+      "open": true
+    },
     "visible": true
   }
 }
diff --git a/src/frc846/cpp/frc846/robot/swerve/control/swerve_ol_calculator.cc b/src/frc846/cpp/frc846/robot/swerve/control/swerve_ol_calculator.cc
new file mode 100644
index 0000000..183cc02
--- /dev/null
+++ b/src/frc846/cpp/frc846/robot/swerve/control/swerve_ol_calculator.cc
@@ -0,0 +1,57 @@
+#include "frc846/robot/swerve/control/swerve_ol_calculator.h"
+
+namespace frc846::robot::swerve::control {
+
+SwerveOpenLoopCalculatorOutput SwerveOpenLoopCalculator::calculate(
+    SwerveOpenLoopCalculatorInputs inputs) {
+  std::pair<double, double> kModuleLocationSigns[4] = {
+      {+0.5, +0.5},  // FR
+      {-0.5, +0.5},  // FL
+      {-0.5, -0.5},  // BL
+      {+0.5, -0.5},  // BR
+  };
+
+  std::array<frc846::math::VectorND<units::feet_per_second, 2>, 4>
+      module_targets;
+
+  units::inch_t radius = units::math::hypot(
+      constants_.wheelbase_horizontal_dim, constants_.wheelbase_forward_dim);
+
+  for (int i = 0; i < 4; i++) {
+    frc846::math::VectorND<units::inch, 2> location{
+        kModuleLocationSigns[i].first * constants_.wheelbase_horizontal_dim,
+        kModuleLocationSigns[i].second * constants_.wheelbase_forward_dim};
+
+    units::degree_t rot_direction = location.angle(false);
+
+    frc846::math::VectorND<units::feet_per_second, 2> rotation{
+        inputs.rotation_target * units::math::cos(rot_direction) * radius /
+            1_rad,
+        inputs.rotation_target * units::math::sin(rot_direction) * radius /
+            1_rad};
+
+    module_targets[i] = inputs.translation_target + rotation;
+  }
+
+  units::feet_per_second_t max_mag = 0_fps;
+  for (int i = 0; i < 4; i++) {
+    if (module_targets[i].magnitude() > max_mag) {
+      max_mag = module_targets[i].magnitude();
+    }
+  }
+
+  if (max_mag > inputs.max_speed) {
+    for (int i = 0; i < 4; i++) {
+      module_targets[i] *= inputs.max_speed / max_mag;
+    }
+  }
+
+  SwerveOpenLoopCalculatorOutput output{};
+  for (int i = 0; i < 4; i++) {
+    output.drive_outputs[i] = module_targets[i].magnitude();
+    output.steer_outputs[i] = module_targets[i].angle(true);
+  }
+  return output;
+}
+
+}  // namespace frc846::robot::swerve::control
\ No newline at end of file
diff --git a/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc b/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc
index cc3ace9..aa5587c 100644
--- a/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc
+++ b/src/frc846/cpp/frc846/robot/swerve/drivetrain.cc
@@ -1,5 +1,6 @@
 #include "frc846/robot/swerve/drivetrain.h"
 
+#include "frc846/robot/swerve/control/swerve_ol_calculator.h"
 #include "frc846/robot/swerve/swerve_module.h"
 
 namespace frc846::robot::swerve {
@@ -18,6 +19,15 @@ DrivetrainSubsystem::DrivetrainSubsystem(DrivetrainConfigs configs)
   RegisterPreference("steer_gains/kI", 0.0);
   RegisterPreference("steer_gains/kD", 0.0);
   RegisterPreference("steer_gains/kF", 0.0);
+
+  RegisterPreference("max_speed", 15_fps);
+  RegisterPreference("max_omega", units::degrees_per_second_t{180});
+
+  odometry_.setConstants({});
+  ol_calculator_.setConstants({
+      .wheelbase_horizontal_dim = configs.wheelbase_horizontal_dim,
+      .wheelbase_forward_dim = configs.wheelbase_forward_dim,
+  });
 }
 
 void DrivetrainSubsystem::Setup() {
@@ -35,7 +45,7 @@ void DrivetrainSubsystem::Setup() {
 }
 
 DrivetrainTarget DrivetrainSubsystem::ZeroTarget() const {
-  return DrivetrainOLControlTarget{{0_fps, 0_fps}};
+  return DrivetrainOLControlTarget{{0_fps, 0_fps}, 0_deg_per_s};
 }
 
 bool DrivetrainSubsystem::VerifyHardware() {
@@ -47,6 +57,36 @@ bool DrivetrainSubsystem::VerifyHardware() {
   return ok;
 }
 
+void DrivetrainSubsystem::ZeroBearing() {
+  if (!is_initialized()) return;
+
+  constexpr int kMaxAttempts = 5;
+  constexpr int kSleepTimeMs = 500;
+  for (int attempts = 1; attempts <= kMaxAttempts; ++attempts) {
+    Log("Gyro zero attempt {}/{}", attempts, kMaxAttempts);
+    if (navX_.IsConnected() && !navX_.IsCalibrating()) {
+      navX_.ZeroYaw();
+      Log("Zeroed bearing");
+
+      for (SwerveModuleSubsystem* module : modules_) {
+        module->ZeroWithCANcoder();
+      }
+      return;
+    }
+
+    Warn("Attempt to zero failed, sleeping {} ms...", kSleepTimeMs);
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(kSleepTimeMs));
+  }
+  Error("Unable to zero after {} attempts", kMaxAttempts);
+}
+
+void DrivetrainSubsystem::SetCANCoderOffsets() {
+  for (SwerveModuleSubsystem* module : modules_) {
+    module->SetCANCoderOffset();
+  }
+}
+
 DrivetrainReadings DrivetrainSubsystem::ReadFromHardware() {
   units::degree_t bearing = navX_.GetAngle() * 1_deg;
 
@@ -90,26 +130,37 @@ DrivetrainReadings DrivetrainSubsystem::ReadFromHardware() {
 }
 
 void DrivetrainSubsystem::WriteToHardware(DrivetrainTarget target) {
-  // TODO: finish
   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;
 
-    /*
-    TODO: For each module, find the target direction and speed based on velocity
-    and turn speed. Then, construct a SwerveModuleOLControlTarget and write it
-    to the module.
-    */
+    auto ol_calc_outputs = ol_calculator_.calculate({ol_target->velocity,
+        ol_target->angular_velocity,
+        GetPreferenceValue_unit_type<units::feet_per_second_t>("max_speed")});
 
     for (int i = 0; i < 4; i++) {
-      // modules_[i]->SetTarget(module_target);
-      // modules_[i]->UpdateHardware();
+      modules_[i]->SetSteerGains({GetPreferenceValue_double("steer_gains/kP"),
+          GetPreferenceValue_double("steer_gains/kI"),
+          GetPreferenceValue_double("steer_gains/kD"),
+          GetPreferenceValue_double("steer_gains/kF")});
+
+      SwerveModuleOLControlTarget module_target{
+          .drive = ol_calc_outputs.drive_outputs[i],
+          .steer = ol_calc_outputs.steer_outputs[i] - bearing};
+      modules_[i]->SetTarget(module_target);
     }
   } else if (DrivetrainAccelerationControlTarget* accel_target =
                  std::get_if<DrivetrainAccelerationControlTarget>(&target)) {
     throw std::runtime_error("Acceleration control not yet implemented");
-    // TODO: finish later
+    // TODO: implement acceleration control for drivetrain
   }
+
+  for (int i = 0; i < 4; i++)
+    modules_[i]->UpdateHardware();
 }
 
 }  // namespace frc846::robot::swerve
\ No newline at end of file
diff --git a/src/frc846/cpp/frc846/robot/swerve/swerve_module.cc b/src/frc846/cpp/frc846/robot/swerve/swerve_module.cc
index 9a02496..1844156 100644
--- a/src/frc846/cpp/frc846/robot/swerve/swerve_module.cc
+++ b/src/frc846/cpp/frc846/robot/swerve/swerve_module.cc
@@ -26,6 +26,11 @@ SwerveModuleSubsystem::SwerveModuleSubsystem(Loggable& parent,
   cancoder_.GetAbsolutePosition().SetUpdateFrequency(20_Hz);
 
   RegisterPreference("cancoder_offset_", 0.0_deg);
+
+  max_speed_ = frc846::control::base::MotorSpecificationPresets::get(
+                   common_config.motor_types)
+                   .free_speed *
+               common_config.drive_reduction;
 }
 
 std::pair<frc846::control::config::MotorConstructionParameters,
@@ -64,7 +69,7 @@ void SwerveModuleSubsystem::Setup() {
 }
 
 SwerveModuleTarget SwerveModuleSubsystem::ZeroTarget() const {
-  return SwerveModuleOLControlTarget{0.0, 0_deg};
+  return SwerveModuleOLControlTarget{0.0_fps, 0_deg};
 }
 
 bool SwerveModuleSubsystem::VerifyHardware() {
@@ -75,7 +80,7 @@ bool SwerveModuleSubsystem::VerifyHardware() {
 }
 
 void SwerveModuleSubsystem::SetCANCoderOffset() {
-  SetCANCoderOffset(GetReadings().steer_pos);
+  SetCANCoderOffset(cancoder_.GetAbsolutePosition().GetValue());
 }
 void SwerveModuleSubsystem::SetCANCoderOffset(units::degree_t offset) {
   SetPreferenceValue("cancoder_offset_", offset);
@@ -135,12 +140,12 @@ void SwerveModuleSubsystem::WriteToHardware(SwerveModuleTarget target) {
 
     Graph("target/cosine_comp", cosine_comp.to<double>());
 
-    drive_helper_.WriteDC(
-        cosine_comp *
-        ol_target->drive);  // Ignore invert_drive, cosine comp should handle it
+    double drive_duty_cycle = ol_target->drive / max_speed_;
+
+    drive_helper_.WriteDC(cosine_comp * drive_duty_cycle);
 
-    if (std::abs(ol_target->drive) > 0.002) {
-      steer_helper_.WritePosition(ol_target->steer);
+    if (std::abs(drive_duty_cycle) > 0.002) {
+      steer_helper_.WritePositionOnController(ol_target->steer);
     }
   } else if (SwerveModuleTorqueControlTarget* torque_target =
                  std::get_if<SwerveModuleTorqueControlTarget>(&target)) {
diff --git a/src/frc846/include/frc846/math/vectors.h b/src/frc846/include/frc846/math/vectors.h
index 82888d6..416c455 100644
--- a/src/frc846/include/frc846/math/vectors.h
+++ b/src/frc846/include/frc846/math/vectors.h
@@ -195,7 +195,9 @@ template <typename UT, size_t N> class VectorND {
   units::degree_t angle(bool angleIsBearing = false) const {
     assert(N == 2 && "Angle can only be calculated for 2D vectors.");
     if (angleIsBearing) { return units::math::atan2(data[0], data[1]); }
-    return units::math::atan2(data[1], data[0]);
+    try {
+      return units::math::atan2(data[1], data[0]);
+    } catch (std::exception& exc) { return 0_deg; }
   }
 
   // Returns the angle between this vector and another
diff --git a/src/frc846/include/frc846/robot/swerve/control/swerve_ol_calculator.h b/src/frc846/include/frc846/robot/swerve/control/swerve_ol_calculator.h
new file mode 100644
index 0000000..1be0927
--- /dev/null
+++ b/src/frc846/include/frc846/robot/swerve/control/swerve_ol_calculator.h
@@ -0,0 +1,45 @@
+#pragma once
+
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+#include <units/length.h>
+#include <units/velocity.h>
+
+#include <array>
+
+#include "frc846/math/calculator.h"
+#include "frc846/math/vectors.h"
+
+namespace frc846::robot::swerve::control {
+
+struct SwerveOpenLoopCalculatorConstants {
+  units::inch_t wheelbase_horizontal_dim;
+  units::inch_t wheelbase_forward_dim;
+};
+
+struct SwerveOpenLoopCalculatorInputs {
+  frc846::math::VectorND<units::feet_per_second, 2> translation_target;
+  units::degrees_per_second_t rotation_target;
+  units::feet_per_second_t max_speed;
+};
+
+struct SwerveOpenLoopCalculatorOutput {
+  std::array<units::feet_per_second_t, 4> drive_outputs;
+  std::array<units::degree_t, 4> steer_outputs;
+};
+
+/*
+SwerveOpenLoopCalculator
+
+Calculates the open-loop control targets for each swerve module, given target
+translational and rotational velocities.
+*/
+class SwerveOpenLoopCalculator
+    : public frc846::math::Calculator<SwerveOpenLoopCalculatorInputs,
+          SwerveOpenLoopCalculatorOutput, SwerveOpenLoopCalculatorConstants> {
+public:
+  SwerveOpenLoopCalculatorOutput calculate(
+      SwerveOpenLoopCalculatorInputs inputs) override;
+};
+
+}  // namespace frc846::robot::swerve::odometry
diff --git a/src/frc846/include/frc846/robot/swerve/drivetrain.h b/src/frc846/include/frc846/robot/swerve/drivetrain.h
index e71f39a..fc43de4 100644
--- a/src/frc846/include/frc846/robot/swerve/drivetrain.h
+++ b/src/frc846/include/frc846/robot/swerve/drivetrain.h
@@ -5,6 +5,7 @@
 #include <units/angular_acceleration.h>
 
 #include "frc846/robot/GenericSubsystem.h"
+#include "frc846/robot/swerve/control/swerve_ol_calculator.h"
 #include "frc846/robot/swerve/odometry/swerve_odometry_calculator.h"
 #include "frc846/robot/swerve/odometry/swerve_pose.h"
 #include "frc846/robot/swerve/swerve_module.h"
@@ -29,6 +30,8 @@ struct DrivetrainConfigs {
 
   units::inch_t wheelbase_horizontal_dim;
   units::inch_t wheelbase_forward_dim;
+
+  units::feet_per_second_t max_speed;
 };
 
 struct DrivetrainReadings {
@@ -38,6 +41,7 @@ struct DrivetrainReadings {
 // Open-loop control, for use during teleop
 struct DrivetrainOLControlTarget {
   frc846::math::VectorND<units::feet_per_second, 2> velocity;
+  units::degrees_per_second_t angular_velocity;
 };
 
 // Allows for acceleration-based control of the drivetrain
@@ -66,6 +70,10 @@ class DrivetrainSubsystem
 
   bool VerifyHardware() override;
 
+  void ZeroBearing();
+
+  void SetCANCoderOffsets();
+
 private:
   DrivetrainReadings ReadFromHardware() override;
 
@@ -77,6 +85,7 @@ class DrivetrainSubsystem
   AHRS navX_;
 
   frc846::robot::swerve::odometry::SwerveOdometryCalculator odometry_;
+  frc846::robot::swerve::control::SwerveOpenLoopCalculator ol_calculator_;
 };
 
 }  // namespace frc846::robot::swerve
\ No newline at end of file
diff --git a/src/frc846/include/frc846/robot/swerve/swerve_module.h b/src/frc846/include/frc846/robot/swerve/swerve_module.h
index 52989b9..1125343 100644
--- a/src/frc846/include/frc846/robot/swerve/swerve_module.h
+++ b/src/frc846/include/frc846/robot/swerve/swerve_module.h
@@ -28,7 +28,7 @@ struct SwerveModuleTorqueControlTarget {
 
 // For open-loop control by DrivetrainSubsystem
 struct SwerveModuleOLControlTarget {
-  double drive;
+  units::feet_per_second_t drive;
   units::degree_t steer;
 };
 
@@ -132,6 +132,8 @@ class SwerveModuleSubsystem
   frc846::control::HMCHelper<units::degree> steer_helper_;
 
   ctre::phoenix6::hardware::CANcoder cancoder_;
+
+  units::feet_per_second_t max_speed_;
 };
 
 }  // namespace frc846::robot::swerve
\ No newline at end of file
diff --git a/src/y2025/cpp/FunkyRobot.cc b/src/y2025/cpp/FunkyRobot.cc
index 08f648d..a49574b 100644
--- a/src/y2025/cpp/FunkyRobot.cc
+++ b/src/y2025/cpp/FunkyRobot.cc
@@ -10,7 +10,7 @@
 #include <frc2/command/button/Trigger.h>
 #include <hal/Notifier.h>
 
-// #include "commands/teleop/drive_command.h"
+#include "commands/teleop/drive_command.h"
 #include "commands/teleop/leds_command.h"
 #include "control_triggers.h"
 #include "field.h"
@@ -32,13 +32,12 @@ void FunkyRobot::OnInitialize() {
   // }
 
   // // Add dashboard buttons
-  // frc::SmartDashboard::PutData(
-  //     "zero_modules", new frc846::ntinf::NTAction(
-  //                         [this] { container_.drivetrain_.ZeroModules(); }));
-  // frc::SmartDashboard::PutData("zero_bearing",
-  //                              new frc846::ntinf::NTAction([this] {
-  //                                container_.drivetrain_.SetBearing(0_deg);
-  //                              }));
+  frc::SmartDashboard::PutData("set_cancoder_offsets",
+      new frc846::wpilib::NTAction(
+          [this] { container_.drivetrain_.SetCANCoderOffsets(); }));
+  frc::SmartDashboard::PutData(
+      "zero_bearing", new frc846::wpilib::NTAction(
+                          [this] { container_.drivetrain_.ZeroBearing(); }));
 
   // frc::SmartDashboard::PutData(
   //     "zero_odometry", new frc846::ntinf::NTAction(
@@ -51,7 +50,7 @@ void FunkyRobot::OnDisable() {
 }
 
 void FunkyRobot::InitTeleop() {
-  // container_.drivetrain_.SetDefaultCommand(DriveCommand{container_});
+  container_.drivetrain_.SetDefaultCommand(DriveCommand{container_});
   container_.leds_.SetDefaultCommand(LEDsCommand{container_});
 
   ControlTriggerInitializer::InitTeleopTriggers(container_);
diff --git a/src/y2025/cpp/commands/teleop/drive_command.cc b/src/y2025/cpp/commands/teleop/drive_command.cc
new file mode 100644
index 0000000..980ff7f
--- /dev/null
+++ b/src/y2025/cpp/commands/teleop/drive_command.cc
@@ -0,0 +1,56 @@
+#include "commands/teleop/drive_command.h"
+
+DriveCommand::DriveCommand(RobotContainer &container)
+    : frc846::robot::GenericCommand<RobotContainer, DriveCommand>{
+          container, "drive_command"} {
+  AddRequirements({&container_.drivetrain_});
+}
+
+void DriveCommand::OnInit() {}
+
+void DriveCommand::Periodic() {
+  ControlInputReadings ci_readings_{container_.control_input_.GetReadings()};
+
+  frc846::robot::swerve::DrivetrainOLControlTarget target{};
+
+  container_.drivetrain_.SetTarget({target});
+
+  double translate_x = frc846::math::HorizontalDeadband(
+      container_.control_input_.GetReadings().translate_x,
+      container_.control_input_.GetPreferenceValue_double(
+          "translation_deadband"),
+      1,
+      container_.control_input_.GetPreferenceValue_int("translation_exponent"),
+      1);
+
+  double translate_y = frc846::math::HorizontalDeadband(
+      container_.control_input_.GetReadings().translate_y,
+      container_.control_input_.GetPreferenceValue_double(
+          "translation_deadband"),
+      1,
+      container_.control_input_.GetPreferenceValue_int("translation_exponent"),
+      1);
+
+  double rotation = frc846::math::HorizontalDeadband(
+      container_.control_input_.GetReadings().rotation,
+      container_.control_input_.GetPreferenceValue_double("rotation_deadband"),
+      1, container_.control_input_.GetPreferenceValue_int("rotation_exponent"),
+      1);
+
+  units::feet_per_second_t max_speed =
+      container_.drivetrain_
+          .GetPreferenceValue_unit_type<units::feet_per_second_t>("max_speed");
+  units::degrees_per_second_t max_omega =
+      container_.drivetrain_
+          .GetPreferenceValue_unit_type<units::degrees_per_second_t>(
+              "max_omega");
+
+  target.velocity = {translate_x * max_speed, translate_y * max_speed};
+  target.angular_velocity = rotation * max_omega;
+
+  container_.drivetrain_.SetTarget({target});
+}
+
+void DriveCommand::OnEnd(bool interrupted) {}
+
+bool DriveCommand::IsFinished() { return false; }
\ No newline at end of file
diff --git a/src/y2025/cpp/commands/teleop/leds_command.cc b/src/y2025/cpp/commands/teleop/leds_command.cc
index 6b0e799..4b8b908 100644
--- a/src/y2025/cpp/commands/teleop/leds_command.cc
+++ b/src/y2025/cpp/commands/teleop/leds_command.cc
@@ -9,8 +9,6 @@ LEDsCommand::LEDsCommand(RobotContainer &container)
 void LEDsCommand::OnInit() {}
 
 void LEDsCommand::Periodic() {
-  ControlInputReadings ci_readings_{container_.control_input_.GetReadings()};
-
   LEDsState lstate{};
 
   container_.leds_.SetTarget({lstate});
diff --git a/src/y2025/cpp/control_triggers.cc b/src/y2025/cpp/control_triggers.cc
index 3375e2b..8ad3c48 100644
--- a/src/y2025/cpp/control_triggers.cc
+++ b/src/y2025/cpp/control_triggers.cc
@@ -5,13 +5,11 @@
 #include <frc2/command/button/Trigger.h>
 
 void ControlTriggerInitializer::InitTeleopTriggers(RobotContainer& container) {
-  // frc2::Trigger drivetrain_zero_bearing_trigger{
-  //     [&] { return container.control_input_.GetReadings().zero_bearing; }};
+  frc2::Trigger drivetrain_zero_bearing_trigger{[&] {
+    return container.control_input_.GetReadings().zero_bearing;
+  }};
 
-  // drivetrain_zero_bearing_trigger.WhileTrue(
-  //     frc2::InstantCommand([&] {
-  //       container.drivetrain_.SetBearing(
-  //           frc846::util::ShareTables::GetBoolean("is_red_side") ? 0_deg
-  //                                                                : 180_deg);
-  //     }).ToPtr());
+  drivetrain_zero_bearing_trigger.WhileTrue(frc2::InstantCommand([&] {
+    container.drivetrain_.ZeroBearing();
+  }).ToPtr());
 }
\ No newline at end of file
diff --git a/src/y2025/cpp/subsystems/abstract/control_input.cc b/src/y2025/cpp/subsystems/abstract/control_input.cc
index 5665669..87e6c4b 100644
--- a/src/y2025/cpp/subsystems/abstract/control_input.cc
+++ b/src/y2025/cpp/subsystems/abstract/control_input.cc
@@ -6,6 +6,12 @@ ControlInputSubsystem::ControlInputSubsystem()
 
 void ControlInputSubsystem::Setup() {
   RegisterPreference("trigger_threshold", 0.3);
+
+  RegisterPreference("translation_deadband", 0.07);
+  RegisterPreference("translation_exponent", 2);
+
+  RegisterPreference("rotation_deadband", 0.07);
+  RegisterPreference("rotation_exponent", 2);
 }
 
 ControlInputTarget ControlInputSubsystem::ZeroTarget() const {
diff --git a/src/y2025/include/commands/teleop/drive_command.h b/src/y2025/include/commands/teleop/drive_command.h
new file mode 100644
index 0000000..432354f
--- /dev/null
+++ b/src/y2025/include/commands/teleop/drive_command.h
@@ -0,0 +1,18 @@
+#pragma once
+
+#include "frc846/robot/GenericCommand.h"
+#include "subsystems/robot_container.h"
+
+class DriveCommand
+    : public frc846::robot::GenericCommand<RobotContainer, DriveCommand> {
+public:
+  DriveCommand(RobotContainer& container);
+
+  void OnInit() override;
+
+  void Periodic() override;
+
+  void OnEnd(bool interrupted) override;
+
+  bool IsFinished() override;
+};
\ No newline at end of file