Diff: main.cpp

Revision:

5:0e197bf776fc

Parent:

4:2930e35b8e8e

Child:

6:ee304066e301

--- a/main.cpp	Thu Sep 19 15:05:38 2019 +0000
+++ b/main.cpp	Tue Oct 29 17:52:48 2019 +0000
@@ -4,22 +4,130 @@
 #include "MODSERIAL.h"
 #include "BiQuad.h"
 #include "FastPWM.h"
+#include "math.h"
+#include <stdio.h>      /* printf */
+#include <stdlib.h>     /* abs */
+#include <complex>
 
-DigitalOut led(LED_BLUE);
+
 MODSERIAL pc(USBTX, USBRX);
 
-int main()
-{
-    pc.baud(115200);
-    pc.printf("\r\nStarting...\r\n\r\n");
-    
-    while (true) {
-        led = !led;
-        wait_ms(500);
-    char c;
-    c = pc.getc();
-    pc.putc(c); 
-   }
-}
+//Filters EMG
+//Filters Linker Biceps
+//In de volgorde High pass, notch, low pass
+BiQuad LBHP1(1, 2.000000023182220, 0.999999981993818, 1.893415601022358, 0.896255659548714); 
+BiQuad LBHP2(1, 1.999999976817780, 0.999999976817780, 1.778313488139574, 0.790599715524785);
+
+BiQuad LBN1( 0.5, 0, 0.5, 0, 0);
+
+BiQuad LBLP1( 0.000155148423475721, 0.000310296846951441, 0.000155148423475721,-1.96446058020523, 0.965081173899135);
+BiQuad LBLP2( 0.000155148423475721, 0.000310296846951441, 0.000155148423475721,-1.96446058020523, 0.965081173899135);
+BiQuadChain LeftBicep;
+
+
+
+//Filters Rechter Biceps
+//In de volgorde High pass, notch, low pass
+BiQuad RBHP1(1, 2.000000023182220, 0.999999981993818, 1.893415601022358, 0.896255659548714); 
+BiQuad RBHP2(1, 1.999999976817780, 0.999999976817780, 1.778313488139574, 0.790599715524785);
+
+BiQuad RBN1( 0.5, 0, 0.5, 0, 0);
+
+BiQuad RBLP1( 0.000155148423475721, 0.000310296846951441, 0.000155148423475721,-1.96446058020523, 0.965081173899135);
+BiQuad RBLP2( 0.000155148423475721, 0.000310296846951441, 0.000155148423475721,-1.96446058020523, 0.965081173899135);
+BiQuadChain RightBicep;
 
 
+//Filters Rechter Quadriceps
+//In de volgorde High pass, notch, low pass
+BiQuad RQHP1(1, 2.000000023182220, 0.999999981993818, 1.893415601022358, 0.896255659548714); 
+BiQuad RQHP2(1, 1.999999976817780, 0.999999976817780, 1.778313488139574, 0.790599715524785);
+
+BiQuad RQN1( 0.5, 0, 0.5, 0, 0);
+
+BiQuad RQLP1( 0.000155148423475721, 0.000310296846951441, 0.000155148423475721,-1.96446058020523, 0.965081173899135);
+BiQuad RQLP2( 0.000155148423475721, 0.000310296846951441, 0.000155148423475721,-1.96446058020523, 0.965081173899135);
+BiQuadChain RightLeg;
+//BiQuadChain bqc;
+//bqc.add( &bq1 ).add( &bq2 );
+
+//double emgLBHP;
+//double emgLBNotch;
+//double emgAbsLPHP;
+//double emgLBLP;
+//double emgAbsLBNotch;
+ 
+//double emgRBHP;
+//double emgRBNotch;
+//double emgAbsRBHP;
+//double emgRBLP;
+//double emgAbsRBNotch;
+ 
+//double emgRQNotch;
+//double emgRQHP;
+//double emgAbsRQHP;
+//double emgRQLP;
+//double emgAbsRQNotch;
+
+double emgLBfiltered;
+double emgRBfiltered;
+double emgRQfiltered;
+double emgLBraw;
+double emgRBraw;
+double emgRQraw;
+
+
+AnalogIn emgLB(A0);     //read EMG
+AnalogIn emgRB(A1);     //read EMG
+AnalogIn emgRQ(A2);     //read EMG
+
+HIDScope scope(6);
+
+Ticker filter;
+
+void Filteren() 
+{
+    //linkerbicep
+    /*emgLBHP = LBHP.step(emgLB.read() );     // High-pass filter
+    emgLBNotch = LBN.step(emgLBHP);         // Notch filter
+    emgAbsLBNotch = abs(emgLBNotch);        // Absolute value
+    emgLBLP = LBLP.step(emgAbsLBNotch);     // Low-pass filter
+    */
+    emgLBraw= emgLB.read();
+    emgLBfiltered=LeftBicep.step(emgLB.read());
+    scope.set(0, emgLBraw);
+    scope.set(1, emgLBfiltered);
+
+    //rechterbicep
+    /*emgRBHP = RBHP.step(emgRB.read()); 
+    emgRBNotch = RBN.step(emgRBHP);     
+    emgAbsRBNotch = abs(emgRBNotch);            
+    emgRBLP = RBLP.step(emgAbsRBNotch);    
+    */
+    emgRBraw= emgRB.read();
+    emgRBfiltered=RightBicep.step(emgRB.read());
+    scope.set(2, emgRBraw);
+    scope.set(3, emgRBfiltered);
+
+       
+    /*
+    emgRQHP = RQHP.step(emgRQ.read());     
+    emgRQNotch = RQN.step(emgRQHP);  
+    emgAbsRQNotch = abs(emgRQNotch);            
+    emgRQLP = RQLP.step(emgAbsRQNotch);
+    */
+    emgRQraw= emgRQ.read();
+    emgRQfiltered=RightLeg.step(emgRQ.read());
+    scope.set(4, emgRQ.read());
+    scope.set(5, emgRQfiltered);
+    
+    }
+    
+int main() {
+        LeftBicep.add( &LBHP1 ).add( &LBHP2 ).add( &LBN1 ).add( &LBLP1 ).add( &LBLP2 );
+        RightBicep.add( &RBHP1 ).add( &RBHP2 ).add( &RBN1 ).add( &RBLP1 ).add( &RBLP2 );
+        RightLeg.add( &RQHP1 ).add( &RQHP2 ).add( &RQN1 ).add( &RQLP1 ).add( &RQLP2 );
+        
+
+  //     filter.attach(Filteren, 0.001f); //ticker aanroepen van filter
+    }
\ No newline at end of file