Jorick Leferink / Mbed 2 deprecated programma_filter

programma voor filter

Dependencies:   MODSERIAL mbed

Fork of BMT-K9-Groep7 by First Last

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?

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