Spawn seperate servo control task

src/main.rs

@@ -3,22 +3,22 @@
 #![feature(type_alias_impl_trait)]
 
 use defmt::*;
-use embassy_executor::Executor;
+use embassy_executor::{Executor, Spawner};
 use embassy_rp::adc::{Adc, Config, InterruptHandler, Pin};
 use embassy_rp::bind_interrupts;
 use embassy_rp::gpio::Pull;
-use embassy_rp::gpio::{Level, Output};
 use embassy_rp::multicore::{spawn_core1, Stack};
-use embassy_rp::peripherals::PIN_25;
 use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
-use embassy_sync::channel::Channel;
+use embassy_sync::{channel::Channel, signal::Signal};
+use embassy_time::Instant;
 use static_cell::StaticCell;
 use {defmt_rtt as _, panic_probe as _};
 
 static mut CORE1_STACK: Stack<4096> = Stack::new();
 static EXECUTOR0: StaticCell<Executor> = StaticCell::new();
 static EXECUTOR1: StaticCell<Executor> = StaticCell::new();
-static BUF_CHANNEL: Channel<CriticalSectionRawMutex, Buffer, 256> = Channel::new();
+static BUF_CHANNEL: Channel<CriticalSectionRawMutex, (Instant, Buffer), 64> = Channel::new();
+static SERVO_SIG: Signal<CriticalSectionRawMutex, Option<State>> = Signal::new();
 bind_interrupts!(struct Irqs {
     ADC_IRQ_FIFO => InterruptHandler;
 });
@@ -61,7 +61,7 @@ fn main() -> ! {
 
     spawn_core1(p.CORE1, unsafe { &mut CORE1_STACK }, move || {
         let executor1 = EXECUTOR1.init(Executor::new());
-        executor1.run(|spawner| unwrap!(spawner.spawn(process_data())));
+        executor1.run(|spawner| unwrap!(spawner.spawn(process_data(spawner))));
     });
 
     let executor0 = EXECUTOR0.init(Executor::new());
@@ -74,18 +74,15 @@ async fn read_eeg() {
     let mut adc = Adc::new(p.ADC, Irqs, Config::default());
     let mut eeg = Pin::new(p.PIN_26, Pull::None);
 
-    use nanorand::{Rng, WyRand};
-    let mut rng = WyRand::new();
-
     let mut buf = Buffer([0; BUF_SIZE]);
     let mut count = 0;
 
     info!("Hello from core 0");
     // Sample EEG data, then append it to buffer
-    // When full, send buffer to be added to the queue
+    // When full, send buffer to the channel queue
     loop {
         if count >= BUF_SIZE {
-            BUF_CHANNEL.send(buf.clone()).await;
+            BUF_CHANNEL.send((Instant::now(), buf.clone())).await;
             count = 0;
         } else {
             buf.0[count] = adc.read(&mut eeg).await.unwrap();
@@ -94,22 +91,46 @@ async fn read_eeg() {
     }
 }
 
+// TODO: async task that goes through queue on timer and merges buffers of similar timestamp
+
 #[embassy_executor::task]
-async fn process_data() {
+async fn process_data(spawner: Spawner) {
+    unwrap!(spawner.spawn(servo_control()));
+
     loop {
-        let mut buffer = BUF_CHANNEL.recv().await;
+        let (time, mut buffer) = BUF_CHANNEL.recv().await;
+        let diff = Instant::now().as_secs() - time.as_secs();
+        if diff >= 10 {
+            // Test value, if buffer is too old throw out
+            break;
+        }
         // Low-pass filter
         // FFT (w/ hanning window)
         info!("Running FFT...");
+        SERVO_SIG.signal(None);
     }
 }
 
-// async fn set_servo_position(state: State) {
-//     // Use a map of positions for each state, and move the servos towards it
-//     // Use lerp and randomness
-//     // I2C control board manages servos
-//     info!("Setting position to {}", state.servo_positions());
-// }
+#[embassy_executor::task]
+async fn servo_control() {
+    loop {
+        let state = SERVO_SIG.wait().await;
+        // Use a map of positions for each state, and move the servos towards it
+        // Use lerp and randomness
+        // I2C control board manages servos
+        match state {
+            Some(s) => set_servo_position(s.servo_positions()).await,
+            None => warn!("No state given"),
+        }
+    }
+}
+
+async fn set_servo_position(pos: ServoPosition) {
+    use nanorand::{Rng, WyRand};
+    let mut rng = WyRand::new();
+
+    info!("Setting position to {}", pos)
+}
 
 #[inline]
 fn lerp(v0: f32, v1: f32, t: f32) -> f32 {