From b4b07b505c0312f75cac9b0ac76bb1cfa9113c66 Mon Sep 17 00:00:00 2001 From: Sebastian Date: Sat, 3 Aug 2024 23:16:19 +0200 Subject: [PATCH] Added basic AZ motor control loop --- firmware/src/main.rs | 107 +++++++++++++++++++++++++++++++++---------- 1 file changed, 83 insertions(+), 24 deletions(-) diff --git a/firmware/src/main.rs b/firmware/src/main.rs index ee05b29..ac5a199 100644 --- a/firmware/src/main.rs +++ b/firmware/src/main.rs @@ -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>, spi_cs3: gpiob::PB15>, spi1: spi::Spi, + + az_enable: gpioa::PA12>, + az_dir: gpioa::PA15>, + az_step: gpiob::PB3>, + + el_enable: gpiob::PB4>, + el_dir: gpioa::PA8>, + el_step: gpioa::PA9>, } #[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; + } } } }