Jorick Leferink
/
programma_filter
programma voor filter
Fork of BMT-K9-Groep7 by
main.cpp@5:12b9a5cfcf73, 2013-10-18 (annotated)
- Committer:
- jorick92
- Date:
- Fri Oct 18 12:31:17 2013 +0000
- Revision:
- 5:12b9a5cfcf73
- Parent:
- 4:d0b4c806f4ea
nog niet helemaaaal af, maar met de functie is al wel te gebruiken, mits aangeroepen zoals in de looper.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
vsluiter | 0:3843c26cd5fd | 1 | #include "mbed.h" |
vsluiter | 0:3843c26cd5fd | 2 | #include "MODSERIAL.h" |
vsluiter | 0:3843c26cd5fd | 3 | |
vsluiter | 0:3843c26cd5fd | 4 | //Define objects |
vsluiter | 0:3843c26cd5fd | 5 | AnalogIn emg_biceps(PTB0); //Analog input |
jorick92 | 5:12b9a5cfcf73 | 6 | AnalogIn emg_triceps(PTB1); |
jorick92 | 5:12b9a5cfcf73 | 7 | AnalogIn emg_flexoren(PTB2); |
jorick92 | 5:12b9a5cfcf73 | 8 | AnalogIn emg_extensoren(PTB3); |
vsluiter | 0:3843c26cd5fd | 9 | PwmOut red(LED_RED); // EMG meting |
vsluiter | 0:3843c26cd5fd | 10 | // PwmOut blue(LED_BLUE); // uitgangssignaal controle |
vsluiter | 0:3843c26cd5fd | 11 | // PwmOut green(LED_GREEN); |
vsluiter | 0:3843c26cd5fd | 12 | |
vsluiter | 0:3843c26cd5fd | 13 | Ticker timer; |
vsluiter | 0:3843c26cd5fd | 14 | MODSERIAL pc(USBTX,USBRX,64,1024); |
vsluiter | 0:3843c26cd5fd | 15 | |
vsluiter | 2:60fc72e8ff66 | 16 | #define GAIN_BICEPS 1 |
vsluiter | 2:60fc72e8ff66 | 17 | #define MAXCOUNT 50 |
vsluiter | 0:3843c26cd5fd | 18 | |
vsluiter | 0:3843c26cd5fd | 19 | #define NUM0 0.8841 // constante |
vsluiter | 0:3843c26cd5fd | 20 | #define NUM1 -3.53647 // z^-1 |
vsluiter | 0:3843c26cd5fd | 21 | #define NUM2 5.3046 // z^-2etc. |
vsluiter | 0:3843c26cd5fd | 22 | #define NUM3 -3.5364 |
vsluiter | 0:3843c26cd5fd | 23 | #define NUM4 0.8841 |
vsluiter | 0:3843c26cd5fd | 24 | |
vsluiter | 0:3843c26cd5fd | 25 | #define DEN0 1 // constante |
vsluiter | 0:3843c26cd5fd | 26 | #define DEN1 -3.7538 |
vsluiter | 0:3843c26cd5fd | 27 | #define DEN2 5.2912 |
vsluiter | 0:3843c26cd5fd | 28 | #define DEN3 -3.3189 |
vsluiter | 0:3843c26cd5fd | 29 | #define DEN4 0.7816 |
vsluiter | 0:3843c26cd5fd | 30 | |
vsluiter | 4:d0b4c806f4ea | 31 | |
vsluiter | 0:3843c26cd5fd | 32 | /* hou in de gaten welke waarden globaal gedefinieerd moeten worden*/ |
jorick92 | 5:12b9a5cfcf73 | 33 | |
jorick92 | 5:12b9a5cfcf73 | 34 | |
jorick92 | 5:12b9a5cfcf73 | 35 | |
jorick92 | 5:12b9a5cfcf73 | 36 | |
jorick92 | 5:12b9a5cfcf73 | 37 | //filter functie definieren. filter(signal_number) |
jorick92 | 5:12b9a5cfcf73 | 38 | //signal_number=1 --> biceps filteren |
jorick92 | 5:12b9a5cfcf73 | 39 | //signal_number=2 --> triceps filteren |
jorick92 | 5:12b9a5cfcf73 | 40 | //signal_number=3 --> flexoren filteren |
jorick92 | 5:12b9a5cfcf73 | 41 | //signal_number=4 --> extensoren filteren |
jorick92 | 5:12b9a5cfcf73 | 42 | |
jorick92 | 5:12b9a5cfcf73 | 43 | float filter(int signal_number){ |
jorick92 | 5:12b9a5cfcf73 | 44 | //static variables keep their values between function calls |
jorick92 | 5:12b9a5cfcf73 | 45 | //the assignents are only executed the first iteration. |
jorick92 | 5:12b9a5cfcf73 | 46 | |
jorick92 | 5:12b9a5cfcf73 | 47 | //variabelen biceps definieren |
jorick92 | 5:12b9a5cfcf73 | 48 | static float in0_biceps =0 , in1_biceps =0, in2_biceps = 0, in3_biceps = 0, in4_biceps = 0; |
jorick92 | 5:12b9a5cfcf73 | 49 | static float out0_biceps = 0, out1_biceps = 0 , out2_biceps = 0, out3_biceps = 0, out4_biceps = 0; |
jorick92 | 5:12b9a5cfcf73 | 50 | |
jorick92 | 5:12b9a5cfcf73 | 51 | // variabelen triceps definieren |
jorick92 | 5:12b9a5cfcf73 | 52 | static float in0_triceps = 0, in1_triceps = 0, in2_triceps = 0, in3_triceps = 0, in4_triceps = 0; |
jorick92 | 5:12b9a5cfcf73 | 53 | static float out0_triceps = 0, out1_triceps = 0, out2_triceps = 0, out3_triceps = 0, out4_triceps = 0; |
jorick92 | 5:12b9a5cfcf73 | 54 | |
jorick92 | 5:12b9a5cfcf73 | 55 | //variabelen flexoren definieren |
jorick92 | 5:12b9a5cfcf73 | 56 | static float in0_flexoren = 0, in1_flexoren = 0, in2_flexoren = 0, in3_flexoren = 0, in4_flexoren = 0; |
jorick92 | 5:12b9a5cfcf73 | 57 | static float out0_flexoren = 0, out1_flexoren = 0, out2_flexoren = 0, out3_flexoren = 0, out4_flexoren = 0; |
jorick92 | 5:12b9a5cfcf73 | 58 | |
jorick92 | 5:12b9a5cfcf73 | 59 | //variablen extensoren definieren |
jorick92 | 5:12b9a5cfcf73 | 60 | static float in0_extensoren = 0, in1_extensoren = 0, in2_extensoren = 0, in3_extensoren = 0, in4_extensoren = 0; |
jorick92 | 5:12b9a5cfcf73 | 61 | static float out0_extensoren = 0, out1_extensoren = 0, out2_extensoren = 0, out3_extensoren = 0, out4_extensoren = 0; |
jorick92 | 5:12b9a5cfcf73 | 62 | |
jorick92 | 5:12b9a5cfcf73 | 63 | //overige variabelen definieren |
jorick92 | 5:12b9a5cfcf73 | 64 | static float count_biceps = 0, count_triceps = 0, count_extensoren = 0, count_flexoren = 0; |
jorick92 | 5:12b9a5cfcf73 | 65 | static float square_biceps = 0, square_triceps = 0, square_flexoren = 0, square_extensoren = 0; |
jorick92 | 5:12b9a5cfcf73 | 66 | static float sum_biceps = 0, sum_triceps = 0, sum_flexoren = 0, sum_extensoren = 0; |
jorick92 | 5:12b9a5cfcf73 | 67 | static float mean_biceps = 0.2, mean_triceps = 0.2, mean_flexoren = 0.2, mean_extensoren = 0.2; |
jorick92 | 5:12b9a5cfcf73 | 68 | static float sig_out_biceps, sig_out_triceps, sig_out_extensoren, sig_out_flexoren; |
jorick92 | 5:12b9a5cfcf73 | 69 | float emg_abs, sig_out; |
jorick92 | 5:12b9a5cfcf73 | 70 | |
jorick92 | 5:12b9a5cfcf73 | 71 | switch (signal_number){ |
jorick92 | 5:12b9a5cfcf73 | 72 | case 1: |
jorick92 | 5:12b9a5cfcf73 | 73 | //biceps filteren |
jorick92 | 5:12b9a5cfcf73 | 74 | in4_biceps = in3_biceps; in3_biceps = in2_biceps; in2_biceps = in1_biceps; in1_biceps = in0_biceps; |
jorick92 | 5:12b9a5cfcf73 | 75 | in0_biceps = emg_biceps.read(); |
jorick92 | 5:12b9a5cfcf73 | 76 | out4_biceps = out3_biceps; out3_biceps = out2_biceps; out2_biceps = out1_biceps; out1_biceps = out0_biceps; |
jorick92 | 5:12b9a5cfcf73 | 77 | out0_biceps = (NUM0*in0_biceps + NUM1*in1_biceps + NUM2*in2_biceps + NUM3*in3_biceps + NUM4*in4_biceps - DEN1*out1_biceps - DEN2*out2_biceps - DEN3*out3_biceps - DEN4*out4_biceps ) / DEN0; |
jorick92 | 5:12b9a5cfcf73 | 78 | |
jorick92 | 5:12b9a5cfcf73 | 79 | //std deviatie bepalen, om de 50 metingen |
jorick92 | 5:12b9a5cfcf73 | 80 | emg_abs = fabs(out0_biceps); |
jorick92 | 5:12b9a5cfcf73 | 81 | sum_biceps += out0_biceps; |
jorick92 | 5:12b9a5cfcf73 | 82 | square_biceps += (emg_abs - mean_biceps)*(emg_abs - mean_biceps); //neem absolute waarde, kwadrateer, voeg toe aan vorige square |
jorick92 | 5:12b9a5cfcf73 | 83 | // voeg rest EMG's toe, variabelen alleen _spier geven als het nodig is. |
jorick92 | 5:12b9a5cfcf73 | 84 | count_biceps += 1; // hou bij hoeveel squares er zijn opgeteld |
jorick92 | 5:12b9a5cfcf73 | 85 | if (count_biceps >= MAXCOUNT) |
jorick92 | 5:12b9a5cfcf73 | 86 | { sig_out_biceps = sqrt(square_biceps/count_biceps); |
jorick92 | 5:12b9a5cfcf73 | 87 | mean_biceps = sum_biceps/count_biceps; |
jorick92 | 5:12b9a5cfcf73 | 88 | count_biceps = 0; square_biceps = 0; sum_biceps = 0; // en neem de RMS als er genoeg zijn geteld, stuur die door, en reset sqaure en count |
jorick92 | 5:12b9a5cfcf73 | 89 | sig_out = sig_out_biceps; |
jorick92 | 5:12b9a5cfcf73 | 90 | } |
jorick92 | 5:12b9a5cfcf73 | 91 | else sig_out = -1; |
jorick92 | 5:12b9a5cfcf73 | 92 | break; |
jorick92 | 5:12b9a5cfcf73 | 93 | case 2: |
jorick92 | 5:12b9a5cfcf73 | 94 | //triceps filteren |
jorick92 | 5:12b9a5cfcf73 | 95 | in4_triceps = in3_triceps; in3_triceps = in2_triceps; in2_triceps = in1_triceps; in1_triceps = in0_triceps; |
jorick92 | 5:12b9a5cfcf73 | 96 | in0_triceps = emg_triceps.read(); |
jorick92 | 5:12b9a5cfcf73 | 97 | out4_triceps = out3_triceps; out3_triceps = out2_triceps; out2_triceps = out1_triceps; out1_triceps = out0_triceps; |
jorick92 | 5:12b9a5cfcf73 | 98 | out0_biceps = (NUM0*in0_triceps + NUM1*in1_triceps + NUM2*in2_triceps + NUM3*in3_triceps + NUM4*in4_triceps - DEN1*out1_triceps - DEN2*out2_triceps - DEN3*out3_triceps - DEN4*out4_triceps ) / DEN0; |
jorick92 | 5:12b9a5cfcf73 | 99 | |
jorick92 | 5:12b9a5cfcf73 | 100 | //std deviatie bepalen om de 50 metingen |
jorick92 | 5:12b9a5cfcf73 | 101 | emg_abs = fabs(out0_triceps); |
jorick92 | 5:12b9a5cfcf73 | 102 | sum_triceps += out0_triceps; |
jorick92 | 5:12b9a5cfcf73 | 103 | square_triceps += (emg_abs - mean_triceps)*(emg_abs - mean_triceps); //neem absolute waarde, kwadrateer, voeg toe aan vorige square |
jorick92 | 5:12b9a5cfcf73 | 104 | // voeg rest EMG's toe, variabelen alleen _spier geven als het nodig is. |
jorick92 | 5:12b9a5cfcf73 | 105 | count_triceps += 1; // hou bij hoeveel squares er zijn opgeteld |
jorick92 | 5:12b9a5cfcf73 | 106 | if (count_triceps >= MAXCOUNT) |
jorick92 | 5:12b9a5cfcf73 | 107 | { sig_out_triceps = sqrt(square_triceps/count_triceps); |
jorick92 | 5:12b9a5cfcf73 | 108 | mean_triceps = sum_triceps/count_triceps; |
jorick92 | 5:12b9a5cfcf73 | 109 | count_triceps = 0; square_triceps = 0; sum_triceps = 0; // en neem de RMS als er genoeg zijn geteld, stuur die door, en reset sqaure en count |
jorick92 | 5:12b9a5cfcf73 | 110 | sig_out = sig_out_triceps; |
jorick92 | 5:12b9a5cfcf73 | 111 | } |
jorick92 | 5:12b9a5cfcf73 | 112 | else sig_out = -1; |
jorick92 | 5:12b9a5cfcf73 | 113 | break; |
jorick92 | 5:12b9a5cfcf73 | 114 | case 3: |
jorick92 | 5:12b9a5cfcf73 | 115 | //flexoren filteren |
jorick92 | 5:12b9a5cfcf73 | 116 | in4_flexoren = in3_flexoren; in3_flexoren = in2_flexoren; in2_flexoren = in1_flexoren; in1_flexoren = in0_flexoren; |
jorick92 | 5:12b9a5cfcf73 | 117 | in0_flexoren = emg_flexoren.read(); |
jorick92 | 5:12b9a5cfcf73 | 118 | out4_flexoren = out3_flexoren; out3_flexoren = out2_flexoren; out2_flexoren = out1_flexoren; out1_flexoren = out0_flexoren; |
jorick92 | 5:12b9a5cfcf73 | 119 | out0_flexoren = (NUM0*in0_flexoren + NUM1*in1_flexoren + NUM2*in2_flexoren + NUM3*in3_flexoren + NUM4*in4_flexoren - DEN1*out1_flexoren - DEN2*out2_flexoren - DEN3*out3_flexoren - DEN4*out4_flexoren) / DEN0; |
jorick92 | 5:12b9a5cfcf73 | 120 | |
jorick92 | 5:12b9a5cfcf73 | 121 | //std deviatie bepalen om de 50 metingen |
jorick92 | 5:12b9a5cfcf73 | 122 | emg_abs = fabs(out0_flexoren); |
jorick92 | 5:12b9a5cfcf73 | 123 | sum_flexoren += out0_flexoren; |
jorick92 | 5:12b9a5cfcf73 | 124 | square_flexoren += (emg_abs - mean_flexoren)*(emg_abs - mean_flexoren); //neem absolute waarde, kwadrateer, voeg toe aan vorige square |
jorick92 | 5:12b9a5cfcf73 | 125 | // voeg rest EMG's toe, variabelen alleen _spier geven als het nodig is. |
jorick92 | 5:12b9a5cfcf73 | 126 | count_flexoren += 1; // hou bij hoeveel squares er zijn opgeteld |
jorick92 | 5:12b9a5cfcf73 | 127 | if (count_flexoren >= MAXCOUNT) |
jorick92 | 5:12b9a5cfcf73 | 128 | { sig_out_flexoren = sqrt(square_flexoren/count_flexoren); |
jorick92 | 5:12b9a5cfcf73 | 129 | mean_flexoren = sum_flexoren/count_flexoren; |
jorick92 | 5:12b9a5cfcf73 | 130 | count_flexoren = 0; square_flexoren = 0; sum_flexoren = 0; // en neem de RMS als er genoeg zijn geteld, stuur die door, en reset sqaure en count |
jorick92 | 5:12b9a5cfcf73 | 131 | sig_out = sig_out_flexoren; |
jorick92 | 5:12b9a5cfcf73 | 132 | } |
jorick92 | 5:12b9a5cfcf73 | 133 | else sig_out = -1; |
jorick92 | 5:12b9a5cfcf73 | 134 | break; |
jorick92 | 5:12b9a5cfcf73 | 135 | case 4: |
jorick92 | 5:12b9a5cfcf73 | 136 | //extensoren filteren |
jorick92 | 5:12b9a5cfcf73 | 137 | in4_extensoren = in3_extensoren; in3_extensoren = in2_extensoren; in2_extensoren = in1_extensoren; in1_extensoren = in0_extensoren; |
jorick92 | 5:12b9a5cfcf73 | 138 | in0_extensoren = emg_extensoren.read(); |
jorick92 | 5:12b9a5cfcf73 | 139 | out4_extensoren = out3_extensoren; out3_extensoren = out2_extensoren; out2_extensoren = out1_extensoren; out1_extensoren = out0_extensoren; |
jorick92 | 5:12b9a5cfcf73 | 140 | out0_extensoren = (NUM0*in0_extensoren + NUM1*in1_extensoren + NUM2*in2_extensoren + NUM3*in3_extensoren + NUM4*in4_extensoren - DEN1*out1_extensoren - DEN2*out2_extensoren - DEN3*out3_extensoren - DEN4*out4_extensoren) / DEN0; |
jorick92 | 5:12b9a5cfcf73 | 141 | |
jorick92 | 5:12b9a5cfcf73 | 142 | //std deviatie bepalen om de 50 metingen |
jorick92 | 5:12b9a5cfcf73 | 143 | emg_abs = fabs(out0_extensoren); |
jorick92 | 5:12b9a5cfcf73 | 144 | sum_extensoren += out0_extensoren; |
jorick92 | 5:12b9a5cfcf73 | 145 | square_extensoren += (emg_abs - mean_extensoren)*(emg_abs - mean_extensoren); //neem absolute waarde, kwadrateer, voeg toe aan vorige square |
jorick92 | 5:12b9a5cfcf73 | 146 | // voeg rest EMG's toe, variabelen alleen _spier geven als het nodig is. |
jorick92 | 5:12b9a5cfcf73 | 147 | count_extensoren += 1; // hou bij hoeveel squares er zijn opgeteld |
jorick92 | 5:12b9a5cfcf73 | 148 | if (count_extensoren >= MAXCOUNT) |
jorick92 | 5:12b9a5cfcf73 | 149 | { sig_out_extensoren = sqrt(square_extensoren/count_extensoren); |
jorick92 | 5:12b9a5cfcf73 | 150 | mean_extensoren = sum_extensoren/count_extensoren; |
jorick92 | 5:12b9a5cfcf73 | 151 | count_extensoren = 0; square_extensoren = 0; sum_extensoren = 0; // en neem de RMS als er genoeg zijn geteld, stuur die door, en reset sqaure en count |
jorick92 | 5:12b9a5cfcf73 | 152 | sig_out=sig_out_extensoren; |
jorick92 | 5:12b9a5cfcf73 | 153 | } |
jorick92 | 5:12b9a5cfcf73 | 154 | else sig_out = -1; |
jorick92 | 5:12b9a5cfcf73 | 155 | break; |
jorick92 | 5:12b9a5cfcf73 | 156 | } |
jorick92 | 5:12b9a5cfcf73 | 157 | |
jorick92 | 5:12b9a5cfcf73 | 158 | return sig_out; |
jorick92 | 5:12b9a5cfcf73 | 159 | |
jorick92 | 5:12b9a5cfcf73 | 160 | } |
vsluiter | 0:3843c26cd5fd | 161 | |
vsluiter | 0:3843c26cd5fd | 162 | void looper() |
vsluiter | 0:3843c26cd5fd | 163 | { |
jorick92 | 5:12b9a5cfcf73 | 164 | static float biceps, triceps, extensoren, flexoren, emg_filter_test; |
vsluiter | 2:60fc72e8ff66 | 165 | |
jorick92 | 5:12b9a5cfcf73 | 166 | emg_filter_test = filter(1); |
jorick92 | 5:12b9a5cfcf73 | 167 | if (emg_filter_test != -1) biceps = emg_filter_test; |
jorick92 | 5:12b9a5cfcf73 | 168 | emg_filter_test = filter(2); |
jorick92 | 5:12b9a5cfcf73 | 169 | if (emg_filter_test != -1) triceps = emg_filter_test; |
jorick92 | 5:12b9a5cfcf73 | 170 | emg_filter_test = filter(3); |
jorick92 | 5:12b9a5cfcf73 | 171 | if (emg_filter_test != -1) flexoren = emg_filter_test; |
jorick92 | 5:12b9a5cfcf73 | 172 | emg_filter_test = filter(3); |
jorick92 | 5:12b9a5cfcf73 | 173 | if (emg_filter_test != -1) extensoren = emg_filter_test; |
jorick92 | 5:12b9a5cfcf73 | 174 | |
vsluiter | 0:3843c26cd5fd | 175 | } |
vsluiter | 0:3843c26cd5fd | 176 | |
vsluiter | 0:3843c26cd5fd | 177 | int main() |
vsluiter | 0:3843c26cd5fd | 178 | { |
vsluiter | 0:3843c26cd5fd | 179 | /*setup baudrate. Choose the same in your program on PC side*/ |
vsluiter | 0:3843c26cd5fd | 180 | pc.baud(115200); |
vsluiter | 0:3843c26cd5fd | 181 | /*set the period for the PWM to the red LED*/ |
vsluiter | 0:3843c26cd5fd | 182 | red.period_ms(2); // periode pwm = 2*Fs , blijkbaar. |
vsluiter | 0:3843c26cd5fd | 183 | // blue.period_ms(2); |
vsluiter | 4:d0b4c806f4ea | 184 | |
vsluiter | 4:d0b4c806f4ea | 185 | |
vsluiter | 0:3843c26cd5fd | 186 | /**Here you attach the 'void looper(void)' function to the Ticker object0 |
vsluiter | 0:3843c26cd5fd | 187 | * The looper() function will be called every 0.001 seconds. |
vsluiter | 0:3843c26cd5fd | 188 | * Please mind that the parentheses after looper are omitted when using attach. |
vsluiter | 0:3843c26cd5fd | 189 | */ |
vsluiter | 0:3843c26cd5fd | 190 | timer.attach(looper, 0.001); |
vsluiter | 0:3843c26cd5fd | 191 | while(1) // Loop |
vsluiter | 0:3843c26cd5fd | 192 | { |
vsluiter | 0:3843c26cd5fd | 193 | // blue = sig_out_biceps; |
vsluiter | 0:3843c26cd5fd | 194 | |
vsluiter | 0:3843c26cd5fd | 195 | } |
vsluiter | 0:3843c26cd5fd | 196 | } |