Diff: main.cpp

Revision:

19:94dc52f8a59e

Parent:

18:9f24792bb39a

Child:

20:0e9218673aa8

diff -r 9f24792bb39a -r 94dc52f8a59e main.cpp
--- a/main.cpp	Fri Oct 25 07:52:41 2019 +0000
+++ b/main.cpp	Fri Oct 25 08:29:50 2019 +0000
@@ -76,18 +76,34 @@
 int trim_cal_i = trim_cal * Fs - 1; // Determine iterator of transient behaviour trim
 
 // Notch biquad filter coefficients (iirnotch Q factor 35 @50Hz) from MATLAB:
-BiQuad bq_notch( 0.995636295063941,  -1.89829218816065,   0.995636295063941,  1, -1.89829218816065,   0.991272590127882); // b01 b11 b21 a01 a11 a21
-BiQuadChain bqc_notch;
+BiQuad bq1_notch( 0.995636295063941,  -1.89829218816065,   0.995636295063941,  1, -1.89829218816065,   0.991272590127882); // b01 b11 b21 a01 a11 a21
+BiQuad bq2_notch = bq1_notch;
+BiQuad bq3_notch = bq1_notch;
+BiQuadChain bqc1_notch;
+BiQuadChain bqc2_notch;
+BiQuadChain bqc3_notch;
 
 // Highpass biquad filter coefficients (butter 4th order @10Hz cutoff) from MATLAB
-BiQuad bq_H1(0.922946103200875, -1.84589220640175,  0.922946103200875,  1,  -1.88920703055163,  0.892769008131025); // b01 b11 b21 a01 a11 a21
-BiQuad bq_H2(1,                 -2,                 1,                  1,  -1.95046575793011,  0.954143234875078); // b02 b12 b22 a02 a12 a22
-BiQuadChain bqc_high;
+BiQuad bq1_H1(0.922946103200875, -1.84589220640175,  0.922946103200875,  1,  -1.88920703055163,  0.892769008131025); // b01 b11 b21 a01 a11 a21
+BiQuad bq1_H2(1,                 -2,                 1,                  1,  -1.95046575793011,  0.954143234875078); // b02 b12 b22 a02 a12 a22
+BiQuad bq2_H1 = bq1_H1;
+BiQuad bq2_H2 = bq1_H2;
+BiQuad bq3_H1 = bq1_H1;
+BiQuad bq3_H2 = bq1_H2;
+BiQuadChain bqc1_high;  
+BiQuadChain bqc2_high;
+BiQuadChain bqc3_high;
 
 // Lowpass biquad filter coefficients (butter 4th order @5Hz cutoff) from MATLAB:
