Revision 15:744a81d5b7ac, committed 2012-12-08

Comitter:

kosaka

Date:

Sat Dec 08 05:01:53 2012 +0000

Parent:

14:1196c2d455ae

Child:

16:759d6f647c83

Commit message:

more readable

Changed in this revision

--- a/main.cpp	Sun Dec 02 10:03:09 2012 +0000
+++ b/main.cpp	Sat Dec 08 05:01:53 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. 121202a by Kosaka lab.
+//      ver. 121208a by Kosaka lab.
 #include "mbed.h"
 #include "rtos.h"
 #include "QEI.h"
@@ -8,25 +8,25 @@
 //#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 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 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
-float   _rmax=100./180.*PI; // [rad], max. of reference signal
+float   _rmax=360./180.*PI; // [rad], max. of reference signal
 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 TS0     0.0001      // [s], sampling time (priority highest: Ticker IRQ) of motor current i control PID using timer interrupt
-#define TS1     0.001       // [s], sampling time (priority high: RtosTimer) of motor angle th PID using rtos-timer
-#define TS2     0.01        // [s], sampling time (priority =main(): precision 4ms) to save data to PC using thread. But, max data length is 1000.
-#define TS3     0.05        // [s], sampling time (priority low: precision 4ms)
-#define TS4     0.1         // [s], sampling time (priority lowest: precision 4ms)  to display data to PC tera term
+#define TS0     0.001//08//8      // [s], sampling time (priority highest: Ticker IRQ) of motor current i control PID using timer interrupt
+#define TS1     0.002//2//0.01       // [s], sampling time (priority high: RtosTimer) of motor angle th PID using rtos-timer
+#define TS2     0.05        // [s], sampling time (priority =main(): precision 4ms)
+#define TS3     0.02        // [s], sampling time (priority low: precision 4ms) to save data to PC using thread. But, max data length is 1000.
+#define TS4     0.2         // [s], sampling time (priority lowest: precision 4ms) to display data to PC tera term
 #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
@@ -38,6 +38,8 @@
 #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
+DigitalOut  debug_p23(p23); // p17 for debug
+DigitalOut  debug_p24(p24); // 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
@@ -67,7 +69,7 @@
 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 TickerTimerTS0;          // 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.
   PwmOut FIN(FIN_PORT);     // PWM for FIN, RIN=0 when forward rotation. H bridge driver PWM mode
@@ -92,12 +94,13 @@
 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   debug[20];      // 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/TS0); // 
+unsigned int    count2=(int)(TS3/TS0); // 
+unsigned int    _count_data=0; // data2mbedUSB()
 #endif
 
 extern "C" void mbed_reset();
@@ -151,7 +154,9 @@
 
 //    y_old = _th;  // y_old=y(t-TS) is older than y by 1 sampling time TS[s]. update data
 #ifdef SIMULATION
-    y = _th + TS1/0.1*(0.2*_iref*100-_th);   //=(1-TS/0.1)*_y + 0.2*TS/0.1*_iref; // G = 0.2/(0.1s+1)
+    if( (u=_iref)>IMAX ){  u-=IMAX;}else if(u<IMIN){    u+=IMIN;}
+    y = _th + TS1/10*(20*u-_th);   //=(1-TS/0.1)*_y + 0.2*TS/0.1*_iref; // G = 20/(10s+1)
+debug[0] =_iref;
 #else
 //    semaphore1.wait();      //
     y = (float)encoder.getPulses()/(float)N_ENC*2.0*PI;   // get angle [rad] from encoder
@@ -161,13 +166,13 @@
     wt = _freq_u *2.0*PI*_time;
     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-TS) is older than e by 1 sampling time TS[s]. update data
     _e = _r - y;    // error e(t)
+//debug[0]=_e;
     if( _e<((360.0/N_ENC)/180*PI) && _e>-((360.0/N_ENC)/180*PI) ){   // e is inside minimum precision?
         _e = 0;
     }
@@ -184,18 +189,18 @@
     u = sin(wt ) * (UMAX-UMIN)/2.0 + (UMAX+UMIN)/2.0;
 #endif
 #if CONTROL_MODE==2   // Step response
-    if( u>=0 )  u = UMAX;
-    else        u = UMIN;
+    if( u>=0 )  u = IMAX/2.;
+    else        u = IMIN/2.;
 #endif
 #if CONTROL_MODE==3                 // u=rand() to identify motor transfer function G(s) from V to angle
