manabu kosaka
Skelton of EMG input method program using timer interrupt and thread.
Dependencies: QEI mbed-rtos mbed
Fork of
DCmotor
by 
manabu kosaka
Diff: main.cpp
- Revision:
- 8:0540582a220e
- Parent:
- 7:613febb8f028
- Child:
- 10:6be424b58abe
--- 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
-}