Revision 9:0540582a220e, committed 2012-11-24

Comitter:

kosaka

Date:

Sat Nov 24 01:10:18 2012 +0000

Parent:

7:613febb8f028

Commit message:

Skelton of EMG input method program using timer interrupt and thread.

Changed in this revision

--- a/main.cpp	Tue Nov 20 09:54:55 2012 +0000
+++ b/main.cpp	Sat Nov 24 01:10:18 2012 +0000
@@ -1,343 +1,104 @@
-//  DC motor control program using H-bridge driver (ex. TA7291P) and 360 resolution rotary encoder with A, B phase.
-//      ver. 121118a by Kosaka lab.
+//  Skelton of EMG input method program using timer interrupt and thread.
+//      ver. 121123 by Kosaka lab.
 #include "mbed.h"
 #include "rtos.h"
-#include "QEI.h"
 #define PI 3.14159265358979 // def. of PI
 /*********** 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 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.
-#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 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
-
-#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);
-//  QEI(PinName     channelA, mbed pin for channel A input.
-//      PinName     channelB, mbed pin for channel B input.
-//      PinName     index,    mbed pin for channel Z input. (index channel input Z phase th=0), (pass NC if not needed).
-//      int         pulsesPerRev, Number of pulses in one revolution(=360 deg).
-//      Encoding    encoding = X2_ENCODING, X2 is default. X2 uses interrupts on the rising and falling edges of only channel A where as 
-//                    X4 uses them on both channels.
-//  )
-//  void     reset (void)
-//     Reset the encoder. 
-//  int      getCurrentState (void)
-//     Read the state of the encoder. 
-//  int      getPulses (void)
-//     Read the number of pulses recorded by the encoder. 
-//  int      getRevolutions (void)
-//     Read the number of revolutions recorded by the encoder on the index channel. 
+AnalogIn emg(p20);      // *3.3 [V], Volt of emg from detection cirquit
+#define N_COUNT 5000    // keep N_COUNT data to identify japanese chracter.
+#define TS      0.0001  // [s], TS, sampling time[s] to detect emg from AD.
+#define TMAX    5       // [s], experiment starts from 0[s] to TMAX[s]
 /*********** User setting for control parameters (end) ***************/
 
-
 Serial pc(USBTX, USBRX);        // Display on tera term in PC 
 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 timer_interrupt;         // 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
-  DigitalOut  FIN(FIN_PORT);// FIN  for DC motor H bridge driver. FIN=1, RIN=0 then forward rotation
-  DigitalOut  RIN(RIN_PORT);// RIN  for DC motor H bridge driver. FIN=0, RIN=1 then reverse rotation
-#endif
-AnalogOut   analog_out(VREF_PORT);// Vref for DC motor H bridge driver. DA converter for control input [0.0-1.0]% in the output range of 0.0 to 3.3[V]
+//extern "C" void mbed_reset();   // if called, mbed is resset.
 
-unsigned long _count;   // sampling number
-float   _time;          // time[s]
-float   _r;             // reference signal
-float   _y;             // control output
-float   _e=0;           // e=r-y for PID controller
-float   _eI=0;          // integral of e for PID controller
-float   _u;             // control input[V]
-unsigned char _f_u_plus=1;// sign(u)
-unsigned char _f_umax=0;// flag showing u 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); // 
-#endif
-
-extern "C" void mbed_reset();
-
-void u2Hbridge(float u){// input u to H bridge driver
-    float           duty;
-    unsigned int    f_deadtime, f_in, r_in;
+float _emg_data[N_COUNT];// emg raw data
+unsigned long _count=0;  // sampling number for emg detection.
+unsigned long _count2=0; // = _count/N_COUNT
 
-    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].
-            f_deadtime = 1; // deadtime is required
-            _f_u_plus=1;
-        }else{
-            f_deadtime = 0; // deadtime is required
-        }
-        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].
-            f_deadtime = 1; // deadtime is required
-            _f_u_plus=0;
-        }else{
-            f_deadtime = 0; // deadtime is required
-        }
-        f_in=0;  r_in=1;    // set reverse direction
-    }else{// if( u == 0 ){  // stop mode
-        duty = 0;
-        f_deadtime = 0; // deadtime is required
-        f_in=0;  r_in=0;    // set FIN & RIN
-    }
+DigitalOut led1(LED1); // for debug
+DigitalOut led2(LED2); // for debug
 
