group14 - k9 / Mbed 2 deprecated emgCalibration

Test code to calibrate EMG - MVC measurement

Dependencies:   HIDScope MODSERIAL mbed

Fork of emgCalibration by Martijn Kern

main.cpp@5:d7ee4a5612af, 2015-10-16 (annotated)

Committer:
Vigilance88
Date:
Fri Oct 16 12:14:39 2015 +0000
Revision:
5:d7ee4a5612af
Parent:
4:329e1022cbd3 
fixed serial com

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Paashaas 0:d8312eea7c50 1 #include "mbed.h"
Paashaas 0:d8312eea7c50 2 #include "HIDScope.h"
Vigilance88 1:93d79d6e96bc 3 #include "MODSERIAL.h"
Paashaas 0:d8312eea7c50 4
Vigilance88 1:93d79d6e96bc 5 #define SAMPLING_RATE 0.002
Vigilance88 1:93d79d6e96bc 6
Vigilance88 1:93d79d6e96bc 7 MODSERIAL pc(USBTX,USBRX);
Vigilance88 1:93d79d6e96bc 8 AnalogIn emg1(A0); // onderste board
Paashaas 0:d8312eea7c50 9 AnalogIn emg2(A1); // 2e board
Paashaas 0:d8312eea7c50 10 AnalogIn emg3(A2); // 3e board
Paashaas 0:d8312eea7c50 11 AnalogIn emg4(A3); // bovenste board
Paashaas 0:d8312eea7c50 12
Vigilance88 5:d7ee4a5612af 13 Ticker sample_timer; // naam van de emg-ticker
Vigilance88 4:329e1022cbd3 14 DigitalOut red(LED_RED);
Vigilance88 4:329e1022cbd3 15 DigitalOut green(LED_GREEN);
Vigilance88 4:329e1022cbd3 16 DigitalOut blue(LED_BLUE);
Vigilance88 4:329e1022cbd3 17
Vigilance88 5:d7ee4a5612af 18 int const window=150; //30 samples
Vigilance88 5:d7ee4a5612af 19 int i=0; //buffer index
Vigilance88 5:d7ee4a5612af 20 double bicepsbuffer [window];
Vigilance88 1:93d79d6e96bc 21
Vigilance88 4:329e1022cbd3 22 //HIDScope scope(4); // aantal kanalen voor je HIDScope
Paashaas 0:d8312eea7c50 23
Vigilance88 5:d7ee4a5612af 24 double avg1=0;
Vigilance88 3:3981bbe6d7b8 25 int muscle;
Vigilance88 3:3981bbe6d7b8 26 double tijd;
Vigilance88 3:3981bbe6d7b8 27 double y5;
Vigilance88 3:3981bbe6d7b8 28
Vigilance88 3:3981bbe6d7b8 29
Paashaas 0:d8312eea7c50 30 //highpass filter 20 Hz
Paashaas 0:d8312eea7c50 31 const double numhigh_1 = 0.956543225556877;
Paashaas 0:d8312eea7c50 32 const double numhigh_2 = -1.91308645113754;
Paashaas 0:d8312eea7c50 33 const double numhigh_3 = 0.956543225556877;
Paashaas 0:d8312eea7c50 34 const double denhigh_2 = -1.91197067426073;
Paashaas 0:d8312eea7c50 35 const double denhigh_3 = 0.9149758348014341;
Paashaas 0:d8312eea7c50 36
Paashaas 0:d8312eea7c50 37 //biquad 1
Vigilance88 1:93d79d6e96bc 38 const double notch1gain = 1.000000;
Vigilance88 1:93d79d6e96bc 39 const double notch1_b0 = 1;
Vigilance88 1:93d79d6e96bc 40 const double notch1_b1 = -1.61816176147 * notch1gain;
Vigilance88 1:93d79d6e96bc 41 const double notch1_b2 = 1.00000000000 * notch1gain;
Vigilance88 1:93d79d6e96bc 42 const double notch1_a1 = -1.58071559235 * notch1gain;
Vigilance88 1:93d79d6e96bc 43 const double notch1_a2 = 0.97319685401 * notch1gain;
Vigilance88 1:93d79d6e96bc 44
Paashaas 0:d8312eea7c50 45 //biquad 2
Vigilance88 1:93d79d6e96bc 46 const double notch2gain = 0.973674;
Vigilance88 1:93d79d6e96bc 47 const double notch2_b0 = 1 * notch2gain;
Vigilance88 1:93d79d6e96bc 48 const double notch2_b1 = -1.61816176147 * notch2gain;
Vigilance88 1:93d79d6e96bc 49 const double notch2_b2 = 1.00000000000 * notch2gain;
Vigilance88 1:93d79d6e96bc 50 const double notch2_a1 = -1.61244708381 * notch2gain;
Vigilance88 1:93d79d6e96bc 51 const double notch2_a2 = 0.97415116257 * notch2gain;
Paashaas 0:d8312eea7c50 52
Paashaas 0:d8312eea7c50 53 //lowpass filter 7 Hz - envelop
Paashaas 0:d8312eea7c50 54 const double numlow_1 = 0.000119046743110057;
Paashaas 0:d8312eea7c50 55 const double numlow_2 = 0.000238093486220118;
Paashaas 0:d8312eea7c50 56 const double numlow_3 = 0.000119046743110057;
Paashaas 0:d8312eea7c50 57 const double denlow_2 = -1.968902268531908;
Paashaas 0:d8312eea7c50 58 const double denlow_3 = 0.9693784555043481;
Paashaas 0:d8312eea7c50 59
Vigilance88 1:93d79d6e96bc 60 //EMG variables
Vigilance88 1:93d79d6e96bc 61 double emg_biceps; double biceps_power; double bicepsMVC = 0;
Vigilance88 1:93d79d6e96bc 62 double emg_triceps; double triceps_power; double tricepsMVC = 0;
Vigilance88 1:93d79d6e96bc 63 double emg_flexor; double flexor_power; double flexorMVC = 0;
Vigilance88 1:93d79d6e96bc 64 double emg_extens; double extens_power; double extensMVC = 0;
Vigilance88 1:93d79d6e96bc 65
Vigilance88 1:93d79d6e96bc 66
Paashaas 0:d8312eea7c50 67 // storage variables definieren
Paashaas 0:d8312eea7c50 68 double f1_v1 = 0, f1_v2 = 0, f2_v1 = 0, f2_v2 = 0, f3_v1 = 0, f3_v2 = 0, f4_v1 = 0, f4_v2 = 0;
Paashaas 0:d8312eea7c50 69
Paashaas 0:d8312eea7c50 70 double biquad( double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)
Paashaas 0:d8312eea7c50 71
Paashaas 0:d8312eea7c50 72 {
Paashaas 0:d8312eea7c50 73 double v = u - a1*v1 - a2*v2;
Paashaas 0:d8312eea7c50 74 double y = b0*v + b1*v1 + b2*v2;
Paashaas 0:d8312eea7c50 75 v2=v1;
Paashaas 0:d8312eea7c50 76 v1=v;
Paashaas 0:d8312eea7c50 77 return y;
Paashaas 0:d8312eea7c50 78 }
Vigilance88 1:93d79d6e96bc 79
Vigilance88 1:93d79d6e96bc 80
Vigilance88 1:93d79d6e96bc 81
Vigilance88 1:93d79d6e96bc 82 void sample_filter()
Vigilance88 1:93d79d6e96bc 83 {
Paashaas 0:d8312eea7c50 84
Paashaas 0:d8312eea7c50 85 // double u1=..., u2...;
Paashaas 0:d8312eea7c50 86 double u1 = emg1.read();
Paashaas 0:d8312eea7c50 87 double y1 = biquad(u1, f1_v1, f1_v2, denhigh_2, denhigh_3, numhigh_1, numhigh_2, numhigh_3);
Vigilance88 1:93d79d6e96bc 88 double y2 = biquad(y1, f2_v1, f2_v2, notch1_a1, notch1_a2, notch1_b0, notch1_b1, notch1_b2);
Vigilance88 1:93d79d6e96bc 89 double y3 = biquad(y2, f3_v1, f3_v2, notch2_a1, notch2_a2, notch2_b0, notch2_b1, notch2_b2);
Vigilance88 1:93d79d6e96bc 90 double y4 = abs(y3);
Vigilance88 1:93d79d6e96bc 91 double y5 = biquad(y4, f4_v1, f4_v2, denlow_2, denlow_3, numlow_1, numlow_2, numlow_3);
Vigilance88 1:93d79d6e96bc 92
Vigilance88 1:93d79d6e96bc 93 biceps_power=y5;
Vigilance88 4:329e1022cbd3 94 //scope.set(0,u1);
Vigilance88 4:329e1022cbd3 95 //scope.set(1,y4);
Vigilance88 4:329e1022cbd3 96 //scope.set(2,biceps_power);
Vigilance88 4:329e1022cbd3 97 //scope.send();
Vigilance88 1:93d79d6e96bc 98
Vigilance88 1:93d79d6e96bc 99 }
Vigilance88 1:93d79d6e96bc 100
Vigilance88 1:93d79d6e96bc 101
Vigilance88 1:93d79d6e96bc 102 //Start sampling
Vigilance88 1:93d79d6e96bc 103 void start_sampling(void)
Vigilance88 1:93d79d6e96bc 104 {
Vigilance88 1:93d79d6e96bc 105 sample_timer.attach(&sample_filter,0.002); //500 Hz EMG
Vigilance88 5:d7ee4a5612af 106 blue=0; green=0;
Vigilance88 5:d7ee4a5612af 107 pc.printf("||- started sampling -|| \r\n");
Vigilance88 1:93d79d6e96bc 108 }
Vigilance88 1:93d79d6e96bc 109
Vigilance88 1:93d79d6e96bc 110 //stop sampling
Vigilance88 1:93d79d6e96bc 111 void stop_sampling(void)
Vigilance88 1:93d79d6e96bc 112 {
Vigilance88 1:93d79d6e96bc 113 sample_timer.detach();
Vigilance88 5:d7ee4a5612af 114 blue=1; green=1;
Vigilance88 5:d7ee4a5612af 115 pc.printf("||- stopped sampling -|| \r\n");
Paashaas 0:d8312eea7c50 116 }
Vigilance88 1:93d79d6e96bc 117
Vigilance88 2:174b10062c61 118 void calibrate_emg()
Vigilance88 1:93d79d6e96bc 119 {
Vigilance88 1:93d79d6e96bc 120 //double sampletime=0;
Vigilance88 1:93d79d6e96bc 121 //sampletime=+SAMPLE_RATE;
Vigilance88 1:93d79d6e96bc 122 //
Vigilance88 1:93d79d6e96bc 123 // if(sampletime<5)
Vigilance88 1:93d79d6e96bc 124 //int muscle=1;
Vigilance88 1:93d79d6e96bc 125 //for(int index=0; index<2500;index++){ //measure 5 seconds@500hz = 2500 samples
Vigilance88 1:93d79d6e96bc 126
Vigilance88 3:3981bbe6d7b8 127 if(muscle==1){
Vigilance88 1:93d79d6e96bc 128
Vigilance88 1:93d79d6e96bc 129 if(biceps_power>bicepsMVC){
Vigilance88 1:93d79d6e96bc 130 printf("+ ");
Vigilance88 1:93d79d6e96bc 131 bicepsMVC=biceps_power;
Vigilance88 1:93d79d6e96bc 132 }
Vigilance88 1:93d79d6e96bc 133 else
Vigilance88 1:93d79d6e96bc 134 printf("- ");
Vigilance88 2:174b10062c61 135 }
Vigilance88 1:93d79d6e96bc 136
Vigilance88 1:93d79d6e96bc 137 if(muscle==2){
Vigilance88 1:93d79d6e96bc 138
Vigilance88 1:93d79d6e96bc 139 if(triceps_power>tricepsMVC){
Vigilance88 1:93d79d6e96bc 140 tricepsMVC=triceps_power;
Vigilance88 1:93d79d6e96bc 141 }
Vigilance88 1:93d79d6e96bc 142 }
Vigilance88 1:93d79d6e96bc 143
Vigilance88 1:93d79d6e96bc 144 if(muscle==3){
Vigilance88 1:93d79d6e96bc 145
Vigilance88 1:93d79d6e96bc 146 if(flexor_power>flexorMVC){
Vigilance88 1:93d79d6e96bc 147 flexorMVC=flexor_power;
Vigilance88 1:93d79d6e96bc 148 }
Vigilance88 1:93d79d6e96bc 149 }
Vigilance88 1:93d79d6e96bc 150
Vigilance88 1:93d79d6e96bc 151 if(muscle==4){
Vigilance88 1:93d79d6e96bc 152
Vigilance88 1:93d79d6e96bc 153 if(extens_power>extensMVC){
Vigilance88 1:93d79d6e96bc 154 extensMVC=extens_power;
Vigilance88 1:93d79d6e96bc 155 }
Vigilance88 1:93d79d6e96bc 156 }
Vigilance88 1:93d79d6e96bc 157
Vigilance88 1:93d79d6e96bc 158 //}
Vigilance88 1:93d79d6e96bc 159 tijd = tijd + 0.002;
Paashaas 0:d8312eea7c50 160
Vigilance88 1:93d79d6e96bc 161
Vigilance88 1:93d79d6e96bc 162
Vigilance88 1:93d79d6e96bc 163 }
Vigilance88 1:93d79d6e96bc 164
Paashaas 0:d8312eea7c50 165 int main()
Paashaas 0:d8312eea7c50 166 {
Vigilance88 4:329e1022cbd3 167 pc.baud(115200);
Vigilance88 5:d7ee4a5612af 168 red=1; green=1; blue=1;
Vigilance88 1:93d79d6e96bc 169 Ticker timer;
Vigilance88 2:174b10062c61 170
Vigilance88 5:d7ee4a5612af 171 pc.printf("|-- Robot Started --| \r\n");
Vigilance88 3:3981bbe6d7b8 172 pc.printf("Testcode calibration \r\n");
Vigilance88 1:93d79d6e96bc 173 wait(1);
Vigilance88 3:3981bbe6d7b8 174 pc.printf("+ means current sample is higher than stored MVC\r\n");
Vigilance88 3:3981bbe6d7b8 175 pc.printf("- means current sample is lower than stored MVC\r\n");
Vigilance88 1:93d79d6e96bc 176 wait(3);
Vigilance88 5:d7ee4a5612af 177 pc.printf(" Biceps is first. "); wait(1);
Vigilance88 3:3981bbe6d7b8 178 pc.printf(" Press any key to begin... "); wait(1);
Vigilance88 3:3981bbe6d7b8 179 char input;
Vigilance88 3:3981bbe6d7b8 180 input=pc.getc();
Vigilance88 5:d7ee4a5612af 181 pc.putc(input);
Vigilance88 3:3981bbe6d7b8 182 pc.printf(" \r\n Starting in 3... \r\n"); wait(1);
Vigilance88 3:3981bbe6d7b8 183 pc.printf(" \r\n Starting in 2... \r\n"); wait(1);
Vigilance88 3:3981bbe6d7b8 184 pc.printf(" \r\n Starting in 1... \r\n"); wait(1);
Vigilance88 1:93d79d6e96bc 185
Vigilance88 1:93d79d6e96bc 186 start_sampling();
Vigilance88 2:174b10062c61 187 muscle=1;
Vigilance88 1:93d79d6e96bc 188 timer.attach(&calibrate_emg,0.002);
Vigilance88 1:93d79d6e96bc 189 wait(5);
Vigilance88 1:93d79d6e96bc 190 timer.detach();
Vigilance88 5:d7ee4a5612af 191 pc.printf("\r\n MVC = %f \r\n",bicepsMVC);
Vigilance88 1:93d79d6e96bc 192
Vigilance88 3:3981bbe6d7b8 193 pc.printf("Calibrate_emg() exited \r\n");
Vigilance88 3:3981bbe6d7b8 194 pc.printf("Measured time: %f seconds \r\n",tijd);
Vigilance88 5:d7ee4a5612af 195 tijd=0;
Vigilance88 1:93d79d6e96bc 196
Vigilance88 5:d7ee4a5612af 197 // Triceps:
Vigilance88 5:d7ee4a5612af 198 pc.printf(" Triceps is next "); wait(1);
Vigilance88 5:d7ee4a5612af 199 pc.printf(" Press any key to begin... "); wait(1);
Vigilance88 5:d7ee4a5612af 200 input=pc.getc();
Vigilance88 5:d7ee4a5612af 201 pc.putc(input);
Vigilance88 5:d7ee4a5612af 202 pc.printf(" \r\n Starting in 3... \r\n"); wait(1);
Vigilance88 5:d7ee4a5612af 203 pc.printf(" \r\n Starting in 2... \r\n"); wait(1);
Vigilance88 5:d7ee4a5612af 204 pc.printf(" \r\n Starting in 1... \r\n"); wait(1);
Vigilance88 5:d7ee4a5612af 205 start_sampling();
Vigilance88 5:d7ee4a5612af 206 muscle=1;
Vigilance88 5:d7ee4a5612af 207 timer.attach(&calibrate_emg,0.002);
Vigilance88 5:d7ee4a5612af 208 wait(5);
Vigilance88 5:d7ee4a5612af 209 timer.detach();
Vigilance88 5:d7ee4a5612af 210 pc.printf("\r\n Triceps MVC = %f \r\n",tricepsMVC);
Vigilance88 1:93d79d6e96bc 211
Vigilance88 5:d7ee4a5612af 212 pc.printf("Calibrate_emg() exited \r\n");
Vigilance88 5:d7ee4a5612af 213 pc.printf("Measured time: %f seconds \r\n",tijd);
Vigilance88 5:d7ee4a5612af 214 tijd=0;
Vigilance88 5:d7ee4a5612af 215
Vigilance88 5:d7ee4a5612af 216 //Flexor:
Vigilance88 5:d7ee4a5612af 217
Vigilance88 5:d7ee4a5612af 218 //Extensor:
Vigilance88 5:d7ee4a5612af 219
Vigilance88 5:d7ee4a5612af 220 stop_sampling();
Paashaas 0:d8312eea7c50 221 while (true){
Vigilance88 1:93d79d6e96bc 222
Vigilance88 1:93d79d6e96bc 223 wait(1);
Paashaas 0:d8312eea7c50 224 }
Paashaas 0:d8312eea7c50 225 }