diff --git a/daemon/src/rotor.rs b/daemon/src/rotor.rs index 30a106f..f5eb29b 100644 --- a/daemon/src/rotor.rs +++ b/daemon/src/rotor.rs @@ -38,6 +38,7 @@ 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 deleted file mode 100644 index 050c133..0000000 --- a/firmware/src/axis.rs +++ /dev/null @@ -1,77 +0,0 @@ -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 6c70309..71f9a63 100644 --- a/firmware/src/main.rs +++ b/firmware/src/main.rs @@ -6,7 +6,6 @@ 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 @@ -23,15 +22,13 @@ fn panic() -> ! { mod app { use as5048a::AS5048A; - use embedded_hal::digital::OutputPin; - use rtic::Mutex; - use core::{convert::Infallible, fmt::Write}; + use core::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, AnyPin, Output, PushPull}, + gpio::{gpioa, gpiob, gpioc, Output, PushPull}, i2c, otg_fs::{UsbBus, UsbBusType, USB}, pac::{I2C1, SPI1}, @@ -49,10 +46,7 @@ mod app { use radomctl_protocol::{HostMessage, *}; - use crate::{ - axis::{move_axis, update_encoder_angle}, - bootloader, - }; + use crate::bootloader; use rtic_monotonics::systick::prelude::*; systick_monotonic!(Mono, 4000); @@ -82,13 +76,13 @@ mod app { spi_cs3: gpiob::PB15>, spi1: spi::Spi, - az_enable: AnyPin>, - az_dir: AnyPin>, - az_step: AnyPin>, + az_enable: gpiob::PB8>, + az_dir: gpioa::PA15>, + az_step: gpiob::PB3>, - el_enable: AnyPin>, - el_dir: AnyPin>, - el_step: AnyPin>, + el_enable: gpiob::PB4>, + el_dir: gpioa::PA8>, + el_step: gpioa::PA9>, usb_dev: UsbDevice<'static, UsbBusType>, usb_serial: SerialPort<'static, UsbBusType>, @@ -212,15 +206,15 @@ mod app { defmt::info!("SPI Setup done"); - let mut az_enable = gpiob.pb8.into_push_pull_output().erase(); + let mut az_enable = gpiob.pb8.into_push_pull_output(); az_enable.set_high(); - let az_dir = gpioa.pa15.into_push_pull_output().erase(); - let az_step = gpiob.pb3.into_push_pull_output().erase(); + let az_dir = gpioa.pa15.into_push_pull_output(); + let az_step = gpiob.pb3.into_push_pull_output(); - let mut el_enable = gpiob.pb4.into_push_pull_output().erase(); + let mut el_enable = gpiob.pb4.into_push_pull_output(); el_enable.set_high(); - let el_dir = gpioa.pa8.into_push_pull_output().erase(); - let el_step = gpioa.pa9.into_push_pull_output().erase(); + let el_dir = gpioa.pa8.into_push_pull_output(); + let el_step = gpioa.pa9.into_push_pull_output(); defmt::info!("Motor Setup done"); @@ -346,8 +340,6 @@ 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(); @@ -378,26 +370,92 @@ mod app { #[task(local = [az_enable, az_dir, az_step], shared = [az_angle, az_target])] async fn move_az(mut cx: move_az::Context) { - 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; + 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; + } + } } #[task(local = [el_enable, el_dir, el_step], shared = [el_angle, el_target])] async fn move_el(mut cx: move_el::Context) { - 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; + 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; + } + } } #[task(binds=OTG_FS, local=[usb_dev, usb_serial, usb_buffer], shared=[az_target, el_target, az_angle, el_angle])] @@ -456,7 +514,7 @@ mod app { bootloader::reboot_to_bootloader(); } }, - Err(err) => defmt::error!("Unable to parse host message: {}", err), + Err(err) => defmt::error!("Unable to parse host message"), }; *buffer = Vec::::from_slice(rest).unwrap();