Marieke M / Mbed 2 deprecated EMG_prog_robot

EMG measurement in HIDScope working!

Dependencies:   HIDScope biquadFilter mbed

main.cpp@2:c32de830a7d9, 2016-11-02 (annotated)

Committer:
Marieke
Date:
Wed Nov 02 09:32:11 2016 +0000
Revision:
2:c32de830a7d9
Parent:
1:dea6b70cd991
Child:
3:35103d6e7d2a
EMG script hidscope working, but T not. Take out the plug for better signal!!

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 1:dea6b70cd991 3 #include "HIDScope.h"
Marieke 1:dea6b70cd991 4 //#define SERIAL_BAUD 115200 // baud rate for serial communication
Marieke 0:18cb9a6c4fc1 5
Marieke 0:18cb9a6c4fc1 6 // Serial communication with PC
Marieke 0:18cb9a6c4fc1 7 Serial pc(USBTX,USBRX);
Marieke 0:18cb9a6c4fc1 8 AnalogIn emg0( A0 );
Marieke 0:18cb9a6c4fc1 9 AnalogIn emg1( A1 );
Marieke 1:dea6b70cd991 10 HIDScope scope( 5 );
Marieke 0:18cb9a6c4fc1 11
Marieke 0:18cb9a6c4fc1 12 Ticker filter_timer, send_timer;
Marieke 0:18cb9a6c4fc1 13 DigitalOut led1(LED_RED);
Marieke 0:18cb9a6c4fc1 14 DigitalOut led2(LED_BLUE);
Marieke 0:18cb9a6c4fc1 15
Marieke 0:18cb9a6c4fc1 16 volatile bool filter_timer_go=false,send_timer_go=false;
Marieke 0:18cb9a6c4fc1 17
Marieke 0:18cb9a6c4fc1 18 double EMGright, EMGleft, inR;
Marieke 0:18cb9a6c4fc1 19 int EMGgain=1;
Marieke 0:18cb9a6c4fc1 20 //set initial conditions
Marieke 2:c32de830a7d9 21 //double biceps_l = 0;
Marieke 2:c32de830a7d9 22 //double biceps_r = 0;
Marieke 0:18cb9a6c4fc1 23 double outRenvelope, outLenvelope;
Marieke 2:c32de830a7d9 24 int T=4;
Marieke 2:c32de830a7d9 25
Marieke 0:18cb9a6c4fc1 26
Marieke 0:18cb9a6c4fc1 27
Marieke 0:18cb9a6c4fc1 28 void filter_timer_act(){filter_timer_go=true;};
Marieke 0:18cb9a6c4fc1 29 void send_timer_act(){send_timer_go=true;};
Marieke 0:18cb9a6c4fc1 30
Marieke 1:dea6b70cd991 31 BiQuadChain bcq1R;
Marieke 1:dea6b70cd991 32 BiQuadChain bcq2R;
Marieke 0:18cb9a6c4fc1 33 // Notch filter wo=50; bw=wo/35
Marieke 1:dea6b70cd991 34 BiQuad bq1R(9.9821e-01,-1.9807e+00,9.9821e-01,-1.9807e+00,9.9642e-01);
Marieke 0:18cb9a6c4fc1 35 // High pass Butterworth filter 2nd order, Fc=10;
Marieke 1:dea6b70cd991 36 BiQuad bq2R(9.8239e-01,-1.9648e+00,9.8239e-01,-1.9645e+00,9.6508e-01);
Marieke 0:18cb9a6c4fc1 37 // Low pass Butterworth filter 2nd order, Fc = 8;
Marieke 1:dea6b70cd991 38 BiQuad bq3R(5.6248e-05,1.1250e-04,5.6248e-05,-1.9787e+00,9.7890e-01);
Marieke 0:18cb9a6c4fc1 39
Marieke 1:dea6b70cd991 40 BiQuadChain bcq1L;
Marieke 1:dea6b70cd991 41 BiQuadChain bcq2L;
Marieke 1:dea6b70cd991 42 // Notch filter wo=50; bw=wo/35
Marieke 1:dea6b70cd991 43 BiQuad bq1L(9.9821e-01,-1.9807e+00,9.9821e-01,-1.9807e+00,9.9642e-01);
Marieke 1:dea6b70cd991 44 // High pass Butterworth filter 2nd order, Fc=10;
Marieke 1:dea6b70cd991 45 BiQuad bq2L(9.8239e-01,-1.9648e+00,9.8239e-01,-1.9645e+00,9.6508e-01);
Marieke 1:dea6b70cd991 46 // Low pass Butterworth filter 2nd order, Fc = 8;
Marieke 1:dea6b70cd991 47 BiQuad bq3L(5.6248e-05,1.1250e-04,5.6248e-05,-1.9787e+00,9.7890e-01);
Marieke 2:c32de830a7d9 48
Marieke 0:18cb9a6c4fc1 49 // In the following: R is used for right arm, L is used for left arm!
Marieke 2:c32de830a7d9 50
Marieke 2:c32de830a7d9 51 void FilteredSample(int &Tout)
Marieke 0:18cb9a6c4fc1 52 {
Marieke 0:18cb9a6c4fc1 53 double inLout = emg0.read();
Marieke 0:18cb9a6c4fc1 54 double inRout = emg1.read();
Marieke 0:18cb9a6c4fc1 55
Marieke 1:dea6b70cd991 56 double outRfilter1 = bcq1R.step(inRout);
Marieke 0:18cb9a6c4fc1 57 double outRrect= fabs(outRfilter1);
Marieke 1:dea6b70cd991 58 double envelopeR = bcq2R.step(outRrect);
Marieke 1:dea6b70cd991 59
Marieke 1:dea6b70cd991 60 double outLfilter1 = bcq1L.step(inLout);
Marieke 1:dea6b70cd991 61 double outLrect = fabs(outLfilter1);
Marieke 1:dea6b70cd991 62 double envelopeL = bcq2L.step(outLrect);
Marieke 0:18cb9a6c4fc1 63
Marieke 2:c32de830a7d9 64 double biceps_l = (double) outLenvelope * EMGgain; //emg0.read(); //velocity or reference position change, EMG with a gain
Marieke 2:c32de830a7d9 65 double biceps_r = (double) outRenvelope * EMGgain; //emg1.read();
Marieke 1:dea6b70cd991 66 if (biceps_l > 0.04 && biceps_r > 0.04){
Marieke 1:dea6b70cd991 67 //both arms activated: stamp moves down
Marieke 1:dea6b70cd991 68 //pc.printf("Stamp down\n\r");
Marieke 1:dea6b70cd991 69 //pc.printf("right: %f\n\r",biceps_r);
Marieke 1:dea6b70cd991 70 //pc.printf("left: %f\n\r",biceps_l);
Marieke 1:dea6b70cd991 71 //wait(0.5);
Marieke 2:c32de830a7d9 72 Tout = -1;
Marieke 1:dea6b70cd991 73 led1=!led1;//blink purple
Marieke 1:dea6b70cd991 74 led2=!led2;
Marieke 1:dea6b70cd991 75 }
Marieke 1:dea6b70cd991 76 else if (biceps_l > 0.04 && biceps_r <= 0.04){
Marieke 1:dea6b70cd991 77 //arm 1 activated, move left
Marieke 1:dea6b70cd991 78 //pc.printf("Move left\n\r");
Marieke 1:dea6b70cd991 79 //pc.printf("right: %f\n\r",biceps_r);
Marieke 1:dea6b70cd991 80 //pc.printf("left: %f\n\r",biceps_l);
Marieke 1:dea6b70cd991 81 //wait(0.5);
Marieke 2:c32de830a7d9 82 Tout = -2;
Marieke 1:dea6b70cd991 83 led2=1;//off
Marieke 1:dea6b70cd991 84 led1=0;//on red
Marieke 1:dea6b70cd991 85 }
Marieke 1:dea6b70cd991 86 else if (biceps_l <= 0.04 && biceps_r > 0.04){
Marieke 1:dea6b70cd991 87 //arm 1 activated, move right
Marieke 1:dea6b70cd991 88 //pc.printf("Move right\n\r");
Marieke 1:dea6b70cd991 89 //pc.printf("right: %f\n\r",biceps_r);
Marieke 1:dea6b70cd991 90 //pc.printf("left: %f\n\r",biceps_l);
Marieke 1:dea6b70cd991 91 //wait(0.5);
Marieke 2:c32de830a7d9 92 Tout = 2;
Marieke 1:dea6b70cd991 93 led2=0;//on blue
Marieke 1:dea6b70cd991 94 led1=1;//off
Marieke 1:dea6b70cd991 95 }
Marieke 1:dea6b70cd991 96 else{
Marieke 1:dea6b70cd991 97 //wait(0.2);
Marieke 1:dea6b70cd991 98 led1 = 1;
Marieke 1:dea6b70cd991 99 led2 = 1; //off
Marieke 1:dea6b70cd991 100 //pc.printf("Nothing...\n\r");
Marieke 1:dea6b70cd991 101 //wait(0.5);
Marieke 2:c32de830a7d9 102 Tout = 5;
Marieke 1:dea6b70cd991 103 }
Marieke 0:18cb9a6c4fc1 104
Marieke 1:dea6b70cd991 105 /*pc.printf("EMG right = %f\n\r",inRout);
Marieke 0:18cb9a6c4fc1 106 pc.printf("EMG left = %f\n\r",inLout);
Marieke 0:18cb9a6c4fc1 107 pc.printf("envelope EMG right = %f\n\r",envelopeR);
Marieke 1:dea6b70cd991 108 pc.printf("envelope EMG left = %f\n\r",envelopeL);*/
Marieke 1:dea6b70cd991 109
Marieke 1:dea6b70cd991 110 scope.set(0, inRout);
Marieke 1:dea6b70cd991 111 scope.set(1, inLout);
Marieke 1:dea6b70cd991 112 scope.set(2, envelopeR);
Marieke 1:dea6b70cd991 113 scope.set(3, envelopeL);
Marieke 2:c32de830a7d9 114 scope.set(4, Tout);
Marieke 1:dea6b70cd991 115
Marieke 1:dea6b70cd991 116 scope.send();
Marieke 1:dea6b70cd991 117 // To indicate that the function is working, the LED is toggled*/
Marieke 2:c32de830a7d9 118 //led2 = !led2;
Marieke 0:18cb9a6c4fc1 119 }
Marieke 0:18cb9a6c4fc1 120
Marieke 1:dea6b70cd991 121 /*void sendValues( double outRenvelope, double outLenvelope){
Marieke 0:18cb9a6c4fc1 122
Marieke 0:18cb9a6c4fc1 123 biceps_l = (double) outLenvelope * EMGgain; //emg0.read(); //velocity or reference position change, EMG with a gain
Marieke 0:18cb9a6c4fc1 124 biceps_r = (double) outRenvelope * EMGgain; //emg1.read();
Marieke 0:18cb9a6c4fc1 125 if (biceps_l > 0.2 && biceps_r > 0.2){
Marieke 0:18cb9a6c4fc1 126 //both arms activated: stamp moves down
Marieke 0:18cb9a6c4fc1 127 pc.printf("Stamp down\n\r");
Marieke 0:18cb9a6c4fc1 128 pc.printf("right: %f\n\r",biceps_r);
Marieke 0:18cb9a6c4fc1 129 pc.printf("left: %f\n\r",biceps_l);
Marieke 0:18cb9a6c4fc1 130 //wait(0.5);
Marieke 0:18cb9a6c4fc1 131 led1=!led1;//blink purple
Marieke 0:18cb9a6c4fc1 132 led2=!led2;
Marieke 0:18cb9a6c4fc1 133 }
Marieke 0:18cb9a6c4fc1 134 else if (biceps_l > 0.2 && biceps_r <= 0.2){
Marieke 0:18cb9a6c4fc1 135 //arm 1 activated, move left
Marieke 0:18cb9a6c4fc1 136 pc.printf("Move left\n\r");
Marieke 0:18cb9a6c4fc1 137 pc.printf("right: %f\n\r",biceps_r);
Marieke 0:18cb9a6c4fc1 138 pc.printf("left: %f\n\r",biceps_l);
Marieke 0:18cb9a6c4fc1 139 //wait(0.5);
Marieke 0:18cb9a6c4fc1 140 led2=1;//off
Marieke 0:18cb9a6c4fc1 141 led1=0;//on red
Marieke 0:18cb9a6c4fc1 142 }
Marieke 0:18cb9a6c4fc1 143 else if (biceps_l <= 0.2 && biceps_r > 0.2){
Marieke 0:18cb9a6c4fc1 144 //arm 1 activated, move right
Marieke 0:18cb9a6c4fc1 145 pc.printf("Move right\n\r");
Marieke 0:18cb9a6c4fc1 146 pc.printf("right: %f\n\r",biceps_r);
Marieke 0:18cb9a6c4fc1 147 pc.printf("left: %f\n\r",biceps_l);
Marieke 0:18cb9a6c4fc1 148 //wait(0.5);
Marieke 0:18cb9a6c4fc1 149 led2=0;//on blue
Marieke 0:18cb9a6c4fc1 150 led1=1;//off
Marieke 0:18cb9a6c4fc1 151 }
Marieke 0:18cb9a6c4fc1 152 else{
Marieke 0:18cb9a6c4fc1 153 wait(0.2);
Marieke 0:18cb9a6c4fc1 154 led1 = 1;
Marieke 0:18cb9a6c4fc1 155 led2 = 1; //off
Marieke 0:18cb9a6c4fc1 156 pc.printf("Nothing...\n\r");
Marieke 0:18cb9a6c4fc1 157 //wait(0.5);
Marieke 0:18cb9a6c4fc1 158 }
Marieke 1:dea6b70cd991 159 // To indicate that the function is working, the LED is toggled
Marieke 0:18cb9a6c4fc1 160 //led2 = !led2; // blue
Marieke 1:dea6b70cd991 161
Marieke 1:dea6b70cd991 162 }*/
Marieke 0:18cb9a6c4fc1 163
Marieke 0:18cb9a6c4fc1 164
Marieke 0:18cb9a6c4fc1 165
Marieke 0:18cb9a6c4fc1 166 int main()
Marieke 0:18cb9a6c4fc1 167 {
Marieke 1:dea6b70cd991 168 //pc.baud(SERIAL_BAUD);
Marieke 0:18cb9a6c4fc1 169 led1=1;
Marieke 0:18cb9a6c4fc1 170 led2=1;
Marieke 0:18cb9a6c4fc1 171 led1=0; //red
Marieke 0:18cb9a6c4fc1 172
Marieke 1:dea6b70cd991 173 bcq1R.add(&bq1R).add(&bq2R);
Marieke 1:dea6b70cd991 174 bcq2R.add(&bq3R);
Marieke 0:18cb9a6c4fc1 175
Marieke 1:dea6b70cd991 176 bcq1L.add(&bq1L).add(&bq2L);
Marieke 1:dea6b70cd991 177 bcq2L.add(&bq3L);
Marieke 0:18cb9a6c4fc1 178
Marieke 0:18cb9a6c4fc1 179 filter_timer.attach(&filter_timer_act, 0.0004); //2500Hz (same as with filter coefficients on matlab!!! Thus adjust!)
Marieke 1:dea6b70cd991 180 //send_timer.attach(&send_timer_act, 0.0004);
Marieke 1:dea6b70cd991 181 //pc.printf("\rMain-loop\n\r");
Marieke 0:18cb9a6c4fc1 182
Marieke 0:18cb9a6c4fc1 183 while(1)
Marieke 0:18cb9a6c4fc1 184 {
Marieke 0:18cb9a6c4fc1 185 if (filter_timer_go){
Marieke 0:18cb9a6c4fc1 186 filter_timer_go=false;
Marieke 2:c32de830a7d9 187 FilteredSample(T);}
Marieke 1:dea6b70cd991 188 /*if (send_timer_go){
Marieke 0:18cb9a6c4fc1 189 send_timer_go=false;
Marieke 1:dea6b70cd991 190 sendValues(outRenvelope, outLenvelope);}*/
Marieke 0:18cb9a6c4fc1 191 }
Marieke 0:18cb9a6c4fc1 192 }