diff --git a/src/app.rs b/src/app.rs index b5fb5ce..604147d 100644 --- a/src/app.rs +++ b/src/app.rs @@ -1,8 +1,13 @@ +use core::cmp::max; + +use crate::bldc_motor_driver_stm32g4::CurrentSensor; use crate::indicator::Indicator; use motml::encoder::Encoder; use motml::motor::ThreePhaseMotor; +use motml::motor_driver::DQCurrent; use motml::motor_driver::DQVoltage; use motml::motor_driver::OutputStatus; +use motml::motor_driver::ThreePhaseCurrent; use motml::motor_driver::ThreePhaseMotorDriver; use motml::motor_driver::ThreePhaseValue; use motml::motor_driver::ThreePhaseVoltage; @@ -11,11 +16,11 @@ use motml::utils::Deg; pub enum ControlMode { Waiting, Calibrating, - Operating, OperatingForcedCommutation, OperatingForcedCommutation2, Operating120DegreeDrive, OperatingQPhase, + FieldOrientedControl, } pub struct App //, T2, M> @@ -25,21 +30,27 @@ where M: ThreePhaseMotorDriver, E: Encoder, { + // [Param value] tv: f32, count: u32, calib_count: u8, // rad control_mode: ControlMode, encoder_offset: f32, + control_err_integral: DQCurrent, + // [Debug] pub last_electrical_angle: f32, // debug pub last_mechanical_angle: f32, // debug + pub last_current: ThreePhaseCurrent, + pub last_dq_current: DQCurrent, pub last_tim_count: u32, pub diff_count: u32, - // + // [Device] led0: T0, led1: T1, bldc: M, encoder: E, pub motor: ThreePhaseMotor, + current_sensor: CurrentSensor, } impl App @@ -49,16 +60,19 @@ where M: ThreePhaseMotorDriver, E: Encoder, { - pub fn new(led0: T0, led1: T1, bldc: M, encoder: E) -> Self { + pub fn new(led0: T0, led1: T1, bldc: M, encoder: E, current_sensor: CurrentSensor) -> Self { Self { tv: 0.0, count: 0, calib_count: 0, control_mode: ControlMode::Waiting, encoder_offset: 0.238, + control_err_integral: DQCurrent::default(), // encoder_offset: 0.0, last_electrical_angle: 0.0, last_mechanical_angle: 0.0, + last_current: ThreePhaseCurrent::default(), + last_dq_current: DQCurrent::default(), last_tim_count: 0, diff_count: 0, led0, @@ -66,6 +80,7 @@ where bldc, encoder, motor: ThreePhaseMotor::new(12), + current_sensor, } } #[rustfmt::skip] @@ -73,8 +88,13 @@ where self.led0.toggle(); let ma = self.read_encoder_data(); let ea = self.motor.mechanical_angle_to_electrical_angle(ma); - self.last_mechanical_angle = ma; + let current = self.current_sensor.get_current(); + let dq_current = current.to_dq(ea); + + self.last_mechanical_angle = ma; self.last_electrical_angle = ea; + self.last_current = current; + self.last_dq_current = dq_current; self.calib_count = 7; let mut tp = ThreePhaseVoltage:: { v_u: 0., v_v: 0., v_w: 0. }; @@ -149,6 +169,36 @@ where v_w: (tpv.v_w + 6.0) / 12.0 } } + ControlMode::FieldOrientedControl =>{ + let ref_current = DQCurrent{ + i_d: 0.0, + i_q: self.tv * 0.5, + }; + let kp = 2.2; + let ki = 0.01; + + let err_current = DQCurrent{ + i_d: dq_current.i_d - ref_current.i_d, + i_q: dq_current.i_q - ref_current.i_q, + }; + + self.control_err_integral.i_d += err_current.i_d; + self.control_err_integral.i_q += err_current.i_q; + + self.control_err_integral.i_d = self.control_err_integral.i_d.clamp(-1.0, 1.0); + self.control_err_integral.i_q = self.control_err_integral.i_q.clamp(-1.0, 1.0); + + tpe = ThreePhaseValue { u: OutputStatus::Enable, v: OutputStatus::Enable, w: OutputStatus::Enable }; + let tpv = DQVoltage{ + v_d: -kp * err_current.i_d - ki * self.control_err_integral.i_d, + v_q: -kp * err_current.i_q - ki * self.control_err_integral.i_q, + }.to_three_phase(ea); + tp = ThreePhaseVoltage{ + v_u: (tpv.v_u + 6.0) / 12.0, + v_v: (tpv.v_v + 6.0) / 12.0, + v_w: (tpv.v_w + 6.0) / 12.0 + } + } ControlMode::OperatingForcedCommutation2 =>{ tpe = ThreePhaseValue { u: OutputStatus::Enable, v: OutputStatus::Enable, w: OutputStatus::Enable }; let s =((self.count as f32/(10.0*1.0)) as u32)%6; @@ -163,13 +213,11 @@ where } self.calib_count = s as u8; } - ControlMode::Operating => { - tpe = ThreePhaseValue { u: OutputStatus::Enable, v: OutputStatus::Enable, w: OutputStatus::Enable }; - tp = ThreePhaseVoltage{ v_u: 0., v_v: 0., v_w: 0. }; - } ControlMode::Calibrating => { - tpe = ThreePhaseValue { u: OutputStatus::Enable, v: OutputStatus::Enable, w: OutputStatus::Enable }; - tp = ThreePhaseVoltage{ v_u: 0.6, v_v: 0.4, v_w: 0.4 }; + // tpe = ThreePhaseValue { u: OutputStatus::Enable, v: OutputStatus::Enable, w: OutputStatus::Enable }; + // tp = ThreePhaseVoltage{ v_u: 0.6, v_v: 0.4, v_w: 0.4 }; + tpe = ThreePhaseValue { u: OutputStatus::Enable, v: OutputStatus::Enable, w: OutputStatus::Disable }; + tp = ThreePhaseVoltage{ v_u: 0.52, v_v: 0.48, v_w: 0.0 }; } _ =>{ tpe = ThreePhaseValue { u: OutputStatus::Disable, v: OutputStatus::Disable, w: OutputStatus::Disable }; diff --git a/src/bldc_motor_driver_stm32g4.rs b/src/bldc_motor_driver_stm32g4.rs index f181ef0..d1a2ed6 100644 --- a/src/bldc_motor_driver_stm32g4.rs +++ b/src/bldc_motor_driver_stm32g4.rs @@ -88,7 +88,7 @@ pub fn clock_init(perip: &Peripherals, core_perip: &mut CorePeripherals) { // For ADC let tim6 = &perip.TIM6; tim6.psc.modify(|_, w| unsafe { w.bits(1400 - 1) }); - tim6.arr.modify(|_, w| unsafe { w.bits(10 - 1) }); // 10kHz + tim6.arr.modify(|_, w| unsafe { w.bits(20 - 1) }); // 5kHz tim6.dier.modify(|_, w| w.uie().set_bit()); tim6.cr2.modify(|_, w| unsafe { w.mms().bits(0b010) }); } @@ -257,6 +257,32 @@ pub fn dma_adc2_start(perip: &Peripherals) { adc.cr.modify(|_, w| w.adstart().start()); // ADC start } +/// ADCは別で実装されている前提 +pub struct CurrentSensor { + u_current_offset: f32, + v_current_offset: f32, +} +impl<'a> CurrentSensor { + pub fn new() -> Self { + Self { + u_current_offset: 0.0, + v_current_offset: -0.0, + } + } + /// Return value in Ampere + pub fn get_current(&self) -> ThreePhaseCurrent { + let adcd = free(|cs| G_ADC_DATA.borrow(cs).borrow().clone()); + // I = V / R + // 60V/V, 0.003 + // 基準電圧1.5V + ThreePhaseCurrent:: { + i_u: (((adcd[2] as f32) / adcd[6] as f32 * 1.5) - 1.5) / 60.0 / 0.003 - self.u_current_offset, + i_v: (((adcd[3] as f32) / adcd[6] as f32 * 1.5) - 1.5) / 60.0 / 0.003 - self.v_current_offset, + i_w: 0.0, + } + } +} + // TODO: Need to check pub struct FrashStorage {} impl<'a> FrashStorage { diff --git a/src/main.rs b/src/main.rs index fb240ed..e68715c 100644 --- a/src/main.rs +++ b/src/main.rs @@ -6,6 +6,7 @@ use core::cell::RefCell; use core::clone; use core::fmt::Write; use core::ops::DerefMut; +use bldc_motor_driver_stm32g4::CurrentSensor; use defmt_rtt as _; use panic_halt as _; @@ -26,7 +27,7 @@ mod indicator; use crate::indicator::Indicator; use motml::encoder::Encoder; -use motml::motor_driver::{self, ThreePhaseCurrent}; +use motml::motor_driver::{self, DQCurrent, ThreePhaseCurrent}; use motml::utils::Deg; use motml::motor::ThreePhaseMotor; @@ -118,7 +119,8 @@ fn TIM3() { if tv > 0.1 { // app.set_control_mode(app::ControlMode::OperatingForcedCommutation2); // app.set_control_mode(app::ControlMode::Operating120DegreeDrive); - app.set_control_mode(app::ControlMode::OperatingQPhase); + // app.set_control_mode(app::ControlMode::OperatingQPhase); + app.set_control_mode(app::ControlMode::FieldOrientedControl); } else if tv < -0.5 { app.set_control_mode(app::ControlMode::Calibrating); } else { @@ -179,6 +181,7 @@ fn main() -> ! { spi_enc.init(); spi_enc.reset_error(); + let current_sensor = bldc_motor_driver_stm32g4::CurrentSensor::new(); // let flash = bldc_motor_driver_stm32g4::FrashStorage::new(); // flash.write(); @@ -192,7 +195,7 @@ fn main() -> ! { spi.txrx(0x800000 | 0x1B_0000); spi.txrx(0x000000 | 0x1B_0000); - let app = app::App::new(led0, led1, pwm, spi_enc); + let app = app::App::new(led0, led1, pwm, spi_enc, current_sensor); free(|cs| G_APP.borrow(cs).replace(Some(app))); let mut t = 0; @@ -226,13 +229,15 @@ fn main() -> ! { }); if (t + 10000 - prev) % 10000 >= 1000 { cnt += 1; - if cnt > 50 { + if cnt > 100 { // defmt::info!("hello from defmt"); cnt = 0; let mut adcd: [u16; 7] = [0; 7]; let mut electrical_angle = 0.0; let mut mechanical_angle = 0.0; + let mut current = ThreePhaseCurrent::::default(); + let mut dq_current = DQCurrent::::default(); let mut diff_count = 0; let mut rad = 0.; @@ -245,6 +250,8 @@ fn main() -> ! { adcd = free(|cs| bldc_motor_driver_stm32g4::G_ADC_DATA.borrow(cs).borrow().clone()); electrical_angle = app.last_electrical_angle; mechanical_angle = app.last_mechanical_angle; + current = app.last_current; + dq_current = app.last_dq_current; diff_count = app.diff_count; } @@ -254,74 +261,65 @@ fn main() -> ! { // write!(uart, "{}, {:4}, {:4}", calib_count, deg, rad).unwrap(); // write!(uart, "\"tv\": {:4}\r\n", tv,).unwrap(); - write!( - uart, - "{{\"ADC\":[{:4}, {:4}, {:4}, {:4}, {:4}, {:4}, {:4}]}}\r\n", - adcd[0], - adcd[1], - adcd[2], - adcd[3], - adcd[4], - adcd[5], - adcd[6] - ) - .unwrap(); + // write!( + // uart, + // "{{\"ADC\":[{:4}, {:4}, {:4}, {:4}, {:4}, {:4}, {:4}]}}\r\n", + // adcd[0], + // adcd[1], + // adcd[2], + // adcd[3], + // adcd[4], + // adcd[5], + // adcd[6] + // ) + // .unwrap(); + + // defmt::info!("diff: {}", diff_count); - // I = V / R - // 60V/V, 0.003 - let current = ThreePhaseCurrent:: { - i_u: (((adcd[2] as f32) / adcd[6] as f32 * 1.5) - 1.5) / 60.0 / 0.003 - 0.0, - i_v: (((adcd[3] as f32) / adcd[6] as f32 * 1.5) - 1.5) / 60.0 / 0.003 - 0.0, - i_w: 0.0, - }; - let dq = current.to_dq(electrical_angle); - - defmt::info!("diff: {}", diff_count); - - // floatのまま送るとFLASHをバカほど食うのでcastする - write!( - uart, - "{{\"ea\":{:4}}}\r\n", - (electrical_angle * 1000.0) as i32, - ) - .unwrap(); + // // floatのまま送るとFLASHをバカほど食うのでcastする + // write!( + // uart, + // "{{\"ea\":{:4}}}\r\n", + // (electrical_angle * 1000.0) as i32, + // ) + // .unwrap(); - // floatのまま送るとFLASHをバカほど食うのでcastする - write!( - uart, - "{{\"ma\":{:4}}}\r\n", - (mechanical_angle * 1000.0) as i32, - ) - .unwrap(); + // // floatのまま送るとFLASHをバカほど食うのでcastする + // write!( + // uart, + // "{{\"ma\":{:4}}}\r\n", + // (mechanical_angle * 1000.0) as i32, + // ) + // .unwrap(); - // floatのまま送るとFLASHをバカほど食うのでcastする - write!( - uart, - "{{\"iu\":{:4}}}\r\n", - (current.i_u * 1000.0) as i32, - ) - .unwrap(); - // floatのまま送るとFLASHをバカほど食うのでcastする - write!( - uart, - "{{\"iv\":{:4}}}\r\n", - (current.i_v * 1000.0) as i32, - ) - .unwrap(); + // // floatのまま送るとFLASHをバカほど食うのでcastする + // write!( + // uart, + // "{{\"iu\":{:4}}}\r\n", + // (current.i_u * 1000.0) as i32, + // ) + // .unwrap(); + // // floatのまま送るとFLASHをバカほど食うのでcastする + // write!( + // uart, + // "{{\"iv\":{:4}}}\r\n", + // (current.i_v * 1000.0) as i32, + // ) + // .unwrap(); // floatのまま送るとFLASHをバカほど食うのでcastする - write!( - uart, - "{{\"d\":{:4}}}\r\n", - (dq.i_d * 1000.0) as i32, - ) - .unwrap(); - write!( - uart, - "{{\"q\":{:4}}}\r\n", - (dq.i_q * 1000.0) as i32, - ) - .unwrap(); + // write!( + // uart, + // "{{\"d\":{:4}}}\r\n", + // (dq_current.i_d * 1000.0) as i32, + // ) + // .unwrap(); + // write!( + // uart, + // "{{\"q\":{:4}}}\r\n", + // (dq_current.i_q * 1000.0) as i32, + // ) + // .unwrap();