「「mbedマイコンによるモータ制御(14)設計法」近畿大学小坂学」を読む

mbedマイコンによるモータ制御設計法」近畿大学小坂学
https://www.amazon.co.jp/dp/4904774108/

補足
https://docs.google.com/viewer?a=v&pid=sites&srcid=ZGVmYXVsdGRvbWFpbnxrb3Nha2EzbGFifGd4OjM4YzU5YzAzZGU3Nzk2NGQ

mbed

mbed NXP LPC1768

driver

driver
http://mbed.org/handbook/Windows-serial-configuration

Tera Term

teraterm
http://sourceforge.jp/projects/ttssh2/releases/

DCmotor

DC moter
http://www.mec.kindai.ac.jp/mech/lab/kosaka

Matlab

MATLAB
https://jp.mathworks.com/products/matlab.html

Scilab

SCILAB
http://www.scilab.org

Mat@Scilab

Mat@Scilab
https://www.vector.co.jp/soft/data/edu/se494955.html

制御(control)

比例(P)制御(proportional control)

PI制御(proportional Integral control)

PD制御(proportional differential control)

PID制御(proportional integral differential control)

算譜(source code)

code
http://www.mec.kindai.ac.jp/mech/lab/kosaka
https://os.mbed.com/users/kosakaLab/code/BLDCmotor/

source code	size
UVWpwm.cpp	12699
UVWpwm.h	1863
controller.cpp	19222
controller.h	6929
fast_math.cpp	2520
fast_math.h	837
main.cpp	9555

main.cpp p117, p118

main.cpp

//  UVW three phases Blushless DC motor control program using 3 H-bridge driver (ex. TA7291P) and incremental rotary encoder with A, B phase.
//      ver. 130903 by Kosaka lab.
#include "mbed.h" /// p.117
#include "rtos.h"
 
#include "fast_math.h"
#include "controller.h"
#include "UVWpwm.h"
 
Serial pc2(USBTX, USBRX);        // Display on tera term in PC 
LocalFileSystem local("mbedUSBdrive"); // save data to mbed USB disk drive in PC /// p.118
//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 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.
 
extern "C" void mbed_reset();
 
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].
        }
        if( (ms=(int)(TS2*1000-(_count-c)*TS0*1000))<=0 ){    ms=1;}
        Thread::wait(ms);
    }
}
void CallTimerTS3(void const *argument) {   // make sampling time TS3 timer (priority 3: precision 4ms)
    int ms;
    unsigned long c;
    while (true) {
        c = _count;
        if( fTimerTS3ON ){
            timerTS3();  // timerTS3() is called every TS3[s].
        }
        if( (ms=(int)(TS3*1000-(_count-c)*TS0*1000))<=0 ){    ms=1;}
        Thread::wait(ms);
    }
}
 
void CallTimerTS4(void const *argument) {   // make sampling time TS4 timer (priority 4: precision 4ms)
    int ms;
    unsigned long c;
    while (true) {
        c = _count;
        if( fTimerTS4ON ){
            timerTS4();  // timerTS4() is called every TS4[s].
        }
        if( (ms=(int)(TS4*1000-(_count-c)*TS0*1000))<=0 ){    ms=1;}
        Thread::wait(ms);
    }
}
 
//void stop_fprintf(){ // emergencyStop if fprintf keep busy for 3 secons
//    fclose(fp);
//    pc2.printf("error: fprintf was in hung-up!");
//}
 
//#define OLD
int main(){
    unsigned short  i, i2=0;
    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);
    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
//  osPriorityNormal        =  0,          ///< priority: normal (default)
//  osPriorityAboveNormal   = +1,          ///< priority: above normal
//  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] = {20* 2*PI, 40* 2*PI};        // [rad/s]（第2要素は指令速度急変後の指令速度）
    float  w_ref_req[2] = {5* 2*PI, 10* 2*PI};        // [rad/s]（第2要素は指令速度急変後の指令速度）
    // 指令ｄｑ電流位相
//    float  beta_ref = 30*PI/180;   // [rad], 電流位相指令βを30度に
    float  beta_ref = 35*PI/180;   // [rad], 電流位相指令βを30度に
    float  tan_beta_ref1;
    float  tan_beta_ref2,tan_beta_ref;
    float  t;   // current time
    extern void    velocity_loop();
 
