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@1:dea6b70cd991, 2016-11-01 (annotated)

Committer:
Marieke
Date:
Tue Nov 01 17:04:39 2016 +0000
Revision:
1:dea6b70cd991
Parent:
0:18cb9a6c4fc1
Child:
2:c32de830a7d9
hidscope and filters working, but T not;

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