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 {
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},
@ -76,13 +78,13 @@ mod app {
spi_cs3: gpiob::PB15<Output<PushPull>>,
spi1: spi::Spi<SPI1>,
az_enable: gpiob::PB8<Output<PushPull>>,
az_dir: gpioa::PA15<Output<PushPull>>,
az_step: gpiob::PB3<Output<PushPull>>,
az_enable: AnyPin<Output<PushPull>>,
az_dir: AnyPin<Output<PushPull>>,
az_step: AnyPin<Output<PushPull>>,
el_enable: gpiob::PB4<Output<PushPull>>,
el_dir: gpioa::PA8<Output<PushPull>>,
el_step: gpioa::PA9<Output<PushPull>>,
el_enable: AnyPin<Output<PushPull>>,
el_dir: AnyPin<Output<PushPull>>,
el_step: AnyPin<Output<PushPull>>,
usb_dev: UsbDevice<'static, UsbBusType>,
usb_serial: SerialPort<'static, UsbBusType>,
@ -206,15 +208,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");
@ -368,22 +370,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;
async fn move_axis<TargetMux, AngleMux>(
enable: &mut AnyPin<Output<PushPull>>,
dir: &mut AnyPin<Output<PushPull>>,
step: &mut AnyPin<Output<PushPull>>,
target: &mut TargetMux,
angle: &mut AngleMux,
) where
TargetMux: rtic::Mutex<T = i32>,
AngleMux: rtic::Mutex<T = i32>,
{
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;
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,
az_target,
az_angle
tmp_target,
tmp_angle
);
let delay = if diff.abs() < 10 {
@ -395,67 +401,46 @@ mod app {
};
if diff.abs() > 50 {
az_enable.set_low();
enable.set_low();
if diff < 0 {
az_dir.set_high();
dir.set_high();
} else {
az_dir.set_low();
dir.set_low();
}
az_step.set_low();
step.set_low();
Mono::delay(delay / 2).await;
az_step.set_high();
step.set_high();
Mono::delay(delay / 2).await;
} else {
az_enable.set_high();
enable.set_high();
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])]
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])]