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

Added basic AZ motor control loop

Browse source
This commit is contained in:
Sebastian 2024-08-03 23:16:19 +02:00
parent 187b23db82
commit b4b07b505c
1 changed files with 83 additions and 24 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

107
firmware/src/main.rs
View file

@ -13,7 +13,6 @@ fn panic() -> ! {
cortex_m::asm::udf() cortex_m::asm::udf()
} }
use rtic::app;
#[rtic::app( #[rtic::app(
device = stm32f4xx_hal::pac, device = stm32f4xx_hal::pac,
@ -21,12 +20,12 @@ use rtic::app;
)] )]
mod app { mod app {
use core::ops::Deref;
use as5048a::AS5048A; use as5048a::AS5048A;
use cortex_m::{asm, singleton};
use stm32f4xx_hal::{ use stm32f4xx_hal::{
gpio::{self, gpioa, gpiob, gpioc, Alternate, Analog, Input, OpenDrain, Output, PushPull}, gpio::{gpioa, gpiob, gpioc, Output, PushPull},
i2c, i2c,
pac::{I2C1, SPI1}, pac::{I2C1, SPI1},
prelude::*, prelude::*,
@ -35,9 +34,9 @@ mod app {
use num_traits::{Float, FloatConst}; use num_traits::{Float, FloatConst};
use xca9548a::{Error, SlaveAddr, Xca9548a}; use xca9548a::{SlaveAddr, Xca9548a};
use qmc5883l::{self, OutputDataRate, OversampleRate, QMC5883L}; use qmc5883l::{self, QMC5883L};
use rtic_monotonics::systick::prelude::*; use rtic_monotonics::systick::prelude::*;
@ -46,7 +45,7 @@ mod app {
// Shared resources go here // Shared resources go here
#[shared] #[shared]
struct Shared { struct Shared {
// TODO: Add resources az_angle: i32,
} }
// Local resources go here // Local resources go here
@ -60,6 +59,14 @@ mod app {
spi_cs2: gpiob::PB14<Output<PushPull>>, spi_cs2: gpiob::PB14<Output<PushPull>>,
spi_cs3: gpiob::PB15<Output<PushPull>>, spi_cs3: gpiob::PB15<Output<PushPull>>,
spi1: spi::Spi<SPI1>, spi1: spi::Spi<SPI1>,
az_enable: gpioa::PA12<Output<PushPull>>,
az_dir: gpioa::PA15<Output<PushPull>>,
az_step: gpiob::PB3<Output<PushPull>>,
el_enable: gpiob::PB4<Output<PushPull>>,
el_dir: gpioa::PA8<Output<PushPull>>,
el_step: gpioa::PA9<Output<PushPull>>,
} }
#[init] #[init]
@ -87,16 +94,16 @@ mod app {
defmt::info!("Clock Setup done"); defmt::info!("Clock Setup done");
// Acquire the GPIO peripherials // Acquire the GPIO peripherials
let mut gpioa = cx.device.GPIOA.split(); let gpioa = cx.device.GPIOA.split();
let mut gpiob = cx.device.GPIOB.split(); let gpiob = cx.device.GPIOB.split();
let mut gpioc = cx.device.GPIOC.split(); let gpioc = cx.device.GPIOC.split();
let board_led = gpioc.pc13.into_push_pull_output(); let board_led = gpioc.pc13.into_push_pull_output();
// Todo: Check if internal pullups work here // Todo: Check if internal pullups work here
let scl = gpiob.pb6.into_alternate_open_drain(); let scl = gpiob.pb6.into_alternate_open_drain();
let sda = gpiob.pb7.into_alternate_open_drain(); let sda = gpiob.pb7.into_alternate_open_drain();
let mut i2c = i2c::I2c::new( let i2c = i2c::I2c::new(
cx.device.I2C1, cx.device.I2C1,
(scl, sda), (scl, sda),
i2c::Mode::Standard { i2c::Mode::Standard {
@ -115,15 +122,15 @@ mod app {
let spi_cs0 = gpiob.pb12.into_push_pull_output(); let spi_cs0 = gpiob.pb12.into_push_pull_output();
let encoder_az = AS5048A::new(spi_cs0); let encoder_az = AS5048A::new(spi_cs0);
let mut spi_cs1 = gpiob.pb13.into_push_pull_output(); let spi_cs1 = gpiob.pb13.into_push_pull_output();
let encoder_el = AS5048A::new(spi_cs1); let encoder_el = AS5048A::new(spi_cs1);
let mut spi_cs2 = gpiob.pb14.into_push_pull_output(); let spi_cs2 = gpiob.pb14.into_push_pull_output();
let mut spi_cs3 = gpiob.pb15.into_push_pull_output(); let spi_cs3 = gpiob.pb15.into_push_pull_output();
let mut sck = gpioa.pa5.into_push_pull_output(); let sck = gpioa.pa5.into_push_pull_output();
let mut poci = gpioa.pa6; let poci = gpioa.pa6;
let mut pico = gpioa.pa7.into_push_pull_output(); let pico = gpioa.pa7.into_push_pull_output();
let spi1 = spi::Spi::new( let spi1 = spi::Spi::new(
cx.device.SPI1, cx.device.SPI1,
(sck, poci, pico), (sck, poci, pico),
@ -137,13 +144,21 @@ mod app {
defmt::info!("SPI Setup done"); defmt::info!("SPI Setup done");
let az_enable = gpioa.pa12.into_push_pull_output();
let az_dir = gpioa.pa15.into_push_pull_output();
let az_step = gpiob.pb3.into_push_pull_output();
let el_enable = gpiob.pb4.into_push_pull_output();
let el_dir = gpioa.pa8.into_push_pull_output();
let el_step = gpioa.pa9.into_push_pull_output();
defmt::info!("Motor Setup done");
poll_i2c::spawn().ok(); poll_i2c::spawn().ok();
poll_spi::spawn().ok(); poll_spi::spawn().ok();
( (
Shared { Shared { az_angle: 0 },
// Initialization of shared resources go here
},
Local { Local {
i2cmux, i2cmux,
board_led, board_led,
@ -152,6 +167,14 @@ mod app {
spi_cs2, spi_cs2,
spi_cs3, spi_cs3,
spi1, spi1,
az_enable,
az_dir,
az_step,
el_enable,
el_dir,
el_step,
}, },
) )
} }
@ -230,8 +253,8 @@ mod app {
} }
} }
#[task(local = [spi1, encoder_az, encoder_el, spi_cs2, spi_cs3])] #[task(local = [spi1, encoder_az, encoder_el, spi_cs2, spi_cs3], shared = [az_angle])]
async fn poll_spi(cx: poll_spi::Context) { async fn poll_spi(mut cx: poll_spi::Context) {
let spi1 = cx.local.spi1; let spi1 = cx.local.spi1;
let encoder_az = cx.local.encoder_az; let encoder_az = cx.local.encoder_az;
@ -241,10 +264,46 @@ mod app {
defmt::info!("magnitude: {:?}", encoder_az.magnitude(spi1).unwrap()); defmt::info!("magnitude: {:?}", encoder_az.magnitude(spi1).unwrap());
let raw_angle = encoder_az.angle(spi1).unwrap(); let raw_angle = encoder_az.angle(spi1).unwrap();
let angle_deg = raw_angle as u32 * 3600 / 16384; let angle_deg = raw_angle as i32 * 3600 / 16384;
cx.shared.az_angle.lock(|az_angle| {
*az_angle = angle_deg;
});
defmt::info!("angle: {:?}", angle_deg); defmt::info!("angle: {:?}", angle_deg);
Mono::delay(100.millis()).await; Mono::delay(50.millis()).await;
}
}
#[task(local = [az_enable, az_dir, az_step], shared = [az_angle])]
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;
let az_target = 42i32;
loop {
let az_angle = cx.shared.az_angle.lock(|az_angle| *az_angle);
let diff = az_angle - az_target;
if diff.abs() > 2 {
az_enable.set_high();
if diff > 0 {
az_dir.set_high();
} else {
az_dir.set_low();
}
az_step.set_high();
Mono::delay(250.micros()).await;
az_step.set_low();
Mono::delay(250.micros()).await;
} else {
az_enable.set_low();
Mono::delay(500.micros()).await;
}
} }
} }
} }