dispensing logic needs work

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);
+const SERVO_HZ: Hertz = Hertz::hz(60);
-static SERVO_MIN_USECS: f32 = 490.0;
+const SERVO_MIN_USECS: f32 = 490.0;
-static SERVO_MAX_USECS: f32 = 2000.0;
+const SERVO_MAX_USECS: f32 = 2000.0;
-static SERVO_MIN_DEG: f32 = -90.0;
+const SERVO_MIN_DEG: f32 = -90.0;
-static SERVO_MAX_DEG: f32 = 90.0;
+const SERVO_MAX_DEG: f32 = 90.0;
-static SERVO_SLOPE: f32 = (SERVO_MAX_USECS - SERVO_MIN_USECS) / (SERVO_MAX_DEG - SERVO_MIN_DEG);
+const SERVO_SLOPE: f32 = (SERVO_MAX_USECS - SERVO_MIN_USECS) / (SERVO_MAX_DEG - SERVO_MIN_DEG);
 
-static SENSOR_VALUE: AtomicU32 = AtomicU32::new(4095);
+const TICKS_ON_WHEEL: usize = 200;
-static SHOULD_EXIT: AtomicBool = AtomicBool::new(false);
+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)));
-
+    spawner.spawn(unwrap!(check_dispense()));
-    // calls the global exit after 30 seconds
-    spawner.spawn(unwrap!(exit()));
 }
 
 #[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) {
+        if SHOULD_DISPENSE.load(Relaxed) {
-            break;
+            dispense(&mut pwm_channel, max_dur, max_duty).await;
+            SHOULD_DISPENSE.store(false, Relaxed);
         }
-        let sensor = SENSOR_VALUE.load(Relaxed);
+        Timer::after_millis(500).await;
-        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));
+async fn dispense(pwm_channel: &mut SimplePwmChannel<'_, TIM3>, max_dur: Duration, max_duty: u32) {
-    Timer::after_millis(600).await;
+    let min_duty = degrees2duty(SERVO_MIN_DEG, max_dur, max_duty);
-
+    pwm_channel.set_duty_cycle(min_duty);
-    treatbot_5k::exit();
+    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() {
+async fn check_dispense() {
-    Timer::after_secs(30).await;
+    let mut last_dispensed = Instant::from_secs(0);
-    SHOULD_EXIT.store(true, Relaxed);
+    let mut last_count = 0usize;
-}
-
-#[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);
+        let mut should = false;
-        defmt::info!("sensor val: {}", val);
+        let count = CURRENT_TICKS.load(Relaxed);
-        SENSOR_VALUE.store(val as u32, Relaxed);
+        let now = Instant::now();
-        Timer::after_millis(100).await;
+        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 {