Diff: main.cpp

Revision:

2:27081b83a58e

Parent:

1:278677bb6b99

Child:

3:339b19905505

--- a/main.cpp	Fri Oct 21 10:15:43 2016 +0000
+++ b/main.cpp	Fri Oct 21 11:58:32 2016 +0000
@@ -1,56 +1,11 @@
 #include "mbed.h"
-#include <stdio.h>
+#include "BiQuad.h"
 #define SERIAL_BAUD 115200
 
 AnalogIn    emg0( A0 );
 AnalogIn    emg1( A1 );
 Serial pc(USBTX,USBRX);
 
-//HIDScope    scope( 6 );
-
- 
-/*int main() {
-  pc.baud(115200);
-  int array[10], maximum, size, c, location = 1;
-  
-  for (int i=0;i<10;i++)
-    {
-    double EMGright = emg0.read();
-    EMGright = array[i];
-    }
- 
-  printf("Enter the number of elements in array\n");
-  scanf("%d", &size);
- 
-  printf("Enter %d integers\n", size);
- 
-  for (c = 0; c < size; c++)
-    scanf("%d", &array[c]);
- 
-  maximum = array[0];
- 
-  for (c = 1; c < size; c++)
-  {
-    if (array[c] > maximum)
-    {
-       maximum  = array[c];
-       location = c+1;
-    }
-  }
- 
-  printf("Maximum element is present at location %d and it's value is %d.\n", location, maximum);
-  return 0;
-}*/
-/*
-#include "mbed.h"
-#include "HIDScope.h"
-#include "BiQuad.h"
-#include <stdio.h>
-
-//Define objects
-AnalogIn    emg0( A0 );
-AnalogIn    emg1( A1 );*/
-
 Ticker      sample_timer, average_timer, filter_timer, t;
 //HIDScope    scope( 6 );
 DigitalOut  led1(LED_RED);
@@ -59,12 +14,13 @@
 volatile int time_passed = 0;
 volatile bool filter_timer_go=false;
 
-double EMGright;
-double EMGleft;
+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;};
+void filter_timer_act(){filter_timer_go=true;};
 
 
 
