diff --git a/embassy-rp/src/pio_programs/mod.rs b/embassy-rp/src/pio_programs/mod.rs index 74a3714c77..26b45f7503 100644 --- a/embassy-rp/src/pio_programs/mod.rs +++ b/embassy-rp/src/pio_programs/mod.rs @@ -8,6 +8,7 @@ pub mod onewire; pub mod pwm; pub mod rotary_encoder; pub mod spi; -pub mod stepper; +pub mod stepper4; +pub mod stepper2; pub mod uart; pub mod ws2812; diff --git a/embassy-rp/src/pio_programs/stepper2.rs b/embassy-rp/src/pio_programs/stepper2.rs new file mode 100644 index 0000000000..ac1cbba244 --- /dev/null +++ b/embassy-rp/src/pio_programs/stepper2.rs @@ -0,0 +1,72 @@ +use core::f64::consts::PI; +use embassy_rp::{ + Peri, + gpio::Output, + pio::{ + Common, Config, Direction, Instance, LoadedProgram, PioPin, StateMachine, + program::pio_asm, + }, + pio_programs::clock_divider::calculate_pio_clock_divider, +}; + +pub struct PioStepper2Program<'a, PIO: Instance> { + prg: LoadedProgram<'a, PIO>, +} + +impl<'a, PIO: Instance> PioStepper2Program<'a, PIO> { + pub fn new(common: &mut Common<'a, PIO>) -> Self { + let prg = pio_asm!( + ".wrap_target", + "pull block", + "out x, 32", + "loop:", + " set pins, 1 [31]", + " set pins, 0 [31]", + " jmp x-- loop", + ".wrap" + ); + + let prg = common.load_program(&prg.program); + + Self { prg } + } +} + +pub struct Stepper2<'d, T: Instance, const SM: usize> { + sm: StateMachine<'d, T, SM>, + dir: Output<'d>, +} + +impl<'d, T: Instance, const SM: usize> Stepper2<'d, T, SM> { + pub fn new( + pio: &mut Common<'d, T>, + mut sm: StateMachine<'d, T, SM>, + stp: Peri<'d, impl PioPin>, + dir: Output<'d>, + program: &PioStepperProgram<'d, T>, + ) -> Self { + let stp = pio.make_pio_pin(stp); + sm.set_pin_dirs(Direction::Out, &[&stp]); + + let mut cfg = Config::default(); + cfg.set_set_pins(&[&stp]); + + cfg.clock_divider = + calculate_pio_clock_divider(100 * 65); + + cfg.use_program(&program.prg, &[]); + sm.set_config(&cfg); + sm.set_enable(true); + Self { sm, dir } + } + + pub fn set_frequency(&mut self, freq: u32) { + let clock_divider = calculate_pio_clock_divider(freq * 65); + let divider_f32 = clock_divider.to_num::(); + assert!(divider_f32 <= 65536.0, "clkdiv must be <= 65536"); + assert!(divider_f32 >= 1.0, "clkdiv must be >= 1"); + + self.sm.set_clock_divider(clock_divider); + self.sm.clkdiv_restart(); + } +} diff --git a/embassy-rp/src/pio_programs/stepper.rs b/embassy-rp/src/pio_programs/stepper4.rs similarity index 93% rename from embassy-rp/src/pio_programs/stepper.rs rename to embassy-rp/src/pio_programs/stepper4.rs index 5762ee1895..38793714db 100644 --- a/embassy-rp/src/pio_programs/stepper.rs +++ b/embassy-rp/src/pio_programs/stepper4.rs @@ -7,11 +7,11 @@ use crate::pio::{Common, Config, Direction, Instance, Irq, LoadedProgram, PioPin use crate::pio_programs::clock_divider::calculate_pio_clock_divider; /// This struct represents a Stepper driver program loaded into pio instruction memory. -pub struct PioStepperProgram<'a, PIO: Instance> { +pub struct PioStepper4Program<'a, PIO: Instance> { prg: LoadedProgram<'a, PIO>, } -impl<'a, PIO: Instance> PioStepperProgram<'a, PIO> { +impl<'a, PIO: Instance> PioStepper4Program<'a, PIO> { /// Load the program into the given pio pub fn new(common: &mut Common<'a, PIO>) -> Self { let prg = pio::pio_asm!( @@ -37,12 +37,12 @@ impl<'a, PIO: Instance> PioStepperProgram<'a, PIO> { } /// Pio backed Stepper driver -pub struct PioStepper<'d, T: Instance, const SM: usize> { +pub struct PioStepper4<'d, T: Instance, const SM: usize> { irq: Irq<'d, T, SM>, sm: StateMachine<'d, T, SM>, } -impl<'d, T: Instance, const SM: usize> PioStepper<'d, T, SM> { +impl<'d, T: Instance, const SM: usize> PioStepper4<'d, T, SM> { /// Configure a state machine to drive a stepper pub fn new( pio: &mut Common<'d, T>, @@ -52,7 +52,7 @@ impl<'d, T: Instance, const SM: usize> PioStepper<'d, T, SM> { pin1: Peri<'d, impl PioPin>, pin2: Peri<'d, impl PioPin>, pin3: Peri<'d, impl PioPin>, - program: &PioStepperProgram<'d, T>, + program: &PioStepper4Program<'d, T>, ) -> Self { let pin0 = pio.make_pio_pin(pin0); let pin1 = pio.make_pio_pin(pin1); diff --git a/examples/rp/src/bin/pio_stepper.rs b/examples/rp/src/bin/pio_stepper.rs index e8f203990f..f51e3aad4b 100644 --- a/examples/rp/src/bin/pio_stepper.rs +++ b/examples/rp/src/bin/pio_stepper.rs @@ -9,7 +9,7 @@ use embassy_executor::Spawner; use embassy_rp::bind_interrupts; use embassy_rp::peripherals::PIO0; use embassy_rp::pio::{InterruptHandler, Pio}; -use embassy_rp::pio_programs::stepper::{PioStepper, PioStepperProgram}; +use embassy_rp::pio_programs::stepper::{PioStepper4, PioStepper4Program}; use embassy_time::{Duration, Timer, with_timeout}; use {defmt_rtt as _, panic_probe as _}; @@ -24,8 +24,8 @@ async fn main(_spawner: Spawner) { mut common, irq0, sm0, .. } = Pio::new(p.PIO0, Irqs); - let prg = PioStepperProgram::new(&mut common); - let mut stepper = PioStepper::new(&mut common, sm0, irq0, p.PIN_4, p.PIN_5, p.PIN_6, p.PIN_7, &prg); + let prg = PioStepper4Program::new(&mut common); + let mut stepper = PioStepper4::new(&mut common, sm0, irq0, p.PIN_4, p.PIN_5, p.PIN_6, p.PIN_7, &prg); stepper.set_frequency(120); loop { info!("CW full steps"); diff --git a/examples/rp235x/src/bin/pio_stepper.rs b/examples/rp235x/src/bin/pio_stepper.rs index 9b33710ad5..cc88de3294 100644 --- a/examples/rp235x/src/bin/pio_stepper.rs +++ b/examples/rp235x/src/bin/pio_stepper.rs @@ -9,7 +9,7 @@ use embassy_executor::Spawner; use embassy_rp::bind_interrupts; use embassy_rp::peripherals::PIO0; use embassy_rp::pio::{InterruptHandler, Pio}; -use embassy_rp::pio_programs::stepper::{PioStepper, PioStepperProgram}; +use embassy_rp::pio_programs::stepper::{PioStepper4, PioStepper4Program}; use embassy_time::{Duration, Timer, with_timeout}; use {defmt_rtt as _, panic_probe as _}; @@ -24,8 +24,8 @@ async fn main(_spawner: Spawner) { mut common, irq0, sm0, .. } = Pio::new(p.PIO0, Irqs); - let prg = PioStepperProgram::new(&mut common); - let mut stepper = PioStepper::new(&mut common, sm0, irq0, p.PIN_4, p.PIN_5, p.PIN_6, p.PIN_7, &prg); + let prg = PioStepper4Program::new(&mut common); + let mut stepper = PioStepper4::new(&mut common, sm0, irq0, p.PIN_4, p.PIN_5, p.PIN_6, p.PIN_7, &prg); stepper.set_frequency(120); loop { info!("CW full steps");