main.cpp

00001 #include "mbed.h"
00002 #include "arm_math.h" 
00003 
00004 #define TRUE        1
00005 #define FALSE       0
00006 #define RAW         0
00007 #define RECTIFIED   1
00008 #define SMOOTH      2        
00009 #define THRESHOLDF  0.25f
00010 #define THRESHOLDE  0.25f
00011 #define FLEXION2    0.0f
00012 
00013 Ticker EMG_Sampler;
00014 //Serial pc(SERIAL_TX, SERIAL_RX);
00015 Serial pc(USBTX, USBRX);
00016 DigitalOut myled(LED1);
00017 InterruptIn sw2(SW2);
00018 AnalogIn   Ref(A0);
00019 AnalogIn   E1(A1);
00020 AnalogIn   E2(A2);
00021 AnalogIn   E3(A3);
00022 PwmOut Thumb(D11);
00023 PwmOut Index(D10);
00024 PwmOut Middle(D9);
00025 PwmOut Pinky(D6);
00026 PwmOut ThumbRot(D5);
00027  
00028 //EMG samples and DSP variables
00029 float32_t EMG1, EMG2, EMG3;
00030 float32_t samples[25];
00031 float32_t samples2[25];
00032 float32_t samples3[25];
00033 float32_t abs_output[25];
00034 float32_t abs_output2[25];
00035 float32_t abs_output3[25];
00036 float32_t mean = 0.0f;
00037 float32_t mean2 = 0.0f; 
00038 float32_t mean3 = 0.0f;
00039 
00040 //variables for state machines
00041 uint8_t state = 0;
00042 uint8_t action = '0';
00043 
00044 //conversion and data collection complete flag
00045 uint8_t COCO = 0;
00046 
00047 //functions declaration
00048 void PWM_Init(void);
00049 void ADC_Sampler(void);
00050 void RX_Interrupt(void);
00051 void Serial_Oscilloscope(uint8_t activation, uint8_t type);
00052 void FingerPosition(float32_t thumb_us, float32_t index_us, float32_t middle_us, float32_t pinky_us, float32_t thumbrot_us);
00053 
00054 void flip() {
00055     if(action<='5') action++;
00056     else action = '1';
00057 } 
00058 
00059 int main() {
00060   pc.baud(115200);                                  //Serial com at 115200 bauds
00061   pc.attach(&RX_Interrupt);                         //Serial RX interrupt attachment
00062   sw2.rise(&flip); 
00063   EMG_Sampler.attach(&ADC_Sampler, 0.001);          //1 ms ticker for ADC Sampler
00064   PWM_Init();                                       //Servo initialization
00065   myled = 1;
00066   
00067   while(1) { 
00068     if(COCO){
00069         arm_abs_f32(samples, abs_output, 25);       //rectifier EMG1
00070         arm_abs_f32(samples2, abs_output2, 25);     //rectifier EMG2
00071         arm_abs_f32(samples3, abs_output3, 25);     //rectifier EMG3
00072         arm_mean_f32(abs_output, 25, &mean);        //mean EMG1
00073         arm_mean_f32(abs_output2, 25, &mean2);      //mean EMG2 
00074         arm_mean_f32(abs_output3, 25, &mean3);      //mean EMG3
00075         Serial_Oscilloscope(TRUE,SMOOTH);
00076         switch(state){   
00077             case 0: {
00078                 if (mean>THRESHOLDF){
00079                     myled = 0;
00080                     state = 1;
00081                     switch(action){
00082                         case '1': FingerPosition(2400, 600, 600,2400,2400); break;  //Close
00083                         case '2': FingerPosition(2400,2400, 600,2400,2400); break;  //Point
00084                         case '3': FingerPosition(2400, 600,2400, 600,2400); break;  //Pinch
00085                         case '4': FingerPosition(2400, 600, 600,2400, 600); break;  //Lateral
00086                         case '5': FingerPosition(2400, 600, 600, 600,2400); break;  //Tripod
00087                         default:  FingerPosition(2400, 600, 600,2400,2400);         //Close
00088                     }
00089                 }    
00090             } break;
00091             case 1: {
00092                 if (mean2>THRESHOLDE){
00093                     myled = 1;
00094                     state = 0;
00095                     FingerPosition(1000,2400,2400, 600, 2400);                       //Open
00096                 }       
00097             }
00098         }
00099         COCO = 0;   
00100     }   
00101   }
00102 }
00103 
00104 //EMG sampler Ts = 1ms
00105 void ADC_Sampler() {
00106     EMG1 = (E1.read()-Ref.read())*3.3f;
00107     EMG2 = (E2.read()-Ref.read())*3.3f;
00108     EMG3 = (E3.read()-Ref.read())*3.3f;
00109     Serial_Oscilloscope(FALSE,RAW);
00110     uint32_t m = __get_PRIMASK();
00111     __disable_irq();        
00112     for(int j=24;j>0;j--) {
00113         samples[j]=samples[j-1];                    //Fill Array
00114         samples2[j]=samples2[j-1];                  //Fill Array
00115         samples3[j]=samples3[j-1];                  //Fill Array
00116     }
00117     samples[0]=EMG1;
00118     samples2[0]=EMG2;
00119     samples3[0]=EMG3;       
00120     __set_PRIMASK(m);
00121     COCO = 1;
00122 }
00123 
00124 //action selection trough serial console
00125 void RX_Interrupt(void){
00126     action = pc.getc();
00127     /*switch(action){
00128         case '1': pc.printf("Power Grip\n"); break;
00129         case '2': pc.printf("Point\n"); break;
00130         case '3': pc.printf("Pinch\n"); break;
00131         case '4': pc.printf("Lateral\n"); break;
00132         case '5': pc.printf("Tripod\n"); break;
00133         default:  pc.printf("Power Grip\n");
00134     }*/
00135 }
00136 
00137 //PWM initialization for servos
00138 void PWM_Init(void){
00139     //Open -> 0.6ms - Close 2.4ms
00140     Thumb.period(0.02f);
00141     Thumb.pulsewidth(0.0010f);
00142     //Open -> 2.4ms - Close 0.6ms
00143     //Index.period(0.02f);
00144     Index.pulsewidth(0.0024f);
00145     //Open -> 2.4ms - Close 0.6ms
00146     Middle.period(0.02f);
00147     Middle.pulsewidth(0.0024f);
00148     //Open -> 0.6ms - Close 2.4ms
00149     //Pinky.period(0.02f);
00150     Pinky.pulsewidth(0.0006f);
00151     //Open -> 0.6ms - Close 2.4ms
00152     ThumbRot.pulsewidth(0.0006f);  
00153 }
00154 
00155 //send data through serail port for oscilloscope visualization
00156 void Serial_Oscilloscope(uint8_t activation, uint8_t type){
00157     if (activation){
00158         switch(type){
00159             case RAW: pc.printf("%.10f,%.10f\n\r",EMG1,EMG2); break; 
00160             case RECTIFIED: pc.printf("%.10f,%.10f\n\r",abs_output[0],abs_output2[0]);break;
00161             case SMOOTH: pc.printf("%.10f,%.10f\n\r",mean,mean2); break;
00162             default: pc.printf("%.10f,%.10f\n\r",EMG1,EMG2); 
00163         } 
00164     }    
00165 }
00166 
00167 //finger position through servos - values on microseconds (600 us - 2400 us)
00168 void FingerPosition(float32_t thumb_us, float32_t index_us, float32_t middle_us, float32_t pinky_us, float32_t thumbrot_us){
00169     Thumb.pulsewidth(thumb_us/1000000.0f);
00170     Index.pulsewidth(index_us/1000000.0f);
00171     Middle.pulsewidth(middle_us/1000000.0f);
00172     Pinky.pulsewidth(pinky_us/1000000.0f);
00173     ThumbRot.pulsewidth(thumbrot_us/1000000.0f);
00174 }