-    if( f_deadtime==1 ){// making deadtime
-        FIN=0;  RIN=0;      // set upper&lower arm zero
-        wait(DEADTIME);
-    }
-#ifdef USE_PWM  // H bridge PWM mode: Vref=Vcc, FIN,2 = PWM or 0
-    FIN = duty*(float)f_in;    RIN = duty*(float)r_in;  // setting pwm FIN & RIN
-    analog_out = 1;          // setting Vref=UMAX, but Vref=Vcc is better.
-#else           //       Analog mode: Vref=analog, FIN, RIN = 1 or 0)
-    FIN = f_in;    RIN = r_in;  // setting FIN & RIN
-    analog_out = duty;          // setting Vref : PID write DA, range is 0-1. Output voltage 0-3.3v
-#endif
+float   _char=0;        //-------- make japanese character from emg
+FILE *fp;               // save data to PC
+unsigned char   _f_req_slow=0;      // flag requesting slow()
+unsigned char   _f_req_slowest=0;   // flag requesting slowest()
+
+
+void disp2PC(){     //-------- display japanese character to tera term on PC
+    pc.printf(" d %f\r\n",_char);
 }
 
-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
-    float   e_old, wt;
-    float   y, u;     // to avoid time shift
-
-    debug_p17 = 1;  // for debug: processing time check
-//    if(debug_p17 == 1)  debug_p17=0;else    debug_p17=1;  // for debug: sampling time check
+void discriminateEMG(){  //-------- discriminate EMG to make japanese character
+    int     i;
+    float   x;
 
-    _count+=1;
-//    y_old = _y;  // y_old=y(t-TS) is older than y by 1 sampling time TS[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
-#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
-    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;
-#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)
-    if( _f_umax==0 ){
-        _eI = _eI + TS*_e;     // integral of e(t)
+    x = 0;
+    for( i=0;i<N_COUNT;i++){
+        x = x + _emg_data[i];
     }
-    
-    u = _Kp*_e + _Kd*(_e-e_old)/TS + _Ki*_eI;   // PID output u(t)
-//debug[0]=_e;//minus
-//debug[0]=u;//minus
- 
-    // 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));
-    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;
-#endif
-#if CONTROL_MODE==3                 // u=rand() to identify motor transfer function G(s) from V to angle
-    if(count2==(int)(TS2/TS)){
-        u = ((float)rand()/RAND_MAX*2.0-1.0) * (UMAX-1.5)/2.0 + (UMAX+1.5)/2.0;
-    }else{
-        u = _u;
-    }
-#endif
-#if CONTROL_MODE==4                 // FFT identification, u=repetive signal
-    if(count2==(int)(TS2/TS)){
-        u = data[count3][4];
-    }else{
-        u = _u;
-    }
-#endif
-//debug[0]=u;//minus
-    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;
-#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( count3<1000 ){
-            data[count3][0]=_r; data[count3][1]=debug[0]; data[count3][2]=_y; data[count3][3]=_time; data[count3][4]=_u;
-            count3++;
-        }
-        count2 = 0;
-    }
-    count2++;
-#endif
-    //-------- update data
-
-    debug_p17 = 0;  // for debug: processing time check
+    _char = x;  // _char = emg_data[0] + emg_data[1] + emg_data[2] + ...
+    pc.printf(" s\r\n");
 }
 
-void main1() {
-    RtosTimer timer_controller(controller);
-    FILE *fp;   // save data to PC
-#ifdef  GOOD_DATA
-    int i;
 
-    count3=0;
-#endif
-    u2Hbridge(0);           // initialize H bridge to stop mode
-    _count=0;
-    _time = 0;  // time
-    _e = _eI = 0;
-    encoder.reset();    // set encoder counter zero
-    _y = (float)encoder.getPulses()/(float)N_ENC*2.0*PI;   // get angle [rad] from encoder
-    _r = _r + _y;
-//    if( _r>2*PI )    _r -= _r-2*PI;
-
-    pc.printf("Control start!!\r\n");
-    if ( NULL == (fp = fopen( "/local/data.csv", "w" )) ){   error( "" );} // save data to PC
-#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]
+//---------------- from here, timer interrupt and threads ---------------
 
-//    for ( i = 0; i < (unsigned int)(TMAX/TS2); 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.
-//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)
-#endif
-        Thread::wait((unsigned int)(TS2*1000.));  //[ms]
-    }
-    timer_controller.stop();    // rtos timer stop
-    u2Hbridge(0);           // initialize H bridge to stop mode
-#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
-    pc.printf("Control completed!!\r\n\r\n");
-}
-
-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
-        Thread::wait(200);
+void slowest(void const *argument) {    // thread priority: Low
+    while(true){
+        if( _f_req_slowest == 1 ){    // if slowest() is requested.
+//          function();
+            _f_req_slowest = 0;     // release to request slowest()
+        }
     }
 }
 
