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;}