Spawn seperate servo control task
This commit is contained in:
parent
e057fd368e
commit
4db6ef5b4e
59
src/main.rs
59
src/main.rs
|
@ -3,22 +3,22 @@
|
||||||
#![feature(type_alias_impl_trait)]
|
#![feature(type_alias_impl_trait)]
|
||||||
|
|
||||||
use defmt::*;
|
use defmt::*;
|
||||||
use embassy_executor::Executor;
|
use embassy_executor::{Executor, Spawner};
|
||||||
use embassy_rp::adc::{Adc, Config, InterruptHandler, Pin};
|
use embassy_rp::adc::{Adc, Config, InterruptHandler, Pin};
|
||||||
use embassy_rp::bind_interrupts;
|
use embassy_rp::bind_interrupts;
|
||||||
use embassy_rp::gpio::Pull;
|
use embassy_rp::gpio::Pull;
|
||||||
use embassy_rp::gpio::{Level, Output};
|
|
||||||
use embassy_rp::multicore::{spawn_core1, Stack};
|
use embassy_rp::multicore::{spawn_core1, Stack};
|
||||||
use embassy_rp::peripherals::PIN_25;
|
|
||||||
use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
|
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 static_cell::StaticCell;
|
||||||
use {defmt_rtt as _, panic_probe as _};
|
use {defmt_rtt as _, panic_probe as _};
|
||||||
|
|
||||||
static mut CORE1_STACK: Stack<4096> = Stack::new();
|
static mut CORE1_STACK: Stack<4096> = Stack::new();
|
||||||
static EXECUTOR0: StaticCell<Executor> = StaticCell::new();
|
static EXECUTOR0: StaticCell<Executor> = StaticCell::new();
|
||||||
static EXECUTOR1: 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 {
|
bind_interrupts!(struct Irqs {
|
||||||
ADC_IRQ_FIFO => InterruptHandler;
|
ADC_IRQ_FIFO => InterruptHandler;
|
||||||
});
|
});
|
||||||
|
@ -61,7 +61,7 @@ fn main() -> ! {
|
||||||
|
|
||||||
spawn_core1(p.CORE1, unsafe { &mut CORE1_STACK }, move || {
|
spawn_core1(p.CORE1, unsafe { &mut CORE1_STACK }, move || {
|
||||||
let executor1 = EXECUTOR1.init(Executor::new());
|
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());
|
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 adc = Adc::new(p.ADC, Irqs, Config::default());
|
||||||
let mut eeg = Pin::new(p.PIN_26, Pull::None);
|
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 buf = Buffer([0; BUF_SIZE]);
|
||||||
let mut count = 0;
|
let mut count = 0;
|
||||||
|
|
||||||
info!("Hello from core 0");
|
info!("Hello from core 0");
|
||||||
// Sample EEG data, then append it to buffer
|
// 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 {
|
loop {
|
||||||
if count >= BUF_SIZE {
|
if count >= BUF_SIZE {
|
||||||
BUF_CHANNEL.send(buf.clone()).await;
|
BUF_CHANNEL.send((Instant::now(), buf.clone())).await;
|
||||||
count = 0;
|
count = 0;
|
||||||
} else {
|
} else {
|
||||||
buf.0[count] = adc.read(&mut eeg).await.unwrap();
|
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]
|
#[embassy_executor::task]
|
||||||
async fn process_data() {
|
async fn process_data(spawner: Spawner) {
|
||||||
|
unwrap!(spawner.spawn(servo_control()));
|
||||||
|
|
||||||
loop {
|
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
|
// Low-pass filter
|
||||||
// FFT (w/ hanning window)
|
// FFT (w/ hanning window)
|
||||||
info!("Running FFT...");
|
info!("Running FFT...");
|
||||||
|
SERVO_SIG.signal(None);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// async fn set_servo_position(state: State) {
|
#[embassy_executor::task]
|
||||||
// // Use a map of positions for each state, and move the servos towards it
|
async fn servo_control() {
|
||||||
// // Use lerp and randomness
|
loop {
|
||||||
// // I2C control board manages servos
|
let state = SERVO_SIG.wait().await;
|
||||||
// info!("Setting position to {}", state.servo_positions());
|
// 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]
|
#[inline]
|
||||||
fn lerp(v0: f32, v1: f32, t: f32) -> f32 {
|
fn lerp(v0: f32, v1: f32, t: f32) -> f32 {
|
||||||
|
|
Loading…
Reference in New Issue