main.cpp

00001 #include "mbed.h"
00002 #include "HIDScope.h"
00003 #include "MODSERIAL.h"
00004 
00005 #define SAMPLING_RATE 0.002
00006 
00007 MODSERIAL pc(USBTX,USBRX); 
00008 AnalogIn emg1(A0); // onderste board
00009 AnalogIn emg2(A1); // 2e board
00010 AnalogIn emg3(A2); // 3e board
00011 AnalogIn emg4(A3); // bovenste board
00012 
00013 Ticker sample_timer; // naam van de emg-ticker
00014 DigitalOut red(LED_RED);
00015 DigitalOut green(LED_GREEN);
00016 DigitalOut blue(LED_BLUE);
00017 
00018 int const window=150;                      //30 samples
00019 int i=0;                        //buffer index
00020 double bicepsbuffer [window];
00021 
00022 //HIDScope scope(4);  // aantal kanalen voor je HIDScope
00023 
00024 double avg1=0;
00025 int muscle;
00026 double tijd;
00027 double y5;
00028 
00029 
00030 //highpass filter 20 Hz
00031 const double numhigh_1 = 0.956543225556877;
00032 const double numhigh_2 = -1.91308645113754;
00033 const double numhigh_3 = 0.956543225556877;
00034 const double denhigh_2 = -1.91197067426073;
00035 const double denhigh_3 = 0.9149758348014341;
00036 
00037 //biquad 1
00038 const double notch1gain = 1.000000;
00039 const double notch1_b0 = 1;
00040 const double notch1_b1 = -1.61816176147 * notch1gain;
00041 const double notch1_b2 = 1.00000000000 * notch1gain;
00042 const double notch1_a1 = -1.58071559235 * notch1gain;
00043 const double notch1_a2 = 0.97319685401 * notch1gain;
00044  
00045 //biquad 2
00046 const double notch2gain = 0.973674;
00047 const double notch2_b0 = 1 * notch2gain;
00048 const double notch2_b1 = -1.61816176147 * notch2gain;
00049 const double notch2_b2 = 1.00000000000 * notch2gain;
00050 const double notch2_a1 = -1.61244708381 * notch2gain;
00051 const double notch2_a2 = 0.97415116257 * notch2gain;
00052 
00053 //lowpass filter 7 Hz  - envelop
00054 const double numlow_1 = 0.000119046743110057;
00055 const double numlow_2 = 0.000238093486220118;
00056 const double numlow_3 = 0.000119046743110057;
00057 const double denlow_2 = -1.968902268531908;
00058 const double denlow_3 = 0.9693784555043481;
00059 
00060 //EMG variables
00061 double emg_biceps; double biceps_power; double bicepsMVC = 0;
00062 double emg_triceps; double triceps_power; double tricepsMVC = 0;
00063 double emg_flexor; double flexor_power; double flexorMVC = 0;
00064 double emg_extens; double extens_power; double extensMVC = 0;
00065 
00066 
00067 // storage variables definieren
00068 double f1_v1 = 0, f1_v2 = 0, f2_v1 = 0, f2_v2 = 0, f3_v1 = 0, f3_v2 = 0, f4_v1 = 0, f4_v2 = 0;
00069 
00070 double biquad( double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)
00071 
00072 {
00073     double v = u  - a1*v1 - a2*v2;
00074     double y = b0*v + b1*v1 + b2*v2;
00075     v2=v1;
00076     v1=v;
00077     return y;
00078     }
00079 
00080         
00081         
00082 void sample_filter()
00083 {
00084     
00085 // double u1=..., u2...;
00086 double u1 = emg1.read();
00087 double y1 = biquad(u1, f1_v1, f1_v2, denhigh_2, denhigh_3, numhigh_1, numhigh_2, numhigh_3);
00088 double y2 = biquad(y1, f2_v1, f2_v2, notch1_a1, notch1_a2, notch1_b0, notch1_b1, notch1_b2);
00089 double y3 = biquad(y2, f3_v1, f3_v2, notch2_a1, notch2_a2, notch2_b0, notch2_b1, notch2_b2);
00090 double y4 = abs(y3);
00091 double y5 = biquad(y4, f4_v1, f4_v2, denlow_2, denlow_3, numlow_1, numlow_2, numlow_3);
00092 
00093     biceps_power=y5;
00094     //scope.set(0,u1);
00095     //scope.set(1,y4);
00096     //scope.set(2,biceps_power);
00097     //scope.send();
00098     
00099 }
00100 
00101 
00102 //Start sampling
00103 void start_sampling(void)
00104 {
00105     sample_timer.attach(&sample_filter,0.002);   //500 Hz EMG 
00106     blue=0; green=0;
00107     pc.printf("||- started sampling -|| \r\n");
00108 }
00109 
00110 //stop sampling
00111 void stop_sampling(void)
00112 {
00113     sample_timer.detach();
00114     blue=1; green=1;
00115     pc.printf("||- stopped sampling -|| \r\n");
00116 }
00117      
00118 void calibrate_emg()
00119 {  
00120     //double sampletime=0;
00121     //sampletime=+SAMPLE_RATE;
00122     //
00123     // if(sampletime<5)
00124     //int muscle=1;
00125     //for(int index=0; index<2500;index++){   //measure 5 seconds@500hz = 2500 samples
00126      
00127         if(muscle==1){
00128             
00129             if(biceps_power>bicepsMVC){
00130             printf("+ ");
00131             bicepsMVC=biceps_power;
00132             }    
00133             else
00134             printf("- ");
00135         }  
00136         
00137         if(muscle==2){
00138             
00139             if(triceps_power>tricepsMVC){
00140             tricepsMVC=triceps_power;
00141             }    
00142         }
00143         
00144         if(muscle==3){
00145             
00146             if(flexor_power>flexorMVC){
00147             flexorMVC=flexor_power;
00148             }    
00149         }
00150         
00151         if(muscle==4){
00152             
00153             if(extens_power>extensMVC){
00154             extensMVC=extens_power;
00155             }    
00156         }
00157         
00158     //}
00159     tijd = tijd + 0.002;
00160     
00161     
00162    
00163 }
00164 
00165 int main()
00166 {
00167    pc.baud(115200);
00168    red=1; green=1; blue=1;
00169    Ticker timer;
00170    
00171    pc.printf("|-- Robot Started --| \r\n");
00172    pc.printf("Testcode calibration \r\n");
00173    wait(1);
00174    pc.printf("+ means current sample is higher than stored MVC\r\n");
00175    pc.printf("- means current sample is lower than stored MVC\r\n");
00176    wait(3);
00177    pc.printf(" Biceps is first. "); wait(1);
00178    pc.printf(" Press any key to begin... "); wait(1);
00179    char input;
00180    input=pc.getc();
00181    pc.putc(input);
00182    pc.printf(" \r\n  Starting in 3... \r\n"); wait(1);
00183    pc.printf(" \r\n  Starting in 2... \r\n"); wait(1);
00184    pc.printf(" \r\n  Starting in 1... \r\n"); wait(1);
00185    
00186    start_sampling();
00187    muscle=1;
00188    timer.attach(&calibrate_emg,0.002);
00189    wait(5);
00190    timer.detach();
00191    pc.printf("\r\n MVC = %f \r\n",bicepsMVC);
00192    
00193    pc.printf("Calibrate_emg() exited \r\n");
00194    pc.printf("Measured time: %f seconds \r\n",tijd);
00195    tijd=0;
00196    
00197    // Triceps:
00198    pc.printf(" Triceps is next "); wait(1);
00199    pc.printf(" Press any key to begin... "); wait(1);
00200    input=pc.getc();
00201    pc.putc(input);
00202    pc.printf(" \r\n  Starting in 3... \r\n"); wait(1);
00203    pc.printf(" \r\n  Starting in 2... \r\n"); wait(1);
00204    pc.printf(" \r\n  Starting in 1... \r\n"); wait(1);
00205    start_sampling();
00206    muscle=1;
00207    timer.attach(&calibrate_emg,0.002);
00208    wait(5);
00209    timer.detach();
00210    pc.printf("\r\n Triceps MVC = %f \r\n",tricepsMVC);
00211    
00212    pc.printf("Calibrate_emg() exited \r\n");
00213    pc.printf("Measured time: %f seconds \r\n",tijd);
00214    tijd=0;
00215    
00216    //Flexor:
00217    
00218    //Extensor:
00219    
00220    stop_sampling();
00221    while (true){   
00222         
00223         wait(1);
00224     }
00225 }