Module 9 Super Team / EMG1

Changed some stuff

Fork of EMG by Kevin Hetterscheid

emg.cpp@32:00c6e3502bd9, 2015-10-29 (annotated)

Committer:
dubbie
Date:
Thu Oct 29 17:47:18 2015 +0000
Revision:
32:00c6e3502bd9
Parent:
30:aa0389e04d47
Child:
33:92b645af1388
WORKS DO NOT TOUCH :D;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:32bb76391d89 1 #include "mbed.h"
vsluiter 11:ce72ec658a95 2 #include "HIDScope.h"
AeroKev 24:b7b3e87e0687 3 #define MOV_AVG_NUM 100
dubbie 30:aa0389e04d47 4 #include "math.h"
dubbie 30:aa0389e04d47 5
vsluiter 4:8b298dfada81 6 //Define objects
AeroKev 24:b7b3e87e0687 7 AnalogIn b_emg(A0); //Analog input bicep
AeroKev 24:b7b3e87e0687 8 AnalogIn t_emg(A1); //Analog input tricep
AeroKev 24:b7b3e87e0687 9 AnalogIn p_emg(A2); //Analog input bicep for push
dubbie 30:aa0389e04d47 10 //HIDScope scope_hid(6);
AeroKev 18:e753220c7ba6 11
AeroKev 24:b7b3e87e0687 12 double b_highV[4];
AeroKev 24:b7b3e87e0687 13 double b_lowV[4];
AeroKev 24:b7b3e87e0687 14 double t_highV[4];
AeroKev 24:b7b3e87e0687 15 double t_lowV[4];
AeroKev 24:b7b3e87e0687 16 double p_highV[4];
AeroKev 24:b7b3e87e0687 17 double p_lowV[4];
AeroKev 29:373a2facb3ae 18 double b_min = 10.0f;
AeroKev 29:373a2facb3ae 19 double t_min = 10.0f;
AeroKev 29:373a2facb3ae 20 double p_min = 10.0f;
AeroKev 24:b7b3e87e0687 21
dubbie 30:aa0389e04d47 22
AeroKev 18:e753220c7ba6 23 double filter(double input, double coeff_input[], double coeff_output[], double prev_outputs[])
AeroKev 18:e753220c7ba6 24 {
AeroKev 18:e753220c7ba6 25 double new_input = input;
dubbie 30:aa0389e04d47 26 for(int i=1; i<5; i++)
AeroKev 18:e753220c7ba6 27 new_input -= coeff_input[i] * prev_outputs[i-1];
AeroKev 18:e753220c7ba6 28
AeroKev 18:e753220c7ba6 29 double new_output = coeff_output[0] * new_input;
dubbie 30:aa0389e04d47 30 for(int i=1; i<5; i++)
AeroKev 18:e753220c7ba6 31 new_output += coeff_output[i] * prev_outputs[i-1];
AeroKev 18:e753220c7ba6 32
AeroKev 18:e753220c7ba6 33 // Set the new output as the first value of the 'recent outputs'
AeroKev 18:e753220c7ba6 34 for(int i = 3; i > 0; i--)
AeroKev 18:e753220c7ba6 35 prev_outputs[i] = prev_outputs[i-1];
AeroKev 18:e753220c7ba6 36 prev_outputs[0] = new_input;
AeroKev 18:e753220c7ba6 37 return new_output;
AeroKev 18:e753220c7ba6 38 }
AeroKev 18:e753220c7ba6 39 double fh_b[]= {0.7602, -3.0406, 4.5609, -3.0406, 0.7602};
AeroKev 18:e753220c7ba6 40 double fh_a[]= {1, -3.4532, 4.5041, -2.6273, 0.5778};
AeroKev 24:b7b3e87e0687 41 double b_highpass_filter(double u)
AeroKev 24:b7b3e87e0687 42 {
AeroKev 24:b7b3e87e0687 43 return filter(u, fh_a, fh_b, t_highV);
dubbie 30:aa0389e04d47 44 }
AeroKev 24:b7b3e87e0687 45 double t_highpass_filter(double u)
AeroKev 18:e753220c7ba6 46 {
AeroKev 24:b7b3e87e0687 47 return filter(u, fh_a, fh_b, b_highV);
AeroKev 24:b7b3e87e0687 48 }
AeroKev 24:b7b3e87e0687 49 double p_highpass_filter(double u)
AeroKev 24:b7b3e87e0687 50 {
AeroKev 24:b7b3e87e0687 51 return filter(u, fh_a, fh_b, p_highV);
AeroKev 18:e753220c7ba6 52 }
AeroKev 18:e753220c7ba6 53
AeroKev 21:49362a17495b 54 double fl_b[]= {0.00001329, 0.00005317, 0.00007976, 0.00005317, 0.00001329};
AeroKev 21:49362a17495b 55 double fl_a[]= {1.0000, -3.6717, 5.0680, -3.1160, 0.7199};
AeroKev 24:b7b3e87e0687 56 double b_lowpass_filter(double u)
AeroKev 24:b7b3e87e0687 57 {
dubbie 30:aa0389e04d47 58 return filter(u, fl_a, fl_b, t_lowV);
AeroKev 24:b7b3e87e0687 59 }
AeroKev 24:b7b3e87e0687 60 double t_lowpass_filter(double u)
AeroKev 18:e753220c7ba6 61 {
AeroKev 24:b7b3e87e0687 62
AeroKev 24:b7b3e87e0687 63 return filter(u, fl_a, fl_b, b_lowV);
AeroKev 24:b7b3e87e0687 64 }
AeroKev 24:b7b3e87e0687 65 double p_lowpass_filter(double u)
AeroKev 24:b7b3e87e0687 66 {
AeroKev 24:b7b3e87e0687 67
AeroKev 24:b7b3e87e0687 68 return filter(u, fl_a, fl_b, p_lowV);
AeroKev 18:e753220c7ba6 69 }
vsluiter 2:e314bb3b2d99 70
dubbie 30:aa0389e04d47 71 double rust;
dubbie 30:aa0389e04d47 72 void emg_cal(int emg)
dubbie 30:aa0389e04d47 73 {
dubbie 30:aa0389e04d47 74 double mem[300];
dubbie 30:aa0389e04d47 75 double cur;
dubbie 30:aa0389e04d47 76 double mean;
dubbie 30:aa0389e04d47 77 if(emg == 0) {
dubbie 30:aa0389e04d47 78 for(int i=0; i<900; i++) {
dubbie 30:aa0389e04d47 79 cur = b_emg.read();
dubbie 30:aa0389e04d47 80 double output1 = b_highpass_filter(cur);
dubbie 30:aa0389e04d47 81 double output2 = fabs(output1);
dubbie 30:aa0389e04d47 82 cur = b_lowpass_filter(output2);
dubbie 30:aa0389e04d47 83 if(i >= 300 && i < 600) {
dubbie 30:aa0389e04d47 84 mem[i-300] = cur;
dubbie 30:aa0389e04d47 85 mean += cur;
dubbie 30:aa0389e04d47 86 }
dubbie 30:aa0389e04d47 87 wait(0.002);
dubbie 30:aa0389e04d47 88 }
dubbie 30:aa0389e04d47 89 mean /= 300;
dubbie 30:aa0389e04d47 90 double variance = 0;
dubbie 30:aa0389e04d47 91 for (int i = 0; i < 300; ++i) {
dubbie 30:aa0389e04d47 92 variance += pow((mem[i] - mean),2);
dubbie 30:aa0389e04d47 93 }
dubbie 30:aa0389e04d47 94 variance /=300;
dubbie 30:aa0389e04d47 95 rust = mean-variance;
dubbie 30:aa0389e04d47 96 printf("Rust %f\r\n",rust);
dubbie 30:aa0389e04d47 97 }
dubbie 30:aa0389e04d47 98
dubbie 30:aa0389e04d47 99 if(emg == 1) {
dubbie 30:aa0389e04d47 100 for(int i=0; i<900; i++) {
dubbie 30:aa0389e04d47 101 cur = b_emg.read();
dubbie 30:aa0389e04d47 102 double output1 = b_highpass_filter(cur);
dubbie 30:aa0389e04d47 103 double output2 = fabs(output1);
dubbie 30:aa0389e04d47 104 cur = b_lowpass_filter(output2);
dubbie 30:aa0389e04d47 105 if(i >= 300 && i < 600) {
dubbie 30:aa0389e04d47 106 mem[i-300] = cur;
dubbie 30:aa0389e04d47 107 mean += cur;
dubbie 30:aa0389e04d47 108 }
dubbie 30:aa0389e04d47 109 wait(0.002);
dubbie 30:aa0389e04d47 110 }
dubbie 30:aa0389e04d47 111 mean /= 300;
dubbie 30:aa0389e04d47 112 double variance = 0;
dubbie 30:aa0389e04d47 113 for (int i = 0; i < 300; ++i) {
dubbie 30:aa0389e04d47 114 variance += pow((mem[i] - mean),2);
dubbie 30:aa0389e04d47 115 }
dubbie 30:aa0389e04d47 116 variance /=300;
dubbie 30:aa0389e04d47 117 b_min = mean-variance;
dubbie 30:aa0389e04d47 118 printf("b %f\r\n",b_min);
dubbie 30:aa0389e04d47 119 printf("r %f\r\n",rust);
dubbie 30:aa0389e04d47 120 b_min += rust;
dubbie 30:aa0389e04d47 121 b_min /= 2;
dubbie 30:aa0389e04d47 122 printf("b_min %f\r\n",b_min);
dubbie 30:aa0389e04d47 123 }
dubbie 30:aa0389e04d47 124 else if(emg == 2) {
dubbie 30:aa0389e04d47 125 for(int i=0; i<900; i++) {
dubbie 30:aa0389e04d47 126 cur = t_emg.read();
dubbie 30:aa0389e04d47 127 double output1 = t_highpass_filter(cur);
dubbie 30:aa0389e04d47 128 double output2 = fabs(output1);
dubbie 30:aa0389e04d47 129 cur = t_lowpass_filter(output2);
dubbie 30:aa0389e04d47 130 if(i >= 300 && i < 600) {
dubbie 30:aa0389e04d47 131 mem[i-300] = cur;
dubbie 30:aa0389e04d47 132 mean += cur;
dubbie 30:aa0389e04d47 133 }
dubbie 30:aa0389e04d47 134 wait(0.002);
dubbie 30:aa0389e04d47 135 }
dubbie 30:aa0389e04d47 136 mean /= 300;
dubbie 30:aa0389e04d47 137 double variance = 0;
dubbie 30:aa0389e04d47 138 for (int i = 0; i < 300; ++i) {
dubbie 30:aa0389e04d47 139 variance += pow((mem[i] - mean),2);
dubbie 30:aa0389e04d47 140 }
dubbie 30:aa0389e04d47 141 variance /=300;
dubbie 30:aa0389e04d47 142 t_min = mean-variance;
dubbie 30:aa0389e04d47 143 printf("b %f\r\n",t_min);
dubbie 30:aa0389e04d47 144 printf("r %f\r\n",rust);
dubbie 30:aa0389e04d47 145 t_min += rust;
dubbie 30:aa0389e04d47 146 t_min /= 2;
dubbie 30:aa0389e04d47 147 printf("t_min %f\r\n",t_min);
dubbie 30:aa0389e04d47 148 }
dubbie 30:aa0389e04d47 149
dubbie 30:aa0389e04d47 150 else if(emg == 3) {
dubbie 30:aa0389e04d47 151 for(int i=0; i<900; i++) {
dubbie 30:aa0389e04d47 152 cur = p_emg.read();
dubbie 30:aa0389e04d47 153 double output1 = p_highpass_filter(cur);
dubbie 30:aa0389e04d47 154 double output2 = fabs(output1);
dubbie 30:aa0389e04d47 155 cur = p_lowpass_filter(output2);
dubbie 30:aa0389e04d47 156 if(i >= 300 && i < 600) {
dubbie 30:aa0389e04d47 157 mem[i-300] = cur;
dubbie 30:aa0389e04d47 158 mean += cur;
dubbie 30:aa0389e04d47 159 }
dubbie 30:aa0389e04d47 160 wait(0.002);
dubbie 30:aa0389e04d47 161 }
dubbie 30:aa0389e04d47 162 mean /= 300;
dubbie 30:aa0389e04d47 163 double variance = 0;
dubbie 30:aa0389e04d47 164 for (int i = 0; i < 300; ++i) {
dubbie 30:aa0389e04d47 165 variance += pow((mem[i] - mean),2);
dubbie 30:aa0389e04d47 166 }
dubbie 30:aa0389e04d47 167 variance /=300;
dubbie 30:aa0389e04d47 168 p_min = mean-variance;
dubbie 30:aa0389e04d47 169 printf("b %f\r\n",p_min);
dubbie 30:aa0389e04d47 170 printf("r %f\r\n",rust);
dubbie 30:aa0389e04d47 171 p_min += rust;
dubbie 30:aa0389e04d47 172 p_min /= 2;
dubbie 30:aa0389e04d47 173 printf("p_min %f\r\n",p_min);
dubbie 30:aa0389e04d47 174 }
dubbie 30:aa0389e04d47 175
dubbie 30:aa0389e04d47 176
dubbie 30:aa0389e04d47 177 }
dubbie 30:aa0389e04d47 178
dubbie 30:aa0389e04d47 179
AeroKev 21:49362a17495b 180 /** Sample function
AeroKev 21:49362a17495b 181 * this function samples the emg and sends it to HIDScope
AeroKev 21:49362a17495b 182 **/
AeroKev 24:b7b3e87e0687 183 void sample(double& bicout, double& tricout, double& pushout)
vsluiter 2:e314bb3b2d99 184 {
dubbie 30:aa0389e04d47 185 double last_biceps = 0;
dubbie 30:aa0389e04d47 186 double last_triceps = 0;
dubbie 30:aa0389e04d47 187 double last_push = 0;
dubbie 30:aa0389e04d47 188
AeroKev 24:b7b3e87e0687 189 double b_input = b_emg.read();
AeroKev 24:b7b3e87e0687 190 double output1 = b_highpass_filter(b_input);
AeroKev 18:e753220c7ba6 191 double output2 = fabs(output1);
AeroKev 24:b7b3e87e0687 192 double output3 = b_lowpass_filter(output2);
dubbie 30:aa0389e04d47 193
AeroKev 24:b7b3e87e0687 194 double t_input = t_emg.read();
AeroKev 24:b7b3e87e0687 195 double output4 = t_highpass_filter(t_input);
AeroKev 24:b7b3e87e0687 196 double output5 = fabs(output4);
AeroKev 24:b7b3e87e0687 197 double output6 = t_lowpass_filter(output5);
dubbie 30:aa0389e04d47 198
AeroKev 24:b7b3e87e0687 199 double p_input = p_emg.read();
AeroKev 24:b7b3e87e0687 200 double output7 = p_highpass_filter(p_input);
AeroKev 24:b7b3e87e0687 201 double output8 = fabs(output7);
AeroKev 24:b7b3e87e0687 202 double output9 = p_lowpass_filter(output8);
dubbie 30:aa0389e04d47 203
AeroKev 24:b7b3e87e0687 204 /*double tot = output3;
AeroKev 24:b7b3e87e0687 205 for(int i=0; i<MOV_AVG_NUM-1; i++) {
AeroKev 24:b7b3e87e0687 206 tot += lastOutputs[i];
AeroKev 24:b7b3e87e0687 207 if(i != 0) lastOutputs[i] = lastOutputs[i-1];
AeroKev 24:b7b3e87e0687 208 }
AeroKev 24:b7b3e87e0687 209 lastOutputs[0] = output3;
AeroKev 24:b7b3e87e0687 210 output3 = tot/MOV_AVG_NUM;*/
dubbie 30:aa0389e04d47 211
AeroKev 24:b7b3e87e0687 212 /* Second, set the sampled emg value in channel zero (the first channel) in the 'HIDScope' variable named 'scope' */
AeroKev 24:b7b3e87e0687 213
dubbie 30:aa0389e04d47 214 if (last_biceps == 0 and output3 > b_min+b_min*0.1) {
AeroKev 24:b7b3e87e0687 215 bicout = 1;
dubbie 30:aa0389e04d47 216 } else if (last_biceps == 1 and output3 < b_min-b_min*0.1) {
AeroKev 24:b7b3e87e0687 217 bicout = 0;
AeroKev 24:b7b3e87e0687 218 } else {
dubbie 30:aa0389e04d47 219 bicout = last_biceps;
AeroKev 24:b7b3e87e0687 220 }
dubbie 32:00c6e3502bd9 221 //printf("minbic %f\t",bicout);
AeroKev 24:b7b3e87e0687 222 last_biceps = bicout;
dubbie 30:aa0389e04d47 223
AeroKev 24:b7b3e87e0687 224 /*if (output3>0.06) {//This is the output for the right bicep
AeroKev 24:b7b3e87e0687 225 bicout=1;
AeroKev 24:b7b3e87e0687 226 } else if(output3>0.04) {
AeroKev 24:b7b3e87e0687 227 bicout=0.5;
AeroKev 24:b7b3e87e0687 228 } else {
AeroKev 24:b7b3e87e0687 229 bicout=0;
AeroKev 24:b7b3e87e0687 230 }*/
dubbie 30:aa0389e04d47 231
AeroKev 24:b7b3e87e0687 232 /*if (output6>0.0561) {//this is the output for the right tricep
AeroKev 24:b7b3e87e0687 233 tricout=1;
AeroKev 24:b7b3e87e0687 234 } else if(output6>0.0380) {
AeroKev 24:b7b3e87e0687 235 tricout=0.5;
AeroKev 24:b7b3e87e0687 236 } else {
AeroKev 24:b7b3e87e0687 237 tricout=0;
AeroKev 24:b7b3e87e0687 238 }*/
dubbie 30:aa0389e04d47 239
dubbie 28:5a12ce2fa441 240 if (last_triceps == 0 and output6 > t_min+0.1*t_min) {
AeroKev 24:b7b3e87e0687 241 tricout = 1;
dubbie 30:aa0389e04d47 242 } else if(last_triceps == 1 and output6 < t_min-0.1*t_min) {
dubbie 30:aa0389e04d47 243 tricout = 0;
AeroKev 24:b7b3e87e0687 244 } else {
AeroKev 24:b7b3e87e0687 245 tricout = last_triceps;
AeroKev 24:b7b3e87e0687 246 }
dubbie 32:00c6e3502bd9 247 //printf("mintric %f\t",tricout);
AeroKev 24:b7b3e87e0687 248 last_triceps = tricout;
dubbie 30:aa0389e04d47 249
dubbie 30:aa0389e04d47 250 if (last_push ==0 and output9>p_min+0.1*p_min) {//this is the output for the left bicep (the push motion)
AeroKev 24:b7b3e87e0687 251 pushout=1;
dubbie 30:aa0389e04d47 252 } else if(last_push == 1 and output9<p_min-0.1*p_min) {
dubbie 25:6d1b035f4838 253 pushout=0;
AeroKev 24:b7b3e87e0687 254 } else {
dubbie 25:6d1b035f4838 255 pushout = last_push;
AeroKev 24:b7b3e87e0687 256 }
dubbie 32:00c6e3502bd9 257 //printf("minpush %f\r\n",pushout);
dubbie 30:aa0389e04d47 258
dubbie 25:6d1b035f4838 259 last_push = pushout;
dubbie 30:aa0389e04d47 260
dubbie 30:aa0389e04d47 261 /*scope_hid.set(0,output3);
dubbie 30:aa0389e04d47 262 scope_hid.set(1,output6);
dubbie 30:aa0389e04d47 263 scope_hid.set(2,output9);
dubbie 30:aa0389e04d47 264 scope_hid.set(3,bicout);
dubbie 30:aa0389e04d47 265 scope_hid.set(4,tricout);
dubbie 30:aa0389e04d47 266 scope_hid.set(5,pushout);
dubbie 30:aa0389e04d47 267
dubbie 30:aa0389e04d47 268 scope_hid.send();*/
AeroKev 21:49362a17495b 269 }