Module 9 Super Team / EMG1

Changed some stuff

Fork of EMG by Kevin Hetterscheid

emg.cpp@31:6845e4099492, 2015-10-29 (annotated)

Committer:
AeroKev
Date:
Thu Oct 29 17:09:45 2015 +0000
Revision:
31:6845e4099492
Parent:
30:aa0389e04d47 
a

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