On RP2040, Using Embassy's PIO and PWM components together for servo control

[allenqm]

I'm following along the examples in the rp folder of the embassy crate, in particular the pwm and pio_servo examples. I am using a pimoroni servo2040 board. I'm trying to use embassy to manipulate multiple arms (each arm has 3 joints), but I've currently focusing on just one arm. I have 2 main issues I am working through:

Issue 1: I attempted to merge the two examples to have 2 servo motors be controlled by PWM0's two channels, and 1 servo controlled by a PIO based pwm signal.

• When I run this code with nothing uncommented, the servos getting instruction from pwm0 are moving as expected,but the servo getting instruction from pioservo0 is not.
• When I comment out the code involving pmw0 in the main loop, the servo getting instruction from pioservo0 moves as expected.

looking for explanation as to why this is happening. I've tried encapsulating the logic for manipulating pwm0 and pioservo0 behind async functions, but this hasn't solved my problem. Admittedly I am new to async.

Note: pioservo is a module that exactly replicates the components build in the embassy pio_servo example. I just renamed ServoBuilder and Servo to PioServoBuilder and PioServo.

Issue 2: I ultimately would like to create an embassy task where this triplet of servos is looping through positions, and I plan to call this task to manipulate multiple triplets. I am getting hung up on the fact that embassy tasks don't accept generics, so I can't provide the lifetimes or generic parameters that Pwm and PioServo require. Any guidance on how to avoid this issue would be helpful.

Here is the code:

//! An attempt to control a PwmSlice's two channels and a PIO based PWM signal at the same time.
//! a servo motor on the pimoroni servo2040 board that has a relay switch.
//!

#![no_std]
#![no_main]

use core::time::Duration;
use defmt::*;
use embassy_executor::Spawner;
use embassy_futures::join::{self, join};
use embassy_rp::gpio::{Level, Output};
use embassy_rp::peripherals::PIO0;
use embassy_rp::pio::{
    Common, Config as PioConfig, Direction, Instance, InterruptHandler, Pio, PioPin, StateMachine,
};
use embassy_rp::pwm::{Config, Pwm};
use embassy_rp::{bind_interrupts, pac, peripherals};
use embassy_rp_examples::pioservo::{self, PioServo};
use embassy_rp_examples::{hwservo, hwservo::us_to_duty};
use embassy_time::Timer;
use fixed::types::extra::U4;
use fixed::FixedU16;
use {defmt_rtt as _, panic_probe as _};

bind_interrupts!(struct Irqs {
    PIO0_IRQ_0 => InterruptHandler<PIO0>;
});

#[embassy_executor::main]
async fn main(_spawner: Spawner) {
    let p = embassy_rp::init(Default::default());

    // Set relay switch signal to high
    let mut a0_pin = Output::new(p.PIN_26, Level::Low);
    a0_pin.set_high();

    // Configure and initialize PWM module
    // Integer part
    let int_part: u16 = 38;
    // Fractional part (3/16 corresponds to 3 in the 4-bit fractional representation)
    let frac_part: u16 = 3;
    let value_bits: u16 = (int_part << 4) | frac_part;
    let divider: FixedU16<U4> = FixedU16::from_bits(value_bits);

    // Single Arm has 3 joints: init Config & Pwm for first 2 joints
    let mut c0: Config = Default::default();
    c0.divider = divider;
    c0.top = hwservo::TOP;
    c0.compare_b = us_to_duty(1000);
    let mut pwm0 = Pwm::new_output_ab(p.PWM_SLICE0, p.PIN_0, p.PIN_1, c0.clone());
    // Init Pio Servo for 3rd join
    let Pio {
        mut common, sm0, ..
    } = Pio::new(p.PIO0, Irqs);
    let pwm_pio = pioservo::PwmPio::new(&mut common, sm0, p.PIN_12);
    let mut pioservo0 = pioservo::PioServoBuilder::new(pwm_pio)
        .set_max_degree_rotation(180) // Example of adjusting values for MG996R servo
        .set_min_pulse_width(Duration::from_micros(800)) // This value was detemined by a rough experiment.
        .set_max_pulse_width(Duration::from_micros(2200)) // Along with this value.
        .build();
    pioservo0.start();

    loop {
        info!("Timestep: 0");
        Timer::after_secs(1).await;
        c0.compare_a = us_to_duty(1500);
        c0.compare_b = us_to_duty(1500);
        pwm0.set_config(&c0);
        pioservo0.write_time(Duration::from_micros(1500));

        info!("Timestep: 1");
        Timer::after_secs(1).await;
        // Leg 0
        c0.compare_a = us_to_duty(1500);
        c0.compare_b = us_to_duty(1500);
        pwm0.set_config(&c0);
        pioservo0.write_time(Duration::from_micros(1500));

        info!("Timestep: 2");
        Timer::after_secs(1).await;
        c0.compare_a = us_to_duty(1900);
        c0.compare_b = us_to_duty(1900);
        pwm0.set_config(&c0);
        pioservo0.write_time(Duration::from_micros(1900));

        info!("Timestep: 3");
        Timer::after_secs(1).await;
        c0.compare_a = us_to_duty(1500);
        c0.compare_b = us_to_duty(1500);
        pwm0.set_config(&c0);
        pioservo0.write_time(Duration::from_micros(1500));
    }
}

pub fn us_to_duty(us: u16) -> u16 {
    (TOP as u32 * us as u32 / PERIOD_US) as u16
}