diff --git a/src/tb6612fng.rs b/src/tb6612fng.rs
index 163ad428655fb4d0fc0b49c58035744fec3843bc..ff637ec5ec908e6acfbb511ccae3299054fdb99e 100644
--- a/src/tb6612fng.rs
+++ b/src/tb6612fng.rs
@@ -1,3 +1,5 @@
+use core::ops::Neg;
+
 use rtic_monotonics::systick::fugit::Hertz;
 use stm32f1xx_hal::{
     afio::MAPR,
@@ -29,13 +31,14 @@ pub enum Movement {
     Stop,
 }
 
-impl Movement {
-    fn into_command(self) -> (PinState, PinState, u16) {
-        match self {
-            Movement::Brake => (PinState::High, PinState::High, 0),
-            Movement::Stop => (PinState::Low, PinState::Low, 0),
-            Movement::Advance(v) if v >= 0 => (PinState::High, PinState::Low, v as u16),
-            Movement::Advance(v) => (PinState::Low, PinState::High, (-v) as u16),
+impl Neg for Movement {
+    type Output = Movement;
+
+    fn neg(self) -> Self::Output {
+        if let Movement::Advance(v) = self {
+            Movement::Advance(-v)
+        } else {
+            self
         }
     }
 }
@@ -79,8 +82,8 @@ impl Tb6612fng {
         }
     }
 
-    pub fn max_pwm(&self) -> u16 {
-        self.pwm.get_max_duty()
+    pub fn max_pwm(&self) -> i32 {
+        self.pwm.get_max_duty() as i32
     }
 
     pub fn standby_enter(&mut self) {
@@ -88,23 +91,19 @@ impl Tb6612fng {
     }
 
     pub fn standby_leave(&mut self) {
+        self.move_both(Movement::Stop, Movement::Stop);
         self.standby.set_high();
     }
 
     pub fn move_right(&mut self, value: Movement) {
-        let (i1, i2, duty) = value.into_command();
+        let (i1, i2, duty) = self.into_command(value);
         self.a1.set_state(i1);
         self.a2.set_state(i2);
         self.pwm.set_duty(Channel::C3, duty);
     }
 
     pub fn move_left(&mut self, value: Movement) {
-        let value = if let Movement::Advance(v) = value {
-            Movement::Advance(-v)
-        } else {
-            value
-        };
-        let (i1, i2, duty) = value.into_command();
+        let (i1, i2, duty) = self.into_command(-value);
         self.b1.set_state(i1);
         self.b2.set_state(i2);
         self.pwm.set_duty(Channel::C4, duty);
@@ -114,4 +113,18 @@ impl Tb6612fng {
         self.move_left(left);
         self.move_right(right);
     }
+
+    fn into_command(&self, movement: Movement) -> (PinState, PinState, u16) {
+        let max_duty = self.max_pwm();
+        match movement {
+            Movement::Brake | Movement::Advance(0) => {
+                (PinState::High, PinState::High, max_duty as u16)
+            }
+            Movement::Stop => (PinState::Low, PinState::Low, 0),
+            Movement::Advance(v) if v > 0 => {
+                (PinState::High, PinState::Low, v.min(max_duty) as u16)
+            }
+            Movement::Advance(v) => (PinState::Low, PinState::High, (-v).min(max_duty) as u16),
+        }
+    }
 }