Revision 12:459af534d1ee, committed 2013-01-04

Comitter:

kosaka

Date:

Fri Jan 04 12:00:48 2013 +0000

Parent:

11:9747752435d1

Child:

13:ba71733c11d7

Commit message:

DC motor control program using TA7291P type H bridge driver and rotary encoder with A, B phase.;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Hbridge.cpp	Fri Jan 04 12:00:48 2013 +0000
@@ -0,0 +1,122 @@
+#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
+
+Timeout pwm;
+
+DigitalOut pwm_upper = UPPER_PORT;
+DigitalOut pwm_lower = LOWER_PORT;
+
+pwm_parameters     IN; // UVW pwm の定数、変数
+
+DigitalOut  debug_p24(p24); // p17 for debug
+//DigitalOut  Led3(LED3);
+
+#if PWM_WAVEFORM==0   // 0: saw tooth wave comparison
+ #if 1
+void pwm_out() {    // pwm out using timer
+//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.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;
+    }
+//debug_p24=0;
+}
+ #else
+void pwm_out() {    // 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;}
+    }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;
+    }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;
+    }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;
+        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;
+        }
+    }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;
+    }
+}
+#endif
+
+
+void start_pwm(){
+    IN.duty = 0.0;
+    pwm_upper = pwm_lower = 0;
+    IN.mode = 0;
+    IN.fDeadtime = 1;
+    IN.fReverse[0] = 0;
+
+    pwm_out();
+}
+
+void stop_pwm(){
+    IN.duty = 0.0;
+    pwm_upper = pwm_lower = 0;
+    IN.mode = 0;
+    pwm.detach();
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Hbridge.h	Fri Jan 04 12:00:48 2013 +0000
@@ -0,0 +1,25 @@
+#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) *****************
+
+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?)
+}pwm_parameters;
+extern pwm_parameters   IN;    // H bridge pwm の定数、変数
+
+extern void start_pwm();
+extern void stop_pwm();
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/controller.cpp	Fri Jan 04 12:00:48 2013 +0000
@@ -0,0 +1,296 @@
+//  BLDCmotor.cpp: 各種３相同期モータに対するセンサあり運転のシミュレーション
+//      Kosaka Lab. 121215
+#include "mbed.h"
+#include "QEI.h"
+
+#include "controller.h"
+#include "Hbridge.h"
+Serial pc(USBTX, USBRX);        // Display on tera term in PC 
+
+motor_parameters            p;  // モータの定数、信号など
+current_loop_parameters     il; // 電流制御マイナーループの定数、変数
+velocity_loop_parameters    vl; // 速度制御メインループの定数、変数
+
+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).
+//      int         pulsesPerRev, Number of pulses in one revolution(=360 deg).
+//      Encoding    encoding = X2_ENCODING, X2 is default. X2 uses interrupts on the rising and falling edges of only channel A where as 
+//                    X4 uses them on both channels.
+//  )
+//  void     reset (void)
+//     Reset the encoder. 
+//  int      getCurrentState (void)
+//     Read the state of the encoder. 
+//  int      getPulses (void)
+//     Read the number of pulses recorded by the encoder. 
+//  int      getRevolutions (void)
+//     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]
+
+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);
+
+#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
+//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の機器定数等の設定, 制御器の初期化
+#ifdef SIMULATION
+    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;
+
+    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
+}
+
+void idq_control(){
+//  ｄｑ座標電流PID制御器（電流マイナーループのフィードバック制御）
+//      入力：指令電流 i_ref [A], 実電流 p.i [A], PI制御積分項 eI, サンプル時間 TS0 [s]
+//      出力：電圧指令 v_ref [A]
+    float  e;
+
+//debug[0]=il.i_ref;
+    // ｄｑ電流偏差の計算
+    e = il.i_ref - p.i;
+
+    // ｄｑ電流偏差の積分項の計算
+    il.eI_i = il.eI_i + TS0*e;
+
+    // ＰＩ制御
+    il.v_ref = iKP*e + iKI*il.eI_i;
+
+}
+
+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;
+#ifdef USE_CURRENT_CONTROL
+    idq_control();
+#else
+    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;
+    }
+    p.v = il.v_ref;
+
+    p.th[1] = p.th[0];  // thを更新
+}
+
+
+void vel_control(){
+//  速度制御器：速度偏差が入力され、q軸電流指令を出力。
+//      入力：指令速度 w_ref [rad/s], 実速度 w_lpf [rad/s], PI制御積分項 eI, サンプル時間 TS1 [s]
+//      出力：電流指令 i_ref [A]
+    float  e;
+
+    // 速度偏差の計算
+    e = vl.w_ref - vl.w_lpf;
+
+    // 速度偏差の積分値の計算
+    vl.eI_w = vl.eI_w + TS1*e;
+
+    // ＰＩ制御
+    vl.i_ref = wKp*e + wKi*vl.eI_w;
+}
+
+void velocity_loop(){   // 速度制御メインループ(w_ref&beta_ref to idq_ref)
+    float  tmp;
+
+    // 速度ωを求めるために、位置θをオイラー微分して、一次ローパスフィルタに通す。
+#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);
+#else
+    vl.w_lpf = p.th[0];
+#endif
+    // 速度制御：速度偏差が入力され、電流指令を出力。
+    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;
+    }
+//debug[0]=vl.eI_w;
+
+    // 電流の目標値を速度制御メインループから電流制御マイナーループへ渡す。
+    il.i_ref = vl.i_ref;
+//debug[0]=il.i_ref;
+}
+
+void    v2Hbridge(){ // vより、PWMを発生
+    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;
+        }
+        IN.fReverse[0] = 0;
+    }else{
+        IN.duty = -duty;
+        if( IN.fReverse[0]==0 ){
+            IN.fDeadtime = 1;
+        }
+        IN.fReverse[0] = 1;
+    }
+}
+
+#ifdef SIMULATION
+void    sim_motor(){
+//  モータシミュレータ
+//      入力信号：電圧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;
+
+    // モータトルクの計算
+    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
+        TL = p.w/(5*2*PI)*p.TL;
+
+    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;
+
+    if( p.th[0]<0 )
+        p.th[0] = p.th[0] + 2*PI;
+//debug[0]=p.v;
+}
+#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++;
+    }
+#if 0
+  if( _count_data<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;
+//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;
+    
+    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;
+}
+
+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
+//    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 timerTS3(){
+    data2mbedUSB();  // data2mbedUSB() is called every TS3[s].
+}
+void timerTS4(){
+    display2PC();  // display to tera term on PC. display2PC() is called every TS4[s].
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/controller.h	Fri Jan 04 12:00:48 2013 +0000
@@ -0,0 +1,106 @@
+#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 DA_PORT p18           // analog out (DA) port of mbed
+
+#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
+//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 iKP    10./2     // 電流制御PIDのPゲイン
+#define iKI    100./2    // 電流制御PIDのIゲイン
+
+#define vMAX  3.3
+
+    // 速度制御メインループ
+#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ゲイン
+#endif
+
+#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) ***************/
+
+
+typedef struct struct_motor_parameters{
+    // モータの定数、信号など
+ #ifdef SIMULATION // シミュレーションのとき
+    float  L;      // [H], インダクタンス
+    float  R;      // [Ω], モータ巻線抵抗
+    float  phi;    // [V s], 永久磁石の鎖交磁束
+    float  Jm;     // [Nms^2], イナーシャ
+    float  Tm;     // [Nm], モータトルク
+    float  TL;     // [Nm], 負荷トルク
+ #endif
+    float  th[2];  // [rad].   ロータの位置, th[0]=th(t), th[1]=th(t-TS0)
+    float  w;      // [rad/s], モータ速度
+    float  w_lpf;  // [rad/s], フィルタで高周波ノイズを除去したモータ速度
+    float  i; // [A], αβ軸電流 iab = [iα;iβ];
+    float  v;      // [V], motor 電圧
+    float  p;      // 極対数
+}motor_parameters;
+
+typedef struct struct_current_loop_parameters{
+    // 電流制御マイナーループの定数、変数
+    float  i_ref;      // iの目標値
+    float  v_ref; // vdqの目標値
+    float  eI_i;  // 電流制御用偏差の積分値（積分項）
+}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;           // 速度制御用偏差の積分値（積分項）
+}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 init_parameters();    // IPMSMの機器定数等の設定, 制御器の初期化
+
+extern unsigned long _count;   // sampling number
+extern float   _time;          // 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]
+
+#endif
\ No newline at end of file

