From 4b50993773d061300d16ea4df7a3b269606aef8c Mon Sep 17 00:00:00 2001
From: Erin Abicht <contact@the-system.eu.org>
Date: Fri, 14 Jul 2023 12:52:38 -0400
Subject: [PATCH] Read analogue EEG data

---
 src/main.rs | 60 ++++++++++++++++++++++++++++++++++++++++++-----------
 1 file changed, 48 insertions(+), 12 deletions(-)

diff --git a/src/main.rs b/src/main.rs
index 3922e8a..662fae8 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -4,6 +4,9 @@
 
 use defmt::*;
 use embassy_executor::Executor;
+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;
@@ -12,37 +15,53 @@ use embassy_sync::channel::Channel;
 use static_cell::StaticCell;
 use {defmt_rtt as _, panic_probe as _};
 
-const BUF_SIZE: usize = 64;
 static mut CORE1_STACK: Stack<4096> = Stack::new();
 static EXECUTOR0: StaticCell<Executor> = StaticCell::new();
 static EXECUTOR1: StaticCell<Executor> = StaticCell::new();
 static CHANNEL: Channel<CriticalSectionRawMutex, Buffer, 1> = Channel::new();
+bind_interrupts!(struct Irqs {
+    ADC_IRQ_FIFO => InterruptHandler;
+});
 
+const BUF_SIZE: usize = 64;
+
+#[derive(Format)]
 enum State {
     Happy,
     Sad,
     Relaxed,
     Surprised,
-    Unknown,
+}
+
+// Position in degrees for each servo
+#[derive(Format)]
+struct ServoPosition(f32, f32, f32, f32);
+
+impl State {
+    fn servo_positions(&self) -> ServoPosition {
+        // This is where we define the positions and pose for each state
+        match self {
+            _ => ServoPosition(0.0, 0.0, 0.0, 0.0),
+        }
+    }
 }
 
 #[derive(Clone)]
-struct Buffer([f32; BUF_SIZE]);
+struct Buffer([u16; BUF_SIZE]);
 
 impl Default for Buffer {
     fn default() -> Self {
-        Buffer([0.0; BUF_SIZE])
+        Buffer([0; BUF_SIZE])
     }
 }
 
 #[cortex_m_rt::entry]
 fn main() -> ! {
     let p = embassy_rp::init(Default::default());
-    let led = Output::new(p.PIN_25, Level::Low);
 
     spawn_core1(p.CORE1, unsafe { &mut CORE1_STACK }, move || {
         let executor1 = EXECUTOR1.init(Executor::new());
-        executor1.run(|spawner| unwrap!(spawner.spawn(core1_task(led))));
+        executor1.run(|spawner| unwrap!(spawner.spawn(core1_task())));
     });
 
     let executor0 = EXECUTOR0.init(Executor::new());
@@ -51,10 +70,14 @@ fn main() -> ! {
 
 #[embassy_executor::task]
 async fn core0_task() {
+    let p = embassy_rp::init(Default::default());
+    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.0; BUF_SIZE]);
+    let mut buf = Buffer([0; BUF_SIZE]);
     let mut count = 0;
 
     info!("Hello from core 0");
@@ -65,14 +88,14 @@ async fn core0_task() {
             CHANNEL.send(buf.clone()).await;
             count = 0;
         } else {
-            buf.0[count] = rng.generate::<f32>();
+            buf.0[count] = adc.read(&mut eeg).await.unwrap();
             count += 1;
         }
     }
 }
 
 #[embassy_executor::task]
-async fn core1_task(mut led: Output<'static, PIN_25>) {
+async fn core1_task() {
     use tinyvec::ArrayVec;
     let mut queue = ArrayVec::<[Buffer; 64]>::new();
 
@@ -82,14 +105,27 @@ async fn core1_task(mut led: Output<'static, PIN_25>) {
         // Need to test
         queue.push(CHANNEL.recv().await);
 
-        process_data(&queue[0]).await;
+        match process_data(&queue[0]).await {
+            Some(s) => {
+                info!("Parsed '{}' state from EEG data", &s);
+                set_servo_position(s).await;
+            }
+            None => warn!("Unable to match EEG data to state"),
+        }
         queue.remove(0);
     }
 }
 
-async fn process_data(buf: &Buffer) -> State {
+async fn process_data(buf: &Buffer) -> Option<State> {
     // Low-pass filter
     // FFT (w/ hanning window)
     info!("Running FFT...");
-    return State::Unknown;
+    return 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());
 }