File content as of revision 14:02411880ffb9:

//  controller.cpp: モータ制御器（位置制御、電流制御）
#include "mbed.h"   // mbedマイコンではstdio.hに相当
#include "QEI.h"    // エンコーダ用ライブラリを使用

#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(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);    // デバッグ用DA(アナログ信号をDA_PORTに出力)

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を点灯


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.


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.p = 2;        // 極対数
#endif
    // モータの初期値の設定
#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], モータ電圧

    // 制御器の初期値の設定
    vl.i_ref=0;         // 電流指令[A]
    vl.w_lpf = 0;       // 検出した速度[rad/s]
    vl.eI = 0;          // 速度制御用偏差の積分値（積分項）
    vl.e_old = 0;       // 速度制御用偏差の1サンプル過去の値
    il.eI = 0;          // 電流制御用偏差の積分値（積分項）
    il.e_old = 0;       // 電流制御用偏差の1サンプル過去の値
}

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;

    e = il.i_ref - p.i;     // 電流偏差の計算

    il.eI = il.eI + TS0*e;  // 電流偏差の積分項の計算

    ed = (e-il.e_old)/TS0;  // 電流偏差の微分値の計算
    il.e_old = e;           // 電流偏差の1サンプル過去の値を更新

    il.v_ref = iKP*e + iKI*il.eI + iKD*ed;  // ＰＩＤ制御器の出力を計算
}

void current_loop(){
// 電流制御マイナーループ、サンプル時間TS0秒
//      親関数： timerTS0()
//      子関数： i_control()
//      入力：指令電流 il.i_ref [A], 実電流 p.i [A]
//      出力：電圧指令 p.v [V]

#ifdef USE_CURRENT_CONTROL
    i_control();    // 電流制御（電流フィードバック制御）
#else
    il.v_ref = il.i_ref/iMAX*vMAX;  // 速度制御の出力をそのまま電圧指令にする
#endif
    // 電圧指令の大きさをMAX制限
    // アンチワインゴアップ：制御入力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.vに格納してv2Hbridge()に渡す
}


void vel_control(){
//  速度制御器：速度偏差が入力され、q軸電流指令を出力。
//      入力：指令速度 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;        // 速度偏差の計算
debug[1]=vl.w_ref/2/PI;//[Hz], for debug

    vl.eI = vl.eI + TS1*e;          // 速度偏差の積分値の計算

    ed = (e-vl.e_old)/TS1;          // 速度偏差の微分値の計算
    vl.e_old = e;                   // 速度偏差の1サンプル過去の値を更新

    vl.i_ref = wKp*e + wKi*vl.eI + wKd*ed; // ＰＩＤ制御器の出力を計算
}

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
    // 速度ωを求めるために、位置θをオイラー微分して、一次ローパスフィルタに通す。
    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制限
    // アンチワインドアップ：制御入力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にする
    }

    il.i_ref = vl.i_ref;    // 電流の目標値を速度制御メインループから電流制御マイナーループへ渡す。
}

void    v2Hbridge(){
// 指令電圧vより、PWM関数pwm_out()のパラメータ（dutyとフラグ）をセット。
//      親関数： timerTS0()
//      子関数： なし
//      入力：電圧指令 p.v [V]
//      出力：フルブリッジのfwdIN, rvsIN用duty,
//            デッドタイムフラグfDeadtime, モータ逆回転フラグfReverse
    float   duty;

    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;            // 逆回転フラグをオフにする
    }else{             // dutyがマイナスでモータが逆回転のとき
        IN.duty = -duty;    // dutyの絶対値をPWM関数pwm_out()に渡す
        if( IN.fReverse==0 ){       // モータが順回転から逆回転に切り替ったとき
            IN.fDeadtime = 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;
    // 電圧方程式より、指令電圧から電流を計算
    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 ){    // 速度が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;  // モータの機械的特性をシミュレートしてトルクから速度を計算

    // モータ角度θの計算
    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;   // ２π足す
}
#endif

void data2mbedUSB(){    // PC上のmbed USB ディスクにセーブするためのデータをTS3[s]ごとに代入    save data to mbed USB drive 
    if( _countTS3<1000 ){   // データ数が1,000の5種類のデータをメモリーに貯める
        data[_countTS3][0]=debug[0]; data[_countTS3][1]=debug[1];
        data[_countTS3][2]=vl.w_lpf; data[_countTS3][3]=_countTS0*TS0; data[_countTS3][4]=il.v_ref;
        _countTS3++;
    }
#if 0
  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]=_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(){    // タイマーtimerTS0()はTS0[s]ごとにコールされる   timer called every TS0[s].
//  debug_p26 = 1;
    _countTS0++;    // カウンターに1足す
    _time += TS0;   // 現在の時間にTS0[s]足す
    
    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 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(){    // タイマーtimerTS2()はTS2[s]ごとにコールされる   
}
void timerTS3(){    // タイマーtimerTS3()はTS3[s]ごとにコールされる   
    data2mbedUSB();  // PC上のmbed USB ディスクにセーブするためのデータをTS3[s]ごとに代入   data2mbedUSB() is called every TS3[s].
}
void timerTS4(){    // タイマーtimerTS4()はTS4[s]ごとにコールされる   
    display2PC();   //  PCのモニタ上のtera termに文字を表示 display to tera term on PC. display2PC() is called every TS4[s].
}