--- a/main.cpp	Thu Nov 29 09:25:56 2012 +0000
+++ b/main.cpp	Fri Jan 04 12:00:48 2013 +0000
@@ -1,406 +1,66 @@
 //  DC motor control program using H-bridge driver (ex. TA7291P) and 360 resolution rotary encoder with A, B phase.
-//      ver. 121129a by Kosaka lab.
+//      ver. 121224 by Kosaka lab.
 #include "mbed.h"
 #include "rtos.h"
-#include "QEI.h"
-#define PI 3.14159265358979 // def. of PI
-/*********** User setting for control parameters (begin) ***************/
-//#define SIMULATION          // Comment this line if not simulation
-#define USE_PWM             // H bridge PWM mode: Vref=Vcc, FIN,2 = PWM or 0. Comment if use Vref=analog mode
-  #define PWM_FREQ 10000.0  //[Hz], pwm freq. available if USE_PWM is defined.
-#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
-//#define R_SIN               // Comment this line if r=step, not r = sin
-float   _freq_u = 0.3;      // [Hz], freq. of Frequency response, or Step response
-float   _rmax=100./180.*PI; // [rad], max. of reference signal
-float   _Kp4th=20;             // P gain for PID from motor volt. to angle.
-float   _Ki4th=20;             // I gain for PID from motor volt. to angle.
-float   _Kd4th=5;              // D gain for PID from motor volt. to angle.
-float   _Kp4i=10.0;            // P gain for PID from motor volt. to motor current.
-float   _Ki4i=10.0;            // I gain for PID from motor volt. to motor current.
-float   _Kd4i=0.0;             // D gain for PID from motor volt. to motor current.
-#define iTS     0.0001       // [s], iTS, sampling time[s] of motor current i control PID using timer interrupt
-#define thTS    0.001        // [s], thTS>=0.001[s], sampling time[s] of motor angle th PID using rtos-timer
-#define TS2     0.01        // [s], TS2>=0.001[s], sampling time[s] to save data to PC using thread. But, max data length is 1000.
-#define TMAX    10          // [s], experiment starts from 0[s] to TMAX[s]
-#define UMAX    3.3         // [V], max of control input u
-#define UMIN   -3.3         // [V], max of control input u
-#define IMAX    0.5         // [A], max of motor current i
-#define IMIN   -0.5         // [A], max of motor current i
-#define DEADTIME    0.0001  // [s], deadtime to be set between plus volt. to/from minus
-                        // H bridge port setting
-#define FIN_PORT    p21     // FIN (IN1) port of mbed
-#define RIN_PORT    p22     // RIN (IN2) port of mbed
-#define VREF_PORT   p18     // Vref      port of mbed (available if USE_PWM is not defined)
-DigitalOut  debug_p17(p17); // p17 for debug
-AnalogIn v_shunt_r(p19);    // *3.3 [V], Volt of shunt R_SHUNT[Ohm]. The motor current i = v_shunt_r/R_SHUNT [A]
-#define R_SHUNT     1.25    // [Ohm], shunt resistanse
-//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.
 
