diff --git a/daemon/src/rotor.rs b/daemon/src/rotor.rs index f5eb29b..30a106f 100644 --- a/daemon/src/rotor.rs +++ b/daemon/src/rotor.rs @@ -38,7 +38,6 @@ impl Encoder for ProtocolCodec { fn encode(&mut self, item: HostMessage, dst: &mut BytesMut) -> Result<(), Self::Error> { let msg_bytes = to_stdvec_cobs(&item).unwrap(); dst.put(msg_bytes.as_slice()); - dst.put_u8(0); Ok(()) } } diff --git a/firmware/src/axis.rs b/firmware/src/axis.rs new file mode 100644 index 0000000..050c133 --- /dev/null +++ b/firmware/src/axis.rs @@ -0,0 +1,77 @@ +use as5048a::AS5048A; +use embedded_hal::{digital::OutputPin, spi::SpiBus}; +use rtic_monotonics::Monotonic; +use stm32f4xx_hal::{ + gpio::{AnyPin, Output, PushPull}, + prelude::*, +}; + +use crate::app::Mono; + +pub async fn move_axis( + enable: &mut AnyPin>, + dir: &mut AnyPin>, + step: &mut AnyPin>, + target: &mut TargetMux, + angle: &mut AngleMux, +) where + TargetMux: rtic::Mutex, + AngleMux: rtic::Mutex, +{ + loop { + let tmp_target = target.lock(|target| *target); + let tmp_angle = angle.lock(|angle| *angle); + let diff = tmp_angle - tmp_target; + + defmt::info!( + "angle diff/target/actual: {:?}/{:?}/{:?}", + diff, + tmp_target, + tmp_angle + ); + + let delay = if diff.abs() < 10 { + 10.millis() + } else if diff < 100 { + 5.millis() + } else { + 1.millis() + }; + + if diff.abs() > 50 { + enable.set_low(); + if diff < 0 { + dir.set_high(); + } else { + dir.set_low(); + } + + step.set_low(); + Mono::delay(delay / 2).await; + step.set_high(); + Mono::delay(delay / 2).await; + } else { + enable.set_high(); + Mono::delay(delay).await; + } + } +} + +pub async fn update_encoder_angle( + encoder: &mut AS5048A, + spi: &mut SPI, + angle: &mut AngleMux, +) where + SPI: SpiBus, + CS: OutputPin, + AngleMux: rtic::Mutex, +{ + let raw_angle = encoder.angle(spi).unwrap(); + let angle_deg = raw_angle as i32 * 3600 / 16384; + + angle.lock(|az_angle| { + *az_angle = angle_deg; + }); + + defmt::info!("angle: {:?}", angle_deg); +} diff --git a/firmware/src/main.rs b/firmware/src/main.rs index 71f9a63..6c70309 100644 --- a/firmware/src/main.rs +++ b/firmware/src/main.rs @@ -6,6 +6,7 @@ use defmt_brtt as _; // global logger use panic_probe as _; use stm32f4xx_hal as _; +mod axis; mod bootloader; // same panicking *behavior* as `panic-probe` but doesn't print a panic message @@ -22,13 +23,15 @@ fn panic() -> ! { mod app { use as5048a::AS5048A; + use embedded_hal::digital::OutputPin; + use rtic::Mutex; - use core::fmt::Write; + use core::{convert::Infallible, fmt::Write}; use heapless::{String, Vec}; use num_traits::{Float, FloatConst}; use postcard::{from_bytes_cobs, to_vec_cobs}; use stm32f4xx_hal::{ - gpio::{gpioa, gpiob, gpioc, Output, PushPull}, + gpio::{gpioa, gpiob, gpioc, AnyPin, Output, PushPull}, i2c, otg_fs::{UsbBus, UsbBusType, USB}, pac::{I2C1, SPI1}, @@ -46,7 +49,10 @@ mod app { use radomctl_protocol::{HostMessage, *}; - use crate::bootloader; + use crate::{ + axis::{move_axis, update_encoder_angle}, + bootloader, + }; use rtic_monotonics::systick::prelude::*; systick_monotonic!(Mono, 4000); @@ -76,13 +82,13 @@ mod app { spi_cs3: gpiob::PB15>, spi1: spi::Spi, - az_enable: gpiob::PB8>, - az_dir: gpioa::PA15>, - az_step: gpiob::PB3>, + az_enable: AnyPin>, + az_dir: AnyPin>, + az_step: AnyPin>, - el_enable: gpiob::PB4>, - el_dir: gpioa::PA8>, - el_step: gpioa::PA9>, + el_enable: AnyPin>, + el_dir: AnyPin>, + el_step: AnyPin>, usb_dev: UsbDevice<'static, UsbBusType>, usb_serial: SerialPort<'static, UsbBusType>, @@ -206,15 +212,15 @@ mod app { defmt::info!("SPI Setup done"); - let mut az_enable = gpiob.pb8.into_push_pull_output(); + let mut az_enable = gpiob.pb8.into_push_pull_output().erase(); az_enable.set_high(); - let az_dir = gpioa.pa15.into_push_pull_output(); - let az_step = gpiob.pb3.into_push_pull_output(); + let az_dir = gpioa.pa15.into_push_pull_output().erase(); + let az_step = gpiob.pb3.into_push_pull_output().erase(); - let mut el_enable = gpiob.pb4.into_push_pull_output(); + let mut el_enable = gpiob.pb4.into_push_pull_output().erase(); el_enable.set_high(); - let el_dir = gpioa.pa8.into_push_pull_output(); - let el_step = gpioa.pa9.into_push_pull_output(); + let el_dir = gpioa.pa8.into_push_pull_output().erase(); + let el_step = gpioa.pa9.into_push_pull_output().erase(); defmt::info!("Motor Setup done"); @@ -340,6 +346,8 @@ mod app { let encoder_az = cx.local.encoder_az; let encoder_el = cx.local.encoder_el; + update_encoder_angle(encoder_az, spi1, &mut cx.shared.el_angle).await; + loop { /* let (diag, gain) = encoder_az.diag_gain(spi1).unwrap(); @@ -370,92 +378,26 @@ mod app { #[task(local = [az_enable, az_dir, az_step], shared = [az_angle, az_target])] async fn move_az(mut cx: move_az::Context) { - let az_enable = cx.local.az_enable; - let az_dir = cx.local.az_dir; - let az_step = cx.local.az_step; - - loop { - let az_target = cx.shared.az_target.lock(|az_target| *az_target); - let az_angle = cx.shared.az_angle.lock(|az_angle| *az_angle); - let diff = az_angle - az_target; - - defmt::info!( - "angle diff/target/actual: {:?}/{:?}/{:?}", - diff, - az_target, - az_angle - ); - - let delay = if diff.abs() < 10 { - 10.millis() - } else if diff < 100 { - 5.millis() - } else { - 1.millis() - }; - - if diff.abs() > 50 { - az_enable.set_low(); - if diff < 0 { - az_dir.set_high(); - } else { - az_dir.set_low(); - } - - az_step.set_low(); - Mono::delay(delay / 2).await; - az_step.set_high(); - Mono::delay(delay / 2).await; - } else { - az_enable.set_high(); - Mono::delay(delay).await; - } - } + move_axis( + cx.local.az_enable, + cx.local.az_dir, + cx.local.az_step, + &mut cx.shared.az_angle, + &mut cx.shared.az_target, + ) + .await; } #[task(local = [el_enable, el_dir, el_step], shared = [el_angle, el_target])] async fn move_el(mut cx: move_el::Context) { - let el_enable = cx.local.el_enable; - let el_dir = cx.local.el_dir; - let el_step = cx.local.el_step; - - loop { - let el_target = cx.shared.el_target.lock(|el_target| *el_target); - let el_angle = cx.shared.el_angle.lock(|el_angle| *el_angle); - let diff = el_angle - el_target; - - defmt::info!( - "angle diff/target/actual: {:?}/{:?}/{:?}", - diff, - el_target, - el_angle - ); - - let delay = if diff.abs() < 10 { - 10.millis() - } else if diff < 100 { - 5.millis() - } else { - 1.millis() - }; - - if diff.abs() > 50 { - el_enable.set_low(); - if diff < 0 { - el_dir.set_high(); - } else { - el_dir.set_low(); - } - - el_step.set_low(); - Mono::delay(delay / 2).await; - el_step.set_high(); - Mono::delay(delay / 2).await; - } else { - el_enable.set_high(); - Mono::delay(delay).await; - } - } + move_axis( + cx.local.el_enable, + cx.local.el_dir, + cx.local.el_step, + &mut cx.shared.el_angle, + &mut cx.shared.el_target, + ) + .await; } #[task(binds=OTG_FS, local=[usb_dev, usb_serial, usb_buffer], shared=[az_target, el_target, az_angle, el_angle])] @@ -514,7 +456,7 @@ mod app { bootloader::reboot_to_bootloader(); } }, - Err(err) => defmt::error!("Unable to parse host message"), + Err(err) => defmt::error!("Unable to parse host message: {}", err), }; *buffer = Vec::::from_slice(rest).unwrap();