//    start_timers(1); // start multi-timers, sampling times are TS0, TS1, TS2, TS3, TS4.
 
    // 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);           // 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
    // 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
    
    // set th by moving rotor to th=zero.
    f_find_origin=1;    // 回転角原点復帰フラグをセット
    while( (t = _count*TS0) < TMAX_FIND_ORIGIN ){  // TMAX秒経過するまで制御実行
        if     (t<1)    p.th_const =  0*2*PI*t*1;       // Set theta with constant angular velosity
        else if(t<3)    p.th_const =  9*2*PI*t*1;       // Set theta with constant angular velosity
        else if(t<5)    p.th_const = 15*2*PI*t*1;       // Set theta with constant angular velosity
        else if(t<TMAX_FIND_ORIGIN-2)
                        p.th_const =  9*2*PI*t*1;       // Set theta with constant angular velosity
        else            p.th_const =  0*2*PI*t*1;       // Set theta with constant angular velosity
#ifdef OLD
        timerTS0();
#endif
        Thread::wait(1);
    }
    // IPMSMの機器定数等の設定, 制御器の初期化
    init_parameters();
    f_find_origin=0;
 
#ifndef OLD
    TickerTimerTS0.detach(); // timer interrupt stop
    // start control (ON)
    TickerTimerTS0.attach(&timerTS0, TS0 ); // Sampling period[s] of i_controller
    RtosTimerTS1.start((unsigned int)(TS1*1000.));   // Sampling period[ms] of th controller
#endif
 
    while( (t = _count*TS0-TMAX_FIND_ORIGIN) < TMAX ){
//        debug_p24 = 1;
 
        // 電流位相（ｄｑ軸電流）を変化させるベクトル制御
        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;
 
#if 0
        // 速度急変
        if( 0.26<=t && t<0.3 ){
            vl.w_ref=w_ref_req[1];   // 目標速度をメインルーチンから速度制御メインループへ渡す。
        }else{
            vl.w_ref=w_ref_req[0];
        }
#else
        vl.w_ref=0.999*vl.w_ref + (1-0.999)*w_ref_req[0];
#endif
#ifdef SIMULATION
        // 負荷トルク急変
        if( t<0.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();
        //current_loop(); // 電流制御マイナーループ(idq_ref to vuvw)
        //vuvw2pwm();     // vuvw to pwm
        //sim_motor();    // IPM, ｄｑ座標
#endif
 
//        if( (ms=(int)(TS1*1000-(_count-c)*TS0*1000))<=0 ){    ms=1;}// ms=TS0
//        debug_p24 = 0;
        Thread::wait(1);         // 1ms待つ
    }
//pc2.printf("\r\n^0^ 2\r\n");
    // stop timers (OFF)
    stop_pwm();
    TickerTimerTS0.detach(); // timer interrupt stop
    RtosTimerTS1.stop();    // rtos timer stop
//    Thread::wait(1000); // wait till timerTS3 completed
    fTimerTS3ON=0;//ThreadTimerTS3.terminate();   // 
    fTimerTS4ON=0;//ThreadTimerTS4.terminate();   // 
//pc2.printf("\r\n^0^ 0\r\n\r\n");
 
    // save data to mbed USB drive
//    if ( NULL == (fp = fopen( "/mbedUSBdrive/data.csv", "w" )) ){   error( "" );} // save data to PC
//pc2.printf("\r\n^0^ %d\r\n\r\n",_count_data);
//    emergencyStop.attach(&stop_fprintf, 0.001); // emergencyStop if fprintf keep busy for 3 secons
    for(i=0;i<_count_data;i++){
//pc2.printf("%d: ",i);
//pc2.printf("%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)
//        sprintf(&chr[0],"Temperature: f ºC\r\n");//,data[i][0]);
//        sprintf(&chr[0],"%d, ", data[i][1]);
//        fprintf(fp,&chr[0]); 
//               fprintf( fp, ", ");
//       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)
//
//        wait(0.1);
//        Thread::wait(100);
    }
//pc2.printf("\r\n^0^ 2\r\n\r\n");
    fclose( fp );               // release mbed USB drive
    
    Thread::wait(100);
    pc2.printf("Control completed!!\r\n\r\n");
}

controller.cpp p117

controller.cpp

//  controller.cpp: 各種３相同期モータに対するセンサあり運転のシミュレーション
//      Kosaka Lab. 130905
#include "mbed.h"
#include "QEI.h"
 
