Diff: main.cpp

Revision:

26:874d50f440d0

Parent:

25:56f5c2786f11

Child:

27:c0d748b7d5d1

--- a/main.cpp	Mon Apr 08 15:55:25 2019 +0000
+++ b/main.cpp	Tue Apr 09 07:51:20 2019 +0000
@@ -1,12 +1,20 @@
 #include "mbed.h"
-//#include "HIDScope.h"
+#include "HIDScope.h"
 #include "BiQuad.h"
-#include "MODSERIAL.h"
+
 #include <iostream>
+#include "MovingAverage.h"
+#define NSAMPLE 200
 DigitalOut led1(LED_GREEN);
 DigitalOut led2(LED_RED);
 DigitalOut led3(LED_BLUE);
-//MODSERIAL pc(USBTX, USBRX);
+
+
+//MovingAverage
+MovingAverage <float>Movag_1(NSAMPLE,0.0); //Make Moving Average, Define NSAMPLE above
+MovingAverage <float>Movag_2(NSAMPLE,0.0); //Make Moving Average, Define NSAMPLE above
+MovingAverage <float>Movag_3(NSAMPLE,0.0); //Make Moving Average, Define NSAMPLE above
+MovingAverage <float>Movag_4(NSAMPLE,0.0); //Make Moving Average, Define NSAMPLE above
 
 //EMG tickers, these tickers are called in the mainscript with fsample 500Hz, also sends to HIDscope with same fsample
 Ticker sample_ticker; //ticker for filtering pref. with 1000Hz, define in tick.attach
@@ -57,11 +65,7 @@
 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;
@@ -154,10 +158,22 @@
     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);
+    double emg1_lp = lowp1.step(emg1_abs);
+    double emg2_lp = lowp2.step(emg2_abs);
+    double emg3_lp = lowp3.step(emg3_abs);
+    double emg4_lp = lowp4.step(emg4_abs);
+    
+    Movag_1.Insert(emg1_lp);
+    Movag_2.Insert(emg2_lp);
+    Movag_3.Insert(emg3_lp);
+    Movag_4.Insert(emg4_lp);
+    
+    emg1_filtered = Movag_1.GetAverage();
+    emg2_filtered = Movag_2.GetAverage();
+    emg3_filtered = Movag_3.GetAverage();
+    emg4_filtered = Movag_4.GetAverage();
+    
+    
 
 }
 
@@ -169,7 +185,7 @@
             led1=!led1;
             if(emg1_filtered>temp_highest_emg1) {
                 temp_highest_emg1= emg1_filtered;
-                //pc.printf("Temp1 = %f \r\n",temp_highest_emg1);
+                
             }
         }
         if(timer_calibration>10 && timer_calibration<15) {
@@ -181,7 +197,7 @@
             led2=!led2;
             if(emg2_filtered>temp_highest_emg2) {
                 temp_highest_emg2= emg2_filtered;
-                //pc.printf("Temp2 = %f \r\n",temp_highest_emg2);
+                
             }
         }
         if(timer_calibration>25 && timer_calibration<30) {
@@ -193,7 +209,7 @@
             led3=!led3;
             if(emg3_filtered>temp_highest_emg3) {
                 temp_highest_emg3= emg3_filtered;
-                //pc.printf("Temp3 = %f \r\n",temp_highest_emg3);
+                
             }
         }
         if(timer_calibration>40 && timer_calibration<45) {
@@ -206,7 +222,7 @@
             led3=!led3;
             if(emg4_filtered>temp_highest_emg4) {
                 temp_highest_emg4= emg4_filtered;
-                //pc.printf("Temp4 = %f \r\n",temp_highest_emg4);
+                
             }
         }
         led1=1;
@@ -215,13 +231,7 @@
 
 
     }
-/*
-    pc.printf("Highest value Duim= %f \r\n", temp_highest_emg1);
-    pc.printf("Highest value Bicep= %f \r\n", temp_highest_emg2);
-    pc.printf("Highest value Tricep= %f \r\n", temp_highest_emg3);
-    pc.printf("Highest value Onderarm= %f \r\n", temp_highest_emg4);
 
-*/
     threshold1 = temp_highest_emg1*duim_p_t;  //Right Biceps
     threshold2 = temp_highest_emg2*bicep_p_t; //Right Triceps
     threshold3 = temp_highest_emg3*tricep_p_t;  //Left Biceps
@@ -257,51 +267,28 @@
         Onderarm= 0;
     }
 
-    /*
-    pc.printf("Duim Right = %i", Duim);
-    pc.printf("Bicep Right = %i",Bicep);
-    pc.printf("tricep Left = %i", Tricep);
-    pc.printf("onderarm Left = %i", Onderarm);
-    */
+
 
 }
 
 
 Ticker      sample_timer;
-//HIDScope    scope( 4 );
-DigitalOut  led(LED1);
 
 
 void sample()
 {
-             
-    //EMG1 threshold check
-    if(emg1_filtered>threshold1) {
-        Duim = 1;
-    } else {
-        Duim= 0;
-    }
-    //EMG2 threshold check
-    if(emg2_filtered>threshold2) {
-        Bicep = 1;
-    } else {
-        Bicep= 0;
-    }
-    /*
-    scope.set(0, emg1_filtered );
-    scope.set(1, emg2_filtered );
-    scope.set(2, emg3_filtered );
-    scope.set(3, emg4_filtered);
+    pc.printf("Duim Right = %i", Duim);
+    pc.printf("Bicep Right = %i",Bicep);
+    pc.printf("tricep Left = %i", Tricep);
+    pc.printf("onderarm Left = %i", Onderarm);
     
-    scope.send();
-    */
     led = !led;
 }
 
 int main()
 {   
     sample_ticker.attach(&emgsample, ts);
-    pc.baud(115200);
+
     //BiQuad Chain add
     highp1.add( &highp1_1 ).add( &highp1_2 );
     notch1.add( &notch1_1 ).add( &notch1_2 );
@@ -318,8 +305,7 @@
     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_emg2 = 0;
@@ -331,8 +317,10 @@
 
                 
                 CalibrationEMG();
-                sample_ticker.detach();
-                timer_calibration.stop();
+                sample_timer.attach(&sample, 0.5);
+                threshold_check_ticker.attach(&threshold_check, 0.001)
+                //sample_ticker.detach();
+                //timer_calibration.stop();