i am going insane

Cargo.lock

@@ -222,6 +222,7 @@ dependencies = [
  "embassy-rp",
  "embassy-sync",
  "embassy-time",
+ "embedded-hal-async",
  "nanorand",
  "panic-probe",
  "static_cell",

Cargo.toml

@@ -19,6 +19,7 @@ embassy-sync = { version = "0.2.0", git = "https://github.com/embassy-rs/embassy
 static_cell = { version = "1.1", features = ["nightly"]}
 nanorand = { version = "0.7.0", default-features = false, features = ["wyrand"] }
 tinyvec = "1.6.0"
+embedded-hal-async = "0.2.0-alpha.2"
 
 [profile.release]
 lto = "fat"

src/main.rs

@@ -3,22 +3,29 @@
 #![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::i2c::{self, Config as I2Config, InterruptHandler as I2InterruptHandler};
 use embassy_rp::multicore::{spawn_core1, Stack};
-use embassy_rp::peripherals::PIN_25;
+use embassy_rp::peripherals::I2C1;
 use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
-use embassy_sync::channel::Channel;
+use embassy_sync::{channel::Channel, signal::Signal};
+use embassy_time::Instant;
+use embedded_hal_async::i2c::I2c;
 use static_cell::StaticCell;
 use {defmt_rtt as _, panic_probe as _};
 
+bind_interrupts!(struct I2Crqs {
+    I2C1_IRQ => I2InterruptHandler<I2C1>;
+});
+
 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 +68,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 +81,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 +98,63 @@ 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() {
+    // TODO: make this work
+    let p = embassy_rp::init(Default::default());
+
+    let sda = p.PIN_14;
+    let scl = p.PIN_15;
+
+    // What the fuck
+    // I hate this
+    // Fuck you I2C
+    info!("set up i2c");
+    let mut i2c = i2c::I2c::new_async(p.I2C1, scl, sda, I2Crqs, I2Config::default());
+    // Completely guessing at these values
+    let addr: u8 = 0x05;
+    let val: u16 = 2047;
+    // Will this work? Who knows!!
+    i2c.write(addr, &[0, val as u8, (val >> 8) as u8])
+        .await
+        .unwrap();
+
+    use nanorand::{Rng, WyRand};
+    let mut rng = WyRand::new();
+
+    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) => {
+                info!("moving servos");
+            }
+            None => warn!("No state given"),
+        }
+    }
+}
 
 #[inline]
 fn lerp(v0: f32, v1: f32, t: f32) -> f32 {