Dan August / Mbed 2 deprecated EMGfinalStDev

testen StDev

Dependencies:   MODSERIAL mbed

main.cpp@0:a9143c04e7ef, 2013-11-02 (annotated)

Committer:
DanAuhust
Date:
Sat Nov 02 13:47:09 2013 +0000
Revision:
0:a9143c04e7ef
testen STDev

Who changed what in which revision?

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