From 4b50504ca3fd43e04b99b114ef557a180d633aab Mon Sep 17 00:00:00 2001
From: Samuel Tardieu <sam@rfc1149.net>
Date: Thu, 11 Jul 2024 21:39:45 +0200
Subject: [PATCH] Implement watchdog command
---
Cargo.lock | 1 +
README.org | 7 +-
controller/Cargo.toml | 1 +
controller/python/controller.py | 14 +++
controller/src/logic.rs | 148 +++++++++++++++++++++++---------
5 files changed, 126 insertions(+), 45 deletions(-)
diff --git a/Cargo.lock b/Cargo.lock
index 5e429b0..180589f 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -133,6 +133,7 @@ version = "0.1.0"
dependencies = [
"cortex-m",
"cortex-m-rt",
+ "critical-section",
"defmt",
"defmt-rtt",
"embassy-executor",
diff --git a/README.org b/README.org
index b0cd2a0..1508a2b 100644
--- a/README.org
+++ b/README.org
@@ -77,9 +77,10 @@ Values are exchanged in little endian format.
- In automatic motor distance, time in 100th of seconds to reach the maximum speed
or to decrease the speed from the maximum to 0.
-** 0x28 Motor shutdown timeout (R/W)
+** [IMPLEMENTED] 0x28 Motor shutdown timeout (R/W)
-- Number of tenths of seconds in before the motor shut down
+- Number of tenths of seconds in before the motor shut down. The minimum is 1
+ (0.1 seconds) and the maximum is 100 (10 seconds).
** [IMPLEMENTED] 0x30-0x31 Motor speed (R/W)
@@ -92,7 +93,7 @@ Values are exchanged in little endian format.
- Left and right encoder counters since last time, 2 complement, 2 bytes each
-** 0x36 Status (R)
+** [IMPLEMENTED] 0x36 Status (R)
- Bit 0: moving (1) or idle (0)
diff --git a/controller/Cargo.toml b/controller/Cargo.toml
index 90f59b0..ff8dfc8 100644
--- a/controller/Cargo.toml
+++ b/controller/Cargo.toml
@@ -7,6 +7,7 @@ edition = "2021"
[dependencies]
cortex-m = { version = "0.7.7", features = ["critical-section-single-core", "inline-asm"] }
cortex-m-rt = "0.7.3"
+critical-section = "1.1.2"
defmt = "0.3.5"
defmt-rtt = "0.4.0"
embassy-executor = { git = "https://github.com/embassy-rs/embassy", features = ["arch-cortex-m", "executor-thread", "defmt", "integrated-timers"] }
diff --git a/controller/python/controller.py b/controller/python/controller.py
index 351916f..44a4c1b 100644
--- a/controller/python/controller.py
+++ b/controller/python/controller.py
@@ -9,6 +9,7 @@ class Controller:
WHO_AM_I = 0x0F
MAX_MOTOR_PERCENTAGE = 0x11
+ MOTOR_SHUTDOWN_TIMEOUT = 0x28
MOTOR_SPEED = 0x30
ENCODER_TICKS = 0x32
STATUS = 0x36
@@ -96,3 +97,16 @@ class Controller:
- "moving": True if at least one motor is moving, False otherwise"""
(status,) = self._read(self.STATUS, 1, "?")
return {"moving": (status & 1) != 0}
+
+ def set_motor_shutdown_timeout(self, duration: float):
+ """Set the duration in seconds after which the motors will shut down if no valid
+ command is received. The minimum is 0.1 seconds, the maximum is 10 seconds.
+ """
+ if duration < 0.1 or duration > 10.0:
+ raise ValueError
+ self._write(self.MOTOR_SHUTDOWN_TIMEOUT, [round(duration * 10)])
+
+ def get_motor_shutdown_timeout(self) -> float:
+ """Get the duration in seconds after which the motors will shut down if no valid
+ command is received."""
+ return self._read(self.MOTOR_SHUTDOWN_TIMEOUT, 1, "B")[0] / 10
diff --git a/controller/src/logic.rs b/controller/src/logic.rs
index 3168fa9..4c9da50 100644
--- a/controller/src/logic.rs
+++ b/controller/src/logic.rs
@@ -2,6 +2,8 @@ use crate::{
encoders::Encoders,
tb6612fng::{Movement, Tb6612fng},
};
+use core::sync::atomic::{AtomicU32, Ordering};
+use embassy_time::{Instant, Timer};
use heapless::Deque;
use i2c2_target::{I2C2, MESSAGE_SIZE};
@@ -12,6 +14,8 @@ struct State {
max_motor_percentage: u8,
motor_speed: [u8; 2],
standby: bool,
+ /// Number of 1/10th of seconds
+ watchdog_ticks: u8,
}
impl State {
@@ -23,12 +27,65 @@ impl State {
max_motor_percentage: 100,
motor_speed: [0, 0],
standby: true,
+ watchdog_ticks: 5,
}
}
+
+ fn set_motor_speed(&mut self, ms: [u8; 2]) {
+ let (max_pwm, mmp) = (
+ self.motors.max_pwm() as i32,
+ i32::from(self.max_motor_percentage),
+ );
+ 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(ms[0]), scaled_speed(ms[1]));
+ if (speed_left, speed_right) == (None, None) {
+ self.motors.standby_enter();
+ self.standby = true;
+ self.motor_speed = [0, 0];
+ } else {
+ if self.standby {
+ self.motors.standby_leave();
+ self.standby = false;
+ }
+ if let Some(speed_left) = speed_left {
+ self.motors.move_left(Movement::Advance(speed_left));
+ self.motor_speed[0] = ms[0];
+ }
+ if let Some(speed_right) = speed_right {
+ self.motors.move_right(Movement::Advance(speed_right));
+ self.motor_speed[1] = ms[1];
+ }
+ }
+ }
+
+ fn standby(&mut self) {
+ self.set_motor_speed([0x80, 0x80]);
+ }
+
+ fn is_moving(&self) -> bool {
+ self.motor_speed != [0, 0]
+ }
+
+ fn with_critical_section<R, F: FnMut(&mut State) -> R>(mut f: F) -> R {
+ let state = unsafe { STATE.as_mut().unwrap() };
+ critical_section::with(|_| f(state))
+ }
}
static mut STATE: Option<State> = None;
+/// Date of the next watchdog expiration. A u32 value can hold up to
+/// 49 days which is more than enough. It is not stored in the state
+/// to be able to be checked outside any critical section.
+static WATCHDOG_EXPIRATION: AtomicU32 = AtomicU32::new(0);
+
#[embassy_executor::task]
pub async fn start_i2c_target(
i2c2: I2C2,
@@ -38,21 +95,48 @@ pub async fn start_i2c_target(
motors.standby_enter();
unsafe {
STATE = Some(State::new(i2c2, motors, encoders));
- STATE.as_mut().unwrap().i2c.start(CALLBACK);
+ STATE.as_mut().unwrap().i2c.start(&i2c_callback);
+ }
+ loop {
+ Timer::after_millis(100).await;
+ if Instant::now().as_millis() as u32 >= WATCHDOG_EXPIRATION.load(Ordering::Relaxed) {
+ State::with_critical_section(|state| {
+ if state.is_moving()
+ && Instant::now().as_millis() as u32
+ >= WATCHDOG_EXPIRATION.load(Ordering::Relaxed)
+ {
+ state.standby();
+ }
+ });
+ }
}
}
-const CALLBACK: i2c2_target::Callback = &handle_command;
+fn i2c_callback(command: &[u8], response: &mut Deque<u8, MESSAGE_SIZE>) {
+ #[allow(static_mut_refs)]
+ let state = unsafe { STATE.as_mut().unwrap() };
+ if process_command(state, command, response) {
+ WATCHDOG_EXPIRATION.store(
+ Instant::now().as_millis() as u32 + 100 * state.watchdog_ticks as u32,
+ Ordering::Relaxed,
+ );
+ } else {
+ state.standby();
+ }
+}
const WHO_AM_I: u8 = 0x0f;
const MAX_MOTOR_PERCENTAGE: u8 = 0x11;
+const MOTOR_SHUTDOWN_TIMEOUT: u8 = 0x28;
const MOTOR_SPEED: u8 = 0x30;
const ENCODER_TICKS: u8 = 0x32;
const STATUS: u8 = 0x36;
-fn handle_command(command: &[u8], response: &mut Deque<u8, MESSAGE_SIZE>) {
- #[allow(static_mut_refs)]
- let state = unsafe { STATE.as_mut().unwrap() };
+fn process_command(
+ state: &mut State,
+ command: &[u8],
+ response: &mut Deque<u8, MESSAGE_SIZE>,
+) -> bool {
defmt::trace!("Processing command {:?}", command);
match command {
[WHO_AM_I, ..] => {
@@ -63,48 +147,30 @@ fn handle_command(command: &[u8], response: &mut Deque<u8, MESSAGE_SIZE>) {
state.max_motor_percentage = p;
} else {
defmt::warn!("Incorrect max percentage {}", p);
+ return false;
}
}
[MAX_MOTOR_PERCENTAGE] => {
response.push_back(state.max_motor_percentage).unwrap();
}
[MOTOR_SPEED, ms @ ..] if ms.len() == 2 => {
- let (max_pwm, mmp) = (
- state.motors.max_pwm() as i32,
- i32::from(state.max_motor_percentage),
- );
- 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(ms[0]), scaled_speed(ms[1]));
- if (speed_left, speed_right) == (None, None) {
- state.motors.standby_enter();
- state.standby = true;
- state.motor_speed = [0, 0];
- } else {
- if state.standby {
- state.motors.standby_leave();
- state.standby = false;
- }
- if let Some(speed_left) = speed_left {
- state.motors.move_left(Movement::Advance(speed_left));
- state.motor_speed[0] = ms[0];
- }
- if let Some(speed_right) = speed_right {
- state.motors.move_right(Movement::Advance(speed_right));
- state.motor_speed[1] = ms[1];
- }
- }
+ state.set_motor_speed([ms[0], ms[1]]);
}
[MOTOR_SPEED] => {
response.push_back(state.motor_speed[0]).unwrap();
response.push_back(state.motor_speed[1]).unwrap();
}
+ &[MOTOR_SHUTDOWN_TIMEOUT, ticks] => {
+ if (1..=100).contains(&ticks) {
+ state.watchdog_ticks = ticks;
+ } else {
+ defmt::warn!("ticks out of range: {}", ticks);
+ return false;
+ }
+ }
+ [MOTOR_SHUTDOWN_TIMEOUT] => {
+ response.push_back(state.watchdog_ticks).unwrap();
+ }
[ENCODER_TICKS] => {
let (left, right) = state.encoders.ticks();
let (left, right) = (left.to_le_bytes(), right.to_le_bytes());
@@ -114,14 +180,12 @@ fn handle_command(command: &[u8], response: &mut Deque<u8, MESSAGE_SIZE>) {
response.push_back(right[1]).unwrap();
}
[STATUS] => {
- let moving = (state.motor_speed != [0, 0]) as u8;
- response.push_back(moving << 0).unwrap();
+ response.push_back(state.is_moving() as u8).unwrap();
}
- &[_, ..] => {
+ _ => {
defmt::warn!("unknown command or args {:#04x}", command);
- }
- &[] => {
- defmt::warn!("received empty command");
+ return false;
}
}
+ true
}
--
GitLab