-#define N_ENC   (24*4)     // "*4": QEI::X4_ENCODING. Number of pulses in one revolution(=360 deg) of rotary encoder.
-QEI encoder (p29, p30, 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).
-//      int         pulsesPerRev, Number of pulses in one revolution(=360 deg).
-//      Encoding    encoding = X2_ENCODING, X2 is default. X2 uses interrupts on the rising and falling edges of only channel A where as 
-//                    X4 uses them on both channels.
-//  )
-//  void     reset (void)
-//     Reset the encoder. 
-//  int      getCurrentState (void)
-//     Read the state of the encoder. 
-//  int      getPulses (void)
-//     Read the number of pulses recorded by the encoder. 
-//  int      getRevolutions (void)
-//     Read the number of revolutions recorded by the encoder on the index channel. 
-/*********** User setting for control parameters (end) ***************/
+#include "controller.h"
+#include "Hbridge.h"
 
 
-Serial pc(USBTX, USBRX);        // Display on tera term in PC 
-LocalFileSystem local("local"); // save data to mbed USB disk drive in PC
-//Semaphore semaphore1(1);      // wait and release to protect memories and so on
-//Mutex stdio_mutex;            // wait and release to protect memories and so on
-Ticker controller_ticker;     // Timer interrupt using TIMER3, TS<0.001 is OK. Priority is higher than rtosTimer.
-
-#ifdef  USE_PWM             // H bridge PWM mode: Vref=Vcc, FIN,2 = PWM or 0.
-  PwmOut FIN(FIN_PORT);     // PWM for FIN, RIN=0 when forward rotation. H bridge driver PWM mode
-  PwmOut RIN(RIN_PORT);     // PWM for RIN, FIN=0 when reverse rotation. H bridge driver PWM mode
-#else                       // H bridge Vref=analog mode
-  DigitalOut  FIN(FIN_PORT);// FIN  for DC motor H bridge driver. FIN=1, RIN=0 then forward rotation
-  DigitalOut  RIN(RIN_PORT);// RIN  for DC motor H bridge driver. FIN=0, RIN=1 then reverse rotation
-#endif
-AnalogOut   analog_out(VREF_PORT);// Vref for DC motor H bridge driver. DA converter for control input [0.0-1.0]% in the output range of 0.0 to 3.3[V]
-
-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[10];      // for debug
-float   disp[10];       // for printf to avoid interrupted by quicker process
-#ifdef  GOOD_DATA
-float data[1000][5];    // memory to save data offline instead of "online fprintf".
-unsigned int    count3; // 
-unsigned int    count2=(int)(TS2/iTS); // 
-#endif
+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.
+//DigitalOut  debug_p24(p24); // p17 for debug
 
 extern "C" void mbed_reset();
 