#include "controller.h"
#include "UVWpwm.h"
#include "fast_math.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);
 
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);
 
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;
 
//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の機器定数等の設定, 制御器の初期化
    float  r2, r3;
 
 
    // 対象の機器定数 PA 5HP scroll from IPEC2000 "High Efficiency Control for Interior Permanent Magnet Synchronous Motor"
    // outside diameter of stator   150 mm
    // outside diameter of rotor    84.0 mm
    // width of rotor           70.0 mm
    // maximum speed        7500 r/min  (min=900rpm)
    // maximum torque       15.0 Nm
    // Ψa           0.176 Wb
    // Ld           3.50 mH
    // Lq           6.30 mH
    // Ra           0.143Ω
    // Rc           200Ω
#ifdef SIMULATION
    p.Ld = 0.0035;  // H
    p.Lq = 0.0063;  // H
    p.Lq0 = p.Lq;
    p.Lq1 = 0;
    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.iab[0] =0;    p.iab[1] = 0;   // iab = [iα;iβ];
    p.vab[0] =0;    p.vab[1] = 0;   // vab = [vα;vβ];
        // UVW座標からαβ座標への変換行列Cuvwの設定
    r2 = sqrt(2.);//1.414213562373095;//2^(1/2);
    r3 = sqrt(3.);//1.732050807568877;//3^(1/2);
    //  p.Cuvw =[ r2/r3 -1/r2/r3 -1/r2/r3; ...
    //                0     1/r2 -1/r2   ];
    p.Cuvw[0][0] = r2/r3;   p.Cuvw[0][1] = -1./r2/r3;   p.Cuvw[0][2] = -1./r2/r3;
    p.Cuvw[1][0] =     0;   p.Cuvw[1][1] =  1/r2   ;    p.Cuvw[1][2] = -1./r2;
 
    p.w = 0;
 
    // 制御器の初期化
    vl.iq_ref=0;        // ｑ軸電流指令[A]
    vl.w_lpf = 0;       // 検出した速度[rad/s]
    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, サンプル時間 TS0 [s]
//      出力：dq軸電圧指令 vdq_ref [A]
//       [vdq_ref,eI_idq] = idq_control(idq_ref,idq_act,eI_idq,ts);
    float  e[2], ed[2];
 
    // ｄｑ電流偏差の計算
    e[0] = il.idq_ref[0] - idq_act[0];
    e[1] = il.idq_ref[1] - idq_act[1];
 
    // ｄｑ電流偏差の積分項の計算
    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] = 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, prev[2];
  if( f_find_origin==1 ){
    th = p.th_const;
  }else{
    th = p.th[0];
  }
 
    // αβ座標からｄｑ座標への変換行列Cdqの設定
#if 1   //BUG!! if move sqrt2 to fast_math.h, sim starts and completed without working!?
    c = cos(th);
    s = sin(th);
#else
    c = (float)(_cos(th/(PI/3.)*(float)DEG60+0.5))/65535.;
    s = (float)(_sin(th/(PI/3.)*(float)DEG60+0.5))/65535.;
#endif
    Cdq[0][0]= c;   Cdq[0][1]=s;    //Cdq ={{ c, s}
    Cdq[1][0]=-s;   Cdq[1][1]=c;    //       {-s, c]};
 
    iu = p.iuvw[0];
    iv = p.iuvw[1];
//  iw = -(iu + iv);    // iu+iv+iw=0であることを利用してiw を計算
 
    // iab = p.Cuvw*[iu;iv;iw];
//  iab[0] = p.Cuvw[0][0]*iu + p.Cuvw[0][1]*iv + p.Cuvw[0][2]*iw;
//  iab[1] = p.Cuvw[1][0]*iu + p.Cuvw[1][1]*iv + p.Cuvw[1][2]*iw;
//  iab[0] = p.Cuvw[0][0]*iu + p.Cuvw[0][1]*(iv+iw);
//  iab[1] =                   p.Cuvw[1][1]*(iv-iw);
    iab[0] = (p.Cuvw[0][0]-p.Cuvw[0][1])*iu;
    iab[1] = p.Cuvw[1][1]*(iu+2*iv);
 
    // αβ座標電流をｄｑ座標電流に変換
    //idq_act = Cdq * iab;
    idq_act[0] = Cdq[0][0]*iab[0] + Cdq[0][1]*iab[1];
    idq_act[1] = Cdq[1][0]*iab[0] + Cdq[1][1]*iab[1];
 
    // dq電流制御（電流フィードバック制御）
