Diff: main.cpp

Revision:

1:ecd6dc3c839b

Parent:

0:c9891e1c62f0

Child:

2:f6060b484caf

--- a/main.cpp	Tue Apr 16 11:03:47 2019 +0000
+++ b/main.cpp	Tue Apr 16 11:32:24 2019 +0000
@@ -10,54 +10,62 @@
 DigitalOut led3(LED_BLUE);
 MODSERIAL pc(USBTX, USBRX);
 
-//MovingAverage
-MovingAverage <float>Movag_1(NSAMPLE,0.0); //Make Moving Average, Define NSAMPLE above
-MovingAverage <float>Movag_2(NSAMPLE,0.0); //Make Moving Average, Define NSAMPLE above
-MovingAverage <float>Movag_3(NSAMPLE,0.0); //Make Moving Average, Define NSAMPLE above
-MovingAverage <float>Movag_4(NSAMPLE,0.0); //Make Moving Average, Define NSAMPLE above
-
-//EMG tickers, these tickers are called in the mainscript with fsample 500Hz, also sends to HIDscope with same fsample
-Ticker sample_ticker; //ticker for filtering pref. with 1000Hz, define in tick.attach
-Ticker threshold_check_ticker;
-Timer t; //timer try out for Astrid
-Timer timer_calibration; //timer for EMG calibration
+// Tickers
+Ticker sample_ticker; //ticker voor filteren met 1000Hz
+Ticker threshold_check_ticker; //ticker voor het checken van de threshold met 1000Hz
+Timer timer_calibration; //timer voor EMG Kalibratie
+double ts = 0.001; //tijdsstap !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+double calibration_time = 37; //Kalibratie tijd
 
 //Input
-AnalogIn    emg1( A1 );
-AnalogIn    emg2( A2 );
-AnalogIn    emg3( A3 );
-AnalogIn    emg4( A4 );
+AnalogIn    emg1( A1 ); //Duim
+AnalogIn    emg2( A2 ); //Bicep
+AnalogIn    emg3( A3 ); //Dorsaal
+AnalogIn    emg4( A4 ); //Palmair
 
 // GLOBALS EMG
-//Filtered EMG signals from the end of the chains
+//Gefilterde EMG signalen
 volatile double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered;
-int i = 0;
 
-//Define doubles for calibration and ticker
-double ts = 0.001; //tijdsstap
-double calibration_time = 55; //time EMG calibration should take
+bool thresholdreach1 = false;
+bool thresholdreach2 = false;
+bool thresholdreach3 = false;
+bool thresholdreach4 = false;
 
-volatile double temp_highest_emg1 = 0; //highest detected value right biceps
+volatile double temp_highest_emg1 = 0; //Hoogste waarde gevonden tijdens kalibratie
 volatile double temp_highest_emg2 = 0;
 volatile double temp_highest_emg3 = 0;
 volatile double temp_highest_emg4 = 0;
 
-//Doubles for calculation threshold
-double duim_p_t = 0.4; //set threshold at percentage of highest value
-double bicep_p_t = 0.4; //
-double tricep_p_t = 0.4; //set threshold at percentage of highest value
-double onderarm_p_t = 0.4;
+//Percentage van de hoogste waarde waar de bovenste treshold gezet moet worden
+double Duim_p_t = 0.5; 
+double Bicep_p_t = 0.4; 
+double Dorsaal_p_t = 0.6; 
+double Palmair_p_t = 0.5;
 
+//Percentage van de hoogste waarde waar de onderste treshold gezet moet worden
+double Duim_p_tL = 0.5; 
+double Bicep_p_tL = 0.4; 
+double Dorsaal_p_tL = 0.5; 
+double Palmair_p_tL = 0.5;
+
+// Waarde bovenste treshold waar het signaal overheen moet om de arm te activeren
 volatile double threshold1;
 volatile double threshold2;
 volatile double threshold3;
 volatile double threshold4;
 
+// Waarde onderste treshold waar het signaal onder moet om de arm te deactiveren
+volatile double threshold1L;
+volatile double threshold2L;
+volatile double threshold3L;
+volatile double threshold4L;
+
 // thresholdreads bools
 int Duim;
 int Bicep;
-int Tricep;
-int Onderarm;
+int Dorsaal;
+int Palmair;
 
 //  EMG OUTPUT
 int EMGxplus;
@@ -67,137 +75,97 @@
 
 
 //EMG1!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
-//Highpass 
+//Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
 BiQuadChain highp1;
