Module 9 Super Team / Mbed 2 deprecated EMG_Kevin

EMG_filter zonder moving average

Dependencies:   HIDScope mbed

Fork of EMG by Kevin Hetterscheid

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?

UserRevisionLine numberNew 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 }