UVW 3 phases Brushless DC motor control
Dependencies: QEI mbed-rtos mbed
Fork of DCmotor by
main.cpp@12:a4b17bb682eb, 2012-12-21 (annotated)
- Committer:
- kosaka
- Date:
- Fri Dec 21 22:06:56 2012 +0000
- Revision:
- 12:a4b17bb682eb
- Parent:
- 11:9747752435d1
- Child:
- 13:791e20f1af43
121222a
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kosaka | 12:a4b17bb682eb | 1 | // UVW three phases Blushless DC motor control program using 3 H-bridge driver (ex. BD6211F) and 360 resolution rotary encoder with A, B phase. |
kosaka | 12:a4b17bb682eb | 2 | // ver. 121206 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 | FILE *fp = fopen("/mbedUSBdrive/data.csv", "w"); //save data to PC |
kosaka | 12:a4b17bb682eb | 21 | Timeout emergencyStop; // kill fprintf process when bug |
kosaka | 0:fe068497f773 | 22 | |
kosaka | 0:fe068497f773 | 23 | |
kosaka | 12:a4b17bb682eb | 24 | void CallTimerTS2(void const *argument) { // make sampling time TS3 timer (priority 3: precision 4ms) |
kosaka | 12:a4b17bb682eb | 25 | int ms; |
kosaka | 12:a4b17bb682eb | 26 | unsigned long c; |
kosaka | 12:a4b17bb682eb | 27 | while (true) { |
kosaka | 12:a4b17bb682eb | 28 | c = _count; |
kosaka | 12:a4b17bb682eb | 29 | if( fTimerTS2ON ){ |
kosaka | 12:a4b17bb682eb | 30 | timerTS2(); // timerTS2() is called every TS2[s]. |
kosaka | 12:a4b17bb682eb | 31 | } |
kosaka | 12:a4b17bb682eb | 32 | if( (ms=(int)(TS2*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;} |
kosaka | 12:a4b17bb682eb | 33 | Thread::wait(ms); |
kosaka | 0:fe068497f773 | 34 | } |
kosaka | 8:b8b31e9b60c2 | 35 | } |
kosaka | 12:a4b17bb682eb | 36 | void CallTimerTS3(void const *argument) { // make sampling time TS3 timer (priority 3: precision 4ms) |
kosaka | 12:a4b17bb682eb | 37 | int ms; |
kosaka | 12:a4b17bb682eb | 38 | unsigned long c; |
kosaka | 12:a4b17bb682eb | 39 | while (true) { |
kosaka | 12:a4b17bb682eb | 40 | c = _count; |
kosaka | 12:a4b17bb682eb | 41 | if( fTimerTS3ON ){ |
kosaka | 12:a4b17bb682eb | 42 | timerTS3(); // timerTS3() is called every TS3[s]. |
kosaka | 2:e056793d6fc5 | 43 | } |
kosaka | 12:a4b17bb682eb | 44 | if( (ms=(int)(TS3*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;} |
kosaka | 12:a4b17bb682eb | 45 | Thread::wait(ms); |
kosaka | 0:fe068497f773 | 46 | } |
kosaka | 0:fe068497f773 | 47 | } |
kosaka | 0:fe068497f773 | 48 | |
kosaka | 12:a4b17bb682eb | 49 | void CallTimerTS4(void const *argument) { // make sampling time TS4 timer (priority 4: precision 4ms) |
kosaka | 12:a4b17bb682eb | 50 | int ms; |
kosaka | 12:a4b17bb682eb | 51 | unsigned long c; |
kosaka | 12:a4b17bb682eb | 52 | while (true) { |
kosaka | 12:a4b17bb682eb | 53 | c = _count; |
kosaka | 12:a4b17bb682eb | 54 | if( fTimerTS4ON ){ |
kosaka | 12:a4b17bb682eb | 55 | timerTS4(); // timerTS4() is called every TS4[s]. |
kosaka | 7:613febb8f028 | 56 | } |
kosaka | 12:a4b17bb682eb | 57 | if( (ms=(int)(TS4*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;} |
kosaka | 12:a4b17bb682eb | 58 | Thread::wait(ms); |
kosaka | 12:a4b17bb682eb | 59 | } |
kosaka | 12:a4b17bb682eb | 60 | } |
kosaka | 0:fe068497f773 | 61 | |
kosaka | 12:a4b17bb682eb | 62 | //void stop_fprintf(){ // emergencyStop if fprintf keep busy for 3 secons |
kosaka | 12:a4b17bb682eb | 63 | // fclose(fp); |
kosaka | 12:a4b17bb682eb | 64 | // pc2.printf("error: fprintf was in hung-up!"); |
kosaka | 12:a4b17bb682eb | 65 | //} |
kosaka | 0:fe068497f773 | 66 | |
kosaka | 12:a4b17bb682eb | 67 | //#define OLD |
kosaka | 12:a4b17bb682eb | 68 | int main(){ |
kosaka | 12:a4b17bb682eb | 69 | int ms=1; |
kosaka | 12:a4b17bb682eb | 70 | unsigned long c, c2; |
kosaka | 12:a4b17bb682eb | 71 | unsigned short i, i2=0; |
kosaka | 12:a4b17bb682eb | 72 | // FILE *fp; // save data to PC |
kosaka | 12:a4b17bb682eb | 73 | // FILE *fp = fopen("/mbedUSBdrive/data.csv", "w"); |
kosaka | 12:a4b17bb682eb | 74 | char chr[100]; |
kosaka | 12:a4b17bb682eb | 75 | RtosTimer RtosTimerTS1(timerTS1); // RtosTimer priority is osPriorityAboveNormal, just one above main() |
kosaka | 12:a4b17bb682eb | 76 | Thread ThreadTimerTS3(CallTimerTS3,NULL,osPriorityBelowNormal); |
kosaka | 12:a4b17bb682eb | 77 | Thread ThreadTimerTS4(CallTimerTS4,NULL,osPriorityLow); |
kosaka | 12:a4b17bb682eb | 78 | // Priority of Thread (RtosTimer is osPriorityAboveNormal) |
kosaka | 0:fe068497f773 | 79 | // osPriorityIdle = -3, ///< priority: idle (lowest)--> then, mbed ERROR!! |
kosaka | 0:fe068497f773 | 80 | // osPriorityLow = -2, ///< priority: low |
kosaka | 0:fe068497f773 | 81 | // osPriorityBelowNormal = -1, ///< priority: below normal |
kosaka | 0:fe068497f773 | 82 | // osPriorityNormal = 0, ///< priority: normal (default) |
kosaka | 0:fe068497f773 | 83 | // osPriorityAboveNormal = +1, ///< priority: above normal |
kosaka | 0:fe068497f773 | 84 | // osPriorityHigh = +2, ///< priority: high |
kosaka | 0:fe068497f773 | 85 | // osPriorityRealtime = +3, ///< priority: realtime (highest) |
kosaka | 0:fe068497f773 | 86 | // osPriorityError = 0x84 ///< system cannot determine priority or thread has illegal priority |
kosaka | 12:a4b17bb682eb | 87 | |
kosaka | 12:a4b17bb682eb | 88 | // シミュレーション総サンプル数 |
kosaka | 12:a4b17bb682eb | 89 | int N;// = 5000; |
kosaka | 12:a4b17bb682eb | 90 | // 指令速度 |
kosaka | 12:a4b17bb682eb | 91 | float w_ref_req[2] = {20* 2*PI, 40* 2*PI}; // [rad/s](第2要素は指令速度急変後の指令速度) |
kosaka | 12:a4b17bb682eb | 92 | float w_ref; |
kosaka | 12:a4b17bb682eb | 93 | // 指令dq電流位相 |
kosaka | 12:a4b17bb682eb | 94 | float beta_ref = 30*PI/180; // rad |
kosaka | 12:a4b17bb682eb | 95 | float tan_beta_ref1; |
kosaka | 12:a4b17bb682eb | 96 | float tan_beta_ref2,tan_beta_ref; |
kosaka | 12:a4b17bb682eb | 97 | |
kosaka | 12:a4b17bb682eb | 98 | // start_timers(1); // start multi-timers, sampling times are TS0, TS1, TS2, TS3, TS4. |
kosaka | 12:a4b17bb682eb | 99 | |
kosaka | 12:a4b17bb682eb | 100 | N = (int)(TMAX/TS0); |
kosaka | 12:a4b17bb682eb | 101 | pc2.printf("N=%d\r\n",N); |
kosaka | 12:a4b17bb682eb | 102 | // IPMSMの機器定数等の設定, 制御器の初期化 |
kosaka | 12:a4b17bb682eb | 103 | init_parameters(); |
kosaka | 12:a4b17bb682eb | 104 | p.th[0] = 2*PI/3; //θの初期値 |
kosaka | 12:a4b17bb682eb | 105 | |
kosaka | 12:a4b17bb682eb | 106 | |
kosaka | 12:a4b17bb682eb | 107 | // p.Lq0 = p.Ld; // SPMSMの場合 |
kosaka | 12:a4b17bb682eb | 108 | // p.phi = 0; // SynRMの場合 |
kosaka | 12:a4b17bb682eb | 109 | |
kosaka | 12:a4b17bb682eb | 110 | // w_ref=p.w; // 速度指令[rad/s] |
kosaka | 12:a4b17bb682eb | 111 | tan_beta_ref1 = tan(beta_ref); |
kosaka | 12:a4b17bb682eb | 112 | tan_beta_ref2 = tan(beta_ref-30*PI/180); |
kosaka | 12:a4b17bb682eb | 113 | tan_beta_ref = tan_beta_ref1; |
kosaka | 12:a4b17bb682eb | 114 | // シミュレーション開始 |
kosaka | 12:a4b17bb682eb | 115 | pc2.printf("Simulation start!!\r\n"); |
kosaka | 12:a4b17bb682eb | 116 | #ifndef OLD |
kosaka | 12:a4b17bb682eb | 117 | // start control (ON) |
kosaka | 12:a4b17bb682eb | 118 | start_pwm(); |
kosaka | 12:a4b17bb682eb | 119 | TickerTimerTS0.attach(&timerTS0, TS0 ); // Sampling period[s] of i_controller |
kosaka | 12:a4b17bb682eb | 120 | // RtosTimerTS1.start((unsigned int)(TS1*1000.)); // Sampling period[ms] of th controller |
kosaka | 12:a4b17bb682eb | 121 | fTimerTS3ON = 1; // timerTS3 start |
kosaka | 12:a4b17bb682eb | 122 | fTimerTS4ON = 1; // timerTS3 start |
kosaka | 12:a4b17bb682eb | 123 | #endif |
kosaka | 12:a4b17bb682eb | 124 | |
kosaka | 12:a4b17bb682eb | 125 | // set th by moving rotor to th=zero. |
kosaka | 12:a4b17bb682eb | 126 | f_find_origin=1; |
kosaka | 12:a4b17bb682eb | 127 | while( _count*TS0<0.1 ){ // while( time<1 ){ |
kosaka | 12:a4b17bb682eb | 128 | // debug_p24 = 1; |
kosaka | 12:a4b17bb682eb | 129 | il.idq_ref[0] = iqMAX/1.0; |
kosaka | 12:a4b17bb682eb | 130 | il.idq_ref[1] = 0; |
kosaka | 12:a4b17bb682eb | 131 | |
kosaka | 12:a4b17bb682eb | 132 | #ifdef OLD |
kosaka | 12:a4b17bb682eb | 133 | timerTS0(); |
kosaka | 12:a4b17bb682eb | 134 | //current_loop(); // 電流制御マイナーループ(idq_ref to vuvw) |
kosaka | 12:a4b17bb682eb | 135 | //vuvw2pwm(); // vuvw to pwm |
kosaka | 12:a4b17bb682eb | 136 | //sim_motor(); // IPM, dq座標 |
kosaka | 12:a4b17bb682eb | 137 | #endif |
kosaka | 12:a4b17bb682eb | 138 | |
kosaka | 12:a4b17bb682eb | 139 | // if( (ms=(int)(TS1*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;}// ms=TS0 |
kosaka | 12:a4b17bb682eb | 140 | Thread::wait(ms); |
kosaka | 12:a4b17bb682eb | 141 | // debug_p24 = 0; |
kosaka | 12:a4b17bb682eb | 142 | } |
kosaka | 12:a4b17bb682eb | 143 | //pc2.printf("\r\n^0^ 0\r\n"); |
kosaka | 12:a4b17bb682eb | 144 | #ifndef SIMULATION |
kosaka | 12:a4b17bb682eb | 145 | encoder.reset(); // set encoder counter zero |
kosaka | 12:a4b17bb682eb | 146 | p.th[0] = p.th[1] = (float)encoder.getPulses()/(float)N_ENC*2.0*PI; // get angle [rad] from encoder |
kosaka | 12:a4b17bb682eb | 147 | #endif |
kosaka | 12:a4b17bb682eb | 148 | c2 = _count; |
kosaka | 12:a4b17bb682eb | 149 | f_find_origin=0; |
kosaka | 12:a4b17bb682eb | 150 | |
kosaka | 12:a4b17bb682eb | 151 | #ifndef OLD |
kosaka | 12:a4b17bb682eb | 152 | TickerTimerTS0.detach(); // timer interrupt stop |
kosaka | 12:a4b17bb682eb | 153 | // start control (ON) |
kosaka | 12:a4b17bb682eb | 154 | TickerTimerTS0.attach(&timerTS0, TS0 ); // Sampling period[s] of i_controller |
kosaka | 12:a4b17bb682eb | 155 | RtosTimerTS1.start((unsigned int)(TS1*1000.)); // Sampling period[ms] of th controller |
kosaka | 12:a4b17bb682eb | 156 | #endif |
kosaka | 12:a4b17bb682eb | 157 | |
kosaka | 12:a4b17bb682eb | 158 | for( i=0;i<N;i++ ){ |
kosaka | 12:a4b17bb682eb | 159 | // debug_p24 = 1; |
kosaka | 12:a4b17bb682eb | 160 | c = _count-c2; |
kosaka | 12:a4b17bb682eb | 161 | // 電流位相(dq軸電流)変化 |
kosaka | 12:a4b17bb682eb | 162 | // if( i>=1500&&i<1900 ){// TS0=0.0001 |
kosaka | 12:a4b17bb682eb | 163 | if( c>=1500*0.0001/TS0&&c<1900*0.0001/TS0 ){ |
kosaka | 12:a4b17bb682eb | 164 | if( tan_beta_ref>tan_beta_ref2 ){ tan_beta_ref=tan_beta_ref-0.002;} |
kosaka | 12:a4b17bb682eb | 165 | }else{ |
kosaka | 12:a4b17bb682eb | 166 | if( tan_beta_ref<tan_beta_ref1){ tan_beta_ref=tan_beta_ref+0.002;} |
kosaka | 12:a4b17bb682eb | 167 | } |
kosaka | 12:a4b17bb682eb | 168 | |
kosaka | 12:a4b17bb682eb | 169 | // 速度急変 |
kosaka | 12:a4b17bb682eb | 170 | w_ref = w_ref_req[0]; |
kosaka | 12:a4b17bb682eb | 171 | if( 2600*0.0001/TS0<=c&&c<3000*0.0001/TS0 ){ |
kosaka | 12:a4b17bb682eb | 172 | w_ref=w_ref_req[1]; |
kosaka | 12:a4b17bb682eb | 173 | //pc2.printf(".\r\n"); |
kosaka | 12:a4b17bb682eb | 174 | } |
kosaka | 12:a4b17bb682eb | 175 | #ifdef SIMULATION |
kosaka | 12:a4b17bb682eb | 176 | // 負荷トルク急変 |
kosaka | 12:a4b17bb682eb | 177 | if( c<4000*0.0001/TS0 ){ |
kosaka | 12:a4b17bb682eb | 178 | p.TL = 1; |
kosaka | 12:a4b17bb682eb | 179 | }else{ |
kosaka | 12:a4b17bb682eb | 180 | p.TL = 2; |
kosaka | 12:a4b17bb682eb | 181 | } |
kosaka | 12:a4b17bb682eb | 182 | #endif |
kosaka | 12:a4b17bb682eb | 183 | vl.w_ref = w_ref; // 目標速度をメインルーチンから速度制御メインループへ渡す。 |
kosaka | 12:a4b17bb682eb | 184 | vl.tan_beta_ref = tan_beta_ref; // 目標電流位相をメインルーチンから速度制御メインループへ渡す。 |
kosaka | 12:a4b17bb682eb | 185 | |
kosaka | 12:a4b17bb682eb | 186 | #ifdef OLD |
kosaka | 12:a4b17bb682eb | 187 | if( (++i2)>=(int)(TS1/TS0) ){ i2=0; |
kosaka | 12:a4b17bb682eb | 188 | timerTS1(&j); //velocity_loop(); // 速度制御メインループ(w_ref&beta_ref to idq_ref) |
kosaka | 12:a4b17bb682eb | 189 | } |
kosaka | 12:a4b17bb682eb | 190 | #endif |
kosaka | 12:a4b17bb682eb | 191 | |
kosaka | 12:a4b17bb682eb | 192 | #ifdef OLD |
kosaka | 12:a4b17bb682eb | 193 | timerTS0(); |
kosaka | 12:a4b17bb682eb | 194 | //current_loop(); // 電流制御マイナーループ(idq_ref to vuvw) |
kosaka | 12:a4b17bb682eb | 195 | //vuvw2pwm(); // vuvw to pwm |
kosaka | 12:a4b17bb682eb | 196 | //sim_motor(); // IPM, dq座標 |
kosaka | 12:a4b17bb682eb | 197 | #endif |
kosaka | 12:a4b17bb682eb | 198 | |
kosaka | 12:a4b17bb682eb | 199 | // if( (ms=(int)(TS1*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;}// ms=TS0 |
kosaka | 12:a4b17bb682eb | 200 | Thread::wait(ms); |
kosaka | 12:a4b17bb682eb | 201 | // debug_p24 = 0; |
kosaka | 12:a4b17bb682eb | 202 | } |
kosaka | 12:a4b17bb682eb | 203 | //pc2.printf("\r\n^0^ 2\r\n"); |
kosaka | 12:a4b17bb682eb | 204 | // stop timers (OFF) |
kosaka | 12:a4b17bb682eb | 205 | stop_pwm(); |
kosaka | 12:a4b17bb682eb | 206 | TickerTimerTS0.detach(); // timer interrupt stop |
kosaka | 12:a4b17bb682eb | 207 | RtosTimerTS1.stop(); // rtos timer stop |
kosaka | 12:a4b17bb682eb | 208 | // Thread::wait(1000); // wait till timerTS3 completed |
kosaka | 12:a4b17bb682eb | 209 | fTimerTS3ON=0;//ThreadTimerTS3.terminate(); // |
kosaka | 12:a4b17bb682eb | 210 | fTimerTS4ON=0;//ThreadTimerTS4.terminate(); // |
kosaka | 12:a4b17bb682eb | 211 | //pc2.printf("\r\n^0^ 0\r\n\r\n"); |
kosaka | 12:a4b17bb682eb | 212 | |
kosaka | 12:a4b17bb682eb | 213 | // save data to mbed USB drive |
kosaka | 12:a4b17bb682eb | 214 | // if ( NULL == (fp = fopen( "/mbedUSBdrive/data.csv", "w" )) ){ error( "" );} // save data to PC |
kosaka | 12:a4b17bb682eb | 215 | //pc2.printf("\r\n^0^ %d\r\n\r\n",_count_data); |
kosaka | 12:a4b17bb682eb | 216 | // emergencyStop.attach(&stop_fprintf, 0.001); // emergencyStop if fprintf keep busy for 3 secons |
kosaka | 12:a4b17bb682eb | 217 | for(i=0;i<_count_data;i++){ |
kosaka | 12:a4b17bb682eb | 218 | //pc2.printf("%d: ",i); |
kosaka | 12:a4b17bb682eb | 219 | //pc2.printf("%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 | // sprintf(&chr[0],"Temperature: f ºC\r\n");//,data[i][0]); |
kosaka | 12:a4b17bb682eb | 222 | // sprintf(&chr[0],"%d, ", data[i][1]); |
kosaka | 12:a4b17bb682eb | 223 | // fprintf(fp,&chr[0]); |
kosaka | 12:a4b17bb682eb | 224 | // fprintf( fp, ", "); |
kosaka | 12:a4b17bb682eb | 225 | // fprintf( fp, "%d, ", data[i][1]*10000); |
kosaka | 12:a4b17bb682eb | 226 | // fprintf( fp, "\r\n"); |
kosaka | 12:a4b17bb682eb | 227 | // |
kosaka | 12:a4b17bb682eb | 228 | // fprintf( fp, "%f, %f, %f, %f, %f\r\n", |
kosaka | 12:a4b17bb682eb | 229 | // 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 | 230 | // |
kosaka | 12:a4b17bb682eb | 231 | // wait(0.1); |
kosaka | 12:a4b17bb682eb | 232 | // Thread::wait(100); |
kosaka | 12:a4b17bb682eb | 233 | } |
kosaka | 12:a4b17bb682eb | 234 | //pc2.printf("\r\n^0^ 2\r\n\r\n"); |
kosaka | 12:a4b17bb682eb | 235 | fclose( fp ); // release mbed USB drive |
kosaka | 12:a4b17bb682eb | 236 | |
kosaka | 12:a4b17bb682eb | 237 | Thread::wait(100); |
kosaka | 12:a4b17bb682eb | 238 | pc2.printf("Control completed!!\r\n\r\n"); |
kosaka | 0:fe068497f773 | 239 | } |