From a146554e3860b6c77823dc89a92fe1b26b79f0f0 Mon Sep 17 00:00:00 2001 From: Joe Ardent Date: Sun, 12 Jul 2026 15:08:52 -0700 Subject: [PATCH] dispensing logic needs work --- src/main.rs | 116 ++++++++++++++++++++++++++++++---------------------- 1 file changed, 68 insertions(+), 48 deletions(-) diff --git a/src/main.rs b/src/main.rs index 399d7c6..0d07563 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1,32 +1,36 @@ #![no_std] #![no_main] -use core::sync::atomic::{AtomicBool, AtomicU32, Ordering::Relaxed}; +use core::sync::atomic::{AtomicBool, AtomicUsize, Ordering::Relaxed}; use defmt::unwrap; use embassy_executor::Spawner; use embassy_stm32::{ + Config, Peri, adc::{Adc, SampleTime}, gpio::OutputType::PushPull, peripherals::{ADC1, PA0, TIM3}, time::Hertz, timer::{ low_level::CountingMode::EdgeAlignedUp, - simple_pwm::{PwmPin, SimplePwm}, + simple_pwm::{PwmPin, SimplePwm, SimplePwmChannel}, }, - Config, Peri, }; -use embassy_time::{Duration, Timer}; +use embassy_time::{Duration, Instant, Timer}; use {defmt_rtt as _, panic_probe as _}; -static SERVO_HZ: Hertz = Hertz::hz(60); -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); +const SERVO_HZ: Hertz = Hertz::hz(60); +const SERVO_MIN_USECS: f32 = 490.0; +const SERVO_MAX_USECS: f32 = 2000.0; +const SERVO_MIN_DEG: f32 = -90.0; +const SERVO_MAX_DEG: f32 = 90.0; +const 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); +const TICKS_ON_WHEEL: usize = 200; +const PREVIOUS_SENSOR_READS_LEN: usize = 21; +const MEDIAN_DELTA_IDX: usize = 10; + +static SHOULD_DISPENSE: AtomicBool = AtomicBool::new(false); +static CURRENT_TICKS: AtomicUsize = AtomicUsize::new(0); fn degrees2duty(degrees: f32, max_dur: Duration, max_duty: u32) -> u32 { let degrees = degrees.clamp(SERVO_MIN_DEG, SERVO_MAX_DEG); @@ -34,13 +38,6 @@ fn degrees2duty(degrees: f32, max_dur: Duration, max_duty: u32) -> u32 { ((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 config = config(); @@ -61,12 +58,9 @@ async fn main(spawner: Spawner) { let adc1 = Adc::new(peripherals.ADC1, Default::default()); let sensor_input = peripherals.PA0.into(); - spawner.spawn(unwrap!(read_adc1_pa0(adc1, sensor_input))); - + spawner.spawn(unwrap!(count_ticks(adc1, sensor_input))); spawner.spawn(unwrap!(servo_loop(pwm))); - - // calls the global exit after 30 seconds - spawner.spawn(unwrap!(exit())); + spawner.spawn(unwrap!(check_dispense())); } #[embassy_executor::task] @@ -88,42 +82,68 @@ async fn servo_loop(mut pwm: SimplePwm<'static, TIM3>) { let min_duty = degrees2duty(SERVO_MIN_DEG, max_dur, max_duty); defmt::info!("min duty: {}", min_duty); - // loop { - if SHOULD_EXIT.load(Relaxed) { - break; + if SHOULD_DISPENSE.load(Relaxed) { + dispense(&mut pwm_channel, max_dur, max_duty).await; + SHOULD_DISPENSE.store(false, Relaxed); } - 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; + Timer::after_millis(500).await; } +} - pwm_channel.set_duty_cycle(degrees2duty(0.0, max_dur, max_duty)); - Timer::after_millis(600).await; - - treatbot_5k::exit(); +async fn dispense(pwm_channel: &mut SimplePwmChannel<'_, TIM3>, max_dur: Duration, max_duty: u32) { + let min_duty = degrees2duty(SERVO_MIN_DEG, max_dur, max_duty); + pwm_channel.set_duty_cycle(min_duty); + Timer::after_millis(500).await; + pwm_channel.set_duty_cycle(degrees2duty(SERVO_MAX_DEG, max_dur, max_duty)); + Timer::after_millis(500).await; + let zero = degrees2duty(0.0, max_dur, max_duty); + pwm_channel.set_duty_cycle(zero); } #[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>) { +async fn check_dispense() { + let mut last_dispensed = Instant::from_secs(0); + let mut last_count = 0usize; 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; + let mut should = false; + let count = CURRENT_TICKS.load(Relaxed); + let now = Instant::now(); + if count.saturating_sub(last_count) > 2 * TICKS_ON_WHEEL { + should = true; + } else if count > last_count && now - last_dispensed > Duration::from_secs(600) { + should = true; + } + + if should { + SHOULD_DISPENSE.store(true, Relaxed); + last_dispensed = now; + last_count = count; + } + + Timer::after_secs(1).await; + } +} + +#[embassy_executor::task] +async fn count_ticks(mut adc1: Adc<'static, ADC1>, mut channel: Peri<'static, PA0>) { + let mut prev = u16::MAX; + let mut current_ticks = 0usize; + loop { + let val = adc1.blocking_read(&mut channel.reborrow(), SampleTime::CYCLES247_5); + let delta = val.saturating_sub(prev); + if delta >= 20 { + current_ticks = current_ticks.wrapping_add(1usize); + CURRENT_TICKS.store(current_ticks, Relaxed); + } + defmt::info!("val: {}, delta: {}, count: {}", val, delta, current_ticks); + prev = val; + Timer::after_millis(10).await; } } fn config() -> Config { - use embassy_stm32::rcc::{mux, Pll, PllMul, PllPreDiv, PllRDiv, PllSource, Sysclk}; + use embassy_stm32::rcc::{Pll, PllMul, PllPreDiv, PllRDiv, PllSource, Sysclk, mux}; let mut config = Config::default(); config.rcc.pll = Some(Pll {