Diff: main.cpp

Revision:

22:ad85b8acf8b5

Parent:

21:2b55d53e11f6

--- a/main.cpp	Mon Oct 24 13:33:09 2016 +0000
+++ b/main.cpp	Mon Oct 24 14:45:10 2016 +0000
@@ -5,13 +5,16 @@
 //Define objects
 AnalogIn    emg0( A0 );
 AnalogIn    emg1( A1 );
-
+DigitalIn   buttonCalibrate(SW2);
 MODSERIAL pc(USBTX, USBRX);
 
 volatile float x;
 volatile float x_prev =0;
 volatile float b; // filtered 'output' of ReadAnalogInAndFilter
 
+bool calibrate = false;
+double threshold_Left = 0;
+double threshold_Right= 0;
 Ticker      sample_timer;
 HIDScope    scope( 2 );
 DigitalOut  led(LED1);
@@ -29,8 +32,10 @@
 double v2_high = 0;
 double v1_low = 0;
 double v2_low = 0;
-double highpassFilter = 0;
-double lowpassFilter = 0;
+double highpassFilterLeft = 0;
+double lowpassFilterLeft = 0;
+double highpassFilterRight = 0;
+double lowpassFilterRight = 0;
 
 double biquad1(double u, double&v1, double&v2, const double a1, const double a2, const double b0,
                const double b1, const double b2)
@@ -41,7 +46,7 @@
     v1 = v;
     return y;
 }
-double biquad2(double u, double&v1, double&v2, const double c1, const double c2, const double d0,
+/*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;
@@ -50,28 +55,27 @@
     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()
+void filterSampleLeft()
 {
-    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 );
+    highpassFilterLeft = fabs(biquad1(emg0.read(), v1_high, v2_high, a1, a2, b0, b1, b2));
+    lowpassFilterLeft = biquad1(highpassFilterLeft, v1_low, v2_low, c1, c2, d0, d1, d2);
+    scope.set(0, lowpassFilterLeft );
     scope.send();
-    pc.printf("%f \n \r ", lowpassFilter);
+    //pc.printf("%f \n \r ", lowpassFilter);
 }
-
+void filterSampleRight()
+{
+    highpassFilterRight = fabs(biquad1(emg1.read(), v1_high, v2_high, a1, a2, b0, b1, b2));
+    lowpassFilterRight = biquad1(highpassFilterRight, v1_low, v2_low, c1, c2, d0, d1, d2);
+    scope.set(1, lowpassFilterRight );
+    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' */
@@ -96,14 +100,20 @@
     /**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(&filterSample, 0.002);
+    //sample_timer.attach(&sample, 0.001953125);
+    sample_timer.attach(&filterSampleLeft, 0.001953125);        //512 Hz
+    sample_timer.attach(&filterSampleRight, 0.001953125);
     pc.baud(115200);
-    pc.printf("hoi\n");
-    /*empty loop, sample() is executed periodically*/
-    while(1) {
-        
+    pc.printf("please push the button to calibrate \n \r");
+    while (1) {
+        if (buttonCalibrate == 0) {
+            calibrate = true;
+            threshold_Left = lowpassFilterLeft*0.7;
+            threshold_Right = lowpassFilterRight*0.7;
         }
+        if (calibrate == true) {
+            pc.printf("calibration complete, left = %f, right = %f \n \r", threshold_Left, threshold_Right);
+            /*empty loop, sample() is executed periodically*/
+        }
+    }
 }
\ No newline at end of file