UVW 3 phases Brushless DC motor control
Dependencies: QEI mbed-rtos mbed
Fork of DCmotor by
Revision 12:a4b17bb682eb, committed 2012-12-21
- Comitter:
- kosaka
- Date:
- Fri Dec 21 22:06:56 2012 +0000
- Parent:
- 11:9747752435d1
- Child:
- 13:791e20f1af43
- Commit message:
- 121222a
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/UVWpwm.cpp Fri Dec 21 22:06:56 2012 +0000 @@ -0,0 +1,204 @@ +#include "mbed.h" +#include "controller.h" +#include "UVWpwm.h" + +#define DEADTIME_US (unsigned long)(DEADTIME*1000000) // [us], deadtime to be set between plus volt. to/from minus + +Timeout pwm[3]; + +DigitalOut pwm_upper[] = {(U_UPPER_PORT), (V_UPPER_PORT),(W_UPPER_PORT)}; +DigitalOut pwm_lower[] = {(U_LOWER_PORT), (V_LOWER_PORT),(W_LOWER_PORT)}; + +pwm_parameters uvw[3]; // UVW pwm の定数、変数 + +// 関数配列: NG +//void (*pwmUVWout[])(int) = {pwmout,pwmout,pwmout}; +// pwmUVWout[i](i); + +#if PWM_WAVEFORM==0 // 0: saw tooth wave comparison +void pwmUout() { // pwm out using timer + unsigned char i=0; + uvw[i].mode += 1; + if( uvw[i].mode==1 ){ + pwm_upper[i] = 1; + pwm_lower[i] = 0; + uvw[i].upper_us = uvw[i].duty*1000000/PWM_FREQ - DEADTIME_US; // ON time of Uupper + pwm[i].attach_us(&pwmUout, uvw[i].upper_us); // setup pwmU to call pwmUout after t [us] + uvw[i].lower_us = 1000000/PWM_FREQ -uvw[i].upper_us - 2*DEADTIME_US; // ON time of Ulower + }else if( uvw[i].mode==2 ){ + pwm[i].attach_us(&pwmUout, DEADTIME_US); // setup pwmU to call pwmUout after t [us] + pwm_upper[i] = 0; + pwm_lower[i] = 0; + }else if( uvw[i].mode==3 ){ + pwm[i].attach_us(&pwmUout, uvw[i].lower_us); // setup pwmU to call pwmUout after t [us] + pwm_upper[i] = 0; + pwm_lower[i] = 1; + }else{// if( u.mode==4 ){ + pwm[i].attach_us(&pwmUout, DEADTIME_US); // setup pwmU to call pwmUout after t [us] + pwm_upper[i] = 0; + pwm_lower[i] = 0; + uvw[i].mode = 0; + } +} + +void pwmVout() { // pwm out using timer + unsigned char i=1; + uvw[i].mode += 1; + if( uvw[i].mode==1 ){ + pwm_upper[i] = 1; + pwm_lower[i] = 0; + uvw[i].upper_us = uvw[i].duty*1000000/PWM_FREQ - DEADTIME_US; // ON time of Uupper + pwm[i].attach_us(&pwmVout, uvw[i].upper_us); // setup pwmU to call pwmUout after t [us] + uvw[i].lower_us = 1000000/PWM_FREQ -uvw[i].upper_us - 2*DEADTIME_US; // ON time of Ulower + }else if( uvw[i].mode==2 ){ + pwm[i].attach_us(&pwmVout, DEADTIME_US); // setup pwmU to call pwmUout after t [us] + pwm_upper[i] = 0; + pwm_lower[i] = 0; + }else if( uvw[i].mode==3 ){ + pwm[i].attach_us(&pwmVout, uvw[i].lower_us); // setup pwmU to call pwmUout after t [us] + pwm_upper[i] = 0; + pwm_lower[i] = 1; + }else{// if( u.mode==4 ){ + pwm[i].attach_us(&pwmVout, DEADTIME_US); // setup pwmU to call pwmUout after t [us] + pwm_upper[i] = 0; + pwm_lower[i] = 0; + uvw[i].mode = 0; + } +} + +void pwmWout() { // pwm out using timer + unsigned char i=2; + uvw[i].mode += 1; + if( uvw[i].mode==1 ){ + pwm_upper[i] = 1; + pwm_lower[i] = 0; + uvw[i].upper_us = uvw[i].duty*1000000/PWM_FREQ - DEADTIME_US; // ON time of Uupper + pwm[i].attach_us(&pwmWout, uvw[i].upper_us); // setup pwmU to call pwmUout after t [us] + uvw[i].lower_us = 1000000/PWM_FREQ -uvw[i].upper_us - 2*DEADTIME_US; // ON time of Ulower + }else if( uvw[i].mode==2 ){ + pwm[i].attach_us(&pwmWout, DEADTIME_US); // setup pwmU to call pwmUout after t [us] + pwm_upper[i] = 0; + pwm_lower[i] = 0; + }else if( uvw[i].mode==3 ){ + pwm[i].attach_us(&pwmWout, uvw[i].lower_us); // setup pwmU to call pwmUout after t [us] + pwm_upper[i] = 0; + pwm_lower[i] = 1; + }else{// if( u.mode==4 ){ + pwm[i].attach_us(&pwmWout, DEADTIME_US); // setup pwmU to call pwmUout after t [us] + pwm_upper[i] = 0; + pwm_lower[i] = 0; + uvw[i].mode = 0; + } +} +#elif PWM_WAVEFORM==1 // 1: triangler wave comparison +void pwmUout() { // pwm out using timer + unsigned char i=0; + uvw[i].mode += 1; + if( uvw[i].mode==1 ){ + uvw[i].upper_us = uvw[i].duty*1000000/PWM_FREQ - DEADTIME_US; // ON time of Uupper + uvw[i].lower_us = 1000000/PWM_FREQ -uvw[i].upper_us - 2*DEADTIME_US; // ON time of Ulower + pwm_upper[i] = 0; + pwm_lower[i] = 1; + uvw[i].lower_us /= 2; + pwm[i].attach_us(&pwmUout, uvw[i].lower_us); // setup pwmU to call pwmUout after t [us] + }else if( uvw[i].mode==2 ){ + pwm[i].attach_us(&pwmUout, DEADTIME_US); // setup pwmU to call pwmUout after t [us] + pwm_upper[i] = 0; + pwm_lower[i] = 0; + }else if( uvw[i].mode==3 ){ + pwm[i].attach_us(&pwmUout, uvw[i].upper_us); // setup pwmU to call pwmUout after t [us] + pwm_upper[i] = 1; + pwm_lower[i] = 0; + }else if( uvw[i].mode==4 ){ + pwm[i].attach_us(&pwmUout, DEADTIME_US); // setup pwmU to call pwmUout after t [us] + pwm_upper[i] = 0; + pwm_lower[i] = 0; + }else{// if( uvw[i].mode==5 ){ + pwm[i].attach_us(&pwmUout, uvw[i].lower_us); // setup pwmU to call pwmUout after t [us] + pwm_upper[i] = 0; + pwm_lower[i] = 1; + uvw[i].mode = 0; + } +} +void pwmVout() { // pwm out using timer + unsigned char i=1; + uvw[i].mode += 1; + if( uvw[i].mode==1 ){ + uvw[i].upper_us = uvw[i].duty*1000000/PWM_FREQ - DEADTIME_US; // ON time of Uupper + uvw[i].lower_us = 1000000/PWM_FREQ -uvw[i].upper_us - 2*DEADTIME_US; // ON time of Ulower + pwm_upper[i] = 0; + pwm_lower[i] = 1; + uvw[i].lower_us /= 2; + pwm[i].attach_us(&pwmVout, uvw[i].lower_us); // setup pwmU to call pwmUout after t [us] + }else if( uvw[i].mode==2 ){ + pwm[i].attach_us(&pwmVout, DEADTIME_US); // setup pwmU to call pwmUout after t [us] + pwm_upper[i] = 0; + pwm_lower[i] = 0; + }else if( uvw[i].mode==3 ){ + pwm[i].attach_us(&pwmVout, uvw[i].upper_us); // setup pwmU to call pwmUout after t [us] + pwm_upper[i] = 1; + pwm_lower[i] = 0; + }else if( uvw[i].mode==4 ){ + pwm[i].attach_us(&pwmVout, DEADTIME_US); // setup pwmU to call pwmUout after t [us] + pwm_upper[i] = 0; + pwm_lower[i] = 0; + }else{// if( uvw[i].mode==5 ){ + pwm[i].attach_us(&pwmVout, uvw[i].lower_us); // setup pwmU to call pwmUout after t [us] + pwm_upper[i] = 0; + pwm_lower[i] = 1; + uvw[i].mode = 0; + } +} + +void pwmWout() { // pwm out using timer + unsigned char i=2; + uvw[i].mode += 1; + if( uvw[i].mode==1 ){ + uvw[i].upper_us = uvw[i].duty*1000000/PWM_FREQ - DEADTIME_US; // ON time of Uupper + uvw[i].lower_us = 1000000/PWM_FREQ -uvw[i].upper_us - 2*DEADTIME_US; // ON time of Ulower + pwm_upper[i] = 0; + pwm_lower[i] = 1; + uvw[i].lower_us /= 2; + pwm[i].attach_us(&pwmWout, uvw[i].lower_us); // setup pwmU to call pwmUout after t [us] + }else if( uvw[i].mode==2 ){ + pwm[i].attach_us(&pwmWout, DEADTIME_US); // setup pwmU to call pwmUout after t [us] + pwm_upper[i] = 0; + pwm_lower[i] = 0; + }else if( uvw[i].mode==3 ){ + pwm[i].attach_us(&pwmWout, uvw[i].upper_us); // setup pwmU to call pwmUout after t [us] + pwm_upper[i] = 1; + pwm_lower[i] = 0; + }else if( uvw[i].mode==4 ){ + pwm[i].attach_us(&pwmWout, DEADTIME_US); // setup pwmU to call pwmUout after t [us] + pwm_upper[i] = 0; + pwm_lower[i] = 0; + }else{// if( uvw[i].mode==5 ){ + pwm[i].attach_us(&pwmWout, uvw[i].lower_us); // setup pwmU to call pwmUout after t [us] + pwm_upper[i] = 0; + pwm_lower[i] = 1; + uvw[i].mode = 0; + } +} +#endif + + +void start_pwm(){ + unsigned char i; + for( i=0;i<3;i++ ){ + uvw[i].duty = 0.5; + pwm_upper[i] = pwm_lower[i] = 0; + uvw[i].mode = 0; + } + pwmUout(); + pwmVout(); + pwmWout(); +} + +void stop_pwm(){ + unsigned char i; + for( i=0;i<3;i++ ){ + pwm_upper[i] = pwm_lower[i] = 0; + uvw[i].mode = 0; + pwm[i].detach(); + } +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/UVWpwm.h Fri Dec 21 22:06:56 2012 +0000 @@ -0,0 +1,26 @@ +#ifndef __UVWpwm_h +#define __UVWpwm_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 U_UPPER_PORT LED1//p21 // port for U phase upper arm +#define U_LOWER_PORT LED2//p22 // port for U phase lower arm +#define V_UPPER_PORT LED3//p23 // port for V phase upper arm +#define V_LOWER_PORT LED4//p24 // port for V phase lower arm +#define W_UPPER_PORT p25 // port for W phase upper arm +#define W_LOWER_PORT p26 // port for W phase lower arm +#define PWM_WAVEFORM 0 // 0: saw tooth wave comparison, 1: triangler wave comparison +//************* User setting parameters (end) ***************** + +typedef struct struct_pwm_parameters{ // parameters of UVW pwm + float duty; // 0-1, duty of UVW + unsigned char mode; // mode + unsigned long upper_us; // [us], time + unsigned long lower_us; // [us], time +}pwm_parameters; +extern pwm_parameters uvw[3]; // UVW 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 Dec 21 22:06:56 2012 +0000 @@ -0,0 +1,451 @@ +// BLDCmotor.cpp: 各種3相同期モータに対するセンサあり運転のシミュレーション +// Kosaka Lab. 121215 +#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); +AnalogIn VshuntR_Uplus(p19); // *3.3 [V], Volt of shunt R_SHUNT[Ohm]. The motor current i = v_shunt_r/R_SHUNT [A] +AnalogIn VshuntR_Uminus(p20); // *3.3 [V], Volt of shunt R_SHUNT[Ohm]. The motor current i = v_shunt_r/R_SHUNT [A] +AnalogIn VshuntR_Vplus(p16); // *3.3 [V], Volt of shunt R_SHUNT[Ohm]. The motor current i = v_shunt_r/R_SHUNT [A] +AnalogIn VshuntR_Vminus(p17); // *3.3 [V], Volt of shunt R_SHUNT[Ohm]. The motor current i = v_shunt_r/R_SHUNT [A] + +unsigned long _count; // sampling number +float _time; // time[s] +float _r; // reference signal +float _th=0; // [rad], motor angle, control output of angle controller +float _i=0; // [A], motor current, control output of current controller +float _e=0; // e=r-y for PID controller +float _eI=0; // integral of e for PID controller +float _iref; // reference current iref [A], output of angle th_contorller +float _u; // control input[V], motor input volt. +float _ei=0; // e=r-y for current PID controller +float _eiI=0; // integral of e for current PID controller +unsigned char _f_u_plus=1;// sign(u) +unsigned char _f_umax=0;// flag showing u is max or not +unsigned char _f_imax=0;// flag showing i is max or not +float debug[20]; // for debug +float disp[10]; // for printf to avoid interrupted by quicker process +DigitalOut led1(LED1); +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の機器定数等の設定, 制御器の初期化 + 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 +#endif + p.th[0] = 0; + p.th[1] = 0; + p.w = 0; + p.iab[0] =0; p.iab[1] = 0; // iab = [iα;iβ]; + p.vab[0] =0; p.vab[1] = 0; // vab = [vα;vβ]; + p.p = 2; // 極対数 + // UVW座標からαβ座標への変換行列Cuvwの設定 + r2 = sqrt(2.);//1.414213562373095;//2^(1/2); + r3 = sqrt(3.);//1.732050807568877;//3^(1/2); + // 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; // q軸電流指令[A] + vl.w_lpf = 0; // 検出した速度[rad/s] + vl.eI_w = 0; // 速度制御用偏差の積分値(積分項) + il.eI_idq[0] = 0; // 電流制御用偏差の積分値(積分項) + il.eI_idq[1] = 0; // 電流制御用偏差の積分値(積分項) +} + +void idq_control(float idq_act[2]){ +// dq座標電流PID制御器(電流マイナーループのフィードバック制御) +// 入力:指令dq座標電流 idq_ref [A], 実dq座標電流 idq_act [A], PI制御積分項 eI, サンプル時間 ts [s] +// 出力:αβ軸電圧指令 vdq_ref [A] +// [vdq_ref,eI_idq] = idq_control(idq_ref,idq_act,eI_idq,ts); + float Kp_d, Kp_q, Ki_d, Ki_q, e[2]; + // 電流制御ゲイン + Kp_d = iKPd; // P gain (d-axis) + Ki_d = iKId; // I gain (d-axis) + Kp_q = iKPq; // P gain (q-axis) + Ki_q = iKIq; // I gain (q-axis) + + // dq電流偏差の計算 + e[0] = il.idq_ref[0] - idq_act[0]; + e[1] = il.idq_ref[1] - idq_act[1]; + + // dq電流偏差の積分項の計算 + il.eI_idq[0] = il.eI_idq[0] + TS0*e[0]; + il.eI_idq[1] = il.eI_idq[1] + TS0*e[1]; + + // PI制御 + // vdq_ref = [Kp_d 0;0 Kp_q]*e + [Ki_d 0;0 Ki_q]*eI; + il.vdq_ref[0] = Kp_d*e[0] + Ki_d*il.eI_idq[0]; + il.vdq_ref[1] = Kp_q*e[1] + Ki_q*il.eI_idq[1]; + +// koko anti-windup +} + +void current_loop(){ // 電流制御マイナーループ + float th, c, s, Cdq[2][2], iu, iv, iab[2], idq_act[2], vab_ref[2],tmp; + if( f_find_origin==1 ){ + th = 0; + }else{ + // 位置θをセンサで検出 +#ifndef SIMULATION + p.th[0] = (float)encoder.getPulses()/(float)N_ENC*2.0*PI; // get angle [rad] from encoder +#endif + th = p.th[0]; + } + + // αβ座標からdq座標への変換行列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, iv を検出 +#ifndef SIMULATION + p.iuvw[0] = (VshuntR_Uplus - VshuntR_Uminus) /R_SHUNT; // get iu [A] from shunt resistance; + p.iuvw[1] = (VshuntR_Vplus - VshuntR_Vminus) /R_SHUNT; // get iv [A] from shunt resistance; +#endif + iu = p.iuvw[0]; + iv = p.iuvw[1]; +// iw = -(iu + iv); // iu+iv+iw=0であることを利用してiw を計算 + + // 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); + + // αβ座標電流をdq座標電流に変換 + //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]; + il.vdq_ref[1] = il.idq_ref[1]; +#endif + // dq軸電圧指令ベクトルの大きさをMAX制限(コンバータ出力電圧値に設定) + // if( norm(vdq_ref) > vdqmax ){ vdq_ref= vdqmax/norm(vdq_ref)*vdq_ref} + if( (tmp=il.vdq_ref[0]*il.vdq_ref[0]+il.vdq_ref[1]*il.vdq_ref[1])>SQRvdqMAX ){ + tmp = sqrt2(SQRvdqMAX/tmp); + il.vdq_ref[0] = tmp*il.vdq_ref[0]; //= vdqmax/norm(vdq_ref)*vdq_ref + il.vdq_ref[1] = tmp*il.vdq_ref[1]; +// koko anti-windup + } + + // dq座標指令電圧 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]; + + // モータに印加する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]; + + p.th[1] = p.th[0]; // thを更新 +} + + +void vel_control(){ +// 速度制御器:速度偏差が入力され、q軸電流指令を出力。 +// 入力:指令速度 w_ref [rad/s], 実速度 w_lpf [rad/s], PI制御積分項 eI, サンプル時間 TS1 [s] +// 出力:q軸電流指令 iq_ref [A] +// [iq_ref,eI_w] = vel_control(w_ref,w_lpf,eI_w,ts); + float Kp, Ki, e; + // 速度制御PIDゲイン + Kp = wKp; // 速度制御PIDのPゲイン + Ki = wKi; // 速度制御PIDのIゲイン + + // 速度偏差の計算 + e = vl.w_ref - vl.w_lpf; + + // 速度偏差の積分値の計算 + vl.eI_w = vl.eI_w + TS1*e; + + // PI制御 + vl.iq_ref = Kp*e + Ki*vl.eI_w; +// koko anti-windup +} + +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 = (1-iLPF)*vl.w_lpf + tmp/TS0 *iLPF; + + // 速度制御:速度偏差が入力され、q軸電流指令を出力。 +// [iq_ref,eI_w] = vel_control(w_ref,w_act,eI_w,ts); + vel_control(); + + // q軸電流指令のMAX制限(異常に大きい指令値を制限する) + if( vl.iq_ref > iqMAX ){ + vl.iq_ref = iqMAX; + }else if( vl.iq_ref < -iqMAX ){ + 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; + + // dq軸電流の目標値を速度制御メインループから電流制御マイナーループへ渡す。 + il.idq_ref[0] = idq_ref[0]; + il.idq_ref[1] = idq_ref[1]; +} + +void vuvw2pwm(){ // vu, vv, vwより、UVW相の上アームPWMを発生 + float duty_u, duty_v, duty_w; + + duty_u = (p.vuvw[0]/vdqMAX+1)*0.5; + duty_v = (p.vuvw[1]/vdqMAX+1)*0.5; + duty_w = (p.vuvw[2]/vdqMAX+1)*0.5; + uvw[0].duty = duty_u; + uvw[1].duty = duty_v; + uvw[2].duty = duty_w; +} + +#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, dq座標 + 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); + + // αβ座標からdq座標への変換行列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; + + // vα, vβからvd, vqを計算 + // 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; + + // dq座標からαβ座標への変換行列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; + + current_loop(); // 電流制御マイナーループ(idq_ref to vuvw) + vuvw2pwm(); // vuvw to pwm + #ifdef SIMULATION + // モータシミュレータ + sim_motor(); // IPM, dq座標 + #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.5f[V]\t%4d [Hz]\t%d\r\n", _time, il.vdq_ref[0], (int)(vl.w_lpf/(2*PI)+0.5), (int)(vl.w_ref/(2*PI)+0.5)); // print to tera term +// pc.printf("%8.1f[s]\t%8.5f[V]\t%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 Dec 21 22:06:56 2012 +0000 @@ -0,0 +1,118 @@ +#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 PWM_FREQ 1000.0 //[Hz], pwm freq. (> 1/(DEAD_TIME*10)) +#define DEADTIME 0.0001 // [s], deadtime to be set between plus volt. to/from minus +#define USE_CURRENT_CONTROL // Current control on. Comment if current control off. +#define CONTROL_MODE 0 // 0:PID control, 1:Frequency response, 2:Step response, 3. u=Rand to identify G(s), 4) FFT identification +#define DEADZONE_PLUS 1. // deadzone of plus side +#define DEADZONE_MINUS -1.5 // deadzone of minus side +#define GOOD_DATA // Comment this line if the length of data TMAX/TS2 > 1000 + // encoder +#define N_ENC (24*4) // "*4": QEI::X4_ENCODING. Number of pulses in one revolution(=360 deg) of rotary encoder. +#define CH_A p29 // A phase port +#define CH_B p30 // A phase port + +#define DA_PORT p18 // analog out (DA) port of mbed + +#define TS0 0.001//08//8 // [s], sampling time (priority highest: Ticker IRQ) of motor current i control PID using timer interrupt +#define TS1 0.002//0.01 // [s], sampling time (priority high: RtosTimer) of motor angle th PID using rtos-timer +#define TS2 0.2 // [s], sampling time (priority =main(): precision 4ms) to save data to PC using thread. But, max data length is 1000. +#define TS3 0.002 // [s], sampling time (priority low: precision 4ms) +#define TS4 0.1 // [s], sampling time (priority lowest: precision 4ms) to display data to PC tera term +//void timerTS1(void const *argument), CallTimerTS3(void const *argument), CallTimerTS4(void const *argument); +// RtosTimer RtosTimerTS1(timerTS1); // RtosTimer priority is osPriorityAboveNormal, just one above main() +// Thread ThreadTimerTS3(CallTimerTS3,NULL,osPriorityBelowNormal); +// Thread ThreadTimerTS4(CallTimerTS4,NULL,osPriorityLow); +#define TMAX 3.0 // [s], experiment starts from 0[s] to TMAX[s] + + // 電流制御マイナーループ +#define iKPd 10./2 // 電流制御d軸PIDのPゲイン (d-axis) +#define iKId 100./2 // 電流制御d軸PIDのIゲイン (d-axis) +#define iKPq 10./2 // 電流制御q軸PIDのPゲイン (q-axis) +#define iKIq 100./2 // 電流制御q軸PIDのIゲイン (q-axis) + +#define vdqMAX 300. +#define SQRvdqMAX (vdqMAX*vdqMAX) // [V^2] vdqの大きさの最大値の二乗 + + // 速度制御メインループ +#ifdef USE_CURRENT_CONTROL + #define wKp 0.05 // 速度制御PIDのPゲイン + #define wKi 2.50 // 速度制御PIDのIゲイン +#else + #define wKp 0.005//0.05 // 速度制御PIDのPゲイン + #define wKi 0.2//2.50 // 速度制御PIDのIゲイン +#endif + +#define iLPF 0.9 // 0-1, 速度に対する1次LPF; Low Pass Filter, G(z)=(1-a)/(z-a) +#define iqMAX 100 // [A], q軸電流指令のMAX制限(異常に大きい指令値を制限する) + +#define R_SHUNT 1.25 // [Ohm], shunt resistanse +/*********** User setting for control parameters (end) ***************/ + + +typedef struct struct_motor_parameters{ + // モータの定数、信号など + #ifdef SIMULATION // シミュレーションのとき + float Ld; // [H], d軸インダクタンス + float Lq; // [H], q軸インダクタンス + float Lq0; // 磁気飽和を考慮 (Lq = Lq0 - Lq1*iq) + float Lq1; // + 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 iab[2]; // [A], αβ軸電流 iab = [iα;iβ]; + float idq[2]; // [A], dq軸電流 idq = [id;iq]; + float vab[2]; // [V], αβ軸電圧 vab = [vα;vβ]; + float vuvw[3];// [V], UVW相電圧 vuvw = [vu;vv;vw]; + float iuvw[3];// [A], UVW相電流 iuvw = [iu;iv;iw]; + float p; // 極対数 + float Cuvw[2][3]; // UVW座標からαβ座標への変換行列Cuvw +}motor_parameters; + +typedef struct struct_current_loop_parameters{ + // 電流制御マイナーループの定数、変数 + float idq_ref[2]; // idqの目標値 + float vdq_ref[2]; // vdqの目標値 + float eI_idq[2]; // 電流制御用偏差の積分値(積分項) +}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 iq_ref; // q軸電流指令[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 unsigned short f_find_origin; // flag to find the origin of the rotor angle theta + +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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/fast_math.cpp Fri Dec 21 22:06:56 2012 +0000 @@ -0,0 +1,51 @@ +#include "mbed.h" +#include "fast_math.h" + +unsigned short sin60[DEG60+1]; // sin table from 0 to 60 deg. (max precision error is 0.003%) + +long _sin(unsigned short th){ // return( 65535*sin(th) ), th=rad*DEG60/(PI/3)=rad*(512*3)/PI (0<=rad<2*PI) +// init_sin60(); +// if( th>2.*PI ){ th -= 2*PI*(float)((int)(th/(2.*PI)));} +// th_int = (unsigned short)(th/(PI/3.)*(float)DEG60+0.5); // rad to deg +// sin = (float)_sin(th)/65535.; + unsigned short f_minus; + long x; + + if( th>=DEG60*3){ f_minus = 1; th -= DEG60*3;} // if th>=180deg, th = th - 180deg; + else{ f_minus = 0;} // else , f_minus = on. + + if( th<DEG60 ){ // th<60deg? + x = sin60[th]; // sin(th) + }else if( th<DEG60*2 ){ // 60<=th<120deg? + x = sin60[DEG60*2-th] + sin60[th-DEG60]; // sin(th)=sin(th+60)+sin(th-60)=sin(180-(th+60))+sin(th-60) because sin(th+60)=s/2+c*root(3)/2, sin(th-60)=s/2-c*root(3)/2. + }else{// if( th<180 ) // 120<=th<180deg? + x = sin60[DEG60*3-th]; // sin(60-(th-120))=sin(180-th) + } + if( f_minus==1 ){ x = -x;} + return(x); +} + +long _cos(unsigned short th){ // return( 65535*sin(th) ), th=rad*DEG60/(PI/3)=rad*(512*3)/PI (0<=rad<2*PI) + th += DEG60*3/2; + if( th>=DEG60*6 ){ th -= DEG60*6;} + return( _sin(th) ); +} + +void init_fast_math(){ // sin0-sin60deg; 0deg=0, 60deg=512 + int i; + + for( i=0;i<=DEG60;i++ ){ // set sin table from 0 to 60 deg.. +// sin60[i] = (unsigned short)(sin((float)i/512.*PI/3.)); + sin60[i] = (unsigned short)(65535.*sinf((float)i/(float)DEG60*PI/3.)); + } +} + +#if 0 +//float norm(float x[0], float x[1]){ // 2ノルムを計算 +// return(sqrt(x[0]*x[0]+x[1]*x[1])); +//} +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 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/fast_math.h Fri Dec 21 22:06:56 2012 +0000 @@ -0,0 +1,16 @@ +#ifndef __fast_math_h +#define __fast_math_h + +#define PI 3.14159265358979 // def. of PI +#define DEG60 512 // 60deg = 512 + +extern unsigned short sin60[]; // sin table from 0 to 60 deg. (max precision error is 0.003%) +// sin(th) = (float)(_sin(th/(PI/3.)*(float)DEG60+0.5))/65535.; +extern long _sin(unsigned short); // return( 65535*sin(th) ), th=rad*DEG60/(PI/3)=rad*(512*3)/PI (0<=rad<2*PI) +extern long _cos(unsigned short); // return( 65535*sin(th) ), th=rad*DEG60/(PI/3)=rad*(512*3)/PI (0<=rad<2*PI) +extern void init_fast_math(); // call before using _sin(). sin0-sin60deg; 0deg=0, 60deg=512 + +//extern float norm(float); // 2ノルムを計算 +extern float sqrt2(float x); // √xのx=1まわりのテイラー展開 √x = 1 + 1/2*(x-1) -1/4*(x-1)^2 + ... + +#endif \ No newline at end of file
--- a/main.cpp Thu Nov 29 09:25:56 2012 +0000 +++ b/main.cpp Fri Dec 21 22:06:56 2012 +0000 @@ -1,406 +1,81 @@ -// DC motor control program using H-bridge driver (ex. TA7291P) and 360 resolution rotary encoder with A, B phase. -// ver. 121129a by Kosaka lab. +// UVW three phases Blushless DC motor control program using 3 H-bridge driver (ex. BD6211F) and 360 resolution rotary encoder with A, B phase. +// ver. 121206 by Kosaka lab. #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 "fast_math.h" +#include "controller.h" +#include "UVWpwm.h" -Serial pc(USBTX, USBRX); // Display on tera term in PC -LocalFileSystem local("local"); // save data to mbed USB disk drive in PC +Serial pc2(USBTX, USBRX); // Display on tera term in PC +LocalFileSystem local("mbedUSBdrive"); // 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 +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 u2Hbridge(float u){// input u to H bridge driver - float duty; - unsigned int f_deadtime, f_in, r_in; +FILE *fp = fopen("/mbedUSBdrive/data.csv", "w"); //save data to PC +Timeout emergencyStop; // kill fprintf process when bug - 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 - } - 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); - } -#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) -// } +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); } - 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); +//void stop_fprintf(){ // emergencyStop if fprintf keep busy for 3 secons +// fclose(fp); +// pc2.printf("error: fprintf was in hung-up!"); +//} -// osStatus set_priority(osPriority osPriorityBelowNormal ); -// Priority of Thread (RtosTimer has no priority?) +//#define OLD +int main(){ + int ms=1; + unsigned long c, c2; + unsigned short i, i2=0; +// FILE *fp; // save data to PC +// FILE *fp = fopen("/mbedUSBdrive/data.csv", "w"); + 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 @@ -409,4 +84,156 @@ // osPriorityHigh = +2, ///< priority: high // osPriorityRealtime = +3, ///< priority: realtime (highest) // osPriorityError = 0x84 ///< system cannot determine priority or thread has illegal priority + + // シミュレーション総サンプル数 + int N;// = 5000; + // 指令速度 + float w_ref_req[2] = {20* 2*PI, 40* 2*PI}; // [rad/s](第2要素は指令速度急変後の指令速度) + float w_ref; + // 指令dq電流位相 + float beta_ref = 30*PI/180; // rad + float tan_beta_ref1; + float tan_beta_ref2,tan_beta_ref; + +// start_timers(1); // start multi-timers, sampling times are TS0, TS1, TS2, TS3, TS4. + + N = (int)(TMAX/TS0); +pc2.printf("N=%d\r\n",N); + // IPMSMの機器定数等の設定, 制御器の初期化 + init_parameters(); + p.th[0] = 2*PI/3; //θの初期値 + + +// p.Lq0 = p.Ld; // SPMSMの場合 +// p.phi = 0; // SynRMの場合 + +// w_ref=p.w; // 速度指令[rad/s] + tan_beta_ref1 = tan(beta_ref); + tan_beta_ref2 = tan(beta_ref-30*PI/180); + tan_beta_ref = tan_beta_ref1; + // シミュレーション開始 + 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( _count*TS0<0.1 ){ // while( time<1 ){ +// debug_p24 = 1; + il.idq_ref[0] = iqMAX/1.0; + il.idq_ref[1] = 0; + +#ifdef OLD + timerTS0(); + //current_loop(); // 電流制御マイナーループ(idq_ref to vuvw) + //vuvw2pwm(); // vuvw to pwm + //sim_motor(); // IPM, dq座標 +#endif + +// if( (ms=(int)(TS1*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;}// ms=TS0 + Thread::wait(ms); +// debug_p24 = 0; + } +//pc2.printf("\r\n^0^ 0\r\n"); +#ifndef SIMULATION + encoder.reset(); // set encoder counter zero + p.th[0] = p.th[1] = (float)encoder.getPulses()/(float)N_ENC*2.0*PI; // get angle [rad] from encoder +#endif + c2 = _count; + 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 + + for( i=0;i<N;i++ ){ +// debug_p24 = 1; + c = _count-c2; + // 電流位相(dq軸電流)変化 + // if( i>=1500&&i<1900 ){// TS0=0.0001 + if( c>=1500*0.0001/TS0&&c<1900*0.0001/TS0 ){ + if( tan_beta_ref>tan_beta_ref2 ){ tan_beta_ref=tan_beta_ref-0.002;} + }else{ + if( tan_beta_ref<tan_beta_ref1){ tan_beta_ref=tan_beta_ref+0.002;} + } + + // 速度急変 + w_ref = w_ref_req[0]; + if( 2600*0.0001/TS0<=c&&c<3000*0.0001/TS0 ){ + w_ref=w_ref_req[1]; +//pc2.printf(".\r\n"); + } +#ifdef SIMULATION + // 負荷トルク急変 + if( c<4000*0.0001/TS0 ){ + p.TL = 1; + }else{ + p.TL = 2; + } +#endif + vl.w_ref = w_ref; // 目標速度をメインルーチンから速度制御メインループへ渡す。 + vl.tan_beta_ref = tan_beta_ref; // 目標電流位相をメインルーチンから速度制御メインループへ渡す。 + +#ifdef OLD + if( (++i2)>=(int)(TS1/TS0) ){ i2=0; + 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, dq座標 +#endif + +// if( (ms=(int)(TS1*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;}// ms=TS0 + Thread::wait(ms); +// debug_p24 = 0; + } +//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"); +// +// 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"); }