Diff: main.cpp

Revision:

1:6aac013b0ba3

Parent:

0:eb16ed402ffa

Child:

2:2c4ee76dc830

--- a/main.cpp	Wed Oct 25 10:40:18 2017 +0000
+++ b/main.cpp	Wed Oct 25 11:00:20 2017 +0000
@@ -27,6 +27,10 @@
 DigitalOut      led_G(LED_GREEN);
 DigitalOut      led_B(LED_BLUE);
 
+//Setting Tickers for sampling EMG and determing if the threshold is met
+Ticker          sample_timer;
+Ticker          threshold_timer;
+
 InterruptIn     button(SW2); // Wordt uiteindelijk vervangen door EMG
 
 Timer           t;
@@ -40,6 +44,102 @@
 int             delta1;
 int             delta2; 
 
+/* Defining all the different BiQuad filters, which contain a Notch filter,
+High-pass filter and Low-pass filter. The Notch filter cancels all frequencies
+between 49 and 51 Hz, the High-pass filter cancels all frequencies below 20 Hz
+and the Low-pass filter cancels out all frequencies below 4 Hz */
+
+/* Defining all the normalized values of b and a in the Notch filter for the
+creation of the Notch BiQuad */
+
+BiQuad          bqNotch1( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
+BiQuad          bqNotch2( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
+
+/* Defining all the normalized values of b and a in the High-pass filter for the
+creation of the High-pass BiQuad */
+
+BiQuad          bqHigh1( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
+BiQuad          bqHigh2( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
+
+/* Defining all the normalized values of b and a in the Low-pass filter for the
+creation of the Low-pass BiQuad */
+
+BiQuad          bqLow1( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
+BiQuad          bqLow2( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
+
+// Creating a variable needed for the creation of the BiQuadChain
+BiQuadChain     bqChain1;
+BiQuadChain     bqChain2;
+
+
+//Declaring all doubles needed in the filtering process
+double emgBRfiltered;   //Right biceps Notch+High pass filter
+double emgBRrectified;  //Right biceps rectified
+double emgBRcomplete;   //Right biceps low-pass filter, filtering complete
+
+double emgBLfiltered;   //Left biceps Notch+High pass filter
+double emgBLrectified;  //Left biceps rectified
+double emgBLcomplete;   //Left biceps low-pass filter, filtering complete
+
+/* Declaring counters and variables needed to get threshold of both muscles.
+This is neeeded for the calibration process*/
+int countBR = 0;
+int countBL = 0;
+
+double numsamples = 500;
+double emgBRsum = 0;
+double emgBRmeanMVC;
+double thresholdBR;
+
+double emgBLsum = 0;
+double emgBLmeanMVC;
+double thresholdBL;
+
+// EMG functions
+void EMG_sample()
+{
+    //Filtering steps for the Right Biceps EMG
+    emgBRfiltered = bqChain1.step( emgBR.read() );   //Notch+High-pass
+    emgBRrectified = fabs(emgBRfiltered);            //Rectification
+    emgBRcomplete = bqLow1.step(emgBRrectified);     //Low-pass
+    
+    
+    /*Getting threshold value for Right Biceps, a value of 30% of 
+    Maximum Voluntary Contraction is chosen as threshold value */
+    if (countBR < numsamples) {
+        emgBRsum = emgBRsum + emgBRcomplete;
+        countBR++;
+        }
+    
+    emgBRmeanMVC = emgBRsum / numsamples;
+    
+    thresholdBR = emgBRmeanMVC * 0.3;
+   
+   
+    //Filtering steps for the Left Biceps EMG
+    emgBLfiltered = bqChain2.step( emgBL.read() );    //Notch+High-pass
+    emgBLrectified = fabs( emgBLfiltered );           //Rectification
+    emgBLcomplete = bqLow2.step( emgBLrectified );    //Low-pass
+    
+    /*Getting threshold value for Left Biceps, a value of 30% of 
+    Maximum Voluntary Contraction is chosen as threshold value */
+    if (countBL < numsamples) {
+        emgBLsum = emgBLsum + emgBLcomplete;
+        countBL++;
+        }
+    
+    emgBLmeanMVC = emgBLsum / numsamples;
+    
+    thresholdBL = emgBLmeanMVC * 0.3;
+}
+
+// Function to make the BiQuadChain for the Notch and High pass filter for both filters
+void getbqChain()
+{
+    bqChain1.add(&bqNotch1).add(&bqHigh1);                 
+    bqChain2.add(&bqNotch2).add(&bqHigh2);
+}
+
 // Initial input value for X                                                            
 int Xin=0;                                                                              //<- Hoe zit het met deze als we de robot daadwerkelijk gebruiken
 double huidigetijdX;
@@ -62,10 +162,10 @@
     led_G=1; 
     led_B=1;
         while(true){
-    button.fall(ledtX);          //This has to be replaced by EMG   
-    /*if (EMG>threshold){
+    //button.fall(ledtX);          //This has to be replaced by EMG   
+    if (emgBRcomplete > thresholdBR){
         ledtX();                 // dit is wat je uiteindelijk wil dat er staat
-    }*/
+    }
     t.start();
     huidigetijdX=t.read();
     if (huidigetijdX>2){
@@ -97,15 +197,15 @@
     led_G=1;
     led_B=0; 
     while(true){
-    button.fall(ledtY);         //See comments at X   
-    /*if (EMG>threshold){
-        Piek();                 // dit is wat je uiteindelijk wil dat er staat
-    }*/
+    //button.fall(ledtY);         //See comments at X   
+    if (emgBRcomplete > thresholdBR){
+        ledtY();                 // dit is wat je uiteindelijk wil dat er staat
+    }
     t.start();
     huidigetijdY=t.read();
     if (huidigetijdY>2){
         led_B=1; 
-        button.fall(0);   
+        button.fall(0);   // Wat is deze?
         return 0;      // ga door naar het volgende programma 
         }
     }
@@ -393,6 +493,8 @@
 int main()
 {
     pc.baud(115200);
+    getbqChain();
+    sample_timer.attach(&EMG_sample, 0.002);
     tellerX();
     tellerY();
     calculator();