Diff: main.cpp

Revision:

0:18cb9a6c4fc1

Child:

1:dea6b70cd991

diff -r 000000000000 -r 18cb9a6c4fc1 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Nov 01 15:54:01 2016 +0000
@@ -0,0 +1,125 @@
+#include "mbed.h"
+#include "BiQuad.h"
+#define SERIAL_BAUD 115200  // baud rate for serial communication
+
+// Serial communication with PC
+Serial pc(USBTX,USBRX);
+AnalogIn    emg0( A0 );
+AnalogIn    emg1( A1 );
+
+Ticker      filter_timer, send_timer;
+DigitalOut  led1(LED_RED);
+DigitalOut  led2(LED_BLUE);
+
+volatile bool filter_timer_go=false,send_timer_go=false;
+
+double EMGright, EMGleft, inR;
+int EMGgain=1; 
+//set initial conditions
+double biceps_l = 0;
+double biceps_r = 0;
+double outRenvelope, outLenvelope;
+
+
+void filter_timer_act(){filter_timer_go=true;};
+void send_timer_act(){send_timer_go=true;};
+
+BiQuadChain bcq1;
+BiQuadChain bcq2;
+// Notch filter wo=50; bw=wo/35
+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.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.9787e+00,9.7890e-01);
+
+
+// In the following: R is used for right arm, L is used for left arm!
+void FilteredSample(double &envelopeR, double &envelopeL)
+{   
+    double inLout = emg0.read();
+    double inRout = emg1.read();
+    
+    double outRfilter1 = bcq1.step(inRout);
+    double outRrect= fabs(outRfilter1);
+    envelopeR = bcq2.step(outRrect);
+    
+    double outLfilter1 = bcq1.step(inLout);
+    double outLrect = fabs(outLfilter1);
+    envelopeL = bcq2.step(outLrect);
+
+    pc.printf("EMG right = %f\n\r",inRout);
+    pc.printf("EMG left =  %f\n\r",inLout);
+    pc.printf("envelope EMG right = %f\n\r",envelopeR);
+    pc.printf("envelope EMG left =  %f\n\r",envelopeL);
+}
+
+void sendValues( double outRenvelope, double outLenvelope){
+    
+    biceps_l = (double) outLenvelope * EMGgain; //emg0.read();     //velocity or reference position change, EMG with a gain
+    biceps_r = (double) outRenvelope * EMGgain; //emg1.read();
+    if (biceps_l > 0.2 && biceps_r > 0.2){
+        //both arms activated: stamp moves down
+        pc.printf("Stamp down\n\r");
+        pc.printf("right: %f\n\r",biceps_r);
+        pc.printf("left:  %f\n\r",biceps_l);
+        //wait(0.5);
+        led1=!led1;//blink purple
+        led2=!led2;
+        }
+    else if (biceps_l > 0.2 && biceps_r <= 0.2){
+        //arm 1 activated, move left
+        pc.printf("Move left\n\r");
+        pc.printf("right: %f\n\r",biceps_r);
+        pc.printf("left:  %f\n\r",biceps_l);
+        //wait(0.5);
+        led2=1;//off
+        led1=0;//on    red
+        }
+    else if (biceps_l <= 0.2 && biceps_r > 0.2){
+        //arm 1 activated, move right
+        pc.printf("Move right\n\r");
+        pc.printf("right: %f\n\r",biceps_r);
+        pc.printf("left:  %f\n\r",biceps_l);
+        //wait(0.5);
+        led2=0;//on    blue
+        led1=1;//off
+        }
+    else{
+        wait(0.2);
+        led1 = 1;
+        led2 = 1;  //off
+        pc.printf("Nothing...\n\r");
+        //wait(0.5);
+        }
+    // To indicate that the function is working, the LED is toggled*/
+    //led2 = !led2; // blue
+}
+
+
+
+int main()
+{   
+    pc.baud(SERIAL_BAUD);
+    led1=1;
+    led2=1; 
+    led1=0; //red
+
+    bcq1.add(&bq1).add(&bq2);
+    bcq2.add(&bq3);
+    
+      
+    filter_timer.attach(&filter_timer_act, 0.0004); //2500Hz (same as with filter coefficients on matlab!!! Thus adjust!)
+    send_timer.attach(&send_timer_act, 0.0004);
+    pc.printf("\rMain-loop\n\r");
+        
+        while(1)
+        {
+        if (filter_timer_go){
+                filter_timer_go=false;
+                FilteredSample(outRenvelope, outLenvelope);}
+        if (send_timer_go){
+                send_timer_go=false;
+                sendValues(outRenvelope, outLenvelope);}
+        }
+}
\ No newline at end of file