UVW 3 phases Brushless DC motor control
Dependencies: QEI mbed-rtos mbed
Fork of BLDCmotor by
Diff: main.cpp
- Revision:
- 10:0984c90b820b
- Parent:
- 9:88d6270f95bc
- Child:
- 11:9747752435d1
--- a/main.cpp Sat Nov 24 01:21:59 2012 +0000 +++ b/main.cpp Sat Nov 24 12:01:02 2012 +0000 @@ -1,5 +1,5 @@ // DC motor control program using H-bridge driver (ex. TA7291P) and 360 resolution rotary encoder with A, B phase. -// ver. 121123b by Kosaka lab. +// ver. 121124a by Kosaka lab. #include "mbed.h" #include "rtos.h" #include "QEI.h" @@ -10,6 +10,8 @@ #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 @@ -20,8 +22,8 @@ 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.001 // [s], iTS, sampling time[s] of motor current i control PID using timer interrupt -#define thTS 0.01 // [s], thTS>=0.001[s], sampling time[s] of motor angle th PID using rtos-timer +#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 @@ -103,6 +105,7 @@ unsigned int f_deadtime, f_in, r_in; 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 @@ -112,6 +115,7 @@ } 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 @@ -143,9 +147,9 @@ float e_old, wt; float y, u; -// y_old = _th; // y_old=y(t-iTS) is older than y by 1 sampling time iTS[s]. update data +// y_old = _th; // y_old=y(t-TS) is older than y by 1 sampling time TS[s]. update data #ifdef SIMULATION - y = _th + iTS/0.1*(0.2*_iref*100-_th); //=(1-iTS/0.1)*_y + 0.2*iTS/0.1*_iref; // G = 0.2/(0.1s+1) + 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 @@ -153,20 +157,24 @@ #endif #define RMIN 0 wt = _freq_u *2.0*PI*_time; - if(wt>2*PI){ wt -= 2*PI*(float)((int)(wt/(2.0*PI)));} + 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-iTS) is older than e by 1 sampling time iTS[s]. update data + 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? +// 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) - } +// } } - u = _Kp4th*_e + _Kd4th*(_e-e_old)/iTS + _Ki4th*_eI; // PID output u(t) + 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; @@ -185,7 +193,7 @@ } #endif #if CONTROL_MODE==4 // FFT identification, u=repetive signal - if(count2==(int)(TS2/iTS)){ + if(count2==(int)(TS2/thTS)){ u = data[count3][4]; }else{ u = _iref; @@ -222,7 +230,7 @@ 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-iTS) is older than e by 1 sampling time iTS[s]. update data + 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) @@ -293,7 +301,7 @@ 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)(iTS*1000.)); // Sampling period[ms] of th 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 ) { @@ -302,7 +310,7 @@ //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], _y, _time, _u); // save data to PC (para, y, time, u) + 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] } @@ -319,7 +327,7 @@ 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), debug[0]*3.3/R_SHUNT); // 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 Thread::wait(200); } } @@ -370,7 +378,7 @@ 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<=5){ pc.scanf("%f",&val);} + 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;}