Module 9 Super Team
/
EMG_Kevin
EMG_filter zonder moving average
Fork of EMG by
main.cpp@20:75600951afdf, 2015-10-22 (annotated)
- Committer:
- dubbie
- Date:
- Thu Oct 22 14:09:54 2015 +0000
- Revision:
- 20:75600951afdf
- Parent:
- 19:105e719a8aa5
EMG push bic tric;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
vsluiter | 0:32bb76391d89 | 1 | #include "mbed.h" |
vsluiter | 11:ce72ec658a95 | 2 | #include "HIDScope.h" |
vsluiter | 0:32bb76391d89 | 3 | |
AeroKev | 18:e753220c7ba6 | 4 | #define MOV_AVG_NUM 100 |
AeroKev | 18:e753220c7ba6 | 5 | |
vsluiter | 4:8b298dfada81 | 6 | //Define objects |
dubbie | 20:75600951afdf | 7 | AnalogIn b_emg(A0); //Analog input bicep |
dubbie | 20:75600951afdf | 8 | AnalogIn t_emg(A1); //Analog input tricep |
dubbie | 20:75600951afdf | 9 | AnalogIn p_emg(A2); //Analog input bicep for push |
tomlankhorst | 14:f83354387756 | 10 | Ticker sample_timer; |
dubbie | 20:75600951afdf | 11 | HIDScope scope(6); |
AeroKev | 18:e753220c7ba6 | 12 | |
dubbie | 20:75600951afdf | 13 | double b_highV[4]; |
dubbie | 20:75600951afdf | 14 | double b_lowV[4]; |
dubbie | 20:75600951afdf | 15 | double t_highV[4]; |
dubbie | 20:75600951afdf | 16 | double t_lowV[4]; |
dubbie | 20:75600951afdf | 17 | double p_highV[4]; |
dubbie | 20:75600951afdf | 18 | double p_lowV[4]; |
dubbie | 20:75600951afdf | 19 | |
dubbie | 20:75600951afdf | 20 | //double lastOutputs[MOV_AVG_NUM-1]; |
AeroKev | 18:e753220c7ba6 | 21 | |
AeroKev | 18:e753220c7ba6 | 22 | double filter(double input, double coeff_input[], double coeff_output[], double prev_outputs[]) |
AeroKev | 18:e753220c7ba6 | 23 | { |
AeroKev | 18:e753220c7ba6 | 24 | double new_input = input; |
AeroKev | 18:e753220c7ba6 | 25 | for(int i=1; i<5; i++) |
AeroKev | 18:e753220c7ba6 | 26 | new_input -= coeff_input[i] * prev_outputs[i-1]; |
AeroKev | 18:e753220c7ba6 | 27 | |
AeroKev | 18:e753220c7ba6 | 28 | double new_output = coeff_output[0] * new_input; |
AeroKev | 18:e753220c7ba6 | 29 | for(int i=1; i<5; i++) |
AeroKev | 18:e753220c7ba6 | 30 | new_output += coeff_output[i] * prev_outputs[i-1]; |
AeroKev | 18:e753220c7ba6 | 31 | |
AeroKev | 18:e753220c7ba6 | 32 | // Set the new output as the first value of the 'recent outputs' |
AeroKev | 18:e753220c7ba6 | 33 | for(int i = 3; i > 0; i--) |
AeroKev | 18:e753220c7ba6 | 34 | prev_outputs[i] = prev_outputs[i-1]; |
AeroKev | 18:e753220c7ba6 | 35 | prev_outputs[0] = new_input; |
AeroKev | 18:e753220c7ba6 | 36 | return new_output; |
AeroKev | 18:e753220c7ba6 | 37 | } |
AeroKev | 18:e753220c7ba6 | 38 | double fh_b[]= {0.7602, -3.0406, 4.5609, -3.0406, 0.7602}; |
AeroKev | 18:e753220c7ba6 | 39 | double fh_a[]= {1, -3.4532, 4.5041, -2.6273, 0.5778}; |
dubbie | 20:75600951afdf | 40 | double b_highpass_filter(double u) |
dubbie | 20:75600951afdf | 41 | { |
dubbie | 20:75600951afdf | 42 | return filter(u, fh_a, fh_b, t_highV); |
dubbie | 20:75600951afdf | 43 | } |
dubbie | 20:75600951afdf | 44 | double t_highpass_filter(double u) |
AeroKev | 18:e753220c7ba6 | 45 | { |
dubbie | 20:75600951afdf | 46 | return filter(u, fh_a, fh_b, b_highV); |
dubbie | 20:75600951afdf | 47 | } |
dubbie | 20:75600951afdf | 48 | double p_highpass_filter(double u) |
dubbie | 20:75600951afdf | 49 | { |
dubbie | 20:75600951afdf | 50 | return filter(u, fh_a, fh_b, p_highV); |
AeroKev | 18:e753220c7ba6 | 51 | } |
AeroKev | 18:e753220c7ba6 | 52 | |
dubbie | 19:105e719a8aa5 | 53 | double fl_b[]= {0.00001329, 0.00005317, 0.00007976, 0.00005317, 0.00001329}; |
dubbie | 19:105e719a8aa5 | 54 | double fl_a[]= {1.0000, -3.6717, 5.0680, -3.1160, 0.7199}; |
dubbie | 20:75600951afdf | 55 | double b_lowpass_filter(double u) |
dubbie | 20:75600951afdf | 56 | { |
dubbie | 20:75600951afdf | 57 | return filter(u, fl_a, fl_b, t_lowV); |
dubbie | 20:75600951afdf | 58 | } |
dubbie | 20:75600951afdf | 59 | double t_lowpass_filter(double u) |
AeroKev | 18:e753220c7ba6 | 60 | { |
dubbie | 20:75600951afdf | 61 | |
dubbie | 20:75600951afdf | 62 | return filter(u, fl_a, fl_b, b_lowV); |
dubbie | 20:75600951afdf | 63 | } |
dubbie | 20:75600951afdf | 64 | double p_lowpass_filter(double u) |
dubbie | 20:75600951afdf | 65 | { |
dubbie | 20:75600951afdf | 66 | |
dubbie | 20:75600951afdf | 67 | return filter(u, fl_a, fl_b, p_lowV); |
AeroKev | 18:e753220c7ba6 | 68 | } |
vsluiter | 2:e314bb3b2d99 | 69 | |
dubbie | 20:75600951afdf | 70 | double last_biceps = 0; |
dubbie | 20:75600951afdf | 71 | double last_triceps = 0; |
dubbie | 20:75600951afdf | 72 | double last_push = 0; |
tomlankhorst | 14:f83354387756 | 73 | /** Sample function |
tomlankhorst | 14:f83354387756 | 74 | * this function samples the emg and sends it to HIDScope |
tomlankhorst | 14:f83354387756 | 75 | **/ |
tomlankhorst | 14:f83354387756 | 76 | void sample() |
vsluiter | 2:e314bb3b2d99 | 77 | { |
dubbie | 20:75600951afdf | 78 | double b_input = b_emg.read(); |
dubbie | 20:75600951afdf | 79 | double output1 = b_highpass_filter(b_input); |
AeroKev | 18:e753220c7ba6 | 80 | double output2 = fabs(output1); |
dubbie | 20:75600951afdf | 81 | double output3 = b_lowpass_filter(output2); |
dubbie | 20:75600951afdf | 82 | |
dubbie | 20:75600951afdf | 83 | double t_input = t_emg.read(); |
dubbie | 20:75600951afdf | 84 | double output4 = t_highpass_filter(t_input); |
dubbie | 20:75600951afdf | 85 | double output5 = fabs(output4); |
dubbie | 20:75600951afdf | 86 | double output6 = t_lowpass_filter(output5); |
dubbie | 20:75600951afdf | 87 | |
dubbie | 20:75600951afdf | 88 | double p_input = p_emg.read(); |
dubbie | 20:75600951afdf | 89 | double output7 = p_highpass_filter(p_input); |
dubbie | 20:75600951afdf | 90 | double output8 = fabs(output7); |
dubbie | 20:75600951afdf | 91 | double output9 = p_lowpass_filter(output8); |
AeroKev | 18:e753220c7ba6 | 92 | |
dubbie | 19:105e719a8aa5 | 93 | /*double tot = output3; |
AeroKev | 18:e753220c7ba6 | 94 | for(int i=0; i<MOV_AVG_NUM-1; i++) { |
AeroKev | 18:e753220c7ba6 | 95 | tot += lastOutputs[i]; |
AeroKev | 18:e753220c7ba6 | 96 | if(i != 0) lastOutputs[i] = lastOutputs[i-1]; |
AeroKev | 18:e753220c7ba6 | 97 | } |
AeroKev | 18:e753220c7ba6 | 98 | lastOutputs[0] = output3; |
dubbie | 19:105e719a8aa5 | 99 | output3 = tot/MOV_AVG_NUM;*/ |
AeroKev | 18:e753220c7ba6 | 100 | |
tomlankhorst | 14:f83354387756 | 101 | /* Second, set the sampled emg value in channel zero (the first channel) in the 'HIDScope' variable named 'scope' */ |
dubbie | 20:75600951afdf | 102 | |
dubbie | 20:75600951afdf | 103 | double bicout; |
dubbie | 20:75600951afdf | 104 | double tricout; |
dubbie | 20:75600951afdf | 105 | double pushout; |
dubbie | 20:75600951afdf | 106 | if (last_biceps == 0 and output3 > 0.06) { |
dubbie | 20:75600951afdf | 107 | bicout = 1; |
dubbie | 20:75600951afdf | 108 | } else if (last_biceps == 1 and output3 < 0.045) { |
dubbie | 20:75600951afdf | 109 | bicout = 0; |
dubbie | 20:75600951afdf | 110 | } else { |
dubbie | 20:75600951afdf | 111 | bicout = last_biceps; |
dubbie | 20:75600951afdf | 112 | } |
dubbie | 20:75600951afdf | 113 | last_biceps = bicout; |
dubbie | 20:75600951afdf | 114 | |
dubbie | 20:75600951afdf | 115 | /*if (output3>0.06) {//This is the output for the right bicep |
dubbie | 20:75600951afdf | 116 | bicout=1; |
dubbie | 20:75600951afdf | 117 | } else if(output3>0.04) { |
dubbie | 20:75600951afdf | 118 | bicout=0.5; |
dubbie | 20:75600951afdf | 119 | } else { |
dubbie | 20:75600951afdf | 120 | bicout=0; |
dubbie | 20:75600951afdf | 121 | }*/ |
dubbie | 20:75600951afdf | 122 | |
dubbie | 20:75600951afdf | 123 | /*if (output6>0.0561) {//this is the output for the right tricep |
dubbie | 20:75600951afdf | 124 | tricout=1; |
dubbie | 20:75600951afdf | 125 | } else if(output6>0.0380) { |
dubbie | 20:75600951afdf | 126 | tricout=0.5; |
dubbie | 20:75600951afdf | 127 | } else { |
dubbie | 20:75600951afdf | 128 | tricout=0; |
dubbie | 20:75600951afdf | 129 | }*/ |
dubbie | 20:75600951afdf | 130 | |
dubbie | 20:75600951afdf | 131 | if (last_triceps == 0 and output6 > 0.045) { |
dubbie | 20:75600951afdf | 132 | tricout = 1; |
dubbie | 20:75600951afdf | 133 | } else if(last_triceps == 1 and output6 < 0.04) { |
dubbie | 20:75600951afdf | 134 | tricout = 0; |
dubbie | 20:75600951afdf | 135 | } else { |
dubbie | 20:75600951afdf | 136 | tricout = last_triceps; |
dubbie | 20:75600951afdf | 137 | } |
dubbie | 20:75600951afdf | 138 | last_triceps = tricout; |
dubbie | 20:75600951afdf | 139 | |
dubbie | 20:75600951afdf | 140 | if (output9>0.05) {//this is the output for the left bicep (the push motion) |
dubbie | 20:75600951afdf | 141 | pushout=1; |
dubbie | 20:75600951afdf | 142 | } else { |
dubbie | 20:75600951afdf | 143 | pushout=0; |
dubbie | 20:75600951afdf | 144 | } |
dubbie | 20:75600951afdf | 145 | |
dubbie | 20:75600951afdf | 146 | scope.set(0,output3); |
dubbie | 20:75600951afdf | 147 | scope.set(1,output6); |
dubbie | 20:75600951afdf | 148 | scope.set(2,output9); |
dubbie | 20:75600951afdf | 149 | scope.set(3,bicout); |
dubbie | 20:75600951afdf | 150 | scope.set(4,tricout); |
dubbie | 20:75600951afdf | 151 | scope.set(5,pushout); |
tomlankhorst | 16:9f7797ffd0fb | 152 | /* Repeat the step above if required for more channels (channel 0 up to 5 = 6 channels) */ |
tomlankhorst | 14:f83354387756 | 153 | /* Finally, send all channels to the PC at once */ |
vsluiter | 11:ce72ec658a95 | 154 | scope.send(); |
vsluiter | 2:e314bb3b2d99 | 155 | } |
vsluiter | 0:32bb76391d89 | 156 | |
vsluiter | 0:32bb76391d89 | 157 | int main() |
vsluiter | 0:32bb76391d89 | 158 | { |
tomlankhorst | 14:f83354387756 | 159 | /**Attach the 'sample' function to the timer 'sample_timer'. |
tomlankhorst | 14:f83354387756 | 160 | * this ensures that 'sample' is executed every... 0.002 seconds |
vsluiter | 4:8b298dfada81 | 161 | */ |
tomlankhorst | 14:f83354387756 | 162 | sample_timer.attach(&sample, 0.002); |
tomlankhorst | 15:0da764eea774 | 163 | |
tomlankhorst | 14:f83354387756 | 164 | /*empty loop, sample() is executed periodically*/ |
tomlankhorst | 15:0da764eea774 | 165 | while(1) {} |
vsluiter | 0:32bb76391d89 | 166 | } |