test
Dependencies: HIDScope MODSERIAL mbed-dsp mbed
Fork of emg_filter2 by
EMGfilter.cpp@53:d90e54fba7d8, 2014-10-20 (annotated)
- Committer:
- Tanja2211
- Date:
- Mon Oct 20 09:11:48 2014 +0000
- Revision:
- 53:d90e54fba7d8
- Parent:
- 52:cce270e0753a
- Child:
- 54:f3a9fa5f2b0e
foutjes weg
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
s1340735 | 22:dc630dbb1dcd | 1 | #include "mbed.h" |
s1340735 | 22:dc630dbb1dcd | 2 | #include "HIDScope.h" |
s1340735 | 23:1c51af8386c9 | 3 | #include "MODSERIAL.h" |
s1340735 | 22:dc630dbb1dcd | 4 | #include "arm_math.h" |
s1340735 | 22:dc630dbb1dcd | 5 | |
s1340735 | 30:5d8e6f0fabc1 | 6 | HIDScope::HIDScope(int channels) : hid(64,64) |
s1340735 | 29:f54123765a47 | 7 | { |
s1340735 | 29:f54123765a47 | 8 | bufferData = new float[channels](); |
s1340735 | 29:f54123765a47 | 9 | channelCount = channels; |
s1340735 | 29:f54123765a47 | 10 | scopeData.length = 64; |
s1340735 | 29:f54123765a47 | 11 | } |
s1340735 | 29:f54123765a47 | 12 | |
s1340735 | 29:f54123765a47 | 13 | void HIDScope::set(int ch, float val) |
s1340735 | 29:f54123765a47 | 14 | { |
s1340735 | 29:f54123765a47 | 15 | bufferData[ch] = val; |
s1340735 | 29:f54123765a47 | 16 | } |
s1340735 | 29:f54123765a47 | 17 | |
s1340735 | 29:f54123765a47 | 18 | void HIDScope::set(int ch, int val) |
s1340735 | 29:f54123765a47 | 19 | { |
s1340735 | 29:f54123765a47 | 20 | set(ch,(float)val); |
s1340735 | 29:f54123765a47 | 21 | } |
s1340735 | 29:f54123765a47 | 22 | |
s1340735 | 29:f54123765a47 | 23 | void HIDScope::set(int ch, bool val) |
s1340735 | 29:f54123765a47 | 24 | { |
s1340735 | 29:f54123765a47 | 25 | set(ch,(val ? 1.0f : 0.0f)); |
s1340735 | 29:f54123765a47 | 26 | } |
s1340735 | 29:f54123765a47 | 27 | |
s1340735 | 29:f54123765a47 | 28 | void HIDScope::set(int ch, double val) |
s1340735 | 29:f54123765a47 | 29 | { |
s1340735 | 29:f54123765a47 | 30 | set(ch,(float)val); |
s1340735 | 29:f54123765a47 | 31 | } |
s1340735 | 29:f54123765a47 | 32 | |
s1340735 | 29:f54123765a47 | 33 | void HIDScope::send() |
s1340735 | 30:5d8e6f0fabc1 | 34 | { |
s1340735 | 29:f54123765a47 | 35 | for(int ch=0; ch<channelCount; ch++) |
s1340735 | 29:f54123765a47 | 36 | memcpy(&scopeData.data[ch*4], &bufferData[ch], 4); // Copy a 4 byte float to the char array |
s1340735 | 30:5d8e6f0fabc1 | 37 | |
s1340735 | 29:f54123765a47 | 38 | // Send non blocking, can be adjusted to blocking (hid.send) |
s1340735 | 29:f54123765a47 | 39 | hid.sendNB(&scopeData); |
s1340735 | 29:f54123765a47 | 40 | } |
s1340735 | 29:f54123765a47 | 41 | |
s1340735 | 29:f54123765a47 | 42 | // ****** emg filter shizzle ****** |
s1340735 | 29:f54123765a47 | 43 | |
s1340735 | 22:dc630dbb1dcd | 44 | //Define objects |
s1340735 | 22:dc630dbb1dcd | 45 | AnalogIn emgB(PTB0); //Analog input bicep |
s1340735 | 22:dc630dbb1dcd | 46 | AnalogIn emgT(PTB1); //Analog input tricep |
s1340735 | 22:dc630dbb1dcd | 47 | |
s1340735 | 23:1c51af8386c9 | 48 | float filtered_emgB; |
s1340735 | 27:24e73fd36859 | 49 | float filtered_emgT; |
Tanja2211 | 51:809e38348c91 | 50 | float B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, MOVAVG_B; |
Tanja2211 | 51:809e38348c91 | 51 | float T0, T1, T2, T3, T4, T5, T6, T7, T8, T9, MOVAVG_T; |
Tanja2211 | 52:cce270e0753a | 52 | float drempelwaardeB1, drempelwaardeB2, drempelwaardeB3, drempelwaardeT; |
s1340735 | 23:1c51af8386c9 | 53 | |
s1340735 | 23:1c51af8386c9 | 54 | MODSERIAL pc(USBTX,USBRX); |
s1340735 | 23:1c51af8386c9 | 55 | |
s1340735 | 26:b93c82fb6e1d | 56 | HIDScope scope(4);//uitgang scherm |
s1340735 | 22:dc630dbb1dcd | 57 | |
s1340735 | 22:dc630dbb1dcd | 58 | arm_biquad_casd_df1_inst_f32 lowpass; |
s1340735 | 22:dc630dbb1dcd | 59 | //constants for 50Hz lowpass |
s1340735 | 30:5d8e6f0fabc1 | 60 | float lowpass_const[] = {0.2928920553, 0.5857841107, 0.2928920554, -0, -0.17156822136};//{a0 a1 a2 -b1 -b2} van online calculator |
s1340735 | 22:dc630dbb1dcd | 61 | //state values |
s1340735 | 26:b93c82fb6e1d | 62 | float lowpass_states[4]; |
s1340735 | 22:dc630dbb1dcd | 63 | |
s1340735 | 22:dc630dbb1dcd | 64 | arm_biquad_casd_df1_inst_f32 highpass; |
s1340735 | 22:dc630dbb1dcd | 65 | //constants for 10Hz highpass |
s1340735 | 26:b93c82fb6e1d | 66 | float highpass_const[] = {0.8005910267, -1.6011820533, 0.8005910267, 1.5610153913, -0.6413487154};//{a0 a1 a2 -b1 -b2} |
s1340735 | 22:dc630dbb1dcd | 67 | //state values |
s1340735 | 26:b93c82fb6e1d | 68 | float highpass_states[4]; |
s1340735 | 22:dc630dbb1dcd | 69 | |
s1340735 | 22:dc630dbb1dcd | 70 | |
s1340735 | 22:dc630dbb1dcd | 71 | /** Looper function |
s1340735 | 22:dc630dbb1dcd | 72 | * functions used for Ticker and Timeout should be of type void <name>(void) |
s1340735 | 22:dc630dbb1dcd | 73 | * i.e. no input arguments, no output arguments. |
s1340735 | 22:dc630dbb1dcd | 74 | * if you want to change a variable that you use in other places (for example in main) |
s1340735 | 22:dc630dbb1dcd | 75 | * you will have to make that variable global in order to be able to reach it both from |
s1340735 | 22:dc630dbb1dcd | 76 | * the function called at interrupt time, and in the main function. |
s1340735 | 22:dc630dbb1dcd | 77 | * To make a variable global, define it under the includes. |
s1340735 | 22:dc630dbb1dcd | 78 | * variables that are changed in the interrupt routine (written to) should be made |
s1340735 | 22:dc630dbb1dcd | 79 | * 'volatile' to let the compiler know that those values may change outside the current context. |
s1340735 | 22:dc630dbb1dcd | 80 | * i.e.: "volatile uint16_t emg_value;" instead of "uint16_t emg_value" |
s1340735 | 22:dc630dbb1dcd | 81 | * in the example below, the variable is not re-used in the main function, and is thus declared |
s1340735 | 22:dc630dbb1dcd | 82 | * local in the looper function only. |
s1340735 | 22:dc630dbb1dcd | 83 | **/ |
s1340735 | 22:dc630dbb1dcd | 84 | |
s1340735 | 22:dc630dbb1dcd | 85 | //BICEP EMG LEZEN |
s1340735 | 22:dc630dbb1dcd | 86 | void looperB() |
s1340735 | 22:dc630dbb1dcd | 87 | { |
s1340735 | 23:1c51af8386c9 | 88 | /*variable to store value in*/ |
s1340735 | 22:dc630dbb1dcd | 89 | uint16_t emg_valueB; |
s1340735 | 30:5d8e6f0fabc1 | 90 | |
s1340735 | 22:dc630dbb1dcd | 91 | float emg_value_f32B; |
s1340735 | 22:dc630dbb1dcd | 92 | /*put raw emg value both in red and in emg_value*/ |
s1340735 | 22:dc630dbb1dcd | 93 | emg_valueB = emgB.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V) |
s1340735 | 22:dc630dbb1dcd | 94 | emg_value_f32B = emgB.read(); |
s1340735 | 22:dc630dbb1dcd | 95 | |
s1340735 | 22:dc630dbb1dcd | 96 | //process emg |
s1340735 | 22:dc630dbb1dcd | 97 | arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32B, &filtered_emgB, 1 ); |
s1340735 | 22:dc630dbb1dcd | 98 | filtered_emgB = fabs(filtered_emgB); |
s1340735 | 22:dc630dbb1dcd | 99 | arm_biquad_cascade_df1_f32(&lowpass, &filtered_emgB, &filtered_emgB, 1 ); |
s1340735 | 23:1c51af8386c9 | 100 | |
s1340735 | 22:dc630dbb1dcd | 101 | /*send value to PC. */ |
s1340735 | 22:dc630dbb1dcd | 102 | scope.set(0,emg_valueB); //uint value |
s1340735 | 22:dc630dbb1dcd | 103 | scope.set(1,filtered_emgB); //processed float |
s1340735 | 22:dc630dbb1dcd | 104 | scope.send(); |
Tanja2211 | 45:7950fa411107 | 105 | |
Tanja2211 | 44:b47f559826ba | 106 | // Moving Average Filter Biceps |
s1340735 | 22:dc630dbb1dcd | 107 | |
Tanja2211 | 51:809e38348c91 | 108 | |
Tanja2211 | 36:82fd9d862266 | 109 | { |
Tanja2211 | 32:1bc34d137942 | 110 | B0=filtered_emgB; |
Tanja2211 | 42:d49b766ffdc7 | 111 | MOVAVG_B=B0*0.1+B1*0.1+B2*0.1+B3*0.1+B4*0.1+B5*0.1+B6*0.1+B7*0.1+B8*0.1+B9*0.1; |
Tanja2211 | 32:1bc34d137942 | 112 | B9=B8; |
Tanja2211 | 32:1bc34d137942 | 113 | B8=B7; |
Tanja2211 | 32:1bc34d137942 | 114 | B7=B6; |
Tanja2211 | 32:1bc34d137942 | 115 | B6=B5; |
Tanja2211 | 32:1bc34d137942 | 116 | B5=B4; |
Tanja2211 | 32:1bc34d137942 | 117 | B4=B3; |
Tanja2211 | 32:1bc34d137942 | 118 | B3=B2; |
Tanja2211 | 32:1bc34d137942 | 119 | B2=B1; |
Tanja2211 | 32:1bc34d137942 | 120 | B1=B0; |
s1340735 | 30:5d8e6f0fabc1 | 121 | |
s1340735 | 30:5d8e6f0fabc1 | 122 | } |
Tanja2211 | 45:7950fa411107 | 123 | } |
Tanja2211 | 36:82fd9d862266 | 124 | |
Tanja2211 | 36:82fd9d862266 | 125 | |
Tanja2211 | 44:b47f559826ba | 126 | // Triceps EMG lezen |
Tanja2211 | 45:7950fa411107 | 127 | void looperT() |
Tanja2211 | 45:7950fa411107 | 128 | { |
Tanja2211 | 45:7950fa411107 | 129 | /*variable to store value in*/ |
Tanja2211 | 45:7950fa411107 | 130 | uint16_t emg_valueT; |
Tanja2211 | 31:b6f7ba4938d4 | 131 | |
Tanja2211 | 45:7950fa411107 | 132 | float emg_value_f32T; |
Tanja2211 | 45:7950fa411107 | 133 | /*put raw emg value both in red and in emg_value*/ |
Tanja2211 | 45:7950fa411107 | 134 | emg_valueT = emgT.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V) |
Tanja2211 | 45:7950fa411107 | 135 | emg_value_f32T = emgT.read(); |
s1340735 | 22:dc630dbb1dcd | 136 | |
Tanja2211 | 45:7950fa411107 | 137 | //process emg |
Tanja2211 | 45:7950fa411107 | 138 | arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32T, &filtered_emgT, 1 ); |
Tanja2211 | 45:7950fa411107 | 139 | filtered_emgT = fabs(filtered_emgT); |
Tanja2211 | 45:7950fa411107 | 140 | arm_biquad_cascade_df1_f32(&lowpass, &filtered_emgT, &filtered_emgT, 1 ); |
s1340735 | 22:dc630dbb1dcd | 141 | |
Tanja2211 | 45:7950fa411107 | 142 | /*send value to PC. */ |
Tanja2211 | 45:7950fa411107 | 143 | scope.set(2,emg_valueT); //uint value |
Tanja2211 | 45:7950fa411107 | 144 | scope.set(3,filtered_emgT); //processed float |
Tanja2211 | 45:7950fa411107 | 145 | scope.send(); |
Tanja2211 | 38:7ed04177892b | 146 | |
Tanja2211 | 45:7950fa411107 | 147 | // Moving Average Filter Triceps |
Tanja2211 | 38:7ed04177892b | 148 | |
Tanja2211 | 51:809e38348c91 | 149 | |
Tanja2211 | 50:650e8e45b870 | 150 | { |
Tanja2211 | 45:7950fa411107 | 151 | T0=filtered_emgT; |
Tanja2211 | 45:7950fa411107 | 152 | MOVAVG_T=T0*0.1+T1*0.1+T2*0.1+T3*0.1+T4*0.1+T5*0.1+T6*0.1+T7*0.1+T8*0.1+T9*0.1; |
Tanja2211 | 38:7ed04177892b | 153 | |
Tanja2211 | 45:7950fa411107 | 154 | T9=T8; |
Tanja2211 | 45:7950fa411107 | 155 | T8=T7; |
Tanja2211 | 45:7950fa411107 | 156 | T7=T6; |
Tanja2211 | 45:7950fa411107 | 157 | T6=T5; |
Tanja2211 | 45:7950fa411107 | 158 | T5=T4; |
Tanja2211 | 45:7950fa411107 | 159 | T4=T3; |
Tanja2211 | 45:7950fa411107 | 160 | T3=T2; |
Tanja2211 | 45:7950fa411107 | 161 | T2=T1; |
Tanja2211 | 45:7950fa411107 | 162 | T1=T0; |
Tanja2211 | 38:7ed04177892b | 163 | |
Tanja2211 | 45:7950fa411107 | 164 | } |
Tanja2211 | 50:650e8e45b870 | 165 | } |
Tanja2211 | 49:b103e9ed5ef2 | 166 | |
Tanja2211 | 36:82fd9d862266 | 167 | |
Tanja2211 | 45:7950fa411107 | 168 | int main() |
Tanja2211 | 45:7950fa411107 | 169 | { |
Tanja2211 | 45:7950fa411107 | 170 | Ticker log_timer; |
Tanja2211 | 45:7950fa411107 | 171 | //set up filters. Use external array for constants |
Tanja2211 | 45:7950fa411107 | 172 | arm_biquad_cascade_df1_init_f32(&lowpass,1 , lowpass_const, lowpass_states); |
Tanja2211 | 45:7950fa411107 | 173 | arm_biquad_cascade_df1_init_f32(&highpass,1 ,highpass_const, highpass_states); |
s1340735 | 22:dc630dbb1dcd | 174 | |
Tanja2211 | 45:7950fa411107 | 175 | /**Here you attach the 'void looper(void)' function to the Ticker object |
Tanja2211 | 45:7950fa411107 | 176 | * The looper() function will be called every 0.01 seconds. |
Tanja2211 | 45:7950fa411107 | 177 | * Please mind that the parentheses after looper are omitted when using attach. |
Tanja2211 | 45:7950fa411107 | 178 | */ |
Tanja2211 | 45:7950fa411107 | 179 | log_timer.attach(looperB, 0.005);//?? |
Tanja2211 | 45:7950fa411107 | 180 | log_timer.attach(looperT, 0.005);//?? |
Tanja2211 | 45:7950fa411107 | 181 | while(1) { //Loop |
Tanja2211 | 45:7950fa411107 | 182 | /*Empty!*/ |
Tanja2211 | 45:7950fa411107 | 183 | /*Everything is handled by the interrupt routine now!*/ |
s1340735 | 23:1c51af8386c9 | 184 | } |
Tanja2211 | 45:7950fa411107 | 185 | } |
s1340735 | 23:1c51af8386c9 | 186 | |
s1340735 | 22:dc630dbb1dcd | 187 | //filtered_emgB |
s1340735 | 22:dc630dbb1dcd | 188 | //filtered_emgT |
s1340735 | 22:dc630dbb1dcd | 189 | |
Tanja2211 | 52:cce270e0753a | 190 | void AntwoordT() |
Tanja2211 | 45:7950fa411107 | 191 | { |
Tanja2211 | 52:cce270e0753a | 192 | drempelwaardeT=4.99; |
Tanja2211 | 45:7950fa411107 | 193 | int y; |
s1340735 | 30:5d8e6f0fabc1 | 194 | |
Tanja2211 | 45:7950fa411107 | 195 | if (MOVAVG_T > drempelwaardeT) { |
Tanja2211 | 45:7950fa411107 | 196 | y=1; |
Tanja2211 | 45:7950fa411107 | 197 | } else { |
Tanja2211 | 45:7950fa411107 | 198 | y=0; |
Tanja2211 | 45:7950fa411107 | 199 | } |
s1340735 | 23:1c51af8386c9 | 200 | |
Tanja2211 | 45:7950fa411107 | 201 | if (y==1) { |
Tanja2211 | 45:7950fa411107 | 202 | pc.printf("Motor 2 beweegt\n"); |
Tanja2211 | 45:7950fa411107 | 203 | } else { |
Tanja2211 | 45:7950fa411107 | 204 | pc.printf("Motor 2 beweegt niet\n"); |
Tanja2211 | 45:7950fa411107 | 205 | } |
Tanja2211 | 52:cce270e0753a | 206 | } |
Tanja2211 | 38:7ed04177892b | 207 | |
Tanja2211 | 52:cce270e0753a | 208 | void AntwoordB() { |
Tanja2211 | 52:cce270e0753a | 209 | drempelwaardeB1=4.99; |
Tanja2211 | 52:cce270e0753a | 210 | drempelwaardeB2=6; |
Tanja2211 | 52:cce270e0753a | 211 | drempelwaardeB3=10; |
Tanja2211 | 45:7950fa411107 | 212 | int yB1; |
Tanja2211 | 45:7950fa411107 | 213 | int yB2; |
Tanja2211 | 45:7950fa411107 | 214 | int yB3; |
Tanja2211 | 38:7ed04177892b | 215 | |
Tanja2211 | 52:cce270e0753a | 216 | if (MOVAVG_B > drempelwaardeB1) { |
Tanja2211 | 45:7950fa411107 | 217 | yB1=1; |
Tanja2211 | 52:cce270e0753a | 218 | if (MOVAVG_B > drempelwaardeB2) { |
Tanja2211 | 45:7950fa411107 | 219 | yB2=1; |
Tanja2211 | 53:d90e54fba7d8 | 220 | if (MOVAVG_B > drempelwaardeB3) { |
Tanja2211 | 53:d90e54fba7d8 | 221 | yB3=1;} |
Tanja2211 | 53:d90e54fba7d8 | 222 | else { |
Tanja2211 | 53:d90e54fba7d8 | 223 | yB3=0;} |
Tanja2211 | 45:7950fa411107 | 224 | } else { |
Tanja2211 | 53:d90e54fba7d8 | 225 | yB2=0; } |
Tanja2211 | 53:d90e54fba7d8 | 226 | } else { |
Tanja2211 | 45:7950fa411107 | 227 | yB1=0; |
Tanja2211 | 45:7950fa411107 | 228 | } |
Tanja2211 | 53:d90e54fba7d8 | 229 | |
Tanja2211 | 45:7950fa411107 | 230 | int snelheidsstand; |
Tanja2211 | 53:d90e54fba7d8 | 231 | |
Tanja2211 | 45:7950fa411107 | 232 | snelheidsstand=yB1+yB2+yB3; |
Tanja2211 | 45:7950fa411107 | 233 | if (snelheidsstand==1) { |
Tanja2211 | 45:7950fa411107 | 234 | pc.printf("Motor 1 beweegt met snelheid 1\n"); |
Tanja2211 | 45:7950fa411107 | 235 | } else { |
Tanja2211 | 48:5a270ba60008 | 236 | pc.printf("Motor 1 beweegt niet met snelheid 1\n"); } |
Tanja2211 | 45:7950fa411107 | 237 | if (snelheidsstand==2) { |
Tanja2211 | 45:7950fa411107 | 238 | pc.printf("Motor 1 beweegt met snelheid 2\n"); |
Tanja2211 | 45:7950fa411107 | 239 | } else { |
Tanja2211 | 48:5a270ba60008 | 240 | pc.printf("Motor 1 beweegt niet met snelheid 2\n"); |
Tanja2211 | 38:7ed04177892b | 241 | } |
Tanja2211 | 45:7950fa411107 | 242 | if (snelheidsstand==3) { |
Tanja2211 | 45:7950fa411107 | 243 | pc.printf("Motor 1 beweegt met snelheid 3\n"); |
Tanja2211 | 45:7950fa411107 | 244 | } else { |
Tanja2211 | 48:5a270ba60008 | 245 | pc.printf("Motor 1 beweegt niet met snelheid 3\n"); |
Tanja2211 | 44:b47f559826ba | 246 | } |
Tanja2211 | 45:7950fa411107 | 247 | } |