-void u2Hbridge(float u){// input u to H bridge driver
-    float           duty;
-    unsigned int    f_deadtime, f_in, r_in;
-
-    if( u > 0 ){        // forward: rotate to plus
-        u += DEADZONE_PLUS; // deadzone compensation
-        duty = u/3.3;       // Vref
-        if(_f_u_plus==0){   // if plus to/from minus, set FIN=RIN=0/1 for deadtime 100[us].
-            f_deadtime = 1; // deadtime is required
-            _f_u_plus=1;
-        }else{
-            f_deadtime = 0; // deadtime is required
+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].
         }
-        f_in=1;  r_in=0;    // set forward direction
-    }else if( u < 0 ){  // reverse: rotate to minus
-        u += DEADZONE_MINUS;// deadzone compensation
-        duty = -u/3.3;
-        if(_f_u_plus==1){   // if plus to/from minus, set FIN=RIN=0/1 for deadtime 100[us].
-            f_deadtime = 1; // deadtime is required
-            _f_u_plus=0;
-        }else{
-            f_deadtime = 0; // deadtime is required
-        }
-        f_in=0;  r_in=1;    // set reverse direction
-    }else{// if( u == 0 ){  // stop mode
-        duty = 0;
-        f_deadtime = 0; // deadtime is required
-        f_in=0;  r_in=0;    // set FIN & RIN
-    }
-
-    if( f_deadtime==1 ){// making deadtime
-        FIN=0;  RIN=0;      // set upper&lower arm zero
-        wait(DEADTIME);
+        if( (ms=(int)(TS2*1000-(_count-c)*TS0*1000))<=0 ){    ms=1;}
+        Thread::wait(ms);
     }
