From 90768d9bf06124ebf6c03b1784b03eb8cda1f0bf Mon Sep 17 00:00:00 2001 From: Sebastian Date: Sun, 21 Aug 2022 18:11:58 +0200 Subject: [PATCH] Added movement task and rewired the channels --- src/display.rs | 28 ++++++++++++++++++++------- src/main.rs | 27 ++++++++++++++++++++++---- src/movement.rs | 50 +++++++++++++++++++++++++++++++++++++++++++++++++ src/usb.rs | 14 +++++++------- 4 files changed, 101 insertions(+), 18 deletions(-) create mode 100644 src/movement.rs diff --git a/src/display.rs b/src/display.rs index 2c21e87..7cb0010 100644 --- a/src/display.rs +++ b/src/display.rs @@ -4,6 +4,7 @@ use embassy_stm32::time::Hertz; use embassy_time::{Duration, Timer}; use embassy_util::blocking_mutex::raw::ThreadModeRawMutex; use embassy_util::channel::mpmc::Receiver; +use embassy_util::{select, Either}; use embedded_graphics::{ mono_font::{ @@ -21,14 +22,14 @@ use heapless::String; use ssd1306::{prelude::*, I2CDisplayInterface, Ssd1306}; -use crate::AzElPair; +use crate::{AzElPair, RotorState}; #[embassy_executor::task] pub async fn display_task( i2c1: peripherals::I2C1, sda: peripherals::PB6, 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()); @@ -46,9 +47,13 @@ pub async fn display_task( .fill_color(BinaryColor::On) .build(); - loop { - let pair = cmd_receiver.recv().await; + let mut rotor_state = RotorState { + actual_pos: AzElPair { az: 0, el: 0 }, + setpoint_pos: AzElPair { az: 0, el: 0 }, + stopped: false, + }; + loop { display.clear(); Text::new("AFG rotor ctrl v0.1.0", Point::new(0, 6), text_small) @@ -69,19 +74,28 @@ pub async fn display_task( .unwrap(); 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) .draw(&mut display) .unwrap(); 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) .draw(&mut display) .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, + } } } diff --git a/src/main.rs b/src/main.rs index 0257995..c273263 100644 --- a/src/main.rs +++ b/src/main.rs @@ -21,18 +21,29 @@ mod display; use display::display_task; 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 { az: u16, el: u16, } +#[derive(PartialEq, Format, Copy, Clone)] +pub struct RotorState { + actual_pos: AzElPair, + setpoint_pos: AzElPair, + stopped: bool, +} + #[embassy_executor::main] async fn main(spawner: Spawner) { - static CMD_CHAN: Channel = Channel::new(); + static CMD_CHAN: Channel = Channel::new(); static POS_CHAN: Channel = Channel::new(); + static STATE_CHAN: Channel = Channel::new(); let mut config = Config::default(); config.rcc.hse = Some(Hertz(8_000_000)); @@ -54,7 +65,15 @@ async fn main(spawner: Spawner) { spawner .spawn(usb_task(p.USB, p.PA12, p.PA11, CMD_CHAN.sender())) .unwrap(); + 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(); } diff --git a/src/movement.rs b/src/movement.rs new file mode 100644 index 0000000..7b1f3b1 --- /dev/null +++ b/src/movement.rs @@ -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; + } +} diff --git a/src/usb.rs b/src/usb.rs index ec0942a..3538814 100644 --- a/src/usb.rs +++ b/src/usb.rs @@ -1,5 +1,6 @@ use defmt::Format; +use core::fmt::Write; use embassy_stm32::interrupt; use embassy_stm32::peripherals; use embassy_stm32::usb::Driver; @@ -9,8 +10,6 @@ use embassy_util::blocking_mutex::raw::ThreadModeRawMutex; use embassy_util::channel::mpmc::Sender; use futures::future::join; - -use core::fmt::Write; use heapless::String; use crate::AzElPair; @@ -20,7 +19,7 @@ pub async fn usb_task( usb: peripherals::USB, dp_pin: peripherals::PA12, 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 driver = Driver::new(usb, irq, dp_pin, dm_pin); @@ -103,11 +102,12 @@ pub async fn usb_task( Gs232Cmd::GetAlEz => { write!(&mut resp, "AZ={} EL={}\r", az_actual, el_actual).unwrap(); } - Gs232Cmd::MoveTo(pair) => { - cmd_sender.send(pair).await; + Gs232Cmd::MoveTo(_) => { + cmd_sender.send(cmd).await; resp.push_str("\r").unwrap(); } Gs232Cmd::Stop => { + cmd_sender.send(cmd).await; resp.push_str("\r").unwrap(); } _ => { @@ -134,7 +134,7 @@ pub async fn usb_task( } #[derive(Format, PartialEq)] -enum Gs232Cmd { +pub enum Gs232Cmd { Unkown, GetAl, GetEz, @@ -167,7 +167,7 @@ fn parse_command(data: &str) -> Gs232Cmd { if data.len() == 8 { if let Ok(az) = data[1..4].parse::() { if let Ok(el) = data[5..].parse::() { - Gs232Cmd::MoveTo(AzElPair { az: az, el: el }) + Gs232Cmd::MoveTo(AzElPair { az, el }) } else { Gs232Cmd::Unkown }