Revision 13:791e20f1af43, committed 2013-03-03

Comitter:

kosaka

Date:

Sun Mar 03 09:09:34 2013 +0000

Parent:

12:a4b17bb682eb

Child:

14:7f83c4b96d34

Commit message:

130214;

Changed in this revision

--- a/UVWpwm.cpp	Fri Dec 21 22:06:56 2012 +0000
+++ b/UVWpwm.cpp	Sun Mar 03 09:09:34 2013 +0000
@@ -1,44 +1,65 @@
 #include "mbed.h"
 #include "controller.h"
-#include "UVWpwm.h"
+#include "UVWpwm.h"  // PWM発生用UVWpwm.cppの変数や定数の定義
+
+#define DEADTIME_US (unsigned long)(DEADTIME*1000000)  // [us], デッドタイム
 
-#define DEADTIME_US (unsigned long)(DEADTIME*1000000)  // [us], deadtime to be set between plus volt. to/from minus
+Timeout pwm[3];    // タイムアウト関数の宣言（ある時間経過後に関数コールする）
 
-Timeout pwm[3];
-
+// デジタル信号を出力するポートupperとlowerをU, V, W相について設定
 DigitalOut pwm_upper[] = {(U_UPPER_PORT), (V_UPPER_PORT),(W_UPPER_PORT)};
 DigitalOut pwm_lower[] = {(U_LOWER_PORT), (V_LOWER_PORT),(W_LOWER_PORT)};
 
-pwm_parameters     uvw[3]; // UVW pwm の定数、変数
+pwm_parameters     uvw[3]; // UVW相pwm用の定数、変数宣言
+
+// U, V相シャント抵抗の両端の電圧のアナログ入力名の設定, *3.3[V]
+AnalogIn VshuntR_Uplus( R_SHUNT_UP_PORT); // *3.3[V], U相+側アナログ入力
+AnalogIn VshuntR_Uminus(R_SHUNT_UM_PORT); // *3.3[V], U相-側アナログ入力
+AnalogIn VshuntR_Vplus( R_SHUNT_VP_PORT); // *3.3[V], V相+側アナログ入力
+AnalogIn VshuntR_Vminus(R_SHUNT_VM_PORT); // *3.3[V], V相-側アナログ入力
 
 // 関数配列: NG
 //void (*pwmUVWout[])(int) = {pwmout,pwmout,pwmout};
 //  pwmUVWout[i](i);
 
 #if PWM_WAVEFORM==0   // 0: saw tooth wave comparison
-void pwmUout() {    // pwm out using timer
-    unsigned char   i=0;
-    uvw[i].mode += 1;
-    if( uvw[i].mode==1 ){
-        pwm_upper[i] = 1;
-        pwm_lower[i] = 0;
-        uvw[i].upper_us = uvw[i].duty*1000000/PWM_FREQ - DEADTIME_US;   // ON time of Uupper
-        pwm[i].attach_us(&pwmUout, uvw[i].upper_us); // setup pwmU to call pwmUout after t [us]
-        uvw[i].lower_us = 1000000/PWM_FREQ -uvw[i].upper_us - 2*DEADTIME_US; // ON time of Ulower
-    }else if( uvw[i].mode==2 ){
-        pwm[i].attach_us(&pwmUout, DEADTIME_US); // setup pwmU to call pwmUout after t [us]
-        pwm_upper[i] = 0;
-        pwm_lower[i] = 0;
-    }else if( uvw[i].mode==3 ){
-        pwm[i].attach_us(&pwmUout, uvw[i].lower_us); // setup pwmU to call pwmUout after t [us]
-        pwm_upper[i] = 0;
-        pwm_lower[i] = 1;
-    }else{// if( u.mode==4 ){
-        pwm[i].attach_us(&pwmUout, DEADTIME_US); // setup pwmU to call pwmUout after t [us]
-        pwm_upper[i] = 0;
-        pwm_lower[i] = 0;
-        uvw[i].mode = 0;
-    }
+void pwmUout() {    // タイムアウト関数でU相PWMを発生する関数
+    unsigned char   i=0;  // i=0のときU相
+    uvw[i].mode += 1;   // チョッピングのオンオフを決定するモードを１増やす
+    if( uvw[i].mode==1 ){       // モードが1のとき、Tonの状態をつくる
+        pwm_upper[i] = 1;         // 上アームUuをオン
+        pwm_lower[i] = 0;         // 下アームUdをオフ
+        // モード1の時間幅 T1 = Ton-Tdt を計算
+        uvw[i].upper_us = uvw[i].duty*1000000/PWM_FREQ - DEADTIME_US;
+        // 時間幅が小さいときはTMINにする
+        if( uvw[i].upper_us < TMIN ){  uvw[i].upper_us=TMIN;}
+        // T1[μs]経過してからタイムアウトでこの関数自身をコール
+        pwm[i].attach_us(&pwmUout, uvw[i].upper_us);
+        // モード3の時間幅 T3=Toff-Tdt=Tpwm-(T1+Tdt)-Tdtを計算
+        uvw[i].lower_us = 1000000/PWM_FREQ -uvw[i].upper_us - 2*DEADTIME_US;
+        // 時間幅が小さいときはTMINにする
+        if( uvw[i].lower_us < TMIN ){  uvw[i].lower_us=TMIN;}
+    }else if( uvw[i].mode==2 ){ // モードが２のとき、デッドタイムをつくる
+        // Tdt[μs]経過してからタイムアウトでこの関数自身をコール
+        pwm[i].attach_us(&pwmUout, DEADTIME_US);
+#ifndef SIMULATION
+        // シャント抵抗の両端の電圧を見てモータ電流を検出
+        p.iuvw[0] = (VshuntR_Uplus - VshuntR_Uminus)*3.3 /R_SHUNT; // iu [A]
+#endif
+        pwm_upper[i] = 0;         // 上アームUuをオフ
+        pwm_lower[i] = 0;         // 下アームUdをオフ
+    }else if( uvw[i].mode==3 ){ // モードが３のとき、Toffの状態をつくる
+        // T3[μs]経過してからタイムアウトでこの関数自身をコール
+        pwm[i].attach_us(&pwmUout, uvw[i].lower_us);
+        pwm_upper[i] = 0;         // 上アームUuをオフ
+        pwm_lower[i] = 1;         // 下アームUdをオン
+    }else{                     // モードが４のとき、デッドタイムをつくる
+        // Tdt[μs]経過してからタイムアウトでこの関数自身をコール
+        pwm[i].attach_us(&pwmUout, DEADTIME_US);
+        pwm_upper[i] = 0;         // 上アームUuをオフ
+        pwm_lower[i] = 0;         // 下アームUdをオフ
+        uvw[i].mode = 0;          // チョッピングのオンオフを決定するモードを
+    }                             // 0にする
 }
 
 void pwmVout() {    // pwm out using timer
@@ -48,10 +69,16 @@
         pwm_upper[i] = 1;
         pwm_lower[i] = 0;
         uvw[i].upper_us = uvw[i].duty*1000000/PWM_FREQ - DEADTIME_US;   // ON time of Uupper
+        if( uvw[i].upper_us < TMIN ){  uvw[i].upper_us=TMIN;}
         pwm[i].attach_us(&pwmVout, uvw[i].upper_us); // setup pwmU to call pwmUout after t [us]
         uvw[i].lower_us = 1000000/PWM_FREQ -uvw[i].upper_us - 2*DEADTIME_US; // ON time of Ulower
+        if( uvw[i].lower_us < TMIN ){  uvw[i].lower_us=TMIN;}
     }else if( uvw[i].mode==2 ){
         pwm[i].attach_us(&pwmVout, DEADTIME_US); // setup pwmU to call pwmUout after t [us]
+#ifndef SIMULATION
+        // シャント抵抗の両端の電圧を見てモータ電流を検出
+        p.iuvw[1] = (VshuntR_Vplus - VshuntR_Vminus)*3.3 /R_SHUNT; // iv [A]
+#endif
         pwm_upper[i] = 0;
         pwm_lower[i] = 0;
     }else if( uvw[i].mode==3 ){
@@ -73,8 +100,10 @@
         pwm_upper[i] = 1;
         pwm_lower[i] = 0;
         uvw[i].upper_us = uvw[i].duty*1000000/PWM_FREQ - DEADTIME_US;   // ON time of Uupper
+        if( uvw[i].upper_us < TMIN ){  uvw[i].upper_us=TMIN;}
         pwm[i].attach_us(&pwmWout, uvw[i].upper_us); // setup pwmU to call pwmUout after t [us]
         uvw[i].lower_us = 1000000/PWM_FREQ -uvw[i].upper_us - 2*DEADTIME_US; // ON time of Ulower
+        if( uvw[i].lower_us < TMIN ){  uvw[i].lower_us=TMIN;}
     }else if( uvw[i].mode==2 ){
         pwm[i].attach_us(&pwmWout, DEADTIME_US); // setup pwmU to call pwmUout after t [us]
         pwm_upper[i] = 0;
@@ -91,34 +120,49 @@
     }
 }
 #elif PWM_WAVEFORM==1   // 1: triangler wave comparison