-#ifdef USE_PWM  // H bridge PWM mode: Vref=Vcc, FIN,2 = PWM or 0
-    FIN = duty*(float)f_in;    RIN = duty*(float)r_in;  // setting pwm FIN & RIN
-    analog_out = 1;          // setting Vref=UMAX, but Vref=Vcc is better.
-#else           //       Analog mode: Vref=analog, FIN, RIN = 1 or 0)
-    FIN = f_in;    RIN = r_in;  // setting FIN & RIN
-    analog_out = duty;          // setting Vref : PID write DA, range is 0-1. Output voltage 0-3.3v
-#endif
-}
-
-void th_controller(void const *argument) {    // if rtos. current controller & velocity controller
-    float   e_old, wt;
-    float   y, u;
-
-//    y_old = _th;  // y_old=y(t-TS) is older than y by 1 sampling time TS[s]. update data
-#ifdef SIMULATION
-    y = _th + thTS/0.1*(0.2*_iref*100-_th);   //=(1-TS/0.1)*_y + 0.2*TS/0.1*_iref; // G = 0.2/(0.1s+1)
-#else
-//    semaphore1.wait();      //
-    y = (float)encoder.getPulses()/(float)N_ENC*2.0*PI;   // get angle [rad] from encoder
-//    semaphore1.release();   //
-#endif
-#define RMIN  0
-    wt = _freq_u *2.0*PI*_time;
-    if(wt>2.0*PI){    wt -= 2.0*PI*(float)((int)(wt/(2.0*PI)));}
-    _r = sin(wt ) * (_rmax-RMIN)/2.0 + (_rmax+RMIN)/2.0;
-//debug[0] =1;
-#ifndef R_SIN
-    if( _r>=(_rmax+RMIN)/2.0 ) _r = _rmax;
-    else        _r = 0;
-#endif
-    e_old = _e;     // e_old=e(t-TS) is older than e by 1 sampling time TS[s]. update data
-    _e = _r - y;    // error e(t)
-    if( _e<((360.0/N_ENC)/180*PI) && _e>-((360.0/N_ENC)/180*PI) ){   // e is inside minimum precision?
-        _e = 0;
-    }
-    if( _f_imax==0 ){   // u is saturated?
-//        if( _e>((360.0/N_ENC)/180*PI) || _e<-((360.0/N_ENC)/180*PI) ){   // e is inside minimum precision?
-            _eI = _eI + thTS*_e;     // integral of e(t)
-//        }
-    }
-    u = _Kp4th*_e + _Kd4th*(_e-e_old)/thTS + _Ki4th*_eI;   // PID output u(t)
- 
-#if CONTROL_MODE==1||CONTROL_MODE==2   // frequency response, or Step response
-    wt = _freq_u *2.0*PI*_time;
-    if(wt>2*PI)    wt -= 2*PI*(float)((int)(wt/2.0*PI));
-    u = sin(wt ) * (UMAX-UMIN)/2.0 + (UMAX+UMIN)/2.0;
-#endif
-#if CONTROL_MODE==2   // Step response
-    if( u>=0 )  u = UMAX;
-    else        u = UMIN;
-#endif
-#if CONTROL_MODE==3                 // u=rand() to identify motor transfer function G(s) from V to angle
-    if(count2==(int)(TS2/iTS)){
-        u = ((float)rand()/RAND_MAX*2.0-1.0) * (UMAX-1.5)/2.0 + (UMAX+1.5)/2.0;
-    }else{
-        u = _iref;
-    }
-#endif
-#if CONTROL_MODE==4                 // FFT identification, u=repetive signal
-    if(count2==(int)(TS2/thTS)){
-        u = data[count3][4];
-    }else{
-        u = _iref;
-    }
-#endif
-    // u is saturated? for anti-windup
-    if( u>IMAX ){
-        _eI -= (u-IMAX)/_Ki4th;    if(_eI<0){   _eI=0;}
-        u = IMAX;
-//        _f_imax = 1;
-    } else if( u<IMIN ){
-        _eI -= (u-IMIN)/_Ki4th;    if(_eI>0){   _eI=0;}
-        u = IMIN;
-//        _f_imax = 1;
-    }else{
-        _f_imax = 0;
-    }
-    //-------- update data
-    _th = y;
-    _iref = u;
 }
