Diff: main.cpp

Revision:

10:39ec51206c8b

Parent:

9:c722418997b5

Child:

11:b95b0e9e1b89

--- a/main.cpp	Mon Oct 15 14:16:33 2018 +0000
+++ b/main.cpp	Fri Oct 19 14:13:59 2018 +0000
@@ -1,8 +1,13 @@
 #include "mbed.h"
 #include "MODSERIAL.h"
+#include "BiQuad.h"
+#include "HIDScope.h"
+#include <math.h>
 
-AnalogIn potmetervalue1     (A1);
-AnalogIn potmetervalue2     (A2);
+AnalogIn emg0_in            (A0);
+AnalogIn emg1_in            (A1);
+AnalogIn emg2_in            (A2);
+
 DigitalIn button2           (D10);                  //Let op, is deze niet bezet?
 InterruptIn encoderA        (D9);
 InterruptIn encoderB        (D8);
@@ -18,9 +23,55 @@
 
 //Global variables
 int encoder = 0;
-   
+
+//Biquad
+BiQuadChain emg0band;
+BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
+BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
+BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
+
+BiQuadChain emg1band;
+BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
+BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
+BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
+
+BiQuadChain emg2band;
+BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
+BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
+BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
+
+BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );                //Notch filter
+
+
+//Tickers
+Ticker emg_filter_tick;
+Ticker Mov_av_tick;
 
 //Functions
+void EMGFilter0()
+{
+    double emg0         = emg0_in.read();
+    double bandpass     = emg0band.step(emg0);
+    double absolute     = fabs(bandpass);
+    double notch0       = notch1.step(absolute);
+}
+
+void EMGFilter1()
+{
+    double emg1         = emg1_in.read();
+    double bandpass     = emg1band.step(emg1);
+    double absolute     = fabs(bandpass);
+    double notch1       = notch1.step(absolute);
+}
+
+void EMGFilter2()
+{
+    double emg2         = emg2_in.read();
+    double bandpass     = emg2band.step(emg2);
+    double absolute     = fabs(bandpass);
+    double notch2       = notch1.step(absolute);
+}
+
 void encoderA_rise()       
 {
     if(encoderB==false)
@@ -76,6 +127,15 @@
 { 
     pc.baud(115200);
     pc.printf("hello\n\r");
+    
+    //EMG signaal filteren 
+        
+        // Deze? of die hierondbqc.add( &bq1 ).add( &bq2 ).add( &bq3 );
+        bqc = bq1 * bq2 * bq3;
+        emgSampleTicker.attach(&emgSample,0.01); //100 Hz
+        while(true){/*do not return from main()*/}
+        
+        
     pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz 
 
     encoderA.rise(&encoderA_rise);
@@ -85,7 +145,7 @@
     
     while (true)
     {
-          
+        //Motor aansturen en encoder uitlezen
           float u1 = potmetervalue1;
           float u2 = potmetervalue2;
           
@@ -97,8 +157,11 @@
         wait(0.01f);                   //zodat de code niet oneindig doorgaat.
         pwmpin2 = fabs(m2*0.6f)+0.4f;    
         directionpin2.write(m2>0);   
+        
+        float encoderDegrees = float(encoder)*(360.0/8400.0);
                 
-        pc.printf("Encoder count: %i \n\r",encoder);   //We moeten de encoder counts nog omzetten naar radialen of graden? 
+        pc.printf("Encoder count: %f \n\r",encoderDegrees);
+        
     }
 }