-    if(count2==(int)(TS2/TS0)){
+    if(count2==(int)(TS3/TS0)){
         u = ((float)rand()/RAND_MAX*2.0-1.0) * (UMAX-1.5)/2.0 + (UMAX+1.5)/2.0;
     }else{
         u = _iref;
     }
 #endif
 #if CONTROL_MODE==4                 // FFT identification, u=repetive signal
-    if(count2==(int)(TS2/TS1)){
+    if(count2==(int)(TS3/TS1)){
         u = data[count3][4];
     }else{
         u = _iref;
@@ -216,6 +221,7 @@
     //-------- update data
     _th = y;
     _iref = u;
+//debug[0] =_iref;
 }
 void i_controller() {    // if ticker. current controller & velocity controller
     void    u2Hbridge(float);    // input u to H bridge (full bridge) driver
@@ -229,7 +235,11 @@
 
     _count+=1;
     // current PID controller
+  #ifdef SIMULATION
     y = v_shunt_r/R_SHUNT;      // get i [A] from shunt resistance
+  #else
+    y = _iref;
+  #endif
     if(_f_u_plus==0){   y=-y;}
 
     e_old = _ei;     // e_old=e(t-TS) is older than e by 1 sampling time TS[s]. update data
@@ -255,26 +265,14 @@
     _i = y;
     _u = u;
 #else
-    _u = _iref/IMAX*VMAX;     // without current control.
+    _u = _iref/IMAX*UMAX;     // without current control.
 #endif
 
     u2Hbridge(_u);  // input u to TA7291 driver
 
     //-------- update data
     _time += TS0;    // time
-debug[0]=v_shunt_r; if(_f_u_plus==0){   debug[0]=-debug[0];}
-#ifdef  GOOD_DATA
-    if(count2==(int)(TS2/TS0)){
-//        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]=_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;
-    }
-    count2++;
-#endif
+//debug[0]=v_shunt_r; if(_f_u_plus==0){   debug[0]=-debug[0];}
     //-------- update data
 
     debug_p17 = 0;  // for debug: processing time check
@@ -291,62 +289,40 @@
     FIN.period( 1.0 / PWM_FREQ );   // PWM period [s]. Common to all PWM
 #endif
 }
-void moror_control() {  // motor control ON for TMAX seconds.
-    RtosTimer timer_controller(th_controller);  // RtosTimer priority is osPriorityAboveNormal, just one above main()
-    FILE *fp;   // save data to PC
-    float   t=0;
-#ifdef  GOOD_DATA
-    int i;
 
-    count3=0;
-#endif
-    init_controller(); // initialize controller parameters and signals
-    _r = _r + _th;
-//    if( _r>2*PI )    _r -= _r-2*PI;
-
-    if ( NULL == (fp = fopen( "/local/data.csv", "w" )) ){   error( "" );} // save data to PC
-
-    // start control (ON)
-    controller_ticker.attach(&i_controller, TS0 ); // Sampling period[s] of i_controller
-    timer_controller.start((unsigned int)(TS1*1000.));   // Sampling period[ms] of th controller
-
-    t = _time;
-    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.
-//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)
-#ifndef GOOD_DATA
-        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-(_time-t))*1000.));  //[ms]
-        t = _time;
+void data2mbedUSB(){    // store data to save to mbedUSB after experiment is over
+    if( _count_data<1000 ){
+        data[_count_data][0]=_r; data[_count_data][1]=debug[0];
+        data[_count_data][2]=_th; data[_count_data][3]=_time; data[_count_data][4]=_u;
+        _count_data++;
     }
-    // stop control (OFF)
-    controller_ticker.detach(); // timer interrupt stop
-    timer_controller.stop();    // rtos timer stop
-
-    init_controller(); // initialize controller parameters and signals
-#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
-    fclose( fp );               // release mbed USB drive
+//BUG    for(j=0;j<19;j++){  fprintf( fp, "%f, ",debug[j]);} fprintf( fp, "%f\n",debug[19]);
 }
-
 void display2PC(){  // display to tera term on PC
+//    pc.printf("%8.1f[s]\t%8.5f[V]\t%4d [Hz]\t%d\r\n", _time, il.vdq_ref[0], (int)(vl.w_lpf/(2*PI)+0.5), (int)(vl.w_ref/(2*PI)+0.5));  // 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
 }
