Jorick Leferink
/
EMGStdevV4
emg eind code
Fork of EMGStdevV3 by
main.cpp@7:2d98f94eda27, 2013-11-06 (annotated)
- Committer:
- jorick92
- Date:
- Wed Nov 06 12:26:17 2013 +0000
- Revision:
- 7:2d98f94eda27
- Parent:
- 6:33f0741dbb5b
eindding emg enzo
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
DanAuhust | 0:59e2ea255892 | 1 | #include "mbed.h" |
DanAuhust | 0:59e2ea255892 | 2 | #include "MODSERIAL.h" |
DanAuhust | 0:59e2ea255892 | 3 | |
DanAuhust | 0:59e2ea255892 | 4 | //Define objects |
DanAuhust | 0:59e2ea255892 | 5 | AnalogIn emg_biceps(PTB0); |
DanAuhust | 0:59e2ea255892 | 6 | AnalogIn emg_triceps(PTB1); |
DanAuhust | 0:59e2ea255892 | 7 | AnalogIn emg_flexoren(PTB2); |
DanAuhust | 0:59e2ea255892 | 8 | AnalogIn emg_extensoren(PTB3); //Analog input |
jorick92 | 7:2d98f94eda27 | 9 | PwmOut dy(PTA12); //PWM output |
jorick92 | 7:2d98f94eda27 | 10 | PwmOut dx(PTA4); |
DanAuhust | 0:59e2ea255892 | 11 | Ticker timer; |
DanAuhust | 0:59e2ea255892 | 12 | MODSERIAL pc(USBTX,USBRX,64,1024); |
DanAuhust | 0:59e2ea255892 | 13 | |
DanAuhust | 0:59e2ea255892 | 14 | #define offset_biceps 0 // offset ruwe invoer met adapter motoren, waar toepassen? |
DanAuhust | 0:59e2ea255892 | 15 | |
DanAuhust | 0:59e2ea255892 | 16 | //high pass filter constantes 15Hz cutoff 4e orde, Fs = 312,5Hz (geeft een wat mooiere waarde voor periode en is geen veelvoud van 50Hz) |
DanAuhust | 0:59e2ea255892 | 17 | #define NUM0 0.6731 // constante |
DanAuhust | 0:59e2ea255892 | 18 | #define NUM1 -2.6925 // z^-1 |
DanAuhust | 0:59e2ea255892 | 19 | #define NUM2 4.0388 // z^-2etc. |
DanAuhust | 0:59e2ea255892 | 20 | #define NUM3 -2.6925 |
DanAuhust | 0:59e2ea255892 | 21 | #define NUM4 0.6731 |
DanAuhust | 0:59e2ea255892 | 22 | |
DanAuhust | 0:59e2ea255892 | 23 | #define DEN0 1 // constante |
DanAuhust | 0:59e2ea255892 | 24 | #define DEN1 -3.2133 |
DanAuhust | 0:59e2ea255892 | 25 | #define DEN2 3.9348 |
DanAuhust | 0:59e2ea255892 | 26 | #define DEN3 -2.1689 |
DanAuhust | 0:59e2ea255892 | 27 | #define DEN4 0.4531 |
DanAuhust | 0:59e2ea255892 | 28 | |
DanAuhust | 1:1ffb9e3ae00f | 29 | //lowpass filter constantes 4Hz 4e orde, Fs = 312,5 Hz |
DanAuhust | 0:59e2ea255892 | 30 | #define NUM0_3 0.00000236 // constante |
DanAuhust | 0:59e2ea255892 | 31 | #define NUM1_3 0.00000944 // z^-1 |
DanAuhust | 0:59e2ea255892 | 32 | #define NUM2_3 0.00001415 // z^-2etc. |
DanAuhust | 0:59e2ea255892 | 33 | #define NUM3_3 0.00000944 |
DanAuhust | 0:59e2ea255892 | 34 | #define NUM4_3 0.00000236 |
DanAuhust | 0:59e2ea255892 | 35 | |
DanAuhust | 0:59e2ea255892 | 36 | #define DEN0_3 1 // constante |
DanAuhust | 0:59e2ea255892 | 37 | #define DEN1_3 -3.7899 |
DanAuhust | 0:59e2ea255892 | 38 | #define DEN2_3 5.3914 |
DanAuhust | 0:59e2ea255892 | 39 | #define DEN3_3 -3.4119 |
DanAuhust | 0:59e2ea255892 | 40 | #define DEN4_3 0.8104 |
DanAuhust | 0:59e2ea255892 | 41 | |
DanAuhust | 5:7644d6b16ceb | 42 | double std_dev(float value, int number) { |
DanAuhust | 5:7644d6b16ceb | 43 | int n = 20; |
DanAuhust | 5:7644d6b16ceb | 44 | static int startcount = 0; |
DanAuhust | 5:7644d6b16ceb | 45 | float sum; |
DanAuhust | 5:7644d6b16ceb | 46 | float sq_sum; |
DanAuhust | 5:7644d6b16ceb | 47 | float mean; |
DanAuhust | 5:7644d6b16ceb | 48 | float variance; |
DanAuhust | 0:59e2ea255892 | 49 | static int count_biceps=0; |
DanAuhust | 0:59e2ea255892 | 50 | static int count_triceps=0; |
DanAuhust | 0:59e2ea255892 | 51 | static int count_flexoren=0; |
DanAuhust | 0:59e2ea255892 | 52 | static int count_extensoren=0; |
DanAuhust | 0:59e2ea255892 | 53 | static float keeper_biceps[20]; |
DanAuhust | 0:59e2ea255892 | 54 | static float keeper_triceps[20]; |
DanAuhust | 0:59e2ea255892 | 55 | static float keeper_flexoren[20]; |
DanAuhust | 0:59e2ea255892 | 56 | static float keeper_extensoren[20]; |
DanAuhust | 5:7644d6b16ceb | 57 | |
DanAuhust | 5:7644d6b16ceb | 58 | if (startcount >= n) |
DanAuhust | 5:7644d6b16ceb | 59 | {switch (number){ |
DanAuhust | 0:59e2ea255892 | 60 | case 1: |
DanAuhust | 5:7644d6b16ceb | 61 | //int n = sizeof(keeper_biceps)/sizeof(float); |
DanAuhust | 5:7644d6b16ceb | 62 | keeper_biceps[count_biceps]=value; |
DanAuhust | 0:59e2ea255892 | 63 | count_biceps++; |
DanAuhust | 5:7644d6b16ceb | 64 | if ( count_biceps >= n) |
DanAuhust | 5:7644d6b16ceb | 65 | count_biceps = 0; |
DanAuhust | 5:7644d6b16ceb | 66 | double sum = 0; |
DanAuhust | 5:7644d6b16ceb | 67 | double sq_sum = 0; |
DanAuhust | 5:7644d6b16ceb | 68 | for(int i = 0; i < n; ++i) { |
DanAuhust | 5:7644d6b16ceb | 69 | sum += keeper_biceps[i]; |
DanAuhust | 5:7644d6b16ceb | 70 | sq_sum += keeper_biceps[i] * keeper_biceps[i]; |
DanAuhust | 0:59e2ea255892 | 71 | } |
DanAuhust | 5:7644d6b16ceb | 72 | double mean = sum / n; |
DanAuhust | 5:7644d6b16ceb | 73 | double variance = sq_sum / n - mean * mean; |
DanAuhust | 5:7644d6b16ceb | 74 | return sqrt(variance); |
DanAuhust | 0:59e2ea255892 | 75 | break; |
DanAuhust | 5:7644d6b16ceb | 76 | |
DanAuhust | 0:59e2ea255892 | 77 | case 2: |
DanAuhust | 5:7644d6b16ceb | 78 | //int n = sizeof(keeper_triceps)/sizeof(float); |
DanAuhust | 5:7644d6b16ceb | 79 | keeper_triceps[count_triceps]=value; |
DanAuhust | 0:59e2ea255892 | 80 | count_triceps++; |
DanAuhust | 5:7644d6b16ceb | 81 | if ( count_triceps >= n) |
DanAuhust | 5:7644d6b16ceb | 82 | count_triceps = 0; |
DanAuhust | 5:7644d6b16ceb | 83 | sum = 0; |
DanAuhust | 5:7644d6b16ceb | 84 | sq_sum = 0; |
DanAuhust | 5:7644d6b16ceb | 85 | for(int i = 0; i < n; ++i) { |
DanAuhust | 5:7644d6b16ceb | 86 | sum += keeper_triceps[i]; |
DanAuhust | 5:7644d6b16ceb | 87 | sq_sum += keeper_triceps[i] * keeper_triceps[i]; |
DanAuhust | 0:59e2ea255892 | 88 | } |
DanAuhust | 5:7644d6b16ceb | 89 | mean = sum / n; |
DanAuhust | 5:7644d6b16ceb | 90 | variance = sq_sum / n - mean * mean; |
DanAuhust | 5:7644d6b16ceb | 91 | return sqrt(variance); |
DanAuhust | 0:59e2ea255892 | 92 | break; |
DanAuhust | 5:7644d6b16ceb | 93 | |
DanAuhust | 3:e609cd999fd2 | 94 | case 3: |
DanAuhust | 5:7644d6b16ceb | 95 | //n = sizeof(keeper_flexoren)/sizeof(float); |
DanAuhust | 5:7644d6b16ceb | 96 | keeper_flexoren[count_flexoren]=value; |
DanAuhust | 0:59e2ea255892 | 97 | count_flexoren++; |
DanAuhust | 5:7644d6b16ceb | 98 | if ( count_flexoren >= n) |
DanAuhust | 5:7644d6b16ceb | 99 | count_flexoren = 0; |
DanAuhust | 5:7644d6b16ceb | 100 | sum = 0; |
DanAuhust | 5:7644d6b16ceb | 101 | sq_sum = 0; |
DanAuhust | 5:7644d6b16ceb | 102 | for(int i = 0; i < n; ++i) { |
DanAuhust | 5:7644d6b16ceb | 103 | sum += keeper_flexoren[i]; |
DanAuhust | 5:7644d6b16ceb | 104 | sq_sum += keeper_flexoren[i] * keeper_flexoren[i]; |
DanAuhust | 0:59e2ea255892 | 105 | } |
DanAuhust | 5:7644d6b16ceb | 106 | mean = sum / n; |
DanAuhust | 5:7644d6b16ceb | 107 | variance = sq_sum / n - mean * mean; |
DanAuhust | 5:7644d6b16ceb | 108 | return sqrt(variance); |
DanAuhust | 0:59e2ea255892 | 109 | break; |
DanAuhust | 5:7644d6b16ceb | 110 | |
DanAuhust | 0:59e2ea255892 | 111 | case 4: |
DanAuhust | 5:7644d6b16ceb | 112 | //n = sizeof(keeper_extensoren)/sizeof(float); |
DanAuhust | 6:33f0741dbb5b | 113 | keeper_extensoren[count_extensoren]=value; |
DanAuhust | 0:59e2ea255892 | 114 | count_extensoren++; |
DanAuhust | 5:7644d6b16ceb | 115 | if ( count_extensoren >= n) |
DanAuhust | 5:7644d6b16ceb | 116 | count_extensoren = 0; |
DanAuhust | 5:7644d6b16ceb | 117 | sum = 0; |
DanAuhust | 5:7644d6b16ceb | 118 | sq_sum = 0; |
DanAuhust | 5:7644d6b16ceb | 119 | for(int i = 0; i < n; ++i) { |
DanAuhust | 5:7644d6b16ceb | 120 | sum += keeper_extensoren[i]; |
DanAuhust | 5:7644d6b16ceb | 121 | sq_sum += keeper_extensoren[i] * keeper_extensoren[i]; |
DanAuhust | 0:59e2ea255892 | 122 | } |
DanAuhust | 5:7644d6b16ceb | 123 | mean = sum / n; |
DanAuhust | 5:7644d6b16ceb | 124 | variance = sq_sum / n - mean * mean; |
DanAuhust | 5:7644d6b16ceb | 125 | return sqrt(variance); |
DanAuhust | 3:e609cd999fd2 | 126 | break; |
DanAuhust | 5:7644d6b16ceb | 127 | } // einde switch |
DanAuhust | 5:7644d6b16ceb | 128 | } // einde if |
DanAuhust | 5:7644d6b16ceb | 129 | else |
DanAuhust | 0:59e2ea255892 | 130 | {startcount+=1; |
DanAuhust | 6:33f0741dbb5b | 131 | switch(number){ |
DanAuhust | 0:59e2ea255892 | 132 | case 1: |
DanAuhust | 6:33f0741dbb5b | 133 | keeper_biceps[count_biceps]=value; |
DanAuhust | 0:59e2ea255892 | 134 | count_biceps++; |
DanAuhust | 5:7644d6b16ceb | 135 | if(count_biceps >= n) |
DanAuhust | 0:59e2ea255892 | 136 | count_biceps=0; |
DanAuhust | 0:59e2ea255892 | 137 | break; |
DanAuhust | 0:59e2ea255892 | 138 | case 2: |
DanAuhust | 6:33f0741dbb5b | 139 | keeper_triceps[count_triceps]=value; |
DanAuhust | 0:59e2ea255892 | 140 | count_triceps++; |
DanAuhust | 5:7644d6b16ceb | 141 | if(count_triceps >= n) |
DanAuhust | 0:59e2ea255892 | 142 | count_triceps=0; |
DanAuhust | 0:59e2ea255892 | 143 | break; |
DanAuhust | 3:e609cd999fd2 | 144 | case 3: |
DanAuhust | 6:33f0741dbb5b | 145 | keeper_flexoren[count_flexoren]=value; |
DanAuhust | 3:e609cd999fd2 | 146 | count_flexoren++; |
DanAuhust | 5:7644d6b16ceb | 147 | if(count_flexoren >= n) |
DanAuhust | 3:e609cd999fd2 | 148 | count_flexoren=0; |
DanAuhust | 3:e609cd999fd2 | 149 | break; |
DanAuhust | 3:e609cd999fd2 | 150 | case 4: |
DanAuhust | 6:33f0741dbb5b | 151 | keeper_extensoren[count_extensoren]=value; |
DanAuhust | 3:e609cd999fd2 | 152 | count_extensoren++; |
DanAuhust | 5:7644d6b16ceb | 153 | if(count_extensoren >= n) |
DanAuhust | 3:e609cd999fd2 | 154 | count_extensoren=0; |
DanAuhust | 3:e609cd999fd2 | 155 | break; |
DanAuhust | 0:59e2ea255892 | 156 | } // einde switch |
DanAuhust | 0:59e2ea255892 | 157 | } // einde else |
DanAuhust | 5:7644d6b16ceb | 158 | } |
DanAuhust | 5:7644d6b16ceb | 159 | |
DanAuhust | 0:59e2ea255892 | 160 | float filter(int sig_number){ |
DanAuhust | 0:59e2ea255892 | 161 | float sig_out; |
DanAuhust | 0:59e2ea255892 | 162 | // eerst variabelen definieren |
DanAuhust | 0:59e2ea255892 | 163 | //biceps |
DanAuhust | 0:59e2ea255892 | 164 | //filter 1 |
DanAuhust | 0:59e2ea255892 | 165 | float in0_biceps =0; |
DanAuhust | 0:59e2ea255892 | 166 | static float in1_biceps =0, in2_biceps = 0, in3_biceps = 0, in4_biceps = 0; |
DanAuhust | 0:59e2ea255892 | 167 | static float out0_biceps = 0, out1_biceps = 0 , out2_biceps = 0, out3_biceps = 0, out4_biceps = 0; |
DanAuhust | 5:7644d6b16ceb | 168 | |
DanAuhust | 0:59e2ea255892 | 169 | //filter 3 |
DanAuhust | 0:59e2ea255892 | 170 | float in0_3_biceps =0; |
DanAuhust | 0:59e2ea255892 | 171 | static float in1_3_biceps =0, in2_3_biceps = 0, in3_3_biceps = 0, in4_3_biceps = 0; |
DanAuhust | 0:59e2ea255892 | 172 | static float out0_3_biceps = 0, out1_3_biceps = 0 , out2_3_biceps = 0, out3_3_biceps = 0, out4_3_biceps = 0; |
DanAuhust | 5:7644d6b16ceb | 173 | |
DanAuhust | 0:59e2ea255892 | 174 | //triceps |
DanAuhust | 0:59e2ea255892 | 175 | //filter 1 |
DanAuhust | 0:59e2ea255892 | 176 | float in0_triceps =0; |
DanAuhust | 0:59e2ea255892 | 177 | static float in1_triceps =0, in2_triceps = 0, in3_triceps = 0, in4_triceps = 0; |
DanAuhust | 0:59e2ea255892 | 178 | static float out0_triceps = 0, out1_triceps = 0 , out2_triceps = 0, out3_triceps = 0, out4_triceps = 0; |
DanAuhust | 5:7644d6b16ceb | 179 | |
DanAuhust | 0:59e2ea255892 | 180 | //filter 3 |
DanAuhust | 0:59e2ea255892 | 181 | float in0_3_triceps =0; |
DanAuhust | 0:59e2ea255892 | 182 | static float in1_3_triceps =0, in2_3_triceps = 0, in3_3_triceps = 0, in4_3_triceps = 0; |
DanAuhust | 0:59e2ea255892 | 183 | static float out0_3_triceps = 0, out1_3_triceps = 0 , out2_3_triceps = 0, out3_3_triceps = 0, out4_3_triceps = 0; |
DanAuhust | 5:7644d6b16ceb | 184 | |
DanAuhust | 0:59e2ea255892 | 185 | //flexoren |
DanAuhust | 0:59e2ea255892 | 186 | //filter 1 |
DanAuhust | 0:59e2ea255892 | 187 | float in0_flexoren =0; |
DanAuhust | 0:59e2ea255892 | 188 | static float in1_flexoren =0, in2_flexoren = 0, in3_flexoren = 0, in4_flexoren = 0; |
DanAuhust | 0:59e2ea255892 | 189 | static float out0_flexoren = 0, out1_flexoren = 0 , out2_flexoren = 0, out3_flexoren = 0, out4_flexoren = 0; |
DanAuhust | 0:59e2ea255892 | 190 | |
DanAuhust | 0:59e2ea255892 | 191 | //filter 3 |
DanAuhust | 0:59e2ea255892 | 192 | float in0_3_flexoren =0; |
DanAuhust | 0:59e2ea255892 | 193 | static float in1_3_flexoren =0, in2_3_flexoren = 0, in3_3_flexoren = 0, in4_3_flexoren = 0; |
DanAuhust | 0:59e2ea255892 | 194 | static float out0_3_flexoren = 0, out1_3_flexoren = 0 , out2_3_flexoren = 0, out3_3_flexoren = 0, out4_3_flexoren = 0; |
DanAuhust | 5:7644d6b16ceb | 195 | |
DanAuhust | 0:59e2ea255892 | 196 | //extensoren |
DanAuhust | 0:59e2ea255892 | 197 | //filter 1 |
DanAuhust | 0:59e2ea255892 | 198 | float in0_extensoren =0; |
DanAuhust | 0:59e2ea255892 | 199 | static float in1_extensoren =0, in2_extensoren = 0, in3_extensoren = 0, in4_extensoren = 0; |
DanAuhust | 0:59e2ea255892 | 200 | static float out0_extensoren = 0, out1_extensoren = 0 , out2_extensoren = 0, out3_extensoren = 0, out4_extensoren = 0; |
DanAuhust | 5:7644d6b16ceb | 201 | |
DanAuhust | 0:59e2ea255892 | 202 | //filter 3 |
DanAuhust | 0:59e2ea255892 | 203 | float in0_3_extensoren =0; |
DanAuhust | 0:59e2ea255892 | 204 | static float in1_3_extensoren =0, in2_3_extensoren = 0, in3_3_extensoren = 0, in4_3_extensoren = 0; |
DanAuhust | 0:59e2ea255892 | 205 | static float out0_3_extensoren = 0, out1_3_extensoren = 0 , out2_3_extensoren = 0, out3_3_extensoren = 0, out4_3_extensoren = 0; |
DanAuhust | 5:7644d6b16ceb | 206 | |
DanAuhust | 0:59e2ea255892 | 207 | switch(sig_number){ |
DanAuhust | 0:59e2ea255892 | 208 | case 1: |
DanAuhust | 0:59e2ea255892 | 209 | // signaal filteren op 15 Hz HIGHPASS |
DanAuhust | 0:59e2ea255892 | 210 | in4_biceps = in3_biceps; in3_biceps = in2_biceps; in2_biceps = in1_biceps; in1_biceps = in0_biceps; |
DanAuhust | 0:59e2ea255892 | 211 | in0_biceps = emg_biceps.read() - offset_biceps; |
DanAuhust | 0:59e2ea255892 | 212 | out4_biceps = out3_biceps; out3_biceps = out2_biceps; out2_biceps = out1_biceps; out1_biceps = out0_biceps; |
DanAuhust | 0:59e2ea255892 | 213 | 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 | 5:7644d6b16ceb | 214 | |
DanAuhust | 0:59e2ea255892 | 215 | //signaal filteren op 5Hz LOWPASS |
DanAuhust | 6:33f0741dbb5b | 216 | /*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:59e2ea255892 | 217 | in0_3_biceps = abs(out0_biceps); // ruw - offset -> filter 1 -> stdev (-> filter 3) |
DanAuhust | 0:59e2ea255892 | 218 | 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 | 5:7644d6b16ceb | 219 | 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 | 6:33f0741dbb5b | 220 | */ |
DanAuhust | 6:33f0741dbb5b | 221 | sig_out = out0_biceps; |
DanAuhust | 0:59e2ea255892 | 222 | break; |
DanAuhust | 5:7644d6b16ceb | 223 | |
DanAuhust | 0:59e2ea255892 | 224 | case 2: |
DanAuhust | 0:59e2ea255892 | 225 | // signaal filteren op 15 Hz HIGHPASS |
DanAuhust | 0:59e2ea255892 | 226 | in4_triceps = in3_triceps; in3_triceps = in2_triceps; in2_triceps = in1_triceps; in1_triceps = in0_triceps; |
DanAuhust | 0:59e2ea255892 | 227 | in0_triceps = emg_triceps.read() - offset_biceps; |
DanAuhust | 0:59e2ea255892 | 228 | out4_triceps = out3_triceps; out3_triceps = out2_triceps; out2_triceps = out1_triceps; out1_triceps = out0_triceps; |
DanAuhust | 0:59e2ea255892 | 229 | 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 | 5:7644d6b16ceb | 230 | |
DanAuhust | 0:59e2ea255892 | 231 | //signaal filteren op 5Hz LOWPASS |
DanAuhust | 6:33f0741dbb5b | 232 | /*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:59e2ea255892 | 233 | in0_3_triceps = abs(out0_triceps); |
DanAuhust | 0:59e2ea255892 | 234 | 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:59e2ea255892 | 235 | 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 | 6:33f0741dbb5b | 236 | */ |
DanAuhust | 6:33f0741dbb5b | 237 | sig_out = out0_triceps; |
DanAuhust | 0:59e2ea255892 | 238 | break; |
DanAuhust | 5:7644d6b16ceb | 239 | |
DanAuhust | 0:59e2ea255892 | 240 | case 3: |
DanAuhust | 0:59e2ea255892 | 241 | // signaal filteren op 15 Hz HIGHPASS |
DanAuhust | 0:59e2ea255892 | 242 | in4_flexoren = in3_flexoren; in3_flexoren = in2_flexoren; in2_flexoren = in1_flexoren; in1_flexoren = in0_flexoren; |
DanAuhust | 0:59e2ea255892 | 243 | in0_flexoren = emg_flexoren.read(); |
DanAuhust | 0:59e2ea255892 | 244 | out4_flexoren = out3_flexoren; out3_flexoren = out2_flexoren; out2_flexoren = out1_flexoren; out1_flexoren = out0_flexoren; |
DanAuhust | 0:59e2ea255892 | 245 | 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 | 5:7644d6b16ceb | 246 | |
DanAuhust | 0:59e2ea255892 | 247 | //signaal filteren op 5Hz LOWPASS |
DanAuhust | 6:33f0741dbb5b | 248 | /*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 | 5:7644d6b16ceb | 249 | in0_3_flexoren = abs(out0_flexoren); |
DanAuhust | 0:59e2ea255892 | 250 | 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:59e2ea255892 | 251 | 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 | 6:33f0741dbb5b | 252 | */ |
DanAuhust | 6:33f0741dbb5b | 253 | sig_out = out0_flexoren; |
DanAuhust | 0:59e2ea255892 | 254 | break; |
DanAuhust | 5:7644d6b16ceb | 255 | |
DanAuhust | 0:59e2ea255892 | 256 | case 4: |
DanAuhust | 0:59e2ea255892 | 257 | // signaal filteren op 15 Hz HIGHPASS |
DanAuhust | 0:59e2ea255892 | 258 | in4_extensoren = in3_extensoren; in3_extensoren = in2_extensoren; in2_extensoren = in1_extensoren; in1_extensoren = in0_extensoren; |
DanAuhust | 0:59e2ea255892 | 259 | in0_extensoren = emg_extensoren.read(); |
DanAuhust | 0:59e2ea255892 | 260 | out4_extensoren = out3_extensoren; out3_extensoren = out2_extensoren; out2_extensoren = out1_extensoren; out1_extensoren = out0_extensoren; |
DanAuhust | 0:59e2ea255892 | 261 | 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 | 5:7644d6b16ceb | 262 | |
DanAuhust | 0:59e2ea255892 | 263 | //signaal filteren op 5Hz LOWPASS |
DanAuhust | 6:33f0741dbb5b | 264 | /*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 | 5:7644d6b16ceb | 265 | in0_3_extensoren = abs(out0_extensoren); |
DanAuhust | 0:59e2ea255892 | 266 | 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:59e2ea255892 | 267 | 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 | 6:33f0741dbb5b | 268 | */ |
DanAuhust | 6:33f0741dbb5b | 269 | sig_out = out0_extensoren; |
DanAuhust | 0:59e2ea255892 | 270 | break; |
DanAuhust | 0:59e2ea255892 | 271 | } |
DanAuhust | 0:59e2ea255892 | 272 | return sig_out; |
DanAuhust | 0:59e2ea255892 | 273 | } |
DanAuhust | 0:59e2ea255892 | 274 | |
DanAuhust | 0:59e2ea255892 | 275 | void looper() |
DanAuhust | 0:59e2ea255892 | 276 | { float emg_value_biceps; |
DanAuhust | 0:59e2ea255892 | 277 | float emg_value_triceps; |
DanAuhust | 0:59e2ea255892 | 278 | float emg_value_flexoren; |
DanAuhust | 0:59e2ea255892 | 279 | float emg_value_extensoren; |
jorick92 | 7:2d98f94eda27 | 280 | // float dy; |
jorick92 | 7:2d98f94eda27 | 281 | // float dx; |
DanAuhust | 0:59e2ea255892 | 282 | //static int sig_count = 1; |
DanAuhust | 5:7644d6b16ceb | 283 | |
DanAuhust | 5:7644d6b16ceb | 284 | emg_value_biceps = std_dev(filter(1),1); |
DanAuhust | 5:7644d6b16ceb | 285 | emg_value_triceps = std_dev(filter(2),2); |
DanAuhust | 6:33f0741dbb5b | 286 | emg_value_flexoren = std_dev(filter(3),3); |
DanAuhust | 6:33f0741dbb5b | 287 | emg_value_extensoren = std_dev(filter(4),4); |
DanAuhust | 3:e609cd999fd2 | 288 | |
DanAuhust | 6:33f0741dbb5b | 289 | |
DanAuhust | 3:e609cd999fd2 | 290 | if(emg_value_biceps < 4.5) |
DanAuhust | 3:e609cd999fd2 | 291 | emg_value_biceps=0; |
DanAuhust | 3:e609cd999fd2 | 292 | else if (emg_value_biceps > 13) |
DanAuhust | 3:e609cd999fd2 | 293 | emg_value_biceps = 13; |
jorick92 | 7:2d98f94eda27 | 294 | emg_value_biceps = emg_value_biceps; |
DanAuhust | 6:33f0741dbb5b | 295 | |
DanAuhust | 6:33f0741dbb5b | 296 | if(emg_value_triceps < 5) |
DanAuhust | 6:33f0741dbb5b | 297 | emg_value_triceps=0; |
DanAuhust | 6:33f0741dbb5b | 298 | else if (emg_value_triceps > 10) |
DanAuhust | 6:33f0741dbb5b | 299 | emg_value_triceps = 10; |
DanAuhust | 6:33f0741dbb5b | 300 | emg_value_triceps = emg_value_triceps; |
DanAuhust | 0:59e2ea255892 | 301 | |
DanAuhust | 6:33f0741dbb5b | 302 | if(emg_value_flexoren < 3) // was 2, wordt 3 |
DanAuhust | 3:e609cd999fd2 | 303 | emg_value_flexoren=0; |
DanAuhust | 3:e609cd999fd2 | 304 | else if (emg_value_flexoren > 8) |
DanAuhust | 3:e609cd999fd2 | 305 | emg_value_flexoren = 8; |
jorick92 | 7:2d98f94eda27 | 306 | emg_value_flexoren = 1.5 * emg_value_flexoren; // was 2, wordt 1.5 |
DanAuhust | 6:33f0741dbb5b | 307 | |
DanAuhust | 3:e609cd999fd2 | 308 | if(emg_value_extensoren < 5) |
DanAuhust | 3:e609cd999fd2 | 309 | emg_value_extensoren=0; |
DanAuhust | 3:e609cd999fd2 | 310 | else if (emg_value_extensoren > 13) |
DanAuhust | 3:e609cd999fd2 | 311 | emg_value_extensoren = 13; |
DanAuhust | 6:33f0741dbb5b | 312 | |
jorick92 | 7:2d98f94eda27 | 313 | dy = (emg_value_biceps - emg_value_triceps)/30+0.5; |
jorick92 | 7:2d98f94eda27 | 314 | dx = (emg_value_flexoren - emg_value_extensoren)/30+0.5; // was gain 2, wordt 1 |
DanAuhust | 3:e609cd999fd2 | 315 | |
DanAuhust | 0:59e2ea255892 | 316 | if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30) |
jorick92 | 7:2d98f94eda27 | 317 | pc.printf("%.6f,%.6f\n",dx, dy); |
DanAuhust | 0:59e2ea255892 | 318 | /**When not using the LED, the above could also have been done this way: |
jorick92 | 7:2d98f94eda27 | 319 | /* pc.printf("%.6\n", emg0.read());*/ |
jorick92 | 7:2d98f94eda27 | 320 | |
DanAuhust | 0:59e2ea255892 | 321 | } |
DanAuhust | 0:59e2ea255892 | 322 | |
DanAuhust | 0:59e2ea255892 | 323 | int main() |
DanAuhust | 5:7644d6b16ceb | 324 | |
DanAuhust | 0:59e2ea255892 | 325 | { |
DanAuhust | 0:59e2ea255892 | 326 | /*setup baudrate. Choose the same in your program on PC side*/ |
DanAuhust | 0:59e2ea255892 | 327 | pc.baud(115200); |
jorick92 | 7:2d98f94eda27 | 328 | dy.period_ms(1); |
jorick92 | 7:2d98f94eda27 | 329 | dx.period_ms(1); |
DanAuhust | 0:59e2ea255892 | 330 | /*set the period for the PWM to the red LED*/ |
DanAuhust | 5:7644d6b16ceb | 331 | //red.period_ms(2); |
DanAuhust | 0:59e2ea255892 | 332 | /**Here you attach the 'void looper(void)' function to the Ticker object |
DanAuhust | 0:59e2ea255892 | 333 | * The looper() function will be called every 1/Fs seconds. |
DanAuhust | 0:59e2ea255892 | 334 | * Please mind that the parentheses after looper are omitted when using attach. |
DanAuhust | 0:59e2ea255892 | 335 | */ |
DanAuhust | 0:59e2ea255892 | 336 | timer.attach(looper,0.0032); //invullen in seconde. .0032 is niet eens afgerond, dus vandaar die frequentie. |
DanAuhust | 0:59e2ea255892 | 337 | while(1) //Loop |
DanAuhust | 0:59e2ea255892 | 338 | { |
DanAuhust | 0:59e2ea255892 | 339 | /*Empty!*/ |
DanAuhust | 0:59e2ea255892 | 340 | /*Everything is handled by the interrupt routine now!*/ |
DanAuhust | 0:59e2ea255892 | 341 | } |
DanAuhust | 0:59e2ea255892 | 342 | } |