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@3:a6c75f643f58, 2013-11-01 (annotated)

Committer:
jorick92
Date:
Fri Nov 01 16:58:52 2013 +0000
Revision:
3:a6c75f643f58
Parent:
2:b0b86581ba50 
Werkt niet, periode moet heel aantal ms zijn, corrigeer.

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