Diff: main.cpp

Revision:

3:339b19905505

Parent:

2:27081b83a58e

Child:

4:ebb36bf9feb8

--- a/main.cpp	Fri Oct 21 11:58:32 2016 +0000
+++ b/main.cpp	Fri Oct 21 13:22:31 2016 +0000
@@ -1,13 +1,15 @@
 #include "mbed.h"
 #include "BiQuad.h"
-#define SERIAL_BAUD 115200
+#include "HIDScope.h"
+
+//#define SERIAL_BAUD 115200
 
 AnalogIn    emg0( A0 );
 AnalogIn    emg1( A1 );
-Serial pc(USBTX,USBRX);
+//Serial pc(USBTX,USBRX);
 
 Ticker      sample_timer, average_timer, filter_timer, t;
-//HIDScope    scope( 6 );
+HIDScope    scope( 6 );
 DigitalOut  led1(LED_RED);
 DigitalOut  led2(LED_BLUE);
 
@@ -17,23 +19,20 @@
 double EMGright, EMGleft, inR;
 double averageEMGr =0;
 double averageEMGl =0;
-//average_timer_go=false,
- // Activates go-flags
-//void average_timer_act(){average_timer_go=true;};
+
 void filter_timer_act(){filter_timer_go=true;};
 
-
-
-BiQuadChain bcq;
+BiQuadChain bcq1;
+BiQuadChain bcq2;
 // Notch filter wo=50; bw=wo/35
-BiQuad bq1(9.7805e-01,-1.1978e-16,9.7805e-01,1.0000e+00,-1.1978e-16,9.5610e-01);
+BiQuad bq1(9.9821e-01,-1.9807e+00,9.9821e-01,-1.9807e+00,9.9642e-01);
 // High pass Butterworth filter 2nd order, Fc=10;
-BiQuad bq2(9.8239e-01,-1.9648e+00,9.8239e-01,1.0000e+00,-1.9645e+00,9.6508e-01);
+BiQuad bq2(9.8239e-01,-1.9648e+00,9.8239e-01,-1.9645e+00,9.6508e-01);
 // Low pass Butterworth filter 2nd order, Fc = 8;
-BiQuad bq3(5.6248e-05,1.1250e-04,5.6248e-05,1.0000e+00,-1.9787e+00,9.7890e-01);
+BiQuad bq3(5.6248e-05,1.1250e-04,5.6248e-05,-1.9787e+00,9.7890e-01);
 // Low pass Butterworth filter 4th order, Fc = 450; plited up in 2x2nd order
-BiQuad bq4(1.0000e+00,2.0000e+00,1.0000e+00,1.0000e+00,-4.6382e-01,8.9354e-02);
-BiQuad bq5(1.0000e+00,2.0000e+00,1.0000e+00,1.0000e+00,-6.3254e-01,4.8559e-01);
+BiQuad bq4(1.0000e+00,2.0000e+00,1.0000e+00,-4.6382e-01,8.9354e-02);
+BiQuad bq5(1.0000e+00,2.0000e+00,1.0000e+00,-6.3254e-01,4.8559e-01);
 
 void KeepTrackOfTime()
 {
@@ -41,101 +40,53 @@
 }
 
 // In the following: R is used for right arm, L is used for left arm!
