Module 9 Super Team / EMG1

Changed some stuff

Fork of EMG by Kevin Hetterscheid

emg.cpp@33:92b645af1388, 2015-11-03 (annotated)

Committer:
AeroKev
Date:
Tue Nov 03 10:15:19 2015 +0000
Revision:
33:92b645af1388
Parent:
32:00c6e3502bd9 
Last Commit;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:32bb76391d89 1 #include "mbed.h"
dubbie 30:aa0389e04d47 2 #include "math.h"
dubbie 30:aa0389e04d47 3
vsluiter 4:8b298dfada81 4 //Define objects
AeroKev 24:b7b3e87e0687 5 AnalogIn b_emg(A0); //Analog input bicep
AeroKev 24:b7b3e87e0687 6 AnalogIn t_emg(A1); //Analog input tricep
AeroKev 24:b7b3e87e0687 7 AnalogIn p_emg(A2); //Analog input bicep for push
AeroKev 18:e753220c7ba6 8
AeroKev 33:92b645af1388 9 double b_highV[4];
AeroKev 33:92b645af1388 10 double b_lowV[4];
AeroKev 33:92b645af1388 11 double t_highV[4];
AeroKev 33:92b645af1388 12 double t_lowV[4];
AeroKev 33:92b645af1388 13 double p_highV[4];
AeroKev 33:92b645af1388 14 double p_lowV[4];
AeroKev 33:92b645af1388 15 double b_min = 10.0f;
AeroKev 33:92b645af1388 16 double t_min = 10.0f;
AeroKev 33:92b645af1388 17 double p_min = 10.0f;
AeroKev 33:92b645af1388 18 double rust;
dubbie 30:aa0389e04d47 19
AeroKev 18:e753220c7ba6 20 double filter(double input, double coeff_input[], double coeff_output[], double prev_outputs[])
AeroKev 18:e753220c7ba6 21 {
AeroKev 18:e753220c7ba6 22 double new_input = input;
dubbie 30:aa0389e04d47 23 for(int i=1; i<5; i++)
AeroKev 18:e753220c7ba6 24 new_input -= coeff_input[i] * prev_outputs[i-1];
AeroKev 18:e753220c7ba6 25
AeroKev 18:e753220c7ba6 26 double new_output = coeff_output[0] * new_input;
dubbie 30:aa0389e04d47 27 for(int i=1; i<5; i++)
AeroKev 18:e753220c7ba6 28 new_output += coeff_output[i] * prev_outputs[i-1];
AeroKev 18:e753220c7ba6 29
AeroKev 18:e753220c7ba6 30 // Set the new output as the first value of the 'recent outputs'
AeroKev 18:e753220c7ba6 31 for(int i = 3; i > 0; i--)
AeroKev 18:e753220c7ba6 32 prev_outputs[i] = prev_outputs[i-1];
AeroKev 18:e753220c7ba6 33 prev_outputs[0] = new_input;
AeroKev 18:e753220c7ba6 34 return new_output;
AeroKev 18:e753220c7ba6 35 }
AeroKev 33:92b645af1388 36
AeroKev 18:e753220c7ba6 37 double fh_b[]= {0.7602, -3.0406, 4.5609, -3.0406, 0.7602};
AeroKev 18:e753220c7ba6 38 double fh_a[]= {1, -3.4532, 4.5041, -2.6273, 0.5778};
AeroKev 33:92b645af1388 39
AeroKev 24:b7b3e87e0687 40 double b_highpass_filter(double u)
AeroKev 24:b7b3e87e0687 41 {
AeroKev 24:b7b3e87e0687 42 return filter(u, fh_a, fh_b, t_highV);
dubbie 30:aa0389e04d47 43 }
AeroKev 33:92b645af1388 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 33:92b645af1388 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 33:92b645af1388 57
AeroKev 24:b7b3e87e0687 58 double b_lowpass_filter(double u)
AeroKev 24:b7b3e87e0687 59 {
dubbie 30:aa0389e04d47 60 return filter(u, fl_a, fl_b, t_lowV);
AeroKev 24:b7b3e87e0687 61 }
AeroKev 33:92b645af1388 62
AeroKev 24:b7b3e87e0687 63 double t_lowpass_filter(double u)
AeroKev 18:e753220c7ba6 64 {
AeroKev 24:b7b3e87e0687 65
AeroKev 24:b7b3e87e0687 66 return filter(u, fl_a, fl_b, b_lowV);
AeroKev 24:b7b3e87e0687 67 }
AeroKev 33:92b645af1388 68
AeroKev 24:b7b3e87e0687 69 double p_lowpass_filter(double u)
AeroKev 24:b7b3e87e0687 70 {
AeroKev 24:b7b3e87e0687 71
AeroKev 24:b7b3e87e0687 72 return filter(u, fl_a, fl_b, p_lowV);
AeroKev 18:e753220c7ba6 73 }
vsluiter 2:e314bb3b2d99 74
dubbie 30:aa0389e04d47 75 void emg_cal(int emg)
dubbie 30:aa0389e04d47 76 {
dubbie 30:aa0389e04d47 77 double mem[300];
dubbie 30:aa0389e04d47 78 double cur;
dubbie 30:aa0389e04d47 79 double mean;
dubbie 30:aa0389e04d47 80 if(emg == 0) {
dubbie 30:aa0389e04d47 81 for(int i=0; i<900; i++) {
dubbie 30:aa0389e04d47 82 cur = b_emg.read();
dubbie 30:aa0389e04d47 83 double output1 = b_highpass_filter(cur);
dubbie 30:aa0389e04d47 84 double output2 = fabs(output1);
dubbie 30:aa0389e04d47 85 cur = b_lowpass_filter(output2);
dubbie 30:aa0389e04d47 86 if(i >= 300 && i < 600) {
dubbie 30:aa0389e04d47 87 mem[i-300] = cur;
dubbie 30:aa0389e04d47 88 mean += cur;
dubbie 30:aa0389e04d47 89 }
dubbie 30:aa0389e04d47 90 wait(0.002);
dubbie 30:aa0389e04d47 91 }
dubbie 30:aa0389e04d47 92 mean /= 300;
dubbie 30:aa0389e04d47 93 double variance = 0;
dubbie 30:aa0389e04d47 94 for (int i = 0; i < 300; ++i) {
dubbie 30:aa0389e04d47 95 variance += pow((mem[i] - mean),2);
dubbie 30:aa0389e04d47 96 }
dubbie 30:aa0389e04d47 97 variance /=300;
dubbie 30:aa0389e04d47 98 rust = mean-variance;
dubbie 30:aa0389e04d47 99 }
dubbie 30:aa0389e04d47 100
dubbie 30:aa0389e04d47 101 if(emg == 1) {
dubbie 30:aa0389e04d47 102 for(int i=0; i<900; i++) {
dubbie 30:aa0389e04d47 103 cur = b_emg.read();
dubbie 30:aa0389e04d47 104 double output1 = b_highpass_filter(cur);
dubbie 30:aa0389e04d47 105 double output2 = fabs(output1);
dubbie 30:aa0389e04d47 106 cur = b_lowpass_filter(output2);
dubbie 30:aa0389e04d47 107 if(i >= 300 && i < 600) {
dubbie 30:aa0389e04d47 108 mem[i-300] = cur;
dubbie 30:aa0389e04d47 109 mean += cur;
dubbie 30:aa0389e04d47 110 }
dubbie 30:aa0389e04d47 111 wait(0.002);
dubbie 30:aa0389e04d47 112 }
dubbie 30:aa0389e04d47 113 mean /= 300;
dubbie 30:aa0389e04d47 114 double variance = 0;
dubbie 30:aa0389e04d47 115 for (int i = 0; i < 300; ++i) {
dubbie 30:aa0389e04d47 116 variance += pow((mem[i] - mean),2);
dubbie 30:aa0389e04d47 117 }
dubbie 30:aa0389e04d47 118 variance /=300;
dubbie 30:aa0389e04d47 119 b_min = mean-variance;
dubbie 30:aa0389e04d47 120 b_min += rust;
dubbie 30:aa0389e04d47 121 b_min /= 2;
dubbie 30:aa0389e04d47 122 }
dubbie 30:aa0389e04d47 123 else if(emg == 2) {
dubbie 30:aa0389e04d47 124 for(int i=0; i<900; i++) {
dubbie 30:aa0389e04d47 125 cur = t_emg.read();
dubbie 30:aa0389e04d47 126 double output1 = t_highpass_filter(cur);
dubbie 30:aa0389e04d47 127 double output2 = fabs(output1);
dubbie 30:aa0389e04d47 128 cur = t_lowpass_filter(output2);
dubbie 30:aa0389e04d47 129 if(i >= 300 && i < 600) {
dubbie 30:aa0389e04d47 130 mem[i-300] = cur;
dubbie 30:aa0389e04d47 131 mean += cur;
dubbie 30:aa0389e04d47 132 }
dubbie 30:aa0389e04d47 133 wait(0.002);
dubbie 30:aa0389e04d47 134 }
dubbie 30:aa0389e04d47 135 mean /= 300;
dubbie 30:aa0389e04d47 136 double variance = 0;
dubbie 30:aa0389e04d47 137 for (int i = 0; i < 300; ++i) {
dubbie 30:aa0389e04d47 138 variance += pow((mem[i] - mean),2);
dubbie 30:aa0389e04d47 139 }
dubbie 30:aa0389e04d47 140 variance /=300;
dubbie 30:aa0389e04d47 141 t_min = mean-variance;
dubbie 30:aa0389e04d47 142 t_min += rust;
dubbie 30:aa0389e04d47 143 t_min /= 2;
dubbie 30:aa0389e04d47 144 }
dubbie 30:aa0389e04d47 145
dubbie 30:aa0389e04d47 146 else if(emg == 3) {
dubbie 30:aa0389e04d47 147 for(int i=0; i<900; i++) {
dubbie 30:aa0389e04d47 148 cur = p_emg.read();
dubbie 30:aa0389e04d47 149 double output1 = p_highpass_filter(cur);
dubbie 30:aa0389e04d47 150 double output2 = fabs(output1);
dubbie 30:aa0389e04d47 151 cur = p_lowpass_filter(output2);
dubbie 30:aa0389e04d47 152 if(i >= 300 && i < 600) {
dubbie 30:aa0389e04d47 153 mem[i-300] = cur;
dubbie 30:aa0389e04d47 154 mean += cur;
dubbie 30:aa0389e04d47 155 }
dubbie 30:aa0389e04d47 156 wait(0.002);
dubbie 30:aa0389e04d47 157 }
dubbie 30:aa0389e04d47 158 mean /= 300;
dubbie 30:aa0389e04d47 159 double variance = 0;
dubbie 30:aa0389e04d47 160 for (int i = 0; i < 300; ++i) {
dubbie 30:aa0389e04d47 161 variance += pow((mem[i] - mean),2);
dubbie 30:aa0389e04d47 162 }
dubbie 30:aa0389e04d47 163 variance /=300;
dubbie 30:aa0389e04d47 164 p_min = mean-variance;
dubbie 30:aa0389e04d47 165 p_min += rust;
dubbie 30:aa0389e04d47 166 p_min /= 2;
dubbie 30:aa0389e04d47 167 }
dubbie 30:aa0389e04d47 168
dubbie 30:aa0389e04d47 169
dubbie 30:aa0389e04d47 170 }
dubbie 30:aa0389e04d47 171
AeroKev 21:49362a17495b 172 /** Sample function
AeroKev 21:49362a17495b 173 * this function samples the emg and sends it to HIDScope
AeroKev 21:49362a17495b 174 **/
AeroKev 24:b7b3e87e0687 175 void sample(double& bicout, double& tricout, double& pushout)
vsluiter 2:e314bb3b2d99 176 {
dubbie 30:aa0389e04d47 177 double last_biceps = 0;
dubbie 30:aa0389e04d47 178 double last_triceps = 0;
dubbie 30:aa0389e04d47 179 double last_push = 0;
dubbie 30:aa0389e04d47 180
AeroKev 24:b7b3e87e0687 181 double b_input = b_emg.read();
AeroKev 24:b7b3e87e0687 182 double output1 = b_highpass_filter(b_input);
AeroKev 18:e753220c7ba6 183 double output2 = fabs(output1);
AeroKev 24:b7b3e87e0687 184 double output3 = b_lowpass_filter(output2);
dubbie 30:aa0389e04d47 185
AeroKev 24:b7b3e87e0687 186 double t_input = t_emg.read();
AeroKev 24:b7b3e87e0687 187 double output4 = t_highpass_filter(t_input);
AeroKev 24:b7b3e87e0687 188 double output5 = fabs(output4);
AeroKev 24:b7b3e87e0687 189 double output6 = t_lowpass_filter(output5);
dubbie 30:aa0389e04d47 190
AeroKev 24:b7b3e87e0687 191 double p_input = p_emg.read();
AeroKev 24:b7b3e87e0687 192 double output7 = p_highpass_filter(p_input);
AeroKev 24:b7b3e87e0687 193 double output8 = fabs(output7);
AeroKev 24:b7b3e87e0687 194 double output9 = p_lowpass_filter(output8);
dubbie 30:aa0389e04d47 195
AeroKev 24:b7b3e87e0687 196 /* Second, set the sampled emg value in channel zero (the first channel) in the 'HIDScope' variable named 'scope' */
AeroKev 24:b7b3e87e0687 197
dubbie 30:aa0389e04d47 198 if (last_biceps == 0 and output3 > b_min+b_min*0.1) {
AeroKev 24:b7b3e87e0687 199 bicout = 1;
dubbie 30:aa0389e04d47 200 } else if (last_biceps == 1 and output3 < b_min-b_min*0.1) {
AeroKev 24:b7b3e87e0687 201 bicout = 0;
AeroKev 24:b7b3e87e0687 202 } else {
dubbie 30:aa0389e04d47 203 bicout = last_biceps;
AeroKev 24:b7b3e87e0687 204 }
AeroKev 24:b7b3e87e0687 205 last_biceps = bicout;
dubbie 30:aa0389e04d47 206
dubbie 28:5a12ce2fa441 207 if (last_triceps == 0 and output6 > t_min+0.1*t_min) {
AeroKev 24:b7b3e87e0687 208 tricout = 1;
dubbie 30:aa0389e04d47 209 } else if(last_triceps == 1 and output6 < t_min-0.1*t_min) {
dubbie 30:aa0389e04d47 210 tricout = 0;
AeroKev 24:b7b3e87e0687 211 } else {
AeroKev 24:b7b3e87e0687 212 tricout = last_triceps;
AeroKev 24:b7b3e87e0687 213 }
AeroKev 24:b7b3e87e0687 214 last_triceps = tricout;
dubbie 30:aa0389e04d47 215
AeroKev 33:92b645af1388 216 if (last_push ==0 and output9>p_min+0.1*p_min) {
AeroKev 24:b7b3e87e0687 217 pushout=1;
dubbie 30:aa0389e04d47 218 } else if(last_push == 1 and output9<p_min-0.1*p_min) {
dubbie 25:6d1b035f4838 219 pushout=0;
AeroKev 24:b7b3e87e0687 220 } else {
dubbie 25:6d1b035f4838 221 pushout = last_push;
AeroKev 24:b7b3e87e0687 222 }
dubbie 30:aa0389e04d47 223
dubbie 25:6d1b035f4838 224 last_push = pushout;
AeroKev 21:49362a17495b 225 }