//  [vdq_ref,eI_idq] = idq_control(idq_ref,idq_act,eI_idq,ts);
#ifdef USE_CURRENT_CONTROL
    idq_control(idq_act);
#else
    il.vdq_ref[0] = il.idq_ref[0]/iqMAX*vdqMAX;
    il.vdq_ref[1] = il.idq_ref[1]/iqMAX*vdqMAX;
#endif
    // ｄｑ軸電圧指令ベクトルの大きさをMAX制限してアンチワインドアップ対策
    // if( norm(vdq_ref) > vdqmax ){    vdq_ref= vdqmax/norm(vdq_ref)*vdq_ref}
    // 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
//analog_out=il.vdq_ref[1]/3.3+0.4;//koko
    // ｄｑ座標指令電圧 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];
    vab_ref[1] = Cdq[0][1]*il.vdq_ref[0] + Cdq[1][1]*il.vdq_ref[1];
//analog_out=vab_ref[1]/3.3+0.4;
 
    // モータに印加するUVW相電圧を計算 （vα, vβからvu, vv, vwを計算）
    //  vu = √(2/3)*va;
    //  vv = -1/√6*va + 1/√2*vb;
    //  vw = -1/√6*va - 1/√2*vb;
    //  p.Cuvw =[ r2/r3 -1/r2/r3 -1/r2/r3; ...
    //                0     1/r2 -1/r2   ];
    // p.vuvw = p.Cuvw'*vab_ref;
    p.vuvw[0] = p.Cuvw[0][0]*vab_ref[0];
    p.vuvw[1] = p.Cuvw[0][1]*vab_ref[0] + p.Cuvw[1][1]*vab_ref[1];
    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];
}
 
 
void vel_control(){
//  速度制御器：速度偏差が入力され、q軸電流指令を出力。
//      入力：指令速度 w_ref [rad/s], 実速度 w_lpf [rad/s], PI制御積分項 eI, サンプル時間 TS1 [s]
//      出力：q軸電流指令 iq_ref [A]
//       [iq_ref,eI] = vel_control(w_ref,w_lpf,eI,ts);
    float  e, ed;
 
    // 速度偏差の計算
    e = vl.w_ref - vl.w_lpf;
 
    // 速度偏差の積分値の計算
    vl.eI = vl.eI + TS1*e;
 
    ed = (e-vl.e_old)/TS1;          // 速度偏差の微分値の計算
    vl.e_old = e;                   // 速度偏差の1サンプル過去の値を更新
 
    // ＰＩ制御
    vl.iq_ref = wKp*e + wKi*vl.eI + wKd*ed; // ＰＩＤ制御器の出力を計算
}
 
void velocity_loop(){   // 速度制御メインループ(w_ref&beta_ref to idq_ref)
    float  tmp, idq_ref[2];
 
    // 速度ωを求めるために、位置θをオイラー微分して、一次ローパスフィルタに通す。
    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=vl.w_lpf/(2*PI) /20; if(tmp>1) tmp=1;else if(tmp<0) tmp=0;
analog_out=tmp;//tmp;//koko
 
    // 速度制御：速度偏差が入力され、q軸電流指令を出力。
//  [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;
    }
 
    // 電流ベクトル制御
    if( vl.iq_ref>=0 ){ tmp = vl.tan_beta_ref;}     // 負のトルクを発生させるときはidは負のままでiqを正から負にする
    else{           tmp = -vl.tan_beta_ref;}// Tm = p((phi+(Ld-Lq)id) iqより
    //idq_ref = {{-tmp, 1}}*iq_ref;
    idq_ref[0] = -tmp*vl.iq_ref;    idq_ref[1] = vl.iq_ref;
 
    // ｄｑ軸電流の目標値を速度制御メインループから電流制御マイナーループへ渡す。
    il.idq_ref[0] = idq_ref[0];
    il.idq_ref[1] = idq_ref[1];
  if( f_find_origin==1 ){
    il.idq_ref[0] = iqMAX/1.0;  // idをプラス、iqをゼロにして、
    il.idq_ref[1] = 0;          // 無負荷のときにθ=0とさせる。
  }
}
 
