Dan August
/
EMG_finalfilter
Laatste testen filter
Fork of EMG_MetFilter_HP_05HZ by
main.cpp@3:a6c75f643f58, 2013-11-01 (annotated)
- Committer:
- jorick92
- Date:
- Fri Nov 01 16:58:52 2013 +0000
- Revision:
- 3:a6c75f643f58
- Parent:
- 2:b0b86581ba50
- Child:
- 4:e12537bac403
Werkt niet, periode moet heel aantal ms zijn, corrigeer.
Who changed what in which revision?
User | Revision | Line number | New 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 | 3:a6c75f643f58 | 5 | AnalogIn emg_biceps(PTB0); |
jorick92 | 3:a6c75f643f58 | 6 | AnalogIn emg_triceps(PTB1); |
jorick92 | 3:a6c75f643f58 | 7 | AnalogIn emg_flexoren(PTB2); |
jorick92 | 3:a6c75f643f58 | 8 | AnalogIn emg_extensoren(PTB3); //Analog input |
jorick92 | 0:8aa426d4db1f | 9 | PwmOut red(LED_RED); //PWM output |
jorick92 | 0:8aa426d4db1f | 10 | Ticker timer; |
jorick92 | 0:8aa426d4db1f | 11 | MODSERIAL pc(USBTX,USBRX,64,1024); |
jorick92 | 0:8aa426d4db1f | 12 | |
jorick92 | 3:a6c75f643f58 | 13 | #define offset_biceps 0.5 // offset ruwe invoer met adapter motoren |
jorick92 | 3:a6c75f643f58 | 14 | |
jorick92 | 3:a6c75f643f58 | 15 | //high pass filter constantes 15Hz cutoff 4e orde, 515Hz |
jorick92 | 3:a6c75f643f58 | 16 | #define NUM0 0.7870 // constante |
jorick92 | 3:a6c75f643f58 | 17 | #define NUM1 -3.1481 // z^-1 |
jorick92 | 3:a6c75f643f58 | 18 | #define NUM2 4.7221 // z^-2etc. |
jorick92 | 3:a6c75f643f58 | 19 | #define NUM3 -3.1481 |
jorick92 | 3:a6c75f643f58 | 20 | #define NUM4 0.7870 |
jorick92 | 0:8aa426d4db1f | 21 | |
jorick92 | 0:8aa426d4db1f | 22 | #define DEN0 1 // constante |
jorick92 | 3:a6c75f643f58 | 23 | #define DEN1 -3.5221 |
jorick92 | 3:a6c75f643f58 | 24 | #define DEN2 4.6772 |
jorick92 | 3:a6c75f643f58 | 25 | #define DEN3 -2.7736 |
jorick92 | 3:a6c75f643f58 | 26 | #define DEN4 0.6194 |
jorick92 | 0:8aa426d4db1f | 27 | |
jorick92 | 0:8aa426d4db1f | 28 | //lowpass filter constantes 40 Hz 4e orde |
jorick92 | 3:a6c75f643f58 | 29 | #define NUM0_2 0.0466 // constante |
jorick92 | 3:a6c75f643f58 | 30 | #define NUM1_2 0.1863 // z^-1 |
jorick92 | 3:a6c75f643f58 | 31 | #define NUM2_2 0.2795 // z^-2etc. |
jorick92 | 3:a6c75f643f58 | 32 | #define NUM3_2 0.1863 |
jorick92 | 3:a6c75f643f58 | 33 | #define NUM4_2 0.0466 |
jorick92 | 0:8aa426d4db1f | 34 | |
jorick92 | 0:8aa426d4db1f | 35 | |
jorick92 | 0:8aa426d4db1f | 36 | #define DEN0_2 1 // constante |
jorick92 | 3:a6c75f643f58 | 37 | #define DEN1_2 -0.7821 |
jorick92 | 3:a6c75f643f58 | 38 | #define DEN2_2 0.6800 |
jorick92 | 3:a6c75f643f58 | 39 | #define DEN3_2 -0.1827 |
jorick92 | 3:a6c75f643f58 | 40 | #define DEN4_2 0.0301 |
jorick92 | 0:8aa426d4db1f | 41 | |
jorick92 | 3:a6c75f643f58 | 42 | //lowpass filter constantes 4Hz 4e orde |
jorick92 | 3:a6c75f643f58 | 43 | #define NUM0_3 0.000000333 // constante |
jorick92 | 3:a6c75f643f58 | 44 | #define NUM1_3 0.000001331 // z^-1 |
jorick92 | 3:a6c75f643f58 | 45 | #define NUM2_3 0.000001997 // z^-2etc. |
jorick92 | 3:a6c75f643f58 | 46 | #define NUM3_3 0.1331 |
jorick92 | 3:a6c75f643f58 | 47 | #define NUM4_3 0.0333 |
jorick92 | 0:8aa426d4db1f | 48 | |
jorick92 | 0:8aa426d4db1f | 49 | |
jorick92 | 0:8aa426d4db1f | 50 | #define DEN0_3 1 // constante |
jorick92 | 3:a6c75f643f58 | 51 | #define DEN1_3 -3.8725 |
jorick92 | 3:a6c75f643f58 | 52 | #define DEN2_3 5.6255 |
jorick92 | 3:a6c75f643f58 | 53 | #define DEN3_3 -3.6333 |
jorick92 | 3:a6c75f643f58 | 54 | #define DEN4_3 0.8803 |
jorick92 | 0:8aa426d4db1f | 55 | |
jorick92 | 3:a6c75f643f58 | 56 | // highpass filter 1 Hz 2de orde, tegen storing motorshield |
DanAuhust | 2:b0b86581ba50 | 57 | #define NUM0_4 0.9780 // constante |
DanAuhust | 2:b0b86581ba50 | 58 | #define NUM1_4 -1.9561 // z^-1 |
DanAuhust | 2:b0b86581ba50 | 59 | #define NUM2_4 0.9780 // z^-2etc. |
jorick92 | 0:8aa426d4db1f | 60 | |
DanAuhust | 2:b0b86581ba50 | 61 | #define DEN0_4 1 // constante |
DanAuhust | 2:b0b86581ba50 | 62 | #define DEN1_4 -1.9556 |
DanAuhust | 2:b0b86581ba50 | 63 | #define DEN2_4 0.9565 |
jorick92 | 3:a6c75f643f58 | 64 | |
jorick92 | 3:a6c75f643f58 | 65 | |
jorick92 | 3:a6c75f643f58 | 66 | float std_dev (float variable, int sig_number){ |
jorick92 | 3:a6c75f643f58 | 67 | // define variables |
jorick92 | 3:a6c75f643f58 | 68 | float sum; |
jorick92 | 3:a6c75f643f58 | 69 | int size; |
jorick92 | 3:a6c75f643f58 | 70 | float sig_out; |
jorick92 | 3:a6c75f643f58 | 71 | float mean; |
jorick92 | 3:a6c75f643f58 | 72 | static int count_biceps=0; |
jorick92 | 3:a6c75f643f58 | 73 | static int count_triceps=0; |
jorick92 | 3:a6c75f643f58 | 74 | static int count_flexoren=0; |
jorick92 | 3:a6c75f643f58 | 75 | static int count_extensoren=0; |
jorick92 | 3:a6c75f643f58 | 76 | static float emg_values_std_dev_biceps[50]; |
jorick92 | 3:a6c75f643f58 | 77 | static float emg_values_std_dev_triceps[50]; |
jorick92 | 3:a6c75f643f58 | 78 | static float emg_values_std_dev_flexoren[50]; |
jorick92 | 3:a6c75f643f58 | 79 | static float emg_values_std_dev_extensoren[50]; |
DanAuhust | 2:b0b86581ba50 | 80 | |
jorick92 | 3:a6c75f643f58 | 81 | switch(sig_number){ |
jorick92 | 3:a6c75f643f58 | 82 | case 1: |
jorick92 | 3:a6c75f643f58 | 83 | emg_values_std_dev_biceps[count_biceps]=variable; |
jorick92 | 3:a6c75f643f58 | 84 | count_biceps++; |
jorick92 | 3:a6c75f643f58 | 85 | if(count_triceps==size) count_biceps=0; |
jorick92 | 3:a6c75f643f58 | 86 | |
jorick92 | 3:a6c75f643f58 | 87 | size=sizeof(emg_values_std_dev_biceps)/sizeof(float); |
jorick92 | 3:a6c75f643f58 | 88 | for(int i=0; i < size; i++){ |
jorick92 | 3:a6c75f643f58 | 89 | sum+=emg_values_std_dev_biceps[i]; |
jorick92 | 3:a6c75f643f58 | 90 | } |
jorick92 | 3:a6c75f643f58 | 91 | mean=sum/size; |
jorick92 | 3:a6c75f643f58 | 92 | sum=0; |
jorick92 | 3:a6c75f643f58 | 93 | for(int i=0; i < size; i++){ |
jorick92 | 3:a6c75f643f58 | 94 | sum+=(emg_values_std_dev_biceps[i]-mean)*(emg_values_std_dev_biceps[i]-mean); |
jorick92 | 3:a6c75f643f58 | 95 | } |
jorick92 | 3:a6c75f643f58 | 96 | sig_out=sqrt(sum/size); |
jorick92 | 3:a6c75f643f58 | 97 | sum=0; |
jorick92 | 3:a6c75f643f58 | 98 | break; |
jorick92 | 3:a6c75f643f58 | 99 | case 2: |
jorick92 | 3:a6c75f643f58 | 100 | emg_values_std_dev_triceps[count_triceps]=variable; |
jorick92 | 3:a6c75f643f58 | 101 | count_triceps++; |
jorick92 | 3:a6c75f643f58 | 102 | if(count_triceps==size) count_triceps=0; |
jorick92 | 3:a6c75f643f58 | 103 | |
jorick92 | 3:a6c75f643f58 | 104 | size=sizeof(emg_values_std_dev_triceps)/sizeof(float); |
jorick92 | 3:a6c75f643f58 | 105 | for(int i=0; i < size; i++){ |
jorick92 | 3:a6c75f643f58 | 106 | sum+=emg_values_std_dev_triceps[i]; |
jorick92 | 3:a6c75f643f58 | 107 | } |
jorick92 | 3:a6c75f643f58 | 108 | mean=sum/size; |
jorick92 | 3:a6c75f643f58 | 109 | sum=0; |
jorick92 | 3:a6c75f643f58 | 110 | for(int i=0; i < size; i++){ |
jorick92 | 3:a6c75f643f58 | 111 | sum+=(emg_values_std_dev_biceps[i]-mean)*(emg_values_std_dev_biceps[i]-mean); |
jorick92 | 3:a6c75f643f58 | 112 | } |
jorick92 | 3:a6c75f643f58 | 113 | sig_out=sqrt(sum/size); |
jorick92 | 3:a6c75f643f58 | 114 | sum=0; |
jorick92 | 3:a6c75f643f58 | 115 | break; |
jorick92 | 3:a6c75f643f58 | 116 | case 3: |
jorick92 | 3:a6c75f643f58 | 117 | emg_values_std_dev_flexoren[count_flexoren]=variable; |
jorick92 | 3:a6c75f643f58 | 118 | count_flexoren++; |
jorick92 | 3:a6c75f643f58 | 119 | if(count_flexoren==size) count_flexoren=0; |
jorick92 | 3:a6c75f643f58 | 120 | |
jorick92 | 3:a6c75f643f58 | 121 | size=sizeof(emg_values_std_dev_flexoren)/sizeof(float); |
jorick92 | 3:a6c75f643f58 | 122 | for(int i; i < size; i++){ |
jorick92 | 3:a6c75f643f58 | 123 | sum+=emg_values_std_dev_flexoren[i]; |
jorick92 | 3:a6c75f643f58 | 124 | } |
jorick92 | 3:a6c75f643f58 | 125 | mean=sum/size; |
jorick92 | 3:a6c75f643f58 | 126 | sum=0; |
jorick92 | 3:a6c75f643f58 | 127 | for(int i; i < size; i++){ |
jorick92 | 3:a6c75f643f58 | 128 | sum+=(emg_values_std_dev_biceps[i]-mean)*(emg_values_std_dev_biceps[i]-mean); |
jorick92 | 3:a6c75f643f58 | 129 | } |
jorick92 | 3:a6c75f643f58 | 130 | sig_out=sqrt(sum/size); |
jorick92 | 3:a6c75f643f58 | 131 | sum=0; |
jorick92 | 3:a6c75f643f58 | 132 | break; |
jorick92 | 3:a6c75f643f58 | 133 | case 4: |
jorick92 | 3:a6c75f643f58 | 134 | emg_values_std_dev_extensoren[count_extensoren]=variable; |
jorick92 | 3:a6c75f643f58 | 135 | count_extensoren++; |
jorick92 | 3:a6c75f643f58 | 136 | if(count_extensoren==size) count_extensoren=0; |
jorick92 | 3:a6c75f643f58 | 137 | |
jorick92 | 3:a6c75f643f58 | 138 | size=sizeof(emg_values_std_dev_extensoren)/sizeof(float); |
jorick92 | 3:a6c75f643f58 | 139 | for(int i; i < size; i++){ |
jorick92 | 3:a6c75f643f58 | 140 | sum+=emg_values_std_dev_extensoren[i]; |
jorick92 | 3:a6c75f643f58 | 141 | } |
jorick92 | 3:a6c75f643f58 | 142 | mean=sum/size; |
jorick92 | 3:a6c75f643f58 | 143 | sum=0; |
jorick92 | 3:a6c75f643f58 | 144 | for(int i; i < size; i++){ |
jorick92 | 3:a6c75f643f58 | 145 | sum+=(emg_values_std_dev_biceps[i]-mean)*(emg_values_std_dev_biceps[i]-mean); |
jorick92 | 3:a6c75f643f58 | 146 | } |
jorick92 | 3:a6c75f643f58 | 147 | sig_out=sqrt(sum/size); |
jorick92 | 3:a6c75f643f58 | 148 | sum=0; |
jorick92 | 3:a6c75f643f58 | 149 | break; |
jorick92 | 3:a6c75f643f58 | 150 | } |
jorick92 | 3:a6c75f643f58 | 151 | |
jorick92 | 3:a6c75f643f58 | 152 | |
jorick92 | 3:a6c75f643f58 | 153 | return sig_out; |
jorick92 | 3:a6c75f643f58 | 154 | } |
jorick92 | 3:a6c75f643f58 | 155 | |
DanAuhust | 2:b0b86581ba50 | 156 | float filter(int sig_number){ |
DanAuhust | 2:b0b86581ba50 | 157 | float sig_out; |
DanAuhust | 1:000418a0aedf | 158 | // eerst variabelen definieren |
DanAuhust | 2:b0b86581ba50 | 159 | |
DanAuhust | 1:000418a0aedf | 160 | //biceps |
DanAuhust | 2:b0b86581ba50 | 161 | //filter 1 |
DanAuhust | 1:000418a0aedf | 162 | float in0_biceps =0; |
DanAuhust | 1:000418a0aedf | 163 | static float in1_biceps =0, in2_biceps = 0, in3_biceps = 0, in4_biceps = 0; |
DanAuhust | 1:000418a0aedf | 164 | static float out0_biceps = 0, out1_biceps = 0 , out2_biceps = 0, out3_biceps = 0, out4_biceps = 0; |
DanAuhust | 2:b0b86581ba50 | 165 | //filter 2 |
DanAuhust | 1:000418a0aedf | 166 | float in0_2_biceps =0; |
DanAuhust | 1:000418a0aedf | 167 | static float in1_2_biceps =0, in2_2_biceps = 0, in3_2_biceps = 0, in4_2_biceps = 0; |
DanAuhust | 1:000418a0aedf | 168 | 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 | 169 | //filter 3 |
DanAuhust | 1:000418a0aedf | 170 | float in0_3_biceps =0; |
DanAuhust | 1:000418a0aedf | 171 | static float in1_3_biceps =0, in2_3_biceps = 0, in3_3_biceps = 0, in4_3_biceps = 0; |
DanAuhust | 1:000418a0aedf | 172 | 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 | 173 | //filter 4 |
DanAuhust | 2:b0b86581ba50 | 174 | float in0_4_biceps =0; |
DanAuhust | 2:b0b86581ba50 | 175 | static float in1_4_biceps =0, in2_4_biceps = 0; |
DanAuhust | 2:b0b86581ba50 | 176 | static float out0_4_biceps = 0, out1_4_biceps = 0 , out2_4_biceps = 0; |
DanAuhust | 2:b0b86581ba50 | 177 | |
DanAuhust | 1:000418a0aedf | 178 | //triceps |
DanAuhust | 2:b0b86581ba50 | 179 | //filter 1 |
DanAuhust | 1:000418a0aedf | 180 | float in0_triceps =0; |
DanAuhust | 1:000418a0aedf | 181 | static float in1_triceps =0, in2_triceps = 0, in3_triceps = 0, in4_triceps = 0; |
DanAuhust | 1:000418a0aedf | 182 | static float out0_triceps = 0, out1_triceps = 0 , out2_triceps = 0, out3_triceps = 0, out4_triceps = 0; |
DanAuhust | 2:b0b86581ba50 | 183 | //filter 2 |
DanAuhust | 1:000418a0aedf | 184 | float in0_2_triceps =0; |
DanAuhust | 1:000418a0aedf | 185 | static float in1_2_triceps =0, in2_2_triceps = 0, in3_2_triceps = 0, in4_2_triceps = 0; |
DanAuhust | 1:000418a0aedf | 186 | 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 | 187 | //filter 3 |
DanAuhust | 1:000418a0aedf | 188 | float in0_3_triceps =0; |
DanAuhust | 1:000418a0aedf | 189 | static float in1_3_triceps =0, in2_3_triceps = 0, in3_3_triceps = 0, in4_3_triceps = 0; |
DanAuhust | 1:000418a0aedf | 190 | 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 | 191 | //filter 4 |
DanAuhust | 2:b0b86581ba50 | 192 | float in0_4_triceps =0; |
DanAuhust | 2:b0b86581ba50 | 193 | static float in1_4_triceps =0, in2_4_triceps = 0; |
DanAuhust | 2:b0b86581ba50 | 194 | static float out0_4_triceps = 0, out1_4_triceps = 0 , out2_4_triceps = 0; |
DanAuhust | 2:b0b86581ba50 | 195 | |
DanAuhust | 1:000418a0aedf | 196 | //flexoren |
DanAuhust | 2:b0b86581ba50 | 197 | //filter 1 |
DanAuhust | 1:000418a0aedf | 198 | float in0_flexoren =0; |
DanAuhust | 1:000418a0aedf | 199 | static float in1_flexoren =0, in2_flexoren = 0, in3_flexoren = 0, in4_flexoren = 0; |
DanAuhust | 1:000418a0aedf | 200 | static float out0_flexoren = 0, out1_flexoren = 0 , out2_flexoren = 0, out3_flexoren = 0, out4_flexoren = 0; |
DanAuhust | 2:b0b86581ba50 | 201 | //filter 2 |
DanAuhust | 1:000418a0aedf | 202 | float in0_2_flexoren =0; |
DanAuhust | 1:000418a0aedf | 203 | static float in1_2_flexoren =0, in2_2_flexoren = 0, in3_2_flexoren = 0, in4_2_flexoren = 0; |
DanAuhust | 1:000418a0aedf | 204 | 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 | 205 | //filter 3 |
DanAuhust | 1:000418a0aedf | 206 | float in0_3_flexoren =0; |
DanAuhust | 1:000418a0aedf | 207 | static float in1_3_flexoren =0, in2_3_flexoren = 0, in3_3_flexoren = 0, in4_3_flexoren = 0; |
DanAuhust | 1:000418a0aedf | 208 | 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 | 209 | //filter 4 |
DanAuhust | 2:b0b86581ba50 | 210 | float in0_4_flexoren =0; |
DanAuhust | 2:b0b86581ba50 | 211 | static float in1_4_flexoren =0, in2_4_flexoren = 0; |
DanAuhust | 2:b0b86581ba50 | 212 | static float out0_4_flexoren = 0, out1_4_flexoren = 0 , out2_4_flexoren = 0; |
DanAuhust | 2:b0b86581ba50 | 213 | |
DanAuhust | 1:000418a0aedf | 214 | //extensoren |
DanAuhust | 2:b0b86581ba50 | 215 | //filter 1 |
DanAuhust | 1:000418a0aedf | 216 | float in0_extensoren =0; |
DanAuhust | 1:000418a0aedf | 217 | static float in1_extensoren =0, in2_extensoren = 0, in3_extensoren = 0, in4_extensoren = 0; |
DanAuhust | 1:000418a0aedf | 218 | static float out0_extensoren = 0, out1_extensoren = 0 , out2_extensoren = 0, out3_extensoren = 0, out4_extensoren = 0; |
DanAuhust | 2:b0b86581ba50 | 219 | //filter 2 |
DanAuhust | 1:000418a0aedf | 220 | float in0_2_extensoren =0; |
DanAuhust | 1:000418a0aedf | 221 | static float in1_2_extensoren =0, in2_2_extensoren = 0, in3_2_extensoren = 0, in4_2_extensoren = 0; |
DanAuhust | 1:000418a0aedf | 222 | 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 | 223 | //filter 3 |
DanAuhust | 1:000418a0aedf | 224 | float in0_3_extensoren =0; |
DanAuhust | 1:000418a0aedf | 225 | static float in1_3_extensoren =0, in2_3_extensoren = 0, in3_3_extensoren = 0, in4_3_extensoren = 0; |
DanAuhust | 1:000418a0aedf | 226 | 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 | 227 | //filter 4 |
DanAuhust | 2:b0b86581ba50 | 228 | float in0_4_extensoren =0; |
DanAuhust | 2:b0b86581ba50 | 229 | static float in1_4_extensoren =0, in2_4_extensoren = 0; |
DanAuhust | 2:b0b86581ba50 | 230 | static float out0_4_extensoren = 0, out1_4_extensoren = 0 , out2_4_extensoren = 0; |
DanAuhust | 1:000418a0aedf | 231 | |
DanAuhust | 1:000418a0aedf | 232 | |
DanAuhust | 2:b0b86581ba50 | 233 | switch(sig_number){ |
DanAuhust | 2:b0b86581ba50 | 234 | case 1: |
DanAuhust | 2:b0b86581ba50 | 235 | // signaal filteren op 15 Hz HIGHPASS |
DanAuhust | 2:b0b86581ba50 | 236 | in4_biceps = in3_biceps; in3_biceps = in2_biceps; in2_biceps = in1_biceps; in1_biceps = in0_biceps; |
jorick92 | 3:a6c75f643f58 | 237 | in0_biceps = emg_biceps.read() - offset_biceps; |
DanAuhust | 2:b0b86581ba50 | 238 | out4_biceps = out3_biceps; out3_biceps = out2_biceps; out2_biceps = out1_biceps; out1_biceps = out0_biceps; |
DanAuhust | 2:b0b86581ba50 | 239 | 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 | 3:a6c75f643f58 | 240 | /* |
DanAuhust | 2:b0b86581ba50 | 241 | //signaal filteren op 40 HZ LOWPASS |
DanAuhust | 2:b0b86581ba50 | 242 | 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 | 243 | in0_2_biceps = out0_biceps; |
DanAuhust | 2:b0b86581ba50 | 244 | 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 | 245 | 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 | 3:a6c75f643f58 | 246 | */ |
DanAuhust | 2:b0b86581ba50 | 247 | //signaal filteren op 5Hz LOWPASS |
DanAuhust | 2:b0b86581ba50 | 248 | 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 | 3:a6c75f643f58 | 249 | in0_3_biceps = out0_biceps; // ruw - offset -> filter 1 -> stdev (-> filter 3) |
DanAuhust | 2:b0b86581ba50 | 250 | 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 | 251 | 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 | 3:a6c75f643f58 | 252 | /* |
jorick92 | 3:a6c75f643f58 | 253 | //signaal filteren op 1 HZ HIGHPASS |
DanAuhust | 2:b0b86581ba50 | 254 | in2_4_biceps = in1_4_biceps; in1_4_biceps = in0_4_biceps; |
DanAuhust | 2:b0b86581ba50 | 255 | in0_4_biceps = out0_3_biceps; |
DanAuhust | 2:b0b86581ba50 | 256 | out2_4_biceps = out1_4_biceps; out1_4_biceps = out0_4_biceps; |
DanAuhust | 2:b0b86581ba50 | 257 | 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; |
jorick92 | 3:a6c75f643f58 | 258 | */ |
jorick92 | 3:a6c75f643f58 | 259 | sig_out = out0_biceps; |
jorick92 | 0:8aa426d4db1f | 260 | break; |
jorick92 | 0:8aa426d4db1f | 261 | case 2: |
DanAuhust | 2:b0b86581ba50 | 262 | // signaal filteren op 15 Hz HIGHPASS |
DanAuhust | 2:b0b86581ba50 | 263 | in4_triceps = in3_triceps; in3_triceps = in2_triceps; in2_triceps = in1_triceps; in1_triceps = in0_triceps; |
DanAuhust | 2:b0b86581ba50 | 264 | in0_triceps = emg_triceps.read(); |
DanAuhust | 2:b0b86581ba50 | 265 | out4_triceps = out3_triceps; out3_triceps = out2_triceps; out2_triceps = out1_triceps; out1_triceps = out0_triceps; |
DanAuhust | 2:b0b86581ba50 | 266 | 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 | 267 | |
DanAuhust | 2:b0b86581ba50 | 268 | //signaal filteren op 40 HZ LOWPASS |
DanAuhust | 2:b0b86581ba50 | 269 | 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 | 270 | in0_2_triceps = out0_triceps; |
DanAuhust | 2:b0b86581ba50 | 271 | 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 | 272 | 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 | 273 | |
DanAuhust | 2:b0b86581ba50 | 274 | //signaal filteren op 5Hz LOWPASS |
DanAuhust | 2:b0b86581ba50 | 275 | 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 | 276 | in0_3_triceps = abs(out0_2_triceps); |
DanAuhust | 2:b0b86581ba50 | 277 | 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 | 278 | 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 | 279 | |
DanAuhust | 2:b0b86581ba50 | 280 | //signaal filteren op .5 HZ HIGHPASS |
DanAuhust | 2:b0b86581ba50 | 281 | in2_4_triceps = in1_4_triceps; in1_4_triceps = in0_4_triceps; |
DanAuhust | 2:b0b86581ba50 | 282 | in0_4_triceps = out0_3_triceps; |
DanAuhust | 2:b0b86581ba50 | 283 | out2_4_triceps = out1_4_triceps; out1_4_triceps = out0_4_triceps; |
DanAuhust | 2:b0b86581ba50 | 284 | 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 | 285 | |
DanAuhust | 2:b0b86581ba50 | 286 | sig_out = out0_4_triceps; |
jorick92 | 0:8aa426d4db1f | 287 | break; |
jorick92 | 0:8aa426d4db1f | 288 | case 3: |
DanAuhust | 2:b0b86581ba50 | 289 | // signaal filteren op 15 Hz HIGHPASS |
DanAuhust | 2:b0b86581ba50 | 290 | in4_flexoren = in3_flexoren; in3_flexoren = in2_flexoren; in2_flexoren = in1_flexoren; in1_flexoren = in0_flexoren; |
DanAuhust | 2:b0b86581ba50 | 291 | in0_flexoren = emg_flexoren.read(); |
DanAuhust | 2:b0b86581ba50 | 292 | out4_flexoren = out3_flexoren; out3_flexoren = out2_flexoren; out2_flexoren = out1_flexoren; out1_flexoren = out0_flexoren; |
DanAuhust | 2:b0b86581ba50 | 293 | 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 | 294 | |
DanAuhust | 2:b0b86581ba50 | 295 | //signaal filteren op 40 HZ LOWPASS |
DanAuhust | 2:b0b86581ba50 | 296 | 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 | 297 | in0_2_flexoren = out0_flexoren; |
DanAuhust | 2:b0b86581ba50 | 298 | 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 | 299 | 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 | 300 | |
DanAuhust | 2:b0b86581ba50 | 301 | //signaal filteren op 5Hz LOWPASS |
DanAuhust | 2:b0b86581ba50 | 302 | 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 | 303 | in0_3_flexoren = abs(out0_2_flexoren); |
DanAuhust | 2:b0b86581ba50 | 304 | 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 | 305 | 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 | 306 | |
DanAuhust | 2:b0b86581ba50 | 307 | //signaal filteren op .5 HZ HIGHPASS |
DanAuhust | 2:b0b86581ba50 | 308 | in2_4_flexoren = in1_4_flexoren; in1_4_flexoren = in0_4_flexoren; |
DanAuhust | 2:b0b86581ba50 | 309 | in0_4_flexoren = out0_3_flexoren; |
DanAuhust | 2:b0b86581ba50 | 310 | out2_4_flexoren = out1_4_flexoren; out1_4_flexoren = out0_4_flexoren; |
DanAuhust | 2:b0b86581ba50 | 311 | 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 | 312 | |
DanAuhust | 2:b0b86581ba50 | 313 | sig_out = out0_4_flexoren; |
jorick92 | 0:8aa426d4db1f | 314 | break; |
jorick92 | 0:8aa426d4db1f | 315 | case 4: |
DanAuhust | 2:b0b86581ba50 | 316 | // signaal filteren op 15 Hz HIGHPASS |
DanAuhust | 2:b0b86581ba50 | 317 | in4_extensoren = in3_extensoren; in3_extensoren = in2_extensoren; in2_extensoren = in1_extensoren; in1_extensoren = in0_extensoren; |
DanAuhust | 2:b0b86581ba50 | 318 | in0_extensoren = emg_extensoren.read(); |
DanAuhust | 2:b0b86581ba50 | 319 | out4_extensoren = out3_extensoren; out3_extensoren = out2_extensoren; out2_extensoren = out1_extensoren; out1_extensoren = out0_extensoren; |
DanAuhust | 2:b0b86581ba50 | 320 | 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 | 321 | |
DanAuhust | 2:b0b86581ba50 | 322 | //signaal filteren op 40 HZ LOWPASS |
DanAuhust | 2:b0b86581ba50 | 323 | 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 | 324 | in0_2_extensoren = out0_extensoren; |
DanAuhust | 2:b0b86581ba50 | 325 | 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 | 326 | 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 | 327 | |
DanAuhust | 2:b0b86581ba50 | 328 | //signaal filteren op 5Hz LOWPASS |
DanAuhust | 2:b0b86581ba50 | 329 | 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 | 330 | in0_3_extensoren = abs(out0_2_extensoren); |
DanAuhust | 2:b0b86581ba50 | 331 | 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 | 332 | 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 | 333 | |
DanAuhust | 2:b0b86581ba50 | 334 | //signaal filteren op .5 HZ HIGHPASS |
DanAuhust | 2:b0b86581ba50 | 335 | in2_4_extensoren = in1_4_extensoren; in1_4_extensoren = in0_4_extensoren; |
DanAuhust | 2:b0b86581ba50 | 336 | in0_4_extensoren = out0_3_extensoren; |
DanAuhust | 2:b0b86581ba50 | 337 | out2_4_extensoren = out1_4_extensoren; out1_4_extensoren = out0_4_extensoren; |
DanAuhust | 2:b0b86581ba50 | 338 | 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 | 339 | |
DanAuhust | 2:b0b86581ba50 | 340 | sig_out = out0_4_extensoren; |
jorick92 | 0:8aa426d4db1f | 341 | break; |
jorick92 | 0:8aa426d4db1f | 342 | } |
DanAuhust | 2:b0b86581ba50 | 343 | return sig_out; |
jorick92 | 0:8aa426d4db1f | 344 | } |
jorick92 | 0:8aa426d4db1f | 345 | |
jorick92 | 0:8aa426d4db1f | 346 | void looper() |
DanAuhust | 2:b0b86581ba50 | 347 | { float emg_value_biceps; |
DanAuhust | 2:b0b86581ba50 | 348 | float emg_value_triceps; |
DanAuhust | 2:b0b86581ba50 | 349 | float emg_value_flexoren; |
DanAuhust | 2:b0b86581ba50 | 350 | float emg_value_extensoren; |
DanAuhust | 2:b0b86581ba50 | 351 | float dy; |
jorick92 | 3:a6c75f643f58 | 352 | emg_value_biceps = std_dev(filter(1), 1); |
jorick92 | 3:a6c75f643f58 | 353 | emg_value_triceps = (100*filter(2)-44); |
jorick92 | 3:a6c75f643f58 | 354 | emg_value_flexoren = (100*filter(3)-44); |
jorick92 | 3:a6c75f643f58 | 355 | emg_value_extensoren = (100*filter(4)-44); |
DanAuhust | 2:b0b86581ba50 | 356 | //emg_value_flexoren = 100*filter(3); |
DanAuhust | 2:b0b86581ba50 | 357 | //emg_value_extensoren = 100*filter(4); |
jorick92 | 0:8aa426d4db1f | 358 | |
DanAuhust | 2:b0b86581ba50 | 359 | /*if(emg_value_biceps < 0.10){ |
DanAuhust | 2:b0b86581ba50 | 360 | emg_value_biceps=0; |
DanAuhust | 2:b0b86581ba50 | 361 | } |
DanAuhust | 2:b0b86581ba50 | 362 | else { |
DanAuhust | 2:b0b86581ba50 | 363 | emg_value_biceps = emg_value_biceps; |
DanAuhust | 2:b0b86581ba50 | 364 | } |
DanAuhust | 2:b0b86581ba50 | 365 | if(emg_value_triceps < 0.10){ |
DanAuhust | 2:b0b86581ba50 | 366 | emg_value_triceps=0; |
DanAuhust | 2:b0b86581ba50 | 367 | } |
DanAuhust | 2:b0b86581ba50 | 368 | else { |
DanAuhust | 2:b0b86581ba50 | 369 | emg_value_triceps=emg_value_triceps; |
DanAuhust | 2:b0b86581ba50 | 370 | } |
DanAuhust | 2:b0b86581ba50 | 371 | */ |
DanAuhust | 2:b0b86581ba50 | 372 | dy = emg_value_biceps-emg_value_triceps; |
jorick92 | 0:8aa426d4db1f | 373 | if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30) |
jorick92 | 3:a6c75f643f58 | 374 | pc.printf("%.6f\n",emg_value_biceps); |
jorick92 | 0:8aa426d4db1f | 375 | /**When not using the LED, the above could also have been done this way: |
jorick92 | 0:8aa426d4db1f | 376 | * pc.printf("%.6\n", emg0.read()); |
jorick92 | 0:8aa426d4db1f | 377 | */ |
jorick92 | 0:8aa426d4db1f | 378 | } |
jorick92 | 0:8aa426d4db1f | 379 | |
jorick92 | 0:8aa426d4db1f | 380 | int main() |
jorick92 | 0:8aa426d4db1f | 381 | { |
jorick92 | 0:8aa426d4db1f | 382 | /*setup baudrate. Choose the same in your program on PC side*/ |
jorick92 | 0:8aa426d4db1f | 383 | pc.baud(115200); |
jorick92 | 0:8aa426d4db1f | 384 | /*set the period for the PWM to the red LED*/ |
jorick92 | 0:8aa426d4db1f | 385 | red.period_ms(2); |
jorick92 | 0:8aa426d4db1f | 386 | /**Here you attach the 'void looper(void)' function to the Ticker object |
jorick92 | 0:8aa426d4db1f | 387 | * The looper() function will be called every 0.001 seconds. |
jorick92 | 0:8aa426d4db1f | 388 | * Please mind that the parentheses after looper are omitted when using attach. |
jorick92 | 0:8aa426d4db1f | 389 | */ |
jorick92 | 3:a6c75f643f58 | 390 | timer.attach(looper,1.942); |
jorick92 | 0:8aa426d4db1f | 391 | while(1) //Loop |
jorick92 | 0:8aa426d4db1f | 392 | { |
jorick92 | 0:8aa426d4db1f | 393 | /*Empty!*/ |
jorick92 | 0:8aa426d4db1f | 394 | /*Everything is handled by the interrupt routine now!*/ |
jorick92 | 0:8aa426d4db1f | 395 | } |
jorick92 | 0:8aa426d4db1f | 396 | } |