Jorick Leferink / Mbed 2 deprecated EMG_MetFilter_HP_05HZ

werkt niet, kies andere freq

Dependencies:   MODSERIAL mbed

Fork of EMGnieuw by Dan August

main.cpp@1:000418a0aedf, 2013-10-31 (annotated)

Committer:
DanAuhust
Date:
Thu Oct 31 12:15:47 2013 +0000
Revision:
1:000418a0aedf
Parent:
0:8aa426d4db1f
Child:
2:b0b86581ba50
alle ins en outs gedefinieerd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jorick92 0:8aa426d4db1f 1 #include "mbed.h"
jorick92 0:8aa426d4db1f 2 #include "MODSERIAL.h"
jorick92 0:8aa426d4db1f 3
jorick92 0:8aa426d4db1f 4 //Define objects
jorick92 0:8aa426d4db1f 5 AnalogIn emg_biceps(PTB0); //Analog input
jorick92 0:8aa426d4db1f 6 PwmOut red(LED_RED); //PWM output
jorick92 0:8aa426d4db1f 7 Ticker timer;
jorick92 0:8aa426d4db1f 8 MODSERIAL pc(USBTX,USBRX,64,1024);
jorick92 0:8aa426d4db1f 9
jorick92 0:8aa426d4db1f 10 //high pass filter constantes 15Hz cutoff 4e orde
jorick92 0:8aa426d4db1f 11 #define NUM0 0.2754 // constante
jorick92 0:8aa426d4db1f 12 #define NUM1 -1.1017 // z^-1
jorick92 0:8aa426d4db1f 13 #define NUM2 1.6525 // z^-2etc.
jorick92 0:8aa426d4db1f 14 #define NUM3 -1.1017
jorick92 0:8aa426d4db1f 15 #define NUM4 0.2754
jorick92 0:8aa426d4db1f 16
jorick92 0:8aa426d4db1f 17 #define DEN0 1 // constante
jorick92 0:8aa426d4db1f 18 #define DEN1 -1.5704
jorick92 0:8aa426d4db1f 19 #define DEN2 1.2756
jorick92 0:8aa426d4db1f 20 #define DEN3 -0.4844
jorick92 0:8aa426d4db1f 21 #define DEN4 0.0762
jorick92 0:8aa426d4db1f 22
jorick92 0:8aa426d4db1f 23 //lowpass filter constantes 40 Hz 4e orde
jorick92 0:8aa426d4db1f 24 #define NUM0_2 0.4328 // constante
jorick92 0:8aa426d4db1f 25 #define NUM1_2 1.7314 // z^-1
jorick92 0:8aa426d4db1f 26 #define NUM2_2 2.5971 // z^-2etc.
jorick92 0:8aa426d4db1f 27 #define NUM3_2 1.7314
jorick92 0:8aa426d4db1f 28 #define NUM4_2 0.4328
jorick92 0:8aa426d4db1f 29
jorick92 0:8aa426d4db1f 30
jorick92 0:8aa426d4db1f 31 #define DEN0_2 1 // constante
jorick92 0:8aa426d4db1f 32 #define DEN1_2 2.3695
jorick92 0:8aa426d4db1f 33 #define DEN2_2 2.3140
jorick92 0:8aa426d4db1f 34 #define DEN3_2 1.0547
jorick92 0:8aa426d4db1f 35 #define DEN4_2 0.1874
jorick92 0:8aa426d4db1f 36
jorick92 0:8aa426d4db1f 37 //lowpass filter constantes 4z 4e orde
jorick92 0:8aa426d4db1f 38 #define NUM0_3 0.0002 // constante
jorick92 0:8aa426d4db1f 39 #define NUM1_3 0.0007 // z^-1
jorick92 0:8aa426d4db1f 40 #define NUM2_3 0.0011 // z^-2etc.
jorick92 0:8aa426d4db1f 41 #define NUM3_3 0.0007
jorick92 0:8aa426d4db1f 42 #define NUM4_3 0.0002
jorick92 0:8aa426d4db1f 43
jorick92 0:8aa426d4db1f 44
jorick92 0:8aa426d4db1f 45 #define DEN0_3 1 // constante
jorick92 0:8aa426d4db1f 46 #define DEN1_3 -3.3441
jorick92 0:8aa426d4db1f 47 #define DEN2_3 4.2389
jorick92 0:8aa426d4db1f 48 #define DEN3_3 -2.4093
jorick92 0:8aa426d4db1f 49 #define DEN4_3 0.5175
jorick92 0:8aa426d4db1f 50
jorick92 0:8aa426d4db1f 51 //variabelen definieren
jorick92 0:8aa426d4db1f 52 float in0_biceps =0;
jorick92 0:8aa426d4db1f 53 static float in1_biceps =0, in2_biceps = 0, in3_biceps = 0, in4_biceps = 0;
jorick92 0:8aa426d4db1f 54 static float out0_biceps = 0, out1_biceps = 0 , out2_biceps = 0, out3_biceps = 0, out4_biceps = 0;
jorick92 0:8aa426d4db1f 55
jorick92 0:8aa426d4db1f 56 float in0_2_biceps =0;
jorick92 0:8aa426d4db1f 57 static float in1_2_biceps =0, in2_2_biceps = 0, in3_2_biceps = 0, in4_2_biceps = 0;
jorick92 0:8aa426d4db1f 58 static float out0_2_biceps = 0, out1_2_biceps = 0 , out2_2_biceps = 0, out3_2_biceps = 0, out4_2_biceps = 0;
jorick92 0:8aa426d4db1f 59
jorick92 0:8aa426d4db1f 60 float in0_3_biceps =0;
jorick92 0:8aa426d4db1f 61 static float in1_3_biceps =0, in2_3_biceps = 0, in3_3_biceps = 0, in4_3_biceps = 0;
jorick92 0:8aa426d4db1f 62 static float out0_3_biceps = 0, out1_3_biceps = 0 , out2_3_biceps = 0, out3_3_biceps = 0, out4_3_biceps = 0;
jorick92 0:8aa426d4db1f 63 /** Looper function
jorick92 0:8aa426d4db1f 64 * functions used for Ticker and Timeout should be of type void <name>(void)
jorick92 0:8aa426d4db1f 65 * i.e. no input arguments, no output arguments.
jorick92 0:8aa426d4db1f 66 * if you want to change a variable that you use in other places (for example in main)
jorick92 0:8aa426d4db1f 67 * you will have to make that variable global in order to be able to reach it both from
jorick92 0:8aa426d4db1f 68 * the function called at interrupt time, and in the main function.
jorick92 0:8aa426d4db1f 69 * To make a variable global, define it under the includes.
jorick92 0:8aa426d4db1f 70 * variables that are changed in the interrupt routine (written to) should be made
jorick92 0:8aa426d4db1f 71 * 'volatile' to let the compiler know that those values may change outside the current context.
jorick92 0:8aa426d4db1f 72 * i.e.: "volatile float emg_value;" instead of "float emg_value"
jorick92 0:8aa426d4db1f 73 * in the example below, the variable is not re-used in the main function, and is thus declared
jorick92 0:8aa426d4db1f 74 * local in the looper function only.
jorick92 0:8aa426d4db1f 75 **/
jorick92 0:8aa426d4db1f 76
jorick92 0:8aa426d4db1f 77 float filter(float value, int sig_number){
DanAuhust 1:000418a0aedf 78 // eerst variabelen definieren
DanAuhust 1:000418a0aedf 79 float emg_value; //output filters
DanAuhust 1:000418a0aedf 80 //biceps
DanAuhust 1:000418a0aedf 81 float in0_biceps =0;
DanAuhust 1:000418a0aedf 82 static float in1_biceps =0, in2_biceps = 0, in3_biceps = 0, in4_biceps = 0;
DanAuhust 1:000418a0aedf 83 static float out0_biceps = 0, out1_biceps = 0 , out2_biceps = 0, out3_biceps = 0, out4_biceps = 0;
DanAuhust 1:000418a0aedf 84
DanAuhust 1:000418a0aedf 85 float in0_2_biceps =0;
DanAuhust 1:000418a0aedf 86 static float in1_2_biceps =0, in2_2_biceps = 0, in3_2_biceps = 0, in4_2_biceps = 0;
DanAuhust 1:000418a0aedf 87 static float out0_2_biceps = 0, out1_2_biceps = 0 , out2_2_biceps = 0, out3_2_biceps = 0, out4_2_biceps = 0;
DanAuhust 1:000418a0aedf 88
DanAuhust 1:000418a0aedf 89 float in0_3_biceps =0;
DanAuhust 1:000418a0aedf 90 static float in1_3_biceps =0, in2_3_biceps = 0, in3_3_biceps = 0, in4_3_biceps = 0;
DanAuhust 1:000418a0aedf 91 static float out0_3_biceps = 0, out1_3_biceps = 0 , out2_3_biceps = 0, out3_3_biceps = 0, out4_3_biceps = 0;
DanAuhust 1:000418a0aedf 92 //triceps
DanAuhust 1:000418a0aedf 93 float in0_triceps =0;
DanAuhust 1:000418a0aedf 94 static float in1_triceps =0, in2_triceps = 0, in3_triceps = 0, in4_triceps = 0;
DanAuhust 1:000418a0aedf 95 static float out0_triceps = 0, out1_triceps = 0 , out2_triceps = 0, out3_triceps = 0, out4_triceps = 0;
DanAuhust 1:000418a0aedf 96
DanAuhust 1:000418a0aedf 97 float in0_2_triceps =0;
DanAuhust 1:000418a0aedf 98 static float in1_2_triceps =0, in2_2_triceps = 0, in3_2_triceps = 0, in4_2_triceps = 0;
DanAuhust 1:000418a0aedf 99 static float out0_2_triceps = 0, out1_2_triceps = 0 , out2_2_triceps = 0, out3_2_triceps = 0, out4_2_triceps = 0;
DanAuhust 1:000418a0aedf 100
DanAuhust 1:000418a0aedf 101 float in0_3_triceps =0;
DanAuhust 1:000418a0aedf 102 static float in1_3_triceps =0, in2_3_triceps = 0, in3_3_triceps = 0, in4_3_triceps = 0;
DanAuhust 1:000418a0aedf 103 static float out0_3_triceps = 0, out1_3_triceps = 0 , out2_3_triceps = 0, out3_3_triceps = 0, out4_3_triceps = 0;
DanAuhust 1:000418a0aedf 104 //flexoren
DanAuhust 1:000418a0aedf 105 float in0_flexoren =0;
DanAuhust 1:000418a0aedf 106 static float in1_flexoren =0, in2_flexoren = 0, in3_flexoren = 0, in4_flexoren = 0;
DanAuhust 1:000418a0aedf 107 static float out0_flexoren = 0, out1_flexoren = 0 , out2_flexoren = 0, out3_flexoren = 0, out4_flexoren = 0;
DanAuhust 1:000418a0aedf 108
DanAuhust 1:000418a0aedf 109 float in0_2_flexoren =0;
DanAuhust 1:000418a0aedf 110 static float in1_2_flexoren =0, in2_2_flexoren = 0, in3_2_flexoren = 0, in4_2_flexoren = 0;
DanAuhust 1:000418a0aedf 111 static float out0_2_flexoren = 0, out1_2_flexoren = 0 , out2_2_flexoren = 0, out3_2_flexoren = 0, out4_2_flexoren = 0;
DanAuhust 1:000418a0aedf 112
DanAuhust 1:000418a0aedf 113 float in0_3_flexoren =0;
DanAuhust 1:000418a0aedf 114 static float in1_3_flexoren =0, in2_3_flexoren = 0, in3_3_flexoren = 0, in4_3_flexoren = 0;
DanAuhust 1:000418a0aedf 115 static float out0_3_flexoren = 0, out1_3_flexoren = 0 , out2_3_flexoren = 0, out3_3_flexoren = 0, out4_3_flexoren = 0;
DanAuhust 1:000418a0aedf 116 //extensoren
DanAuhust 1:000418a0aedf 117 float in0_extensoren =0;
DanAuhust 1:000418a0aedf 118 static float in1_extensoren =0, in2_extensoren = 0, in3_extensoren = 0, in4_extensoren = 0;
DanAuhust 1:000418a0aedf 119 static float out0_extensoren = 0, out1_extensoren = 0 , out2_extensoren = 0, out3_extensoren = 0, out4_extensoren = 0;
DanAuhust 1:000418a0aedf 120
DanAuhust 1:000418a0aedf 121 float in0_2_extensoren =0;
DanAuhust 1:000418a0aedf 122 static float in1_2_extensoren =0, in2_2_extensoren = 0, in3_2_extensoren = 0, in4_2_extensoren = 0;
DanAuhust 1:000418a0aedf 123 static float out0_2_extensoren = 0, out1_2_extensoren = 0 , out2_2_extensoren = 0, out3_2_extensoren = 0, out4_2_extensoren = 0;
DanAuhust 1:000418a0aedf 124
DanAuhust 1:000418a0aedf 125 float in0_3_extensoren =0;
DanAuhust 1:000418a0aedf 126 static float in1_3_extensoren =0, in2_3_extensoren = 0, in3_3_extensoren = 0, in4_3_extensoren = 0;
DanAuhust 1:000418a0aedf 127 static float out0_3_extensoren = 0, out1_3_extensoren = 0 , out2_3_extensoren = 0, out3_3_extensoren = 0, out4_3_extensoren = 0;
DanAuhust 1:000418a0aedf 128
DanAuhust 1:000418a0aedf 129
jorick92 0:8aa426d4db1f 130 switch(sig_number){
jorick92 0:8aa426d4db1f 131 case 1:
jorick92 0:8aa426d4db1f 132 break;
jorick92 0:8aa426d4db1f 133 case 2:
jorick92 0:8aa426d4db1f 134 break;
jorick92 0:8aa426d4db1f 135 case 3:
jorick92 0:8aa426d4db1f 136 break;
jorick92 0:8aa426d4db1f 137 case 4:
jorick92 0:8aa426d4db1f 138 break;
jorick92 0:8aa426d4db1f 139 }
jorick92 0:8aa426d4db1f 140 }
jorick92 0:8aa426d4db1f 141
jorick92 0:8aa426d4db1f 142 void looper()
jorick92 0:8aa426d4db1f 143 {
jorick92 0:8aa426d4db1f 144
DanAuhust 1:000418a0aedf 145
jorick92 0:8aa426d4db1f 146 /*put raw emg value both in red and in emg_value*/
jorick92 0:8aa426d4db1f 147
jorick92 0:8aa426d4db1f 148 /*send value to PC. use 6 digits after decimal sign*/
jorick92 0:8aa426d4db1f 149
jorick92 0:8aa426d4db1f 150 // signaal filteren op 15 Hz HIGHPASS
jorick92 0:8aa426d4db1f 151 in4_biceps = in3_biceps; in3_biceps = in2_biceps; in2_biceps = in1_biceps; in1_biceps = in0_biceps;
jorick92 0:8aa426d4db1f 152 in0_biceps = emg_biceps.read();
jorick92 0:8aa426d4db1f 153 out4_biceps = out3_biceps; out3_biceps = out2_biceps; out2_biceps = out1_biceps; out1_biceps = out0_biceps;
jorick92 0:8aa426d4db1f 154 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 0:8aa426d4db1f 155
jorick92 0:8aa426d4db1f 156 //signaal filteren op 40 HZ LOWPASS
jorick92 0:8aa426d4db1f 157 in4_2_biceps = in3_2_biceps; in3_2_biceps = in2_2_biceps; in2_2_biceps = in1_2_biceps; in1_2_biceps = in0_2_biceps;
jorick92 0:8aa426d4db1f 158 in0_2_biceps = out0_biceps;
jorick92 0:8aa426d4db1f 159 out4_2_biceps = out3_2_biceps; out3_2_biceps = out2_2_biceps; out2_2_biceps = out1_2_biceps; out1_2_biceps = out0_2_biceps;
jorick92 0:8aa426d4db1f 160 out0_2_biceps = (NUM0_2*in0_2_biceps + NUM1_2*in1_2_biceps + NUM2_2*in2_2_biceps + NUM3_2*in3_2_biceps + NUM4_2*in4_2_biceps - DEN1_2*out1_2_biceps - DEN2_2*out2_2_biceps - DEN3_2*out3_2_biceps - DEN4_2*out4_2_biceps ) / DEN0_2;
jorick92 0:8aa426d4db1f 161
jorick92 0:8aa426d4db1f 162 //signaal filteren op 5Hz LOWPASS
jorick92 0:8aa426d4db1f 163 in4_3_biceps = in3_3_biceps; in3_3_biceps = in2_3_biceps; in2_3_biceps = in1_3_biceps; in1_3_biceps = in0_3_biceps;
jorick92 0:8aa426d4db1f 164 in0_3_biceps = abs(out0_2_biceps);
jorick92 0:8aa426d4db1f 165 out4_3_biceps = out3_3_biceps; out3_3_biceps = out2_3_biceps; out2_3_biceps = out1_3_biceps; out1_3_biceps = out0_3_biceps;
jorick92 0:8aa426d4db1f 166 out0_3_biceps = (NUM0_3*in0_3_biceps + NUM1_3*in1_3_biceps + NUM2_3*in2_3_biceps + NUM3_3*in3_3_biceps + NUM4_3*in4_3_biceps - DEN1_3*out1_3_biceps - DEN2_3*out2_3_biceps - DEN3_3*out3_3_biceps - DEN4_3*out4_3_biceps ) / DEN0_3;
jorick92 0:8aa426d4db1f 167 emg_value = out0_3_biceps;
jorick92 0:8aa426d4db1f 168 if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30)
jorick92 0:8aa426d4db1f 169 pc.printf("%.6f\n",emg_value);
jorick92 0:8aa426d4db1f 170 /**When not using the LED, the above could also have been done this way:
jorick92 0:8aa426d4db1f 171 * pc.printf("%.6\n", emg0.read());
jorick92 0:8aa426d4db1f 172 */
jorick92 0:8aa426d4db1f 173 }
jorick92 0:8aa426d4db1f 174
jorick92 0:8aa426d4db1f 175 int main()
jorick92 0:8aa426d4db1f 176 {
jorick92 0:8aa426d4db1f 177 /*setup baudrate. Choose the same in your program on PC side*/
jorick92 0:8aa426d4db1f 178 pc.baud(115200);
jorick92 0:8aa426d4db1f 179 /*set the period for the PWM to the red LED*/
jorick92 0:8aa426d4db1f 180 red.period_ms(2);
jorick92 0:8aa426d4db1f 181 /**Here you attach the 'void looper(void)' function to the Ticker object
jorick92 0:8aa426d4db1f 182 * The looper() function will be called every 0.001 seconds.
jorick92 0:8aa426d4db1f 183 * Please mind that the parentheses after looper are omitted when using attach.
jorick92 0:8aa426d4db1f 184 */
jorick92 0:8aa426d4db1f 185 timer.attach(looper, 0.01);
jorick92 0:8aa426d4db1f 186 while(1) //Loop
jorick92 0:8aa426d4db1f 187 {
jorick92 0:8aa426d4db1f 188 /*Empty!*/
jorick92 0:8aa426d4db1f 189 /*Everything is handled by the interrupt routine now!*/
jorick92 0:8aa426d4db1f 190 }
jorick92 0:8aa426d4db1f 191 }