From f3f2604ff885d98a6731618edf8cb6b8105f7ad3 Mon Sep 17 00:00:00 2001 From: Joe Ardent Date: Mon, 6 Jul 2026 16:07:37 -0700 Subject: [PATCH] moves the servo in response to sensor output --- src/main.rs | 174 +++++++++++++++++++++++++++------------------------- 1 file changed, 91 insertions(+), 83 deletions(-) diff --git a/src/main.rs b/src/main.rs index 7077623..399d7c6 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1,17 +1,17 @@ #![no_std] #![no_main] -use cortex_m::prelude::_embedded_hal_Pwm; +use core::sync::atomic::{AtomicBool, AtomicU32, Ordering::Relaxed}; use defmt::unwrap; use embassy_executor::Spawner; use embassy_stm32::{ adc::{Adc, SampleTime}, - gpio::{AnyPin, Level, Output, Speed}, - peripherals::{ADC1, PA0}, + gpio::OutputType::PushPull, + peripherals::{ADC1, PA0, TIM3}, time::Hertz, timer::{ + low_level::CountingMode::EdgeAlignedUp, simple_pwm::{PwmPin, SimplePwm}, - Channel, }, Config, Peri, }; @@ -19,116 +19,124 @@ use embassy_time::{Duration, Timer}; use {defmt_rtt as _, panic_probe as _}; static SERVO_HZ: Hertz = Hertz::hz(60); -static SERVO_MIN_USECS: f32 = 550.0; -static SERVO_MAX_USECS: f32 = 1900.0; -static SERVO_MIN_DEG: f32 = -180.0; -static SERVO_MAX_DEG: f32 = 180.0; +static SERVO_MIN_USECS: f32 = 490.0; +static SERVO_MAX_USECS: f32 = 2000.0; +static SERVO_MIN_DEG: f32 = -90.0; +static SERVO_MAX_DEG: f32 = 90.0; static SERVO_SLOPE: f32 = (SERVO_MAX_USECS - SERVO_MIN_USECS) / (SERVO_MAX_DEG - SERVO_MIN_DEG); +static SENSOR_VALUE: AtomicU32 = AtomicU32::new(4095); +static SHOULD_EXIT: AtomicBool = AtomicBool::new(false); + fn degrees2duty(degrees: f32, max_dur: Duration, max_duty: u32) -> u32 { let degrees = degrees.clamp(SERVO_MIN_DEG, SERVO_MAX_DEG); let micros = 0.5 + SERVO_MIN_USECS + SERVO_SLOPE * (degrees - SERVO_MIN_DEG); ((micros / max_dur.as_micros() as f32) * max_duty as f32) as u32 } +fn sensor2duty(sensor: u32, max_dur: Duration, max_duty: u32) -> u32 { + let slope = 180.0 / (4095.0 - 300.0); + let degrees = SERVO_MIN_DEG + slope * (sensor as f32 - 300.00); + defmt::debug!("degrees {}", degrees); + degrees2duty(degrees, max_dur, max_duty) +} + #[embassy_executor::main] async fn main(spawner: Spawner) { - let mut config = Config::default(); + let config = config(); - { - use embassy_stm32::rcc::{mux, Pll, PllMul, PllPreDiv, PllRDiv, PllSource, Sysclk}; - config.rcc.pll = Some(Pll { - source: PllSource::HSI, - prediv: PllPreDiv::DIV4, - mul: PllMul::MUL85, - divp: None, - divq: None, - // Main system clock at 170 MHz - divr: Some(PllRDiv::DIV2), - }); - config.rcc.mux.adc12sel = mux::Adcsel::SYS; - config.rcc.sys = Sysclk::PLL1_R; - } + let peripherals = embassy_stm32::init(config); - let p = embassy_stm32::init(config); - - let pwm_pin = PwmPin::new(p.PC7, embassy_stm32::gpio::OutputType::PushPull); - let mut pwm = SimplePwm::new( - p.TIM3, + let pwm_pin = PwmPin::new(peripherals.PC7, PushPull); + let pwm = SimplePwm::new( + peripherals.TIM3, None, Some(pwm_pin), None, None, SERVO_HZ, - embassy_stm32::timer::low_level::CountingMode::EdgeAlignedUp, + EdgeAlignedUp, ); - pwm.enable(Channel::Ch2); + let adc1 = Adc::new(peripherals.ADC1, Default::default()); - let pwm_max_dur = Duration::from_hz(SERVO_HZ.0 as u64); - let pwm_max_duty = pwm.get_max_duty(); + let sensor_input = peripherals.PA0.into(); + spawner.spawn(unwrap!(read_adc1_pa0(adc1, sensor_input))); - let a = Adc::new(p.ADC1, Default::default()); + spawner.spawn(unwrap!(servo_loop(pwm))); - let channel = p.PA0.into(); - spawner.spawn(unwrap!(read_adc1_pa0(a, channel))); - - let zero = degrees2duty(0.0, pwm_max_dur, pwm_max_duty); - pwm.set_duty(Channel::Ch2, zero); - Timer::after_millis(500).await; - - for _ in 0..10 { - let duty = degrees2duty(SERVO_MIN_DEG, pwm_max_dur, pwm_max_duty); - pwm.set_duty(Channel::Ch2, duty); - Timer::after_secs(1).await; - - let duty = degrees2duty(SERVO_MAX_DEG, pwm_max_dur, pwm_max_duty); - pwm.set_duty(Channel::Ch2, duty); - Timer::after_secs(1).await; - } - - pwm.set_duty(Channel::Ch2, zero); - Timer::after_millis(600).await; - - treatbot_5k::exit() + // calls the global exit after 30 seconds + spawner.spawn(unwrap!(exit())); } #[embassy_executor::task] -async fn read_adc1_pa0(mut a: Adc<'static, ADC1>, mut channel: Peri<'static, PA0>) { +async fn servo_loop(mut pwm: SimplePwm<'static, TIM3>) { + let mut pwm_channel = pwm.ch2(); + pwm_channel.enable(); + + let max_duty = pwm_channel.max_duty_cycle(); + let max_dur = Duration::from_hz(SERVO_HZ.0 as u64); + + let zero = degrees2duty(0.0, max_dur, max_duty); + pwm_channel.set_duty_cycle(zero); + Timer::after_millis(500).await; + + defmt::info!( + "max duty: {}", + degrees2duty(SERVO_MAX_DEG, max_dur, max_duty) + ); + let min_duty = degrees2duty(SERVO_MIN_DEG, max_dur, max_duty); + defmt::info!("min duty: {}", min_duty); + + // loop { - let val = a.blocking_read(&mut channel.reborrow(), SampleTime::CYCLES640_5); - if val < 20 { - defmt::info!("low: {}", val); - } else { - defmt::info!("high {}", val); + if SHOULD_EXIT.load(Relaxed) { + break; } + let sensor = SENSOR_VALUE.load(Relaxed); + let duty = sensor2duty(sensor, max_dur, max_duty); + defmt::info!("duty: {}", duty); + pwm_channel.set_duty_cycle(duty); + Timer::after_millis(100).await; + } + + pwm_channel.set_duty_cycle(degrees2duty(0.0, max_dur, max_duty)); + Timer::after_millis(600).await; + + treatbot_5k::exit(); +} + +#[embassy_executor::task] +async fn exit() { + Timer::after_secs(30).await; + SHOULD_EXIT.store(true, Relaxed); +} + +#[embassy_executor::task] +async fn read_adc1_pa0(mut adc1: Adc<'static, ADC1>, mut channel: Peri<'static, PA0>) { + loop { + let val = adc1.blocking_read(&mut channel.reborrow(), SampleTime::CYCLES640_5); + defmt::info!("sensor val: {}", val); + SENSOR_VALUE.store(val as u32, Relaxed); Timer::after_millis(100).await; } } -#[embassy_executor::task] -async fn blinky(p: Peri<'static, AnyPin>) { - let mut led = Output::new(p, Level::High, Speed::Low); +fn config() -> Config { + use embassy_stm32::rcc::{mux, Pll, PllMul, PllPreDiv, PllRDiv, PllSource, Sysclk}; - led.set_low(); + let mut config = Config::default(); + config.rcc.pll = Some(Pll { + source: PllSource::HSI, + prediv: PllPreDiv::DIV4, + mul: PllMul::MUL85, + divp: None, + divq: None, + // Main system clock at 170 MHz + divr: Some(PllRDiv::DIV2), + }); + config.rcc.mux.adc12sel = mux::Adcsel::SYS; + config.rcc.sys = Sysclk::PLL1_R; - loop { - led.set_high(); - Timer::after_millis(300).await; - - led.set_low(); - Timer::after_millis(300).await; - - led.set_high(); - Timer::after_millis(300).await; - - led.set_low(); - Timer::after_millis(300).await; - - led.set_high(); - Timer::after_millis(600).await; - - led.set_low(); - Timer::after_millis(300).await; - } + config }