Gerhard Berman / Mbed 2 deprecated EMG_prog_robot

EMG test for robot is working (to get thresholds)

Dependencies:   HIDScope biquadFilter mbed

Fork of EMG_prog_robot by Marieke M

main.cpp@0:18cb9a6c4fc1, 2016-11-01 (annotated)

Committer:
Marieke
Date:
Tue Nov 01 15:54:01 2016 +0000
Revision:
0:18cb9a6c4fc1
Child:
1:dea6b70cd991
EMG_script working for putty, but filtered signal too similar;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Marieke 0:18cb9a6c4fc1 1 #include "mbed.h"
Marieke 0:18cb9a6c4fc1 2 #include "BiQuad.h"
Marieke 0:18cb9a6c4fc1 3 #define SERIAL_BAUD 115200 // baud rate for serial communication
Marieke 0:18cb9a6c4fc1 4
Marieke 0:18cb9a6c4fc1 5 // Serial communication with PC
Marieke 0:18cb9a6c4fc1 6 Serial pc(USBTX,USBRX);
Marieke 0:18cb9a6c4fc1 7 AnalogIn emg0( A0 );
Marieke 0:18cb9a6c4fc1 8 AnalogIn emg1( A1 );
Marieke 0:18cb9a6c4fc1 9
Marieke 0:18cb9a6c4fc1 10 Ticker filter_timer, send_timer;
Marieke 0:18cb9a6c4fc1 11 DigitalOut led1(LED_RED);
Marieke 0:18cb9a6c4fc1 12 DigitalOut led2(LED_BLUE);
Marieke 0:18cb9a6c4fc1 13
Marieke 0:18cb9a6c4fc1 14 volatile bool filter_timer_go=false,send_timer_go=false;
Marieke 0:18cb9a6c4fc1 15
Marieke 0:18cb9a6c4fc1 16 double EMGright, EMGleft, inR;
Marieke 0:18cb9a6c4fc1 17 int EMGgain=1;
Marieke 0:18cb9a6c4fc1 18 //set initial conditions
Marieke 0:18cb9a6c4fc1 19 double biceps_l = 0;
Marieke 0:18cb9a6c4fc1 20 double biceps_r = 0;
Marieke 0:18cb9a6c4fc1 21 double outRenvelope, outLenvelope;
Marieke 0:18cb9a6c4fc1 22
Marieke 0:18cb9a6c4fc1 23
Marieke 0:18cb9a6c4fc1 24 void filter_timer_act(){filter_timer_go=true;};
Marieke 0:18cb9a6c4fc1 25 void send_timer_act(){send_timer_go=true;};
Marieke 0:18cb9a6c4fc1 26
Marieke 0:18cb9a6c4fc1 27 BiQuadChain bcq1;
Marieke 0:18cb9a6c4fc1 28 BiQuadChain bcq2;
Marieke 0:18cb9a6c4fc1 29 // Notch filter wo=50; bw=wo/35
Marieke 0:18cb9a6c4fc1 30 BiQuad bq1(9.9821e-01,-1.9807e+00,9.9821e-01,-1.9807e+00,9.9642e-01);
Marieke 0:18cb9a6c4fc1 31 // High pass Butterworth filter 2nd order, Fc=10;
Marieke 0:18cb9a6c4fc1 32 BiQuad bq2(9.8239e-01,-1.9648e+00,9.8239e-01,-1.9645e+00,9.6508e-01);
Marieke 0:18cb9a6c4fc1 33 // Low pass Butterworth filter 2nd order, Fc = 8;
Marieke 0:18cb9a6c4fc1 34 BiQuad bq3(5.6248e-05,1.1250e-04,5.6248e-05,-1.9787e+00,9.7890e-01);
Marieke 0:18cb9a6c4fc1 35
Marieke 0:18cb9a6c4fc1 36
Marieke 0:18cb9a6c4fc1 37 // In the following: R is used for right arm, L is used for left arm!
Marieke 0:18cb9a6c4fc1 38 void FilteredSample(double &envelopeR, double &envelopeL)
Marieke 0:18cb9a6c4fc1 39 {
Marieke 0:18cb9a6c4fc1 40 double inLout = emg0.read();
Marieke 0:18cb9a6c4fc1 41 double inRout = emg1.read();
Marieke 0:18cb9a6c4fc1 42
Marieke 0:18cb9a6c4fc1 43 double outRfilter1 = bcq1.step(inRout);
Marieke 0:18cb9a6c4fc1 44 double outRrect= fabs(outRfilter1);
Marieke 0:18cb9a6c4fc1 45 envelopeR = bcq2.step(outRrect);
Marieke 0:18cb9a6c4fc1 46
Marieke 0:18cb9a6c4fc1 47 double outLfilter1 = bcq1.step(inLout);
Marieke 0:18cb9a6c4fc1 48 double outLrect = fabs(outLfilter1);
Marieke 0:18cb9a6c4fc1 49 envelopeL = bcq2.step(outLrect);
Marieke 0:18cb9a6c4fc1 50
Marieke 0:18cb9a6c4fc1 51 pc.printf("EMG right = %f\n\r",inRout);
Marieke 0:18cb9a6c4fc1 52 pc.printf("EMG left = %f\n\r",inLout);
Marieke 0:18cb9a6c4fc1 53 pc.printf("envelope EMG right = %f\n\r",envelopeR);
Marieke 0:18cb9a6c4fc1 54 pc.printf("envelope EMG left = %f\n\r",envelopeL);
Marieke 0:18cb9a6c4fc1 55 }
Marieke 0:18cb9a6c4fc1 56
Marieke 0:18cb9a6c4fc1 57 void sendValues( double outRenvelope, double outLenvelope){
Marieke 0:18cb9a6c4fc1 58
Marieke 0:18cb9a6c4fc1 59 biceps_l = (double) outLenvelope * EMGgain; //emg0.read(); //velocity or reference position change, EMG with a gain
Marieke 0:18cb9a6c4fc1 60 biceps_r = (double) outRenvelope * EMGgain; //emg1.read();
Marieke 0:18cb9a6c4fc1 61 if (biceps_l > 0.2 && biceps_r > 0.2){
Marieke 0:18cb9a6c4fc1 62 //both arms activated: stamp moves down
Marieke 0:18cb9a6c4fc1 63 pc.printf("Stamp down\n\r");
Marieke 0:18cb9a6c4fc1 64 pc.printf("right: %f\n\r",biceps_r);
Marieke 0:18cb9a6c4fc1 65 pc.printf("left: %f\n\r",biceps_l);
Marieke 0:18cb9a6c4fc1 66 //wait(0.5);
Marieke 0:18cb9a6c4fc1 67 led1=!led1;//blink purple
Marieke 0:18cb9a6c4fc1 68 led2=!led2;
Marieke 0:18cb9a6c4fc1 69 }
Marieke 0:18cb9a6c4fc1 70 else if (biceps_l > 0.2 && biceps_r <= 0.2){
Marieke 0:18cb9a6c4fc1 71 //arm 1 activated, move left
Marieke 0:18cb9a6c4fc1 72 pc.printf("Move left\n\r");
Marieke 0:18cb9a6c4fc1 73 pc.printf("right: %f\n\r",biceps_r);
Marieke 0:18cb9a6c4fc1 74 pc.printf("left: %f\n\r",biceps_l);
Marieke 0:18cb9a6c4fc1 75 //wait(0.5);
Marieke 0:18cb9a6c4fc1 76 led2=1;//off
Marieke 0:18cb9a6c4fc1 77 led1=0;//on red
Marieke 0:18cb9a6c4fc1 78 }
Marieke 0:18cb9a6c4fc1 79 else if (biceps_l <= 0.2 && biceps_r > 0.2){
Marieke 0:18cb9a6c4fc1 80 //arm 1 activated, move right
Marieke 0:18cb9a6c4fc1 81 pc.printf("Move right\n\r");
Marieke 0:18cb9a6c4fc1 82 pc.printf("right: %f\n\r",biceps_r);
Marieke 0:18cb9a6c4fc1 83 pc.printf("left: %f\n\r",biceps_l);
Marieke 0:18cb9a6c4fc1 84 //wait(0.5);
Marieke 0:18cb9a6c4fc1 85 led2=0;//on blue
Marieke 0:18cb9a6c4fc1 86 led1=1;//off
Marieke 0:18cb9a6c4fc1 87 }
Marieke 0:18cb9a6c4fc1 88 else{
Marieke 0:18cb9a6c4fc1 89 wait(0.2);
Marieke 0:18cb9a6c4fc1 90 led1 = 1;
Marieke 0:18cb9a6c4fc1 91 led2 = 1; //off
Marieke 0:18cb9a6c4fc1 92 pc.printf("Nothing...\n\r");
Marieke 0:18cb9a6c4fc1 93 //wait(0.5);
Marieke 0:18cb9a6c4fc1 94 }
Marieke 0:18cb9a6c4fc1 95 // To indicate that the function is working, the LED is toggled*/
Marieke 0:18cb9a6c4fc1 96 //led2 = !led2; // blue
Marieke 0:18cb9a6c4fc1 97 }
Marieke 0:18cb9a6c4fc1 98
Marieke 0:18cb9a6c4fc1 99
Marieke 0:18cb9a6c4fc1 100
Marieke 0:18cb9a6c4fc1 101 int main()
Marieke 0:18cb9a6c4fc1 102 {
Marieke 0:18cb9a6c4fc1 103 pc.baud(SERIAL_BAUD);
Marieke 0:18cb9a6c4fc1 104 led1=1;
Marieke 0:18cb9a6c4fc1 105 led2=1;
Marieke 0:18cb9a6c4fc1 106 led1=0; //red
Marieke 0:18cb9a6c4fc1 107
Marieke 0:18cb9a6c4fc1 108 bcq1.add(&bq1).add(&bq2);
Marieke 0:18cb9a6c4fc1 109 bcq2.add(&bq3);
Marieke 0:18cb9a6c4fc1 110
Marieke 0:18cb9a6c4fc1 111
Marieke 0:18cb9a6c4fc1 112 filter_timer.attach(&filter_timer_act, 0.0004); //2500Hz (same as with filter coefficients on matlab!!! Thus adjust!)
Marieke 0:18cb9a6c4fc1 113 send_timer.attach(&send_timer_act, 0.0004);
Marieke 0:18cb9a6c4fc1 114 pc.printf("\rMain-loop\n\r");
Marieke 0:18cb9a6c4fc1 115
Marieke 0:18cb9a6c4fc1 116 while(1)
Marieke 0:18cb9a6c4fc1 117 {
Marieke 0:18cb9a6c4fc1 118 if (filter_timer_go){
Marieke 0:18cb9a6c4fc1 119 filter_timer_go=false;
Marieke 0:18cb9a6c4fc1 120 FilteredSample(outRenvelope, outLenvelope);}
Marieke 0:18cb9a6c4fc1 121 if (send_timer_go){
Marieke 0:18cb9a6c4fc1 122 send_timer_go=false;
Marieke 0:18cb9a6c4fc1 123 sendValues(outRenvelope, outLenvelope);}
Marieke 0:18cb9a6c4fc1 124 }
Marieke 0:18cb9a6c4fc1 125 }