Diff: main.cpp

Revision:

30:0988a637a342

Parent:

29:86ef596c9e70

Child:

31:4011beb5a03c

--- a/main.cpp	Tue Apr 16 08:53:30 2019 +0000
+++ b/main.cpp	Tue Apr 16 10:56:11 2019 +0000
@@ -14,14 +14,14 @@
 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.0005; //tijdsstap !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+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
 //Gefilterde EMG signalen
@@ -76,7 +76,7 @@
 
 //EMG1!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 //Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
-BiQuadChain highnotch1;
+BiQuadChain highp1;
 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 );
@@ -85,85 +85,62 @@
 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 = emg1.read();     //////////////////////////PROEF
+    double emgread2 = emg2.read();
     double emgread3 = emg3.read();
     double emgread4 = emg4.read();
-
-    double emg1_highpassed = highnotch1.step(emgread1);
+    
+    // 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
+    //Rectificatie
     double emg1_abs = abs(emg1_highpassed);
-    double emg2_abs = abs(emg2_notched);
-    double emg3_abs = abs(emg3_notched);
-    double emg4_abs = abs(emg4_notched);
+    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_filtered = lowp1.step(emg1_abs);
-    double emg2_filtered = lowp2.step(emg2_abs);
-    double emg3_filtered = lowp3.step(emg3_abs);
-    double emg4_filtered = lowp4.step(emg4_abs);
+    emg1_filtered = lowp1.step(emg1_abs);
+    emg2_filtered = lowp2.step(emg2_abs);
+    emg3_filtered = lowp3.step(emg3_abs);
+    emg4_filtered = lowp4.step(emg4_abs);
 
 }
 
@@ -331,51 +308,65 @@
 
 
 Ticker      sample_timer;
-HIDScope    scope( 4 );
+HIDScope    scope( 2 );
 DigitalOut  led(LED1);
 
 
 void sample()
 {
              
-       
+    /*   
     scope.set(0, emg1.read() );
-    scope.set(1, emg1_highpassed);
-    scope.set(2, emg3.read() );
-    scope.set(3, emg3_filtered);
+    scope.set(1, emg1_filtered);
+    scope.set(2, emg3_filtered );
+    scope.set(3, emg4_filtered);
     
     scope.send();
-    
-    led = !led;
+    */
+    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()
 {   
-        sample_ticker.attach(&emgsample, ts);
+    // Ticker voor EMG filtering
+    sample_ticker.attach(&emgsample, ts);
 
     //BiQuad Chain add
-    highp1.add( &highp1_1 ).add( &highp1_2 );
-    notch1.add( &notch1_1 ).add( &notch1_2 );
-    lowp1.add( &lowp1_1 ).add(&lowp1_2);
-
-    /*/BiQuad Chain add
-    highnotch1.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);
-    sample_timer.attach(&sample, 0.001);
+    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_emg2 = 0;
@@ -386,11 +377,15 @@
                 timer_calibration.start();
 
                 
-                //CalibrationEMG();
+                CalibrationEMG();
                 
-                //threshold_check_ticker.attach(&threshold_check, 0.001);
-                //sample_ticker.detach();
-                //timer_calibration.stop();
+                threshold_check_ticker.attach(&threshold_check, 0.001);
+                led1 = 1;
+                led2 = 1;
+                led3 = 1;
+                sample_timer.attach(&sample, 0.001);
+                sample_ticker.detach();
+                timer_calibration.stop();