Kosaka Lab / Mbed 2 deprecated BLDCmotor

UVW 3 phases Brushless DC motor control

Dependencies:   QEI mbed-rtos mbed

Fork of BLDCmotor by manabu kosaka

main.cpp@17:1ac855d69c78, 2013-09-07 (annotated)

Committer:
kosakaLab
Date:
Sat Sep 07 02:47:09 2013 +0000
Revision:
17:1ac855d69c78
Parent:
15:427f5ae8e957 
for TBLM-01 18T and OME-360-2MCA

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kosaka 13:791e20f1af43 1 // UVW three phases Blushless DC motor control program using 3 H-bridge driver (ex. TA7291P) and incremental rotary encoder with A, B phase.
kosakaLab 15:427f5ae8e957 2 // ver. 130903 by Kosaka lab.
kosaka 0:fe068497f773 3 #include "mbed.h"
kosaka 0:fe068497f773 4 #include "rtos.h"
kosaka 0:fe068497f773 5
kosaka 12:a4b17bb682eb 6 #include "fast_math.h"
kosaka 12:a4b17bb682eb 7 #include "controller.h"
kosaka 12:a4b17bb682eb 8 #include "UVWpwm.h"
kosaka 0:fe068497f773 9
kosaka 0:fe068497f773 10
kosaka 12:a4b17bb682eb 11 Serial pc2(USBTX, USBRX); // Display on tera term in PC
kosaka 12:a4b17bb682eb 12 LocalFileSystem local("mbedUSBdrive"); // save data to mbed USB disk drive in PC
kosaka 0:fe068497f773 13 //Semaphore semaphore1(1); // wait and release to protect memories and so on
kosaka 0:fe068497f773 14 //Mutex stdio_mutex; // wait and release to protect memories and so on
kosaka 12:a4b17bb682eb 15 Ticker TickerTimerTS0; // Timer interrupt using TIMER3, TS<0.001 is OK. Priority is higher than rtosTimer.
kosaka 12:a4b17bb682eb 16 unsigned char fTimerTS2ON=0, fTimerTS3ON=0, fTimerTS4ON=0; // ON/OFF flag for timerTS2, 3, 4.
kosaka 0:fe068497f773 17
kosaka 7:613febb8f028 18 extern "C" void mbed_reset();
kosaka 3:b6b9b8c7dce6 19
kosaka 12:a4b17bb682eb 20 void CallTimerTS2(void const *argument) { // make sampling time TS3 timer (priority 3: precision 4ms)
kosaka 12:a4b17bb682eb 21 int ms;
kosaka 12:a4b17bb682eb 22 unsigned long c;
kosaka 12:a4b17bb682eb 23 while (true) {
kosaka 12:a4b17bb682eb 24 c = _count;
kosaka 12:a4b17bb682eb 25 if( fTimerTS2ON ){
kosaka 12:a4b17bb682eb 26 timerTS2(); // timerTS2() is called every TS2[s].
kosaka 12:a4b17bb682eb 27 }
kosaka 12:a4b17bb682eb 28 if( (ms=(int)(TS2*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;}
kosaka 12:a4b17bb682eb 29 Thread::wait(ms);
kosaka 0:fe068497f773 30 }
kosaka 8:b8b31e9b60c2 31 }
kosaka 12:a4b17bb682eb 32 void CallTimerTS3(void const *argument) { // make sampling time TS3 timer (priority 3: precision 4ms)
kosaka 12:a4b17bb682eb 33 int ms;
kosaka 12:a4b17bb682eb 34 unsigned long c;
kosaka 12:a4b17bb682eb 35 while (true) {
kosaka 12:a4b17bb682eb 36 c = _count;
kosaka 12:a4b17bb682eb 37 if( fTimerTS3ON ){
kosaka 12:a4b17bb682eb 38 timerTS3(); // timerTS3() is called every TS3[s].
kosaka 2:e056793d6fc5 39 }
kosaka 12:a4b17bb682eb 40 if( (ms=(int)(TS3*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;}
kosaka 12:a4b17bb682eb 41 Thread::wait(ms);
kosaka 0:fe068497f773 42 }
kosaka 0:fe068497f773 43 }
kosaka 0:fe068497f773 44
kosaka 12:a4b17bb682eb 45 void CallTimerTS4(void const *argument) { // make sampling time TS4 timer (priority 4: precision 4ms)
kosaka 12:a4b17bb682eb 46 int ms;
kosaka 12:a4b17bb682eb 47 unsigned long c;
kosaka 12:a4b17bb682eb 48 while (true) {
kosaka 12:a4b17bb682eb 49 c = _count;
kosaka 12:a4b17bb682eb 50 if( fTimerTS4ON ){
kosaka 12:a4b17bb682eb 51 timerTS4(); // timerTS4() is called every TS4[s].
kosaka 7:613febb8f028 52 }
kosaka 12:a4b17bb682eb 53 if( (ms=(int)(TS4*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;}
kosaka 12:a4b17bb682eb 54 Thread::wait(ms);
kosaka 12:a4b17bb682eb 55 }
kosaka 12:a4b17bb682eb 56 }
kosaka 0:fe068497f773 57
kosaka 12:a4b17bb682eb 58 //void stop_fprintf(){ // emergencyStop if fprintf keep busy for 3 secons
kosaka 12:a4b17bb682eb 59 // fclose(fp);
kosaka 12:a4b17bb682eb 60 // pc2.printf("error: fprintf was in hung-up!");
kosaka 12:a4b17bb682eb 61 //}
kosaka 0:fe068497f773 62
kosaka 12:a4b17bb682eb 63 //#define OLD
kosaka 12:a4b17bb682eb 64 int main(){
kosaka 12:a4b17bb682eb 65 unsigned short i, i2=0;
kosaka 13:791e20f1af43 66 FILE *fp = fopen("/mbedUSBdrive/data.csv", "w"); // save data to PC
kosakaLab 15:427f5ae8e957 67 // char chr[100];
kosaka 12:a4b17bb682eb 68 RtosTimer RtosTimerTS1(timerTS1); // RtosTimer priority is osPriorityAboveNormal, just one above main()
kosaka 12:a4b17bb682eb 69 Thread ThreadTimerTS3(CallTimerTS3,NULL,osPriorityBelowNormal);
kosaka 12:a4b17bb682eb 70 Thread ThreadTimerTS4(CallTimerTS4,NULL,osPriorityLow);
kosaka 12:a4b17bb682eb 71 // Priority of Thread (RtosTimer is osPriorityAboveNormal)
kosaka 0:fe068497f773 72 // osPriorityIdle = -3, ///< priority: idle (lowest)--> then, mbed ERROR!!
kosaka 0:fe068497f773 73 // osPriorityLow = -2, ///< priority: low
kosaka 0:fe068497f773 74 // osPriorityBelowNormal = -1, ///< priority: below normal
kosaka 0:fe068497f773 75 // osPriorityNormal = 0, ///< priority: normal (default)
kosaka 0:fe068497f773 76 // osPriorityAboveNormal = +1, ///< priority: above normal
kosaka 0:fe068497f773 77 // osPriorityHigh = +2, ///< priority: high
kosaka 0:fe068497f773 78 // osPriorityRealtime = +3, ///< priority: realtime (highest)
kosaka 0:fe068497f773 79 // osPriorityError = 0x84 ///< system cannot determine priority or thread has illegal priority
kosaka 12:a4b17bb682eb 80
kosaka 12:a4b17bb682eb 81 // 指令速度
kosakaLab 14:8e205264baa8 82 // float w_ref_req[2] = {20* 2*PI, 40* 2*PI}; // [rad/s]（第2要素は指令速度急変後の指令速度）
kosakaLab 17:1ac855d69c78 83 float w_ref_req[2] = {5* 2*PI, 10* 2*PI}; // [rad/s]（第2要素は指令速度急変後の指令速度）
kosaka 12:a4b17bb682eb 84 // 指令ｄｑ電流位相
kosakaLab 15:427f5ae8e957 85 // float beta_ref = 30*PI/180; // [rad], 電流位相指令βを30度に
kosakaLab 17:1ac855d69c78 86 float beta_ref = 35*PI/180; // [rad], 電流位相指令βを30度に
kosaka 12:a4b17bb682eb 87 float tan_beta_ref1;
kosaka 12:a4b17bb682eb 88 float tan_beta_ref2,tan_beta_ref;
kosaka 13:791e20f1af43 89 float t; // current time
kosakaLab 15:427f5ae8e957 90 extern void velocity_loop();
kosaka 12:a4b17bb682eb 91
kosaka 12:a4b17bb682eb 92 // start_timers(1); // start multi-timers, sampling times are TS0, TS1, TS2, TS3, TS4.
kosaka 12:a4b17bb682eb 93
kosaka 12:a4b17bb682eb 94 // IPMSMの機器定数等の設定, 制御器の初期化
kosaka 12:a4b17bb682eb 95 init_parameters();
kosaka 12:a4b17bb682eb 96 p.th[0] = 2*PI/3; //θの初期値
kosaka 12:a4b17bb682eb 97
kosaka 12:a4b17bb682eb 98 // p.Lq0 = p.Ld; // SPMSMの場合
kosaka 12:a4b17bb682eb 99 // p.phi = 0; // SynRMの場合
kosaka 12:a4b17bb682eb 100
kosaka 12:a4b17bb682eb 101 // w_ref=p.w; // 速度指令[rad/s]
kosaka 13:791e20f1af43 102 tan_beta_ref1 = tan(beta_ref); // tan30°を計算
kosaka 13:791e20f1af43 103 tan_beta_ref2 = tan(beta_ref-30*PI/180); // tan0°を計算
kosaka 13:791e20f1af43 104 tan_beta_ref = tan_beta_ref1; // βの指令値をtan30°に
kosaka 12:a4b17bb682eb 105 // シミュレーション開始
kosaka 12:a4b17bb682eb 106 pc2.printf("Simulation start!!\r\n");
kosaka 12:a4b17bb682eb 107 #ifndef OLD
kosaka 12:a4b17bb682eb 108 // start control (ON)
kosaka 12:a4b17bb682eb 109 start_pwm();
kosaka 12:a4b17bb682eb 110 TickerTimerTS0.attach(&timerTS0, TS0 ); // Sampling period[s] of i_controller
kosakaLab 15:427f5ae8e957 111 RtosTimerTS1.start((unsigned int)(TS1*1000.)); // Sampling period[ms] of th controller
kosaka 12:a4b17bb682eb 112 fTimerTS3ON = 1; // timerTS3 start
kosaka 12:a4b17bb682eb 113 fTimerTS4ON = 1; // timerTS3 start
kosaka 12:a4b17bb682eb 114 #endif
kosaka 12:a4b17bb682eb 115
kosaka 12:a4b17bb682eb 116 // set th by moving rotor to th=zero.
kosaka 13:791e20f1af43 117 f_find_origin=1; // 回転角原点復帰フラグをセット
kosaka 13:791e20f1af43 118 while( (t = _count*TS0) < TMAX_FIND_ORIGIN ){ // TMAX秒経過するまで制御実行
kosakaLab 15:427f5ae8e957 119 if (t<1) p.th_const = 0*2*PI*t*1; // Set theta with constant angular velosity
kosakaLab 15:427f5ae8e957 120 else if(t<3) p.th_const = 9*2*PI*t*1; // Set theta with constant angular velosity
kosakaLab 15:427f5ae8e957 121 else if(t<5) p.th_const = 15*2*PI*t*1; // Set theta with constant angular velosity
kosakaLab 15:427f5ae8e957 122 else if(t<TMAX_FIND_ORIGIN-2)
kosakaLab 15:427f5ae8e957 123 p.th_const = 9*2*PI*t*1; // Set theta with constant angular velosity
kosakaLab 15:427f5ae8e957 124 else p.th_const = 0*2*PI*t*1; // Set theta with constant angular velosity
kosaka 12:a4b17bb682eb 125 #ifdef OLD
kosaka 12:a4b17bb682eb 126 timerTS0();
kosaka 12:a4b17bb682eb 127 #endif
kosaka 13:791e20f1af43 128 Thread::wait(1);
kosaka 12:a4b17bb682eb 129 }
kosaka 13:791e20f1af43 130 // IPMSMの機器定数等の設定, 制御器の初期化
kosaka 13:791e20f1af43 131 init_parameters();
kosaka 12:a4b17bb682eb 132 f_find_origin=0;
kosaka 12:a4b17bb682eb 133
kosaka 12:a4b17bb682eb 134 #ifndef OLD
kosaka 12:a4b17bb682eb 135 TickerTimerTS0.detach(); // timer interrupt stop
kosaka 12:a4b17bb682eb 136 // start control (ON)
kosaka 12:a4b17bb682eb 137 TickerTimerTS0.attach(&timerTS0, TS0 ); // Sampling period[s] of i_controller
kosaka 12:a4b17bb682eb 138 RtosTimerTS1.start((unsigned int)(TS1*1000.)); // Sampling period[ms] of th controller
kosaka 12:a4b17bb682eb 139 #endif
kosaka 12:a4b17bb682eb 140
kosaka 13:791e20f1af43 141 while( (t = _count*TS0-TMAX_FIND_ORIGIN) < TMAX ){
kosaka 12:a4b17bb682eb 142 // debug_p24 = 1;
kosaka 13:791e20f1af43 143
kosaka 13:791e20f1af43 144 // 電流位相（ｄｑ軸電流）を変化させるベクトル制御
kosaka 13:791e20f1af43 145 if( t>=0.15 && t<0.19 ){ // 0.15～0.19秒の間に
kosaka 13:791e20f1af43 146 if( tan_beta_ref>tan_beta_ref2 ){ // βの指令値が0度以上のとき
kosaka 13:791e20f1af43 147 tan_beta_ref=tan_beta_ref-0.002; // 指令値を小さくする
kosaka 13:791e20f1af43 148 }
kosaka 13:791e20f1af43 149 }else{ // 0.15～0.19秒の間でないときに
kosaka 13:791e20f1af43 150 if( tan_beta_ref<tan_beta_ref1){ // βの指令値が30度以下のとき
kosaka 13:791e20f1af43 151 tan_beta_ref=tan_beta_ref+0.002; // 指令値を大きくする
kosaka 13:791e20f1af43 152 }
kosaka 12:a4b17bb682eb 153 }
kosaka 13:791e20f1af43 154 // 目標電流位相をメインルーチンから速度制御メインループへ渡す。
kosaka 13:791e20f1af43 155 vl.tan_beta_ref = tan_beta_ref;
kosaka 12:a4b17bb682eb 156
kosakaLab 15:427f5ae8e957 157 #if 0
kosaka 12:a4b17bb682eb 158 // 速度急変
kosaka 13:791e20f1af43 159 if( 0.26<=t && t<0.3 ){
kosaka 13:791e20f1af43 160 vl.w_ref=w_ref_req[1]; // 目標速度をメインルーチンから速度制御メインループへ渡す。
kosaka 13:791e20f1af43 161 }else{
kosaka 13:791e20f1af43 162 vl.w_ref=w_ref_req[0];
kosaka 12:a4b17bb682eb 163 }
kosakaLab 15:427f5ae8e957 164 #else
kosakaLab 15:427f5ae8e957 165 vl.w_ref=0.999*vl.w_ref + (1-0.999)*w_ref_req[0];
kosakaLab 15:427f5ae8e957 166 #endif
kosaka 12:a4b17bb682eb 167 #ifdef SIMULATION
kosaka 12:a4b17bb682eb 168 // 負荷トルク急変
kosaka 13:791e20f1af43 169 if( t<0.4 ){
kosaka 12:a4b17bb682eb 170 p.TL = 1;
kosaka 12:a4b17bb682eb 171 }else{
kosaka 12:a4b17bb682eb 172 p.TL = 2;
kosaka 12:a4b17bb682eb 173 }
kosaka 12:a4b17bb682eb 174 #endif
kosaka 12:a4b17bb682eb 175
kosaka 12:a4b17bb682eb 176 #ifdef OLD
kosaka 12:a4b17bb682eb 177 if( (++i2)>=(int)(TS1/TS0) ){ i2=0;
kosaka 12:a4b17bb682eb 178 timerTS1(&j); //velocity_loop(); // 速度制御メインループ(w_ref&beta_ref to idq_ref)
kosaka 12:a4b17bb682eb 179 }
kosaka 12:a4b17bb682eb 180 #endif
kosaka 12:a4b17bb682eb 181
kosaka 12:a4b17bb682eb 182 #ifdef OLD
kosaka 12:a4b17bb682eb 183 timerTS0();
kosaka 12:a4b17bb682eb 184 //current_loop(); // 電流制御マイナーループ(idq_ref to vuvw)
kosaka 12:a4b17bb682eb 185 //vuvw2pwm(); // vuvw to pwm
kosaka 12:a4b17bb682eb 186 //sim_motor(); // IPM, ｄｑ座標
kosaka 12:a4b17bb682eb 187 #endif
kosaka 12:a4b17bb682eb 188
kosaka 12:a4b17bb682eb 189 // if( (ms=(int)(TS1*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;}// ms=TS0
kosaka 12:a4b17bb682eb 190 // debug_p24 = 0;
kosaka 13:791e20f1af43 191 Thread::wait(1); // 1ms待つ
kosaka 12:a4b17bb682eb 192 }
kosaka 12:a4b17bb682eb 193 //pc2.printf("\r\n^0^ 2\r\n");
kosaka 12:a4b17bb682eb 194 // stop timers (OFF)
kosaka 12:a4b17bb682eb 195 stop_pwm();
kosaka 12:a4b17bb682eb 196 TickerTimerTS0.detach(); // timer interrupt stop
kosaka 12:a4b17bb682eb 197 RtosTimerTS1.stop(); // rtos timer stop
kosaka 12:a4b17bb682eb 198 // Thread::wait(1000); // wait till timerTS3 completed
kosaka 12:a4b17bb682eb 199 fTimerTS3ON=0;//ThreadTimerTS3.terminate(); //
kosaka 12:a4b17bb682eb 200 fTimerTS4ON=0;//ThreadTimerTS4.terminate(); //
kosaka 12:a4b17bb682eb 201 //pc2.printf("\r\n^0^ 0\r\n\r\n");
kosaka 12:a4b17bb682eb 202
kosaka 12:a4b17bb682eb 203 // save data to mbed USB drive
kosaka 12:a4b17bb682eb 204 // if ( NULL == (fp = fopen( "/mbedUSBdrive/data.csv", "w" )) ){ error( "" );} // save data to PC
kosaka 12:a4b17bb682eb 205 //pc2.printf("\r\n^0^ %d\r\n\r\n",_count_data);
kosaka 12:a4b17bb682eb 206 // emergencyStop.attach(&stop_fprintf, 0.001); // emergencyStop if fprintf keep busy for 3 secons
kosaka 12:a4b17bb682eb 207 for(i=0;i<_count_data;i++){
kosaka 12:a4b17bb682eb 208 //pc2.printf("%d: ",i);
kosaka 12:a4b17bb682eb 209 //pc2.printf("%f, %f, %f, %f, %f\r\n",
kosaka 12:a4b17bb682eb 210 // data[i][0],data[i][1],data[i][2],data[i][3],data[i][4]); // save data to PC (para, y, time, u)
kosaka 12:a4b17bb682eb 211 // sprintf(&chr[0],"Temperature: f ºC\r\n");//,data[i][0]);
kosaka 12:a4b17bb682eb 212 // sprintf(&chr[0],"%d, ", data[i][1]);
kosaka 12:a4b17bb682eb 213 // fprintf(fp,&chr[0]);
kosaka 12:a4b17bb682eb 214 // fprintf( fp, ", ");
kosaka 12:a4b17bb682eb 215 // fprintf( fp, "%d, ", data[i][1]*10000);
kosaka 12:a4b17bb682eb 216 // fprintf( fp, "\r\n");
kosaka 12:a4b17bb682eb 217 //
kosaka 13:791e20f1af43 218 // pc2.printf("%f, %f, %f, %f, %f\r\n",
kosaka 12:a4b17bb682eb 219 // fprintf( fp, "%f, %f, %f, %f, %f\r\n",
kosaka 12:a4b17bb682eb 220 // data[i][0],data[i][1],data[i][2],data[i][3],data[i][4]); // save data to PC (para, y, time, u)
kosaka 12:a4b17bb682eb 221 //
kosaka 12:a4b17bb682eb 222 // wait(0.1);
kosaka 12:a4b17bb682eb 223 // Thread::wait(100);
kosaka 12:a4b17bb682eb 224 }
kosaka 12:a4b17bb682eb 225 //pc2.printf("\r\n^0^ 2\r\n\r\n");
kosaka 12:a4b17bb682eb 226 fclose( fp ); // release mbed USB drive
kosaka 12:a4b17bb682eb 227
kosaka 12:a4b17bb682eb 228 Thread::wait(100);
kosaka 12:a4b17bb682eb 229 pc2.printf("Control completed!!\r\n\r\n");
kosaka 0:fe068497f773 230 }