void    vuvw2pwm(){ // vu, vv, vwより、UVW相の上アームPWMを発生
    float   duty_u, duty_v, 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
void    sim_motor(){
//  モータシミュレータ
//      入力信号：UVW相電圧p.vuvw [V]、負荷トルクp.TL [Nm]
//      出力信号：モータ角度p.th[0] [rad], モータ速度p.w [rad/s], モータUVW相電流p.iuvw [A]
//      p = motor(p, ts);   // IPM, ｄｑ座標
    float c, s, Cdq[2][2], idq_dot[2], id,iq, vdq[2], idq[2], Tall,TL, Cdq_inv[2][2];
analog_out=p.vuvw[0]/100.+0.5;//debug
    // vu, vv, vwからvα, vβを計算
    p.vab[0] = p.Cuvw[0][0]*p.vuvw[0] + p.Cuvw[0][1]*p.vuvw[1] + p.Cuvw[0][2]*p.vuvw[2];
    p.vab[1] = p.Cuvw[1][0]*p.vuvw[0] + p.Cuvw[1][1]*p.vuvw[1] + p.Cuvw[1][2]*p.vuvw[2];
//printf("vab=%f, %f ",p.vab[0],p.vab[1]);scanf("%f",&c);
 
    // αβ座標からｄｑ座標への変換行列Cdqの設定
    c = cos(p.th[0]);
    s = sin(p.th[0]);
    // Cdq =[ c s; ...
    //       -s c];
    Cdq[0][0] = c;  Cdq[0][1] = s;
    Cdq[1][0] =-s;  Cdq[1][1] = c;
 
    // ｖα, ｖβからｖｄ, ｖｑを計算
    //  vd = c*p.va + s*p.vb;
    //  vq =-s*p.va + c*p.vb;
    // vdq = Cdq * p.vab;
    vdq[0] = Cdq[0][0]*p.vab[0] + Cdq[0][1]*p.vab[1];
    vdq[1] = Cdq[1][0]*p.vab[0] + Cdq[1][1]*p.vab[1];
 
    // iα, iβからid, iqを計算
    //  id = c*p.ia + s*p.ib;
    //  iq =-s*p.ia + c*p.ib;
    // idq = Cdq * p.iab;
    idq[0] = Cdq[0][0]*p.iab[0] + Cdq[0][1]*p.iab[1];
    idq[1] = Cdq[1][0]*p.iab[0] + Cdq[1][1]*p.iab[1];
 
    // get id,iq
    //  id_dot = (1.0/p.Ld) * ( vd - (p.R*id - p.w*p.Lq*iq) );
    //  iq_dot = (1.0/p.Lq) * ( vq - (p.R*iq + p.w*p.Ld*id + p.w*p.phi) );
    // idq_dot = [p.Ld 0;0 p.Lq]\( vdq - p.R*idq - p.w*[0 -p.Lq;p.Ld 0]*idq - p.w*[0;p.phi]);
    idq_dot[0] = (1.0/p.Ld) * ( vdq[0] - (p.R*idq[0] - p.w*p.Lq*idq[1]) );
    idq_dot[1] = (1.0/p.Lq) * ( vdq[1] - (p.R*idq[1] + p.w*p.Ld*idq[0] + p.w*p.phi) );
    //  id = id + ts * id_dot;
    //  iq = iq + ts * iq_dot;
    p.idq[0] = idq[0] + TS0*idq_dot[0];
    p.idq[1] = idq[1] + TS0*idq_dot[1];
    id = p.idq[0];
    iq = p.idq[1];
 
    // 磁気飽和を考慮したLqの計算
    p.Lq = p.Lq0 + p.Lq1*abs(iq);
    if( p.Lq < p.Ld )
        p.Lq = p.Ld;
 
    // モータトルクの計算
    p.Tm = p.p * (p.phi + (p.Ld-p.Lq)*id) * iq;
 
    // モータ速度ωの計算
    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]>4*PI)
        p.th[0] = p.th[0] - 4*PI;
 
    if( p.th[0]<0 )
        p.th[0] = p.th[0] + 4*PI;
 
    // ｄｑ座標からαβ座標への変換行列Cdq_invの設定
    c = cos(p.th[0]);
    s = sin(p.th[0]);
    // Cdq_inv =[ c -s; ...
    //            s c];
    Cdq_inv[0][0] = c;  Cdq_inv[0][1] =-s;
    Cdq_inv[1][0] = s;  Cdq_inv[1][1] = c;
 
    // id, iqからiα, iβを計算
    //p.iab = Cdq_inv * p.idq;
    p.iab[0] = Cdq_inv[0][0]*p.idq[0] + Cdq_inv[0][1]*p.idq[1];
    p.iab[1] = Cdq_inv[1][0]*p.idq[0] + Cdq_inv[1][1]*p.idq[1];
 
    // αβ座標からUVW座標への変換行列Cuvw_inv=Cuvw'
    // iα, iβからiu, iv, iwを計算
    //  iu = r2/r3*ia;
    //  iv = -1/r2/r3*ia + 1/r2*ib;
    //  iw = -1/r2/r3*ia - 1/r2*ib;
    //p.iuvw = p.Cuvw' * p.iab;
    p.iuvw[0] = p.Cuvw[0][0]*p.iab[0] + p.Cuvw[1][0]*p.iab[1];
    p.iuvw[1] = p.Cuvw[0][1]*p.iab[0] + p.Cuvw[1][1]*p.iab[1];
    p.iuvw[2] = p.Cuvw[0][2]*p.iab[0] + p.Cuvw[1][2]*p.iab[1];
}
#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]=p.vuvw[0];
        data[_count_data][2]=vl.w_lpf; data[_count_data][3]=_count*TS0; data[_count_data][4]=il.vdq_ref[1];
        _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;
    
    p.th[1] = p.th[0];  // thを更新
 #ifdef SIMULATION
    // モータシミュレータ
    sim_motor();    // IPM, ｄｑ座標
 #else