-void i_controller() {    // if ticker. current controller & velocity controller
-    void    u2Hbridge(float);    // input u to H bridge (full bridge) driver
-#ifdef USE_CURRENT_CONTROL
-    float   e_old;
-    float   y, u;
-
-//    _iref=_r*180/PI;  // step response from v to i, useful to tune PID gains.
-    debug_p17 = 1;  // for debug: processing time check
-//    if(debug_p17 == 1)  debug_p17=0;else    debug_p17=1;  // for debug: sampling time check
-
-    _count+=1;
-    // current PID controller
-    y = v_shunt_r/R_SHUNT;      // get i [A] from shunt resistance
-    if(_f_u_plus==0){   y=-y;}
-
-    e_old = _ei;     // e_old=e(t-TS) is older than e by 1 sampling time TS[s]. update data
-    _ei = _iref - y;    // error e(t)
-    if( _f_umax==0 ){
-        _eiI = _eiI + iTS*_ei;     // integral of e(t)
-    }
-    u = _Kp4i*_e + _Kd4i*(_ei-e_old)/iTS + _Ki4i*_eiI;   // PID output u(t)
-
-     // u is saturated? for anti-windup
-    if( u>UMAX ){
-        _eiI -= (u-UMAX)/_Ki4i;    if(_eiI<0){   _eiI=0;}
-        u = UMAX;
-//        _f_umax = 1;
-    } else if( u<UMIN ){
-        _eiI -= (u-UMIN)/_Ki4i;    if(_eiI>0){   _eiI=0;}
-        u = UMIN;
-//        _f_umax = 1;
-    }else{
-        _f_umax = 0;
-    }
-    //-------- update data
-    _i = y;
-    _u = u;
-#else
-    _u = _iref/IMAX*VMAX;     // without current control.
-#endif
-
-    u2Hbridge(_u);  // input u to TA7291 driver
-
-    //-------- update data
-    _time += iTS;    // time
-debug[0]=v_shunt_r; if(_f_u_plus==0){   debug[0]=-debug[0];}
-#ifdef  GOOD_DATA
-    if(count2==(int)(TS2/iTS)){
-//        j=0; if(_count>=j&&_count<j+1000){i=_count-j;  data[i][0]=_r; data[i][1]=debug[0]; data[i][2]=_th; data[i][3]=_time; data[i][4]=_u;}
-        if( count3<1000 ){
-            data[count3][0]=_r; data[count3][1]=debug[0]; data[count3][2]=_th; data[count3][3]=_time; data[count3][4]=_u;
-//            data[count3][0]=_iref; data[count3][1]=debug[0]; data[count3][2]=_i; data[count3][3]=_time; data[count3][4]=_u;
-            count3++;
+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].
         }
-        count2 = 0;
-    }
-    count2++;
-#endif
-    //-------- update data
-
-    debug_p17 = 0;  // for debug: processing time check
-}
-
-void main1() {
-    RtosTimer timer_controller(th_controller);
-    FILE *fp;   // save data to PC
-#ifdef  GOOD_DATA
-    int i;
-
-    count3=0;
-#endif
-    u2Hbridge(0);           // initialize H bridge to stop mode
-    _count=0;
-    _time = 0;  // time
-    _eI = _eiI = 0;    // reset integrater
-    encoder.reset();    // set encoder counter zero
-    _th = (float)encoder.getPulses()/(float)N_ENC*2.0*PI;   // get angle [rad] from encoder
-    _r = _r + _th;
-//    if( _r>2*PI )    _r -= _r-2*PI;
-
-    pc.printf("Control start!!\r\n");
-    if ( NULL == (fp = fopen( "/local/data.csv", "w" )) ){   error( "" );} // save data to PC
-#ifdef  USE_PWM
-    FIN.period( 1.0 / PWM_FREQ );   // PWM period [s]. Common to all PWM
-#endif
-    controller_ticker.attach(&i_controller, iTS ); // Sampling period[s] of i_controller
-    timer_controller.start((unsigned int)(thTS*1000.));   // Sampling period[ms] of th controller
-
-//    for ( i = 0; i < (unsigned int)(TMAX/iTS2); i++ ) {
-    while ( _time <= TMAX ) {
-        // BUG!! Dangerous if TS2<0.1 because multi interrupt by fprintf is not prohibited! 1st aug of fprintf will be destroyed.
-        //     fprintf returns before process completed.
-//BUG   fprintf( fp, "%8.2f, %8.4f,\t%8.1f,\t%8.2f\r\n", disp[3], disp[1], disp[0], tmp);  // save data to PC (para, y, time, u)
-//OK?   fprintf( fp, "%f, %f, %f, %f, %f\r\n", _time, debug[0], debug[3], (_y/(2*PI)*360.0),_u);  // save data to PC (para, y, time, u)
-#ifndef GOOD_DATA
-        fprintf( fp, "%f, %f, %f, %f, %f\r\n", _r, debug[0], _th, _time, _u);  // save data to PC (para, y, time, u)
-#endif
-        Thread::wait((unsigned int)(TS2*1000.));  //[ms]
-    }
-    controller_ticker.detach(); // timer interrupt stop
-    timer_controller.stop();    // rtos timer stop
-    u2Hbridge(0);           // initialize H bridge to stop mode
-    _eI = _eiI = 0;    // reset integrater
-#ifdef  GOOD_DATA
-    for(i=0;i<1000;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)
-#endif
-    fclose( fp );               // release mbed USB drive
-    pc.printf("Control completed!!\r\n\r\n");
-}
-
-void thread_print2PC(void const *argument) {
-    while (true) {
-        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
-        Thread::wait(200);
+        if( (ms=(int)(TS3*1000-(_count-c)*TS0*1000))<=0 ){    ms=1;}
+        Thread::wait(ms);
     }
 }
 
