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