Diff: main.cpp

Revision:

2:bf1c9d7afabd

Parent:

1:9e871647a074

Child:

3:edfc159b024e

--- a/main.cpp	Thu Oct 26 12:56:31 2017 +0000
+++ b/main.cpp	Thu Oct 26 13:05:51 2017 +0000
@@ -4,10 +4,6 @@
 #include "QEI.h"
 #include "BiQuad.h"
 
-bool Move_done=false;
-
-
-
 Serial          pc(USBTX, USBRX);
 
 //Defining all in- and outputs
@@ -32,14 +28,21 @@
 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
 
 Timer           t;
-double          speedfactor1; // = 0.01; snelheid in, zonder potmeter gebruik            <- waarom is dit zo?
-double          speedfactor2;
+double          speedfactor; // = 0.01; snelheid in, zonder potmeter gebruik            <- waarom is dit zo?
+
 // Getting the counts from the Encoder
 int             counts1 = Encoder1.getPulses();
 int             counts2 = Encoder2.getPulses();
@@ -48,14 +51,136 @@
 int             delta1;
 int             delta2; 
 
+bool   Move_done = false;
+
 /* 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 */
+and the Low-pass filter cancels out all frequencies below 4 Hz. The filters are
+declared four times, so that they can be used for sampling of right and left
+biceps, during measurements and calibration. */
 
 /* 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      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      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      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 bqChainTR;
+BiQuadChain bqChainTL;
+
+//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
+
+double emgBLfiltered;   //Left biceps Notch+High pass filter
+double emgBLrectified;  //Left biceps rectified
+double emgBLcomplete;   //Left biceps low-pass filter, filtering complete
+
+int countBR = 0;
+int countBL = 0;
+
+//double threshold = 0.03;
+
+double numsamples = 500;
+double emgBRsum = 0;
+double emgBRmeanMVC;
+double thresholdBR;
+
+double emgBLsum = 0;
+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()
+{
+    //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
+      
+    //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
+
+}
+// Function to make the BiQuadChain for the Notch and High pass filter for all three filters    
+void getbqChain()
+{
+    bqChain1.add(&bqNotch1).add(&bqHigh1);                 //Making the BiQuadChain
+    bqChain2.add(&bqNotch2).add(&bqHigh2);
+    
+    bqChainTR.add(&bqNotchTR).add(&bqHighTR);
+    bqChainTL.add(&bqNotchTR).add(&bqHighTL);
+}
+
 // Initial input value for X                                                            
 int Xin=0;  
 int Xin_new;                                                                           
@@ -271,17 +396,13 @@
 double Psty;
 double T=0.02;//seconds
 
+double speedfactor1;
+double speedfactor2;
 
 
 //Deel om motor(en) aan te sturen--------------------------------------------
 
-/*double Accelaration(){
-    if (countso < statedsverander)
-        { speedfactor = 0.01;
-        speedfactor = speedfactor + 0.01;
-        }
-  }*/  
-    
+   
 
 void calcdelta1() {    
     delta1 = (dcounto - Encoder1.getPulses());
@@ -540,7 +661,9 @@
 int main()
 {
     pc.baud(115200);
-    //getbqChain();
+    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);