diff --git a/Cargo.lock b/Cargo.lock index 51c6363..0a0d1cb 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -1029,7 +1029,7 @@ dependencies = [ "embassy-util", "embedded-graphics", "embedded-hal 0.2.7", - "futures", + "futures-util", "heapless", "nb 1.0.0", "panic-probe", diff --git a/Cargo.toml b/Cargo.toml index 303f31f..25eca46 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -20,7 +20,9 @@ panic-probe = { version = "0.3.0"} cortex-m = { version = "0.7.6", features = ["critical-section-single-core"] } cortex-m-rt = "0.7.0" embedded-hal = "0.2.6" -futures = { version = "0.3.17", default-features = false, features = ["async-await"] } +futures-util = { version = "0.3.23", default-features = false, features = ["async-await"] } + + heapless = { version = "0.7.5", default-features = false, features = ['ufmt-impl']} nb = "1.0.0" ufmt = "0.2.0" diff --git a/src/display.rs b/src/display.rs index 5869265..54106e7 100644 --- a/src/display.rs +++ b/src/display.rs @@ -50,7 +50,7 @@ pub async fn display_task( let mut rotor_state = RotorState { actual_pos: AzElPair { az: 0, el: 0 }, setpoint_pos: AzElPair { az: 0, el: 0 }, - stopped: false, + stopped: true, }; loop { @@ -72,20 +72,28 @@ pub async fn display_task( .draw(&mut display) .unwrap(); - Rectangle::new(Point::new(0, 19), Size::new(128, 23)) - .into_styled(style_filled) - .draw(&mut display) - .unwrap(); + if !rotor_state.stopped { + Rectangle::new(Point::new(0, 19), Size::new(128, 23)) + .into_styled(style_filled) + .draw(&mut display) + .unwrap(); + }; + + let setpoint_style = if !rotor_state.stopped { + text_large_inv + } else { + text_large + }; tmp.clear(); uwrite!(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), setpoint_style) .draw(&mut display) .unwrap(); tmp.clear(); uwrite!(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), setpoint_style) .draw(&mut display) .unwrap(); diff --git a/src/main.rs b/src/main.rs index 21278b1..672b49b 100644 --- a/src/main.rs +++ b/src/main.rs @@ -2,9 +2,7 @@ #![no_main] #![feature(type_alias_impl_trait)] -use core::fmt::Write; - -use defmt::{panic, Format}; +use defmt::Format; use defmt_rtt as _; use panic_probe as _; @@ -15,7 +13,6 @@ use embassy_stm32::Config; use embassy_time::{Duration, Timer}; use embassy_util::blocking_mutex::raw::ThreadModeRawMutex; use embassy_util::channel::mpmc::Channel; -use embassy_util::Forever; mod display; use display::display_task; @@ -74,10 +71,13 @@ async fn main(spawner: Spawner) { spawner .spawn(movement_task( + p.ADC1, + p.PA0, + p.PA1, + p.PA2, 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 211164d..c27de03 100644 --- a/src/movement.rs +++ b/src/movement.rs @@ -1,21 +1,33 @@ +use embassy_stm32::adc::Adc; use embassy_stm32::gpio::{Level, Output, Speed}; use embassy_stm32::peripherals; -use embassy_time::{Duration, Instant, Timer}; +use embassy_time::{Delay, Duration, 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 futures_util::future::join; +use heapless::Vec; use crate::usb::Gs232Cmd; use crate::{AzElPair, RotorState}; +const AZ_MIN_READING: f32 = 0.0; +const AZ_MAX_READING: f32 = 4096.0; +const AZ_RANGE: f32 = 360.0; + +const EL_MIN_READING: f32 = 0.0; +const EL_MAX_READING: f32 = 4096.0; +const EL_RANGE: f32 = 90.0; + #[embassy_executor::task] pub async fn movement_task( - cw_pin: peripherals::PA3, - ccw_pin: peripherals::PA4, - up_pin: peripherals::PA5, - down_pin: peripherals::PA6, + adc1: peripherals::ADC1, + mut az_pin: peripherals::PA0, + mut el_pin: peripherals::PA1, + cw_pin: peripherals::PA2, + ccw_pin: peripherals::PA3, + up_pin: peripherals::PA4, + down_pin: peripherals::PA5, cmd_receiver: Receiver<'static, ThreadModeRawMutex, Gs232Cmd, 1>, pos_sender: Sender<'static, ThreadModeRawMutex, AzElPair, 1>, state_sender: Sender<'static, ThreadModeRawMutex, RotorState, 1>, @@ -23,14 +35,21 @@ pub async fn movement_task( let mut rotor_state = RotorState { actual_pos: AzElPair { az: 0, el: 0 }, setpoint_pos: AzElPair { az: 0, el: 0 }, - stopped: false, + stopped: true, }; + let mut adc = Adc::new(adc1, &mut Delay); + 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); + let az_reading = adc.read(&mut az_pin) as f32; + let el_reading = adc.read(&mut el_pin) as f32; + let mut az_average = Average::new(az_reading); + let mut el_average = Average::new(el_reading); + loop { match select( cmd_receiver.recv(), @@ -49,14 +68,28 @@ pub async fn movement_task( _ => {} }, Either::Second(_) => { + let az_reading = adc.read(&mut az_pin) as f32; + let el_reading = adc.read(&mut el_pin) as f32; + + az_average.add(az_reading); + el_average.add(el_reading); + + let az_actual = (az_average.average() - AZ_MIN_READING) + / (AZ_MAX_READING - AZ_MIN_READING) + * AZ_RANGE; + let el_actual = (el_average.average() - EL_MIN_READING) + / (EL_MAX_READING - EL_MIN_READING) + * EL_RANGE; + + rotor_state.actual_pos.az = az_actual as u16; + rotor_state.actual_pos.el = el_actual as u16; + 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 { @@ -65,13 +98,11 @@ pub async fn movement_task( } 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 { @@ -84,8 +115,34 @@ pub async fn movement_task( state_sender.send(rotor_state), ) .await; - //state_sender.send(rotor_state).await; } }; } } + +struct Average { + pos: usize, + data: Vec, +} + +impl Average { + fn new(initial: f32) -> Average { + let mut data: Vec = Vec::new(); + data.resize(10, initial).unwrap(); + Average { pos: 0, data } + } + + fn add(&mut self, sample: f32) { + self.data[self.pos] = sample; + self.pos = (self.pos + 1) % self.data.len(); + } + + fn average(&self) -> f32 { + let mut sum = 0.0; + for sample in &self.data { + sum += sample; + } + + sum / self.data.len() as f32 + } +} diff --git a/src/usb.rs b/src/usb.rs index 5bfd5d5..c6b3c3a 100644 --- a/src/usb.rs +++ b/src/usb.rs @@ -9,7 +9,7 @@ use embassy_util::blocking_mutex::raw::ThreadModeRawMutex; use embassy_util::channel::mpmc::{Receiver, Sender}; use embassy_util::{select, Either}; -use futures::future::join; +use futures_util::future::join; use heapless::String; use ufmt::uwrite;