Dan August / Mbed 2 deprecated EMG_finalfilter

Laatste testen filter

Dependencies:   MODSERIAL mbed

Fork of EMG_MetFilter_HP_05HZ by Jorick Leferink

main.cpp@2:b0b86581ba50, 2013-11-01 (annotated)

Committer:
DanAuhust
Date:
Fri Nov 01 11:20:55 2013 +0000
Revision:
2:b0b86581ba50
Parent:
1:000418a0aedf
Child:
3:a6c75f643f58
meting biceps en triceps met 4 filters, gain 100, threshold optioneel

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
DanAuhust 2:b0b86581ba50 51 // highpass filter .5 Hz 2de orde, tegen storing motorshield
DanAuhust 2:b0b86581ba50 52 #define NUM0_4 0.9780 // constante
DanAuhust 2:b0b86581ba50 53 #define NUM1_4 -1.9561 // z^-1
DanAuhust 2:b0b86581ba50 54 #define NUM2_4 0.9780 // z^-2etc.
jorick92 0:8aa426d4db1f 55
DanAuhust 2:b0b86581ba50 56 #define DEN0_4 1 // constante
DanAuhust 2:b0b86581ba50 57 #define DEN1_4 -1.9556
DanAuhust 2:b0b86581ba50 58 #define DEN2_4 0.9565
DanAuhust 2:b0b86581ba50 59
DanAuhust 2:b0b86581ba50 60 float filter(int sig_number){
DanAuhust 2:b0b86581ba50 61 float sig_out;
DanAuhust 1:000418a0aedf 62 // eerst variabelen definieren
DanAuhust 2:b0b86581ba50 63
DanAuhust 1:000418a0aedf 64 //biceps
DanAuhust 2:b0b86581ba50 65 //filter 1
DanAuhust 1:000418a0aedf 66 float in0_biceps =0;
DanAuhust 1:000418a0aedf 67 static float in1_biceps =0, in2_biceps = 0, in3_biceps = 0, in4_biceps = 0;
DanAuhust 1:000418a0aedf 68 static float out0_biceps = 0, out1_biceps = 0 , out2_biceps = 0, out3_biceps = 0, out4_biceps = 0;
DanAuhust 2:b0b86581ba50 69 //filter 2
DanAuhust 1:000418a0aedf 70 float in0_2_biceps =0;
DanAuhust 1:000418a0aedf 71 static float in1_2_biceps =0, in2_2_biceps = 0, in3_2_biceps = 0, in4_2_biceps = 0;
DanAuhust 1:000418a0aedf 72 static float out0_2_biceps = 0, out1_2_biceps = 0 , out2_2_biceps = 0, out3_2_biceps = 0, out4_2_biceps = 0;
DanAuhust 2:b0b86581ba50 73 //filter 3
DanAuhust 1:000418a0aedf 74 float in0_3_biceps =0;
DanAuhust 1:000418a0aedf 75 static float in1_3_biceps =0, in2_3_biceps = 0, in3_3_biceps = 0, in4_3_biceps = 0;
DanAuhust 1:000418a0aedf 76 static float out0_3_biceps = 0, out1_3_biceps = 0 , out2_3_biceps = 0, out3_3_biceps = 0, out4_3_biceps = 0;
DanAuhust 2:b0b86581ba50 77 //filter 4
DanAuhust 2:b0b86581ba50 78 float in0_4_biceps =0;
DanAuhust 2:b0b86581ba50 79 static float in1_4_biceps =0, in2_4_biceps = 0;
DanAuhust 2:b0b86581ba50 80 static float out0_4_biceps = 0, out1_4_biceps = 0 , out2_4_biceps = 0;
DanAuhust 2:b0b86581ba50 81
DanAuhust 1:000418a0aedf 82 //triceps
DanAuhust 2:b0b86581ba50 83 //filter 1
DanAuhust 1:000418a0aedf 84 float in0_triceps =0;
DanAuhust 1:000418a0aedf 85 static float in1_triceps =0, in2_triceps = 0, in3_triceps = 0, in4_triceps = 0;
DanAuhust 1:000418a0aedf 86 static float out0_triceps = 0, out1_triceps = 0 , out2_triceps = 0, out3_triceps = 0, out4_triceps = 0;
DanAuhust 2:b0b86581ba50 87 //filter 2
DanAuhust 1:000418a0aedf 88 float in0_2_triceps =0;
DanAuhust 1:000418a0aedf 89 static float in1_2_triceps =0, in2_2_triceps = 0, in3_2_triceps = 0, in4_2_triceps = 0;
DanAuhust 1:000418a0aedf 90 static float out0_2_triceps = 0, out1_2_triceps = 0 , out2_2_triceps = 0, out3_2_triceps = 0, out4_2_triceps = 0;
DanAuhust 2:b0b86581ba50 91 //filter 3
DanAuhust 1:000418a0aedf 92 float in0_3_triceps =0;
DanAuhust 1:000418a0aedf 93 static float in1_3_triceps =0, in2_3_triceps = 0, in3_3_triceps = 0, in4_3_triceps = 0;
DanAuhust 1:000418a0aedf 94 static float out0_3_triceps = 0, out1_3_triceps = 0 , out2_3_triceps = 0, out3_3_triceps = 0, out4_3_triceps = 0;
DanAuhust 2:b0b86581ba50 95 //filter 4
DanAuhust 2:b0b86581ba50 96 float in0_4_triceps =0;
DanAuhust 2:b0b86581ba50 97 static float in1_4_triceps =0, in2_4_triceps = 0;
DanAuhust 2:b0b86581ba50 98 static float out0_4_triceps = 0, out1_4_triceps = 0 , out2_4_triceps = 0;
DanAuhust 2:b0b86581ba50 99
DanAuhust 1:000418a0aedf 100 //flexoren
DanAuhust 2:b0b86581ba50 101 //filter 1
DanAuhust 1:000418a0aedf 102 float in0_flexoren =0;
DanAuhust 1:000418a0aedf 103 static float in1_flexoren =0, in2_flexoren = 0, in3_flexoren = 0, in4_flexoren = 0;
DanAuhust 1:000418a0aedf 104 static float out0_flexoren = 0, out1_flexoren = 0 , out2_flexoren = 0, out3_flexoren = 0, out4_flexoren = 0;
DanAuhust 2:b0b86581ba50 105 //filter 2
DanAuhust 1:000418a0aedf 106 float in0_2_flexoren =0;
DanAuhust 1:000418a0aedf 107 static float in1_2_flexoren =0, in2_2_flexoren = 0, in3_2_flexoren = 0, in4_2_flexoren = 0;
DanAuhust 1:000418a0aedf 108 static float out0_2_flexoren = 0, out1_2_flexoren = 0 , out2_2_flexoren = 0, out3_2_flexoren = 0, out4_2_flexoren = 0;
DanAuhust 2:b0b86581ba50 109 //filter 3
DanAuhust 1:000418a0aedf 110 float in0_3_flexoren =0;
DanAuhust 1:000418a0aedf 111 static float in1_3_flexoren =0, in2_3_flexoren = 0, in3_3_flexoren = 0, in4_3_flexoren = 0;
DanAuhust 1:000418a0aedf 112 static float out0_3_flexoren = 0, out1_3_flexoren = 0 , out2_3_flexoren = 0, out3_3_flexoren = 0, out4_3_flexoren = 0;
DanAuhust 2:b0b86581ba50 113 //filter 4
DanAuhust 2:b0b86581ba50 114 float in0_4_flexoren =0;
DanAuhust 2:b0b86581ba50 115 static float in1_4_flexoren =0, in2_4_flexoren = 0;
DanAuhust 2:b0b86581ba50 116 static float out0_4_flexoren = 0, out1_4_flexoren = 0 , out2_4_flexoren = 0;
DanAuhust 2:b0b86581ba50 117
DanAuhust 1:000418a0aedf 118 //extensoren
DanAuhust 2:b0b86581ba50 119 //filter 1
DanAuhust 1:000418a0aedf 120 float in0_extensoren =0;
DanAuhust 1:000418a0aedf 121 static float in1_extensoren =0, in2_extensoren = 0, in3_extensoren = 0, in4_extensoren = 0;
DanAuhust 1:000418a0aedf 122 static float out0_extensoren = 0, out1_extensoren = 0 , out2_extensoren = 0, out3_extensoren = 0, out4_extensoren = 0;
DanAuhust 2:b0b86581ba50 123 //filter 2
DanAuhust 1:000418a0aedf 124 float in0_2_extensoren =0;
DanAuhust 1:000418a0aedf 125 static float in1_2_extensoren =0, in2_2_extensoren = 0, in3_2_extensoren = 0, in4_2_extensoren = 0;
DanAuhust 1:000418a0aedf 126 static float out0_2_extensoren = 0, out1_2_extensoren = 0 , out2_2_extensoren = 0, out3_2_extensoren = 0, out4_2_extensoren = 0;
DanAuhust 2:b0b86581ba50 127 //filter 3
DanAuhust 1:000418a0aedf 128 float in0_3_extensoren =0;
DanAuhust 1:000418a0aedf 129 static float in1_3_extensoren =0, in2_3_extensoren = 0, in3_3_extensoren = 0, in4_3_extensoren = 0;
DanAuhust 1:000418a0aedf 130 static float out0_3_extensoren = 0, out1_3_extensoren = 0 , out2_3_extensoren = 0, out3_3_extensoren = 0, out4_3_extensoren = 0;
DanAuhust 2:b0b86581ba50 131 //filter 4
DanAuhust 2:b0b86581ba50 132 float in0_4_extensoren =0;
DanAuhust 2:b0b86581ba50 133 static float in1_4_extensoren =0, in2_4_extensoren = 0;
DanAuhust 2:b0b86581ba50 134 static float out0_4_extensoren = 0, out1_4_extensoren = 0 , out2_4_extensoren = 0;
DanAuhust 1:000418a0aedf 135
DanAuhust 1:000418a0aedf 136
DanAuhust 2:b0b86581ba50 137 switch(sig_number){
DanAuhust 2:b0b86581ba50 138 case 1:
DanAuhust 2:b0b86581ba50 139 // signaal filteren op 15 Hz HIGHPASS
DanAuhust 2:b0b86581ba50 140 in4_biceps = in3_biceps; in3_biceps = in2_biceps; in2_biceps = in1_biceps; in1_biceps = in0_biceps;
DanAuhust 2:b0b86581ba50 141 in0_biceps = emg_biceps.read();
DanAuhust 2:b0b86581ba50 142 out4_biceps = out3_biceps; out3_biceps = out2_biceps; out2_biceps = out1_biceps; out1_biceps = out0_biceps;
DanAuhust 2:b0b86581ba50 143 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;
DanAuhust 2:b0b86581ba50 144
DanAuhust 2:b0b86581ba50 145 //signaal filteren op 40 HZ LOWPASS
DanAuhust 2:b0b86581ba50 146 in4_2_biceps = in3_2_biceps; in3_2_biceps = in2_2_biceps; in2_2_biceps = in1_2_biceps; in1_2_biceps = in0_2_biceps;
DanAuhust 2:b0b86581ba50 147 in0_2_biceps = out0_biceps;
DanAuhust 2:b0b86581ba50 148 out4_2_biceps = out3_2_biceps; out3_2_biceps = out2_2_biceps; out2_2_biceps = out1_2_biceps; out1_2_biceps = out0_2_biceps;
DanAuhust 2:b0b86581ba50 149 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;
DanAuhust 2:b0b86581ba50 150
DanAuhust 2:b0b86581ba50 151 //signaal filteren op 5Hz LOWPASS
DanAuhust 2:b0b86581ba50 152 in4_3_biceps = in3_3_biceps; in3_3_biceps = in2_3_biceps; in2_3_biceps = in1_3_biceps; in1_3_biceps = in0_3_biceps;
DanAuhust 2:b0b86581ba50 153 in0_3_biceps = abs(out0_2_biceps);
DanAuhust 2:b0b86581ba50 154 out4_3_biceps = out3_3_biceps; out3_3_biceps = out2_3_biceps; out2_3_biceps = out1_3_biceps; out1_3_biceps = out0_3_biceps;
DanAuhust 2:b0b86581ba50 155 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;
DanAuhust 2:b0b86581ba50 156
DanAuhust 2:b0b86581ba50 157 //signaal filteren op .5 HZ HIGHPASS
DanAuhust 2:b0b86581ba50 158 in2_4_biceps = in1_4_biceps; in1_4_biceps = in0_4_biceps;
DanAuhust 2:b0b86581ba50 159 in0_4_biceps = out0_3_biceps;
DanAuhust 2:b0b86581ba50 160 out2_4_biceps = out1_4_biceps; out1_4_biceps = out0_4_biceps;
DanAuhust 2:b0b86581ba50 161 out0_4_biceps = (NUM0_4*in0_4_biceps + NUM1_4*in1_4_biceps + NUM2_4*in2_4_biceps - DEN1_4*out1_4_biceps - DEN2_4*out2_4_biceps ) / DEN0_4;
DanAuhust 2:b0b86581ba50 162
DanAuhust 2:b0b86581ba50 163 sig_out = out0_4_biceps;
jorick92 0:8aa426d4db1f 164 break;
jorick92 0:8aa426d4db1f 165 case 2:
DanAuhust 2:b0b86581ba50 166 // signaal filteren op 15 Hz HIGHPASS
DanAuhust 2:b0b86581ba50 167 in4_triceps = in3_triceps; in3_triceps = in2_triceps; in2_triceps = in1_triceps; in1_triceps = in0_triceps;
DanAuhust 2:b0b86581ba50 168 in0_triceps = emg_triceps.read();
DanAuhust 2:b0b86581ba50 169 out4_triceps = out3_triceps; out3_triceps = out2_triceps; out2_triceps = out1_triceps; out1_triceps = out0_triceps;
DanAuhust 2:b0b86581ba50 170 out0_triceps = (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;
DanAuhust 2:b0b86581ba50 171
DanAuhust 2:b0b86581ba50 172 //signaal filteren op 40 HZ LOWPASS
DanAuhust 2:b0b86581ba50 173 in4_2_triceps = in3_2_triceps; in3_2_triceps = in2_2_triceps; in2_2_triceps = in1_2_triceps; in1_2_triceps = in0_2_triceps;
DanAuhust 2:b0b86581ba50 174 in0_2_triceps = out0_triceps;
DanAuhust 2:b0b86581ba50 175 out4_2_triceps = out3_2_triceps; out3_2_triceps = out2_2_triceps; out2_2_triceps = out1_2_triceps; out1_2_triceps = out0_2_triceps;
DanAuhust 2:b0b86581ba50 176 out0_2_triceps = (NUM0_2*in0_2_triceps + NUM1_2*in1_2_triceps + NUM2_2*in2_2_triceps + NUM3_2*in3_2_triceps + NUM4_2*in4_2_triceps - DEN1_2*out1_2_triceps - DEN2_2*out2_2_triceps - DEN3_2*out3_2_triceps - DEN4_2*out4_2_triceps ) / DEN0_2;
DanAuhust 2:b0b86581ba50 177
DanAuhust 2:b0b86581ba50 178 //signaal filteren op 5Hz LOWPASS
DanAuhust 2:b0b86581ba50 179 in4_3_triceps = in3_3_triceps; in3_3_triceps = in2_3_triceps; in2_3_triceps = in1_3_triceps; in1_3_triceps = in0_3_triceps;
DanAuhust 2:b0b86581ba50 180 in0_3_triceps = abs(out0_2_triceps);
DanAuhust 2:b0b86581ba50 181 out4_3_triceps = out3_3_triceps; out3_3_triceps = out2_3_triceps; out2_3_triceps = out1_3_triceps; out1_3_triceps = out0_3_triceps;
DanAuhust 2:b0b86581ba50 182 out0_3_triceps = (NUM0_3*in0_3_triceps + NUM1_3*in1_3_triceps + NUM2_3*in2_3_triceps + NUM3_3*in3_3_triceps + NUM4_3*in4_3_triceps - DEN1_3*out1_3_triceps - DEN2_3*out2_3_triceps - DEN3_3*out3_3_triceps - DEN4_3*out4_3_triceps ) / DEN0_3;
DanAuhust 2:b0b86581ba50 183
DanAuhust 2:b0b86581ba50 184 //signaal filteren op .5 HZ HIGHPASS
DanAuhust 2:b0b86581ba50 185 in2_4_triceps = in1_4_triceps; in1_4_triceps = in0_4_triceps;
DanAuhust 2:b0b86581ba50 186 in0_4_triceps = out0_3_triceps;
DanAuhust 2:b0b86581ba50 187 out2_4_triceps = out1_4_triceps; out1_4_triceps = out0_4_triceps;
DanAuhust 2:b0b86581ba50 188 out0_4_triceps = (NUM0_4*in0_4_triceps + NUM1_4*in1_4_triceps + NUM2_4*in2_4_triceps - DEN1_4*out1_4_triceps - DEN2_4*out2_4_triceps ) / DEN0_4;
DanAuhust 2:b0b86581ba50 189
DanAuhust 2:b0b86581ba50 190 sig_out = out0_4_triceps;
jorick92 0:8aa426d4db1f 191 break;
jorick92 0:8aa426d4db1f 192 case 3:
DanAuhust 2:b0b86581ba50 193 // signaal filteren op 15 Hz HIGHPASS
DanAuhust 2:b0b86581ba50 194 in4_flexoren = in3_flexoren; in3_flexoren = in2_flexoren; in2_flexoren = in1_flexoren; in1_flexoren = in0_flexoren;
DanAuhust 2:b0b86581ba50 195 in0_flexoren = emg_flexoren.read();
DanAuhust 2:b0b86581ba50 196 out4_flexoren = out3_flexoren; out3_flexoren = out2_flexoren; out2_flexoren = out1_flexoren; out1_flexoren = out0_flexoren;
DanAuhust 2:b0b86581ba50 197 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;
DanAuhust 2:b0b86581ba50 198
DanAuhust 2:b0b86581ba50 199 //signaal filteren op 40 HZ LOWPASS
DanAuhust 2:b0b86581ba50 200 in4_2_flexoren = in3_2_flexoren; in3_2_flexoren = in2_2_flexoren; in2_2_flexoren = in1_2_flexoren; in1_2_flexoren = in0_2_flexoren;
DanAuhust 2:b0b86581ba50 201 in0_2_flexoren = out0_flexoren;
DanAuhust 2:b0b86581ba50 202 out4_2_flexoren = out3_2_flexoren; out3_2_flexoren = out2_2_flexoren; out2_2_flexoren = out1_2_flexoren; out1_2_flexoren = out0_2_flexoren;
DanAuhust 2:b0b86581ba50 203 out0_2_flexoren = (NUM0_2*in0_2_flexoren + NUM1_2*in1_2_flexoren + NUM2_2*in2_2_flexoren + NUM3_2*in3_2_flexoren + NUM4_2*in4_2_flexoren - DEN1_2*out1_2_flexoren - DEN2_2*out2_2_flexoren - DEN3_2*out3_2_flexoren - DEN4_2*out4_2_flexoren ) / DEN0_2;
DanAuhust 2:b0b86581ba50 204
DanAuhust 2:b0b86581ba50 205 //signaal filteren op 5Hz LOWPASS
DanAuhust 2:b0b86581ba50 206 in4_3_flexoren = in3_3_flexoren; in3_3_flexoren = in2_3_flexoren; in2_3_flexoren = in1_3_flexoren; in1_3_flexoren = in0_3_flexoren;
DanAuhust 2:b0b86581ba50 207 in0_3_flexoren = abs(out0_2_flexoren);
DanAuhust 2:b0b86581ba50 208 out4_3_flexoren = out3_3_flexoren; out3_3_flexoren = out2_3_flexoren; out2_3_flexoren = out1_3_flexoren; out1_3_flexoren = out0_3_flexoren;
DanAuhust 2:b0b86581ba50 209 out0_3_flexoren = (NUM0_3*in0_3_flexoren + NUM1_3*in1_3_flexoren + NUM2_3*in2_3_flexoren + NUM3_3*in3_3_flexoren + NUM4_3*in4_3_flexoren - DEN1_3*out1_3_flexoren - DEN2_3*out2_3_flexoren - DEN3_3*out3_3_flexoren - DEN4_3*out4_3_flexoren ) / DEN0_3;
DanAuhust 2:b0b86581ba50 210
DanAuhust 2:b0b86581ba50 211 //signaal filteren op .5 HZ HIGHPASS
DanAuhust 2:b0b86581ba50 212 in2_4_flexoren = in1_4_flexoren; in1_4_flexoren = in0_4_flexoren;
DanAuhust 2:b0b86581ba50 213 in0_4_flexoren = out0_3_flexoren;
DanAuhust 2:b0b86581ba50 214 out2_4_flexoren = out1_4_flexoren; out1_4_flexoren = out0_4_flexoren;
DanAuhust 2:b0b86581ba50 215 out0_4_flexoren = (NUM0_4*in0_4_flexoren + NUM1_4*in1_4_flexoren + NUM2_4*in2_4_flexoren - DEN1_4*out1_4_flexoren - DEN2_4*out2_4_flexoren ) / DEN0_4;
DanAuhust 2:b0b86581ba50 216
DanAuhust 2:b0b86581ba50 217 sig_out = out0_4_flexoren;
jorick92 0:8aa426d4db1f 218 break;
jorick92 0:8aa426d4db1f 219 case 4:
DanAuhust 2:b0b86581ba50 220 // signaal filteren op 15 Hz HIGHPASS
DanAuhust 2:b0b86581ba50 221 in4_extensoren = in3_extensoren; in3_extensoren = in2_extensoren; in2_extensoren = in1_extensoren; in1_extensoren = in0_extensoren;
DanAuhust 2:b0b86581ba50 222 in0_extensoren = emg_extensoren.read();
DanAuhust 2:b0b86581ba50 223 out4_extensoren = out3_extensoren; out3_extensoren = out2_extensoren; out2_extensoren = out1_extensoren; out1_extensoren = out0_extensoren;
DanAuhust 2:b0b86581ba50 224 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;
DanAuhust 2:b0b86581ba50 225
DanAuhust 2:b0b86581ba50 226 //signaal filteren op 40 HZ LOWPASS
DanAuhust 2:b0b86581ba50 227 in4_2_extensoren = in3_2_extensoren; in3_2_extensoren = in2_2_extensoren; in2_2_extensoren = in1_2_extensoren; in1_2_extensoren = in0_2_extensoren;
DanAuhust 2:b0b86581ba50 228 in0_2_extensoren = out0_extensoren;
DanAuhust 2:b0b86581ba50 229 out4_2_extensoren = out3_2_extensoren; out3_2_extensoren = out2_2_extensoren; out2_2_extensoren = out1_2_extensoren; out1_2_extensoren = out0_2_extensoren;
DanAuhust 2:b0b86581ba50 230 out0_2_extensoren = (NUM0_2*in0_2_extensoren + NUM1_2*in1_2_extensoren + NUM2_2*in2_2_extensoren + NUM3_2*in3_2_extensoren + NUM4_2*in4_2_extensoren - DEN1_2*out1_2_extensoren - DEN2_2*out2_2_extensoren - DEN3_2*out3_2_extensoren - DEN4_2*out4_2_extensoren ) / DEN0_2;
DanAuhust 2:b0b86581ba50 231
DanAuhust 2:b0b86581ba50 232 //signaal filteren op 5Hz LOWPASS
DanAuhust 2:b0b86581ba50 233 in4_3_extensoren = in3_3_extensoren; in3_3_extensoren = in2_3_extensoren; in2_3_extensoren = in1_3_extensoren; in1_3_extensoren = in0_3_extensoren;
DanAuhust 2:b0b86581ba50 234 in0_3_extensoren = abs(out0_2_extensoren);
DanAuhust 2:b0b86581ba50 235 out4_3_extensoren = out3_3_extensoren; out3_3_extensoren = out2_3_extensoren; out2_3_extensoren = out1_3_extensoren; out1_3_extensoren = out0_3_extensoren;
DanAuhust 2:b0b86581ba50 236 out0_3_extensoren = (NUM0_3*in0_3_extensoren + NUM1_3*in1_3_extensoren + NUM2_3*in2_3_extensoren + NUM3_3*in3_3_extensoren + NUM4_3*in4_3_extensoren - DEN1_3*out1_3_extensoren - DEN2_3*out2_3_extensoren - DEN3_3*out3_3_extensoren - DEN4_3*out4_3_extensoren ) / DEN0_3;
DanAuhust 2:b0b86581ba50 237
DanAuhust 2:b0b86581ba50 238 //signaal filteren op .5 HZ HIGHPASS
DanAuhust 2:b0b86581ba50 239 in2_4_extensoren = in1_4_extensoren; in1_4_extensoren = in0_4_extensoren;
DanAuhust 2:b0b86581ba50 240 in0_4_extensoren = out0_3_extensoren;
DanAuhust 2:b0b86581ba50 241 out2_4_extensoren = out1_4_extensoren; out1_4_extensoren = out0_4_extensoren;
DanAuhust 2:b0b86581ba50 242 out0_4_extensoren = (NUM0_4*in0_4_extensoren + NUM1_4*in1_4_extensoren + NUM2_4*in2_4_extensoren - DEN1_4*out1_4_extensoren - DEN2_4*out2_4_extensoren ) / DEN0_4;
DanAuhust 2:b0b86581ba50 243
DanAuhust 2:b0b86581ba50 244 sig_out = out0_4_extensoren;
jorick92 0:8aa426d4db1f 245 break;
jorick92 0:8aa426d4db1f 246 }
DanAuhust 2:b0b86581ba50 247 return sig_out;
jorick92 0:8aa426d4db1f 248 }
jorick92 0:8aa426d4db1f 249
jorick92 0:8aa426d4db1f 250 void looper()
DanAuhust 2:b0b86581ba50 251 { float emg_value_biceps;
DanAuhust 2:b0b86581ba50 252 float emg_value_triceps;
DanAuhust 2:b0b86581ba50 253 float emg_value_flexoren;
DanAuhust 2:b0b86581ba50 254 float emg_value_extensoren;
DanAuhust 2:b0b86581ba50 255 float dy;
DanAuhust 2:b0b86581ba50 256 emg_value_biceps = 100*filter(1);
DanAuhust 2:b0b86581ba50 257 emg_value_triceps = 100*filter(2);
DanAuhust 2:b0b86581ba50 258 //emg_value_flexoren = 100*filter(3);
DanAuhust 2:b0b86581ba50 259 //emg_value_extensoren = 100*filter(4);
jorick92 0:8aa426d4db1f 260
DanAuhust 2:b0b86581ba50 261 /*if(emg_value_biceps < 0.10){
DanAuhust 2:b0b86581ba50 262 emg_value_biceps=0;
DanAuhust 2:b0b86581ba50 263 }
DanAuhust 2:b0b86581ba50 264 else {
DanAuhust 2:b0b86581ba50 265 emg_value_biceps = emg_value_biceps;
DanAuhust 2:b0b86581ba50 266 }
DanAuhust 2:b0b86581ba50 267 if(emg_value_triceps < 0.10){
DanAuhust 2:b0b86581ba50 268 emg_value_triceps=0;
DanAuhust 2:b0b86581ba50 269 }
DanAuhust 2:b0b86581ba50 270 else {
DanAuhust 2:b0b86581ba50 271 emg_value_triceps=emg_value_triceps;
DanAuhust 2:b0b86581ba50 272 }
DanAuhust 2:b0b86581ba50 273 */
DanAuhust 2:b0b86581ba50 274 dy = emg_value_biceps-emg_value_triceps;
jorick92 0:8aa426d4db1f 275 if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30)
DanAuhust 2:b0b86581ba50 276 pc.printf("%.6f, %.6f\n",emg_value_biceps,emg_value_triceps);
jorick92 0:8aa426d4db1f 277 /**When not using the LED, the above could also have been done this way:
jorick92 0:8aa426d4db1f 278 * pc.printf("%.6\n", emg0.read());
jorick92 0:8aa426d4db1f 279 */
jorick92 0:8aa426d4db1f 280 }
jorick92 0:8aa426d4db1f 281
jorick92 0:8aa426d4db1f 282 int main()
jorick92 0:8aa426d4db1f 283 {
jorick92 0:8aa426d4db1f 284 /*setup baudrate. Choose the same in your program on PC side*/
jorick92 0:8aa426d4db1f 285 pc.baud(115200);
jorick92 0:8aa426d4db1f 286 /*set the period for the PWM to the red LED*/
jorick92 0:8aa426d4db1f 287 red.period_ms(2);
jorick92 0:8aa426d4db1f 288 /**Here you attach the 'void looper(void)' function to the Ticker object
jorick92 0:8aa426d4db1f 289 * The looper() function will be called every 0.001 seconds.
jorick92 0:8aa426d4db1f 290 * Please mind that the parentheses after looper are omitted when using attach.
jorick92 0:8aa426d4db1f 291 */
jorick92 0:8aa426d4db1f 292 timer.attach(looper, 0.01);
jorick92 0:8aa426d4db1f 293 while(1) //Loop
jorick92 0:8aa426d4db1f 294 {
jorick92 0:8aa426d4db1f 295 /*Empty!*/
jorick92 0:8aa426d4db1f 296 /*Everything is handled by the interrupt routine now!*/
jorick92 0:8aa426d4db1f 297 }
jorick92 0:8aa426d4db1f 298 }