Diff: main.cpp

Revision:

8:dc0858fead9f

Parent:

7:a928724ef731

--- a/main.cpp	Thu Oct 27 09:57:26 2016 +0000
+++ b/main.cpp	Thu Oct 27 10:13:44 2016 +0000
@@ -12,21 +12,20 @@
 
 //Notch filter
 BiQuadChain notch_50;
-BiQuad bq3( 0.98116526140, 0.00000000044, 0.98116526140, 0.00000000043, 0.95391787621);
-BiQuad bq4( 0.97224232015, 0.00000000043, 0.97224232015, -0.04036799459, 0.97670000725);
-BiQuad bq5( 1.00000000000, 0.00000000044, 1.00000000000, 0.04036799547, 0.9767000072);
+BiQuad bq1( 0.98116526140, 0.00000000044, 0.98116526140, 0.00000000043, 0.95391787621);
+BiQuad bq2( 0.97224232015, 0.00000000043, 0.97224232015, -0.04036799459, 0.97670000725);
+BiQuad bq3( 1.00000000000, 0.00000000044, 1.00000000000, 0.04036799547, 0.9767000072);
 
 //High pass filter
 BiQuadChain high_pass;
-BiQuad bq6( 0.80254782780,-1.60509565560, 0.80254782780, -1.58011656361, 0.63006219630);
-BiQuad bq7( 0.90006571973,-1.80013143945, 0.900065719734, -1.77213098592, 0.8281459694);
-//BiQuad bq8( 0.92490714701,-1.84981429401, 0.92490714701, -1.90032503529, 0.9352152620);
+BiQuad bq4( 0.80254782780,-1.60509565560, 0.80254782780, -1.58011656361, 0.63006219630);
+BiQuad bq5( 0.90006571973,-1.80013143945, 0.900065719734, -1.77213098592, 0.8281459694);
 
 //Low pass filter
 BiQuadChain low_pass;
-BiQuad bq9( 0.00801840797, 0.01603681594, 0.00801840797,-1.65212256130, 0.68416767240);
-BiQuad bq10( 0.00836524486, 0.01673048973, 0.00836524486,-1.72511837232, 0.75857933411);
-BiQuad bq11( 0.00905039996, 0.01810079992, 0.00905039996,-1.86807725180, 0.9043110909);
+BiQuad bq6( 0.00801840797, 0.01603681594, 0.00801840797,-1.65212256130, 0.68416767240);
+BiQuad bq7( 0.00836524486, 0.01673048973, 0.00836524486,-1.72511837232, 0.75857933411);
+BiQuad bq8( 0.00905039996, 0.01810079992, 0.00905039996,-1.86807725180, 0.9043110909);
 //Ticker
 Ticker emgSampleTicker;
 
@@ -45,27 +44,12 @@
 AnalogIn emg2( A2 );
 
 bool go_emgSample;
-bool go_find_minmax;
 double emg_sample[3];
-double emg_low_passed_200[3];
 double emg_notch[3];
 double emg_high_passed[3];
 double emg_low_passed[3];
 double min_emg[3];
 double max_emg[3];
-
-const int n = 100;
-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;
@@ -91,9 +75,9 @@
 
 int main() {
     //combine biquads in biquad chains for notch/high- low-pass filters
-    notch_50.add( &bq3 ).add( &bq4 ).add( &bq5 );
-    high_pass.add( &bq6 ).add( &bq7 );
-    low_pass.add( &bq9 ).add( &bq10 ).add( &bq11 );
+    notch_50.add( &bq1 ).add( &bq2 ).add( &bq3 );
+    high_pass.add( &bq4 ).add( &bq5 );
+    low_pass.add( &bq6 ).add( &bq7 ).add( &bq8 );
     led.write(1);
     
     change_state.attach( &calibrate,5);
@@ -116,86 +100,68 @@
             
             //filter out the 50Hz components with a notch filter
             emg_notch[0] = notch_50.step(emg_sample[0]);
+            emg_notch[1] = notch_50.step(emg_sample[1]);
+            emg_notch[2] = notch_50.step(emg_sample[2]);
             
             //high pass the signal (removing motion artifacts and offset)
             emg_high_passed[0] = high_pass.step(emg_notch[0]);
+            emg_high_passed[1] = high_pass.step(emg_notch[1]);
+            emg_high_passed[2] = high_pass.step(emg_notch[2]);
                 
             //low pass the rectified emg signal
             emg_low_passed[0] = low_pass.step(fabs(emg_high_passed[0]));
-            
-            //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);
+            emg_low_passed[1] = low_pass.step(fabs(emg_high_passed[1]));
+            emg_low_passed[2] = low_pass.step(fabs(emg_high_passed[2]));
             
             //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;
+                    min_emg[0] = emg_low_passed[0];
+                    max_emg[0] = emg_low_passed[0];
+                    min_emg[1] = emg_low_passed[1];
+                    max_emg[1] = emg_low_passed[1];
+                    min_emg[2] = emg_low_passed[2];
+                    max_emg[2] = emg_low_passed[2];
                     start_calibration++;
                 }
                 else {
                     //finding min and max of emg0
-                    if (RMS0 < min_emg[0]) {
-                        min_emg[0] = RMS0;
+                    if (emg_low_passed[0] < min_emg[0]) {
+                        min_emg[0] = emg_low_passed[0];
                     }
-                    else if (RMS0 > max_emg[0]) {
-                        max_emg[0] = RMS0;
+                    else if (emg_low_passed[0] > max_emg[0]) {
+                        max_emg[0] = emg_low_passed[0];
                     }
                     
                     //finding min and max of emg1
-                    if (RMS1 < min_emg[1]) {
-                        min_emg[1] = RMS1;
+                    if (emg_low_passed[0] < min_emg[1]) {
+                        min_emg[1] = emg_low_passed[0];
                     }
-                    else if (RMS1 > max_emg[1]) {
-                        max_emg[1] = RMS1;
+                    else if (emg_low_passed[0] > max_emg[1]) {
+                        max_emg[1] = emg_low_passed[0];
                     }
                     
                     //finding min and max of emg2
-                    if (RMS2 < min_emg[2]) {
-                        min_emg[2] = RMS2;
+                    if (emg_low_passed[2] < min_emg[2]) {
+                        min_emg[2] = emg_low_passed[2];
                     }
-                    else if (RMS2 > max_emg[2]) {
-                        max_emg[2] = RMS2;
+                    else if (emg_low_passed[2] > max_emg[2]) {
+                        max_emg[2] = emg_low_passed[2];
                     }
                 }   
             } 
             
             //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]);
+            input_force0 = (emg_low_passed[0] - min_emg[0])/(max_emg[0]-min_emg[0]);
+            input_force1 = (emg_low_passed[1] - min_emg[1])/(max_emg[1]-min_emg[1]);
+            input_force2 = (emg_low_passed[2] - min_emg[2])/(max_emg[2]-min_emg[2]);
             
             //Send scope data
             scope.set(0,emg_sample[0]);
             scope.set(1,emg_notch[0]);
             scope.set(2,emg_high_passed[0]);
-            scope.set(3,emg_low_passed[0]);
-            //scope.set(2,emg_low_passed[0]);
-            //scope.set(3,input_force2);
+            scope.set(3,input_force0);
             scope.send();
 
             go_emgSample = false;