From 7fc09f89e8e4b611a868bc986104762b1c5ba81a Mon Sep 17 00:00:00 2001 From: rafael Date: Sun, 20 Oct 2024 23:28:47 +0200 Subject: 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 --- examples/rp23/Cargo.toml | 3 + .../rp23/src/bin/pwm_tb6612fng_motor_driver.rs | 114 +++++++++++++++++++++ 2 files changed, 117 insertions(+) create mode 100644 examples/rp23/src/bin/pwm_tb6612fng_motor_driver.rs (limited to 'examples') 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; + } +} + -- cgit From 8baf88f8f4668bdb54c1415bdb8ad62cf2fa5f24 Mon Sep 17 00:00:00 2001 From: rafael Date: Sun, 20 Oct 2024 23:31:53 +0200 Subject: rustfmt --- examples/rp23/src/bin/pwm_tb6612fng_motor_driver.rs | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) (limited to 'examples') diff --git a/examples/rp23/src/bin/pwm_tb6612fng_motor_driver.rs b/examples/rp23/src/bin/pwm_tb6612fng_motor_driver.rs index 92c1ff6ba..36b8d9620 100644 --- a/examples/rp23/src/bin/pwm_tb6612fng_motor_driver.rs +++ b/examples/rp23/src/bin/pwm_tb6612fng_motor_driver.rs @@ -1,23 +1,23 @@ //! # 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 assign_resources::assign_resources; use defmt::*; use embassy_executor::Spawner; use embassy_rp::block::ImageDef; use embassy_rp::config::Config; +use embassy_rp::gpio; 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; +use {defmt_rtt as _, panic_probe as _}; /// Maximum PWM value (fully on) const PWM_MAX: u16 = 50000; @@ -94,21 +94,20 @@ async fn main(_spawner: Spawner) { Timer::after(Duration::from_secs(2)).await; // actively brake - info!("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"); + 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"); + info!("standby"); control.enable_standby().unwrap(); Timer::after(Duration::from_secs(5)).await; } } - -- cgit From f32b0fbc3b614ab79b7756b49e44a380e5e60192 Mon Sep 17 00:00:00 2001 From: rafael Date: Mon, 21 Oct 2024 11:55:10 +0200 Subject: change existing pwm example to reflect both existing ways to use pwm output --- examples/rp23/src/bin/pwm.rs | 55 ++++++++++++++++++++++++++++++++++++++------ 1 file changed, 48 insertions(+), 7 deletions(-) (limited to 'examples') diff --git a/examples/rp23/src/bin/pwm.rs b/examples/rp23/src/bin/pwm.rs index 15eae09ee..7314ee637 100644 --- a/examples/rp23/src/bin/pwm.rs +++ b/examples/rp23/src/bin/pwm.rs @@ -1,6 +1,8 @@ -//! This example shows how to use PWM (Pulse Width Modulation) in the RP2040 chip. +//! This example shows how to use PWM (Pulse Width Modulation) in the RP235x chip. //! -//! The LED on the RP Pico W board is connected differently. Add a LED and resistor to another pin. +//! We demonstrate two ways of using PWM: +//! 1. Via config +//! 2. Via setting a duty cycle #![no_std] #![no_main] @@ -8,8 +10,10 @@ use defmt::*; use embassy_executor::Spawner; use embassy_rp::block::ImageDef; -use embassy_rp::pwm::{Config, Pwm}; +use embassy_rp::peripherals::{PIN_25, PIN_4, PWM_SLICE2, PWM_SLICE4}; +use embassy_rp::pwm::{Config, Pwm, SetDutyCycle}; use embassy_time::Timer; +// use embedded_hal_1::pwm::SetDutyCycle; use {defmt_rtt as _, panic_probe as _}; #[link_section = ".start_block"] @@ -17,13 +21,22 @@ use {defmt_rtt as _, panic_probe as _}; pub static IMAGE_DEF: ImageDef = ImageDef::secure_exe(); #[embassy_executor::main] -async fn main(_spawner: Spawner) { +async fn main(spawner: Spawner) { let p = embassy_rp::init(Default::default()); + spawner.spawn(pwm_set_config(p.PWM_SLICE4, p.PIN_25)).unwrap(); + spawner.spawn(pwm_set_dutycycle(p.PWM_SLICE2, p.PIN_4)).unwrap(); +} - let mut c: Config = Default::default(); - c.top = 0x8000; +/// Demonstrate PWM by modifying & applying the config +/// +/// Using the onboard led, if You are using a different Board than plain Pico2 (i.e. W variant) +/// you must use another slice & pin and an appropriate resistor. +#[embassy_executor::task] +async fn pwm_set_config(slice4: PWM_SLICE4, pin25: PIN_25) { + let mut c = Config::default(); + c.top = 32_768; c.compare_b = 8; - let mut pwm = Pwm::new_output_b(p.PWM_SLICE4, p.PIN_25, c.clone()); + let mut pwm = Pwm::new_output_b(slice4, pin25, c.clone()); loop { info!("current LED duty cycle: {}/32768", c.compare_b); @@ -32,3 +45,31 @@ async fn main(_spawner: Spawner) { pwm.set_config(&c); } } + +/// Demonstrate PWM by setting duty cycle +/// +/// Using GP4 in Slice2, make sure to use an appropriate resistor. +#[embassy_executor::task] +async fn pwm_set_dutycycle(slice2: PWM_SLICE2, pin4: PIN_4) { + let mut c = Config::default(); + c.top = 32_768; + let mut pwm = Pwm::new_output_a(slice2, pin4, c.clone()); + + loop { + // 100% duty cycle, fully on + pwm.set_duty_cycle_fully_on().unwrap(); + Timer::after_secs(1).await; + + // 50% duty cycle, half on. Expressed as simple percentage. + pwm.set_duty_cycle_percent(50).unwrap(); + Timer::after_secs(1).await; + + // 25% duty cycle, quarter on. Expressed as (duty / max_duty) + pwm.set_duty_cycle(8_192 / c.top).unwrap(); + Timer::after_secs(1).await; + + // 0% duty cycle, fully off. + pwm.set_duty_cycle_fully_off().unwrap(); + Timer::after_secs(1).await; + } +} -- cgit From d92fb002ecc3ff4dcac51d8e74927d977b2343b0 Mon Sep 17 00:00:00 2001 From: rafael Date: Mon, 21 Oct 2024 12:19:10 +0200 Subject: rustfmt --- examples/rp23/src/bin/pwm.rs | 1 - examples/rp23/src/bin/pwm_tb6612fng_motor_driver.rs | 7 ++----- 2 files changed, 2 insertions(+), 6 deletions(-) (limited to 'examples') diff --git a/examples/rp23/src/bin/pwm.rs b/examples/rp23/src/bin/pwm.rs index 7314ee637..1dd5ca3de 100644 --- a/examples/rp23/src/bin/pwm.rs +++ b/examples/rp23/src/bin/pwm.rs @@ -13,7 +13,6 @@ use embassy_rp::block::ImageDef; use embassy_rp::peripherals::{PIN_25, PIN_4, PWM_SLICE2, PWM_SLICE4}; use embassy_rp::pwm::{Config, Pwm, SetDutyCycle}; use embassy_time::Timer; -// use embedded_hal_1::pwm::SetDutyCycle; use {defmt_rtt as _, panic_probe as _}; #[link_section = ".start_block"] diff --git a/examples/rp23/src/bin/pwm_tb6612fng_motor_driver.rs b/examples/rp23/src/bin/pwm_tb6612fng_motor_driver.rs index 36b8d9620..6c3a8998c 100644 --- a/examples/rp23/src/bin/pwm_tb6612fng_motor_driver.rs +++ b/examples/rp23/src/bin/pwm_tb6612fng_motor_driver.rs @@ -10,12 +10,9 @@ use defmt::*; use embassy_executor::Spawner; use embassy_rp::block::ImageDef; use embassy_rp::config::Config; -use embassy_rp::gpio; use embassy_rp::gpio::Output; -use embassy_rp::peripherals; -use embassy_rp::pwm; -use embassy_time::Duration; -use embassy_time::Timer; +use embassy_rp::{gpio, peripherals, pwm}; +use embassy_time::{Duration, Timer}; use tb6612fng::{DriveCommand, Motor, Tb6612fng}; use {defmt_rtt as _, panic_probe as _}; -- cgit From 8dfc9ba1a3e3f69aedf5bce748783fb6a8f5e92e Mon Sep 17 00:00:00 2001 From: rafael Date: Mon, 21 Oct 2024 21:14:49 +0200 Subject: also add to the rp pwm example --- examples/rp/src/bin/pwm.rs | 52 +++++++++++++++++++++++++++++++++++++++----- examples/rp23/src/bin/pwm.rs | 8 +++---- 2 files changed, 50 insertions(+), 10 deletions(-) (limited to 'examples') diff --git a/examples/rp/src/bin/pwm.rs b/examples/rp/src/bin/pwm.rs index 26e233260..862a7da22 100644 --- a/examples/rp/src/bin/pwm.rs +++ b/examples/rp/src/bin/pwm.rs @@ -1,24 +1,36 @@ //! This example shows how to use PWM (Pulse Width Modulation) in the RP2040 chip. //! -//! The LED on the RP Pico W board is connected differently. Add a LED and resistor to another pin. +//! We demonstrate two ways of using PWM: +//! 1. Via config +//! 2. Via setting a duty cycle #![no_std] #![no_main] use defmt::*; use embassy_executor::Spawner; -use embassy_rp::pwm::{Config, Pwm}; +use embassy_rp::peripherals::{PIN_25, PIN_4, PWM_SLICE2, PWM_SLICE4}; +use embassy_rp::pwm::{Config, Pwm, SetDutyCycle}; use embassy_time::Timer; use {defmt_rtt as _, panic_probe as _}; #[embassy_executor::main] -async fn main(_spawner: Spawner) { +async fn main(spawner: Spawner) { let p = embassy_rp::init(Default::default()); + spawner.spawn(pwm_set_config(p.PWM_SLICE4, p.PIN_25)).unwrap(); + spawner.spawn(pwm_set_dutycycle(p.PWM_SLICE2, p.PIN_4)).unwrap(); +} - let mut c: Config = Default::default(); - c.top = 0x8000; +/// Demonstrate PWM by modifying & applying the config +/// +/// Using the onboard led, if You are using a different Board than plain Pico2 (i.e. W variant) +/// you must use another slice & pin and an appropriate resistor. +#[embassy_executor::task] +async fn pwm_set_config(slice4: PWM_SLICE4, pin25: PIN_25) { + let mut c = Config::default(); + c.top = 32_768; c.compare_b = 8; - let mut pwm = Pwm::new_output_b(p.PWM_SLICE4, p.PIN_25, c.clone()); + let mut pwm = Pwm::new_output_b(slice4, pin25, c.clone()); loop { info!("current LED duty cycle: {}/32768", c.compare_b); @@ -27,3 +39,31 @@ async fn main(_spawner: Spawner) { pwm.set_config(&c); } } + +/// Demonstrate PWM by setting duty cycle +/// +/// Using GP4 in Slice2, make sure to use an appropriate resistor. +#[embassy_executor::task] +async fn pwm_set_dutycycle(slice2: PWM_SLICE2, pin4: PIN_4) { + let mut c = Config::default(); + c.top = 32_768; + let mut pwm = Pwm::new_output_a(slice2, pin4, c.clone()); + + loop { + // 100% duty cycle, fully on + pwm.set_duty_cycle_fully_on().unwrap(); + Timer::after_secs(1).await; + + // 66% duty cycle. Expressed as simple percentage. + pwm.set_duty_cycle_percent(66).unwrap(); + Timer::after_secs(1).await; + + // 25% duty cycle. Expressed as 32768/4 = 8192. + pwm.set_duty_cycle(8_192).unwrap(); + Timer::after_secs(1).await; + + // 0% duty cycle, fully off. + pwm.set_duty_cycle_fully_off().unwrap(); + Timer::after_secs(1).await; + } +} diff --git a/examples/rp23/src/bin/pwm.rs b/examples/rp23/src/bin/pwm.rs index 1dd5ca3de..838eee625 100644 --- a/examples/rp23/src/bin/pwm.rs +++ b/examples/rp23/src/bin/pwm.rs @@ -59,12 +59,12 @@ async fn pwm_set_dutycycle(slice2: PWM_SLICE2, pin4: PIN_4) { pwm.set_duty_cycle_fully_on().unwrap(); Timer::after_secs(1).await; - // 50% duty cycle, half on. Expressed as simple percentage. - pwm.set_duty_cycle_percent(50).unwrap(); + // 66% duty cycle. Expressed as simple percentage. + pwm.set_duty_cycle_percent(66).unwrap(); Timer::after_secs(1).await; - // 25% duty cycle, quarter on. Expressed as (duty / max_duty) - pwm.set_duty_cycle(8_192 / c.top).unwrap(); + // 25% duty cycle. Expressed as 32768/4 = 8192. + pwm.set_duty_cycle(8_192).unwrap(); Timer::after_secs(1).await; // 0% duty cycle, fully off. -- cgit From 14e69309ebe25a76f67c38411c7a80a4d83da5ed Mon Sep 17 00:00:00 2001 From: rafael Date: Mon, 21 Oct 2024 22:42:18 +0200 Subject: add pwm frequency to examples --- examples/rp/src/bin/pwm.rs | 10 ++++++++-- examples/rp23/src/bin/pwm.rs | 10 ++++++++-- examples/rp23/src/bin/pwm_tb6612fng_motor_driver.rs | 17 +++++++---------- 3 files changed, 23 insertions(+), 14 deletions(-) (limited to 'examples') diff --git a/examples/rp/src/bin/pwm.rs b/examples/rp/src/bin/pwm.rs index 862a7da22..791b88b5b 100644 --- a/examples/rp/src/bin/pwm.rs +++ b/examples/rp/src/bin/pwm.rs @@ -45,8 +45,14 @@ async fn pwm_set_config(slice4: PWM_SLICE4, pin25: PIN_25) { /// Using GP4 in Slice2, make sure to use an appropriate resistor. #[embassy_executor::task] async fn pwm_set_dutycycle(slice2: PWM_SLICE2, pin4: PIN_4) { + // If we aim for a specific frequency, here is how we can calculate the top value. + // The top value sets the period of the PWM cycle, so a counter goes from 0 to top and then wraps around to 0. + // Every such wraparound is one PWM cycle. So here is how we get 25KHz: let mut c = Config::default(); - c.top = 32_768; + let pwm_freq = 25_000; // Hz, our desired frequency + let clock_freq = embassy_rp::clocks::clk_sys_freq(); + c.top = (clock_freq / pwm_freq) as u16 - 1; + let mut pwm = Pwm::new_output_a(slice2, pin4, c.clone()); loop { @@ -59,7 +65,7 @@ async fn pwm_set_dutycycle(slice2: PWM_SLICE2, pin4: PIN_4) { Timer::after_secs(1).await; // 25% duty cycle. Expressed as 32768/4 = 8192. - pwm.set_duty_cycle(8_192).unwrap(); + pwm.set_duty_cycle(c.top / 4).unwrap(); Timer::after_secs(1).await; // 0% duty cycle, fully off. diff --git a/examples/rp23/src/bin/pwm.rs b/examples/rp23/src/bin/pwm.rs index 838eee625..5a4457158 100644 --- a/examples/rp23/src/bin/pwm.rs +++ b/examples/rp23/src/bin/pwm.rs @@ -50,8 +50,14 @@ async fn pwm_set_config(slice4: PWM_SLICE4, pin25: PIN_25) { /// Using GP4 in Slice2, make sure to use an appropriate resistor. #[embassy_executor::task] async fn pwm_set_dutycycle(slice2: PWM_SLICE2, pin4: PIN_4) { + // If we aim for a specific frequency, here is how we can calculate the top value. + // The top value sets the period of the PWM cycle, so a counter goes from 0 to top and then wraps around to 0. + // Every such wraparound is one PWM cycle. So here is how we get 25KHz: let mut c = Config::default(); - c.top = 32_768; + let pwm_freq = 25_000; // Hz, our desired frequency + let clock_freq = embassy_rp::clocks::clk_sys_freq(); + c.top = (clock_freq / pwm_freq) as u16 - 1; + let mut pwm = Pwm::new_output_a(slice2, pin4, c.clone()); loop { @@ -64,7 +70,7 @@ async fn pwm_set_dutycycle(slice2: PWM_SLICE2, pin4: PIN_4) { Timer::after_secs(1).await; // 25% duty cycle. Expressed as 32768/4 = 8192. - pwm.set_duty_cycle(8_192).unwrap(); + pwm.set_duty_cycle(c.top / 4).unwrap(); Timer::after_secs(1).await; // 0% duty cycle, fully off. diff --git a/examples/rp23/src/bin/pwm_tb6612fng_motor_driver.rs b/examples/rp23/src/bin/pwm_tb6612fng_motor_driver.rs index 6c3a8998c..263b551de 100644 --- a/examples/rp23/src/bin/pwm_tb6612fng_motor_driver.rs +++ b/examples/rp23/src/bin/pwm_tb6612fng_motor_driver.rs @@ -16,12 +16,6 @@ use embassy_time::{Duration, Timer}; use tb6612fng::{DriveCommand, Motor, Tb6612fng}; use {defmt_rtt as _, panic_probe as _}; -/// 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(); @@ -46,6 +40,11 @@ async fn main(_spawner: Spawner) { let s = split_resources!(p); let r = s.motor; + // we want a PWM frequency of 25KHz + let pwm_freq = 25_000; // Hz, our desired frequency + let clock_freq = embassy_rp::clocks::clk_sys_freq(); + let period = (clock_freq / pwm_freq) as u16 - 1; + // we need a standby output and two motors to construct a full TB6612FNG // standby pin @@ -55,8 +54,7 @@ async fn main(_spawner: Spawner) { 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; + left_speed.top = period; 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(); @@ -64,8 +62,7 @@ async fn main(_spawner: Spawner) { 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; + right_speed.top = period; 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(); -- cgit From 548f11d5aef37d5ab1edac6970ceb7bee4300f4f Mon Sep 17 00:00:00 2001 From: rafael Date: Mon, 21 Oct 2024 23:19:45 +0200 Subject: cheaper motors need lower pwm frequency --- examples/rp23/src/bin/pwm_tb6612fng_motor_driver.rs | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) (limited to 'examples') diff --git a/examples/rp23/src/bin/pwm_tb6612fng_motor_driver.rs b/examples/rp23/src/bin/pwm_tb6612fng_motor_driver.rs index 263b551de..3fad2928c 100644 --- a/examples/rp23/src/bin/pwm_tb6612fng_motor_driver.rs +++ b/examples/rp23/src/bin/pwm_tb6612fng_motor_driver.rs @@ -40,8 +40,8 @@ async fn main(_spawner: Spawner) { let s = split_resources!(p); let r = s.motor; - // we want a PWM frequency of 25KHz - let pwm_freq = 25_000; // Hz, our desired frequency + // we want a PWM frequency of 1KHz, especially cheaper motors do not respond well to higher frequencies + let pwm_freq = 1_000; // Hz, our desired frequency let clock_freq = embassy_rp::clocks::clk_sys_freq(); let period = (clock_freq / pwm_freq) as u16 - 1; @@ -77,8 +77,8 @@ async fn main(_spawner: Spawner) { // 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(); + control.motor_a.drive(DriveCommand::Forward(80)).unwrap(); + control.motor_b.drive(DriveCommand::Forward(80)).unwrap(); Timer::after(Duration::from_secs(5)).await; // coast for 2s @@ -95,8 +95,8 @@ async fn main(_spawner: Spawner) { // slowly turn for 3s info!("turn"); - control.motor_a.drive(DriveCommand::Backward(10)).unwrap(); - control.motor_b.drive(DriveCommand::Forward(10)).unwrap(); + control.motor_a.drive(DriveCommand::Backward(50)).unwrap(); + control.motor_b.drive(DriveCommand::Forward(50)).unwrap(); Timer::after(Duration::from_secs(3)).await; // and put the driver in standby mode and wait for 5s -- cgit