@@ -77,33 +33,18 @@
 BiQuad bq3(5.6248e-05,1.1250e-04,5.6248e-05,1.0000e+00,-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 bq5(1.0000e+00,2.0000e+00,1.0000e+00,1.0000e+00,-6.3254e-01,4.8559e-01);
 
 void KeepTrackOfTime()
 {
     time_passed++;
 }
 
-void sample()
-{
-    double EMGright = emg0;
-    double EMGleft = emg1;
-    /*.set(0, emg0.read() );
-    scope.set(1, emg1.read() );
-    
-    scope.send();
-    
-    led1 = 0;
-    wait(0.5f);
-    led1 = 1;
-    wait(0.5f);*/
-}
-
 // In the following: R is used for right arm, L is used for left arm!
-/*void FilteredSample()
+void FilteredSample(double averageEMGr, double averageEMGl)
 {   
-    double inR = emg0.read();
-    double inL = emg1.read();
+    double inR = emg0.read()-averageEMGr;
+    double inL = emg1.read()-averageEMGl;
     //double inLcal = inL-averageL(sumL,size);
     double outRnotch = bq1.step(inR);
     double outRhigh = bq2.step(outRnotch);
@@ -114,54 +55,22 @@
     double outLrect = fabs(outLhigh);
     double outLlow = bcq.step(outLrect);
     
-    scope.set(0, inR);
+    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);
+    
+    led2 = !led2;
+    
+    /*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;
-}*/
-
-    
-/*
-void average(double averageR, double averageL)
-{
-    int averageR(int,int);
-    int averageL(int,int);
-    int size = 5000; //10/0.002
-    int arrayR[size]; //declaring array
-    int arrayL[size];
-    int sumR = 0;
-    int sumL = 0;
-    
-    for (int i=0;i<size;i++)
-    {
-        double EMGright = emg0.read();
-        double EMGleft = emg1.read();
-        EMGright = arrayR[i];
-        EMGleft = arrayL[i];
-        sumL = sumL+arrayR[i];
-        sumR=sumR+arrayR[i];
-        //double averageR = sumR/arrayR;
-        //double averageL = sumL/arrayL;
-    }
-    return averageR=sumR/sizeR;
-    return averageL(sumL,sizeL);*/
-    
-float averageR(int sumR, int size)
-{
-    return (float) sumR/size;
+    // To indicate that the function is working, the LED is toggled*/
 }
 
-float averageL(int sumL, int size)
-{
-    return (float) sumL/size;
-}
-
-
 int main()
 {   
     pc.baud(115200);
@@ -169,54 +78,33 @@
     led2=1;
     t.attach(&KeepTrackOfTime,1.0); //taking the time in seconds
 
-    //for (;;) {
-        //if (time_passed<=10) 
-        //{
-        //sample_timer.attach(&sample, 0.002); //500Hz
+    for (;;) {
+        if (time_passed<=5) 
+        {
         
-        /*float averageR(float,int);
-        float averageL(float,int);
-        int size = 500; //10/0.002
-        int arrayR[size]; //declaring array
-        int arrayL[size];
-        int sumR = 0;
-        int sumL = 0;
-        float EMGright = emg0.read();
-        float EMGleft = emg1.read();
+        led1 =!led1;
+        //sample_timer.attach(&sample, 0.002); //500Hz
+        pc.printf("\rStart IF-loop\r\n");
         
-            for (int i=0;i<size;i++)
+        int size = 12500;
+        int i; 
+        double EMGsumR;
+        double EMGsumL; 
+            
+            while(i<size)
             {
-            emg0.read() = arrayR[i];
-            arrayL[i]= emg1.read();
-            sumL = sumL+arrayL[i];
-            sumR = sumR+arrayR[i];
-            }
-            // Calibration step with the gained averages
-            //double CalibrationFactorR = 0.5/averageR(sumR,size);
-            //double CalibrationFactorL = 0.5/averageL(sumL,size);
-            */
-            int size = 12500;
-            int i; 
-            double EMGsumR;
-            double EMGsumL; 
-            
-            while(i<size){
-            sample();
-            EMGsumR = emg0.read()+EMGsumR;
-            EMGsumL = EMGleft+EMGsumL;
-            i++;
-            wait(0.0004);
+                EMGsumR = emg0.read()+EMGsumR;
+                EMGsumL = emg1.read()+EMGsumL;
+                i++;
+                wait(0.0004);
             }
             
-        double averageEMGr = (double) EMGsumR/size;
-        double averageEMGl = (double) EMGsumL/size;
+        averageEMGr = (double) EMGsumR/size;
+        averageEMGl = (double) EMGsumL/size;
         double InRcal = emg0.read()-averageEMGr;
         double InLcal = emg1.read()-averageEMGl;
         
-        led1 =!led1;
-        //wait(0.5f);
-        
-        /*scope.set(0, emg0.read() );
+        scope.set(0, emg0.read() );
         scope.set(1, 1 );
         scope.set(2, averageEMGr);
         scope.set(3, averageL(sumL,size));
@@ -225,32 +113,29 @@
         
         scope.send();*/
         //pc.printf("EMG-signal = %f\n\r",emg0.read());
-        pc.printf("averageR = %f\n\r",averageEMGr);
-        pc.printf("averageR = %f\n\r",averageEMGl);
-        //pc.printf("averageRvoid = %f\n\r",averageR(sumR,size));
-        //pc.printf("Detrend EMG = %f\n\r",InRcal);
-        //pc.printf("array value = %f\n\r",arrayR[3]);
-        pc.printf("test\n\r");
-        pc.printf("EMG sum = %f\n\r",EMGsumR);
-        pc.printf("EMG signal  = %f\n\r",EMGright);
         
-        /*else
-        {
-                   
+        /*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);
       
         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)
+           while(1)
             {
             if (filter_timer_go){
                 filter_timer_go=false;
-                FilteredSample();}
+                FilteredSample(averageEMGr, averageEMGl);}
             }
         
-        }*/
-
+        }
+}
 }
\ No newline at end of file