From 045eada9d3cf01a4b02f085a76fb5b5b7d0963e5 Mon Sep 17 00:00:00 2001 From: Sebastian Date: Sun, 28 Aug 2022 22:03:17 +0200 Subject: [PATCH] Added comments to movement.rs --- src/movement.rs | 45 ++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 40 insertions(+), 5 deletions(-) diff --git a/src/movement.rs b/src/movement.rs index bdb5e31..0a33d4d 100644 --- a/src/movement.rs +++ b/src/movement.rs @@ -1,7 +1,7 @@ use embassy_stm32::adc::Adc; use embassy_stm32::gpio::{Level, Output, Speed}; use embassy_stm32::peripherals; -use embassy_time::{Delay, Duration, Timer, with_timeout}; +use embassy_time::{with_timeout, Delay, Duration, Timer}; use embassy_util::blocking_mutex::raw::ThreadModeRawMutex; use embassy_util::channel::mpmc::{Receiver, Sender}; use embassy_util::{select, Either}; @@ -11,12 +11,18 @@ use heapless::Vec; use crate::usb::Gs232Cmd; use crate::{AzElPair, RotorState}; +// ADC reading for azimuth 0° const AZ_MIN_READING: f32 = 0.0; +// ADC reading for azimuth 360° const AZ_MAX_READING: f32 = 4096.0; +// Range of motion for azimuth (0 to AZ_RANGE) const AZ_RANGE: f32 = 360.0; +// ADC reading for elevation 0° const EL_MIN_READING: f32 = 0.0; +// ADC reading for elevation 360° const EL_MAX_READING: f32 = 4096.0; +// Range of motion for elevantion (0 to EL_RANGE) const EL_RANGE: f32 = 90.0; #[embassy_executor::task] @@ -32,48 +38,62 @@ pub async fn movement_task( pos_sender: Sender<'static, ThreadModeRawMutex, AzElPair, 1>, state_sender: Sender<'static, ThreadModeRawMutex, RotorState, 1>, ) { + // Initialize the rotor state let mut rotor_state = RotorState { actual_pos: AzElPair { az: 0, el: 0 }, setpoint_pos: AzElPair { az: 0, el: 0 }, stopped: true, }; - let mut adc = Adc::new(adc1, &mut Delay); - + // Setup output pins for moving the rotor 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); + // Setup the ADC for reading the rotor positions + let mut adc = Adc::new(adc1, &mut Delay); + + // Do an initial ADC reading to initialize the averages 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 { + // Wait until either a new command has been received or 100ms have elapsed match select( cmd_receiver.recv(), Timer::after(Duration::from_millis(100)), ) .await { + // A new command has been received. This task only cares about MoveTo and Stop. Either::First(cmd) => match cmd { + // Move to command. Update the setpoint pair in the rotor state Gs232Cmd::MoveTo(pair) => { rotor_state.setpoint_pos = pair; rotor_state.stopped = false; } + // Stop command. Set the stopped flag. Gs232Cmd::Stop => { rotor_state.stopped = true; } + // Everthing elese is an noop. _ => {} }, + + // Second case of the select statement. Timer has elapsed. Either::Second(_) => { + // First read the current rotor position let az_reading = adc.read(&mut az_pin) as f32; let el_reading = adc.read(&mut el_pin) as f32; + // Apply the averaging filters az_average.add(az_reading); el_average.add(el_reading); + // Calculate the position in degreee let az_actual = (az_average.average() - AZ_MIN_READING) / (AZ_MAX_READING - AZ_MIN_READING) * AZ_RANGE; @@ -81,38 +101,49 @@ pub async fn movement_task( / (EL_MAX_READING - EL_MIN_READING) * EL_RANGE; + // Update the rotor state 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 { + // Azimuth needs to move clockwise cw_pin.set_high(); ccw_pin.set_low(); } else if !rotor_state.stopped && rotor_state.actual_pos.az > rotor_state.setpoint_pos.az { + // Azimuth needs to move counter clockwise cw_pin.set_low(); ccw_pin.set_high(); } else { + // Either azimuth is on the setpoint or the rotor has beend stopped. cw_pin.set_low(); ccw_pin.set_low(); } if !rotor_state.stopped && rotor_state.actual_pos.el < rotor_state.setpoint_pos.el { + // Elevation needs to move up up_pin.set_high(); down_pin.set_low(); } else if !rotor_state.stopped && rotor_state.actual_pos.el > rotor_state.setpoint_pos.el { + // Elevation needs to move down up_pin.set_low(); down_pin.set_high(); } else { + // Either elevation is on the setpoint or the rotor has beend stopped. up_pin.set_low(); down_pin.set_low(); } - // Send with timeouts to prevent blocking if display or usb task are blocked. + // Send the state to the display task and the position usb. + // Use timeouts to prevent blocking if display or usb task are unresponsive. join( - with_timeout(Duration::from_millis(100), pos_sender.send(rotor_state.actual_pos)), + with_timeout( + Duration::from_millis(100), + pos_sender.send(rotor_state.actual_pos), + ), with_timeout(Duration::from_millis(100), state_sender.send(rotor_state)), ) .await; @@ -121,23 +152,27 @@ pub async fn movement_task( } } +// Simple sliding average filter struct Average { pos: usize, data: Vec, } impl Average { + // Create a new filter and prefill the state using an initial value fn new(initial: f32) -> Average { let mut data: Vec = Vec::new(); data.resize(10, initial).unwrap(); Average { pos: 0, data } } + // Adds a new value to internal state fn add(&mut self, sample: f32) { self.data[self.pos] = sample; self.pos = (self.pos + 1) % self.data.len(); } + // Calculate the average value from the internal state fn average(&self) -> f32 { let mut sum = 0.0; for sample in &self.data {