#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;
#else
    // 位置θをセンサで検出
    p.th[0] = (float)encoder.getPulses()/(float)N_ENC*2.0*PI;   // get angle [rad] from encoder
debug[0]=p.th[0]/PI*180;
debug[1]=p.th[0]/(2*PI);    debug[1]=debug[1]-(int)debug[1];    if(debug[1]<0)  debug[1]+=1;
debug[0]=debug[1]*360;
//analog_out=debug[1];
#endif
 #endif
    current_loop(); // 電流制御マイナーループ(idq_ref to vuvw)
    vuvw2pwm();     // vuvw to pwm
//    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.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(){
}
void timerTS3(){
    data2mbedUSB();  // data2mbedUSB() is called every TS3[s].
}
void timerTS4(){
    display2PC();  // display to tera term on PC. display2PC() is called every TS4[s].
}
 

##個人的な算譜見直し(code review)

1. BLDCmotor.hを宣言
   Serial pc2(USBTX, USBRX); // Display on tera term in PC
   はBLDCmotor.hにあれば良い。

道具類(tools)

MATLAB

SCILAB

Treater pro
http://sourceforge.jp/projects/ttssh2/release

#参考文献(reference)

##motor @ qiita

ステッピングモーターをDCモーターの様に扱うモジュール
https://qiita.com/ohisama@github/items/bb22e55bcfff19ae570e
電動機制御算譜（プログラム）設計における３つの罠６つの教訓（実機）
https://qiita.com/kaizen_nagoya/items/b39b6b7ba0d90dff471d

勝手に！ACサーボ制振制御 動画コンテスト
https://qiita.com/motorcontrolman/items/6c36f8bf08a5d5bdd9d9

モーター制御について
https://qiita.com/Shunk_/items/fe2e8f73abb1dabeffd6

マイコンにおけるDCモーター制御について
https://qiita.com/qa65000/items/4c82bff4f29b80cde1e1

arduinoによるインバータ制御
https://qiita.com/wataame9kukki/items/e22436ae9d1d6b9ddb90

極配置法、状態フィードバック制御を感覚的に理解する
https://qiita.com/motorcontrolman/items/5401d344eb634dc00d42

適応制御の基礎
https://qiita.com/MENDY/items/1ca1409d365b66d7ea6d

線形最適制御（LQR）とRiccatiソルバーについて
https://qiita.com/taka_horibe/items/5d053cdaaf4c886d27b5

システム同定による倒立振子のLQR制御
https://qiita.com/fumiya_sato/items/f33ff2152686a648f2bb

PID制御

0からわかるPID制御
https://qiita.com/RyosukeH/items/9e5ce2ebdadd90e3db00
0からわかるPID制御（実装編）
https://qiita.com/RyosukeH/items/373f6451c4946b1e447e
PythonでPID制御をやってみる
https://qiita.com/BIG_LARGE_STONE/items/4f8af62b3edc4a03c4a5
##mbed @ qiita