-void main2(void const *argument) {
-#if CONTROL_MODE==0     // PID control
-    char    f;
-    float   val;
-#endif
-#if CONTROL_MODE==4                 // FFT identification, u=repetive signal
-    int i, j;
-    float   max_u;
-#endif
-
+void slow(void const *argument) {    // thread priority: below normal
     while(true){
-#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]
-            if( data[i][4]>max_u ){   max_u=data[i][4];}
+        if( _f_req_slow == 1 ){    // if slow() is requested.
+            led2 = 1;   // check calculate time
+            discriminateEMG();  //-------- discriminate EMG to make japanese character
+            disp2PC();          //-------- display japanese character to tera term on PC
+            _f_req_slow = 0;    // release to request slow()
+            _f_req_slowest = 1; // request slowest()
+            led2 = 0;   // check calculate time
         }
-        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);
-                if( data[i][4]>max_u ){   max_u=data[i][4];}
-            }
-        }
-        for( i=0;i<1000;i++ ){
-//            data[i][4] *= UMAX/max_u;
-            data[i][4] = (data[i][4]/max_u+3)/4*UMAX;
-        }
-#endif
-        main1();
+    }
+}
 
-#if CONTROL_MODE>=1     // frequency response, or Step response
-        pc.printf("Input u(t) Frequency[Hz]? (if 9, reset mbed)...");
-        pc.scanf("%f",&_freq_u);
-        pc.printf("%8.3f[Hz]\r\n", _freq_u);  // print to tera term
-        if(_freq_u==9){    mbed_reset();}
-#else                   // PID control
-//  #ifdef R_SIN
-//        pc.printf("Reference signal r(t) Frequency[Hz]?...");
-//        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 ?");
-        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==4){    _freq_u = val;}
-        if(f==5){    _rmax = val/180.*PI;}
-        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);
-#endif
-    }    
+void fastest() {                    // ticker using TIMER3 interrupt
+    led1 = 1;   // check calculate time
+//    if( led1==0 ){  led1=1;}else{           led1=0;}// for debug
+    _emg_data[_count] = emg;
+    _count = _count + 1;
+    if( _count==N_COUNT ){
+        _count = 0;
+        _count2 += 1;
+        _f_req_slow = 1;    // request slow()
+    }
+    led1 = 0;   // check calculate time
 }
+
 int main() {
-//    void main1();
-    Thread save2PC(main2,NULL,osPriorityBelowNormal);
-    Thread print2PC(thread_print2PC,NULL,osPriorityLow);
+    Thread threadSlow(slow,NULL,osPriorityBelowNormal); // call thread slow()
+    Thread threadSlowest(slowest,NULL,osPriorityLow);   // call thread slowest()
 
+    pc.printf("Start!!\r\n");
+//    if ( NULL == (fp = fopen( "/local/data.csv", "w" )) ){   error( "" );} // open mbed USB drive
+    timer_interrupt.attach(&fastest, TS );  // start timer interrupt: call fastest() on each TS[s].
+    while( _count2 < TMAX/TS/N_COUNT ){
+        Thread::wait(1000);  // [ms], wait
+    }
+    timer_interrupt.detach();   // stop timer interrupt fastest
+//    fclose( fp );               // release mbed USB drive
+    pc.printf("Completed!!\r\n\r\n");
+}
 //    osStatus set_priority(osPriority osPriorityBelowNormal );
 // Priority of Thread (RtosTimer has no priority?)
 //  osPriorityIdle          = -3,          ///< priority: idle (lowest)--> then, mbed ERROR!!
@@ -348,4 +109,3 @@
 //  osPriorityHigh          = +2,          ///< priority: high 
 //  osPriorityRealtime      = +3,          ///< priority: realtime (highest)
 //  osPriorityError         =  0x84        ///< system cannot determine priority or thread has illegal priority
-}