Diff: controller.cpp

Revision:

13:ba71733c11d7

Parent:

12:459af534d1ee

Child:

14:02411880ffb9

--- 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].
 }