-BiQuad bq_L1(5.32116245737504e-08,  1.06423249147501e-07,   5.32116245737504e-08,   1,  -1.94396715039462,  0.944882378004138); // b01 b11 b21 a01 a11 a21
-BiQuad bq_L2(1,                     2,                      1,                      1,  -1.97586467534468,  0.976794920438162); // b02 b12 b22 a02 a12 a22
-BiQuadChain bqc_low;
+BiQuad bq1_L1(5.32116245737504e-08,  1.06423249147501e-07,   5.32116245737504e-08,   1,  -1.94396715039462,  0.944882378004138); // b01 b11 b21 a01 a11 a21
+BiQuad bq1_L2(1,                     2,                      1,                      1,  -1.97586467534468,  0.976794920438162); // b02 b12 b22 a02 a12 a22
+BiQuad bq2_L1 = bq1_L1;
+BiQuad bq2_L2 = bq1_L2;
+BiQuad bq3_L1 = bq1_L1;
+BiQuad bq3_L2 = bq1_L2;
+BiQuadChain bqc1_low;
+BiQuadChain bqc2_low;
+BiQuadChain bqc3_low;
 
 /*
 ------ HELPER FUNCTIONS ------
@@ -121,9 +137,9 @@
 // Check filter stability
 bool checkBQChainStable()
 {
-    bool n_stable = bqc_notch.stable();
-    bool hp_stable =  bqc_high.stable();
-    bool l_stable = bqc_low.stable();
+    bool n_stable = bqc1_notch.stable();
+    bool hp_stable =  bqc1_high.stable();
+    bool l_stable = bqc1_low.stable();
 
     if (n_stable && hp_stable && l_stable) {
         return true;
@@ -148,13 +164,13 @@
     scope.set(0, emg1 );
 
     // Filter notch and highpass
-    double emg1_n_hp = bqc_notch_high.step( emg1 );
+    double emg1_n_hp = bqc1_notch_high.step( emg1 );
 
     // Rectify
     double emg1_rectify = fabs( emg1_n_hp );
 
     // Filter lowpass (completes envelope)
-    double emg1_env = bqc_low.step( emg1_rectify );
+    double emg1_env = bqc1_low.step( emg1_rectify );
 
     // Output EMG after filters
     scope.set(1, emg1_env );
@@ -170,21 +186,21 @@
     emg3 = emg3_in.read();
 
     // Output raw EMG input
-    scope.set(0, emg1 );
-    scope.set(1, emg2 );
+    //scope.set(0, emg1 );
+    // scope.set(1, emg2 );
     
-    double emg1_n = bqc_notch.step( emg1 );         // Filter notch
-    double emg1_hp = bqc_high.step( emg1_n );       // Filter highpass
+    double emg1_n = bqc1_notch.step( emg1 );         // Filter notch
+    double emg1_hp = bqc1_high.step( emg1_n );       // Filter highpass
     double emg1_rectify = fabs( emg1_hp );          // Rectify
-    double emg1_env = bqc_low.step( emg1_rectify ); // Filter lowpass (completes envelope)
+    double emg1_env = bqc1_low.step( emg1_rectify ); // Filter lowpass (completes envelope)
     
-    double emg2_n = bqc_notch.step( emg2 );         // Filter notch
-    double emg2_hp = bqc_high.step( emg2_n );       // Filter highpass
+    double emg2_n = bqc2_notch.step( emg2 );         // Filter notch
+    double emg2_hp = bqc2_high.step( emg2_n );       // Filter highpass
     double emg2_rectify = fabs( emg2_hp );          // Rectify
-    double emg2_env = bqc_low.step( emg2_rectify ); // Filter lowpass (completes envelope)
+    double emg2_env = bqc2_low.step( emg2_rectify ); // Filter lowpass (completes envelope)
     
-    // scope.set(1, emg1_n);
-    // scope.set(1, emg1_n);
+    scope.set(0, emg1_n);
+    scope.set(1, emg2_n);
     // scope.set(3, emg1_rectify);
 
     // Output EMG after filters
@@ -208,6 +224,9 @@
     emg1_MVC = getMean(emg1_cal); // Store MVC globally
     emg1_MVC_stdev = getStdev(emg1_cal, emg1_MVC); // Store MVC stde globally
     emg1_cal.clear(); // Empty vector to prevent memory overflow
+    emg2_MVC = getMean(emg2_cal); // Store MVC globally
+    emg2_MVC_stdev = getStdev(emg2_cal, emg2_MVC); // Store MVC stde globally
+    emg2_cal.clear(); // Empty vector to prevent memory overflow
     led_b = 1; // Turn off calibration led
 }
 
@@ -226,6 +245,9 @@
     emg1_rest = getMean(emg1_cal); // Store rest globally
     emg1_rest_stdev = getStdev(emg1_cal, emg1_rest);// Store rest stdev globally
     emg1_cal.clear(); // Empty vector to prevent memory overflow
+    emg2_rest = getMean(emg2_cal); // Store rest globally
+    emg2_rest_stdev = getStdev(emg2_cal, emg2_rest);// Store rest stdev globally
+    emg2_cal.clear(); // Empty vector to prevent memory overflow
     led_b = 1; // Turn off calibration led
 }
 
@@ -261,9 +283,17 @@
     // tickSample.attach(&sample, Ts); // Initialize sample ticker
 
     // Create BQ chains to reduce computations
-    bqc_notch.add( &bq_notch );
-    bqc_high.add( &bq_H1 ).add( &bq_H2 );
-    bqc_low.add( &bq_L1 ).add( &bq_L2 );
+    bqc1_notch.add( &bq1_notch );
+    bqc1_high.add( &bq1_H1 ).add( &bq1_H2 );
+    bqc1_low.add( &bq1_L1 ).add( &bq1_L2 );
+    
+    bqc2_notch.add( &bq2_notch );
+    bqc2_high.add( &bq2_H1 ).add( &bq2_H2 );
+    bqc2_low.add( &bq2_L1 ).add( &bq2_L2 );
+    
+    bqc3_notch.add( &bq3_notch );
+    bqc3_high.add( &bq3_H1 ).add( &bq3_H2 );
+    bqc3_low.add( &bq3_L1 ).add( &bq3_L2 );
 
     led_b = 1; // Turn blue led off at startup
     led_g = 1; // Turn green led off at startup
@@ -285,6 +315,6 @@
         // Show that system is running
         // led_g = !led_g;
         pc.printf("EMG MVC:  %f   stdev: %f\r\nEMG Rest: %f   stdev: %f\r\n", emg1_MVC, emg1_MVC_stdev, emg1_rest, emg1_rest_stdev);
-        wait(0.5);
+        wait(1.0f);
     }
 }
\ No newline at end of file