Added basic AZ motor control loop

This commit is contained in:
Sebastian 2024-08-03 23:16:19 +02:00
parent 187b23db82
commit b4b07b505c

View file

@ -13,7 +13,6 @@ fn panic() -> ! {
cortex_m::asm::udf()
}
use rtic::app;
#[rtic::app(
device = stm32f4xx_hal::pac,
@ -21,12 +20,12 @@ use rtic::app;
)]
mod app {
use core::ops::Deref;
use as5048a::AS5048A;
use cortex_m::{asm, singleton};
use stm32f4xx_hal::{
gpio::{self, gpioa, gpiob, gpioc, Alternate, Analog, Input, OpenDrain, Output, PushPull},
gpio::{gpioa, gpiob, gpioc, Output, PushPull},
i2c,
pac::{I2C1, SPI1},
prelude::*,
@ -35,9 +34,9 @@ mod app {
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::*;
@ -46,7 +45,7 @@ mod app {
// Shared resources go here
#[shared]
struct Shared {
// TODO: Add resources
az_angle: i32,
}
// Local resources go here
@ -60,6 +59,14 @@ mod app {
spi_cs2: gpiob::PB14<Output<PushPull>>,
spi_cs3: gpiob::PB15<Output<PushPull>>,
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]
@ -87,16 +94,16 @@ mod app {
defmt::info!("Clock Setup done");
// Acquire the GPIO peripherials
let mut gpioa = cx.device.GPIOA.split();
let mut gpiob = cx.device.GPIOB.split();
let mut gpioc = cx.device.GPIOC.split();
let gpioa = cx.device.GPIOA.split();
let gpiob = cx.device.GPIOB.split();
let gpioc = cx.device.GPIOC.split();
let board_led = gpioc.pc13.into_push_pull_output();
// Todo: Check if internal pullups work here
let scl = gpiob.pb6.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,
(scl, sda),
i2c::Mode::Standard {
@ -115,15 +122,15 @@ mod app {
let spi_cs0 = gpiob.pb12.into_push_pull_output();
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 mut spi_cs2 = gpiob.pb14.into_push_pull_output();
let mut spi_cs3 = gpiob.pb15.into_push_pull_output();
let spi_cs2 = gpiob.pb14.into_push_pull_output();
let spi_cs3 = gpiob.pb15.into_push_pull_output();
let mut sck = gpioa.pa5.into_push_pull_output();
let mut poci = gpioa.pa6;
let mut pico = gpioa.pa7.into_push_pull_output();
let sck = gpioa.pa5.into_push_pull_output();
let poci = gpioa.pa6;
let pico = gpioa.pa7.into_push_pull_output();
let spi1 = spi::Spi::new(
cx.device.SPI1,
(sck, poci, pico),
@ -137,13 +144,21 @@ mod app {
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_spi::spawn().ok();
(
Shared {
// Initialization of shared resources go here
},
Shared { az_angle: 0 },
Local {
i2cmux,
board_led,
@ -152,6 +167,14 @@ mod app {
spi_cs2,
spi_cs3,
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])]
async fn poll_spi(cx: poll_spi::Context) {
#[task(local = [spi1, encoder_az, encoder_el, spi_cs2, spi_cs3], shared = [az_angle])]
async fn poll_spi(mut cx: poll_spi::Context) {
let spi1 = cx.local.spi1;
let encoder_az = cx.local.encoder_az;
@ -241,10 +264,46 @@ mod app {
defmt::info!("magnitude: {:?}", encoder_az.magnitude(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);
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;
}
}
}
}