Diff: main.cpp

Revision:

0:c9891e1c62f0

Child:

1:ecd6dc3c839b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Apr 16 11:03:47 2019 +0000
@@ -0,0 +1,348 @@
+#include "mbed.h"
+#include "BiQuad.h"
+#include "MODSERIAL.h"
+#include <iostream>
+#include "MovingAverage.h"
+#define NSAMPLE 100
+
+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
+Ticker threshold_check_ticker;
+Timer t; //timer try out for Astrid
+Timer timer_calibration; //timer for EMG calibration
+
+//Input
+AnalogIn    emg1( A1 );
+AnalogIn    emg2( A2 );
+AnalogIn    emg3( A3 );
+AnalogIn    emg4( A4 );
+
+// GLOBALS EMG
+//Filtered EMG signals from the end of the chains
+volatile double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered;
+int i = 0;
+
+//Define doubles for calibration and ticker
+double ts = 0.001; //tijdsstap
+double calibration_time = 55; //time EMG calibration should take
+
+volatile double temp_highest_emg1 = 0; //highest detected value right biceps
+volatile double temp_highest_emg2 = 0;
+volatile double temp_highest_emg3 = 0;
+volatile double temp_highest_emg4 = 0;
+
+//Doubles for calculation threshold
+double duim_p_t = 0.4; //set threshold at percentage of highest value
+double bicep_p_t = 0.4; //
+double tricep_p_t = 0.4; //set threshold at percentage of highest value
+double onderarm_p_t = 0.4;
+
+volatile double threshold1;
+volatile double threshold2;
+volatile double threshold3;
+volatile double threshold4;
+
+// thresholdreads bools
+int Duim;
+int Bicep;
+int Tricep;
+int Onderarm;
+
+//  EMG OUTPUT
+int EMGxplus;
+int EMGxmin ;
+int EMGyplus;
+int EMGymin ;
+
+
+//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();
+    double emgread3 = emg3.read();
+    double emgread4 = emg4.read();
+
+    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
+    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
+    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();
+    
+    
+
+}
+
+void CalibrationEMG()
+{
+    //static float samples = calibration_time/ts;
+    while(timer_calibration<37) {
+        if(timer_calibration>0 && timer_calibration<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>7 && timer_calibration<10) {
+            led1=0;
+            led2=0;
+            led3=0;
+        }
+        if(timer_calibration>10 && timer_calibration<17) {
+            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>17 && timer_calibration<20) {
+            led1=0;
+            led2=0;
+            led3=0;
+        }
+        if(timer_calibration>20 && timer_calibration<27) {
+            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>27 && timer_calibration<30) {
+            led1=0;
+            led2=0;
+            led3=0;
+        }
+        if(timer_calibration>30 && timer_calibration<37) {
+            led2=!led2;
+            led3=!led3;
+            if(emg4_filtered>temp_highest_emg4) {
+                temp_highest_emg4= emg4_filtered;
+                pc.printf("Temp4 = %f \r\n",temp_highest_emg4);
+            }
+        }
+        led1=1;
+        led2=1;
+        led3=1;
+
+
+    }
+
+    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
+    threshold4 = temp_highest_emg4*onderarm_p_t; //Left Triceps
+}
+
+//Check if emg_filtered has reached their threshold
+void threshold_check()
+{
+
+    //EMG1 threshold check
+    if(emg1_filtered>threshold1) {
+        Duim = 1;
+        led1 = !led1;
+    } else {
+        Duim= 0;
+    }
+    //EMG2 threshold check
+    if(emg2_filtered>threshold2) {
+        Bicep = 1;
+        led2 = !led2;
+    } else {
+        Bicep= 0;
+    }
+    //EMG3 threshold check
+    if(emg3_filtered>threshold3) {
+        Tricep = 1;
+        led3 = !led3;
+    } else {
+        Tricep= 0;
+    }
+    //EMG4 threshold check
+    if(emg4_filtered>threshold4) {
+        Onderarm = 1;
+        led1 = !led1;
+        led2 = !led1;
+        led3 = !led1;
+    } else {
+        Onderarm= 0;
+    }
+
+    
+
+}
+
+
+Ticker      sample_timer;
+
+
+
+void sample()
+{
+pc.printf("Duim Right = %i\r\n", Duim);
+pc.printf("Bicep Right = %i\r\n",Bicep);
+pc.printf("tricep Left = %i\r\n", Tricep);
+pc.printf("onderarm Left = %i\r\n", Onderarm);
+wait(0.01);             
+
+    
+}
+
+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 );
+    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);
+
+    
+    
+                temp_highest_emg1 = 0; //highest detected value right biceps
+                temp_highest_emg2 = 0;
+                temp_highest_emg3 = 0;
+                temp_highest_emg4 = 0;
+
+                timer_calibration.reset();
+                timer_calibration.start();
+
+                
+                CalibrationEMG();
+                threshold_check_ticker.attach(threshold_check, 0.1);
+                sample_timer.attach(&sample, 0.1);
+                //sample_ticker.detach();
+                timer_calibration.stop();
+               
+    
+    
+
+    /*empty loop, sample() is executed periodically*/
+    while(1) {
+        wait(0.01);
+        }
+}
\ No newline at end of file