diff --git a/southbridge/src/main.rs b/southbridge/src/main.rs
index 9aa906d..4ddc9a7 100644
--- a/southbridge/src/main.rs
+++ b/southbridge/src/main.rs
@@ -1,16 +1,18 @@
 #![no_std]
 #![no_main]
 
-use core::panic::PanicInfo;
+use core::{panic::PanicInfo, sync::atomic::Ordering};
 
 use common::Command;
 use embassy_executor::Spawner;
 use embassy_rp::{bind_interrupts, peripherals::{UART0, UART1, USB}, pwm::{self, Pwm}, uart::{BufferedInterruptHandler, BufferedUart, BufferedUartRx, Config}, usb::Driver};
 use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::Channel};
-use embassy_time::{Duration, Timer};
+use embassy_time::{with_deadline, with_timeout, Duration, Instant, Timer};
 use embedded_io_async::{BufRead, Read};
 use framed::{bytes, FRAME_END_SYMBOL};
-use log::{error, info, warn};
+use log::{error, info, trace, warn};
+use nalgebra::clamp;
+use portable_atomic::AtomicBool;
 use static_cell::{ConstStaticCell, StaticCell};
 
 bind_interrupts!(struct Irqs {
@@ -23,7 +25,7 @@ pub static COMMANDS: Channel<CriticalSectionRawMutex, Command, 5> = Channel::new
 
 #[embassy_executor::task]
 async fn logger_task(driver: Driver<'static, USB>) {
-    embassy_usb_logger::run!(1024, log::LevelFilter::Debug, driver);
+    embassy_usb_logger::run!(1024, log::LevelFilter::Trace, driver);
 }
 
 #[panic_handler]
@@ -44,6 +46,7 @@ async fn main(spawner: Spawner) {
     drive_conf.top = 62500; // 20ms
     drive_conf.compare_b = 4687; // 1.5ms
     drive_conf.compare_a = 4687; // 1.5ms
+    let stopped = drive_conf.clone();
     let mut drive_pwm = Pwm::new_output_ab(p.PWM_SLICE0, p.PIN_16, p.PIN_17, drive_conf.clone());
 
     let config = embassy_rp::i2c::Config::default();
@@ -60,8 +63,50 @@ async fn main(spawner: Spawner) {
 
     spawner.spawn(decoder(rx)).unwrap();
 
+    let enabled = AtomicBool::new(false);
+    let mut enable_watchdog = Instant::now();
+    let enable_watchdog_time = Duration::from_millis(50);
 
     loop {
+        if !enabled.load(Ordering::Acquire) {
+            // stop all motors
+            drive_pwm.set_config(&stopped);
+        }
+
+        let command = if enabled.load(Ordering::Acquire) { 
+            let Ok(command) = with_deadline(enable_watchdog + enable_watchdog_time, COMMANDS.receive()).await else {
+                warn!("no message received");
+                enabled.store(false, Ordering::Release);
+                continue;
+            };
+            command
+        } else {
+            COMMANDS.receive().await
+        };
+
+        match command {
+            Command::Twist(forward, right) => {
+                drive_conf.compare_a = calc_speed(forward - right);
+                drive_conf.compare_b = calc_speed(forward + right);
+                drive_pwm.set_config(&drive_conf);
+            },
+            Command::Stop => { 
+                drive_pwm.set_config(&stopped);
+            },
+            Command::Enable => {
+                enabled.store(true, Ordering::Release);
+                enable_watchdog = Instant::now();
+            },
+            Command::Disable => {
+                enabled.store(false, Ordering::Release);
+                drive_pwm.set_config(&stopped);
+            },
+            Command::FeedWatchdog => {
+                enable_watchdog = Instant::now();
+            }
+            c => { error!("{c:?} unimplemented") }
+        };
+
         info!("Blink");
         Timer::after(Duration::from_millis(100)).await;
     }
@@ -89,8 +134,20 @@ async fn decoder(mut rx: BufferedUartRx<'static, UART1>) {
             continue;
         };
 
+        trace!("received {data:?}");
+
         COMMANDS.send(data).await;
 
         rx.consume(end);
     }
 }
+
+/// -1 to 1
+fn calc_speed(speed: f32) -> u16 {
+    const COUNTS_PER_MS: f32 = 3125.;
+    let speed = speed.clamp(-1., 1.);
+
+    let ms = (speed/2.0)+1.5;
+
+    (COUNTS_PER_MS * ms) as u16
+}