Skip to content

Commit

Permalink
再びデバッグ表示が問題に。制御周期も5kHzまでしかあげられていない
Browse files Browse the repository at this point in the history
  • Loading branch information
@chama1176
chama1176 committed Sep 8, 2024
1 parent 3d39cda commit 87661b2
Show file tree
Hide file tree
Showing 3 changed files with 150 additions and 78 deletions.
68 changes: 58 additions & 10 deletions src/app.rs
View file
Original file line number Diff line number Diff line change
@@ -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;
Expand All @@ -11,11 +16,11 @@ use motml::utils::Deg;
pub enum ControlMode {
Waiting,
Calibrating,
Operating,
OperatingForcedCommutation,
OperatingForcedCommutation2,
Operating120DegreeDrive,
OperatingQPhase,
FieldOrientedControl,
}
pub struct App<T0, T1, M, E>
//, T2, M>
Expand All @@ -25,21 +30,27 @@ where
M: ThreePhaseMotorDriver,
E: Encoder<f32>,
{
// [Param value]
tv: f32,
count: u32,
calib_count: u8, // rad
control_mode: ControlMode,
encoder_offset: f32,
control_err_integral: DQCurrent<f32>,
// [Debug]
pub last_electrical_angle: f32, // debug
pub last_mechanical_angle: f32, // debug
pub last_current: ThreePhaseCurrent<f32>,
pub last_dq_current: DQCurrent<f32>,
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<T0, T1, M, E> App<T0, T1, M, E>
Expand All @@ -49,32 +60,41 @@ where
M: ThreePhaseMotorDriver,
E: Encoder<f32>,
{
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,
led1,
bldc,
encoder,
motor: ThreePhaseMotor::new(12),
current_sensor,
}
}
#[rustfmt::skip]
pub fn periodic_task(&mut self) {
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::<f32> { v_u: 0., v_v: 0., v_w: 0. };
Expand Down Expand Up @@ -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;
Expand All @@ -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 };
Expand Down
28 changes: 27 additions & 1 deletion src/bldc_motor_driver_stm32g4.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) });
}
Expand Down Expand Up @@ -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<f32> {
let adcd = free(|cs| G_ADC_DATA.borrow(cs).borrow().clone());
// I = V / R
// 60V/V, 0.003
// 基準電圧1.5V
ThreePhaseCurrent::<f32> {
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 {
Expand Down
132 changes: 65 additions & 67 deletions src/main.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 _;

Expand All @@ -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;

Expand Down Expand Up @@ -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 {
Expand Down Expand Up @@ -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();
Expand All @@ -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;
Expand Down Expand Up @@ -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::<f32>::default();
let mut dq_current = DQCurrent::<f32>::default();
let mut diff_count = 0;

let mut rad = 0.;
Expand All @@ -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;

}
Expand All @@ -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::<f32> {
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();



Expand Down

0 comments on commit 87661b2

Please sign in to comment.