-void TS3timer(void const *argument) {   // make sampling time TS4 timer (priority low: precision 4ms)
+void timerTS2(void const *argument) {   // make sampling time TS2 timer (priority 2: precision 4ms)
     int ms;
     unsigned long c;
     while (true) {
         c = _count;
-        //dummy();  // dummy() is called every TS3[s].
+        //dummy();  // dummy() is called every TS2[s].
+        if( (ms=(int)(TS2*1000-(_count-c)*TS0*1000))<=0 ){    ms=1;}
+        Thread::wait(ms);
+    }
+}
+void timerTS3(void const *argument) {   // make sampling time TS3 timer (priority 3: precision 4ms)
+    int ms;
+    unsigned long c;
+    while (true) {
+        c = _count;
+        data2mbedUSB();  // dummy() is called every TS3[s].
         if( (ms=(int)(TS3*1000-(_count-c)*TS0*1000))<=0 ){    ms=1;}
         Thread::wait(ms);
     }
 }
-
-void TS4timer(void const *argument) {   // make sampling time TS4 timer (priority lowest: precision 4ms)
+void timerTS4(void const *argument) {   // make sampling time TS4 timer (priority 4: precision 4ms)
     int ms;
     unsigned long c;
     while (true) {
@@ -357,10 +333,16 @@
     }
 }
 
-int main() {
-    Thread startTS3timer(TS3timer,NULL,osPriorityBelowNormal);
-    Thread startTS4timer(TS4timer,NULL,osPriorityLow);
-// Priority of Thread (RtosTimer has no priority?)
+void motor_control() {  // motor control ON for TMAX seconds.
+    FILE *fp;   // save data to PC
+    float   t=0;
+#ifdef  GOOD_DATA
+    int i;
+    RtosTimer RtosTimerTS1(th_controller);  // RtosTimer priority is osPriorityAboveNormal, just one above main()
+//BUG(unstable!!)    Thread startTimerTS2(timerTS2,NULL,osPriorityNormal);
+    Thread ThreadTimerTS3(timerTS3,NULL,osPriorityBelowNormal);
+    Thread ThreadTimerTS4(timerTS4,NULL,osPriorityLow);
+// Priority of Thread (RtosTimer is osPriorityAboveNormal)
 //  osPriorityIdle          = -3,          ///< priority: idle (lowest)--> then, mbed ERROR!!
 //  osPriorityLow           = -2,          ///< priority: low
 //  osPriorityBelowNormal   = -1,          ///< priority: below normal
@@ -369,6 +351,45 @@
 //  osPriorityHigh          = +2,          ///< priority: high 
 //  osPriorityRealtime      = +3,          ///< priority: realtime (highest)
 //  osPriorityError         =  0x84        ///< system cannot determine priority or thread has illegal priority
+
+    count3=0;
+    _count_data=0;
+#endif
+    init_controller(); // initialize controller parameters and signals
+    _r = _r + _th;
+//    if( _r>2*PI )    _r -= _r-2*PI;
+
+    if ( NULL == (fp = fopen( "/local/data.csv", "w" )) ){   error( "" );} // save data to PC
+
+    // start control (ON)
+    TickerTimerTS0.attach(&i_controller, TS0 ); // Sampling period[s] of i_controller
+    RtosTimerTS1.start((unsigned int)(TS1*1000.));   // Sampling period[ms] of th controller
+
+    t = _time;
+    while ( _time <= TMAX ) {
+        // BUG!! Dangerous if TS3<0.1 because multi interrupt by fprintf is not prohibited! 1st aug of fprintf will be destroyed.
+        //     fprintf returns before process completed.
+//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)
+#ifndef GOOD_DATA   // fprintf is dangerous because priority is higher than Ticker!
+        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)((TS3-(_time-t))*1000.));  //[ms]
+        t = _time;
+    }
+    // stop control (OFF)
+    TickerTimerTS0.detach(); // timer interrupt stop
+    RtosTimerTS1.stop();    // rtos timer stop
+//    ThreadTimerTS3.terminate();   // if remove comment, mbed hangs up! why?
+//    ThreadTimerTS4.terminate();   // if remove comment, mbed hangs up! why?
+
+    init_controller(); // initialize controller parameters and signals
+#ifdef  GOOD_DATA
+    for(i=0;i<_count_data;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
+    fclose( fp );               // release mbed USB drive
+}
+
+int main() {
 #if CONTROL_MODE==0     // PID control
     char    f;
     float   val;
@@ -382,12 +403,12 @@
 #if CONTROL_MODE==4                 // FFT identification, u=repetive signal
         max_u = 0;
         for( i=0;i<1000;i++ ){  // u=data[i][4]: memory for FFT identification input signal.
-            data[i][4] = sin(_freq_u*2*PI * i*TS2);   // _u_freq = 10/2 * i [Hz]
+            data[i][4] = sin(_freq_u*2*PI * i*TS1);   // _u_freq = 10/2 * i [Hz]
             if( data[i][4]>max_u ){   max_u=data[i][4];}
         }
         for( j=1;j<50;j++ ){
             for( i=0;i<1000;i++ ){
-                data[i][4] += sin((float)(j+1)*_freq_u*2*PI * i*TS2);
+                data[i][4] += sin((float)(j+1)*_freq_u*2*PI * i*TS1);
                 if( data[i][4]>max_u ){   max_u=data[i][4];}
             }
         }
@@ -397,7 +418,7 @@
         }
 #endif
         pc.printf("Control start!!\r\n");
-        moror_control();    // motor control ON for TMAX seconds.
+        motor_control();    // motor control ON for TMAX seconds.
         pc.printf("Control completed!!\r\n\r\n");
 
         // Change parameters using tera term