-void FilteredSample(double averageEMGr, double averageEMGl)
+void FilteredSample()
 {   
-    double inR = emg0.read()-averageEMGr;
-    double inL = emg1.read()-averageEMGl;
+    double inR = emg0.read();
+    double inL = emg1.read();
     //double inLcal = inL-averageL(sumL,size);
-    double outRnotch = bq1.step(inR);
-    double outRhigh = bq2.step(outRnotch);
-    double outRrect = fabs(outRhigh);
-    double outRlow = bcq.step(outRrect);
-    double outLnotch = bq1.step(inL);
-    double outLhigh = bq2.step(outLnotch);
-    double outLrect = fabs(outLhigh);
-    double outLlow = bcq.step(outLrect);
+    double outRnotch = bcq1.step(inR);
+    double outRrect = fabs(outRnotch);
+    double outRlow = bcq2.step(outRrect);
     
-    pc.printf("Detrend EMG = %f\n\r",inR);
-    pc.printf("EMG signal= %f\n\r",emg0.read());
-    pc.printf("average EMG right = %f\n\r",averageEMGr);
+    double outLnotch = bcq1.step(inL);
+    double outLrect = fabs(outLnotch);
+    double outLlow = bcq2.step(outLrect);
     
-    led2 = !led2;
+    //pc.printf("Detrend EMG = %f\n\r",inR);
+    //pc.printf("EMG signal= %f\n\r",emg0.read());
+    //pc.printf("average EMG right = %f\n\r",averageEMGr);
     
-    /*scope.set(0, inR);
+    scope.set(0, inR);
     scope.set(1, inL);
     scope.set(2, outRlow);
     scope.set(3, outLlow);
     
-    
     scope.send();
     // To indicate that the function is working, the LED is toggled*/
+    led2 = !led2;
 }
 
 int main()
 {   
-    pc.baud(115200);
+    //pc.baud(115200);
     led1=1;
     led2=1;
-    t.attach(&KeepTrackOfTime,1.0); //taking the time in seconds
+    led2=!led2;
+    //t.attach(&KeepTrackOfTime,1.0); //taking the time in seconds
 
-    for (;;) {
-        if (time_passed<=5) 
-        {
-        
-        led1 =!led1;
-        //sample_timer.attach(&sample, 0.002); //500Hz
-        pc.printf("\rStart IF-loop\r\n");
-        
-        int size = 12500;
-        int i; 
-        double EMGsumR;
-        double EMGsumL; 
-            
-            while(i<size)
-            {
-                EMGsumR = emg0.read()+EMGsumR;
-                EMGsumL = emg1.read()+EMGsumL;
-                i++;
-                wait(0.0004);
-            }
-            
-        averageEMGr = (double) EMGsumR/size;
-        averageEMGl = (double) EMGsumL/size;
-        double InRcal = emg0.read()-averageEMGr;
-        double InLcal = emg1.read()-averageEMGl;
-        
-        scope.set(0, emg0.read() );
-        scope.set(1, 1 );
-        scope.set(2, averageEMGr);
-        scope.set(3, averageL(sumL,size));
-        scope.set(5, InRcal );
-        scope.set(6, InLcal );
-        
-        scope.send();*/
-        //pc.printf("EMG-signal = %f\n\r",emg0.read());
-        
-        /*pc.printf("\r\nIF-loop end:");
-        pc.printf("time is %i sec\n",time_passed);
-        pc.printf("averageR = %f\n\r", averageEMGr);*/
-        }
-        else
-        {        
-        led1 = 1; 
-        led2=!led2;
-        
-        bcq.add(&bq3).add(&bq4).add(&bq5);
+    bcq1.add(&bq1).add(&bq2);
+    bcq2.add(&bq3).add(&bq4).add(&bq5);
+    
       
-        filter_timer.attach(&filter_timer_act, 0.0004); //2500Hz (same as with filter coefficients on matlab!!! Thus adjust!)
-        pc.printf("\rMain-loop\n\r");
+    filter_timer.attach(&filter_timer_act, 0.0004); //2500Hz (same as with filter coefficients on matlab!!! Thus adjust!)
+        //pc.printf("\rMain-loop\n\r");
         //pc.printf("Detrend EMG = %f\n\r",inR);
         
-           while(1)
-            {
-            if (filter_timer_go){
+        while(1)
+        {
+        if (filter_timer_go){
                 filter_timer_go=false;
-                FilteredSample(averageEMGr, averageEMGl);}
-            }
-        
+                FilteredSample();}
         }
-}
 }
\ No newline at end of file