AFG/radomctld
3
0
Fork 0
Code Issues 1 Pull requests Projects Releases Packages Wiki Activity

Deduplicated axis movement code

Browse source
This commit is contained in:
Sebastian 2026-01-02 23:42:15 +01:00
parent 8e371bc6a2
commit 7b9d1e0c66
1 changed files with 57 additions and 72 deletions
Download patch file Download diff file Expand all files Collapse all files

129
firmware/src/main.rs
View file

@ -22,13 +22,15 @@ fn panic() -> ! {
mod app { mod app {
use as5048a::AS5048A; 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 heapless::{String, Vec};
use num_traits::{Float, FloatConst}; use num_traits::{Float, FloatConst};
use postcard::{from_bytes_cobs, to_vec_cobs}; use postcard::{from_bytes_cobs, to_vec_cobs};
use stm32f4xx_hal::{ use stm32f4xx_hal::{
gpio::{gpioa, gpiob, gpioc, Output, PushPull}, gpio::{gpioa, gpiob, gpioc, AnyPin, Output, PushPull},
i2c, i2c,
otg_fs::{UsbBus, UsbBusType, USB}, otg_fs::{UsbBus, UsbBusType, USB},
pac::{I2C1, SPI1}, pac::{I2C1, SPI1},
@ -76,13 +78,13 @@ mod app {
spi_cs3: gpiob::PB15<Output<PushPull>>, spi_cs3: gpiob::PB15<Output<PushPull>>,
spi1: spi::Spi<SPI1>, spi1: spi::Spi<SPI1>,
az_enable: gpiob::PB8<Output<PushPull>>, az_enable: AnyPin<Output<PushPull>>,
az_dir: gpioa::PA15<Output<PushPull>>, az_dir: AnyPin<Output<PushPull>>,
az_step: gpiob::PB3<Output<PushPull>>, az_step: AnyPin<Output<PushPull>>,
el_enable: gpiob::PB4<Output<PushPull>>, el_enable: AnyPin<Output<PushPull>>,
el_dir: gpioa::PA8<Output<PushPull>>, el_dir: AnyPin<Output<PushPull>>,
el_step: gpioa::PA9<Output<PushPull>>, el_step: AnyPin<Output<PushPull>>,
usb_dev: UsbDevice<'static, UsbBusType>, usb_dev: UsbDevice<'static, UsbBusType>,
usb_serial: SerialPort<'static, UsbBusType>, usb_serial: SerialPort<'static, UsbBusType>,
@ -206,15 +208,15 @@ mod app {
defmt::info!("SPI Setup done"); 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(); az_enable.set_high();
let az_dir = gpioa.pa15.into_push_pull_output(); let az_dir = gpioa.pa15.into_push_pull_output().erase();
let az_step = gpiob.pb3.into_push_pull_output(); 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(); el_enable.set_high();
let el_dir = gpioa.pa8.into_push_pull_output(); let el_dir = gpioa.pa8.into_push_pull_output().erase();
let el_step = gpioa.pa9.into_push_pull_output(); let el_step = gpioa.pa9.into_push_pull_output().erase();
defmt::info!("Motor Setup done"); defmt::info!("Motor Setup done");
@ -368,22 +370,26 @@ mod app {
} }
} }
#[task(local = [az_enable, az_dir, az_step], shared = [az_angle, az_target])] async fn move_axis<TargetMux, AngleMux>(
async fn move_az(mut cx: move_az::Context) { enable: &mut AnyPin<Output<PushPull>>,
let az_enable = cx.local.az_enable; dir: &mut AnyPin<Output<PushPull>>,
let az_dir = cx.local.az_dir; step: &mut AnyPin<Output<PushPull>>,
let az_step = cx.local.az_step; target: &mut TargetMux,
angle: &mut AngleMux,
) where
TargetMux: rtic::Mutex<T = i32>,
AngleMux: rtic::Mutex<T = i32>,
{
loop { loop {
let az_target = cx.shared.az_target.lock(|az_target| *az_target); let tmp_target = target.lock(|target| *target);
let az_angle = cx.shared.az_angle.lock(|az_angle| *az_angle); let tmp_angle = angle.lock(|angle| *angle);
let diff = az_angle - az_target; let diff = tmp_angle - tmp_target;
defmt::info!( defmt::info!(
"angle diff/target/actual: {:?}/{:?}/{:?}", "angle diff/target/actual: {:?}/{:?}/{:?}",
diff, diff,
az_target, tmp_target,
az_angle tmp_angle
); );
let delay = if diff.abs() < 10 { let delay = if diff.abs() < 10 {
@ -395,67 +401,46 @@ mod app {
}; };
if diff.abs() > 50 { if diff.abs() > 50 {
az_enable.set_low(); enable.set_low();
if diff < 0 { if diff < 0 {
az_dir.set_high(); dir.set_high();
} else { } else {
az_dir.set_low(); dir.set_low();
} }
az_step.set_low(); step.set_low();
Mono::delay(delay / 2).await; Mono::delay(delay / 2).await;
az_step.set_high(); step.set_high();
Mono::delay(delay / 2).await; Mono::delay(delay / 2).await;
} else { } else {
az_enable.set_high(); enable.set_high();
Mono::delay(delay).await; Mono::delay(delay).await;
} }
} }
} }
#[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;
}
#[task(local = [el_enable, el_dir, el_step], shared = [el_angle, el_target])] #[task(local = [el_enable, el_dir, el_step], shared = [el_angle, el_target])]
async fn move_el(mut cx: move_el::Context) { async fn move_el(mut cx: move_el::Context) {
let el_enable = cx.local.el_enable; move_axis(
let el_dir = cx.local.el_dir; cx.local.el_enable,
let el_step = cx.local.el_step; cx.local.el_dir,
cx.local.el_step,
loop { &mut cx.shared.el_angle,
let el_target = cx.shared.el_target.lock(|el_target| *el_target); &mut cx.shared.el_target,
let el_angle = cx.shared.el_angle.lock(|el_angle| *el_angle); )
let diff = el_angle - el_target; .await;
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])] #[task(binds=OTG_FS, local=[usb_dev, usb_serial, usb_buffer], shared=[az_target, el_target, az_angle, el_angle])]