-void main2(void const *argument) {
-#if CONTROL_MODE==0     // PID control
-    char    f;
-    float   val;
-#endif
-#if CONTROL_MODE==4                 // FFT identification, u=repetive signal
-    int i, j;
-    float   max_u;
-#endif
-
-    while(true){
-#if CONTROL_MODE==4                 // FFT identification, u=repetive signal
-        max_u = 0;
-        for( i=0;i<1000;i++ ){  // u=data[i][4]: memory for FFT identification input signal.
-            data[i][4] = sin(_freq_u*2*PI * i*TS2);   // _u_freq = 10/2 * i [Hz]
-            if( data[i][4]>max_u ){   max_u=data[i][4];}
+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].
         }
-        for( j=1;j<50;j++ ){
-            for( i=0;i<1000;i++ ){
-                data[i][4] += sin((float)(j+1)*_freq_u*2*PI * i*TS2);
-                if( data[i][4]>max_u ){   max_u=data[i][4];}
-            }
-        }
-        for( i=0;i<1000;i++ ){
-//            data[i][4] *= UMAX/max_u;
-            data[i][4] = (data[i][4]/max_u+3)/4*UMAX;
-        }
-#endif
-        main1();
+        if( (ms=(int)(TS4*1000-(_count-c)*TS0*1000))<=0 ){    ms=1;}
+        Thread::wait(ms);
+    }
+}
 
