Diff: main.cpp

Revision:

0:ae0bec143f2d

Child:

1:7fb4a74d33ff

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 24 11:16:02 2016 +0000
@@ -0,0 +1,187 @@
+#include "mbed.h"
+#include "BiQuad.h"
+#include "HIDScope.h"
+//Enum with states
+enum states {STATE_DEFAULT , STATE_CALIBRATION, STATE_RUN};
+
+//Variable called 'state'
+states state = STATE_DEFAULT;
+
+//Creating two scope channels
+HIDScope scope(2);
+
+//Notch filter
+BiQuadChain notch_50;
+BiQuad bq1( 1.00000000000, -1.60956348896,  1.00000000000, -1.40195621505,  0.74203282402);
+BiQuad bq2( 1.00000000000, -1.60724786352,  1.00000000000, -1.33646101015,  0.85967899264);
+BiQuad bq3( 1.00000000000, -1.61186693071,  1.00000000000, -1.64415455961,  0.89726621230);
+
+//High pass filter
+BiQuadChain high_pass;
+BiQuad bq4( 1.00000000000, -1.99999967822,  1.00000000000, -1.98388291862,  0.98395921205);
+BiQuad bq5( 1.00000000000, -1.99999812453,  1.00000000000, -1.99324612474,  0.99332432675);
+
+//Ticker
+Ticker emgSampleTicker;
+
+//Timeout to change state after 5 seconds
+Timeout change_state;
+
+//Timeout to change state after 10 seconds
+Timeout change_state2;
+
+//Emg input
+AnalogIn emg0( A0 );
+AnalogIn emg1( A1 );
+AnalogIn emg2( A2 );
+
+bool go_emgSample;
+bool go_find_minmax;
+double emg_sample[3];
+double emg_notch[3];
+double emg_high_passed[3];
+double emg_rectified;
+double min_emg[3];
+double max_emg[3];
+
+const int n = 200;
+int counter = 0;
+double RMSArray0[n] = {0};
+double RMSArray1[n] = {0};
+double RMSArray2[n] = {0};
+double RMS0;
+double RMS1;
+double RMS2;
+double SumRMS0;
+double SumRMS1;
+double SumRMS2;
+
+double input_force0;
+double input_force1;
+double input_force2;
+
+//count for emg min max
+int start_calibration = 0;
+
+void emgSample() {
+    go_emgSample = true;
+}
+
+void calibrate() {
+    state = STATE_CALIBRATION;  
+}
+
+void run() {
+    state = STATE_RUN;  
+}
+
+void EMG_filter();
+
+int main() {
+    //combine biquads in biquad chains for notch/high- low-pass filters
+    notch_50.add( &bq1 ).add( &bq2 ).add( &bq3 );
+    high_pass.add( &bq4 ).add( &bq5 );
+    
+    change_state.attach( &calibrate,5);
+    change_state2.attach( &run,10);
+    emgSampleTicker.attach( &emgSample, 0.002);
+    while( true ){
+        if(go_emgSample == true){
+            EMG_filter();
+        }
+   }     
+}
+
+
+void EMG_filter() {
+    if(go_emgSample == true){
+            //read the emg signal
+            emg_sample[0] = emg0.read();
+            emg_sample[1] = emg1.read();
+            emg_sample[2] = emg2.read();
+            
+            for (int i = 0; i < 3; i++){
+                //filter out the 50Hz components with a notch filter
+                //emg_notch[i] = notch_50.step(emg_sample[i]);
+            
+                //high pass the signal (removing motion artifacts and offset)
+                emg_high_passed[i] = high_pass.step(emg_sample[i]);
+            }
+            
+            //Calculating RMS
+            SumRMS0 -= pow(RMSArray0[counter],2);
+            SumRMS1 -= pow(RMSArray1[counter],2);
+            SumRMS2 -= pow(RMSArray2[counter],2);
+            
+            RMSArray0[counter] = emg_high_passed[0];
+            RMSArray1[counter] = emg_high_passed[1];
+            RMSArray2[counter] = emg_high_passed[2];
+            
+            SumRMS0 += pow(RMSArray0[counter],2);
+            SumRMS1 += pow(RMSArray1[counter],2);
+            SumRMS2 += pow(RMSArray2[counter],2);
+            
+            counter++;
+            if (counter == n){
+                counter = 0;
+            }
+            
+            RMS0 = sqrt(SumRMS0/n);
+            RMS1 = sqrt(SumRMS1/n);
+            RMS2 = sqrt(SumRMS2/n);
+            
+            //Calculating min value and max value of emg signal
+            if(state == STATE_CALIBRATION)
+            {
+                if (start_calibration == 0) {
+                    min_emg[0] = RMS0;
+                    max_emg[0] = RMS0;
+                    min_emg[1] = RMS1;
+                    max_emg[1] = RMS1;
+                    min_emg[2] = RMS2;
+                    max_emg[2] = RMS2;
+                    start_calibration++;
+                }
+                else {
+                    //finding min and max of emg0
+                    if (RMS0 < min_emg[0]) {
+                        min_emg[0] = RMS0;
+                    }
+                    else if (RMS0 > max_emg[0]) {
+                        max_emg[0] = RMS0;
+                    }
+                    
+                    //finding min and max of emg1
+                    if (RMS1 < min_emg[1]) {
+                        min_emg[1] = RMS1;
+                    }
+                    else if (RMS1 > max_emg[1]) {
+                        max_emg[1] = RMS1;
+                    }
+                    
+                    //finding min and max of emg2
+                    if (RMS2 < min_emg[2]) {
+                        min_emg[2] = RMS2;
+                    }
+                    else if (RMS2 > max_emg[2]) {
+                        max_emg[2] = RMS2;
+                    }
+                }   
+            } 
+            
+            //calculating input_forces for controller
+            input_force0 = (RMS0 - min_emg[0])/(max_emg[0]-min_emg[0]);
+            input_force1 = (RMS1 - min_emg[1])/(max_emg[1]-min_emg[1]);
+            input_force2 = (RMS2 - min_emg[2])/(max_emg[2]-min_emg[2]);
+            
+            //Send scope data
+            scope.set(0,emg_sample[0]);
+            scope.set(1,input_force0);
+            //scope.set(2,input_force1);
+            //scope.set(3,input_force2);
+            scope.send();
+
+            go_emgSample = false;
+        }   
+    }
+