Separate motor code into a module

controller/src/logic.rs → controller/src/logic/mod.rs

-use crate::{
-    encoders::Encoders,
-    tb6612fng::{Movement, Tb6612fng},
-};
-use core::sync::atomic::{AtomicU16, AtomicU32, AtomicU8, Ordering};
+use crate::{encoders::Encoders, tb6612fng::Tb6612fng};
+use core::sync::atomic::{AtomicU8, Ordering};
 use embassy_executor::Spawner;
 use embassy_stm32::{
     pac::{self, wwdg::vals::Wdgtb},
     peripherals::WWDG,
-    time::{hz, Hertz},
-};
-use embassy_sync::{
-    blocking_mutex::raw::CriticalSectionRawMutex, channel::Channel, signal::Signal,
+    time::hz,
 };
+use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, signal::Signal};
 use embassy_time::{Duration, Instant, Ticker};
 use futures::FutureExt;
 use heapless::Deque;
 use i2c2_target::{I2C2, MESSAGE_SIZE};
 
-const STANDBY: u8 = 0x80;
-const STANDBY_BOTH: u16 = ((STANDBY as u16) << 8) | STANDBY as u16;
+mod motor_control;
 
-static MAX_MOTOR_PERCENTAGE: AtomicU8 = AtomicU8::new(100);
-static MOTOR_SPEED: AtomicU16 = AtomicU16::new(STANDBY_BOTH);
-static PWM_FREQUENCY: AtomicU32 = AtomicU32::new(0);
 static WATCHDOG_TICKS: AtomicU8 = AtomicU8::new(5);
 
 struct State {
@@ -76,7 +67,7 @@ pub fn start_i2c_target(
         STATE.as_mut().unwrap().i2c.start(&i2c_callback);
     }
     spawner.spawn(watchdog()).unwrap();
-    spawner.spawn(motor_control(motors)).unwrap();
+    spawner.spawn(motor_control::motor_control(motors)).unwrap();
 }
 
 static WATCHDOG_PING: Signal<CriticalSectionRawMutex, Instant> = Signal::new();
@@ -93,81 +84,9 @@ async fn watchdog() {
             watchdog_expiration = ping
                 + Duration::from_millis(100 * u64::from(WATCHDOG_TICKS.load(Ordering::Relaxed)));
         }
-        if is_moving() && Instant::now() >= watchdog_expiration {
+        if motor_control::is_moving() && Instant::now() >= watchdog_expiration {
             defmt::debug!("stopping motors after watchdog has expired");
-            standby().await;
-        }
-    }
-}
-
-async fn standby() {
-    MOTOR_CONTROL
-        .send(MotorCommand::SetSpeed(STANDBY, STANDBY))
-        .await;
-}
-
-fn is_moving() -> bool {
-    let motor_speed = MOTOR_SPEED.load(Ordering::Relaxed);
-    motor_speed != 0 && motor_speed != STANDBY_BOTH
-}
-
-enum MotorCommand {
-    SetSpeed(u8, u8),
-    SetFrequency(Hertz),
-}
-
-// Set channel size to 4 for:
-//  - two regular motor commands
-//  - one standby command sent by I²C
-//  - one standby command sent by the software watchdog
-static MOTOR_CONTROL: Channel<CriticalSectionRawMutex, MotorCommand, 4> = Channel::new();
-
-#[embassy_executor::task]
-#[allow(clippy::cast_possible_wrap)]
-async fn motor_control(mut motors: Tb6612fng<'static>) {
-    let (mut left, mut right, mut standby) = (0u8, 0u8, true);
-    motors.standby_enter();
-    PWM_FREQUENCY.store(motors.get_frequency().0, Ordering::Relaxed);
-    loop {
-        match MOTOR_CONTROL.receive().await {
-            MotorCommand::SetSpeed(new_left, new_right) => {
-                let (max_pwm, mmp) = (
-                    motors.max_pwm() as i32,
-                    i32::from(MAX_MOTOR_PERCENTAGE.load(Ordering::Relaxed)),
-                );
-                let scaled_speed = |speed| match speed as i8 {
-                    -128 => None,
-                    speed @ (-100..=100) => Some(i32::from(speed) * max_pwm * mmp / (100 * 100)),
-                    speed => {
-                        defmt::warn!("speed {} out of range [-100, 100]", speed);
-                        None
-                    }
-                };
-                let (speed_left, speed_right) = (scaled_speed(new_left), scaled_speed(new_right));
-                if (speed_left, speed_right) == (None, None) {
-                    motors.standby_enter();
-                    standby = true;
-                    (left, right) = (new_left, new_right);
-                } else {
-                    if standby {
-                        motors.standby_leave();
-                        standby = false;
-                    }
-                    if let Some(speed_left) = speed_left {
-                        motors.move_left(Movement::Advance(speed_left));
-                        left = new_left;
-                    }
-                    if let Some(speed_right) = speed_right {
-                        motors.move_right(Movement::Advance(speed_right));
-                        right = new_right;
-                    }
-                }
-                MOTOR_SPEED.store((u16::from(left) << 8) | u16::from(right), Ordering::Relaxed);
-            }
-            MotorCommand::SetFrequency(freq) => {
-                motors.set_frequency(freq);
-                PWM_FREQUENCY.store(freq.0, Ordering::Relaxed);
-            }
+            motor_control::standby().await;
         }
     }
 }
