Diff: main.cpp

Revision:

24:0db52d2e1d2a

Parent:

23:f01a1bd3b3c4

Child:

25:56f5c2786f11

--- a/main.cpp	Thu Mar 28 14:09:33 2019 +0000
+++ b/main.cpp	Fri Apr 05 15:06:52 2019 +0000
@@ -1,7 +1,8 @@
 #include "mbed.h"
 #include "HIDScope.h"
 #include "BiQuad.h"
-#include "BiQuadchains_zelfbeun.h"
+//#include "BiQuadchains_zelfbeun.h"
+#include <iostream>
 DigitalOut led1(LED_GREEN);
 DigitalOut led2(LED_RED);
 DigitalOut led3(LED_BLUE);
@@ -52,9 +53,79 @@
 int EMGyplus;
 int EMGymin ;
 
+//filters
+BiQuadChain bqc2; //chain voor High Pass en Notch
+BiQuad bq3(0.9561305540521468,-1.9122611081042935,0.9561305540521468,-1.9103725395337858,0.9141496766748013); //High Pass Filter
+BiQuad bq4(9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01); //Notch Filter
+BiQuad bq6(0.6370466299626938,1.2740932599253876,0.6370466299626938,1.13958365554699,0.40860286430378506); //Lowpass Filter
+//EMG1!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+//Highpass 
+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 );
+
+//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 );
+
+//EMG2!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+//Highpass
+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 );
+
+//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 );
+
+//EMG3!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+//Highpass
+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 );
+
+//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 );
+
+//EMG4!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+//Highpass
+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 );
+
+//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 );
+
 // ~~~~~~~~~~~~~~~~~~~EMG FUNCTIONS~~~~~~~~~~~~~~~~~~
 void emgsample()
 {
+    /*
     //All EMG signal through Highpass
     double emgread1 = emg1.read();
     double emgread2 = emg2.read();
@@ -83,7 +154,7 @@
     emg2_filtered = lowp2.step(emg2_abs);
     emg3_filtered = lowp3.step(emg3_abs);
     emg4_filtered = lowp4.step(emg4_abs);
-
+*/
 }
 
 void CalibrationEMG()
@@ -193,17 +264,59 @@
 
 
 Ticker      sample_timer;
-HIDScope    scope( 4 );
+HIDScope    scope( 2 );
 DigitalOut  led(LED1);
 
 
 void sample()
 {
-   
+   //All EMG signal through Highpass
+    double emgread1 = emg1.read();
+    double emgread2 = emg2.read();
+    double emgread3 = emg3.read();
+    double emgread4 = emg4.read();
+    
+float LB_Filter_1 = bqc2.step(emg1.read()); 
+
+    double emg1_highpassed = highp1.step(emg1.read());
+    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);
+
+    //All EMG abs into lowpass
+    emg1_filtered = lowp1.step(emg1_abs);
+    emg2_filtered = lowp2.step(emg2_abs);
+    emg3_filtered = lowp3.step(emg3_abs);
+    emg4_filtered = lowp4.step(emg4_abs);
+    
+    //EMG1 threshold check
+    if(emg1_filtered>threshold1) {
+        bicepsR = 1;
+    } else {
+        bicepsR= 0;
+    }
+    //EMG2 threshold check
+    if(emg2_filtered>threshold2) {
+        tricepsR = 1;
+    } else {
+        tricepsR= 0;
+    }
     scope.set(0, emg1.read() );
-    scope.set(1, emg2.read() );
-    scope.set(2, bicepsR );
-    scope.set(3, tricepsR);
+    //scope.set(1, emg2.read() );
+    scope.set(1, emg1_filtered );
+    //scope.set(3, emg2_filtered);
     
     scope.send();
     
@@ -212,9 +325,28 @@
 
 int main()
 {   
-    sample_timer.attach(&sample, 0.002);
+    //sample_ticker.attach(&emgsample, ts);
+    //bqc1.add( &bq1 ).add( &bq2 ).add( &bq5 ); //make BiQuadChain EMG left
+    //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);
+    bqc2.add( &bq3 ).add( &bq4 ).add( &bq6 ); //make BiQuadChain EMG right
+    sample_timer.attach(&sample, 0.001);
     
-                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;
@@ -222,14 +354,16 @@
                 timer_calibration.reset();
                 timer_calibration.start();
 
-                sample_ticker.attach(&emgsample, ts);
-                CalibrationEMG();
-                sample_ticker.detach();
+                
+                //CalibrationEMG();
+                //sample_ticker.detach();
                 timer_calibration.stop();
-                threshold_check_ticker.attach(threshold_check, ts)
-    
+               
+    */
     
 
     /*empty loop, sample() is executed periodically*/
-    while(1) {}
+    while(1) {
+        wait(0.01);
+        }
 }
\ No newline at end of file