diff --git a/gui/firmware/src/main.rs b/gui/firmware/src/main.rs index 5bddc3b..8df705e 100644 --- a/gui/firmware/src/main.rs +++ b/gui/firmware/src/main.rs @@ -18,6 +18,10 @@ mod app { use icd::Endpoint; use rtt_target::{rtt_init, ChannelMode}; use stm32g4xx_hal::{ + adc::{ + config::{Continuous, SampleTime, Sequence}, + Adc, AdcClaim, ClockSource, Configured, + }, cordic::{ self, func::dynamic::{Any, Mode as _}, @@ -25,6 +29,7 @@ mod app { types::Q31, Cordic, Ext, }, + delay::SYSTDelayExt as _, gpio::{ gpioc::{PC10, PC11, PC12, PC15, PC9}, Alternate, GpioExt as _, Output, PushPull, AF6, @@ -33,7 +38,7 @@ mod app { pwr::PwrExt as _, rcc::{Config, PllMDiv, PllNMul, PllRDiv, PllSrc, RccExt as _}, spi::{Spi, SpiExt as _}, - stm32::SPI3, + stm32::{ADC1, SPI3}, time::{ExtU32 as _, RateExtU32 as _}, timer::{CountDownTimer, Event, Timer}, }; @@ -60,6 +65,7 @@ mod app { timer_handler: CountDownTimer, cordic: DynamicCordic, encoder: AS5048A>>, + adc: Option>, } #[init] @@ -101,6 +107,7 @@ mod app { let cordic = cx.device.CORDIC.constrain(&mut rcc).into_dynamic(); + let gpioa = cx.device.GPIOA.split(&mut rcc); let gpioc = cx.device.GPIOC.split(&mut rcc); let cs0 = gpioc.pc9.into_push_pull_output(); @@ -121,6 +128,23 @@ mod app { let encoder = AS5048A::new(spi, cs0); + // These don't implement drop, so it should be fine to drop them but + // still use the ADC + let motor_current_a = gpioa.pa0.into_analog(); + let motor_current_b = gpioa.pa1.into_analog(); + + let mut delay = cx.core.SYST.delay(&rcc.clocks); + let mut adc = cx + .device + .ADC1 + .claim(ClockSource::SystemClock, &rcc, &mut delay, false); + + adc.set_continuous(Continuous::Single); + adc.reset_sequence(); + adc.configure_channel(&motor_current_a, Sequence::One, SampleTime::Cycles_640_5); + adc.configure_channel(&motor_current_b, Sequence::Two, SampleTime::Cycles_640_5); + let adc = adc.enable(); + let led = gpioc.pc15.into_push_pull_output(); let timer2 = Timer::new(cx.device.TIM2, &rcc.clocks); @@ -138,6 +162,7 @@ mod app { timer_handler: timer2, cordic, encoder, + adc: Some(adc), }, ) } @@ -154,7 +179,7 @@ mod app { cx.local.timer_handler.clear_interrupt(Event::TimeOut); } - #[task(local = [rpc_channel, cordic, encoder])] + #[task(local = [rpc_channel, cordic, encoder, adc])] async fn rtt_rpc(cx: rtt_rpc::Context) { let mut rx_buffer_raw = [0; 64]; let mut rx_buffer_frame = [0; 64]; @@ -178,7 +203,7 @@ mod app { icd::generate_endpoint_handler! { frame, tx_buffer, (icd::PingEndpoint, ping) - (icd::ReadEndpoint, |_: ()| -> u32 { stored_value }) + (icd::ReadEndpoint, |_: ()| { stored_value }) (icd::WriteEndpoint, |value: u32| { stored_value = value; }) (icd::SinCosEndpoint, |value: f32| { let (sin, cos) = cx.local.cordic.run::(I1F31::from_num(value)); @@ -187,6 +212,18 @@ mod app { (icd::EncoderAngle, |_: ()| { cx.local.encoder.angle().unwrap() }) + (icd::MotorPhaseCurrents, |_: ()| { + let adc = cx.local.adc.take().unwrap().start_conversion(); + + let adc = adc.wait_for_conversion_sequence().unwrap_active(); + let current_a_mv = adc.sample_to_millivolts(adc.current_sample()); + let adc = adc.wait_for_conversion_sequence().unwrap_stopped(); + let current_b_mv = adc.sample_to_millivolts(adc.current_sample()); + + *cx.local.adc = Some(adc); + + [current_a_mv, current_b_mv] + }) } }; diff --git a/gui/gui/src/connection.rs b/gui/gui/src/connection.rs index a608107..3becfd2 100644 --- a/gui/gui/src/connection.rs +++ b/gui/gui/src/connection.rs @@ -60,6 +60,10 @@ impl Device { pub fn encoder_angle(&mut self) -> Result { self.rpc_channel.call::(()) } + + pub fn motor_currents(&mut self) -> Result<[u16; 2], anyhow::Error> { + self.rpc_channel.call::(()) + } } struct RpcChannel { diff --git a/gui/gui/src/main.rs b/gui/gui/src/main.rs index ef0e9d9..da3b8c7 100644 --- a/gui/gui/src/main.rs +++ b/gui/gui/src/main.rs @@ -28,6 +28,7 @@ struct Behaviour { value_to_write: u32, angle: f32, sin_cos: Option<(f32, f32)>, + currents: Option<[u16; 2]>, } impl Behaviour { @@ -54,6 +55,8 @@ impl Behaviour { self.motor_state.mechanical_angle_rad = device.encoder_angle().unwrap() as f32 / 2f32.powi(14); + + self.currents = Some(device.motor_currents().unwrap()); } } } @@ -225,6 +228,12 @@ impl egui_tiles::Behavior for Behaviour { ui.label(format!("Cos: {cos:.3}")); } }); + if let Some([a, b]) = self.currents { + ui.horizontal(|ui| { + ui.label(format!("A: {a:.3}")); + ui.label(format!("B: {b:.3}")); + }); + } }); } @@ -377,6 +386,7 @@ fn main() -> Result<(), eframe::Error> { value_to_write: 0, angle: 0., sin_cos: None, + currents: None, }; behavior.update_probe_list(); diff --git a/gui/icd/src/lib.rs b/gui/icd/src/lib.rs index 10f1e10..c31171d 100644 --- a/gui/icd/src/lib.rs +++ b/gui/icd/src/lib.rs @@ -53,3 +53,4 @@ endpoint!(1, ReadEndpoint, (), u32); endpoint!(2, WriteEndpoint, u32, ()); endpoint!(3, SinCosEndpoint, f32, (f32, f32)); endpoint!(4, EncoderAngle, (), u16); +endpoint!(5, MotorPhaseCurrents, (), [u16; 2]);