Added basic AZ motor control loop
This commit is contained in:
Sebastian
parent
187b23db82
commit
b4b07b505c
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in a new issue