-#if CONTROL_MODE>=1     // frequency response, or Step response
-        pc.printf("Input u(t) Frequency[Hz]? (if 9, reset mbed)...");
-        pc.scanf("%f",&_freq_u);
-        pc.printf("%8.3f[Hz]\r\n", _freq_u);  // print to tera term
-        if(_freq_u==9){    mbed_reset();}
-#else                   // PID control
-//  #ifdef R_SIN
-//        pc.printf("Reference signal r(t) Frequency[Hz]?...");
-//        pc.scanf("%f",&_freq_u);
-//        pc.printf("%8.3f[Hz]\r\n", _freq_u);  // print to tera term
-//  #endif
-        pc.printf("th-loop: Kp=%f, Ki=%f, Kd=%f, r=%f[deg], %f Hz\r\n",_Kp4th, _Ki4th, _Kd4th, _rmax*180./PI, _freq_u);
-        pc.printf(" i-loop: Kp=%f, Ki=%f, Kd=%f\r\n",_Kp4i, _Ki4i, _Kd4i);
-        pc.printf("Which number do you like to change?\r\n ... 0)no change, 1)Kp, 2)Ki, 3)Kd, 4)r(t) freq.[Hz], 5)r(t) amp.[deg].\r\n     6)iKp, 7)iKi, 8)iKd, 9)reset mbed ?");
-        f=pc.getc()-48; //int = char-48
-        pc.printf("\r\n    Value?... ");
-        if(f>=1&&f<=8){ pc.scanf("%f",&val);}
-        pc.printf("%8.3f\r\n", val);  // print to tera term
-        if(f==1){    _Kp4th = val;}
-        if(f==2){    _Ki4th = val;}
-        if(f==3){    _Kd4th = val;}
-        if(f==4){    _freq_u = val;}
-        if(f==5){    _rmax = val/180.*PI;}
-        if(f==6){    _Kp4i = val;}
-        if(f==7){    _Ki4i = val;}
-        if(f==8){    _Kd4i = val;}
-        if(f==9){    mbed_reset();}
-        pc.printf("th-loop: Kp=%f, Ki=%f, Kd=%f, r=%f[deg], %f Hz\r\n",_Kp4th, _Ki4th, _Kd4th, _rmax*180./PI, _freq_u);
-        pc.printf(" i-loop: Kp=%f, Ki=%f, Kd=%f\r\n",_Kp4i, _Ki4i, _Kd4i);
-#endif
-    }    
-}
-int main() {
-//    void main1();
-    Thread save2PC(main2,NULL,osPriorityBelowNormal);
-    Thread print2PC(thread_print2PC,NULL,osPriorityLow);
-
-//    osStatus set_priority(osPriority osPriorityBelowNormal );
-// Priority of Thread (RtosTimer has no priority?)
+//#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);
+// Priority of Thread (RtosTimer is osPriorityAboveNormal)
 //  osPriorityIdle          = -3,          ///< priority: idle (lowest)--> then, mbed ERROR!!
 //  osPriorityLow           = -2,          ///< priority: low
 //  osPriorityBelowNormal   = -1,          ///< priority: below normal
@@ -409,4 +69,70 @@
 //  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;   // current time
+
+    // IPMSMの機器定数等の設定, 制御器の初期化
+    init_parameters();
+
+    // シミュレーション開始
+    pc2.printf("Simulation start!!\r\n");
+#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
+#endif
+
+    while( (t = _count*TS0) < TMAX ){
+//        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];
+        }
+#ifdef SIMULATION
+        // 負荷トルク急変
+        if( t<3.4 ){
+            p.TL = 1;
+        }else{
+            p.TL = 2;
+        }
+#endif
+
+#ifdef OLD
+        if( (++i2)>=(int)(TS1/TS0) ){  i2=0;
+            timerTS1(&j);   //velocity_loop();    // 速度制御メインループ(w_ref&beta_ref to idq_ref)
+        }
+#endif
+
+#ifdef OLD
+        timerTS0();
+#endif
+
+//        debug_p24 = 0;
+        Thread::wait(1);
+    }
+    // stop timers (OFF)
+    stop_pwm();
+    TickerTimerTS0.detach(); // timer interrupt stop
+    RtosTimerTS1.stop();    // rtos timer stop
+    fTimerTS3ON=0;//ThreadTimerTS3.terminate();   // 
+    fTimerTS4ON=0;//ThreadTimerTS4.terminate();   // 
+
+    // save data to mbed USB drive
+    for(i=0;i<_count_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)
+    }
+    fclose( fp );               // release mbed USB drive
+    
+    Thread::wait(100);
+    pc2.printf("Control completed!!\r\n\r\n");
 }