-BiQuad highp1_1( 9.21171e-01, -1.84234e+00, 9.21171e-01, -1.88661e+00, 8.90340e-01 );
-BiQuad highp1_2( 1.00000e+00, -2.00000e+00, 1.00000e+00, -1.94922e+00, 9.53070e-01 );
+BiQuad highp1_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
+BiQuad highp1_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 );
+BiQuad notch1_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 );
 
-//Notch
-BiQuadChain notch1;
-BiQuad notch1_1( 9.56543e-01, -1.82035e+00, 9.56543e-01, -1.84459e+00, 9.53626e-01 );
-BiQuad notch1_2( 1.00000e+00, -1.90305e+00, 1.00000e+00, -1.87702e+00, 9.59471e-01 );
-
-//Lowpass 4th order cutoff 3Hz
-BiQuadChain lowp1;
-BiQuad lowp1_1( 7.69910e-09, 1.53982e-08, 7.69910e-09, -1.96542e+00, 9.65769e-01 );
-BiQuad lowp1_2( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.98532e+00, 9.85677e-01 );
+//Lowpass first order cutoff 0.4Hz
+BiQuad lowp1( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );
 
 //EMG2!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
-//Highpass
+//Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
 BiQuadChain highp2;
-BiQuad highp2_1( 9.21171e-01, -1.84234e+00, 9.21171e-01, -1.88661e+00, 8.90340e-01 );
-BiQuad highp2_2( 1.00000e+00, -2.00000e+00, 1.00000e+00, -1.94922e+00, 9.53070e-01 );
+BiQuad highp2_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
+BiQuad highp2_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 );
+BiQuad notch2_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 );
 
-//Notch
-BiQuadChain notch2;
-BiQuad notch2_1( 9.56543e-01, -1.82035e+00, 9.56543e-01, -1.84459e+00, 9.53626e-01 );
-BiQuad notch2_2( 1.00000e+00, -1.90305e+00, 1.00000e+00, -1.87702e+00, 9.59471e-01 );
-
-//Lowpass 4th order cutoff 3Hz
-BiQuadChain lowp2;
-BiQuad lowp2_1( 7.69910e-09, 1.53982e-08, 7.69910e-09, -1.96542e+00, 9.65769e-01 );
-BiQuad lowp2_2( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.98532e+00, 9.85677e-01 );
+//Lowpass first order cutoff 0.4Hz
+BiQuad lowp2( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );
 
 //EMG3!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
-//Highpass
+//Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
 BiQuadChain highp3;
-BiQuad highp3_1( 9.21171e-01, -1.84234e+00, 9.21171e-01, -1.88661e+00, 8.90340e-01 );
-BiQuad highp3_2( 1.00000e+00, -2.00000e+00, 1.00000e+00, -1.94922e+00, 9.53070e-01 );
+BiQuad highp3_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
+BiQuad highp3_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 );
+BiQuad notch3_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 );
 
-//Notch
-BiQuadChain notch3;
-BiQuad notch3_1( 9.56543e-01, -1.82035e+00, 9.56543e-01, -1.84459e+00, 9.53626e-01 );
-BiQuad notch3_2( 1.00000e+00, -1.90305e+00, 1.00000e+00, -1.87702e+00, 9.59471e-01 );
-
-//Lowpass 4th order cutoff 3Hz
-BiQuadChain lowp3;
-BiQuad lowp3_1( 7.69910e-09, 1.53982e-08, 7.69910e-09, -1.96542e+00, 9.65769e-01 );
-BiQuad lowp3_2( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.98532e+00, 9.85677e-01 );
+//Lowpass first order cutoff 0.4Hz
+BiQuad lowp3( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );
 
 //EMG4!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
-//Highpass
+//Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
 BiQuadChain highp4;
-BiQuad highp4_1( 9.21171e-01, -1.84234e+00, 9.21171e-01, -1.88661e+00, 8.90340e-01 );
-BiQuad highp4_2( 1.00000e+00, -2.00000e+00, 1.00000e+00, -1.94922e+00, 9.53070e-01 );
+BiQuad highp4_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
+BiQuad highp4_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 );
+BiQuad notch4_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 );
 
