manabu kosaka
DC motor control program using TA7291P type H bridge driver and rotary encoder with A, B phase.
Dependencies: QEI mbed-rtos mbed
Fork of
DCmotor
by 
manabu kosaka
Revision 13:ba71733c11d7, committed 2013-03-01
- Comitter:
- kosaka
- Date:
- Fri Mar 01 02:10:59 2013 +0000
- Parent:
- 12:459af534d1ee
- Child:
- 14:02411880ffb9
- Commit message:
- 130214
Changed in this revision
--- a/Hbridge.cpp Fri Jan 04 12:00:48 2013 +0000
+++ b/Hbridge.cpp Fri Mar 01 02:10:59 2013 +0000
@@ -1,122 +1,133 @@
+// Hbridge.cpp: モータ駆動用ドライバのフルブリッジ（Ｈブリッジ）をＰＷＭ駆動する。
+
#include "mbed.h"
#include "controller.h"
#include "Hbridge.h"
-#define DEADTIME_US (unsigned long)(DEADTIME*1000000) // [us], deadtime to be set between plus volt. to/from minus
+#define DEADTIME_US (unsigned long)(DEADTIME*1000000) // [us], デッドタイム deadtime to be set between plus volt. to/from minus
-Timeout pwm;
+Timeout pwm; // タイムアウト関数の宣言（ある時間経過後に関数コールする）
-DigitalOut pwm_upper = UPPER_PORT;
-DigitalOut pwm_lower = LOWER_PORT;
+DigitalOut pwm_fwdIN = fwdIN_PORT; // デジタル信号を出力するポートを設定
+DigitalOut pwm_rvsIN = rvsIN_PORT; // デジタル信号を出力するポートを設定
-pwm_parameters IN; // UVW pwm の定数、変数
+pwm_parameters IN; // フルブリッジのパラメータ宣言
+
+AnalogIn VshuntR_plus(R_SHUNT_P_PORT); // *3.3 [V], シャント抵抗のプラス側アナログ入力, Volt of shunt R_SHUNT[Ohm]. The motor current i = v_shunt_r/R_SHUNT [A]
+AnalogIn VshuntR_minus(R_SHUNT_M_PORT); // *3.3 [V], シャント抵抗のマイナス側アナログ入力, Volt of shunt R_SHUNT[Ohm]. The motor current i = v_shunt_r/R_SHUNT [A]
DigitalOut debug_p24(p24); // p17 for debug
//DigitalOut Led3(LED3);
-#if PWM_WAVEFORM==0 // 0: saw tooth wave comparison
+#if PWM_WAVEFORM==0 // PWM変調方式がノコギリ波比較のとき
#if 1
-void pwm_out() { // pwm out using timer
+void pwm_out() { // タイムアウト関数でPWMを発生する関数: [fwdIN,rvsIN]=[PWM,0] or [0,PWM] or [0,0]
//debug_p24=1;
- IN.mode += 1;
-//IN.duty=0.9;IN.fReverse[0]=1;
- if( IN.fDeadtime==1 && IN.mode==1){
- pwm.attach_us(&pwm_out, DEADTIME_US); // setup pwmU to call pwm_out after t [us]
- pwm_upper = 0; pwm_lower = 0;
- IN.fDeadtime = 0;
- IN.fReverse[1] = IN.fReverse[0];
- IN.mode = 0;
- }else if( IN.mode==1 ){
- if( IN.fReverse[1]==0 ){
- pwm_upper = 1; pwm_lower = 0;
- }else{
- pwm_upper = 0; pwm_lower = 1;
+ IN.mode += 1; // チョッピングのオンオフを決定するモードを１増やす
+//IN.duty=0.9;IN.fReverse=1;
+ if( IN.fDeadtime==1 && IN.mode==1){ // デッドタイムフラグがオンで、モードが１のとき
+ pwm.attach_us(&pwm_out, DEADTIME_US); // fwdIN, rvsIN上下アームをデッドタイム時間オフしてからタイムアウトでこの関数自身をコール setup pwmU to call pwm_out after t [us]
+ pwm_fwdIN = 0; pwm_rvsIN = 0; // fwdIN, rvsIN上下アームともに０にする
+ IN.fDeadtime = 0; // デッドタイムフラグをゼロクリアする
+ IN.mode = 0; // モードを0にする
+ }else if( IN.mode==1 ){ // モードが1のとき
+ if( IN.fReverse==0 ){ // モータの回転が順方向のとき
+ pwm_fwdIN = 1; pwm_rvsIN = 0; // fwdINのみオンにする
+ }else{ // モータの回転が逆方向のとき
+ pwm_fwdIN = 0; pwm_rvsIN = 1; // rvsINのみオンにする
}
- IN.upper_us = IN.duty*1000000/PWM_FREQ; // ON time of pwm
- if( IN.upper_us < TMIN ){ IN.upper_us=TMIN;}
- pwm.attach_us(&pwm_out, IN.upper_us); // setup pwmU to call pwm_out after t [us]
- IN.lower_us = 1000000/PWM_FREQ -IN.upper_us; // OFF time of pwm
- if( IN.lower_us < TMIN ){ IN.lower_us=TMIN;}
- }else{// if( IN.mode==2 ){
- pwm.attach_us(&pwm_out, IN.lower_us); // setup pwmU to call pwm_out after t [us]
- pwm_upper = 0; pwm_lower = 0;
- IN.mode = 0;
+ IN.fwdIN_us = IN.duty*1000000/PWM_FREQ; // fwdINをオンする時間幅を計算 ON time of pwm
+ if( IN.fwdIN_us < TMIN ){ IN.fwdIN_us=TMIN;} // 時間幅が負のときはTMINにする
+ pwm.attach_us(&pwm_out, IN.fwdIN_us); // fwdINをオンする時間幅経過後にタイムアウトでこの関数自身をコール setup pwmU to call pwm_out after t [us]
+ IN.rvsIN_us = 1000000/PWM_FREQ -IN.fwdIN_us; // rvsINをオンする時間幅を計算 OFF time of pwm
+ if( IN.rvsIN_us < TMIN ){ IN.rvsIN_us=TMIN;} // 時間幅が負のときはTMINにする
+ }else{// if( IN.mode==2 ){ // モードが２のとき
+ pwm.attach_us(&pwm_out, IN.rvsIN_us); // fwdIN, rvsINを共にオフする時間幅経過後にタイムアウトでこの関数自身をコール setup pwmU to call pwm_out after t [us]
+#ifndef SIMULATION
+ // モータ電流の検出
+ p.i = (VshuntR_plus - VshuntR_minus) /R_SHUNT; // シャント抵抗の両端の電圧を見てモータ電流を検出 get i [A] from shunt resistance;
+#endif
+ pwm_fwdIN = 0; pwm_rvsIN = 0; // fwdIN, rvsIN共に０にする
+ IN.mode = 0; // チョッピングのオンオフを決定するモードを0にする
}
//debug_p24=0;
}
#else
-void pwm_out() { // pwm out using timer
+void pwm_out() { // 補PWM pwm out using timer
IN.mode += 1;
if( IN.mode==1 ){
- pwm_upper = 1;
- pwm_lower = 0;
- IN.upper_us = IN.duty*1000000/PWM_FREQ - DEADTIME_US; // ON time of Uupper
- if( IN.upper_us < TMIN ){ IN.upper_us=TMIN;}
- pwm.attach_us(&pwm_out, IN.upper_us); // setup pwmU to call pwm_out after t [us]
- IN.lower_us = 1000000/PWM_FREQ -IN.upper_us - 2*DEADTIME_US; // ON time of Ulower
- if( IN.lower_us < TMIN ){ IN.lower_us=TMIN;}
+ pwm_fwdIN = 1;
+ pwm_rvsIN = 0;
+ IN.fwdIN_us = IN.duty*1000000/PWM_FREQ - DEADTIME_US; // ON time of UfwdIN
+ if( IN.fwdIN_us < TMIN ){ IN.fwdIN_us=TMIN;}
+ pwm.attach_us(&pwm_out, IN.fwdIN_us); // setup pwmU to call pwm_out after t [us]
+ IN.rvsIN_us = 1000000/PWM_FREQ -IN.fwdIN_us - 2*DEADTIME_US; // ON time of UrvsIN
+ if( IN.rvsIN_us < TMIN ){ IN.rvsIN_us=TMIN;}
}else if( IN.mode==2 ){
pwm.attach_us(&pwm_out, DEADTIME_US); // setup pwmU to call pwm_out after t [us]
- pwm_upper = 0;
- pwm_lower = 0;
+ pwm_fwdIN = 0;
+ pwm_rvsIN = 0;
}else if( IN.mode==3 ){
- pwm.attach_us(&pwm_out, IN.lower_us); // setup pwmU to call pwm_out after t [us]
- pwm_upper = 0;
- pwm_lower = 1;
+ pwm.attach_us(&pwm_out, IN.rvsIN_us); // setup pwmU to call pwm_out after t [us]
+ pwm_fwdIN = 0;
+ pwm_rvsIN = 1;
}else{// if( u.mode==4 ){
pwm.attach_us(&pwm_out, DEADTIME_US); // setup pwmU to call pwm_out after t [us]
- pwm_upper = 0;
- pwm_lower = 0;
+ pwm_fwdIN = 0;
+ pwm_rvsIN = 0;
IN.mode = 0;
}
}
#endif
-#elif PWM_WAVEFORM==1 // 1: triangler wave comparison
-void pwm_out() { // pwm out using timer
- IN.mode += 1;
- if( IN.fDeadtime==1 && IN.mode==1){
- pwm.attach_us(&pwm_out, DEADTIME_US); // setup pwmU to call pwm_out after t [us]
- pwm_upper = 0; pwm_lower = 0;
- IN.fDeadtime = 0;
- IN.fReverse[1] = IN.fReverse[0];
- IN.mode = 0;
- }else if( IN.mode==1 ){
- IN.upper_us = IN.duty*1000000/PWM_FREQ; // ON time of Uupper
- IN.lower_us = 1000000/PWM_FREQ -IN.upper_us; // ON time of Ulower
- IN.lower_us /= 2;
- if( IN.lower_us < TMIN ){ IN.lower_us=TMIN;}
- pwm.attach_us(&pwm_out, IN.lower_us); // setup pwmU to call pwm_out after t [us]
- if( IN.upper_us < TMIN ){ IN.upper_us=TMIN;}
- pwm_upper = 0; pwm_lower = 0;
- }else if( IN.mode==2 ){
- pwm.attach_us(&pwm_out, IN.upper_us); // setup pwmU to call pwm_out after t [us]
- if( IN.fReverse[1]==0 ){
- pwm_upper = 1; pwm_lower = 0;
- }else{
- pwm_upper = 0; pwm_lower = 1;
+#elif PWM_WAVEFORM==1 // PWM変調方式が三角波比較のとき
+void pwm_out() { // タイムアウト関数でPWMを発生する関数: [fwdIN,rvsIN]=[PWM,0] or [0,PWM] or [0,0]
+ IN.mode += 1; // チョッピングのオンオフを決定するモードを１増やす
+ if( IN.fDeadtime==1 && IN.mode==1){ // デッドタイムフラグがオンで、モードが１のとき
+ pwm.attach_us(&pwm_out, DEADTIME_US); // fwdIN, rvsIN上下アームをデッドタイム時間オフしてからタイムアウトでこの関数自身をコール setup pwmU to call pwm_out after t [us]
+ pwm_fwdIN = 0; pwm_rvsIN = 0; // fwdIN, rvsINともに０にする
+ IN.fDeadtime = 0; // デッドタイムフラグをゼロクリアする
+ IN.mode = 0; // モードを0にする
+ }else if( IN.mode==1 ){ // モードが1のとき
+ IN.fwdIN_us = IN.duty*1000000/PWM_FREQ; // fwdINをオンする時間幅を計算 // ON time of UfwdIN
+ IN.rvsIN_us = 1000000/PWM_FREQ -IN.fwdIN_us; // fwdIN, rvsINをオンする時間幅を計算 // ON time of UrvsIN
+ IN.rvsIN_us /= 2; // その時間幅を2で割る
+ if( IN.rvsIN_us < TMIN ){ IN.rvsIN_us=TMIN;} // 時間幅が負のときはTMINにする
+ pwm.attach_us(&pwm_out, IN.rvsIN_us); // fwdIN, rvsINをオフする時間幅経過後にタイムアウトでこの関数自身をコール setup pwmU to call pwm_out after t [us]
+ if( IN.fwdIN_us < TMIN ){ IN.fwdIN_us=TMIN;} // 時間幅が負のときはTMINにする
+ pwm_fwdIN = 0; pwm_rvsIN = 0; // fwdIN, rvsINともに０にする
+ }else if( IN.mode==2 ){ // モードが２のとき
+ pwm.attach_us(&pwm_out, IN.fwdIN_us); // fwdINをオンする時間幅経過後にタイムアウトでこの関数自身をコール setup pwmU to call pwm_out after t [us]
+ if( IN.fReverse==0 ){ // モータの回転が順方向のとき
+ pwm_fwdIN = 1; pwm_rvsIN = 0; // fwdINのみオンにする
+ }else{ // モータの回転が逆方向のとき
+ pwm_fwdIN = 0; pwm_rvsIN = 1; // rvsINのみオンにする
}
- }else{// if( IN.mode==3 ){
- pwm.attach_us(&pwm_out, IN.lower_us); // setup pwmU to call pwm_out after t [us]
- pwm_upper = 0; pwm_lower = 0;
- IN.mode = 0;
+ }else{// if( IN.mode==3 ){ // モードが３のとき
+ pwm.attach_us(&pwm_out, IN.rvsIN_us); // fwdIN, rvsINを共にオフする時間幅経過後にタイムアウトでこの関数自身をコール setup pwmU to call pwm_out after t [us]
+#ifndef SIMULATION
+ // モータ電流の検出
+ p.i = (VshuntR_plus - VshuntR_minus) /R_SHUNT; // シャント抵抗の両端の電圧を見てモータ電流を検出 get i [A] from shunt resistance;
+#endif
+ pwm_fwdIN = 0; pwm_rvsIN = 0; // fwdIN, rvsINともに０にする
+ IN.mode = 0; // チョッピングのオンオフを決定するモードを0にする
}
}
#endif
+void start_pwm(){ // PWMスタートする関数
+ IN.duty = 0.0; // デューティーを０にする
+ pwm_fwdIN = pwm_rvsIN = 0; // fwdIN, rvsIN上下アームともに０にする
+ IN.mode = 0; // チョッピングのオンオフを決定するモードを０にする
+ IN.fDeadtime = 1; // 正負切替時にデッドタイムを要求するフラグをオフにする
+ IN.fReverse = 0; // モータ逆回転フラグをオフにする：回転方向が順方向のとき0、逆方向のとき1。[0]が現在の値、[1]はその前の値
-void start_pwm(){
- IN.duty = 0.0;
- pwm_upper = pwm_lower = 0;
- IN.mode = 0;
- IN.fDeadtime = 1;
- IN.fReverse[0] = 0;
-
- pwm_out();
+ pwm_out(); // タイムアウト関数でPWMを発生する関数をスタート
}
-void stop_pwm(){
- IN.duty = 0.0;
- pwm_upper = pwm_lower = 0;
- IN.mode = 0;
- pwm.detach();
+void stop_pwm(){ // PWMストップする関数
+ IN.duty = 0.0; // デューティーを０にする
+ pwm_fwdIN = pwm_rvsIN = 0; // fwdIN, rvsIN上下アームともに０にする
+ IN.mode = 0; // チョッピングのオンオフを決定するモードを０にする
+
+ pwm.detach(); // タイムアウト関数をストップ
}
--- a/Hbridge.h Fri Jan 04 12:00:48 2013 +0000
+++ b/Hbridge.h Fri Mar 01 02:10:59 2013 +0000
@@ -1,25 +1,28 @@
#ifndef __Hbridge_h
#define __Hbridge_h
-//************* User setting parameters (begin) *****************
-//#define PWM_FREQ 0.5 //[Hz], pwm freq.
-//#define DEADTIME 0.2 // [s], deadtime to be set between plus volt. to/from minus
-#define UPPER_PORT p21//LED1 // port for U phase upper arm
-#define LOWER_PORT p22 // port for U phase lower arm
-#define PWM_WAVEFORM 0 // 0: saw tooth wave comparison, 1: triangler wave comparison
-#define TMIN 5 // [us], processing time of pwm_out()
-//************* User setting parameters (end) *****************
+//************* 使用するポートなどの設定 (ここから) *****************
+#define PWM_FREQ 20000.0 // [Hz], PWMチョッピング周波数 pwm freq. (> 1/(DEAD_TIME*10))
+#define DEADTIME 0.0001 // [s], デッドタイム（モータに加える電圧の正負が変わるときに上下アーム短絡を避けるために同時オフする時間） // [s], deadtime to be set between plus volt. to/from minus
+#define fwdIN_PORT p21 //LED1 // ポート：fwdIN port for U phase fwdIN arm
+#define rvsIN_PORT p22 // ポート：rvsIN port for U phase rvsIN arm
+#define PWM_WAVEFORM 0 // PWM変調方式 0: ノコギリ波比較, 1: 三角波比較 saw tooth wave comparison, 1: triangler wave comparison
+#define TMIN 5 // [us], pwm_out()の演算時間 processing time of pwm_out()
+#define R_SHUNT_P_PORT p19 // ポート：電流検出用シャント抵抗のプラス側アナログ入力
+#define R_SHUNT_M_PORT p20 // ポート：電流検出用シャント抵抗のマイナス側アナログ入力
+#define R_SHUNT 0.47 // [Ω], 電流検出用シャント抵抗値 shunt resistanse
+//************* 使用するポートなどの設定 (ここまで) *****************
-typedef struct struct_pwm_parameters{ // parameters of H bridge pwm
- float duty; // 0-1, duty of H bridge
- unsigned char mode; // mode
- long upper_us; // [us], time
- long lower_us; // [us], time
- unsigned char fReverse[2]; // reverse direction?
- unsigned char fDeadtime; // set deadtime? (v is plus to/from minus?)
+typedef struct struct_pwm_parameters{ // フルブリッジのパラメータ宣言 parameters of H bridge pwm
+ float duty; // 0-1, PWMデューティ duty of H bridge
+ unsigned char mode; // チョッピングのオンオフを決定するモード
+ long fwdIN_us; // [us], fwdINをオンする時間幅
+ long rvsIN_us; // [us], rvsINをオンする時間幅
+ unsigned char fReverse; // モータ逆回転フラグ：回転方向が順方向のとき0、逆方向のとき1。[0]が現在の値、[1]はその前の値 reverse direction?
+ unsigned char fDeadtime; // 正負切替時にデッドタイムを要求するフラグ set deadtime? (v is plus to/from minus?)
}pwm_parameters;
-extern pwm_parameters IN; // H bridge pwm の定数、変数
+extern pwm_parameters IN; // フルブリッジのパラメータ宣言
-extern void start_pwm();
-extern void stop_pwm();
+extern void start_pwm(); // PWMスタートする関数を宣言
+extern void stop_pwm(); // PWMストップする関数を宣言
#endif
\ No newline at end of file
--- a/controller.cpp Fri Jan 04 12:00:48 2013 +0000
+++ b/controller.cpp Fri Mar 01 02:10:59 2013 +0000
@@ -1,17 +1,16 @@
-// BLDCmotor.cpp: 各種３相同期モータに対するセンサあり運転のシミュレーション
-// Kosaka Lab. 121215
-#include "mbed.h"
-#include "QEI.h"
+// controller.cpp: モータ制御器（位置制御、電流制御）
+#include "mbed.h" // mbedマイコンではstdio.hに相当
+#include "QEI.h" // エンコーダ用ライブラリを使用
-#include "controller.h"
-#include "Hbridge.h"
-Serial pc(USBTX, USBRX); // Display on tera term in PC
+#include "controller.h" // controller.cpp: モータ制御器（位置制御、電流制御）
+#include "Hbridge.h" // Hbridge.cpp: モータ駆動用ドライバのフルブリッジ（Ｈブリッジ）をＰＷＭ駆動する。
+Serial pc(USBTX, USBRX); // PCのモニタ上のtera termに文字を表示する宣言
motor_parameters p; // モータの定数、信号など
current_loop_parameters il; // 電流制御マイナーループの定数、変数
velocity_loop_parameters vl; // 速度制御メインループの定数、変数
-QEI encoder (CH_A, CH_B, NC, N_ENC, QEI::X4_ENCODING);
+QEI encoder (CH_A, CH_B, NC, N_ENC, QEI::X4_ENCODING); // エンコーダ用ライブラリを使用
// QEI(PinName channelA, mbed pin for channel A input.
// PinName channelB, mbed pin for channel B input.
// PinName index, mbed pin for channel Z input. (index channel input Z phase th=0), (pass NC if not needed).
@@ -29,268 +28,271 @@
// Read the number of revolutions recorded by the encoder on the index channel.
/*********** User setting for control parameters (end) ***************/
-AnalogOut analog_out(DA_PORT);
-AnalogIn VshuntR_Uplus(p19); // *3.3 [V], Volt of shunt R_SHUNT[Ohm]. The motor current i = v_shunt_r/R_SHUNT [A]
-AnalogIn VshuntR_Uminus(p20); // *3.3 [V], Volt of shunt R_SHUNT[Ohm]. The motor current i = v_shunt_r/R_SHUNT [A]
-AnalogIn VshuntR_Vplus(p16); // *3.3 [V], Volt of shunt R_SHUNT[Ohm]. The motor current i = v_shunt_r/R_SHUNT [A]
-AnalogIn VshuntR_Vminus(p17); // *3.3 [V], Volt of shunt R_SHUNT[Ohm]. The motor current i = v_shunt_r/R_SHUNT [A]
+AnalogOut analog_out(DA_PORT); // デバッグ用DA(アナログ信号をDA_PORTに出力)
-unsigned long _count; // sampling number
-float _time; // time[s]
-float debug[20]; // for debug
-float disp[10]; // for printf to avoid interrupted by quicker process
-DigitalOut led1(LED1);
-DigitalOut led2(LED2);
-DigitalOut led3(LED3);
-DigitalOut led4(LED4);
+unsigned long _countTS0; // TS0[s]ごとのカウント数
+float _time; // [s], プログラム開始時からの経過時間
+float debug[20]; // デバッグ用変数
+DigitalOut led1(LED1); // mbedマイコンのLED1を点灯
+DigitalOut led2(LED2); // mbedマイコンのLED2を点灯
+DigitalOut led3(LED3); // mbedマイコンのLED3を点灯
+DigitalOut led4(LED4); // mbedマイコンのLED4を点灯
-#ifdef GOOD_DATA
-float data[1000][5]; // memory to save data offline instead of "online fprintf".
-unsigned int count3; //
-unsigned int count2=(int)(TS2/TS0); //
-unsigned short _count_data=0;
-#endif
+
+float data[1000][5]; // PC上のmbed USB ディスクにセーブするデータ memory to save data offline instead of "online fprintf".
+//unsigned int count3; //
+//unsigned int count2=(int)(TS2/TS0); //
+unsigned short _countTS3=0;
+
DigitalOut debug_p26(p26); // p17 for debug
DigitalOut debug_p25(p25); // p17 for debug
//DigitalOut debug_p24(p24); // p17 for debug
//AnalogIn VCC(p19); // *3.3 [V], Volt of VCC for motor
//AnalogIn VCC2(p20); // *3.3 [V], Volt of (VCC-R i), R=2.5[Ohm]. R is for preventing too much i when deadtime is failed.
-unsigned short f_find_origin; // flag to find the origin of the rotor angle theta
-#if 1 //BUG!! if move sqrt2 to fast_math.h, sim starts and completed without working!?
-float sqrt2(float x){ // √xのx=1まわりのテイラー展開 √x = 1 + 1/2*(x-1) -1/4*(x-1)^2 + ...
-// return((1+x)*0.5); // 一次近似
- return(x+(1-x*x)*0.25); // 二次近似
-}
-#endif
-
-void init_parameters(){ // IPMSMの機器定数等の設定, 制御器の初期化
+void init_parameters(){
+// モータ機器定数等、モータ・制御器の初期値の設定
+// 親関数： main()
+// 子関数： なし
#ifdef SIMULATION
- p.L = 0.0063; // H
- p.R = 0.143; // Ω
- p.phi = 0.176; // V s
- p.Jm = 0.00018; // Nms^2
+ // モータ機器定数の設定
+ p.L = 0.0063; // [H], インダクタンス
+ p.R = 0.143; // [Ω], モータ巻線抵抗
+ p.phi = 0.176; // [V s], 永久磁石の鎖交磁束
+ p.Jm = 0.00018; // [Nms^2], イナーシャ
p.p = 2; // 極対数
#endif
- p.th[0] = p.th[1] = 0;
- p.w = 0;
- p.i = 0;
- p.v =0;
+ // モータの初期値の設定
+#ifdef SIMULATION
+ p.th[0] = p.th[1] = 0; // [rad], ロータの回転角, th[0]は現在の値, th[1]はTS0[s]だけ過去の値
+#else
+ encoder.reset(); // 現在の回転角を原点に設定（エンコーダのカウント数をゼロに設定） set encoder counter zero
+ p.th[0] = p.th[1] = (float)encoder.getPulses()/(float)N_ENC*2.0*PI; // [rad], ロータの回転角をエンコーダ検出値に設定, th[0]は現在の値, th[1]はTS0[s]だけ過去の値 get angle [rad] from encoder
+#endif
+ p.w = 0; // [rad/s], モータの回転速度
+ p.i = 0; // [A], モータ電流
+ p.v =0; // [V], モータ電圧
- p.w = 0;
-
- // 制御器の初期化
+ // 制御器の初期値の設定
vl.i_ref=0; // 電流指令[A]
vl.w_lpf = 0; // 検出した速度[rad/s]
- vl.eI_w = 0; // 速度制御用偏差の積分値（積分項）
- il.eI_i = 0; // 電流制御用偏差の積分値（積分項）
-#ifndef SIMULATION
- encoder.reset(); // set encoder counter zero
- p.th[0] = p.th[1] = (float)encoder.getPulses()/(float)N_ENC*2.0*PI; // get angle [rad] from encoder
-#endif
+ vl.eI = 0; // 速度制御用偏差の積分値（積分項）
+ vl.e_old = 0; // 速度制御用偏差の1サンプル過去の値
+ il.eI = 0; // 電流制御用偏差の積分値（積分項）
+ il.e_old = 0; // 電流制御用偏差の1サンプル過去の値
}
-void idq_control(){
-// ｄｑ座標電流PID制御器（電流マイナーループのフィードバック制御）
-// 入力：指令電流 i_ref [A], 実電流 p.i [A], PI制御積分項 eI, サンプル時間 TS0 [s]
-// 出力：電圧指令 v_ref [A]
- float e;
+void i_control(){
+// 電流PID制御器（電流マイナーループのフィードバック制御）
+// 親関数： current_loop()
+// 子関数： なし
+// 入力：指令電流 il.i_ref [A], 実電流 p.i [A], PID制御積分項 il.eI, サンプル時間 TS0 [s]
+// 出力：電圧指令 il.v_ref [A]
+ float e, ed;
-//debug[0]=il.i_ref;
- // ｄｑ電流偏差の計算
- e = il.i_ref - p.i;
+ e = il.i_ref - p.i; // 電流偏差の計算
+
+ il.eI = il.eI + TS0*e; // 電流偏差の積分項の計算
- // ｄｑ電流偏差の積分項の計算
- il.eI_i = il.eI_i + TS0*e;
+ ed = (e-il.e_old)/TS0; // 電流偏差の微分値の計算
+ il.e_old = e; // 電流偏差の1サンプル過去の値を更新
- // ＰＩ制御
- il.v_ref = iKP*e + iKI*il.eI_i;
-
+ il.v_ref = iKP*e + iKI*il.eI + iKD*ed; // ＰＩＤ制御器の出力を計算
}
-void current_loop(){ // 電流制御マイナーループ
- // 電流センサによってiu, iv を検出
-#ifndef SIMULATION
- p.i = (VshuntR_Uplus - VshuntR_Uminus) /R_SHUNT; // get i [A] from shunt resistance;
-#endif
-//debug[0]=p.i;
- // dq電流制御（電流フィードバック制御）
-debug[0]=il.i_ref;
+void current_loop(){
+// 電流制御マイナーループ、サンプル時間TS0秒
+// 親関数： timerTS0()
+// 子関数： i_control()
+// 入力：指令電流 il.i_ref [A], 実電流 p.i [A]
+// 出力：電圧指令 p.v [V]
+
#ifdef USE_CURRENT_CONTROL
- idq_control();
+ i_control(); // 電流制御（電流フィードバック制御）
#else
- il.v_ref = il.i_ref/iMAX*vMAX;
+ il.v_ref = il.i_ref/iMAX*vMAX; // 速度制御の出力をそのまま電圧指令にする
#endif
// 電圧指令の大きさをMAX制限
- // anti-windup: if u=v_ref is saturated, then reduce eI.
- if( il.v_ref > vMAX ){
- il.eI_i -= (il.v_ref - vMAX)/iKI; if( il.eI_i<0 ){ il.eI_i=0;}
- il.v_ref = vMAX;
- }else if( il.v_ref < -vMAX ){
- il.eI_i -= (il.v_ref + vMAX)/iKI; if( il.eI_i>0 ){ il.eI_i=0;}
- il.v_ref = -vMAX;
+ // アンチワインゴアップ：制御入力v_refが飽和したとき積分項eIを減衰させる anti-windup: if u=v_ref is saturated, then reduce eI.
+ if( il.v_ref > vMAX ){ // 電圧指令がプラス側に大きすぎるとき
+ il.eI -= (il.v_ref - vMAX)/iKI; // 電圧指令がvMAXと等しくなる積分項を計算
+ if( il.eI<0 ){ il.eI=0;} // 積分項が負になるとゼロにする
+ il.v_ref = vMAX; // 電圧指令をvMAXにする
+ }else if( il.v_ref < -vMAX ){ // 電圧指令がマイナス側に大きすぎるとき
+ il.eI -= (il.v_ref + vMAX)/iKI; // 電圧指令が-vMAXと等しくなる積分項を計算
+ if( il.eI>0 ){ il.eI=0;} // 積分項が正になるとゼロにする
+ il.v_ref = -vMAX; // 電圧指令を-vMAXにする
}
- p.v = il.v_ref;
-
- p.th[1] = p.th[0]; // thを更新
+ p.v = il.v_ref; // 指令電圧をp.vに格納してv2Hbridge()に渡す
}
void vel_control(){
// 速度制御器：速度偏差が入力され、q軸電流指令を出力。
-// 入力：指令速度 w_ref [rad/s], 実速度 w_lpf [rad/s], PI制御積分項 eI, サンプル時間 TS1 [s]
-// 出力：電流指令 i_ref [A]
- float e;
+// 入力：指令速度 vl.w_ref [rad/s], 実速度 vl.w_lpf [rad/s], PI制御積分項 vl.eI, サンプル時間 TS1 [s]
+// 出力：電流指令 vl.i_ref [A]
+ float e, ed;
+
+ e = vl.w_ref - vl.w_lpf; // 速度偏差の計算
- // 速度偏差の計算
- e = vl.w_ref - vl.w_lpf;
+ vl.eI = vl.eI + TS1*e; // 速度偏差の積分値の計算
- // 速度偏差の積分値の計算
- vl.eI_w = vl.eI_w + TS1*e;
+ ed = (e-vl.e_old)/TS1; // 速度偏差の微分値の計算
+ vl.e_old = e; // 速度偏差の1サンプル過去の値を更新
- // ＰＩ制御
- vl.i_ref = wKp*e + wKi*vl.eI_w;
+ vl.i_ref = wKp*e + wKi*vl.eI + wKd*ed; // ＰＩＤ制御器の出力を計算
}
-void velocity_loop(){ // 速度制御メインループ(w_ref&beta_ref to idq_ref)
+void velocity_loop(){
+// 速度制御メインループ、サンプル時間TS1秒
+// 親関数： timerTS1()
+// 子関数： vel_control()
+// 入力：指令速度 vl.w_ref [rad/s], 実速度 vl.w_lpf [rad/s]
+// 出力：電圧指令 il.i_ref [A]
float tmp;
+#if 1
// 速度ωを求めるために、位置θをオイラー微分して、一次ローパスフィルタに通す。
-#if 1
- tmp = p.th[0]-p.th[1];
- while( tmp> PI ){ tmp -= 2*PI;}
- while( tmp<-PI ){ tmp += 2*PI;}
- vl.w_lpf = iLPF*vl.w_lpf + tmp/TS0 *(1-iLPF);
+ tmp = p.th[0]-p.th[1]; // 回転角の差分を取る
+ while( tmp> PI ){ tmp -= 2*PI;} // 差分の値域を-π～+πに設定
+ while( tmp<-PI ){ tmp += 2*PI;} // 〃
+ vl.w_lpf = iLPF*vl.w_lpf + tmp/TS0 *(1-iLPF); // オイラー微分近似＋１次ＬＰＦで現在速度を計算
#else
vl.w_lpf = p.th[0];
#endif
- // 速度制御：速度偏差が入力され、電流指令を出力。
- vel_control();
+
+ vel_control(); // 速度制御：速度偏差が入力され、電流指令を出力。
- // 電流指令のMAX制限（異常に大きい指令値を制限する）
- // anti-windup: if u=i_ref is saturated, then reduce eI.
- if( vl.i_ref > iMAX ){
- vl.eI_w -= (vl.i_ref - iMAX)/wKi; if( vl.eI_w<0 ){ vl.eI_w=0;}
- vl.i_ref = iMAX;
- }else if( vl.i_ref < -iMAX ){
- vl.eI_w -= (vl.i_ref + iMAX)/wKi; if( vl.eI_w>0 ){ vl.eI_w=0;}
- vl.i_ref = -iMAX;
+ // 電流指令の大きさをMAX制限
+ // アンチワインドアップ：制御入力i_refが飽和したとき積分項eIを減衰させる anti-windup: if u=v_ref is saturated, then reduce eI.
+ if( vl.i_ref > iMAX ){ // 電流指令がプラス側に大きすぎるとき
+ vl.eI -= (vl.i_ref - iMAX)/wKi; // 電流指令がvMAXと等しくなる積分項を計算
+ if( vl.eI<0 ){ vl.eI=0;} // 積分項が負になるとゼロにする
+ vl.i_ref = iMAX; // 電流指令をvMAXにする
+ }else if( vl.i_ref < -iMAX ){ // 電流指令がマイナス側に大きすぎるとき
+ vl.eI -= (vl.i_ref + iMAX)/wKi; // 電流指令が-vMAXと等しくなる積分項を計算
+ if( vl.eI>0 ){ vl.eI=0;} // 積分項が正になるとゼロにする
+ vl.i_ref = -iMAX; // 電流指令を-vMAXにする
}
-//debug[0]=vl.eI_w;
- // 電流の目標値を速度制御メインループから電流制御マイナーループへ渡す。
- il.i_ref = vl.i_ref;
-//debug[0]=il.i_ref;
+ il.i_ref = vl.i_ref; // 電流の目標値を速度制御メインループから電流制御マイナーループへ渡す。
}
-void v2Hbridge(){ // vより、PWMを発生
+void v2Hbridge(){
+// 指令電圧vより、PWM関数pwm_out()のパラメータ（dutyとフラグ）をセット。
+// 親関数： timerTS0()
+// 子関数： なし
+// 入力：電圧指令 p.v [V]
+// 出力：フルブリッジのfwdIN, rvsIN用duty,
+// デッドタイムフラグfDeadtime, モータ逆回転フラグfReverse
float duty;
-// duty = (p.v/vMAX+1)*0.5;
-// IN.duty = duty;
- duty = p.v/vMAX;
- if( duty>=0 ){
- IN.duty = duty;
- if( IN.fReverse[0]==1 ){
- IN.fDeadtime = 1;
+ duty = p.v/vMAX; // 指令電圧p.vの値を最大電圧vMAXで割って-1～1にしてdutyに代入
+ if( duty>=0 ){ // dutyがプラスでモータが順回転のとき
+ IN.duty = duty; // dutyをPWM関数pwm_out()に渡す
+ if( IN.fReverse==1 ){ // モータが逆回転から順回転に切り替ったとき
+ IN.fDeadtime = 1; // デッドタイム要求フラグをオンにする
}
- IN.fReverse[0] = 0;
- }else{
- IN.duty = -duty;
- if( IN.fReverse[0]==0 ){
- IN.fDeadtime = 1;
+ IN.fReverse = 0; // 逆回転フラグをオフにする
+ }else{ // dutyがマイナスでモータが逆回転のとき
+ IN.duty = -duty; // dutyの絶対値をPWM関数pwm_out()に渡す
+ if( IN.fReverse==0 ){ // モータが順回転から逆回転に切り替ったとき
+ IN.fDeadtime = 1; // デッドタイム要求フラグをオンにする
}
- IN.fReverse[0] = 1;
+ IN.fReverse = 1; // 逆回転フラグをオンにする
}
}
#ifdef SIMULATION
void sim_motor(){
-// モータシミュレータ
+// ブラシ付きDCモータシミュレータ
// 入力信号：電圧p.v [V]、負荷トルクp.TL [Nm]
// 出力信号：モータ角度p.th[0] [rad], モータ速度p.w [rad/s], モータ電流p.i [A]
float i_dot, Tall, TL;
analog_out=p.v/100.+0.5;//debug
//debug[0]=p.v;
- // get i
- i_dot = (1.0/p.L) * ( p.v - (p.R*p.i + p.w*p.phi) );
- p.i = p.i + TS0*i_dot;
+ // 電圧方程式より、指令電圧から電流を計算
+ i_dot = (1.0/p.L) * ( p.v - (p.R*p.i + p.w*p.phi) ); // 電圧方程式より電流の微分値を計算
+ p.i = p.i + TS0*i_dot; // オイラー近似微分で電流の微分値を積分して電流を求める
- // モータトルクの計算
+ // トルク方程式より、電流からモータトルクを計算
p.Tm = p.p * p.phi * p.i;
// モータ速度ωの計算
- if( abs(p.w) > 5*2*PI )
- if( p.w>=0 ) TL= p.TL;
- else TL=-p.TL;
- else
+ if( abs(p.w) > 5*2*PI ){ // 速度が5rps以上のとき、減速するように負荷トルクTLをセット
+ if( p.w>=0 ){ TL= p.TL;} // 速度が正なら負荷トルクを+TLにセット
+ else{ TL=-p.TL;} // 速度が負なら負荷トルクを-TLにセット
+ }else{ // 速度が5rps以下のとき、速度に比例した負荷トルクをセット
TL = p.w/(5*2*PI)*p.TL;
+ }
- Tall = p.Tm - TL;
- p.w = p.w + TS0 * (1.0 / p.Jm) * Tall;
+ Tall = p.Tm - TL; // モータのシャフトにかかるトルクを計算
+ p.w = p.w + TS0 * (1.0 / p.Jm) * Tall; // モータの機械的特性をシミュレートしてトルクから速度を計算
// モータ角度θの計算
- p.th[0] = p.th[0] + TS0 * p.w;
- if( p.th[0]>2*PI)
- p.th[0] = p.th[0] - 2*PI;
+ p.th[0] = p.th[0] + TS0 * p.w; // オイラー近似微分で速度を積分して回転角を計算
- if( p.th[0]<0 )
- p.th[0] = p.th[0] + 2*PI;
-//debug[0]=p.v;
+ // 回転角の値域を０～２πにする
+ if( p.th[0]>2*PI) // 回転角が２π以上のとき
+ p.th[0] = p.th[0] - 2*PI; // ２π引く
+ if( p.th[0]<0 ) // 回転角が負のとき
+ p.th[0] = p.th[0] + 2*PI; // ２π足す
}
#endif
-void data2mbedUSB(){ // save data to mbed USB drive
- if( _count_data<1000 ){
- data[_count_data][0]=p.th[0]/*vl.w_ref*/; data[_count_data][1]=debug[0];
- data[_count_data][2]=vl.w_lpf; data[_count_data][3]=_count*TS0; data[_count_data][4]=il.v_ref;
- _count_data++;
+void data2mbedUSB(){ // PC上のmbed USB ディスクにセーブするためのデータをTS3[s]ごとに代入 save data to mbed USB drive
+ if( _countTS3<1000 ){ // データ数が1,000の5種類のデータをメモリーに貯める
+ data[_countTS3][0]=p.th[0]/*vl.w_ref*/; data[_countTS3][1]=debug[0];
+ data[_countTS3][2]=vl.w_lpf; data[_countTS3][3]=_countTS0*TS0; data[_countTS3][4]=il.v_ref;
+ _countTS3++;
}
#if 0
- if( _count_data<500 ){
+ if( _countTS3<500 ){
debug[0]=p.vab[0]; debug[1]=p.vab[1]; debug[2]=il.vdq_ref[0]; debug[3]=il.vdq_ref[1]; debug[4]=p.iab[0];
debug[5]=p.iab[1]; debug[6]=p.vuvw[0]; debug[7]=p.vuvw[1]; debug[8]=p.vuvw[2]; debug[9]=p.iuvw[0];
debug[10]=p.iuvw[1]; debug[11]=p.iuvw[2]; debug[12]=p.idq[0]; debug[13]=p.idq[1]; debug[14]=p.TL;
- debug[15]=p.Tm; debug[16]=p.w; debug[17]=vl.w_lpf; debug[18]=p.th[0]; debug[19]=_count*TS0;//_time;
+ debug[15]=p.Tm; debug[16]=p.w; debug[17]=vl.w_lpf; debug[18]=p.th[0]; debug[19]=_countTS0*TS0;//_time;
//BUG for(j=0;j<19;j++){ fprintf( fp, "%f, ",debug[j]);} fprintf( fp, "%f\n",debug[19]);
for(j=0;j<19;j++){ printf("%f, ",debug[j]);} printf("%f\n",debug[19]);
// for(j=0;j<19;j++){ pc.printf("%f, ",debug[j]);} pc.printf("%f\n",debug[19]);
}
#endif
}
-void timerTS0(){ // timer called every TS0[s].
- debug_p26 = 1;
- _count++;
- _time += TS0;
+void timerTS0(){ // タイマーtimerTS0()はTS0[s]ごとにコールされる timer called every TS0[s].
+// debug_p26 = 1;
+ _countTS0++; // カウンターに1足す
+ _time += TS0; // 現在の時間にTS0[s]足す
- current_loop(); // 電流制御マイナーループ(i_ref to volt)
- v2Hbridge(); // volt. to Hbridge
- #ifdef SIMULATION
-//debug[0]=p.v;
- // モータシミュレータ
- sim_motor(); // IPM, ｄｑ座標
- #else
- p.th[1] = p.th[0];
- p.th[0] = (float)encoder.getPulses()/(float)N_ENC*2.0*PI; // get angle [rad] from encoder
- #endif
- debug_p26 = 0;
-}
-void timerTS1(void const *argument){
- debug_p25 = 1;
- velocity_loop(); // 速度制御メインループ(w_ref&beta_ref to idq_ref)
- debug_p25 = 0;
+ current_loop(); // 電流制御マイナーループ(指令電流i_refからPID制御で指令電圧を計算)
+ v2Hbridge(); // 指令電圧を見てPWM関数pwm_out()のパラメータを決める volt. to Hbridge
+
+ // モータ回転角の検出
+ p.th[1] = p.th[0]; // // 1サンプル前の回転角p.th[1]を更新
+#ifdef SIMULATION
+ sim_motor(); // モータシミュレータ
+#else
+ p.th[0] = (float)encoder.getPulses()/(float)N_ENC*2.0*PI; // エンコーダで回転角を検出 get angle [rad] from encoder
+#endif
+// debug_p26 = 0;
}
-void display2PC(){ // display to tera term on PC
- pc.printf("%8.1f[s]\t%8.2f[V]\t%8.2f [Hz]\t%8.2f\t%8.2f\r\n", _time, il.v_ref, vl.w_lpf/(2*PI), vl.w_ref/(2*PI), debug[0]); // print to tera term
+void timerTS1(void const *argument){ // タイマーtimerTS1()はTS1[s]ごとにコールされる
+// debug_p25 = 1;
+ velocity_loop(); // 速度制御メインループ(指令速度w_refから指令電流i_refを求める)
+// debug_p25 = 0;
+}
+
+void display2PC(){ // PCのモニタ上のtera termに諸量を表示 display to tera term on PC
+ pc.printf("%8.1f[s]\t%8.2f[V]\t%8.2f [Hz]\t%8.2f\t%8.2f\r\n",
+ _time, il.v_ref, vl.w_lpf/(2*PI), vl.w_ref/(2*PI), debug[0]);
// pc.printf("%8.1f[s]\t%8.5f[V]\t%4d [deg]\t%8.2f\r\n", _time, _u, (int)(_th/(2*PI)*360.0), _r);//debug[0]*3.3/R_SHUNT); // print to tera term
}
-void timerTS2(){
+void timerTS2(){ // タイマーtimerTS2()はTS2[s]ごとにコールされる
}
-void timerTS3(){
- data2mbedUSB(); // data2mbedUSB() is called every TS3[s].
+void timerTS3(){ // タイマーtimerTS3()はTS3[s]ごとにコールされる
+ data2mbedUSB(); // PC上のmbed USB ディスクにセーブするためのデータをTS3[s]ごとに代入 data2mbedUSB() is called every TS3[s].
}
-void timerTS4(){
- display2PC(); // display to tera term on PC. display2PC() is called every TS4[s].
+void timerTS4(){ // タイマーtimerTS4()はTS4[s]ごとにコールされる
+ display2PC(); // PCのモニタ上のtera termに文字を表示 display to tera term on PC. display2PC() is called every TS4[s].
}
--- a/controller.h Fri Jan 04 12:00:48 2013 +0000
+++ b/controller.h Fri Mar 01 02:10:59 2013 +0000
@@ -1,58 +1,60 @@
#ifndef __controller_h
#define __controller_h
-#define PI 3.14159265358979 // def. of PI
-/*********** User setting for control parameters (begin) ***************/
-//#define SIMULATION // Comment this line if not simulation
-//#define USE_CURRENT_CONTROL // Current control on. Comment if current control off.
-#define CONTROL_MODE 0 // 0:PID control, 1:Frequency response, 2:Step response, 3. u=Rand to identify G(s), 4) FFT identification
-#define DEADZONE_PLUS 0//1. // deadzone of plus side
-#define DEADZONE_MINUS 0//-1.5 // deadzone of minus side
-#define GOOD_DATA // Comment this line if the length of data TMAX/TS2 > 1000
- // encoder
-#define N_ENC (24*4) // "*4": QEI::X4_ENCODING. Number of pulses in one revolution(=360 deg) of rotary encoder.
-#define CH_A p29 // A phase port
-#define CH_B p30 // A phase port
+#define PI 3.14159265358979 // 円周率πの定義
+
+/*********** 使用するポート、サンプル時間、制御ゲインなどの設定 (ここから) ***************/
+//#define SIMULATION // ブラシ付DCモータの特性をシミュレーションするとき、コメント外す
+//#define USE_CURRENT_CONTROL // 電流制御ありのとき、コメント外す Current control on. Comment if current control off.
+
+//#define DEADZONE_PLUS 0//1. // deadzone of plus side
+//#define DEADZONE_MINUS 0//-1.5 // deadzone of minus side
-#define DA_PORT p18 // analog out (DA) port of mbed
+ // エンコーダの設定
+#define N_ENC (24*4) // エンコーダ分解能（4逓倍）＝1回転分のパルス数×４ "*4": QEI::X4_ENCODING. Number of pulses in one revolution(=360 deg) of rotary encoder.
+#define CH_A p29 // A相用ポート A phase port
+#define CH_B p30 // B相用ポート A phase port
-#define PWM_FREQ 20000.0 //[Hz], pwm freq. (> 1/(DEAD_TIME*10))
-#define DEADTIME 0.0001 // [s], deadtime to be set between plus volt. to/from minus
-#define TS0 0.002//0.001 // [s], sampling time (priority highest: Ticker IRQ) of motor current i control PID using timer interrupt
-#define TS1 0.002//0.01 // [s], sampling time (priority high: RtosTimer) of motor angle th PID using rtos-timer
-#define TS2 0.2 // [s], sampling time (priority =main(): precision 4ms) to save data to PC using thread. But, max data length is 1000.
-#define TS3 0.002 // [s], sampling time (priority low: precision 4ms)
-#define TS4 0.2 // [s], sampling time (priority lowest: precision 4ms) to display data to PC tera term
+ // タイマーのサンプル周期等
+//#define PWM_FREQ 20000.0 // [Hz], PWMチョッピング周波数 pwm freq. (> 1/(DEAD_TIME*10))
+//#define DEADTIME 0.0001 // [s], デッドタイム（モータに加える電圧の正負が変わるときに上下アーム短絡を避けるために同時オフする時間） // [s], deadtime to be set between plus volt. to/from minus
+#define TS0 0.002 // [s], timerTS0のサンプル時間（電流制御用） sampling time (priority highest: Ticker IRQ) of motor current i control PID using timer interrupt
+#define TS1 0.002 // [s], timerTS1のサンプル時間（位置制御用） sampling time (priority high: RtosTimer) of motor angle th PID using rtos-timer
+#define TS2 0.2 // [s], timerTS2のサンプル時間（不使用） sampling time (priority =main(): precision 4ms) to save data to PC using thread. But, max data length is 1000.
+#define TS3 0.002 // [s], timerTS3のサンプル時間（データセーブ用） sampling time (priority low: precision 4ms)
+#define TS4 0.2 // [s], timerTS4のサンプル時間（モニタ表示用） sampling time (priority lowest: precision 4ms) to display data to PC tera term
//void timerTS1(void const *argument), CallTimerTS3(void const *argument), CallTimerTS4(void const *argument);
// RtosTimer RtosTimerTS1(timerTS1); // RtosTimer priority is osPriorityAboveNormal, just one above main()
// Thread ThreadTimerTS3(CallTimerTS3,NULL,osPriorityBelowNormal);
// Thread ThreadTimerTS4(CallTimerTS4,NULL,osPriorityLow);
-#define TMAX 5.0 // [s], experiment starts from 0[s] to TMAX[s]
+#define TMAX 5.0 // [s], プログラム開始から終了までの時間 experiment starts from 0[s] to TMAX[s]
+#define N_DATA 1000 // PC上のmbed USB ディスクにセーブするデータの数
// 電流制御マイナーループ
-#define iKP 10./2 // 電流制御PIDのPゲイン
-#define iKI 100./2 // 電流制御PIDのIゲイン
-
-#define vMAX 3.3
+#define iKP 10./2 // [V/A], 電流制御PIDのPゲイン
+#define iKI 100./2 // [V/A s], 電流制御PIDのIゲイン
+#define iKD 0 // [V/A/s], 電流制御PIDのDゲイン
+#define vMAX 3.3 // [V], 指令電圧の最大値（超えるとこの値に制限する）
// 速度制御メインループ
-#ifdef USE_CURRENT_CONTROL
- #define wKp 0.05 // 速度制御PIDのPゲイン
- #define wKi 2.50 // 速度制御PIDのIゲイン
-#else
- #define wKp 0.05 // 速度制御PIDのPゲイン
- #define wKi 0.5//2.50 // 速度制御PIDのIゲイン
+#ifdef USE_CURRENT_CONTROL // 電流制御ありのとき
+ #define wKp 0.05 // [A/(rad/s)], 速度制御PIDのPゲイン
+ #define wKi 2.50 // [A/(rad/s) s], 速度制御PIDのIゲイン
+ #define wKd 0 // [A/(rad/s)/s], 速度制御PIDのDゲイン
+#else // 電流制御なしのとき
+ #define wKp 0.05 // [A/(rad/s)], 速度制御PIDのPゲイン
+ #define wKi 0.5//2.50 // [A/(rad/s) s], 速度制御PIDのIゲイン
+ #define wKd 0 // [A/(rad/s)/s], 速度制御PIDのDゲイン
#endif
+#define iLPF 0.95 // 0-1, 速度に対する1次LPFの強さ; Low Pass Filter, G(z)=(1-a)/(z-a)
+#define iMAX 3.3 // [A], 電流指令の最大値
-#define iLPF 0.95 // 0-1, 速度に対する1次LPF; Low Pass Filter, G(z)=(1-a)/(z-a)
-#define iMAX 3.3 // [A], ｑ軸電流指令のMAX制限（異常に大きい指令値を制限する）
-
-#define R_SHUNT 1.25 // [Ohm], shunt resistanse
-/*********** User setting for control parameters (end) ***************/
+#define DA_PORT p18 // デバッグ用DAポート analog out (DA) port of mbed
+/*********** 使用するポートやサンプル時間、制御ゲインなどの設定 (ここから) ***************/
+ // モータの定数、信号などの宣言
typedef struct struct_motor_parameters{
- // モータの定数、信号など
#ifdef SIMULATION // シミュレーションのとき
float L; // [H], インダクタンス
float R; // [Ω], モータ巻線抵抗
@@ -60,47 +62,49 @@
float Jm; // [Nms^2], イナーシャ
float Tm; // [Nm], モータトルク
float TL; // [Nm], 負荷トルク
+ float p; // 極対数
#endif
- float th[2]; // [rad]. ロータの位置, th[0]=th(t), th[1]=th(t-TS0)
- float w; // [rad/s], モータ速度
+ float th[2]; // [rad], モータの回転角, th[0]は現在の値, th[1]はTS0[s]だけ過去の値
+ float w; // [rad/s], モータの回転速度
float w_lpf; // [rad/s], フィルタで高周波ノイズを除去したモータ速度
- float i; // [A], αβ軸電流 iab = [iα;iβ];
- float v; // [V], motor 電圧
- float p; // 極対数
+ float i; // [A], モータ電流
+ float v; // [V], モータ電圧
}motor_parameters;
+ // 電流制御マイナーループの定数、変数の宣言
typedef struct struct_current_loop_parameters{
- // 電流制御マイナーループの定数、変数
- float i_ref; // iの目標値
- float v_ref; // vdqの目標値
- float eI_i; // 電流制御用偏差の積分値（積分項）
+ float i_ref; // iの目標値
+ float v_ref; // vの目標値
+ float eI; // 電流制御用偏差の積分値（積分項）
+ float e_old; // 電流制御用偏差の1サンプル過去の値
}current_loop_parameters;
+ // 速度制御メインループの定数、変数の宣言
typedef struct struct_velocity_loop_parameters{
- // 速度制御メインループの定数、変数
float w_lpf; // [rad/s], モータ速度(LPF通過後)
float w_ref; // [rad/s], モータ目標速度
- float tan_beta_ref; // [rad], モータ電流位相
float i_ref; // 電流指令[A]
- float eI_w; // 速度制御用偏差の積分値（積分項）
+ float eI; // 速度制御用偏差の積分値（積分項）
+ float e_old; // 速度制御用偏差の1サンプル過去の値
}velocity_loop_parameters;
-extern void timerTS0(); // timer called every TS0[s].
-extern void timerTS1(void const *argument); // timer called every TS1[s].
-extern void timerTS2(); // timer called every TS2[s].
-extern void timerTS3(); // timer called every TS3[s].
-extern void timerTS4(); // timer called every TS4[s].
+ // タイマー宣言
+extern void timerTS0(); // TS0[s]ごとにコールされるタイマー timer called every TS0[s].
+extern void timerTS1(void const *argument); // TS1[s]ごとにコールされるタイマー timer called every TS1[s].
+extern void timerTS2(); // TS2[s]ごとにコールされるタイマー timer called every TS2[s].
+extern void timerTS3(); // TS3[s]ごとにコールされるタイマー timer called every TS3[s].
+extern void timerTS4(); // TS4[s]ごとにコールされるタイマー timer called every TS4[s].
-extern void init_parameters(); // IPMSMの機器定数等の設定, 制御器の初期化
+extern void init_parameters(); // モータの機器定数等の設定, 制御器の初期化する関数の宣言
-extern unsigned long _count; // sampling number
-extern float _time; // time[s]
+extern unsigned long _countTS0; // TS0[s]ごとのカウント数
+extern float _time; // [s], プログラム開始時からの経過時間
extern motor_parameters p; // モータの定数、信号など
extern current_loop_parameters il; // 電流制御マイナーループの定数、変数
extern velocity_loop_parameters vl; // 速度制御メインループの定数、変数
-extern float data[][5]; // memory to save data offline instead of "online fprintf".
-extern unsigned short _count_data; // counter for data[1000][5]
+extern float data[][5]; // PC上のmbed USB ディスクにセーブするデータ // memory to save data offline instead of "online fprintf".
+extern unsigned short _countTS3_data; // data[i][5]のカウンタ
#endif
\ No newline at end of file
--- a/main.cpp Fri Jan 04 12:00:48 2013 +0000
+++ b/main.cpp Fri Mar 01 02:10:59 2013 +0000
@@ -1,65 +1,75 @@
-// DC motor control program using H-bridge driver (ex. TA7291P) and 360 resolution rotary encoder with A, B phase.
-// ver. 121224 by Kosaka lab.
-#include "mbed.h"
-#include "rtos.h"
+// DC motor control program using H-bridge driver (ex. TA7291P) and incremental rotary encoder with A, B phase.
+// ブラシ付DCモータ制御マイコンプログラム・・・Hブリッジ(例えばTA7291P)＋インクリメンタル型AB２相エンコーダ（例えば1回転24パルス）
+// ver. 130214 by Kosaka lab.
+//
+// main.cpp: 優先度の異なるつぎのタイマー処理を生成。
+// 優先度| コールされる関数名| サンプル時間[s]| 割込み方法| 用途
+// ---------------------------------------------------------
+// 最高 | timerTS0()| TS0| ハードウェアタイマー | 電流制御
+// ２位 | timerTS1()| TS1| RTOSタイマー | 位置制御または速度制御
+// ３位 | timerTS2()| TS2| RTOSスレッド | 不使用 (main()と同じ優先度)
+// ４位 | timerTS3()| TS3| RTOSスレッド | データセーブ
+// 最低 | timerTS4()| TS4| RTOSスレッド | PCモニタ表示
+#include "mbed.h" // mbedマイコンではstdio.hに相当
+#include "rtos.h" // リアルタイムOS用
-#include "controller.h"
-#include "Hbridge.h"
+#include "controller.h" // ブラシ付DCモータの位置制御器と電流制御器用
+#include "Hbridge.h" // Hブリッジ用
-Serial pc2(USBTX, USBRX); // Display on tera term in PC
-LocalFileSystem local("mbedUSBdrive"); // save data to mbed USB disk drive in PC
-Ticker TickerTimerTS0; // Timer interrupt using TIMER3, TS<0.001 is OK. Priority is higher than rtosTimer.
-unsigned char fTimerTS2ON=0, fTimerTS3ON=0, fTimerTS4ON=0; // ON/OFF flag for timerTS2, 3, 4.
+Serial pc2(USBTX, USBRX); // PCのモニタ上のtera termに文字を表示する宣言
+LocalFileSystem local("mbedUSBdrive"); // PCのmbed USB ディスク上にデータをセーブする宣言
+Ticker TickerTimerTS0; // タイマー割込みを宣言。1ms以下もOK。RTOSよりも優先度が高い
+unsigned char fTimerTS2ON=0, fTimerTS3ON=0, fTimerTS4ON=0; // timerTS2, TS3, TS4のスタート・ストップ指定フラグ
//DigitalOut debug_p24(p24); // p17 for debug
-extern "C" void mbed_reset();
+//extern "C" void mbed_reset(); // mbedマイコンをリセットする関数の宣言
-void CallTimerTS2(void const *argument) { // make sampling time TS3 timer (priority 3: precision 4ms)
- int ms;
- unsigned long c;
- while (true) {
- c = _count;
- if( fTimerTS2ON ){
- timerTS2(); // timerTS2() is called every TS2[s].
+void CallTimerTS2(void const *argument) { // タイマーtimerTS2()をTS2ごとにコール make sampling time TS3 timer (priority 3: precision 4ms)
+ int ms; // [ms], 処理時間
+ unsigned long c; // カウンタ
+ while (true) { // 永遠に繰り返す
+ c = _countTS0; // timerTS0()によるカウント数cを記憶
+ if( fTimerTS2ON ){ // タイマースタートフラグがオンのとき
+ timerTS2(); // timerTS2()をTS2[s]ごとにコールする is called every TS2[s].
}
- if( (ms=(int)(TS2*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;}
- Thread::wait(ms);
+ if( (ms=(int)(TS2*1000-(_countTS0-c)*TS0*1000))<=0 ){ ms=1;} // c記憶時点から今まで時間を計算してTS2から引く。それが負なら1msに設定
+ Thread::wait(ms); // while()内の処理がTS2[s]ごとに実行されるようにms[ms]待つ
}
}
-void CallTimerTS3(void const *argument) { // make sampling time TS3 timer (priority 3: precision 4ms)
- int ms;
- unsigned long c;
- while (true) {
- c = _count;
- if( fTimerTS3ON ){
- timerTS3(); // timerTS3() is called every TS3[s].
+void CallTimerTS3(void const *argument) { // タイマーtimerTS3()をTS3ごとにコール make sampling time TS3 timer (priority 3: precision 4ms)
+ int ms; // [ms], 処理時間
+ unsigned long c; // カウンタ
+ while (true) { // 永遠に繰り返す
+ c = _countTS0; // timerTS0()によるカウント数cを記憶
+ if( fTimerTS3ON ){ // タイマースタートフラグがオンのとき
+ timerTS3(); // timerTS3()をTS3[s]ごとにコールする is called every TS3[s].
}
- if( (ms=(int)(TS3*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;}
- Thread::wait(ms);
+ if( (ms=(int)(TS3*1000-(_countTS0-c)*TS0*1000))<=0 ){ ms=1;} // c記憶時点から今まで時間を計算してTS3から引く。それが負なら1msに設定
+ Thread::wait(ms); // while()内の処理がTS3[s]ごとに実行されるようにms[ms]待つ
}
}
-void CallTimerTS4(void const *argument) { // make sampling time TS4 timer (priority 4: precision 4ms)
- int ms;
- unsigned long c;
- while (true) {
- c = _count;
- if( fTimerTS4ON ){
- timerTS4(); // timerTS4() is called every TS4[s].
+void CallTimerTS4(void const *argument) { // タイマーtimerTS4()をTS4ごとにコール make sampling time TS4 timer (priority 4: precision 4ms)
+ int ms; // [ms], 処理時間
+ unsigned long c; // カウンタ
+ while (true) { // 永遠に繰り返す
+ c = _countTS0; // timerTS0()によるカウント数cを記憶
+ if( fTimerTS4ON ){ // タイマースタートフラグがオンのとき
+ timerTS4(); // timerTS4()をTS4[s]ごとにコールする is called every TS4[s].
}
- if( (ms=(int)(TS4*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;}
- Thread::wait(ms);
+ if( (ms=(int)(TS4*1000-(_countTS0-c)*TS0*1000))<=0 ){ ms=1;} // c記憶時点から今まで時間を計算してTS4から引く。それが負なら1msに設定
+ Thread::wait(ms); // while()内の処理がTS4[s]ごとに実行されるようにms[ms]待つ
}
}
-//#define OLD
-int main(){
+//#define OLD // タイマーを使わないシミュレーションをするときコメント外す
+int main(){ // モータ制御プログラム本体：マイコン起動時に最初にコールされる
unsigned short i;
- FILE *fp = fopen("/mbedUSBdrive/data.csv", "w"); // save data to PC
- RtosTimer RtosTimerTS1(timerTS1); // RtosTimer priority is osPriorityAboveNormal, just one above main()
- Thread ThreadTimerTS3(CallTimerTS3,NULL,osPriorityBelowNormal);
- Thread ThreadTimerTS4(CallTimerTS4,NULL,osPriorityLow);
+ FILE *fp = fopen("/mbedUSBdrive/data.csv", "w"); // PC上のmbed USB ディスクにデータをセーブするためにディスクをオープン
+ RtosTimer RtosTimerTS1(timerTS1); // timerTS1のためのRTOSタイマーを宣言
+ Thread ThreadTimerTS3(CallTimerTS3, NULL, osPriorityBelowNormal); // timerTS3のためのRTOSスレッドを宣言
+ Thread ThreadTimerTS4(CallTimerTS4, NULL, osPriorityLow); // timerTS4のためのRTOSスレッドを宣言
// Priority of Thread (RtosTimer is osPriorityAboveNormal)
// osPriorityIdle = -3, ///< priority: idle (lowest)--> then, mbed ERROR!!
// osPriorityLow = -2, ///< priority: low
@@ -69,46 +79,43 @@
// osPriorityHigh = +2, ///< priority: high
// osPriorityRealtime = +3, ///< priority: realtime (highest)
// osPriorityError = 0x84 ///< system cannot determine priority or thread has illegal priority
+ float w_ref_req[2] = {2* 2*PI, 4* 2*PI}; // [rad/s], 指令速度（第2要素は指令速度急変後の指令速度）
+ float t; // [s], 現在の時間
- // 指令速度
- float w_ref_req[2] = {2* 2*PI, 4* 2*PI}; // [rad/s]（第2要素は指令速度急変後の指令速度）
- float t; // current time
-
- // IPMSMの機器定数等の設定, 制御器の初期化
- init_parameters();
+ init_parameters(); // モータの機器定数等の設定, 制御器の初期化
// シミュレーション開始
- pc2.printf("Simulation start!!\r\n");
+ pc2.printf("Simulation start!!\r\n"); // PCのモニタ上のtera termに文字を表示
#ifndef OLD
- // start control (ON)
- start_pwm();
- TickerTimerTS0.attach(&timerTS0, TS0 ); // Sampling period[s] of i_controller
- RtosTimerTS1.start((unsigned int)(TS1*1000.)); // Sampling period[ms] of th controller
- fTimerTS3ON = 1; // timerTS3 start
- fTimerTS4ON = 1; // timerTS3 start
+ // PWMとすべてのタイマーをスタートする
+ start_pwm(); // PWMスタート
+ TickerTimerTS0.attach(&timerTS0, TS0 ); // timerTS0スタート
+ RtosTimerTS1.start((unsigned int)(TS1*1000.)); // timerTS1スタート
+ fTimerTS3ON = 1; // timerTS3スタート
+ fTimerTS4ON = 1; // timerTS4スタート
#endif
- while( (t = _count*TS0) < TMAX ){
+ while( (t = _countTS0*TS0) < TMAX ){ // 現在の時間tを見て、目標速度等を設定し、TMAX[s]に終了
// debug_p24 = 1;
// 速度急変
- if( 0.26<=t && t<2.3 ){
- vl.w_ref=w_ref_req[1]; // 目標速度をメインルーチンから速度制御メインループへ渡す。
- }else{
- vl.w_ref=w_ref_req[0];
+ if( 0.26<=t && t<2.3 ){ // 0.26<t<2.3[s] のとき
+ vl.w_ref=w_ref_req[1]; // 目標速度を速度制御メインループへ渡す。
+ }else{ // 0<t0.26, 2.3<t[s] のとき
+ vl.w_ref=w_ref_req[0]; // 目標速度を速度制御メインループへ渡す。
}
#ifdef SIMULATION
// 負荷トルク急変
- if( t<3.4 ){
- p.TL = 1;
- }else{
- p.TL = 2;
+ if( t<3.4 ){ // 0<t<3.4[s]のとき
+ p.TL = 1; // 負荷トルクは１
+ }else{ // 3.4<t[s]のとき
+ p.TL = 2; // 負荷トルクは１
}
#endif
#ifdef OLD
if( (++i2)>=(int)(TS1/TS0) ){ i2=0;
- timerTS1(&j); //velocity_loop(); // 速度制御メインループ(w_ref&beta_ref to idq_ref)
+ timerTS1(&j); //velocity_loop(); // 速度制御メインループ(w_ref&beta_ref to idq_ref)
}
#endif
@@ -117,22 +124,22 @@
#endif
// debug_p24 = 0;
- Thread::wait(1);
+ Thread::wait(1); // 1ms待つ（待つ間にtimerTS3とtimerTS4が実行される）
}
- // stop timers (OFF)
- stop_pwm();
- TickerTimerTS0.detach(); // timer interrupt stop
- RtosTimerTS1.stop(); // rtos timer stop
- fTimerTS3ON=0;//ThreadTimerTS3.terminate(); //
- fTimerTS4ON=0;//ThreadTimerTS4.terminate(); //
+ // PWMとすべてのタイマーをストップする
+ stop_pwm(); // PWMストップ
+ TickerTimerTS0.detach();// timerTS0ストップ
+ RtosTimerTS1.stop(); // timerTS1ストップ
+ fTimerTS3ON=0; // timerTS3ストップ
+ fTimerTS4ON=0; // timerTS4ストップ
- // save data to mbed USB drive
- for(i=0;i<_count_data;i++){
+ // PC上のmbed USB ディスクにデータをセーブする
+ for(i=0;i<N_DATA;i++){
fprintf( fp, "%f, %f, %f, %f, %f\r\n",
- data[i][0],data[i][1],data[i][2],data[i][3],data[i][4]); // save data to PC (para, y, time, u)
+ data[i][0],data[i][1],data[i][2],data[i][3],data[i][4]); // PCのmbed USB ディスク上にデータをセーブする
}
- fclose( fp ); // release mbed USB drive
-
- Thread::wait(100);
- pc2.printf("Control completed!!\r\n\r\n");
+ fclose( fp ); // PC上のmbed USB ディスクをリリース
+ Thread::wait(100); // セーブ完了まで待つ
+
+ pc2.printf("Control completed!!\r\n\r\n"); // 制御実験終了をPCモニタ上に表示
}