Added AZ and EL position readouts

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",

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"

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))
+        if !rotor_state.stopped {
-            .into_styled(style_filled)
+            Rectangle::new(Point::new(0, 19), Size::new(128, 23))
-            .draw(&mut display)
+                .into_styled(style_filled)
-            .unwrap();
+                .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();
 

src/main.rs

@@ -2,9 +2,7 @@
 #![no_main]
 #![feature(type_alias_impl_trait)]
 
-use core::fmt::Write;
+use defmt::Format;
-
-use defmt::{panic, 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(),

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_util::future::join;
-use futures::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,
+    adc1: peripherals::ADC1,
-    ccw_pin: peripherals::PA4,
+    mut az_pin: peripherals::PA0,
-    up_pin: peripherals::PA5,
+    mut el_pin: peripherals::PA1,
-    down_pin: peripherals::PA6,
+    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<f32, 10>,
+}
+
+impl Average {
+    fn new(initial: f32) -> Average {
+        let mut data: Vec<f32, 10> = 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
+    }
+}

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;