diff --git a/.cargo/config.toml b/.cargo/config.toml
index 6c3c3f8e780200345c4278a702b30280d7cff159..971c54803626c62bbb93ae6a47d043394d8c3dc9 100644
--- a/.cargo/config.toml
+++ b/.cargo/config.toml
@@ -8,3 +8,6 @@ rustflags = [
[build]
target = "thumbv7m-none-eabi"
+
+[env]
+DEFMT_LOG = "info"
diff --git a/Cargo.lock b/Cargo.lock
index 10b48f3ddab3b0caefbc335da7956388622c7965..f5040fab89ca2b83e1f24aba8edc340eda5abbd4 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -145,6 +145,7 @@ dependencies = [
"embassy-executor",
"embassy-stm32",
"embassy-time",
+ "i2c2-slave",
"panic-probe",
]
@@ -594,10 +595,6 @@ dependencies = [
"defmt",
]
-[[package]]
-name = "pid"
-version = "0.1.0"
-
[[package]]
name = "pin-project-lite"
version = "0.2.13"
diff --git a/README.org b/README.org
index a678f37005958a456b9dadc46ccba6ba874f1dbc..b0cd2a0d592671a9ee2326f3e2e02f2c368fd1b6 100644
--- a/README.org
+++ b/README.org
@@ -48,7 +48,7 @@ the TB6612FNG motor driver.
Values are exchanged in little endian format.
-** 0x0F Who am I? (R)
+** [IMPLEMENTED] 0x0F Who am I? (R)
- The I²C address
@@ -56,7 +56,7 @@ Values are exchanged in little endian format.
- Frequency in kHz, from 1 to 100 (default: 1)
-** 0x11 Max motor percentage (R/W)
+** [IMPLEMENTED] 0x11 Max motor percentage (R/W)
- Limit the maximum speed, between 1% and 100% of the duty cycle
(default: 100)
@@ -81,16 +81,16 @@ Values are exchanged in little endian format.
- Number of tenths of seconds in before the motor shut down
-** 0x30-0x31 Motor speed (R/W)
+** [IMPLEMENTED] 0x30-0x31 Motor speed (R/W)
- Left and right motor speed, between -127 and 127 each, max speed is
±127. -128 means standby. ±127 corresponds to the value limited by the
max motor percentage.
- If written while in automatic mode, the robot switches to manual mode
-** 0x32-0x35 Encoder ticks (R)
+** [IMPLEMENTED] 0x32-0x35 Encoder ticks (R)
-- Left and right encoder counters, 2 complement, 2 bytes each
+- Left and right encoder counters since last time, 2 complement, 2 bytes each
** 0x36 Status (R)
diff --git a/controller/Cargo.toml b/controller/Cargo.toml
index 0b6750e78ce4115a516c42809ef19f1f057cc22f..cee26e164b348095bba0761a01ab0e046ac19292 100644
--- a/controller/Cargo.toml
+++ b/controller/Cargo.toml
@@ -12,4 +12,5 @@ defmt-rtt = "0.4.0"
embassy-executor = { git = "https://github.com/embassy-rs/embassy", features = ["arch-cortex-m", "executor-thread", "defmt", "integrated-timers"] }
embassy-stm32 = { git = "https://github.com/embassy-rs/embassy", features = ["defmt", "stm32f103c8", "unstable-pac", "time-driver-tim4", "memory-x"] }
embassy-time = { git = "https://github.com/embassy-rs/embassy", features = ["defmt", "defmt-timestamp-uptime", "tick-hz-32_768"] }
+i2c2-slave = { path = "../i2c2-slave" }
panic-probe = { version = "0.3.1", features = ["print-defmt"] }
diff --git a/controller/python/controller.py b/controller/python/controller.py
new file mode 100644
index 0000000000000000000000000000000000000000..baa0fd52815a6c00b83ed4b8901c259c18e52a69
--- /dev/null
+++ b/controller/python/controller.py
@@ -0,0 +1,58 @@
+import smbus
+import struct
+from typing import Optional
+
+class Controller:
+
+ I2C_BUS = 1
+ I2C_ADDR = 0x57
+
+ WHO_AM_I = 0x0f
+ MAX_MOTOR_PERCENTAGE = 0x11
+ MOTOR_SPEED = 0x30
+ ENCODER_TICKS = 0x32
+
+ def __init__(self):
+ self.i2c = smbus.SMBus(1)
+
+ def who_am_i(self) -> int:
+ """Check that the motors controller board is present. This should return
+ the same value as Controller.I2C_ADDR."""
+ return self.i2c.read_i2c_block_data(self.I2C_ADDR, self.WHO_AM_I, 1)[0]
+
+ def set_max_percentage(self, percent: int):
+ """Set the maximum percentage of power which will be used (between 1 and 100)."""
+ if percent <= 0 or percent > 100:
+ raise ValueError
+ self.i2c.write_i2c_block_data(self.I2C_ADDR, self.MAX_MOTOR_PERCENTAGE, [percent])
+
+ def set_motor_speed(self, left: Optional[int], right: Optional[int]):
+ """Set the motor speed between -127 and 127. None means not to change the
+ motor value. Using None for both motors will put the controller board in standby
+ mode and motors will stop."""
+ def convert(v, arg):
+ if v is None:
+ return -128
+ if not isinstance(v, int) or v < -127 or v > 127:
+ raise ValueError(f"{arg} must be an integer between -127 and 127 or None")
+ return v
+ self.i2c.write_i2c_block_data(self.I2C_ADDR, self.MOTOR_SPEED,
+ list(struct.pack('bb', convert(left, "left"), convert(right, "right"))))
+
+ def set_left_motor_speed(self, speed: int):
+ """Set the left motor speed between -127 and 127."""
+ self.set_motor_speed(speed, None)
+
+ def set_right_motor_speed(self, speed: int):
+ """Set the right motor speed between -127 and 127."""
+ self.set_motor_speed(None, speed)
+
+ def standby(self):
+ """Stop the motors by putting the controller board in standby mode."""
+ self.set_motor_speed(None, None)
+
+ def get_encoder_ticks(self) -> (int, int):
+ """Retrieve the encoder ticks since the last time it was queried. The ticks must be
+ retrieved before they overflow a 2 byte signed integer (-32768..32767) or the result will
+ make no sense. Return a pair with left and right data."""
+ return struct.unpack('hh', bytes(self.i2c.read_i2c_block_data(self.I2C_ADDR, self.ENCODER_TICKS, 4)))
diff --git a/controller/src/logic.rs b/controller/src/logic.rs
new file mode 100644
index 0000000000000000000000000000000000000000..c6d3e7b52f34beb03e026e438adc5a7fda690c2e
--- /dev/null
+++ b/controller/src/logic.rs
@@ -0,0 +1,114 @@
+use crate::{
+ encoders::Encoders,
+ tb6612fng::{Movement, Tb6612fng},
+};
+use i2c2_slave::I2C2;
+
+struct State {
+ i2c: I2C2,
+ motors: Tb6612fng<'static>,
+ encoders: Encoders<'static>,
+ max_motor_percentage: u8,
+ motor_speed: [u8; 2],
+ standby: bool,
+}
+
+impl State {
+ fn new(i2c2: I2C2, motors: Tb6612fng<'static>, encoders: Encoders<'static>) -> Self {
+ Self {
+ i2c: i2c2,
+ motors,
+ encoders,
+ max_motor_percentage: 100,
+ motor_speed: [0, 0],
+ standby: true,
+ }
+ }
+}
+
+#[derive(Debug)]
+enum Error {
+ #[allow(unused)]
+ I2c(i2c2_slave::Error),
+ #[allow(unused)]
+ UnknownRegister(u8),
+}
+
+impl From<i2c2_slave::Error> for Error {
+ fn from(value: i2c2_slave::Error) -> Self {
+ Error::I2c(value)
+ }
+}
+
+type Result<T, E = Error> = core::result::Result<T, E>;
+
+#[embassy_executor::task]
+pub async fn main_loop(i2c2: I2C2, mut motors: Tb6612fng<'static>, encoders: Encoders<'static>) {
+ motors.standby_enter();
+ let mut state = State::new(i2c2, motors, encoders);
+ let mut buffer = [0; 8];
+ loop {
+ match state.i2c.receive(&mut buffer).await {
+ Ok(n) => {
+ if let Err(e) = handle_command(&buffer[..n], &mut state) {
+ defmt::warn!("Error processing command: {:?}", defmt::Debug2Format(&e));
+ }
+ }
+ Err(e) => {
+ defmt::warn!("I2C receive error: {:?}", defmt::Debug2Format(&e));
+ }
+ }
+ }
+}
+
+const WHO_AM_I: u8 = 0x0f;
+const MAX_MOTOR_PERCENTAGE: u8 = 0x11;
+const MOTOR_SPEED: u8 = 0x30;
+const ENCODER_TICKS: u8 = 0x32;
+
+fn handle_command(command: &[u8], state: &mut State) -> Result<()> {
+ defmt::trace!("Processing command {:?}", command);
+ match command {
+ [WHO_AM_I, ..] => state.i2c.send(&[0x57])?,
+ &[MAX_MOTOR_PERCENTAGE, p, ..] => {
+ if (1..=100).contains(&p) {
+ state.max_motor_percentage = p;
+ } else {
+ defmt::warn!("Incorrect max percentage {}", p);
+ }
+ }
+ [MAX_MOTOR_PERCENTAGE] => state.i2c.send(&[state.max_motor_percentage])?,
+ [MOTOR_SPEED, ms @ ..] if ms.len() >= 2 => {
+ state.motor_speed = [ms[0], ms[1]];
+ if ms[0] == 0x80 && ms[1] == 0x80 {
+ state.motors.standby_enter();
+ state.standby = true;
+ } else {
+ if state.standby {
+ state.motors.standby_leave();
+ state.standby = false;
+ }
+ let max_pwm = state.motors.max_pwm() / 100;
+ if ms[0] != 0x80 {
+ state.motors.move_left(Movement::Advance(
+ ms[0] as i8 as i32 * state.max_motor_percentage as i32 * max_pwm / 100,
+ ));
+ }
+ if ms[1] != 0x80 {
+ state.motors.move_right(Movement::Advance(
+ ms[1] as i8 as i32 * state.max_motor_percentage as i32 * max_pwm / 100,
+ ));
+ }
+ }
+ }
+ [MOTOR_SPEED, ..] => state.i2c.send(&state.motor_speed)?,
+ [ENCODER_TICKS, ..] => {
+ let (left, right) = state.encoders.ticks();
+ let (left, right) = (left.to_le_bytes(), right.to_le_bytes());
+ state.i2c.send(&[left[0], left[1], right[0], right[1]])?;
+ }
+ &[idx, ..] => return Err(Error::UnknownRegister(idx)),
+ [] => (),
+ }
+ Ok(())
+}
diff --git a/controller/src/main.rs b/controller/src/main.rs
index f67e326bd7da3d0e6105f7b41c65a50d7a921f6f..6b20a278e688a9ccd02a805d48cc50b2fe6bd286 100644
--- a/controller/src/main.rs
+++ b/controller/src/main.rs
@@ -1,31 +1,23 @@
#![no_std]
#![no_main]
-use crate::{
- encoders::Encoders,
- tb6612fng::{Movement, Tb6612fng},
-};
+use crate::{encoders::Encoders, tb6612fng::Tb6612fng};
use defmt_rtt as _;
use embassy_executor::Spawner;
use embassy_stm32::{
- bind_interrupts,
- dma::NoDma,
gpio::{Level, Output, Speed},
- peripherals::{PB15, USART2},
+ interrupt::Priority,
+ peripherals::PB15,
time::{khz, mhz},
- usart::{self, Uart},
Config,
};
use embassy_time::Timer;
use panic_probe as _;
mod encoders;
+mod logic;
mod tb6612fng;
-bind_interrupts!(struct Irqs {
- USART2 => usart::InterruptHandler<USART2>;
-});
-
#[embassy_executor::main]
async fn main(spawner: Spawner) {
let mut config = Config::default();
@@ -49,22 +41,15 @@ async fn main(spawner: Spawner) {
p.TIM1,
khz(100),
);
- spawner.spawn(calibrate_motors(motors)).unwrap();
-
- let mut serial = Uart::new(
- p.USART2,
- p.PA3,
- p.PA2,
- Irqs,
- p.DMA1_CH7,
- NoDma,
- Default::default(),
- )
- .unwrap();
- serial.write(b"Hello, world\r\n").await.unwrap();
+ // spawner.spawn(calibrate_motors(motors)).unwrap();
let encoders = Encoders::new(p.PA0, p.PA1, p.PA6, p.PA7, p.TIM2, p.TIM3);
- spawner.spawn(debug_encoders(encoders)).unwrap();
+ // spawner.spawn(debug_encoders(encoders)).unwrap();
+
+ let i2c2 = i2c2_slave::I2C2::new(p.I2C2, p.PB10, p.PB11, 0x57, Priority::P4, Priority::P3);
+ spawner
+ .spawn(logic::main_loop(i2c2, motors, encoders))
+ .unwrap();
}
#[embassy_executor::task]
@@ -78,48 +63,3 @@ async fn blink(pin: PB15) {
Timer::after_secs(1).await;
}
}
-
-#[embassy_executor::task]
-async fn calibrate_motors(mut motors: Tb6612fng<'static>) {
- let max = motors.max_pwm();
- defmt::info!("Will start motors in 1 second – max duty cycle is {}", max);
- Timer::after_secs(1).await;
- motors.move_both(Movement::Stop, Movement::Stop);
- motors.standby_leave();
- let speed = max / 2;
- let (forward, backward) = (Movement::Advance(speed), Movement::Advance(-speed));
- defmt::info!("Left motor, forward 10%");
- motors.move_both(forward, Movement::Stop);
- Timer::after_secs(1).await;
- defmt::info!("Left motor, backward 10%");
- motors.move_both(backward, Movement::Stop);
- Timer::after_secs(1).await;
- defmt::info!("Right motor, forward 10%");
- motors.move_both(Movement::Stop, forward);
- Timer::after_secs(1).await;
- defmt::info!("Right motor, backward 10%");
- motors.move_both(Movement::Stop, backward);
- Timer::after_secs(1).await;
- for i in 0..100 {
- let val = max * i / 100;
- motors.move_both(Movement::Advance(val), Movement::Advance(val));
- Timer::after_millis(30).await;
- }
- motors.move_both(Movement::Brake, Movement::Brake);
- defmt::info!("Returning to standby mode");
- motors.standby_enter();
-}
-
-#[embassy_executor::task]
-async fn debug_encoders(mut encoders: Encoders<'static>) {
- loop {
- let (left, right) = encoders.read();
- defmt::info!(
- "left = {:?}, right = {:?}",
- defmt::Debug2Format(&left),
- defmt::Debug2Format(&right)
- );
- defmt::info!("ticks = {:?}", encoders.ticks());
- Timer::after_millis(100).await;
- }
-}
diff --git a/controller/src/tb6612fng.rs b/controller/src/tb6612fng.rs
index 1661e5ebf435ee3e622d26d52f1839c643261699..1b23882856cf8c643e7065107e4dae3b1435e9a1 100644
--- a/controller/src/tb6612fng.rs
+++ b/controller/src/tb6612fng.rs
@@ -22,6 +22,7 @@ pub struct Tb6612fng<'a> {
#[derive(Clone, Copy)]
pub enum Movement {
Advance(i32),
+ #[allow(unused)]
Brake,
Stop,
}
diff --git a/i2c-bootloader/src/main.rs b/i2c-bootloader/src/main.rs
index b209a13597152f898b4081f0d9fef87c62169ae6..e8ef00bccb7cd32f56f08b73be529f98b1365d22 100644
--- a/i2c-bootloader/src/main.rs
+++ b/i2c-bootloader/src/main.rs
@@ -25,7 +25,7 @@ async fn main(spawner: Spawner) {
spawner.spawn(blink(p.PB15)).unwrap();
- let mut i2c2 = i2c2_slave::I2C2::new(p.I2C2, p.PB10, p.PB11, 0x57, Priority::P3);
+ let mut i2c2 = i2c2_slave::I2C2::new(p.I2C2, p.PB10, p.PB11, 0x57, Priority::P4, Priority::P3);
loop {
let mut buffer = [0; 8];
match i2c2.receive(&mut buffer).await {
diff --git a/i2c2-slave/src/lib.rs b/i2c2-slave/src/lib.rs
index 34485029a5499c76cca7428c0298e7f4fbd6e05b..2b280b43d087976c8edde1900aac67c6383b5669 100644
--- a/i2c2-slave/src/lib.rs
+++ b/i2c2-slave/src/lib.rs
@@ -60,7 +60,15 @@ pub struct I2C2 {
impl I2C2 {
#[must_use]
- pub fn new(i2c2: peripherals::I2C2, pb10: PB10, pb11: PB11, addr: u16, prio: Priority) -> Self {
+ pub fn new(
+ i2c2: peripherals::I2C2,
+ pb10: PB10,
+ pb11: PB11,
+ addr: u16,
+ prio: Priority,
+ prio_urg: Priority,
+ ) -> Self {
+ assert!(prio_urg < prio);
// Set pins in AF mode
pb10.set_as_af(SclPin::af_num(&pb10), AFType::OutputOpenDrain);
pb11.set_as_af(i2c::SdaPin::af_num(&pb11), AFType::OutputOpenDrain);
@@ -87,7 +95,7 @@ impl I2C2 {
w.set_ack(true);
});
interrupt::I2C2_EV.set_priority(prio);
- interrupt::I2C2_ER.set_priority(prio);
+ interrupt::I2C2_ER.set_priority(prio_urg);
unsafe {
interrupt::I2C2_EV.enable();
interrupt::I2C2_ER.enable();
@@ -108,7 +116,11 @@ impl I2C2 {
}
for extra in 0..usize::MAX {
if matches!(RX_QUEUE.receive().await, I2cRxEvent::Stop) {
- return Err(Error::ExtraBytes(extra));
+ if extra > 0 {
+ return Err(Error::ExtraBytes(extra));
+ } else {
+ return Ok(buffer.len());
+ }
}
}
unreachable!();
@@ -116,29 +128,21 @@ impl I2C2 {
pub fn send(&mut self, data: &[u8]) -> Result<()> {
defmt::trace!("Enqueuing {:?}", data);
- // Enqueue data to send next
+ // Enqueue the data to send next
for &b in data {
if TX_QUEUE.try_send(b).is_err() {
return Err(Error::FifoFull);
}
}
- // If the controller is already waiting for data (and
- // stretching the clock down), send it right away by
- // trigerring an interrupt.
- if matches!(
- State::get_state(),
- State::ReadInProgress {
- waiting_for_data: true
- }
- ) {
- pac::I2C2.cr2().modify(|w| {
- w.set_itbufen(true);
- });
- }
+ pac::I2C2.cr2().modify(|w| {
+ defmt::trace!("Setting itbufen to start receiving TXE events");
+ w.set_itbufen(true);
+ });
Ok(())
}
- fn empty_tx_queue() {
+ fn clear_tx_queue() {
+ defmt::trace!("Emptying TX queue");
while TX_QUEUE.try_receive().is_ok() {}
}
}
@@ -146,12 +150,45 @@ impl I2C2 {
#[interrupt]
unsafe fn I2C2_EV() {
let sr1 = pac::I2C2.sr1().read();
- defmt::info!(
+ defmt::trace!(
"I2C2_EV, sr1 = {:#06x}, cr1 = {:#06x}, cr2 = {:#06x}",
sr1.0,
pac::I2C2.cr1().read().0,
pac::I2C2.cr2().read().0
);
+ if sr1.stopf() {
+ defmt::trace!(
+ "I2C2_EV STOPF while state is {:?}",
+ defmt::Debug2Format(&State::get_state())
+ );
+ // Clear stopf by writing to cr1
+ pac::I2C2.cr1().modify(|_w| ());
+ // Opt out of the TXE/RXNE interrupt
+ pac::I2C2.cr2().modify(|w| {
+ w.set_itbufen(false);
+ });
+ // Do a clearing sequence as recommended in the reference manual
+ // by reading and writing dr
+ pac::I2C2.dr().modify(|_w| ());
+ // If receiving, indicate the end of data
+ let previous_state = State::get_state();
+ if matches!(previous_state, State::WriteInProgress) {
+ if RX_QUEUE.try_send(I2cRxEvent::Stop).is_err() {
+ defmt::error!("Unable to send stop event to the buffer");
+ }
+ State::set_state(State::Idle { after_write: true });
+ } else {
+ State::set_state(State::Idle { after_write: false });
+ }
+ }
+ if sr1.rxne() {
+ defmt::trace!("I2C2_EV RXNE");
+ let b = pac::I2C2.dr().read().0 as u8;
+ defmt::trace!("Received byte {:#04x}", &b);
+ if RX_QUEUE.try_send(I2cRxEvent::Byte(b)).is_err() {
+ defmt::error!("I2C byte has overfilled the buffer");
+ }
+ }
if sr1.addr() {
defmt::trace!("I2C2_EV addr");
// Addr event is cleared by reading sr2
@@ -168,7 +205,7 @@ unsafe fn I2C2_EV() {
let outgoing = if !previous_state.is_write() {
// Another read transaction happened just before this one,
// send 0 to avoid locking the bus.
- defmt::trace!("Read transaction without write, sending 0 to prevent bus locking");
+ defmt::warn!("Read transaction without write, sending 0 to prevent bus locking");
Some(0)
} else if let Ok(b) = TX_QUEUE.try_receive() {
// If bytes are ready to send already, send the first
@@ -177,7 +214,7 @@ unsafe fn I2C2_EV() {
defmt::trace!("Sending already ready byte {:#04x}", b);
Some(b)
} else {
- // Otherwise, indicate that data is waiting.
+ // Otherwise, indicate that data is waiting and do not generate TXE events.
defmt::trace!("No data ready, stretching clock");
None
};
@@ -202,7 +239,7 @@ unsafe fn I2C2_EV() {
defmt::trace!("Current state of CR2: {:#06x}", pac::I2C2.cr2().read().0);
// Empty a previously filled TX queue in case a read request
// arrives right after.
- I2C2::empty_tx_queue();
+ I2C2::clear_tx_queue();
defmt::trace!("Waiting for RXNE or STOPF event");
State::WriteInProgress
};
@@ -215,48 +252,25 @@ unsafe fn I2C2_EV() {
}
))
});
- } else if sr1.rxne() {
- defmt::trace!("I2C2_EV RXNE");
- let b = pac::I2C2.dr().read().0 as u8;
- defmt::trace!("Received byte {:#04x}", &b);
- if RX_QUEUE.try_send(I2cRxEvent::Byte(b)).is_err() {
- defmt::error!("I2C byte has overfilled the buffer");
- }
- } else if sr1.txe() {
+ }
+ if sr1.txe() && pac::I2C2.cr2().read().itbufen() {
defmt::trace!("I2C2_EV TXE");
if let Ok(b) = TX_QUEUE.try_receive() {
defmt::trace!("Sending byte {:#04x}", b);
pac::I2C2.dr().write(|w| w.set_dr(b));
} else {
- defmt::trace!("Nothing to send, storing 0 in DR to bus locking");
- pac::I2C2.dr().write(|w| w.set_dr(0));
- }
- } else if sr1.stopf() {
- defmt::trace!(
- "I2C2_EV STOPF while state is {:?}",
- defmt::Debug2Format(&State::get_state())
- );
- // Clear stopf by writing to cr1
- pac::I2C2.cr1().modify(|_w| ());
- // Opt out of the TXE/RXNE interrupt
- pac::I2C2.cr2().modify(|w| {
- w.set_itbufen(false);
- });
- // Do a clearing sequence as recommended in the reference manual
- // by reading and writing dr
- pac::I2C2.dr().modify(|_w| ());
- // If receiving, indicate the end of data
- let previous_state = State::get_state();
- if matches!(previous_state, State::WriteInProgress) {
- if RX_QUEUE.try_send(I2cRxEvent::Stop).is_err() {
- defmt::error!("Unable to send stop event to the buffer");
- }
- State::set_state(State::Idle { after_write: true });
- } else {
- State::set_state(State::Idle { after_write: false });
+ defmt::trace!("Inhibiting TXE interrupts and noting that we are waiting");
+ pac::I2C2.cr2().modify(|w| w.set_itbufen(false));
+ State::set_state(State::ReadInProgress {
+ waiting_for_data: true,
+ });
+ // defmt::trace!("Nothing to send, storing 0 in DR to avoid bus locking");
+ // pac::I2C2.dr().write(|w| w.set_dr(0));
}
- } else {
- defmt::error!("I2C2_EV interrupt without cause");
+ } else if sr1.txe() && sr1.btf() {
+ defmt::trace!("I2C2_EV TXE + BTF");
+ defmt::warn!("Sending 0 to avoid locking the bus");
+ pac::I2C2.dr().write(|w| w.set_dr(0));
}
}