@@ -180,7 +99,7 @@ fn i2c_callback(command: &[u8], response: &mut Deque<u8, MESSAGE_SIZE>) {
     } else {
         // If this does not succeed, this will reset due to
         // a lack of petting the window watchdog.
-        while standby().now_or_never().is_none() {}
+        while motor_control::standby().now_or_never().is_none() {}
     }
 }
 
@@ -214,9 +133,9 @@ fn process_command(
         &[CMD_PWM_FREQUENCY, a, b, c] => {
             let f = u32::from_le_bytes([a, b, c, 0]);
             if (1..=100_000).contains(&f) {
-                if MOTOR_CONTROL
-                    .try_send(MotorCommand::SetFrequency(hz(f)))
-                    .is_err()
+                if motor_control::set_pwm_frequency(hz(f))
+                    .now_or_never()
+                    .is_none()
                 {
                     defmt::error!("unable to queue up motor frequency");
                     return false;
@@ -227,14 +146,14 @@ fn process_command(
             }
         }
         [CMD_PWM_FREQUENCY] => {
-            let freq = PWM_FREQUENCY.load(Ordering::Relaxed);
-            for &b in &freq.to_le_bytes()[..3] {
+            let freq = motor_control::get_pwm_frequency();
+            for &b in &freq.0.to_le_bytes()[..3] {
                 response.push_back(b).unwrap();
             }
         }
         &[CMD_MAX_MOTOR_PERCENTAGE, p] => {
             if (1..=100).contains(&p) {
-                MAX_MOTOR_PERCENTAGE.store(p, Ordering::Relaxed);
+                motor_control::set_max_motor_percentage(p);
             } else {
                 defmt::warn!("incorrect max percentage {}", p);
                 return false;
@@ -242,22 +161,22 @@ fn process_command(
         }
         [CMD_MAX_MOTOR_PERCENTAGE] => {
             response
-                .push_back(MAX_MOTOR_PERCENTAGE.load(Ordering::Relaxed))
+                .push_back(motor_control::get_max_motor_percentage())
                 .unwrap();
         }
         &[CMD_MOTOR_SPEED, left, right] => {
-            if MOTOR_CONTROL
-                .try_send(MotorCommand::SetSpeed(left, right))
-                .is_err()
+            if motor_control::set_speed(left, right)
+                .now_or_never()
+                .is_none()
             {
                 defmt::error!("unable to queue up motor speeds");
                 return false;
             }
         }
         [CMD_MOTOR_SPEED] => {
-            let motor_speed = MOTOR_SPEED.load(Ordering::Relaxed);
-            response.push_back((motor_speed >> 8) as u8).unwrap();
-            response.push_back(motor_speed as u8).unwrap();
+            let (left, right) = motor_control::get_speed();
+            response.push_back(left).unwrap();
+            response.push_back(right).unwrap();
         }
         &[CMD_MOTOR_SHUTDOWN_TIMEOUT, ticks] => {
             if (1..=100).contains(&ticks) {
@@ -281,7 +200,9 @@ fn process_command(
             response.push_back(right[1]).unwrap();
         }
         [CMD_STATUS] => {
-            response.push_back(u8::from(is_moving())).unwrap();
+            response
+                .push_back(u8::from(motor_control::is_moving()))
+                .unwrap();
         }
         _ => {
             defmt::warn!("unknown command or args {:#04x}", command);

controller/src/logic/motor_control.rs

+use crate::tb6612fng::{Movement, Tb6612fng};
+use core::sync::atomic::{AtomicU16, AtomicU32, AtomicU8, Ordering};
+use embassy_stm32::time::{hz, Hertz};
+use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::Channel};
+
+const STANDBY: u8 = 0x80;
+const STANDBY_BOTH: u16 = ((STANDBY as u16) << 8) | STANDBY as u16;
+
+static MAX_MOTOR_PERCENTAGE: AtomicU8 = AtomicU8::new(100);
+static MOTOR_SPEED: AtomicU16 = AtomicU16::new(STANDBY_BOTH);
+static PWM_FREQUENCY: AtomicU32 = AtomicU32::new(0);
+
+pub fn set_max_motor_percentage(percent: u8) {
+    MAX_MOTOR_PERCENTAGE.store(percent, Ordering::Relaxed);
+}
+
+pub fn get_max_motor_percentage() -> u8 {
+    MAX_MOTOR_PERCENTAGE.load(Ordering::Relaxed)
+}
+
+pub async fn set_speed(left: u8, right: u8) {
+    MOTOR_CONTROL
+        .send(MotorCommand::SetSpeed(left, right))
+        .await;
+}
+
+#[allow(clippy::cast_possible_truncation)]
+pub fn get_speed() -> (u8, u8) {
+    let speed = MOTOR_SPEED.load(Ordering::Relaxed);
+    ((speed >> 8) as u8, speed as u8)
+}
+
+pub async fn standby() {
+    set_speed(STANDBY, STANDBY).await;
+}
+
+pub fn is_moving() -> bool {
+    let motor_speed = MOTOR_SPEED.load(Ordering::Relaxed);
+    motor_speed != 0 && motor_speed != STANDBY_BOTH
+}
+
+enum MotorCommand {
+    SetSpeed(u8, u8),
+    SetFrequency(Hertz),
+}
+
+pub async fn set_pwm_frequency(freq: Hertz) {
+    MOTOR_CONTROL.send(MotorCommand::SetFrequency(freq)).await;
+}
+
+pub fn get_pwm_frequency() -> Hertz {
+    hz(PWM_FREQUENCY.load(Ordering::Relaxed))
+}
+
+// Set channel size to 4 for:
+//  - two regular motor commands
+//  - one standby command sent by I²C
+//  - one standby command sent by the software watchdog
+static MOTOR_CONTROL: Channel<CriticalSectionRawMutex, MotorCommand, 4> = Channel::new();
+
+#[embassy_executor::task]
+#[allow(clippy::cast_possible_wrap)]
+pub async fn motor_control(mut motors: Tb6612fng<'static>) {
+    let (mut left, mut right, mut standby) = (0u8, 0u8, true);
+    motors.standby_enter();
+    PWM_FREQUENCY.store(motors.get_frequency().0, Ordering::Relaxed);
+    loop {
+        match MOTOR_CONTROL.receive().await {
+            MotorCommand::SetSpeed(new_left, new_right) => {
+                let (max_pwm, mmp) = (
+                    motors.max_pwm() as i32,
+                    i32::from(MAX_MOTOR_PERCENTAGE.load(Ordering::Relaxed)),
+                );
+                let scaled_speed = |speed| match speed as i8 {
+                    -128 => None,
+                    speed @ (-100..=100) => Some(i32::from(speed) * max_pwm * mmp / (100 * 100)),
+                    speed => {
+                        defmt::warn!("speed {} out of range [-100, 100]", speed);
+                        None
+                    }
+                };
+                let (speed_left, speed_right) = (scaled_speed(new_left), scaled_speed(new_right));
+                if (speed_left, speed_right) == (None, None) {
+                    motors.standby_enter();
+                    standby = true;
+                    (left, right) = (new_left, new_right);
+                } else {
+                    if standby {
+                        motors.standby_leave();
+                        standby = false;
+                    }
+                    if let Some(speed_left) = speed_left {
+                        motors.move_left(Movement::Advance(speed_left));
+                        left = new_left;
+                    }
+                    if let Some(speed_right) = speed_right {
+                        motors.move_right(Movement::Advance(speed_right));
+                        right = new_right;
+                    }
+                }
+                MOTOR_SPEED.store((u16::from(left) << 8) | u16::from(right), Ordering::Relaxed);
+            }
+            MotorCommand::SetFrequency(freq) => {
+                motors.set_frequency(freq);
+                PWM_FREQUENCY.store(freq.0, Ordering::Relaxed);
+            }
+        }
+    }
+}