Kosaka Lab
UVW 3 phases Brushless DC motor control
Dependencies: QEI mbed-rtos mbed
Fork of
BLDCmotor
by 
manabu kosaka
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: 各種３相同期モータに対するセンサあり運転のシミュレーション
+// 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; // ｑ軸電流指令[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]){
+// ｄｑ座標電流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)
+
+ // ｄｑ電流偏差の計算
+ 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];
+
+ // ＰＩ制御
+ // 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];
+ }
+
+ // αβ座標からｄｑ座標への変換行列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);
+
+ // αβ座標電流をｄｑ座標電流に変換
+ //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
+ // ｄｑ軸電圧指令ベクトルの大きさを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
+ }
+
+ // ｄｑ座標指令電圧 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;
+
+ // ＰＩ制御
+ 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();
+
+ // ｑ軸電流指令の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;
+
+ // ｄｑ軸電流の目標値を速度制御メインループから電流制御マイナーループへ渡す。
+ 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, ｄｑ座標
+ 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;
+
+ current_loop(); // 電流制御マイナーループ(idq_ref to vuvw)
+ vuvw2pwm(); // vuvw to pwm
+ #ifdef SIMULATION
+ // モータシミュレータ
+ sim_motor(); // IPM, ｄｑ座標
+ #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], ｑ軸電流指令の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; // ｑ軸電流指令[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]){ // ２ノルムを計算
+// 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); // ２ノルムを計算 +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;
+ // 指令ｄｑ電流位相
+ 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, ｄｑ座標
+#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;
+ // 電流位相（ｄｑ軸電流）変化
+ // 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, ｄｑ座標
+#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");
}