change at hidscope

Dependencies:   HIDScope MODSERIAL biquadFilter mbed

Fork of a_check_emg by Daniqe Kottelenberg

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 //libraries
00002 #include "mbed.h"
00003 #include "HIDScope.h"
00004 #include "BiQuad.h"  
00005 #include "MODSERIAL.h"
00006 
00007 //Define objects
00008 AnalogIn    emg_biceps_right_in( A0 );              //analog in to get EMG biceps  (r) in to c++
00009 AnalogIn    emg_triceps_right_in(A1);               //analog in to get EMG triceps (r) in to c++
00010 AnalogIn    emg_biceps_left_in  (A2);               //analog in to get EMG biceps  (l) in to c++
00011 Ticker      sample_timer;           //ticker
00012 Ticker      switch_function;         //ticker
00013 HIDScope    scope(5);               //open 3 channels in hidscope
00014 MODSERIAL pc(USBTX, USBRX);            //pc connection
00015 
00016 //motors
00017 DigitalOut richting_motor1(D4);
00018 PwmOut pwm_motor1(D5);
00019 DigitalOut richting_motor2(D7);
00020 PwmOut pwm_motor2(D6);
00021 DigitalOut  led(LED_GREEN);         //led included to check where code is
00022 
00023 //define variables
00024 //other
00025 int    onoffsignal_rightarm=0;              // on/off signal: 1; biceps activation, 0: nothing, -1, triceps activation
00026 int    switch_signal_leftarm=0;             // switching between motors. 
00027 double cut_off_value_biceps =0.06;              //gespecificeerd door floortje
00028 double cut_off_value_triceps=-0.03;             //gespecificeerd door floortje
00029 double signal_right_arm;
00030 int n = 0; //start van de teller wordt op nul gesteld
00031  
00032 //biceps  arm 1, right arm
00033 double emg_biceps_right;
00034 double emg_filtered_high_biceps_right;
00035 double emg_abs_biceps_right;
00036 double emg_filtered_biceps_right;
00037 double emg_filtered_high_notch_1_biceps_right;
00038 double emg_filtered_high_notch_1_2_biceps_right;
00039 
00040 //triceps arm 1, right arm
00041 double emg_triceps_right;
00042 double emg_filtered_high_triceps_right;
00043 double emg_abs_triceps_right;
00044 double emg_filtered_triceps_right;
00045 double emg_filtered_high_notch_1_triceps_right;
00046 double emg_filtered_high_notch_1_2_triceps_right;
00047 
00048 //biceps  arm 1, left arm
00049 double emg_biceps_left;
00050 double emg_filtered_high_biceps_left;
00051 double emg_abs_biceps_left;
00052 double emg_filtered_biceps_left;
00053 double emg_filtered_high_notch_1_biceps_left;
00054 double emg_filtered_high_notch_1_2_biceps_left;
00055 
00056 //before abs filtering
00057 
00058 //b1 = biceps right arm
00059 BiQuad filterhigh_b1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);
00060 BiQuad filternotch1_b1 (9.9115e-01, -1.8853e+00, 9.9115e-01 ,-1.8909e+00  , 9.9103e-01);
00061 BiQuad filternotch2_b1(1.0000e+00, -1.9022e+00, 1.0000e+00,  -1.8965e+00 , 9.9127e-01);
00062 
00063 //t1= triceps right arm
00064 BiQuad filterhigh_t1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);
00065 BiQuad filternotch1_t1 (9.9115e-01, -1.8853e+00, 9.9115e-01 ,-1.8909e+00  , 9.9103e-01);
00066 BiQuad filternotch2_t1(1.0000e+00, -1.9022e+00, 1.0000e+00,  -1.8965e+00 , 9.9127e-01);
00067 
00068 //b2= biceps left arm
00069 BiQuad filterhigh_b2(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);
00070 BiQuad filternotch1_b2 (9.9115e-01, -1.8853e+00, 9.9115e-01 ,-1.8909e+00  , 9.9103e-01);
00071 BiQuad filternotch2_b2(1.0000e+00, -1.9022e+00, 1.0000e+00,  -1.8965e+00 , 9.9127e-01);
00072 
00073 //after abs filtering
00074 BiQuad filterlow_b1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);
00075 BiQuad filterlow_t1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);
00076 BiQuad filterlow_b2 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);
00077 
00078 //function teller
00079 void SwitchN() {                        // maakt simpele functie die 1 bij n optelt
00080     if(switch_signal_leftarm==1)
00081     {
00082         n++;
00083         wait(0.5f);
00084     }    
00085     }
00086     
00087 //functions which are called in ticker to sample the analog signal
00088 
00089 void filter(){
00090         //biceps right arm read+filtering
00091         emg_biceps_right=emg_biceps_right_in.read();                            //read the emg value from the elektrodes
00092         emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right);
00093         emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right);
00094         emg_filtered_high_notch_1_2_biceps_right=filternotch2_b1.step(emg_filtered_high_notch_1_biceps_right);
00095         emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_2_biceps_right); //fabs because float
00096         emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right);
00097         
00098         //triceps right arm read+filtering
00099         emg_triceps_right=emg_triceps_right_in.read();                            //read the emg value from the elektrodes
00100         emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right);
00101         emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right);
00102         emg_filtered_high_notch_1_2_triceps_right=filternotch2_t1.step(emg_filtered_high_notch_1_triceps_right);
00103         emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_2_triceps_right); //fabs because float
00104         emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right);
00105         
00106         //biceps left arm read+filtering
00107         emg_biceps_left=emg_biceps_left_in.read();                            //read the emg value from the elektrodes
00108         emg_filtered_high_biceps_left= filterhigh_b2.step(emg_biceps_left);
00109         emg_filtered_high_notch_1_biceps_left=filternotch1_b2.step(emg_filtered_high_biceps_left);
00110         emg_filtered_high_notch_1_2_biceps_left=filternotch2_b2.step(emg_filtered_high_notch_1_biceps_left);
00111         emg_abs_biceps_left=fabs(emg_filtered_high_notch_1_2_biceps_left); //fabs because float
00112         emg_filtered_biceps_left=filterlow_b2.step(emg_abs_biceps_left);
00113           
00114         //signal substraction of filter biceps and triceps. Biceps +,triceps -
00115         signal_right_arm=emg_filtered_biceps_right-emg_filtered_triceps_right;
00116                
00117         //creating of on/off signal with the created on/off signals, with if statement   for right arm!    
00118         if (signal_right_arm>cut_off_value_biceps)
00119         {onoffsignal_rightarm=1;}
00120           
00121         else if (signal_right_arm<cut_off_value_triceps)
00122         {
00123         onoffsignal_rightarm=-1;
00124         }    
00125         
00126         else
00127         {onoffsignal_rightarm=0;}
00128                       
00129         //creating on/off signal for switch (left arm)
00130         
00131         if (emg_filtered_biceps_left>cut_off_value_biceps)
00132         {
00133         switch_signal_leftarm=1;    
00134         }    
00135         
00136         else
00137         {
00138         switch_signal_leftarm=0;              
00139         }
00140         
00141         //send signals  to scope
00142         scope.set(0, emg_filtered_biceps_right);            //set emg signal to scope in channel 0 // change into raw signal! 
00143         scope.set(1, emg_filtered_triceps_right);           // set emg signal to scope in channel 1// change into raw signal!
00144         scope.set(2, emg_filtered_biceps_left);             // set emg signal to scope in channel 2
00145         scope.set(3, onoffsignal_rightarm);                 // set emg signal to scope in channel 3
00146         scope.set(4, switch_signal_leftarm);
00147         
00148         scope.send();                       //send all the signals to the scope
00149                 }
00150 
00151 //program
00152 
00153 int main()
00154 {  
00155 pc.baud(115200);
00156 sample_timer.attach(&filter, 0.001);        //continously execute the EMG reader and filter, it ensures that filter and sampling is executed every 1/frequency seconds
00157 switch_function.attach(&SwitchN,1);
00158 //endless loop
00159 
00160     while (true) {                        // zorgt er voor dat de code oneindig doorgelopen wordt  
00161        
00162     if (onoffsignal_rightarm==1)                           // als s ingedrukt wordt gebeurd het volgende
00163     {
00164          if (n%2==0)                     // als s ingedrukt wordt en het getal is even gebeurd het onderstaande
00165          {
00166            pc.printf("n is even \n\r"); // print lijn "n is even"
00167            pc.printf("up \n\r");        // print lijn "up"   
00168            richting_motor1 = 2.5;
00169            pwm_motor1 = 1; 
00170                   
00171          } 
00172          
00173          else                           // als s is ingedrukt maar het getal is niet even (dus oneven) gebeurdt het onderstaande
00174          {
00175            pc.printf("n is odd \n\r");  // print lijn "n is odd"
00176            pc.printf("left \n\r");      // print lijn "left"
00177            richting_motor2 = 2.5;
00178            pwm_motor2 = 1;
00179            
00180          }      
00181               
00182     }
00183     else if (onoffsignal_rightarm==-1)                     // als d ingedrukt wordt gebeurd het volgende
00184     {
00185          if (n%2==0)                     // als d is ingedrukt en n is even dan gebeurd het volgende
00186          {
00187            pc.printf("n is even \n\r"); // print lijn "n is even"
00188            pc.printf("down \n\r");      // print lijn "down"   
00189            richting_motor1 = 0;
00190            pwm_motor1 = 1;
00191            
00192         } 
00193          else                           // als d is ingedrukt maar het getal is niet even (dus oneven) gebeurdt het onderstaande
00194          {
00195            pc.printf("n is odd \n\r"); // print lijn "n is odd"
00196            pc.printf("right \n\r");    // print lijn "right"
00197            richting_motor2 = 0;
00198            pwm_motor2 = 1;
00199                 
00200          }  
00201     }   
00202     else{
00203        pc.printf("motor staat stil \n\r");
00204     pwm_motor2=0;
00205     pwm_motor1=0;
00206        }              
00207                
00208 }
00209         
00210 }