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@4:24e2a5e50387, 2016-11-02 (annotated)

Committer:
GerhardBerman
Date:
Wed Nov 02 13:22:42 2016 +0000
Revision:
4:24e2a5e50387
Parent:
3:35103d6e7d2a 
EMG Test to check EMG function on robot

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