-void pwmUout() {    // pwm out using timer
-    unsigned char   i=0;
-    uvw[i].mode += 1;
-    if( uvw[i].mode==1 ){
-        uvw[i].upper_us = uvw[i].duty*1000000/PWM_FREQ - DEADTIME_US;   // ON time of Uupper
-        uvw[i].lower_us = 1000000/PWM_FREQ -uvw[i].upper_us - 2*DEADTIME_US; // ON time of Ulower
-        pwm_upper[i] = 0;
-        pwm_lower[i] = 1;
+void pwmUout() {    // タイムアウト関数でU相PWMを発生する関数
+    unsigned char   i=0;  // i=0のときU相
+    uvw[i].mode += 1;   // チョッピングのオンオフを決定するモードを１増やす
+    if( uvw[i].mode==1 ){       // モードが1のとき、Toffの状態をつくる
+        pwm_upper[i] = 0;         // 上アームUuをオフ
+        pwm_lower[i] = 1;         // 下アームUdをオン
+        // モード3の時間幅 T3 = Ton-Tdt を計算
+        uvw[i].upper_us = uvw[i].duty*1000000/PWM_FREQ - DEADTIME_US;
+        // モード1,5の時間幅 T1=(Toff-Tdt)/2=(Tpwm-T3-2Tdt)/2を計算
+        uvw[i].lower_us = 1000000/PWM_FREQ -uvw[i].upper_us - 2*DEADTIME_US;
         uvw[i].lower_us /= 2;
-        pwm[i].attach_us(&pwmUout, uvw[i].lower_us); // setup pwmU to call pwmUout after t [us]
-    }else if( uvw[i].mode==2 ){
-        pwm[i].attach_us(&pwmUout, DEADTIME_US); // setup pwmU to call pwmUout after t [us]
-        pwm_upper[i] = 0;
-        pwm_lower[i] = 0;
-    }else if( uvw[i].mode==3 ){
-        pwm[i].attach_us(&pwmUout, uvw[i].upper_us); // setup pwmU to call pwmUout after t [us]
-        pwm_upper[i] = 1;
-        pwm_lower[i] = 0;
-    }else if( uvw[i].mode==4 ){
-        pwm[i].attach_us(&pwmUout, DEADTIME_US); // setup pwmU to call pwmUout after t [us]
-        pwm_upper[i] = 0;
-        pwm_lower[i] = 0;
-    }else{// if( uvw[i].mode==5 ){
-        pwm[i].attach_us(&pwmUout, uvw[i].lower_us); // setup pwmU to call pwmUout after t [us]
-        pwm_upper[i] = 0;
-        pwm_lower[i] = 1;
-        uvw[i].mode = 0;
-    }
+        // 時間幅が小さいときはTMINにする
+        if( uvw[i].lower_us < TMIN ){  uvw[i].lower_us=TMIN;}
+        // T1[μs]経過してからタイムアウトでこの関数自身をコール
+        pwm[i].attach_us(&pwmUout, uvw[i].lower_us);
+        // 時間幅が小さいときはTMINにする
+        if( uvw[i].upper_us < TMIN ){  uvw[i].upper_us=TMIN;}
+    }else if( uvw[i].mode==2 ){ // モードが２のとき、デッドタイムをつくる
+        // Tdt[μs]経過してからタイムアウトでこの関数自身をコール
+        pwm[i].attach_us(&pwmUout, DEADTIME_US);
+        pwm_upper[i] = 0;         // 上アームUuをオフ
+        pwm_lower[i] = 0;         // 下アームUdをオフ
+    }else if( uvw[i].mode==3 ){ // モードが３のとき、Tonの状態をつくる
+        // T3[μs]経過してからタイムアウトでこの関数自身をコール
+        pwm[i].attach_us(&pwmUout, uvw[i].upper_us);
+        pwm_upper[i] = 1;         // 上アームUuをオン
+        pwm_lower[i] = 0;         // 下アームUdをオフ
+    }else if( uvw[i].mode==4 ){ // モードが４のとき、デッドタイムをつくる
+        // Tdt[μs]経過してからタイムアウトでこの関数自身をコール
+        pwm[i].attach_us(&pwmUout, DEADTIME_US);
+#ifndef SIMULATION
+        // シャント抵抗の両端の電圧を見てモータ電流を検出
+        p.iuvw[0] = (VshuntR_Uplus - VshuntR_Uminus)*3.3 /R_SHUNT; // iu [A]
+#endif
+        pwm_upper[i] = 0;         // 上アームUuをオフ
+        pwm_lower[i] = 0;         // 下アームUdをオフ
+    }else{                     // モードが５のとき、Toffの状態をつくる
+        // T5(=T1)[μs]経過してからタイムアウトでこの関数自身をコール
+        pwm[i].attach_us(&pwmUout, uvw[i].lower_us);
+        pwm_upper[i] = 0;         // 上アームUuをオフ
+        pwm_lower[i] = 1;         // 下アームUdをオン
+        uvw[i].mode = 0;          // チョッピングのオンオフを決定するモードを
+    }                             // 0にする
 }
 void pwmVout() {    // pwm out using timer
     unsigned char   i=1;
@@ -129,7 +173,9 @@
         pwm_upper[i] = 0;
         pwm_lower[i] = 1;
         uvw[i].lower_us /= 2;
+        if( uvw[i].lower_us < TMIN ){  uvw[i].lower_us=TMIN;}
         pwm[i].attach_us(&pwmVout, uvw[i].lower_us); // setup pwmU to call pwmUout after t [us]
+        if( uvw[i].upper_us < TMIN ){  uvw[i].upper_us=TMIN;}
     }else if( uvw[i].mode==2 ){
         pwm[i].attach_us(&pwmVout, DEADTIME_US); // setup pwmU to call pwmUout after t [us]
         pwm_upper[i] = 0;
@@ -140,6 +186,10 @@
         pwm_lower[i] = 0;
     }else if( uvw[i].mode==4 ){
         pwm[i].attach_us(&pwmVout, DEADTIME_US); // setup pwmU to call pwmUout after t [us]
+#ifndef SIMULATION
+        // シャント抵抗の両端の電圧を見てモータ電流を検出
+        p.iuvw[1] = (VshuntR_Vplus - VshuntR_Vminus)*3.3 /R_SHUNT; // iv [A]
+#endif
         pwm_upper[i] = 0;
         pwm_lower[i] = 0;
     }else{// if( uvw[i].mode==5 ){
@@ -159,7 +209,9 @@
         pwm_upper[i] = 0;
         pwm_lower[i] = 1;
         uvw[i].lower_us /= 2;
+        if( uvw[i].lower_us < TMIN ){  uvw[i].lower_us=TMIN;}
         pwm[i].attach_us(&pwmWout, uvw[i].lower_us); // setup pwmU to call pwmUout after t [us]
+        if( uvw[i].upper_us < TMIN ){  uvw[i].upper_us=TMIN;}
     }else if( uvw[i].mode==2 ){
         pwm[i].attach_us(&pwmWout, DEADTIME_US); // setup pwmU to call pwmUout after t [us]
         pwm_upper[i] = 0;
@@ -185,7 +237,7 @@
 void start_pwm(){
     unsigned char i; 
     for( i=0;i<3;i++ ){
-        uvw[i].duty = 0.5;
+        uvw[i].duty = 0.5;  // 0.5のときにVu=0[V]
         pwm_upper[i] = pwm_lower[i] = 0;
         uvw[i].mode = 0;
     }

--- a/UVWpwm.h	Fri Dec 21 22:06:56 2012 +0000
+++ b/UVWpwm.h	Sun Mar 03 09:09:34 2013 +0000
@@ -4,22 +4,29 @@
 //*************  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 U_UPPER_PORT    LED1//p21    // port for U phase upper arm
-#define U_LOWER_PORT    LED2//p22    // port for U phase lower arm
-#define V_UPPER_PORT    LED3//p23    // port for V phase upper arm
-#define V_LOWER_PORT    LED4//p24    // port for V phase lower arm
-#define W_UPPER_PORT    p25     // port for W phase upper arm
-#define W_LOWER_PORT    p26     // port for W phase lower arm
-#define PWM_WAVEFORM    0       // 0: saw tooth wave comparison, 1: triangler wave comparison
+#define U_UPPER_PORT    p21    // U相上アームUu用ポート
+#define U_LOWER_PORT    p22    // U相下アームUd用ポート
+#define V_UPPER_PORT    p23    // V相上アームVu用ポート
+#define V_LOWER_PORT    p24    // V相下アームVd用ポート
+#define W_UPPER_PORT    p25    // W相上アームWu用ポート
+#define W_LOWER_PORT    p26    // W相下アームWd用ポート
+#define PWM_WAVEFORM    1      // 0: saw tooth wave comparison, 1: triangler wave comparison
+#define TMIN            3      // [us], processing time of pwm_out()
+
+#define R_SHUNT_UP_PORT p16 // ポート：U相電流検出用抵抗の+側アナログ入力
+#define R_SHUNT_UM_PORT p17 // ポート：U相電流検出用抵抗の-側アナログ入力
+#define R_SHUNT_VP_PORT p19 // ポート：V相電流検出用抵抗の+側アナログ入力
+#define R_SHUNT_VM_PORT p20 // ポート：V相電流検出用抵抗の-側アナログ入力
+#define R_SHUNT     0.47   // [Ω], 電流検出用シャント抵抗の値
 //*************  User setting parameters (end) *****************
 
-typedef struct struct_pwm_parameters{    // parameters of UVW pwm
-    float duty;            // 0-1, duty of UVW
-    unsigned char  mode;      // mode
-    unsigned long   upper_us;  // [us], time
-    unsigned long   lower_us;  // [us], time
+typedef struct struct_pwm_parameters{ // UVW相pwm用の変数宣言
+    float duty;          // 0-1, PWMデューティ
+    unsigned char  mode; // チョッピングのオンオフを決定するモード
+    long   upper_us;     // [us], 上アームをオンする時間幅
+    long   lower_us;     // [us], 下アームをオンする時間幅
 }pwm_parameters;
-extern pwm_parameters     uvw[3]; // UVW pwm の定数、変数
+extern pwm_parameters     uvw[]; // UVW pwm の定数、変数
 
 extern void start_pwm();
 extern void stop_pwm();

--- a/controller.cpp	Fri Dec 21 22:06:56 2012 +0000
+++ b/controller.cpp	Sun Mar 03 09:09:34 2013 +0000
@@ -1,5 +1,5 @@
-//  BLDCmotor.cpp: 各種３相同期モータに対するセンサあり運転のシミュレーション
-//      Kosaka Lab. 121215
+//  controller.cpp: 各種３相同期モータに対するセンサあり運転のシミュレーション
+//      Kosaka Lab. 130214
 #include "mbed.h"
 #include "QEI.h"
 
@@ -31,25 +31,9 @@
 /*********** 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]
 
 unsigned long _count;   // sampling number
 float   _time;          // time[s]
-float   _r;             // reference signal
-float   _th=0;          // [rad], motor angle, control output of angle controller
-float   _i=0;           // [A], motor current, control output of current controller
-float   _e=0;           // e=r-y for PID controller
-float   _eI=0;          // integral of e for PID controller
-float   _iref;          // reference current iref [A], output of angle th_contorller
-float   _u;             // control input[V], motor input volt.
-float   _ei=0;          // e=r-y for current PID controller
-float   _eiI=0;         // integral of e for current PID controller
-unsigned char _f_u_plus=1;// sign(u)
-unsigned char _f_umax=0;// flag showing u is max or not
-unsigned char _f_imax=0;// flag showing i is max or not
 float   debug[20];      // for debug
 float   disp[10];       // for printf to avoid interrupted by quicker process
 DigitalOut  led1(LED1);
@@ -57,15 +41,14 @@
 DigitalOut  led3(LED3);
 DigitalOut  led4(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
-DigitalOut  debug_p26(p26); // p17 for debug
-DigitalOut  debug_p25(p25); // p17 for debug
-DigitalOut  debug_p24(p24); // p17 for debug
+
+//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.
 
@@ -101,13 +84,12 @@
     p.R = 0.143;    // Ω
     p.phi = 0.176;  // V s
     p.Jm = 0.00018; // Nms^2
+    p.p = 2;    // 極対数
 #endif
-    p.th[0] = 0;
-    p.th[1] = 0;
+    p.th[0] = p.th[1] = 0;
     p.w = 0;
     p.iab[0] =0;    p.iab[1] = 0;   // iab = [iα;iβ];
     p.vab[0] =0;    p.vab[1] = 0;   // vab = [vα;vβ];
-    p.p = 2;    // 極対数
         // UVW座標からαβ座標への変換行列Cuvwの設定
     r2 = sqrt(2.);//1.414213562373095;//2^(1/2);
     r3 = sqrt(3.);//1.732050807568877;//3^(1/2);
@@ -121,22 +103,23 @@
     // 制御器の初期化
     vl.iq_ref=0;        // ｑ軸電流指令[A]
     vl.w_lpf = 0;       // 検出した速度[rad/s]
-    vl.eI_w = 0;        // 速度制御用偏差の積分値（積分項）
-    il.eI_idq[0] = 0;   // 電流制御用偏差の積分値（積分項）
-    il.eI_idq[1] = 0;   // 電流制御用偏差の積分値（積分項）
+    vl.eI = 0;          // 速度制御用偏差の積分値（積分項）
+    il.eI_idq[0] = 0;   // d軸電流制御用偏差の積分値（積分項）
+    il.eI_idq[1] = 0;   // q軸電流制御用偏差の積分値（積分項）
+    il.e_old[0] = 0;    // d軸電流制御用偏差の1サンプル過去の値
+    il.e_old[1] = 0;    // q軸電流制御用偏差の1サンプル過去の値
+#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
 }
 
 void idq_control(float idq_act[2]){
 //  ｄｑ座標電流PID制御器（電流マイナーループのフィードバック制御）
-//      入力：指令dq座標電流 idq_ref [A], 実dq座標電流 idq_act [A], PI制御積分項 eI, サンプル時間 ts [s]
-//      出力：αβ軸電圧指令 vdq_ref [A]
+//      入力：指令dq座標電流 idq_ref [A], 実dq座標電流 idq_act [A], PI制御積分項 eI, サンプル時間 TS0 [s]
+//      出力：dq軸電圧指令 vdq_ref [A]
 //       [vdq_ref,eI_idq] = idq_control(idq_ref,idq_act,eI_idq,ts);
-    float  Kp_d, Kp_q, Ki_d, Ki_q, e[2];
-    // 電流制御ゲイン
-    Kp_d = iKPd;    // P gain (d-axis)
-    Ki_d = iKId;    // I gain (d-axis)
-    Kp_q = iKPq;    // P gain (q-axis)
-    Ki_q = iKIq;    // I gain (q-axis)
+    float  e[2], ed[2];
 
     // ｄｑ電流偏差の計算
     e[0] = il.idq_ref[0] - idq_act[0];
@@ -146,23 +129,24 @@
     il.eI_idq[0] = il.eI_idq[0] + TS0*e[0];
     il.eI_idq[1] = il.eI_idq[1] + TS0*e[1];
 
-    // ＰＩ制御
+
+    // ｄｑ電流偏差の微分値の計算
+    ed[0] = (e[0]-il.e_old[0])/TS0;
+    ed[1] = (e[1]-il.e_old[1])/TS0;
+    il.e_old[0] = e[0];  // 電流偏差の1サンプル過去の値を更新
+    il.e_old[1] = e[1];  // 電流偏差の1サンプル過去の値を更新
+
+    // PID制御
     //  vdq_ref = [Kp_d 0;0 Kp_q]*e + [Ki_d 0;0 Ki_q]*eI;
-    il.vdq_ref[0] = Kp_d*e[0] + Ki_d*il.eI_idq[0];
-    il.vdq_ref[1] = Kp_q*e[1] + Ki_q*il.eI_idq[1];
-
-// koko anti-windup
+    il.vdq_ref[0] = iKPd*e[0] + iKId*il.eI_idq[0] + iKDd*ed[0];
+    il.vdq_ref[1] = iKPq*e[1] + iKIq*il.eI_idq[1] + iKDq*ed[0];
 }
 
 void current_loop(){    // 電流制御マイナーループ
-    float  th, c, s, Cdq[2][2], iu, iv, iab[2], idq_act[2], vab_ref[2],tmp;
+    float  th, c, s, Cdq[2][2], iu, iv, iab[2], idq_act[2], vab_ref[2], tmp, prev[2];
   if( f_find_origin==1 ){
     th = 0;
   }else{
-    // 位置θをセンサで検出
-#ifndef SIMULATION
-    p.th[0] = (float)encoder.getPulses()/(float)N_ENC*2.0*PI;   // get angle [rad] from encoder
-#endif
     th = p.th[0];
   }
 
@@ -177,11 +161,6 @@
     Cdq[0][0]= c;   Cdq[0][1]=s;    //Cdq ={{ c, s}
     Cdq[1][0]=-s;   Cdq[1][1]=c;    //       {-s, c]};
 
-    // 電流センサによってiu, iv を検出
-#ifndef SIMULATION
-    p.iuvw[0] = (VshuntR_Uplus - VshuntR_Uminus) /R_SHUNT;      // get iu [A] from shunt resistance;
-    p.iuvw[1] = (VshuntR_Vplus - VshuntR_Vminus) /R_SHUNT;      // get iv [A] from shunt resistance;
-#endif
     iu = p.iuvw[0];
     iv = p.iuvw[1];
 //  iw = -(iu + iv);    // iu+iv+iw=0であることを利用してiw を計算
@@ -204,18 +183,30 @@
 #ifdef USE_CURRENT_CONTROL
     idq_control(idq_act);
 #else
-    il.vdq_ref[0] = il.idq_ref[0];
-    il.vdq_ref[1] = il.idq_ref[1];
+    il.vdq_ref[0] = il.idq_ref[0]/iqMAX*vdqMAX;
+    il.vdq_ref[1] = il.idq_ref[1]/iqMAX*vdqMAX;
 #endif
-    // ｄｑ軸電圧指令ベクトルの大きさをMAX制限（コンバータ出力電圧値に設定）
+    // ｄｑ軸電圧指令ベクトルの大きさをMAX制限してアンチワインドアップ対策
     // if( norm(vdq_ref) > vdqmax ){    vdq_ref= vdqmax/norm(vdq_ref)*vdq_ref}
-    if( (tmp=il.vdq_ref[0]*il.vdq_ref[0]+il.vdq_ref[1]*il.vdq_ref[1])>SQRvdqMAX ){
-        tmp = sqrt2(SQRvdqMAX/tmp);
-        il.vdq_ref[0] = tmp*il.vdq_ref[0]; //= vdqmax/norm(vdq_ref)*vdq_ref
-        il.vdq_ref[1] = tmp*il.vdq_ref[1];
-// koko anti-windup
+    // anti-windup: if u=v_ref is saturated, then reduce eI. 
+    //電圧振幅の2乗 vd^2+vq^2 を計算
+    tmp=il.vdq_ref[0]*il.vdq_ref[0]+il.vdq_ref[1]*il.vdq_ref[1];
+    if( tmp > SQRvdqMAX ){  // 電圧振幅の2乗がVMAXより大きいとき
+        prev[0] = il.vdq_ref[0];    // vdを記憶
+        prev[1] = il.vdq_ref[1];    // vqを記憶
+        tmp = sqrt2(SQRvdqMAX/tmp); // 振幅をVMAXまで小さくする比を求める
+        il.vdq_ref[0] = tmp*il.vdq_ref[0]; // vdにその比をかける
+        il.vdq_ref[1] = tmp*il.vdq_ref[1]; // vqにその比をかける
+        il.eI_idq[0] -= (prev[0]-il.vdq_ref[0])/iKId; // 振幅を小さくした分、
+        if( il.eI_idq[0]<0 ){   il.eI_idq[0]=0;}      // I項を小さくする
+        il.eI_idq[1] -= (prev[1]-il.vdq_ref[1])/iKIq; // q軸にも同じ処理
+        if( il.eI_idq[1]<0 ){   il.eI_idq[1]=0;}
     }
-
+//#define DOUKI
+#ifdef DOUKI
+il.vdq_ref[0]=0;
+il.vdq_ref[1]=vdqMAX;
+#endif
     // ｄｑ座標指令電圧 vd_ref, vq_refからiα, iβを計算
     // vab_ref = Cdq'*vdq_ref;
     vab_ref[0] = Cdq[0][0]*il.vdq_ref[0] + Cdq[1][0]*il.vdq_ref[1];
@@ -233,8 +224,6 @@
     p.vuvw[2] = -p.vuvw[0] - p.vuvw[1];
 //  p.vuvw[0] = p.Cuvw[0][0]*vab_ref[0] + p.Cuvw[1][0]*vab_ref[1];
 //  p.vuvw[2] = p.Cuvw[0][2]*vab_ref[0] + p.Cuvw[1][2]*vab_ref[1];
-
-    p.th[1] = p.th[0];  // thを更新
 }
 
 
@@ -242,21 +231,20 @@
 //  速度制御器：速度偏差が入力され、q軸電流指令を出力。
 //      入力：指令速度 w_ref [rad/s], 実速度 w_lpf [rad/s], PI制御積分項 eI, サンプル時間 TS1 [s]
 //      出力：q軸電流指令 iq_ref [A]
-//       [iq_ref,eI_w] = vel_control(w_ref,w_lpf,eI_w,ts);
-    float  Kp, Ki, e;
-    // 速度制御PIDゲイン
-    Kp = wKp;   // 速度制御PIDのPゲイン
-    Ki = wKi;   // 速度制御PIDのIゲイン
+//       [iq_ref,eI] = vel_control(w_ref,w_lpf,eI,ts);
+    float  e, ed;
 
     // 速度偏差の計算
     e = vl.w_ref - vl.w_lpf;
 
     // 速度偏差の積分値の計算
-    vl.eI_w = vl.eI_w + TS1*e;
+    vl.eI = vl.eI + TS1*e;
+
+    ed = (e-vl.e_old)/TS1;          // 速度偏差の微分値の計算
+    vl.e_old = e;                   // 速度偏差の1サンプル過去の値を更新
 
     // ＰＩ制御
-    vl.iq_ref = Kp*e + Ki*vl.eI_w;
-// koko anti-windup
+    vl.iq_ref = wKp*e + wKi*vl.eI + wKd*ed; // ＰＩＤ制御器の出力を計算
 }
 
 void velocity_loop(){   // 速度制御メインループ(w_ref&beta_ref to idq_ref)
@@ -266,16 +254,19 @@
     tmp = p.th[0]-p.th[1];
     while( tmp> PI ){ tmp -= 2*PI;}
     while( tmp<-PI ){ tmp += 2*PI;}
-    vl.w_lpf = (1-iLPF)*vl.w_lpf + tmp/TS0 *iLPF;
+    vl.w_lpf = iLPF*vl.w_lpf + tmp/TS0 *(1-iLPF);
 
     // 速度制御：速度偏差が入力され、q軸電流指令を出力。
-//  [iq_ref,eI_w] = vel_control(w_ref,w_act,eI_w,ts);
+//  [iq_ref,eI] = vel_control(w_ref,w_act,eI,ts);
     vel_control();
 
     // ｑ軸電流指令のMAX制限（異常に大きい指令値を制限する）
+    // anti-windup: if u=i_ref is saturated, then reduce eI. 
     if( vl.iq_ref > iqMAX ){
+        vl.eI -= (vl.iq_ref - iqMAX)/wKi;    if( vl.eI<0 ){   vl.eI=0;}
         vl.iq_ref = iqMAX;
     }else if( vl.iq_ref < -iqMAX ){
+        vl.eI -= (vl.iq_ref + iqMAX)/wKi;    if( vl.eI>0 ){   vl.eI=0;}
         vl.iq_ref = -iqMAX;
     }
 
@@ -293,12 +284,12 @@
 void    vuvw2pwm(){ // vu, vv, vwより、UVW相の上アームPWMを発生
     float   duty_u, duty_v, duty_w;
 
-    duty_u = (p.vuvw[0]/vdqMAX+1)*0.5;
-    duty_v = (p.vuvw[1]/vdqMAX+1)*0.5;
-    duty_w = (p.vuvw[2]/vdqMAX+1)*0.5;
-    uvw[0].duty = duty_u;
-    uvw[1].duty = duty_v;
-    uvw[2].duty = duty_w;
+    duty_u = p.vuvw[0]/vdqMAX+0.5; // dutyを計算
+    duty_v = p.vuvw[1]/vdqMAX+0.5; // dutyを計算
+    duty_w = p.vuvw[2]/vdqMAX+0.5; // dutyを計算
+    uvw[0].duty = duty_u;  // dutyをPWM発生関数に渡す
+    uvw[1].duty = duty_v;  // dutyをPWM発生関数に渡す
+    uvw[2].duty = duty_w;  // dutyをPWM発生関数に渡す
 }
 
 #ifdef SIMULATION
@@ -419,26 +410,38 @@
 #endif
 }
 void timerTS0(){    // timer called every TS0[s].
-    debug_p26 = 1;
+//    debug_p26 = 1;
     _count++;
     _time += TS0;
     
-    current_loop(); // 電流制御マイナーループ(idq_ref to vuvw)
-    vuvw2pwm();     // vuvw to pwm
+    p.th[1] = p.th[0];  // thを更新
  #ifdef SIMULATION
     // モータシミュレータ
     sim_motor();    // IPM, ｄｑ座標
+ #else
+//koko    p.th[0] = (float)encoder.getPulses()/(float)N_ENC*2.0*PI;   // get angle [rad] from encoder
+    // 位置θをセンサで検出
+#ifdef DOUKI
+led1=1;
+p.th[0] += 2*PI*TS0 * 1; if(p.th[0]>4*PI){   p.th[0]-=4*PI;}
+debug[0]=p.th[0]/PI*180;
+analog_out=debug[0]/180*PI/4/PI;
+led1=0;
+#endif
  #endif
-    debug_p26 = 0;
+    current_loop(); // 電流制御マイナーループ(idq_ref to vuvw)
+    vuvw2pwm();     // vuvw to pwm
+//    debug_p26 = 0;
 }
 void timerTS1(void const *argument){
-    debug_p25 = 1;
+//    debug_p25 = 1;
     velocity_loop();    // 速度制御メインループ(w_ref&beta_ref to idq_ref)
-    debug_p25 = 0;
+//    debug_p25 = 0;
 }
 
 void display2PC(){  // display to tera term on PC
-    pc.printf("%8.1f[s]\t%8.5f[V]\t%4d [Hz]\t%d\r\n", _time, il.vdq_ref[0], (int)(vl.w_lpf/(2*PI)+0.5), (int)(vl.w_ref/(2*PI)+0.5));  // print to tera term
+    pc.printf("%8.1f[s]\t%8.5f[V]\t%8.2f [Hz]\t%8.2f\t%8.2f\r\n", 
+        _time, il.vdq_ref[1], vl.w_lpf/(2*PI), vl.w_ref/(2*PI), debug[0]);  // print to tera term
 //    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(){

--- a/controller.h	Fri Dec 21 22:06:56 2012 +0000
+++ b/controller.h	Sun Mar 03 09:09:34 2013 +0000
@@ -4,13 +4,9 @@
 //#define PI 3.14159265358979 // def. of PI
 /*********** User setting for control parameters (begin) ***************/
 #define SIMULATION          // Comment this line if not simulation
-#define PWM_FREQ 1000.0  //[Hz], pwm freq. (> 1/(DEAD_TIME*10))
-#define DEADTIME    0.0001  // [s], deadtime to be set between plus volt. to/from minus
 #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   1.  // deadzone of plus side
 #define DEADZONE_MINUS -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
@@ -18,22 +14,27 @@
 
 #define DA_PORT p18           // analog out (DA) port of mbed
 
+#define PWM_FREQ 1000.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.001//08//8      // [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.1         // [s], sampling time (priority lowest: precision 4ms)  to display data to PC tera term
+#define TS4     0.2         // [s], 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    3.0          // [s], experiment starts from 0[s] to TMAX[s]
+#define TMAX_FIND_ORIGIN    0.1//1.0          // [s], finding th origin starts from 0[s] to TMAX[s]
 
     // 電流制御マイナーループ
 #define iKPd    10./2     // 電流制御d軸PIDのPゲイン (d-axis)
 #define iKId    100./2    // 電流制御d軸PIDのIゲイン (d-axis)
+#define iKDd    0         // 電流制御d軸PIDのDゲイン (d-axis)
 #define iKPq    10./2     // 電流制御q軸PIDのPゲイン (q-axis)
 #define iKIq    100./2    // 電流制御q軸PIDのIゲイン (q-axis)
+#define iKDq    0         // 電流制御q軸PIDのDゲイン (q-axis)
 
 #define vdqMAX  300.
 #define SQRvdqMAX (vdqMAX*vdqMAX)   // [V^2] vdqの大きさの最大値の二乗
@@ -42,15 +43,16 @@
 #ifdef USE_CURRENT_CONTROL
  #define wKp 0.05        // 速度制御PIDのPゲイン
  #define wKi 2.50        // 速度制御PIDのIゲイン
+ #define wKd 0           // 速度制御PIDのDゲイン
 #else
- #define wKp 0.005//0.05        // 速度制御PIDのPゲイン
- #define wKi 0.2//2.50        // 速度制御PIDのIゲイン
+ #define wKp 0.005       // 速度制御PIDのPゲイン
+ #define wKi 0.2         // 速度制御PIDのIゲイン
+ #define wKd 0           // 速度制御PIDのDゲイン
 #endif
 
 #define iLPF    0.9     // 0-1, 速度に対する1次LPF; Low Pass Filter, G(z)=(1-a)/(z-a)
 #define iqMAX   100       // [A], ｑ軸電流指令のMAX制限（異常に大きい指令値を制限する）
 
-#define R_SHUNT     1.25    // [Ohm], shunt resistanse
 /*********** User setting for control parameters (end) ***************/
 
 
@@ -84,6 +86,7 @@
     float  idq_ref[2]; // idqの目標値
     float  vdq_ref[2]; // vdqの目標値
     float  eI_idq[2];  // 電流制御用偏差の積分値（積分項）
+    float  e_old[2];   // 電流制御用偏差の1サンプル過去の値
 }current_loop_parameters;
 
 typedef struct struct_velocity_loop_parameters{
@@ -92,7 +95,8 @@
     float  w_ref;          // [rad/s], モータ目標速度
     float  tan_beta_ref;   // [rad],   モータ電流位相
     float  iq_ref;         // ｑ軸電流指令[A]
-    float  eI_w;           // 速度制御用偏差の積分値（積分項）
+    float  eI;             // 速度制御用偏差の積分値（積分項）
+    float  e_old;          // 速度制御用偏差の1サンプル過去の値
 }velocity_loop_parameters;
 
 extern void timerTS0();    // timer called every TS0[s].

--- a/fast_math.cpp	Fri Dec 21 22:06:56 2012 +0000
+++ b/fast_math.cpp	Sun Mar 03 09:09:34 2013 +0000
@@ -1,31 +1,37 @@
 #include "mbed.h"
 #include "fast_math.h"
 
-unsigned short  sin60[DEG60+1];  // sin table from 0 to 60 deg. (max precision error is 0.003%)
+unsigned short  sin60[DEG60+1];  // 0～60度, 振幅65535のsinテーブル(最大誤差0.003%) from 0 to 60 deg. (max precision error is 0.003%)
 
 long _sin(unsigned short th){    // return( 65535*sin(th) ), th=rad*DEG60/(PI/3)=rad*(512*3)/PI (0<=rad<2*PI)
-//    init_sin60();
+// 入力 : th = rad*DEG60/(PI/3)=rad*(512*3)/PI, (0<=rad<2*PI)
+// 出力 : 65535*sin(th)
+//    init_fast_math();
 //    if( th>2.*PI ){ th -= 2*PI*(float)((int)(th/(2.*PI)));}
 //    th_int = (unsigned short)(th/(PI/3.)*(float)DEG60+0.5);  // rad to deg
 //    sin = (float)_sin(th)/65535.;
     unsigned short f_minus;
     long    x;
 
+    // sinがマイナスのとき、thから180度引いて、f_minus=1にする
     if( th>=DEG60*3){   f_minus = 1;    th -= DEG60*3;} // if th>=180deg, th = th - 180deg;
     else{               f_minus = 0;}                   // else         , f_minus = on. 
 
-    if( th<DEG60 ){         // th<60deg?
+    if( th<DEG60 ){         // th<60度のとき
         x = sin60[th];                              // sin(th)
-    }else if( th<DEG60*2 ){ // 60<=th<120deg?
+    }else if( th<DEG60*2 ){ // 60≦th<120度のとき
         x = sin60[DEG60*2-th] + sin60[th-DEG60];    // sin(th)=sin(th+60)+sin(th-60)=sin(180-(th+60))+sin(th-60) because sin(th+60)=s/2+c*root(3)/2, sin(th-60)=s/2-c*root(3)/2.
-    }else{// if( th<180 )   // 120<=th<180deg?
+    }else{                  // 120≦th<180度のとき
         x = sin60[DEG60*3-th];                      // sin(60-(th-120))=sin(180-th)
     }
-    if( f_minus==1 ){   x = -x;}
+    if( f_minus==1 ){   x = -x;}  // sinがマイナスのときマイナスにする
     return(x);
 }
 
 long _cos(unsigned short th){    // return( 65535*sin(th) ), th=rad*DEG60/(PI/3)=rad*(512*3)/PI (0<=rad<2*PI)
+// 入力 : th = rad*DEG60/(PI/3)=rad*(512*3)/PI, (0<=rad<2*PI)
+// 出力 : 65535*cos(th)
+    th += DEG60*3/2;
     th += DEG60*3/2;
     if( th>=DEG60*6 ){  th -= DEG60*6;}
     return( _sin(th) );
@@ -34,7 +40,7 @@
 void init_fast_math(){  // sin0-sin60deg; 0deg=0, 60deg=512
     int i;
     
-    for( i=0;i<=DEG60;i++ ){    // set sin table from 0 to 60 deg..
+    for( i=0;i<=DEG60;i++ ){    // 0～60度までのsinテーブルをつくるset sin table from 0 to 60 deg..
 //        sin60[i] = (unsigned short)(sin((float)i/512.*PI/3.));
         sin60[i] = (unsigned short)(65535.*sinf((float)i/(float)DEG60*PI/3.));
     }

--- a/fast_math.h	Fri Dec 21 22:06:56 2012 +0000
+++ b/fast_math.h	Sun Mar 03 09:09:34 2013 +0000
@@ -1,8 +1,8 @@
 #ifndef __fast_math_h
 #define __fast_math_h
 
-#define PI 3.14159265358979 // def. of PI
-#define DEG60   512     // 60deg = 512
+#define PI 3.14159265358979 // π: def. of PI
+#define DEG60   512         // 60degを512に定義
 
 extern unsigned short  sin60[]; // sin table from 0 to 60 deg. (max precision error is 0.003%)
 // sin(th) = (float)(_sin(th/(PI/3.)*(float)DEG60+0.5))/65535.;

--- a/main.cpp	Fri Dec 21 22:06:56 2012 +0000
+++ b/main.cpp	Sun Mar 03 09:09:34 2013 +0000
@@ -1,5 +1,5 @@
-//  UVW three phases Blushless DC motor control program using 3 H-bridge driver (ex. BD6211F) and 360 resolution rotary encoder with A, B phase.
-//      ver. 121206 by Kosaka lab.
+//  UVW three phases Blushless DC motor control program using 3 H-bridge driver (ex. TA7291P) and incremental rotary encoder with A, B phase.
+//      ver. 130214 by Kosaka lab.
 #include "mbed.h"
 #include "rtos.h"
 
@@ -17,10 +17,6 @@
 
 extern "C" void mbed_reset();
 
-FILE *fp = fopen("/mbedUSBdrive/data.csv", "w");    //save data to PC
-Timeout emergencyStop;          // kill fprintf process when bug
-
-
 void CallTimerTS2(void const *argument) {   // make sampling time TS3 timer (priority 3: precision 4ms)
     int ms;
     unsigned long c;
@@ -66,11 +62,8 @@
 
 //#define OLD
 int main(){
-    int ms=1;
-    unsigned long c, c2;
     unsigned short  i, i2=0;
-//    FILE *fp;   // save data to PC
-//    FILE *fp = fopen("/mbedUSBdrive/data.csv", "w"); 
+    FILE *fp = fopen("/mbedUSBdrive/data.csv", "w");    // save data to PC
     char chr[100];
     RtosTimer RtosTimerTS1(timerTS1);  // RtosTimer priority is osPriorityAboveNormal, just one above main()
     Thread ThreadTimerTS3(CallTimerTS3,NULL,osPriorityBelowNormal);
@@ -85,32 +78,27 @@
 //  osPriorityRealtime      = +3,          ///< priority: realtime (highest)
 //  osPriorityError         =  0x84        ///< system cannot determine priority or thread has illegal priority
 
-    // シミュレーション総サンプル数
-    int N;// = 5000;
     // 指令速度
     float  w_ref_req[2] = {20* 2*PI, 40* 2*PI};        // [rad/s]（第2要素は指令速度急変後の指令速度）
-    float  w_ref;
     // 指令ｄｑ電流位相
-    float  beta_ref = 30*PI/180;   // rad
+    float  beta_ref = 30*PI/180;   // [rad], 電流位相指令βを30度に
     float  tan_beta_ref1;
     float  tan_beta_ref2,tan_beta_ref;
+    float  t;   // current time
 
 //    start_timers(1); // start multi-timers, sampling times are TS0, TS1, TS2, TS3, TS4.
 
-    N = (int)(TMAX/TS0);
-pc2.printf("N=%d\r\n",N);
     // IPMSMの機器定数等の設定, 制御器の初期化
     init_parameters();
     p.th[0] = 2*PI/3;   //θの初期値
 
-
 //  p.Lq0 = p.Ld;   // SPMSMの場合
 //  p.phi = 0;      // SynRMの場合
 
 //    w_ref=p.w;          // 速度指令[rad/s]
-    tan_beta_ref1 = tan(beta_ref);
-    tan_beta_ref2 = tan(beta_ref-30*PI/180);
-    tan_beta_ref = tan_beta_ref1;
+    tan_beta_ref1 = tan(beta_ref);           // tan30°を計算
+    tan_beta_ref2 = tan(beta_ref-30*PI/180); // tan0°を計算
+    tan_beta_ref = tan_beta_ref1;            // βの指令値をtan30°に
     // シミュレーション開始
     pc2.printf("Simulation start!!\r\n");
 #ifndef OLD
@@ -123,11 +111,11 @@
 #endif
     
     // set th by moving rotor to th=zero.
-    f_find_origin=1;
-    while( _count*TS0<0.1 ){  // while( time<1 ){
+    f_find_origin=1;    // 回転角原点復帰フラグをセット
+    while( (t = _count*TS0) < TMAX_FIND_ORIGIN ){  // TMAX秒経過するまで制御実行
 //        debug_p24 = 1;
-        il.idq_ref[0] = iqMAX/1.0;
-        il.idq_ref[1] = 0;
+        il.idq_ref[0] = iqMAX/1.0;  // idをプラス、iqをゼロにして、
+        il.idq_ref[1] = 0;          // 無負荷のときにθ=0とさせる。
 
 #ifdef OLD
         timerTS0();
@@ -137,15 +125,12 @@
 #endif
 
 //        if( (ms=(int)(TS1*1000-(_count-c)*TS0*1000))<=0 ){    ms=1;}// ms=TS0
-        Thread::wait(ms);
 //        debug_p24 = 0;
+        Thread::wait(1);
     }
 //pc2.printf("\r\n^0^ 0\r\n");
-#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
-    c2 = _count;
+    // IPMSMの機器定数等の設定, 制御器の初期化
+    init_parameters();
     f_find_origin=0;
 
 #ifndef OLD
@@ -155,33 +140,36 @@
     RtosTimerTS1.start((unsigned int)(TS1*1000.));   // Sampling period[ms] of th controller
 #endif
 
-    for( i=0;i<N;i++ ){
+    while( (t = _count*TS0-TMAX_FIND_ORIGIN) < TMAX ){
 //        debug_p24 = 1;
-        c = _count-c2;
-        // 電流位相（ｄｑ軸電流）変化
-        // if( i>=1500&&i<1900 ){// TS0=0.0001
-        if( c>=1500*0.0001/TS0&&c<1900*0.0001/TS0 ){
-            if( tan_beta_ref>tan_beta_ref2 ){   tan_beta_ref=tan_beta_ref-0.002;}
-        }else{
-            if( tan_beta_ref<tan_beta_ref1){    tan_beta_ref=tan_beta_ref+0.002;}
+
+        // 電流位相（ｄｑ軸電流）を変化させるベクトル制御
+        if( t>=0.15 && t<0.19 ){  // 0.15～0.19秒の間に
+            if( tan_beta_ref>tan_beta_ref2 ){ // βの指令値が0度以上のとき
+                tan_beta_ref=tan_beta_ref-0.002;  // 指令値を小さくする
+            }
+        }else{                   // 0.15～0.19秒の間でないときに
+            if( tan_beta_ref<tan_beta_ref1){ // βの指令値が30度以下のとき
+                tan_beta_ref=tan_beta_ref+0.002;  // 指令値を大きくする
+            }
         }
+        // 目標電流位相をメインルーチンから速度制御メインループへ渡す。
+        vl.tan_beta_ref = tan_beta_ref;
 
         // 速度急変
-        w_ref = w_ref_req[0];
-        if( 2600*0.0001/TS0<=c&&c<3000*0.0001/TS0 ){
-            w_ref=w_ref_req[1];
-//pc2.printf(".\r\n");
+        if( 0.26<=t && t<0.3 ){
+            vl.w_ref=w_ref_req[1];   // 目標速度をメインルーチンから速度制御メインループへ渡す。
+        }else{
+            vl.w_ref=w_ref_req[0];
         }
 #ifdef SIMULATION
         // 負荷トルク急変
-        if( c<4000*0.0001/TS0 ){
+        if( t<0.4 ){
             p.TL = 1;
         }else{
             p.TL = 2;
         }
 #endif
-        vl.w_ref = w_ref;               // 目標速度をメインルーチンから速度制御メインループへ渡す。
-        vl.tan_beta_ref = tan_beta_ref; // 目標電流位相をメインルーチンから速度制御メインループへ渡す。
 
 #ifdef OLD
         if( (++i2)>=(int)(TS1/TS0) ){  i2=0;
@@ -197,8 +185,8 @@
 #endif
 
 //        if( (ms=(int)(TS1*1000-(_count-c)*TS0*1000))<=0 ){    ms=1;}// ms=TS0
-        Thread::wait(ms);
 //        debug_p24 = 0;
+        Thread::wait(1);         // 1ms待つ
     }
 //pc2.printf("\r\n^0^ 2\r\n");
     // stop timers (OFF)
@@ -225,6 +213,7 @@
 //       fprintf( fp, "%d, ", data[i][1]*10000);
 //       fprintf( fp, "\r\n");
 //
+//        pc2.printf("%f, %f, %f, %f, %f\r\n", 
 //        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)
 //