【mbed】mbedの使い方（開封からユーザー登録まで）
https://qiita.com/SoyaTakahashi/items/1c9cfcec517048eef7bb
【mbed】mbedの使い方（プログラムを書いて動かす）
https://qiita.com/SoyaTakahashi/items/c4aefd5d5587fac5560f
Mbedでレジスタを直接制御してLチカする
https://qiita.com/matecha851/items/31f01c1c5bae49a65312

matlab @ qiita

MATLABで使いそうな機能まとめ 【MATLAB入門】
https://qiita.com/hamukun8686/items/70c6cafa63a347fc5448
SILS環境の構築のためのDCモータのパラメタ同定及び制御実験入門
https://qiita.com/fumiya_sato/items/dc6c37d4e620b514a1a4
Matlabを使った倒立2輪ロボットの安定化
https://qiita.com/K_A_Lab/items/cd1c568c1984c6068f3a
最適サーボ系(積分型最適サーボ)を実装してみた
https://qiita.com/MENDY/items/9b33ee852d6d43fc523c
最適追従系+積分補償器(最適サーボ)実装してみた
https://qiita.com/MENDY/items/439ea46f2c723031d169
##scilab @ qiita

TOPPERSコンテスト受賞作品紹介(5)第三回(2013)アプリケーション開発部門銅賞 Toppers_JSPとScicos_lab（Scilabでも可）による組込みメカトロニクス制御シミュレーション 塩出 武（個人）
https://qiita.com/kaizen_nagoya/items/2aa0454c7bde7058e3f1
TOPPERSコンテスト受賞作品紹介(9)第四回(2014)アプリケーション開発部門 金賞Toppers_ASPカーネルとScilab による組込みメカトロニクス制御シミュレーション 塩出武 (個人)
https://qiita.com/kaizen_nagoya/items/fe398eb623889f587bba

自己参照(self reference)

電動機制御算譜（プログラム）設計における３つの罠６つの教訓（実機）
https://qiita.com/kaizen_nagoya/items/b39b6b7ba0d90dff471d

「「mbedマイコンによるモータ制御設計法」近畿大学小坂学」を読む
https://qiita.com/kaizen_nagoya/items/b8ac7c9b7cb746a4e6ce

プログラマが電動機(electric motor)制御する際に陥る穴
https://qiita.com/kaizen_nagoya/items/1f26d75bb1a8fb0f9e1b

mbedマイコンによるモータ制御設計法　参考文献欄

小坂学：センサレスドライブ制御技術の基礎とMATLABシミュレーション，トリケップス，2005

小坂学：高校数学でマスターする制御工学-本質の理解からMat@Scilabによる実践まで，コロナ社，2012

高橋久：C言語によるモータ制御入門講座　 SHマイコンで学ぶプログラミングと制御技法，電波新聞社，2007

武田洋次，森本茂雄，松井信行，本田幸夫：埋込磁石同期モータの設計と制御，オーム社，2002

矢部正明，坂廼辺和憲 （三菱電機）：過変調 PWMを併用したIPMモータのセンサレス駆動，電気学会回転機研究会資料，RM-01-160， 2001

萩野弘司，井桁健一郎：実験で学ぶDCモータのマイコン制御術，CQ出版社，2012

トランジスタ技術SPECIAL編集部：省エネを目指すマイコン制御とパワー回路の実例，CQ出版社，2011

谷腰欣司：DCモータ活用の実践，CQ出版社，2000
https://mbed.org/handbook/mbed-NXP-LPC1768

<この項は書きかけです。順次追記します。>
This article is not completed. I will add some words in order.

自己参照

物理記事　上位100
https://qiita.com/kaizen_nagoya/items/66e90fe31fbe3facc6ff

数学関連記事１００
https://qiita.com/kaizen_nagoya/items/d8dadb49a6397e854c6d

言語・文学記事　１００
https://qiita.com/kaizen_nagoya/items/42d58d5ef7fb53c407d6

医工連携関連記事一覧
https://qiita.com/kaizen_nagoya/items/6ab51c12ba51bc260a82

通信記事１００
https://qiita.com/kaizen_nagoya/items/1d67de5e1cd207b05ef7

