From 7fc09f89e8e4b611a868bc986104762b1c5ba81a Mon Sep 17 00:00:00 2001 From: rafael Date: Sun, 20 Oct 2024 23:28:47 +0200 Subject: [PATCH] embassy_rp: implement pwm traits from embedded_hal MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit • Update crate versions • Implement embedded-hal PWM traits • Add TB6612FNG motor driver example --- embassy-rp/Cargo.toml | 8 +- embassy-rp/src/pwm.rs | 42 +++++++ examples/rp23/Cargo.toml | 3 + .../src/bin/pwm_tb6612fng_motor_driver.rs | 114 ++++++++++++++++++ 4 files changed, 163 insertions(+), 4 deletions(-) create mode 100644 examples/rp23/src/bin/pwm_tb6612fng_motor_driver.rs diff --git a/embassy-rp/Cargo.toml b/embassy-rp/Cargo.toml index 54de238b3..baa92398b 100644 --- a/embassy-rp/Cargo.toml +++ b/embassy-rp/Cargo.toml @@ -118,18 +118,18 @@ embassy-usb-driver = {version = "0.1.0", path = "../embassy-usb-driver" } atomic-polyfill = "1.0.1" defmt = { version = "0.3", optional = true } log = { version = "0.4.14", optional = true } -nb = "1.0.0" +nb = "1.1.0" cfg-if = "1.0.0" cortex-m-rt = ">=0.6.15,<0.8" cortex-m = "0.7.6" -critical-section = "1.1" +critical-section = "1.2.0" chrono = { version = "0.4", default-features = false, optional = true } embedded-io = { version = "0.6.1" } embedded-io-async = { version = "0.6.1" } embedded-storage = { version = "0.3" } embedded-storage-async = { version = "0.4.1" } rand_core = "0.6.4" -fixed = "1.23.1" +fixed = "1.28.0" rp-pac = { git = "https://github.com/embassy-rs/rp-pac.git", rev = "a7f42d25517f7124ad3b4ed492dec8b0f50a0e6c", feature = ["rt"] } @@ -141,7 +141,7 @@ embedded-hal-nb = { version = "1.0" } pio-proc = {version= "0.2" } pio = {version= "0.2.1" } rp2040-boot2 = "0.3" -document-features = "0.2.7" +document-features = "0.2.10" sha2-const-stable = "0.1" rp-binary-info = { version = "0.1.0", optional = true } smart-leds = "0.4.0" diff --git a/embassy-rp/src/pwm.rs b/embassy-rp/src/pwm.rs index 027f5504e..79e626802 100644 --- a/embassy-rp/src/pwm.rs +++ b/embassy-rp/src/pwm.rs @@ -1,6 +1,7 @@ //! Pulse Width Modulation (PWM) use embassy_hal_internal::{into_ref, Peripheral, PeripheralRef}; +use embedded_hal_1::pwm::{Error, ErrorKind, ErrorType, SetDutyCycle}; use fixed::traits::ToFixed; use fixed::FixedU16; use pac::pwm::regs::{ChDiv, Intr}; @@ -80,6 +81,21 @@ impl From for Divmode { } } +/// PWM error. +#[derive(Debug)] +pub enum PwmError { + /// Invalid Duty Cycle. + InvalidDutyCycle, +} + +impl Error for PwmError { + fn kind(&self) -> ErrorKind { + match self { + PwmError::InvalidDutyCycle => ErrorKind::Other, + } + } +} + /// PWM driver. pub struct Pwm<'d> { pin_a: Option>, @@ -87,6 +103,32 @@ pub struct Pwm<'d> { slice: usize, } +impl<'d> ErrorType for Pwm<'d> { + type Error = PwmError; +} + +impl<'d> SetDutyCycle for Pwm<'d> { + fn max_duty_cycle(&self) -> u16 { + pac::PWM.ch(self.slice).top().read().top() + } + + fn set_duty_cycle(&mut self, duty: u16) -> Result<(), Self::Error> { + info!("duty {}",&duty); + let max_duty = self.max_duty_cycle(); + info!("max duty {}", &max_duty); + if duty > max_duty { + return Err(PwmError::InvalidDutyCycle); + } + + let p = pac::PWM.ch(self.slice); + p.cc().modify(|w| { + w.set_a(duty); + w.set_b(duty); + }); + Ok(()) + } +} + impl<'d> Pwm<'d> { fn new_inner( slice: usize, diff --git a/examples/rp23/Cargo.toml b/examples/rp23/Cargo.toml index 08646463c..f35e3a11d 100644 --- a/examples/rp23/Cargo.toml +++ b/examples/rp23/Cargo.toml @@ -30,6 +30,9 @@ serde-json-core = "0.5.1" # for assign resources example assign-resources = { git = "https://github.com/adamgreig/assign-resources", rev = "94ad10e2729afdf0fd5a77cd12e68409a982f58a" } +# for TB6612FNG example +tb6612fng = "1.0.0" + #cortex-m = { version = "0.7.6", features = ["critical-section-single-core"] } cortex-m = { version = "0.7.6", features = ["inline-asm"] } cortex-m-rt = "0.7.0" diff --git a/examples/rp23/src/bin/pwm_tb6612fng_motor_driver.rs b/examples/rp23/src/bin/pwm_tb6612fng_motor_driver.rs new file mode 100644 index 000000000..92c1ff6ba --- /dev/null +++ b/examples/rp23/src/bin/pwm_tb6612fng_motor_driver.rs @@ -0,0 +1,114 @@ +//! # PWM TB6612FNG motor driver +//! +//! This example shows the use of a TB6612FNG motor driver. The driver is built on top of embedded_hal and the example demonstrates how embassy_rp can be used to interact with ist. + +#![no_std] +#![no_main] + +use defmt::*; +use embassy_executor::Spawner; +use embassy_rp::block::ImageDef; +use embassy_rp::config::Config; +use embassy_rp::gpio::Output; +use embassy_rp::peripherals; +use embassy_rp::gpio; +use embassy_rp::pwm; +use embassy_time::Duration; +use embassy_time::Timer; +use {defmt_rtt as _, panic_probe as _}; +use tb6612fng::{DriveCommand, Motor, Tb6612fng}; +use assign_resources::assign_resources; + +/// Maximum PWM value (fully on) +const PWM_MAX: u16 = 50000; + +/// Minimum PWM value (fully off) +const PWM_MIN: u16 = 0; + +#[link_section = ".start_block"] +#[used] +pub static IMAGE_DEF: ImageDef = ImageDef::secure_exe(); + +assign_resources! { + motor: MotorResources { + standby_pin: PIN_22, + left_slice: PWM_SLICE6, + left_pwm_pin: PIN_28, + left_forward_pin: PIN_21, + left_backward_pin: PIN_20, + right_slice: PWM_SLICE5, + right_pwm_pin: PIN_27, + right_forward_pin: PIN_19, + right_backward_pin: PIN_18, + }, +} + +#[embassy_executor::main] +async fn main(_spawner: Spawner) { + let p = embassy_rp::init(Config::default()); + let s = split_resources!(p); + let r = s.motor; + + // we need a standby output and two motors to construct a full TB6612FNG + + // standby pin + let stby = Output::new(r.standby_pin, gpio::Level::Low); + + // motor A, here defined to be the left motor + let left_fwd = gpio::Output::new(r.left_forward_pin, gpio::Level::Low); + let left_bckw = gpio::Output::new(r.left_backward_pin, gpio::Level::Low); + let mut left_speed = pwm::Config::default(); + left_speed.top = PWM_MAX; + left_speed.compare_a = PWM_MIN; + let left_pwm = pwm::Pwm::new_output_a(r.left_slice, r.left_pwm_pin, left_speed); + let left_motor = Motor::new(left_fwd, left_bckw, left_pwm).unwrap(); + + // motor B, here defined to be the right motor + let right_fwd = gpio::Output::new(r.right_forward_pin, gpio::Level::Low); + let right_bckw = gpio::Output::new(r.right_backward_pin, gpio::Level::Low); + let mut right_speed = pwm::Config::default(); + right_speed.top = PWM_MAX; + right_speed.compare_b = PWM_MIN; + let right_pwm = pwm::Pwm::new_output_b(r.right_slice, r.right_pwm_pin, right_speed); + let right_motor = Motor::new(right_fwd, right_bckw, right_pwm).unwrap(); + + // construct the motor driver + let mut control = Tb6612fng::new(left_motor, right_motor, stby).unwrap(); + + loop { + // wake up the motor driver + info!("end standby"); + control.disable_standby().unwrap(); + Timer::after(Duration::from_millis(100)).await; + + // drive a straight line forward at 20% speed for 5s + info!("drive straight"); + control.motor_a.drive(DriveCommand::Forward(20)).unwrap(); + control.motor_b.drive(DriveCommand::Forward(20)).unwrap(); + Timer::after(Duration::from_secs(5)).await; + + // coast for 2s + info!("coast"); + control.motor_a.drive(DriveCommand::Stop).unwrap(); + control.motor_b.drive(DriveCommand::Stop).unwrap(); + Timer::after(Duration::from_secs(2)).await; + + // actively brake + info!("brake"); + control.motor_a.drive(DriveCommand::Brake).unwrap(); + control.motor_b.drive(DriveCommand::Brake).unwrap(); + Timer::after(Duration::from_secs(1)).await; + + // slowly turn for 3s + info!( "turn"); + control.motor_a.drive(DriveCommand::Backward(10)).unwrap(); + control.motor_b.drive(DriveCommand::Forward(10)).unwrap(); + Timer::after(Duration::from_secs(3)).await; + + // and put the driver in standby mode and wait for 5s + info!( "standby"); + control.enable_standby().unwrap(); + Timer::after(Duration::from_secs(5)).await; + } +} +