Diff: main.cpp

Revision:

21:2b55d53e11f6

Parent:

20:97059009a491

Child:

22:ad85b8acf8b5

--- a/main.cpp	Thu Sep 22 08:53:50 2016 +0000
+++ b/main.cpp	Mon Oct 24 13:33:09 2016 +0000
@@ -1,37 +1,109 @@
 #include "mbed.h"
 #include "HIDScope.h"
+#include "MODSERIAL.h"
 
 //Define objects
 AnalogIn    emg0( A0 );
 AnalogIn    emg1( A1 );
 
+MODSERIAL pc(USBTX, USBRX);
+
+volatile float x;
+volatile float x_prev =0;
+volatile float b; // filtered 'output' of ReadAnalogInAndFilter
+
 Ticker      sample_timer;
 HIDScope    scope( 2 );
 DigitalOut  led(LED1);
+const double a1 = -1.6475;
+const double a2 = 0.7009;
+const double b0 = 0.8371;
+const double b1 = -1.6742;
+const double b2 = 0.8371;
+const double c1 = -1.9645;
+const double c2 = 0.9651;
+const double d0 = 0.0001551;
+const double d1 = 0.0003103;
+const double d2 = 0.0001551;
+double v1_high = 0;
+double v2_high = 0;
+double v1_low = 0;
+double v2_low = 0;
+double highpassFilter = 0;
+double lowpassFilter = 0;
 
+double biquad1(double u, double&v1, double&v2, const double a1, const double a2, const double b0,
+               const double b1, const double b2)
+{
+    double v = u - a1*v1 - a2*v2;
+    double y = b0*v + b1*v1 + b2*v2;
+    v2 = v1;
+    v1 = v;
+    return y;
+}
+double biquad2(double u, double&v1, double&v2, const double c1, const double c2, const double d0,
+               const double d1, const double d2)
+{
+    double v = u - c1*v1 - c2*v2;
+    double y = d0*v + d1*v1 + d2*v2;
+    v2 = v1;
+    v1 = v;
+    return y;
+}
+double biquad3(double u, double&v1, double&v2, const double c1, const double c2, const double d0,
+               const double d1, const double d2)
+{
+    double v = u - c1*v1 - c2*v2;
+    double y = d0*v + d1*v1 + d2*v2;
+    v2 = v1;
+    v1 = v;
+    return y;
+}
 /** Sample function
  * this function samples the emg and sends it to HIDScope
  **/
+
+void filterSample()
+{
+    highpassFilter = fabs(biquad1(emg0.read(), v1_high, v2_high, a1, a2, b0, b1, b2));
+    lowpassFilter = biquad2(highpassFilter, v1_low, v2_low, c1, c2, d0, d1, d2);
+    scope.set(0, lowpassFilter );
+    scope.send();
+    pc.printf("%f \n \r ", lowpassFilter);
+}
+
 void sample()
 {
     /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
     scope.set(0, emg0.read() );
     scope.set(1, emg1.read() );
-    /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) 
+    /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
     *  Ensure that enough channels are available (HIDScope scope( 2 ))
     *  Finally, send all channels to the PC at once */
+
+    x = emg0;   // Capture data        scope.set(0, x);   // store data in first element of scope memory
+    b = (x_prev + x)/2.0;   // averaging filter
+    x_prev = x; // Prepare for next round
+
     scope.send();
     /* To indicate that the function is working, the LED is toggled */
     led = !led;
+    pc.printf("%f, %f \n \r ", x, b);
 }
 
 int main()
-{   
+{
     /**Attach the 'sample' function to the timer 'sample_timer'.
     * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
     */
-    sample_timer.attach(&sample, 0.002);
-
+    
+    
+    //sample_timer.attach(&sample, 0.002);
+    sample_timer.attach(&filterSample, 0.002);
+    pc.baud(115200);
+    pc.printf("hoi\n");
     /*empty loop, sample() is executed periodically*/
-    while(1) {}
+    while(1) {
+        
+        }
 }
\ No newline at end of file