Added movement task and rewired the channels

This commit is contained in:
Sebastian 2022-08-21 18:11:58 +02:00
parent ace48440f3
commit 90768d9bf0
4 changed files with 101 additions and 18 deletions

View file

@ -4,6 +4,7 @@ use embassy_stm32::time::Hertz;
use embassy_time::{Duration, Timer}; use embassy_time::{Duration, Timer};
use embassy_util::blocking_mutex::raw::ThreadModeRawMutex; use embassy_util::blocking_mutex::raw::ThreadModeRawMutex;
use embassy_util::channel::mpmc::Receiver; use embassy_util::channel::mpmc::Receiver;
use embassy_util::{select, Either};
use embedded_graphics::{ use embedded_graphics::{
mono_font::{ mono_font::{
@ -21,14 +22,14 @@ use heapless::String;
use ssd1306::{prelude::*, I2CDisplayInterface, Ssd1306}; use ssd1306::{prelude::*, I2CDisplayInterface, Ssd1306};
use crate::AzElPair; use crate::{AzElPair, RotorState};
#[embassy_executor::task] #[embassy_executor::task]
pub async fn display_task( pub async fn display_task(
i2c1: peripherals::I2C1, i2c1: peripherals::I2C1,
sda: peripherals::PB6, sda: peripherals::PB6,
scl: peripherals::PB7, scl: peripherals::PB7,
cmd_receiver: Receiver<'static, ThreadModeRawMutex, AzElPair, 1>, cmd_receiver: Receiver<'static, ThreadModeRawMutex, RotorState, 1>,
) { ) {
let i2c = i2c::I2c::new(i2c1, sda, scl, Hertz::hz(100_000), i2c::Config::default()); let i2c = i2c::I2c::new(i2c1, sda, scl, Hertz::hz(100_000), i2c::Config::default());
@ -46,9 +47,13 @@ pub async fn display_task(
.fill_color(BinaryColor::On) .fill_color(BinaryColor::On)
.build(); .build();
loop { let mut rotor_state = RotorState {
let pair = cmd_receiver.recv().await; actual_pos: AzElPair { az: 0, el: 0 },
setpoint_pos: AzElPair { az: 0, el: 0 },
stopped: false,
};
loop {
display.clear(); display.clear();
Text::new("AFG rotor ctrl v0.1.0", Point::new(0, 6), text_small) Text::new("AFG rotor ctrl v0.1.0", Point::new(0, 6), text_small)
@ -69,19 +74,28 @@ pub async fn display_task(
.unwrap(); .unwrap();
let mut tmp: String<16> = String::new(); let mut tmp: String<16> = String::new();
write!(tmp, "AZ: {}", pair.az).unwrap(); write!(tmp, "AZ: {}", rotor_state.setpoint_pos.az).unwrap();
Text::new(&tmp, Point::new(1, 30), text_large_inv) Text::new(&tmp, Point::new(1, 30), text_large_inv)
.draw(&mut display) .draw(&mut display)
.unwrap(); .unwrap();
tmp.clear(); tmp.clear();
write!(tmp, "EL: {}", pair.el).unwrap(); write!(tmp, "EL: {}", rotor_state.setpoint_pos.el).unwrap();
Text::new(&tmp, Point::new(64, 30), text_large_inv) Text::new(&tmp, Point::new(64, 30), text_large_inv)
.draw(&mut display) .draw(&mut display)
.unwrap(); .unwrap();
display.flush().unwrap(); display.flush().unwrap();
Timer::after(Duration::from_millis(500)).await; let result = select(
Timer::after(Duration::from_millis(100)),
cmd_receiver.recv(),
)
.await;
match result {
Either::First(_) => {}
Either::Second(new_state) => rotor_state = new_state,
}
} }
} }

View file

@ -21,18 +21,29 @@ mod display;
use display::display_task; use display::display_task;
mod usb; mod usb;
use usb::usb_task; use usb::{usb_task, Gs232Cmd};
#[derive(PartialEq, Format)] mod movement;
use movement::movement_task;
#[derive(PartialEq, Format, Copy, Clone)]
pub struct AzElPair { pub struct AzElPair {
az: u16, az: u16,
el: u16, el: u16,
} }
#[derive(PartialEq, Format, Copy, Clone)]
pub struct RotorState {
actual_pos: AzElPair,
setpoint_pos: AzElPair,
stopped: bool,
}
#[embassy_executor::main] #[embassy_executor::main]
async fn main(spawner: Spawner) { async fn main(spawner: Spawner) {
static CMD_CHAN: Channel<ThreadModeRawMutex, AzElPair, 1> = Channel::new(); static CMD_CHAN: Channel<ThreadModeRawMutex, Gs232Cmd, 1> = Channel::new();
static POS_CHAN: Channel<ThreadModeRawMutex, AzElPair, 1> = Channel::new(); static POS_CHAN: Channel<ThreadModeRawMutex, AzElPair, 1> = Channel::new();
static STATE_CHAN: Channel<ThreadModeRawMutex, RotorState, 1> = Channel::new();
let mut config = Config::default(); let mut config = Config::default();
config.rcc.hse = Some(Hertz(8_000_000)); config.rcc.hse = Some(Hertz(8_000_000));
@ -54,7 +65,15 @@ async fn main(spawner: Spawner) {
spawner spawner
.spawn(usb_task(p.USB, p.PA12, p.PA11, CMD_CHAN.sender())) .spawn(usb_task(p.USB, p.PA12, p.PA11, CMD_CHAN.sender()))
.unwrap(); .unwrap();
spawner spawner
.spawn(display_task(p.I2C1, p.PB6, p.PB7, CMD_CHAN.receiver())) .spawn(movement_task(
CMD_CHAN.receiver(),
POS_CHAN.sender(),
STATE_CHAN.sender(),
))
.unwrap();
spawner
.spawn(display_task(p.I2C1, p.PB6, p.PB7, STATE_CHAN.receiver()))
.unwrap(); .unwrap();
} }

50
src/movement.rs Normal file
View file

@ -0,0 +1,50 @@
use defmt::Format;
use core::fmt::Write;
use embassy_stm32::interrupt;
use embassy_stm32::peripherals;
use embassy_stm32::usb::Driver;
use embassy_usb::Builder;
use embassy_usb_serial::{CdcAcmClass, State};
use embassy_util::blocking_mutex::raw::ThreadModeRawMutex;
use embassy_util::channel::mpmc::{Receiver, Sender};
use futures::future::join;
use heapless::String;
use crate::usb::Gs232Cmd;
use crate::{AzElPair, RotorState};
#[embassy_executor::task]
pub async fn movement_task(
cmd_receiver: Receiver<'static, ThreadModeRawMutex, Gs232Cmd, 1>,
pos_sender: Sender<'static, ThreadModeRawMutex, AzElPair, 1>,
state_sender: Sender<'static, ThreadModeRawMutex, RotorState, 1>,
) {
let mut rotor_state = RotorState {
actual_pos: AzElPair { az: 0, el: 0 },
setpoint_pos: AzElPair { az: 0, el: 0 },
stopped: false,
};
loop {
let cmd = cmd_receiver.recv().await;
match cmd {
Gs232Cmd::MoveTo(pair) => {
rotor_state.setpoint_pos = pair;
rotor_state.stopped = false;
}
Gs232Cmd::Stop => {
rotor_state.stopped = true;
}
_ => {}
};
join(
pos_sender.send(rotor_state.actual_pos),
state_sender.send(rotor_state),
)
.await;
}
}

View file

@ -1,5 +1,6 @@
use defmt::Format; use defmt::Format;
use core::fmt::Write;
use embassy_stm32::interrupt; use embassy_stm32::interrupt;
use embassy_stm32::peripherals; use embassy_stm32::peripherals;
use embassy_stm32::usb::Driver; use embassy_stm32::usb::Driver;
@ -9,8 +10,6 @@ use embassy_util::blocking_mutex::raw::ThreadModeRawMutex;
use embassy_util::channel::mpmc::Sender; use embassy_util::channel::mpmc::Sender;
use futures::future::join; use futures::future::join;
use core::fmt::Write;
use heapless::String; use heapless::String;
use crate::AzElPair; use crate::AzElPair;
@ -20,7 +19,7 @@ pub async fn usb_task(
usb: peripherals::USB, usb: peripherals::USB,
dp_pin: peripherals::PA12, dp_pin: peripherals::PA12,
dm_pin: peripherals::PA11, dm_pin: peripherals::PA11,
cmd_sender: Sender<'static, ThreadModeRawMutex, AzElPair, 1>, cmd_sender: Sender<'static, ThreadModeRawMutex, Gs232Cmd, 1>,
) { ) {
let irq = interrupt::take!(USB_LP_CAN1_RX0); let irq = interrupt::take!(USB_LP_CAN1_RX0);
let driver = Driver::new(usb, irq, dp_pin, dm_pin); let driver = Driver::new(usb, irq, dp_pin, dm_pin);
@ -103,11 +102,12 @@ pub async fn usb_task(
Gs232Cmd::GetAlEz => { Gs232Cmd::GetAlEz => {
write!(&mut resp, "AZ={} EL={}\r", az_actual, el_actual).unwrap(); write!(&mut resp, "AZ={} EL={}\r", az_actual, el_actual).unwrap();
} }
Gs232Cmd::MoveTo(pair) => { Gs232Cmd::MoveTo(_) => {
cmd_sender.send(pair).await; cmd_sender.send(cmd).await;
resp.push_str("\r").unwrap(); resp.push_str("\r").unwrap();
} }
Gs232Cmd::Stop => { Gs232Cmd::Stop => {
cmd_sender.send(cmd).await;
resp.push_str("\r").unwrap(); resp.push_str("\r").unwrap();
} }
_ => { _ => {
@ -134,7 +134,7 @@ pub async fn usb_task(
} }
#[derive(Format, PartialEq)] #[derive(Format, PartialEq)]
enum Gs232Cmd { pub enum Gs232Cmd {
Unkown, Unkown,
GetAl, GetAl,
GetEz, GetEz,
@ -167,7 +167,7 @@ fn parse_command(data: &str) -> Gs232Cmd {
if data.len() == 8 { if data.len() == 8 {
if let Ok(az) = data[1..4].parse::<u16>() { if let Ok(az) = data[1..4].parse::<u16>() {
if let Ok(el) = data[5..].parse::<u16>() { if let Ok(el) = data[5..].parse::<u16>() {
Gs232Cmd::MoveTo(AzElPair { az: az, el: el }) Gs232Cmd::MoveTo(AzElPair { az, el })
} else { } else {
Gs232Cmd::Unkown Gs232Cmd::Unkown
} }