-//Notch
-BiQuadChain notch4;
-BiQuad notch4_1( 9.56543e-01, -1.82035e+00, 9.56543e-01, -1.84459e+00, 9.53626e-01 );
-BiQuad notch4_2( 1.00000e+00, -1.90305e+00, 1.00000e+00, -1.87702e+00, 9.59471e-01 );
-
-//Lowpass 4th order cutoff 3Hz
-BiQuadChain lowp4;
-BiQuad lowp4_1( 7.69910e-09, 1.53982e-08, 7.69910e-09, -1.96542e+00, 9.65769e-01 );
-BiQuad lowp4_2( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.98532e+00, 9.85677e-01 );
+//Lowpass first order cutoff 0.4Hz
+BiQuad lowp4( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );
 
 // ~~~~~~~~~~~~~~~~~~~EMG FUNCTIONS~~~~~~~~~~~~~~~~~~
 void emgsample()
 {
     
-    //All EMG signal through Highpass
+    // EMG signaal lezen
     double emgread1 = emg1.read();
     double emgread2 = emg2.read();
     double emgread3 = emg3.read();
     double emgread4 = emg4.read();
-
+    
+    // Vierde orde highpass filter + notch filter
     double emg1_highpassed = highp1.step(emgread1);
     double emg2_highpassed = highp2.step(emgread2);
     double emg3_highpassed = highp3.step(emgread3);
     double emg4_highpassed = highp4.step(emgread4);
 
-    //All EMG highpassed through Notch
-    double emg1_notched = notch1.step(emg1_highpassed);
-    double emg2_notched = notch2.step(emg2_highpassed);
-    double emg3_notched = notch3.step(emg3_highpassed);
-    double emg4_notched = notch4.step(emg4_highpassed);
-
-    //All EMG notched rectify
-    double emg1_abs = abs(emg1_notched);
-    double emg2_abs = abs(emg2_notched);
-    double emg3_abs = abs(emg3_notched);
-    double emg4_abs = abs(emg4_notched);
+    //Rectificatie
+    double emg1_abs = abs(emg1_highpassed);
+    double emg2_abs = abs(emg2_highpassed);
+    double emg3_abs = abs(emg3_highpassed);
+    double emg4_abs = abs(emg4_highpassed);
 
     //All EMG abs into lowpass
-    double emg1_lp = lowp1.step(emg1_abs);
-    double emg2_lp = lowp2.step(emg2_abs);
-    double emg3_lp = lowp3.step(emg3_abs);
-    double emg4_lp = lowp4.step(emg4_abs);
-    
-    Movag_1.Insert(emg1_lp);
-    Movag_2.Insert(emg2_lp);
-    Movag_3.Insert(emg3_lp);
-    Movag_4.Insert(emg4_lp);
-    
-    emg1_filtered = Movag_1.GetAverage();
-    emg2_filtered = Movag_2.GetAverage();
-    emg3_filtered = Movag_3.GetAverage();
-    emg4_filtered = Movag_4.GetAverage();
-    
-    
+    emg1_filtered = lowp1.step(emg1_abs);
+    emg2_filtered = lowp2.step(emg2_abs);
+    emg3_filtered = lowp3.step(emg3_abs);
+    emg4_filtered = lowp4.step(emg4_abs);
 
 }
 
 void CalibrationEMG()
 {
-    //static float samples = calibration_time/ts;
-    while(timer_calibration<37) {
+    
+    while(timer_calibration<39) {                           //Duim
         if(timer_calibration>0 && timer_calibration<7) {
             led1=!led1;
             if(emg1_filtered>temp_highest_emg1) {
                 temp_highest_emg1= emg1_filtered;
-                pc.printf("Temp1 = %f \r\n",temp_highest_emg1);
+                pc.printf("Highest value Duim= %f \r\n", temp_highest_emg1);
+                
             }
         }
-        if(timer_calibration>7 && timer_calibration<10) {
+        if(timer_calibration>7 && timer_calibration<10) {    
             led1=0;
             led2=0;
             led3=0;
         }
-        if(timer_calibration>10 && timer_calibration<17) {
+        if(timer_calibration>10 && timer_calibration<17) {  //Bicep
             led2=!led2;
             if(emg2_filtered>temp_highest_emg2) {
                 temp_highest_emg2= emg2_filtered;
-                pc.printf("Temp2 = %f \r\n",temp_highest_emg2);
+                pc.printf("Highest value Bicep= %f \r\n", temp_highest_emg2);
             }
         }
         if(timer_calibration>17 && timer_calibration<20) {
@@ -205,11 +173,11 @@
             led2=0;
             led3=0;
         }
-        if(timer_calibration>20 && timer_calibration<27) {
+        if(timer_calibration>20 && timer_calibration<27) {  //Dorsaal
             led3=!led3;
             if(emg3_filtered>temp_highest_emg3) {
                 temp_highest_emg3= emg3_filtered;
-                pc.printf("Temp3 = %f \r\n",temp_highest_emg3);
+                pc.printf("Highest value Dorsaal= %f \r\n", temp_highest_emg3);
             }
         }
         if(timer_calibration>27 && timer_calibration<30) {
@@ -217,71 +185,127 @@
             led2=0;
             led3=0;
         }
-        if(timer_calibration>30 && timer_calibration<37) {
+        if(timer_calibration>30 && timer_calibration<37) {  //Palmair
             led2=!led2;
             led3=!led3;
             if(emg4_filtered>temp_highest_emg4) {
                 temp_highest_emg4= emg4_filtered;
-                pc.printf("Temp4 = %f \r\n",temp_highest_emg4);
+                pc.printf("Highest value Palmair= %f \r\n", temp_highest_emg4);
             }
         }
         led1=1;
         led2=1;
         led3=1;
 
-
     }
-
-    pc.printf("Highest value Duim= %f \r\n", temp_highest_emg1);
-    pc.printf("Highest value Bicep= %f \r\n", temp_highest_emg2);
-    pc.printf("Highest value Tricep= %f \r\n", temp_highest_emg3);
-    pc.printf("Highest value Onderarm= %f \r\n", temp_highest_emg4);
-
-
-    threshold1 = temp_highest_emg1*duim_p_t;  //Right Biceps
-    threshold2 = temp_highest_emg2*bicep_p_t; //Right Triceps
-    threshold3 = temp_highest_emg3*tricep_p_t;  //Left Biceps
-    threshold4 = temp_highest_emg4*onderarm_p_t; //Left Triceps
+    pc.printf("threshold calculation\r\n");
+    threshold1 = temp_highest_emg1*Duim_p_t;  
+    threshold2 = temp_highest_emg2*Bicep_p_t; 
+    threshold3 = temp_highest_emg3*Dorsaal_p_t;  
+    threshold4 = temp_highest_emg4*Palmair_p_t;
+    
+    threshold1L = temp_highest_emg1*Duim_p_tL;
+    threshold2L = temp_highest_emg2*Bicep_p_tL; 
+    threshold3L = temp_highest_emg3*Dorsaal_p_tL; 
+    threshold4L = temp_highest_emg4*Palmair_p_tL; 
 }
 
-//Check if emg_filtered has reached their threshold
+//Check of emg_filtered boven de threshold is
 void threshold_check()
 {
 
-    //EMG1 threshold check
+// EMG1 Check    
+if (thresholdreach1 == false){            //Als emg_filtered nog niet boven de bovenste threshold is geweest
+    //bovenste threshold check
     if(emg1_filtered>threshold1) {
         Duim = 1;
-        led1 = !led1;
+        thresholdreach1 = true;
+        
     } else {
         Duim= 0;
     }
-    //EMG2 threshold check
+    }
+else{                                   //Als emg_filtered boven de bovenste threshold is geweest
+    //onderste threshold check
+    if(emg1_filtered<threshold1L) {
+        Duim = 0;
+        thresholdreach1 = false;
+        
+    } else {
+        Duim= 1;
+    }
+    }
+
+// EMG2 Check    
+if (thresholdreach2 == false){            //Als emg_filtered nog niet boven de bovenste threshold is geweest
+    //bovenste threshold check
     if(emg2_filtered>threshold2) {
         Bicep = 1;
-        led2 = !led2;
+        thresholdreach2 = true;
     } else {
         Bicep= 0;
     }
-    //EMG3 threshold check
-    if(emg3_filtered>threshold3) {
-        Tricep = 1;
-        led3 = !led3;
-    } else {
-        Tricep= 0;
     }
-    //EMG4 threshold check
-    if(emg4_filtered>threshold4) {
-        Onderarm = 1;
-        led1 = !led1;
-        led2 = !led1;
-        led3 = !led1;
+else{                                   //Als emg_filtered boven de bovenste threshold is geweest
+    //onderste threshold check
+    if(emg2_filtered<threshold2L) {
+        Bicep = 0;
+        thresholdreach2 = false;
+        
     } else {
-        Onderarm= 0;
+        Bicep= 1;
+    }
     }
 
-    
+// EMG3 Check    
+if (thresholdreach3 == false){            //Als emg_filtered nog niet boven de bovenste threshold is geweest
+    //bovenste threshold check
+    if(emg3_filtered>threshold3) {
+        Dorsaal = 1;
+        thresholdreach3 = true;
+    } else {
+        Dorsaal= 0;
+    }
+    }
+else{                                   //Als emg_filtered boven de bovenste threshold is geweest
+    //onderste threshold check
+    if(emg3_filtered<threshold3L) {
+        Dorsaal = 0;
+        thresholdreach3 = false;
+        
+    } else {
+        Dorsaal= 1;
+    }
+    }
 
-}
+// EMG4 Check    
+if (thresholdreach4 == false){            //Als emg_filtered nog niet boven de bovenste threshold is geweest
+    //bovenste threshold check
+    if(emg4_filtered>threshold4) {
+        Palmair = 1;
+        thresholdreach4 = true;
+    } else {
+        Palmair= 0;
+    }
+    }
+else{                                   //Als emg_filtered boven de bovenste threshold is geweest
+    //onderste threshold check
+    if(emg4_filtered<threshold4L) {
+        Palmair = 0;
+        thresholdreach4 = false;
+        
+    } else {
+        Palmair= 1;
+    }
+    }
+
+    }
+
+
+
+   
+
+
 
 
 Ticker      sample_timer;
@@ -292,52 +316,73 @@
 {
 pc.printf("Duim Right = %i\r\n", Duim);
 pc.printf("Bicep Right = %i\r\n",Bicep);
-pc.printf("tricep Left = %i\r\n", Tricep);
-pc.printf("onderarm Left = %i\r\n", Onderarm);
-wait(0.01);             
+pc.printf("Dorsaal Left = %i\r\n", Dorsaal);
+pc.printf("Palmair Left = %i\r\n", Palmair);           
+    if (Duim == 1){     //groen
+    led1 = 0;
+    }
+    else {
+    led1 = 1;
+    }
+    if (Bicep == 1){        //rood
+    led2 = 0;
+    }
+    else {
+    led2 = 1;
+    }
+    if (Dorsaal == 1){      //blauw
+    led3 = 0;
+    }
+    else {
+    led3 = 1;
+    }
+    if (Palmair == 1){
+    led1 = 0;
+    led2 = 0;
+    led3 = 0;
+    }
+    else {
+    led1 = 1;
+    led1 = 2;
+    led1 = 3;
+    }
+}
 
-    
-}
 
 int main()
 {   
+                led1 = 1;
+                led2 = 1;
+                led3 = 1;
     sample_ticker.attach(&emgsample, ts);
     pc.baud(115200);
     
-    //BiQuad Chain add
-    highp1.add( &highp1_1 ).add( &highp1_2 );
-    notch1.add( &notch1_1 ).add( &notch1_2 );
-    lowp1.add( &lowp1_1 ).add(&lowp1_2);
-
-    highp2.add( &highp2_1 ).add( &highp2_2 );
-    notch2.add( &notch2_1 ).add( &notch2_2 );
-    lowp2.add( &lowp2_1 ).add(&lowp2_2);
-
-    highp3.add( &highp3_1 ).add( &highp3_2 );
-    notch3.add( &notch3_1 ).add( &notch3_2 );
-    lowp3.add( &lowp3_1 ).add(&lowp3_2);
-
-    highp4.add( &highp4_1 ).add( &highp4_2 );
-    notch4.add( &notch4_1 ).add( &notch4_2 );
-    lowp4.add( &lowp4_1 ).add(&lowp4_2);
-
+//BiQuad Chain add
+    highp1.add( &highp1_1 ).add( &highp1_2 ).add( &notch1_1 );
+    highp2.add( &highp2_1 ).add( &highp2_2 ).add( &notch2_1 );
+    highp3.add( &highp3_1 ).add( &highp3_2 ).add( &notch3_1 );
+    highp4.add( &highp4_1 ).add( &highp4_2 ).add( &notch4_1 );
+    
+    // Ticker voor EMG uitlezen
     
     
-                temp_highest_emg1 = 0; //highest detected value right biceps
+                temp_highest_emg1 = 0; //highest detected value right Biceps
                 temp_highest_emg2 = 0;
                 temp_highest_emg3 = 0;
                 temp_highest_emg4 = 0;
 
                 timer_calibration.reset();
                 timer_calibration.start();
-
+                CalibrationEMG();
+                pc.printf("threshold1 = %i, threshold1L = %f\r\n", threshold1, threshold1L);
+                threshold_check_ticker.attach(&threshold_check, 0.01);
+                led1 = 1;
+                led2 = 1;
+                led3 = 1;
+                sample_timer.attach(&sample, 0.1);
+                pc.printf("sample timer attached\r\n");
                 
-                CalibrationEMG();
-                threshold_check_ticker.attach(threshold_check, 0.1);
-                sample_timer.attach(&sample, 0.1);
-                //sample_ticker.detach();
-                timer_calibration.stop();
-               
+                timer_calibration.stop();