From 5afddae6e4153dda8ab05a07f6ffe4481f63530e Mon Sep 17 00:00:00 2001
From: Samuel Tardieu <sam@rfc1149.net>
Date: Wed, 17 Jul 2024 00:40:50 +0200
Subject: [PATCH] Separate motor code into a module
---
controller/src/{logic.rs => logic/mod.rs} | 129 +++++-----------------
controller/src/logic/motor_control.rs | 109 ++++++++++++++++++
2 files changed, 134 insertions(+), 104 deletions(-)
rename controller/src/{logic.rs => logic/mod.rs} (56%)
create mode 100644 controller/src/logic/motor_control.rs
diff --git a/controller/src/logic.rs b/controller/src/logic/mod.rs
similarity index 56%
rename from controller/src/logic.rs
rename to controller/src/logic/mod.rs
index 1a4e426..00ee9fc 100644
--- a/controller/src/logic.rs
+++ b/controller/src/logic/mod.rs
@@ -1,28 +1,19 @@
-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);
diff --git a/controller/src/logic/motor_control.rs b/controller/src/logic/motor_control.rs
new file mode 100644
index 0000000..97ba144
--- /dev/null
+++ b/controller/src/logic/motor_control.rs
@@ -0,0 +1,109 @@
+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);
+ }
+ }
+ }
+}
--
GitLab