自動車　記事　１００
https://qiita.com/kaizen_nagoya/items/f7f0b9ab36569ad409c5

Qiita(0)Qiita関連記事一覧（自分）
https://qiita.com/kaizen_nagoya/items/58db5fbf036b28e9dfa6

鉄道（０）鉄道のシステム考察はてっちゃんがてつだってくれる
https://qiita.com/kaizen_nagoya/items/26bda595f341a27901a0

日本語（０）一欄
https://qiita.com/kaizen_nagoya/items/7498dcfa3a9ba7fd1e68

英語(0) 一覧
https://qiita.com/kaizen_nagoya/items/680e3f5cbf9430486c7d

転職(0)一覧
https://qiita.com/kaizen_nagoya/items/f77520d378d33451d6fe

仮説（0）一覧（目標100現在40）
https://qiita.com/kaizen_nagoya/items/f000506fe1837b3590df

安全（0）安全工学シンポジウムに向けて: 21
https://qiita.com/kaizen_nagoya/items/c5d78f3def8195cb2409

Error一覧 error(0)
https://qiita.com/kaizen_nagoya/items/48b6cbc8d68eae2c42b8

Ethernet 記事一覧　Ethernet(0)
https://qiita.com/kaizen_nagoya/items/88d35e99f74aefc98794

Wireshark 一覧 wireshark(0)、Ethernet(48)
https://qiita.com/kaizen_nagoya/items/fbed841f61875c4731d0

線網（Wi-Fi）空中線(antenna)(0) 記事一覧(118/300目標)
https://qiita.com/kaizen_nagoya/items/5e5464ac2b24bd4cd001

OSEK OS設計の基礎　OSEK(100)
https://qiita.com/kaizen_nagoya/items/7528a22a14242d2d58a3

官公庁・学校・公的団体（NPOを含む）システムの課題、官（０）
https://qiita.com/kaizen_nagoya/items/04ee6eaf7ec13d3af4c3

Error一覧(C/C++, python, bash...) Error(0)
https://qiita.com/kaizen_nagoya/items/48b6cbc8d68eae2c42b8

C++ Support(0)　
https://qiita.com/kaizen_nagoya/items/8720d26f762369a80514

Coding Rules(0) C Secure , MISRA and so on
https://qiita.com/kaizen_nagoya/items/400725644a8a0e90fbb0

なぜdockerで機械学習するか 書籍・ソース一覧作成中 (目標100)
https://qiita.com/kaizen_nagoya/items/ddd12477544bf5ba85e2

言語処理100本ノックをdockerで。python覚えるのに最適。:10+12
https://qiita.com/kaizen_nagoya/items/7e7eb7c543e0c18438c4

プログラムちょい替え（0）一覧:4件
https://qiita.com/kaizen_nagoya/items/296d87ef4bfd516bc394

TOPPERSまとめ　#名古屋のIoTは名古屋のOSで
https://qiita.com/kaizen_nagoya/items/9026c049cb0309b9d451

自動制御、制御工学一覧（０）
https://qiita.com/kaizen_nagoya/items/7767a4e19a6ae1479e6b

プログラマが知っていると良い「公序良俗」
https://qiita.com/kaizen_nagoya/items/9fe7c0dfac2fbd77a945

一覧の一覧( The directory of directories of mine.) Qiita(100)
https://qiita.com/kaizen_nagoya/items/7eb0e006543886138f39

自動制御、制御工学一覧（０）
https://qiita.com/kaizen_nagoya/items/7767a4e19a6ae1479e6b

小川清最終講義、小川清最終講義（再）計画, Ethernet(100) 英語(100) 安全(100)
https://qiita.com/kaizen_nagoya/items/e2df642e3951e35e6a53

＜この記事は個人の過去の経験に基づく個人の感想です。現在所属する組織、業務とは関係がありません。＞
This article is an individual impression based on the individual's experience. It has nothing to do with the organization or business to which I currently belong.

文書履歴(document history)

ver. 0.10 初稿 20180710
ver. 0.11 mbed, scilab, matlab 追記 20180712
ver. 0.12 参考資料追記 20190301
ver. 0.13 ありがとう追記　20230521

最後までおよみいただきありがとうございました。

いいね　💚、フォローをお願いします。

Thank you very much for reading to the last sentence.

Please press the like icon 💚　and follow me for your happy life.

http://b.hatena.ne.jp/guide/bbutton