Read analogue EEG data

Cargo.toml

@@ -12,6 +12,10 @@ defmt = "0.3"
 defmt-rtt = "0.3"
 panic-probe = { version = "0.3", features = ["print-defmt"] }
 
-embassy-executor = { version = "0.1.0", git = "https://github.com/embassy-rs/embassy", features = ["defmt", "integrated-timers"] }
+embassy-executor = { version = "0.2.0", git = "https://github.com/embassy-rs/embassy", features = ["defmt", "integrated-timers", "nightly", "arch-cortex-m", "executor-thread", "executor-interrupt"] }
 embassy-time = { version = "0.1.0", git = "https://github.com/embassy-rs/embassy", features = ["defmt", "defmt-timestamp-uptime"] }
 embassy-rp = { version = "0.1.0", git = "https://github.com/embassy-rs/embassy", features = ["defmt", "unstable-traits", "nightly", "unstable-pac", "time-driver"] }
+embassy-sync = { version = "0.2.0", git = "https://github.com/embassy-rs/embassy", features = ["defmt"] }
+static_cell = { version = "1.1", features = ["nightly"]}
+nanorand = { version = "0.7.0", default-features = false, features = ["wyrand"] }
+tinyvec = "1.6.0"

README.md

@@ -0,0 +1,5 @@
+# CyberTail Code
+This code will run on the **Pi Pico** microcontroller.
+It will read in data from the **EEG Module**, use *FFT* to interpret it, and then send positions to the Servo Controller.
+
+Very much a work in progress at the moment.

src/main.rs

@@ -3,38 +3,129 @@
 #![feature(type_alias_impl_trait)]
 
 use defmt::*;
-use embassy_executor::Spawner;
-use embassy_rp::gpio;
-use embassy_time::{Duration, Timer};
-use gpio::{Level, Output};
+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;
+use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
+use embassy_sync::channel::Channel;
+use static_cell::StaticCell;
 use {defmt_rtt as _, panic_probe as _};
 
-#[embassy_executor::main]
-async fn main(_spawner: Spawner) {
-    // Initialise Peripherals
-    let p = embassy_rp::init(Default::default());
+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;
+});
 
-    // Create LED
-    let mut led = Output::new(p.PIN_25, Level::Low);
+const BUF_SIZE: usize = 64;
 
-    // Loop
-    loop {
-        // Log
-        info!("LED On!");
+#[derive(Format)]
+enum State {
+    Happy,
+    Sad,
+    Relaxed,
+    Surprised,
+}
 
-        // Turn LED On
-        led.set_high();
+// Position in degrees for each servo
+#[derive(Format)]
+struct ServoPosition(f32, f32, f32, f32);
 
-        // Wait 100ms
-        Timer::after(Duration::from_millis(100)).await;
-
-        // Log
-        info!("LED Off!");
-
-        // Turn Led Off
-        led.set_low();
-
-        // Wait 100ms
-        Timer::after(Duration::from_millis(100)).await;
+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([u16; BUF_SIZE]);
+
+impl Default for Buffer {
+    fn default() -> Self {
+        Buffer([0; BUF_SIZE])
+    }
+}
+
+#[cortex_m_rt::entry]
+fn main() -> ! {
+    let p = embassy_rp::init(Default::default());
+
+    spawn_core1(p.CORE1, unsafe { &mut CORE1_STACK }, move || {
+        let executor1 = EXECUTOR1.init(Executor::new());
+        executor1.run(|spawner| unwrap!(spawner.spawn(core1_task())));
+    });
+
+    let executor0 = EXECUTOR0.init(Executor::new());
+    executor0.run(|spawner| unwrap!(spawner.spawn(core0_task())));
+}
+
+#[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; 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
+    loop {
+        if count >= BUF_SIZE {
+            CHANNEL.send(buf.clone()).await;
+            count = 0;
+        } else {
+            buf.0[count] = adc.read(&mut eeg).await.unwrap();
+            count += 1;
+        }
+    }
+}
+
+#[embassy_executor::task]
+async fn core1_task() {
+    use tinyvec::ArrayVec;
+    let mut queue = ArrayVec::<[Buffer; 64]>::new();
+
+    info!("Hello from core 1");
+    loop {
+        // Shouldn't block on waiting for message?
+        // Need to test
+        queue.push(CHANNEL.recv().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) -> Option<State> {
+    // Low-pass filter
+    // FFT (w/ hanning window)
+    info!("Running FFT...");
+    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());
+}