90 lines
2.7 KiB
Rust
90 lines
2.7 KiB
Rust
use as5600::As5600;
|
|
use embassy_executor::Spawner;
|
|
use embassy_rp::{
|
|
Peri,
|
|
gpio::{AnyPin, Input},
|
|
i2c::{Config, I2c},
|
|
peripherals::{I2C0, PIN_4, PIN_5},
|
|
};
|
|
use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::Channel, mutex::Mutex};
|
|
use embassy_time::{Duration, Timer};
|
|
use ufmt::uwrite;
|
|
|
|
use crate::{Irqs, WHEEL_VALUE, screen::SCREEN_BUFFER, unwrap};
|
|
|
|
/// Button input notification channel
|
|
pub static INPUT: Channel<CriticalSectionRawMutex, u8, 64> = Channel::new();
|
|
|
|
#[embassy_executor::task(pool_size = 4)]
|
|
/// Polls a single pin for falling edge (assumes buttons are pulled up and shorted when pressed)
|
|
/// Sends [INPUT] with given id
|
|
pub async fn button_poll_task(mut button: Input<'static>, id: u8) {
|
|
loop {
|
|
button.wait_for_falling_edge().await;
|
|
INPUT.send(id).await;
|
|
// uwrite!(SCREEN_BUFFER.lock().await.line1(), "btn {}", id);
|
|
Timer::after(Duration::from_millis(50)).await;
|
|
}
|
|
}
|
|
|
|
pub static ANGLE: Mutex<CriticalSectionRawMutex, u16> = Mutex::new(0);
|
|
|
|
#[embassy_executor::task]
|
|
pub async fn rotation_read_task(
|
|
i2c: Peri<'static, I2C0>,
|
|
scl: Peri<'static, PIN_5>,
|
|
sda: Peri<'static, PIN_4>,
|
|
) {
|
|
let i2c = I2c::new_async(i2c, scl, sda, Irqs, Config::default());
|
|
let mut as5600 = As5600::new(i2c);
|
|
|
|
loop {
|
|
Timer::after(Duration::from_millis(50)).await;
|
|
let angle = as5600.angle().unwrap_or(0);
|
|
let mut locked = ANGLE.lock().await;
|
|
let old = *locked as i32;
|
|
*locked = angle;
|
|
drop(locked);
|
|
let left_diff = old - angle as i32;
|
|
let right_diff = angle as i32 - old;
|
|
let diff = if left_diff.abs() < right_diff.abs() {
|
|
-left_diff
|
|
} else {
|
|
right_diff
|
|
};
|
|
let mut wheel = WHEEL_VALUE.lock().await;
|
|
if wheel.max != wheel.min {
|
|
let wheel_range = wheel.max - wheel.min;
|
|
wheel.value += diff * wheel_range / 4096;
|
|
if wheel.value < wheel.min {
|
|
wheel.value = wheel.min;
|
|
} else if wheel.value > wheel.max {
|
|
wheel.value = wheel.max;
|
|
}
|
|
}
|
|
drop(wheel);
|
|
}
|
|
}
|
|
|
|
pub struct InputConfig {
|
|
pub i2c0: Peri<'static, I2C0>,
|
|
pub scl: Peri<'static, PIN_5>,
|
|
pub sda: Peri<'static, PIN_4>,
|
|
|
|
pub button_pins: [(Peri<'static, AnyPin>, u8); 4],
|
|
}
|
|
|
|
pub fn setup(spawner: Spawner, config: InputConfig) {
|
|
spawner.spawn(unwrap!(rotation_read_task(
|
|
config.i2c0,
|
|
config.scl,
|
|
config.sda,
|
|
)));
|
|
|
|
for (pin, id) in config.button_pins {
|
|
spawner.spawn(unwrap!(button_poll_task(
|
|
Input::new(pin, embassy_rp::gpio::Pull::Up),
|
|
id,
|
|
)));
|
|
}
|
|
}
|