diff --git a/src/display.rs b/src/display.rs index 7cb0010..df831fb 100644 --- a/src/display.rs +++ b/src/display.rs @@ -60,11 +60,15 @@ pub async fn display_task( .draw(&mut display) .unwrap(); - Text::new("AZ: 23", Point::new(1, 17), text_large) + let mut tmp: String<16> = String::new(); + write!(tmp, "AZ: {}", rotor_state.actual_pos.az).unwrap(); + Text::new(&tmp, Point::new(1, 17), text_large) .draw(&mut display) .unwrap(); - Text::new("EL: 42", Point::new(64, 17), text_large) + tmp.clear(); + write!(tmp, "EL: {}", rotor_state.actual_pos.el).unwrap(); + Text::new(&tmp, Point::new(64, 17), text_large) .draw(&mut display) .unwrap(); @@ -73,7 +77,7 @@ pub async fn display_task( .draw(&mut display) .unwrap(); - let mut tmp: String<16> = String::new(); + tmp.clear(); write!(tmp, "AZ: {}", rotor_state.setpoint_pos.az).unwrap(); Text::new(&tmp, Point::new(1, 30), text_large_inv) .draw(&mut display) @@ -88,7 +92,7 @@ pub async fn display_task( display.flush().unwrap(); let result = select( - Timer::after(Duration::from_millis(100)), + Timer::after(Duration::from_millis(500)), cmd_receiver.recv(), ) .await; diff --git a/src/main.rs b/src/main.rs index c273263..21278b1 100644 --- a/src/main.rs +++ b/src/main.rs @@ -63,11 +63,21 @@ async fn main(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(), + POS_CHAN.receiver(), + )) .unwrap(); spawner .spawn(movement_task( + p.PA3, + p.PA4, + p.PA5, + p.PA6, CMD_CHAN.receiver(), POS_CHAN.sender(), STATE_CHAN.sender(), diff --git a/src/movement.rs b/src/movement.rs index 7b1f3b1..211164d 100644 --- a/src/movement.rs +++ b/src/movement.rs @@ -1,22 +1,21 @@ -use defmt::Format; - -use core::fmt::Write; -use embassy_stm32::interrupt; +use embassy_stm32::gpio::{Level, Output, Speed}; use embassy_stm32::peripherals; -use embassy_stm32::usb::Driver; -use embassy_usb::Builder; -use embassy_usb_serial::{CdcAcmClass, State}; +use embassy_time::{Duration, Instant, Timer}; use embassy_util::blocking_mutex::raw::ThreadModeRawMutex; use embassy_util::channel::mpmc::{Receiver, Sender}; +use embassy_util::{select, Either}; use futures::future::join; -use heapless::String; use crate::usb::Gs232Cmd; use crate::{AzElPair, RotorState}; #[embassy_executor::task] pub async fn movement_task( + cw_pin: peripherals::PA3, + ccw_pin: peripherals::PA4, + up_pin: peripherals::PA5, + down_pin: peripherals::PA6, cmd_receiver: Receiver<'static, ThreadModeRawMutex, Gs232Cmd, 1>, pos_sender: Sender<'static, ThreadModeRawMutex, AzElPair, 1>, state_sender: Sender<'static, ThreadModeRawMutex, RotorState, 1>, @@ -27,24 +26,66 @@ pub async fn movement_task( stopped: false, }; + let mut cw_pin = Output::new(cw_pin, Level::Low, Speed::Low); + let mut ccw_pin = Output::new(ccw_pin, Level::Low, Speed::Low); + let mut up_pin = Output::new(up_pin, Level::Low, Speed::Low); + let mut down_pin = Output::new(down_pin, Level::Low, Speed::Low); + 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), + match select( + cmd_receiver.recv(), + Timer::after(Duration::from_millis(100)), ) - .await; + .await + { + Either::First(cmd) => match cmd { + Gs232Cmd::MoveTo(pair) => { + rotor_state.setpoint_pos = pair; + rotor_state.stopped = false; + } + Gs232Cmd::Stop => { + rotor_state.stopped = true; + } + _ => {} + }, + Either::Second(_) => { + if !rotor_state.stopped && rotor_state.actual_pos.az < rotor_state.setpoint_pos.az { + rotor_state.actual_pos.az += 1; + cw_pin.set_high(); + ccw_pin.set_low(); + } else if !rotor_state.stopped + && rotor_state.actual_pos.az > rotor_state.setpoint_pos.az + { + rotor_state.actual_pos.az -= 1; + cw_pin.set_low(); + ccw_pin.set_high(); + } else { + cw_pin.set_low(); + ccw_pin.set_low(); + } + + if !rotor_state.stopped && rotor_state.actual_pos.el < rotor_state.setpoint_pos.el { + rotor_state.actual_pos.el += 1; + up_pin.set_high(); + down_pin.set_low(); + } else if !rotor_state.stopped + && rotor_state.actual_pos.el > rotor_state.setpoint_pos.el + { + rotor_state.actual_pos.el -= 1; + up_pin.set_low(); + down_pin.set_high(); + } else { + up_pin.set_low(); + down_pin.set_low(); + } + + join( + pos_sender.send(rotor_state.actual_pos), + state_sender.send(rotor_state), + ) + .await; + //state_sender.send(rotor_state).await; + } + }; } } diff --git a/src/usb.rs b/src/usb.rs index 3538814..5d25041 100644 --- a/src/usb.rs +++ b/src/usb.rs @@ -7,7 +7,8 @@ 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::Sender; +use embassy_util::channel::mpmc::{Receiver, Sender}; +use embassy_util::{select, Either}; use futures::future::join; use heapless::String; @@ -20,6 +21,7 @@ pub async fn usb_task( dp_pin: peripherals::PA12, dm_pin: peripherals::PA11, cmd_sender: Sender<'static, ThreadModeRawMutex, Gs232Cmd, 1>, + pos_receiver: Receiver<'static, ThreadModeRawMutex, AzElPair, 1>, ) { let irq = interrupt::take!(USB_LP_CAN1_RX0); let driver = Driver::new(usb, irq, dp_pin, dm_pin); @@ -53,11 +55,10 @@ pub async fn usb_task( // Build the builder. let mut usb = builder.build(); - let az_actual: u16 = 0; - let el_actual: u16 = 0; - // Do stuff with the class! let usb_handler_fut = async { + let mut current_pos = AzElPair { az: 0, el: 0 }; + loop { class.wait_connection().await; defmt::info!("USB connected"); @@ -65,11 +66,17 @@ pub async fn usb_task( let mut buffer: String<64> = String::new(); loop { - let n = match class.read_packet(&mut packet).await { - Ok(n) => n, - Err(err) => { - defmt::error!("Unable to read packet: {}", err); - break; + let n = match select(class.read_packet(&mut packet), pos_receiver.recv()).await { + Either::First(res) => match res { + Ok(n) => n, + Err(err) => { + defmt::error!("Unable to read packet: {}", err); + break; + } + }, + Either::Second(pair) => { + current_pos = pair; + continue; } }; @@ -94,13 +101,14 @@ pub async fn usb_task( let mut resp: String<16> = String::new(); match cmd { Gs232Cmd::GetAl => { - write!(&mut resp, "AZ={}\r", az_actual).unwrap(); + write!(&mut resp, "AZ={}\r", current_pos.az).unwrap(); } Gs232Cmd::GetEz => { - write!(&mut resp, "EL={}\r", el_actual).unwrap(); + write!(&mut resp, "EL={}\r", current_pos.el).unwrap(); } Gs232Cmd::GetAlEz => { - write!(&mut resp, "AZ={} EL={}\r", az_actual, el_actual).unwrap(); + write!(&mut resp, "AZ={} EL={}\r", current_pos.az, current_pos.el) + .unwrap(); } Gs232Cmd::MoveTo(_) => { cmd_sender.send(cmd).await;