Diff: main.cpp

Revision:

6:5437cc97e1e6

Parent:

5:3d65f89e3755

Child:

7:7a088536f1c9

--- a/main.cpp	Sun Oct 20 19:40:57 2019 +0000
+++ b/main.cpp	Sun Oct 20 19:53:00 2019 +0000
@@ -19,7 +19,8 @@
 AnalogIn emg3_in (A3); //emg van een derde (nog te bepalen) spier, voor het vernaderen van de richting
 
 // LED
-DigitalOut      led(LED_GREEN);
+DigitalOut      led_g(LED_GREEN);
+DigitalOut      led_r(LED_RED);
 
 //variablen voor EMG
 double emg1;
@@ -62,6 +63,21 @@
 BiQuad bq_L2(1,                     2,                      1,                      1,  -1.97586467534468,  0.976794920438162);
 BiQuadChain bqc_low; // Used to chain two 2nd other filters into a 4th order filter
 
+// Check if filters are stable
+bool checkBQChainStable()
+{
+    bool n_hp_stable =  bqc_notch_high.stable();
+    bool l_stable = bqc_low.stable();
+
+    if (n_hp_stable && l_stable) {
+        return true;
+    } else {
+        return false;
+    }
+}
+
+
+// Read samples, filter samples and output to HIDScope
 void sample()
 {
     // Read EMG inputs
@@ -71,13 +87,13 @@
 
     // Output raw EMG input
     scope.set(0, emg1 );
-    
+
     // Filter notch and highpass
     double emg1_n_hp = bqc_notch_high.step( emg1 );
-    
+
     // Rectify
     double emg1_rectify = fabs( emg1_n_hp );
-    
+
     // Filter lowpass (completes envelope)
     double emg1_env = bqc_low.step( emg1_rectify );
 
@@ -86,38 +102,27 @@
     scope.send();
 }
 
-/*
-//notch filter toepassen
-notch1 = N1.step(emg1);
-notch2 = N2.step(emg2);
-notch3 = N3.step(emg3);
-
-//high pass filter
-high1 = H1.step(notch1);
-high2 = H2.step(notch2);
-high3 = H3.step(notch3);
-
-//rectify toepassen, oftewel absolute waarde van EMG signaal nemen
-absolute1 = fabs(high1);
-absolute2 = fabs(high2);
-absolute3 = fabs(high3);
-
-//low pass filter
-low1 = L1.step(absolute1);
-low2 = L2.step(absolute2);
-low3 = L3.step(absolute3);
-*/
-
 void main()
 {
+    // Initialize sample ticker
     const double Ts = 1/Fs;
     tickSample.attach(&sample, Ts);
-    
+
+    // Create BQ chains to reduce computations
     bqc_notch_high.add( &bq_notch ).add( &bq_H1 ).add( &bq_H2 );
     bqc_low.add( &bq_L1 ).add( &bq_L2 );
 
+    // If any filter chain is unstable, red led will light up
+    if (checkBQChainStable) {
+        led_r = 1; // LED off
+    } else {
+        led_r = 0; // LED on
+    }
+
     while(true) {
-        led = !led;
+        
+        // Show that system is running
+        led_g = !led_g;
         wait(0.5);
     }
 }
\ No newline at end of file