Revision 8:b8b31e9b60c2, committed 2012-11-23

Comitter:

kosaka

Date:

Fri Nov 23 05:53:26 2012 +0000

Parent:

7:613febb8f028

Child:

10:88d6270f95bc

Commit message:

motor current control minor-loop is added. ** shunt resistance is required **

Changed in this revision

--- a/main.cpp	Tue Nov 20 09:54:55 2012 +0000
+++ b/main.cpp	Fri Nov 23 05:53:26 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. 121118a by Kosaka lab.
+//      ver. 121123a by Kosaka lab.
 #include "mbed.h"
 #include "rtos.h"
 #include "QEI.h"
@@ -7,25 +7,37 @@
 /*********** 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 CONTROL_MODE    4   // 0:PID control, 1:Frequency response, 2:Step response, 3. u=Rand to identify G(s), 4) FFT identification
+  #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 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   _Kp=20;             // P gain for PID ... Kp=1, Ki=0, Kd=0 is good.
-float   _Ki=20;             // I gain for PID
-float   _Kd=5;              // D gain for PID
-#define TS      0.001       // [s], TS>=0.001[s], sampling time[s] of PID controller
-#define TS2     0.01        // [s], TS2>=0.001[s], sampling time[s] of data save to PC. But, max data length is 1000.
+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.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 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);
@@ -51,10 +63,9 @@
 LocalFileSystem local("local"); // 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.
+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.
-  #define PWM_FREQ 10000.0  //[Hz], pwm freq.
   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
@@ -66,18 +77,23 @@
 unsigned long _count;   // sampling number
 float   _time;          // time[s]
 float   _r;             // reference signal
-float   _y;             // control output
+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   _u;             // control input[V]
+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/TS); // 
+unsigned int    count2=(int)(TS2/iTS); // 
 #endif
 
 extern "C" void mbed_reset();
@@ -88,7 +104,7 @@
 
     if( u > 0 ){        // forward: rotate to plus
         duty = u/3.3;       // Vref
-        if(_f_u_plus==0){   // if plus to/from minus, set FIN=RIN=0/1 for 100[us].
+        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{
@@ -97,7 +113,7 @@
         f_in=1;  r_in=0;    // set forward direction
     }else if( u < 0 ){  // reverse: rotate to minus
         duty = -u/3.3;
-        if(_f_u_plus==1){   // if plus to/from minus, set FIN=RIN=0/1 for 100[us].
+        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{
@@ -123,28 +139,19 @@
 #endif
 }
 
-void controller(void const *argument) {    // if rtos. current controller & velocity controller
-//void controller() {    // if ticker. current controller & velocity controller
-    void    u2Hbridge(float);    // input u to TA7291 driver
+void th_controller(void const *argument) {    // if rtos. current controller & velocity controller
     float   e_old, wt;
-    float   y, u;     // to avoid time shift
+    float   y, u;
 
-    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;
-//    y_old = _y;  // y_old=y(t-TS) is older than y by 1 sampling time TS[s]. update data
+//    y_old = _th;  // y_old=y(t-iTS) is older than y by 1 sampling time iTS[s]. update data
 #ifdef SIMULATION
-    y = _y + TS/0.1*(0.2*_u*100-_y);   //=(1-TS/0.1)*_y + 0.2*TS/0.1*_u; // G = 0.2/(0.1s+1)
-//debug[0]=_u;//plus
+    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)
 #else
 //    semaphore1.wait();      //
     y = (float)encoder.getPulses()/(float)N_ENC*2.0*PI;   // get angle [rad] from encoder
 //    semaphore1.release();   //
 #endif
-//#ifdef R_SIN
-//  #define RMAX  (100./180.*PI)
-  #define RMIN  0
+#define RMIN  0
     wt = _freq_u *2.0*PI*_time;
     if(wt>2*PI){    wt -= 2*PI*(float)((int)(wt/(2.0*PI)));}
     _r = sin(wt ) * (_rmax-RMIN)/2.0 + (_rmax+RMIN)/2.0;
@@ -152,29 +159,15 @@
     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_old = _e;     // e_old=e(t-iTS) is older than e by 1 sampling time iTS[s]. update data
     _e = _r - y;    // error e(t)
-    if( _f_umax==0 ){
-        _eI = _eI + TS*_e;     // integral of e(t)
+    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)
+        }
     }
-    
-    u = _Kp*_e + _Kd*(_e-e_old)/TS + _Ki*_eI;   // PID output u(t)
-//debug[0]=_e;//minus
-//debug[0]=u;//minus
+    u = _Kp4th*_e + _Kd4th*(_e-e_old)/iTS + _Ki4th*_eI;   // PID output u(t)
  
-    // u is saturated? for anti-windup
-    if( u>UMAX ){
-        _eI -= (u-UMAX)/_Ki;    if(_eI<0){   _eI=0;}
-        u = UMAX;
-//        _f_umax = 1;
-    } else if( u<UMIN ){
-        _eI -= (u-UMIN)/_Ki;    if(_eI>0){   _eI=0;}
-        u = UMIN;
-//        _f_umax = 1;
-    }else{
-        _f_umax = 0;
-    }
-//#define CONTROL_MODE   2   // 0:PID control, 1:Frequency response, 2:Step response
 #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));
@@ -185,34 +178,87 @@
     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/TS)){
+    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 = _u;
+        u = _iref;
     }
 #endif
 #if CONTROL_MODE==4                 // FFT identification, u=repetive signal
-    if(count2==(int)(TS2/TS)){
+    if(count2==(int)(TS2/iTS)){
         u = data[count3][4];
     }else{
-        u = _u;
+        u = _iref;
     }
 #endif
-//debug[0]=u;//minus
-    u2Hbridge(u);    // input u to TA7291 driver
+    // 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-iTS) is older than e by 1 sampling time iTS[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;     // without current control.
+#endif
+
+    u2Hbridge(_u);  // input u to TA7291 driver
 
     //-------- update data
-    _time += TS;    // time
-    _y = y;
-    _u = u;
-//debug[0]=_u;//minus
-//debug[0]=_eI;
-debug[0]=_r;
+    _time += iTS;    // time
+debug[0]=v_shunt_r; if(_f_u_plus==0){   debug[0]=-debug[0];}
 #ifdef  GOOD_DATA
-    if(count2==(int)(TS2/TS)){
-//        j=0; if(_count>=j&&_count<j+1000){i=_count-j;  data[i][0]=_r; data[i][1]=debug[0]; data[i][2]=_y; data[i][3]=_time; data[i][4]=_u;}
+    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]=_y; data[count3][3]=_time; data[count3][4]=_u;
+            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++;
         }
         count2 = 0;
@@ -225,7 +271,7 @@
 }
 
 void main1() {
-    RtosTimer timer_controller(controller);
+    RtosTimer timer_controller(th_controller);
     FILE *fp;   // save data to PC
 #ifdef  GOOD_DATA
     int i;
@@ -235,10 +281,10 @@
     u2Hbridge(0);           // initialize H bridge to stop mode
     _count=0;
     _time = 0;  // time
-    _e = _eI = 0;
+    _eI = _eiI = 0;    // reset integrater
     encoder.reset();    // set encoder counter zero
-    _y = (float)encoder.getPulses()/(float)N_ENC*2.0*PI;   // get angle [rad] from encoder
-    _r = _r + _y;
+    _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");
@@ -246,10 +292,10 @@
 #ifdef  USE_PWM
     FIN.period( 1.0 / PWM_FREQ );   // PWM period [s]. Common to all PWM
 #endif
-//    controller_ticker.attach(&controller, TS ); // period [s]
-    timer_controller.start((unsigned int)(TS*1000.));   // Sampling period[ms]
+    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
 
-//    for ( i = 0; i < (unsigned int)(TMAX/TS2); i++ ) {
+//    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.
@@ -260,8 +306,10 @@
 #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
@@ -271,7 +319,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)(_y/(2*PI)*360.0), debug[0]/(2*PI)*360.0);  // 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), debug[0]*3.3/R_SHUNT);  // print to tera term
         Thread::wait(200);
     }
 }
@@ -317,19 +365,24 @@
 //        pc.scanf("%f",&_freq_u);
 //        pc.printf("%8.3f[Hz]\r\n", _freq_u);  // print to tera term
 //  #endif
-        pc.printf("Kp=%f, Ki=%f, Kd=%f, r=%f[deg], %f Hz\r\n",_Kp, _Ki, _Kd, _rmax*180./PI, _freq_u);
-        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]. 9)reset mbed ?");
+        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<=5){ pc.scanf("%f",&val);}
         pc.printf("%8.3f\r\n", val);  // print to tera term
-        if(f==1){    _Kp = val;}
-        if(f==2){    _Ki = val;}
-        if(f==3){    _Kd = val;}
+        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("Kp=%f, Ki=%f, Kd=%f, r=%f[deg], %f Hz\r\n",_Kp, _Ki, _Kd, _rmax*180./PI, _freq_u);
+        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
     }    
 }