Diff: main.cpp

Revision:

4:fddab1c875a9

Parent:

3:59b504840b95

Child:

5:81d3b53087c0

--- a/main.cpp	Thu Oct 26 09:55:24 2017 +0000
+++ b/main.cpp	Thu Oct 26 11:32:53 2017 +0000
@@ -28,8 +28,15 @@
 DigitalOut      led_B(LED_BLUE);
 
 //Setting Tickers for sampling EMG and determing if the threshold is met
-Ticker          sample_timer;
-Ticker          threshold_timer;
+Ticker      sample_timer;
+Ticker      threshold_timerR;
+Ticker      threshold_timerL;
+
+Timer       t_thresholdR;
+Timer       t_thresholdL;
+
+double      currentTimeTR;
+double      currentTimeTL;
 
 InterruptIn     button(SW2); // Wordt uiteindelijk vervangen door EMG
 
@@ -54,27 +61,38 @@
 /* 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 );
+BiQuad      bqNotch1( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
+BiQuad      bqNotch2( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
+
+BiQuad      bqNotchTR( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
+BiQuad      bqNotchTL( 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 );
+BiQuad      bqHigh1( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
+BiQuad      bqHigh2( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
+
+BiQuad      bqHighTR( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
+BiQuad      bqHighTL( 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 );
+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 );
+
+BiQuad      bqLowTR( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
+BiQuad      bqLowTL( 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;
+BiQuadChain bqChain1;
+BiQuadChain bqChain2;
 
+BiQuadChain bqChainTR;
+BiQuadChain bqChainTL;
 
-//Declaring all doubles needed in the filtering process
+//Defining 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
@@ -83,11 +101,11 @@
 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 threshold = 0.03;
+
 double numsamples = 500;
 double emgBRsum = 0;
 double emgBRmeanMVC;
@@ -97,6 +115,46 @@
 double emgBLmeanMVC;
 double thresholdBL;
 
+/* Function to sample the EMG and get a Threshold value from it, 
+which can be used throughout the process */
+
+void Threshold_samplingBR() {
+    t_thresholdR.start();
+    currentTimeTR = t_thresholdR.read();
+    
+    if (currentTimeTR <= 1) {
+           
+        emgBRfiltered = bqChainTR.step( emgBR.read() );   //Notch+High-pass
+        emgBRrectified = fabs(emgBRfiltered);            //Rectification
+        emgBRcomplete = bqLowTR.step(emgBRrectified);     //Low-pass
+    
+        emgBRsum = emgBRsum + emgBRcomplete;
+        }
+    emgBRmeanMVC = emgBRsum/numsamples; 
+    thresholdBR = emgBRmeanMVC * 0.20;
+    
+
+    
+    //pc.printf("ThresholdBR = %f \n", thresholdBR);
+}
+void Threshold_samplingBL() {
+    t_thresholdL.start();  
+    currentTimeTL = t_thresholdL.read();
+    
+    if (currentTimeTL <= 1) {
+            
+        emgBLfiltered = bqChain2.step( emgBL.read() );    //Notch+High-pass
+        emgBLrectified = fabs( emgBLfiltered );           //Rectification
+        emgBLcomplete = bqLow2.step( emgBLrectified );    //Low-pass
+        
+        emgBLsum = emgBLsum + emgBLcomplete;
+        }
+        
+    emgBLmeanMVC = emgBLsum/numsamples;
+    thresholdBL = emgBLmeanMVC * 0.20;
+
+}
+
 // EMG functions
 void EMG_sample()
 {
@@ -108,7 +166,7 @@
     
     /*Getting threshold value for Right Biceps, a value of 20% of 
     Maximum Voluntary Contraction is chosen as threshold value */
-    if (countBR < numsamples) {
+    /*if (countBR < numsamples) {
         emgBRsum = emgBRsum + emgBRcomplete;
         countBR++;
         led_R = 0;
@@ -118,7 +176,7 @@
     
     emgBRmeanMVC = emgBRsum / numsamples;
     
-    thresholdBR = emgBRmeanMVC * 0.25;
+    thresholdBR = emgBRmeanMVC * 0.25;*/
     
     //Filtering steps for the Left Biceps EMG
     emgBLfiltered = bqChain2.step( emgBL.read() );    //Notch+High-pass
@@ -128,7 +186,7 @@
     
     /*Getting threshold value for Left Biceps, a value of 20% of 
     Maximum Voluntary Contraction is chosen as threshold value */
-    if (countBL < numsamples) {
+    /*if (countBL < numsamples) {
         emgBLsum = emgBLsum + emgBLcomplete;
         countBL++;
         }
@@ -137,7 +195,7 @@
     
     thresholdBL = emgBLmeanMVC * 0.25;
         
-    //pc.printf("ThresholdBR = %0.3f, ThresholdBL = %0.3f \n", thresholdBR,thresholdBL);
+    //pc.printf("ThresholdBR = %0.3f, ThresholdBL = %0.3f \n", thresholdBR,thresholdBL);*/
 }
 
 // Function to make the BiQuadChain for the Notch and High pass filter for both filters
@@ -528,6 +586,8 @@
 {
     pc.baud(115200);
     getbqChain();
+    threshold_timerR.attach(&Threshold_samplingBR, 0.002);
+    threshold_timerL.attach(&Threshold_samplingBL, 0.002);
     while(true){
         sample_timer.attach(&EMG_sample, 0.002);
         wait(2.5f);