From 9dc3028e71a3daf06fc34dd4eae0816b39da58fb Mon Sep 17 00:00:00 2001
From: Samuel Tardieu <sam@rfc1149.net>
Date: Mon, 15 Jul 2024 17:37:21 +0200
Subject: [PATCH] Implement PWM_FREQUENCY commands
---
Cargo.lock | 2 +-
README.org | 4 ++--
controller/Cargo.toml | 2 +-
controller/python/controller.py | 16 +++++++++++++++-
controller/src/logic.rs | 19 ++++++++++++++++++-
controller/src/main.rs | 2 +-
controller/src/tb6612fng.rs | 13 +++++++++++++
7 files changed, 51 insertions(+), 7 deletions(-)
diff --git a/Cargo.lock b/Cargo.lock
index 409a64f..9673353 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -129,7 +129,7 @@ dependencies = [
[[package]]
name = "dc-motor-driver-hat"
-version = "0.1.0"
+version = "0.2.0"
dependencies = [
"cortex-m",
"cortex-m-rt",
diff --git a/README.org b/README.org
index 9f5f9c0..540ddfa 100644
--- a/README.org
+++ b/README.org
@@ -57,9 +57,9 @@ Multibyte values are exchanged in little endian format.
- The I²C address
-** 0x10 PWM frequency (R/W)
+** 0x10 [IMPLEMENTED] PWM frequency (R/W)
-- Frequency in kHz, from 1 to 100 (default: 1)
+- Frequency in Hz on 3 bytes, from 1 to 100_000 (default: 10_000)
** [IMPLEMENTED] 0x11 Max motor percentage (R/W)
diff --git a/controller/Cargo.toml b/controller/Cargo.toml
index b6d96c3..5bcdbcd 100644
--- a/controller/Cargo.toml
+++ b/controller/Cargo.toml
@@ -1,6 +1,6 @@
[package]
name = "dc-motor-driver-hat"
-version = "0.1.0" # Update in python/controller.py as well
+version = "0.2.0" # Update in python/controller.py as well
authors = ["Samuel Tardieu <sam@rfc1149.net>"]
edition = "2021"
diff --git a/controller/python/controller.py b/controller/python/controller.py
index 83b9a35..3013c4d 100644
--- a/controller/python/controller.py
+++ b/controller/python/controller.py
@@ -3,7 +3,7 @@ from numbers import Real
from typing import Any, Optional
# Major and minor version of required firmware
-_REQUIRED_FIRMWARE_VERSION = (0, 1)
+_REQUIRED_FIRMWARE_VERSION = (0, 2)
class FirmwareVersionMismatch(Exception):
@@ -16,6 +16,7 @@ class Controller:
FIRMWARE_VERSION = 0x08
WHO_AM_I = 0x0F
+ PWM_FREQUENCY = 0x10
MAX_MOTOR_PERCENTAGE = 0x11
MOTOR_SHUTDOWN_TIMEOUT = 0x28
MOTOR_SPEED = 0x30
@@ -146,3 +147,16 @@ class Controller:
f"is not compatible with this library version ({VERSION})"
)
raise FirmwareVersionMismatch(error)
+
+ def set_pwm_frequency(self, freq: int):
+ """Set the PWM frequency in Hz, between 1 and 100000."""
+ if freq < 1 or freq > 100000:
+ raise ValueError(f"PWM frequency is out of [1, 100000] range: {freq}")
+ self._write(
+ self.PWM_FREQUENCY, [freq & 0xFF, (freq >> 8) & 0xFF, (freq >> 16) & 0xFF]
+ )
+
+ def get_pwm_frequency(self):
+ """Return the PWM frequency in Hz."""
+ a, b, c = self._read(self.PWM_FREQUENCY, 3, "BBB")
+ return a | (b << 8) | (c << 16)
diff --git a/controller/src/logic.rs b/controller/src/logic.rs
index beeec99..68df072 100644
--- a/controller/src/logic.rs
+++ b/controller/src/logic.rs
@@ -3,6 +3,7 @@ use crate::{
tb6612fng::{Movement, Tb6612fng},
};
use core::sync::atomic::{AtomicU32, Ordering};
+use embassy_stm32::time::hz;
use embassy_time::{Instant, Timer};
use heapless::Deque;
use i2c2_target::{I2C2, MESSAGE_SIZE};
@@ -133,6 +134,7 @@ fn i2c_callback(command: &[u8], response: &mut Deque<u8, MESSAGE_SIZE>) {
const FIRMWARE_VERSION: u8 = 0x08;
const WHO_AM_I: u8 = 0x0f;
+const PWM_FREQUENCY: u8 = 0x10;
const MAX_MOTOR_PERCENTAGE: u8 = 0x11;
const MOTOR_SHUTDOWN_TIMEOUT: u8 = 0x28;
const MOTOR_SPEED: u8 = 0x30;
@@ -156,11 +158,26 @@ fn process_command(
[WHO_AM_I, ..] => {
response.push_back(0x57).unwrap();
}
+ &[PWM_FREQUENCY, a, b, c] => {
+ let f = u32::from_le_bytes([a, b, c, 0]);
+ if (1..=100_000).contains(&f) {
+ state.motors.set_frequency(hz(f));
+ } else {
+ defmt::warn!("incorrect PWM frequency {}", f);
+ return false;
+ }
+ }
+ [PWM_FREQUENCY] => {
+ let freq = state.motors.get_frequency().0;
+ for &b in &freq.to_le_bytes()[..3] {
+ response.push_back(b).unwrap();
+ }
+ }
&[MAX_MOTOR_PERCENTAGE, p] => {
if (1..=100).contains(&p) {
state.max_motor_percentage = p;
} else {
- defmt::warn!("Incorrect max percentage {}", p);
+ defmt::warn!("incorrect max percentage {}", p);
return false;
}
}
diff --git a/controller/src/main.rs b/controller/src/main.rs
index d231548..64852e2 100644
--- a/controller/src/main.rs
+++ b/controller/src/main.rs
@@ -56,7 +56,7 @@ async fn main(spawner: Spawner) {
p.PB6,
p.PB8,
p.TIM1,
- khz(100),
+ khz(10),
);
let encoders = Encoders::new(p.PA0, p.PA1, p.PA6, p.PA7, p.TIM2, p.TIM3);
diff --git a/controller/src/tb6612fng.rs b/controller/src/tb6612fng.rs
index 1384484..883cb9a 100644
--- a/controller/src/tb6612fng.rs
+++ b/controller/src/tb6612fng.rs
@@ -18,6 +18,7 @@ pub struct Tb6612fng<'a> {
b1: Output<'a>,
b2: Output<'a>,
standby: Output<'a>,
+ freq: Hertz,
}
#[derive(Clone, Copy)]
@@ -71,6 +72,7 @@ impl Tb6612fng<'_> {
);
pwm.enable(Channel::Ch3);
pwm.enable(Channel::Ch4);
+
Self {
pwm,
a1,
@@ -78,6 +80,7 @@ impl Tb6612fng<'_> {
b1,
b2,
standby,
+ freq,
}
}
@@ -85,6 +88,16 @@ impl Tb6612fng<'_> {
self.pwm.get_max_duty()
}
+ pub fn set_frequency(&mut self, freq: Hertz) {
+ self.freq = freq;
+ self.move_both(Movement::Stop, Movement::Stop);
+ self.pwm.set_frequency(freq);
+ }
+
+ pub fn get_frequency(&self) -> Hertz {
+ self.freq
+ }
+
pub fn standby_enter(&mut self) {
self.standby.set_low();
}
--
GitLab