test
Dependencies: HIDScope MODSERIAL mbed-dsp mbed
Fork of emg_filter2 by
EMGfilter.cpp@30:5d8e6f0fabc1, 2014-10-17 (annotated)
- Committer:
- s1340735
- Date:
- Fri Oct 17 10:36:29 2014 +0000
- Revision:
- 30:5d8e6f0fabc1
- Parent:
- 29:f54123765a47
- Child:
- 31:b6f7ba4938d4
moving average toegevoegd
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 | 30:5d8e6f0fabc1 | 5 | #include "MAF.h" |
s1340735 | 22:dc630dbb1dcd | 6 | |
s1340735 | 30:5d8e6f0fabc1 | 7 | HIDScope::HIDScope(int channels) : hid(64,64) |
s1340735 | 29:f54123765a47 | 8 | { |
s1340735 | 29:f54123765a47 | 9 | bufferData = new float[channels](); |
s1340735 | 29:f54123765a47 | 10 | channelCount = channels; |
s1340735 | 29:f54123765a47 | 11 | scopeData.length = 64; |
s1340735 | 29:f54123765a47 | 12 | } |
s1340735 | 29:f54123765a47 | 13 | |
s1340735 | 29:f54123765a47 | 14 | void HIDScope::set(int ch, float val) |
s1340735 | 29:f54123765a47 | 15 | { |
s1340735 | 29:f54123765a47 | 16 | bufferData[ch] = val; |
s1340735 | 29:f54123765a47 | 17 | } |
s1340735 | 29:f54123765a47 | 18 | |
s1340735 | 29:f54123765a47 | 19 | void HIDScope::set(int ch, int val) |
s1340735 | 29:f54123765a47 | 20 | { |
s1340735 | 29:f54123765a47 | 21 | set(ch,(float)val); |
s1340735 | 29:f54123765a47 | 22 | } |
s1340735 | 29:f54123765a47 | 23 | |
s1340735 | 29:f54123765a47 | 24 | void HIDScope::set(int ch, bool val) |
s1340735 | 29:f54123765a47 | 25 | { |
s1340735 | 29:f54123765a47 | 26 | set(ch,(val ? 1.0f : 0.0f)); |
s1340735 | 29:f54123765a47 | 27 | } |
s1340735 | 29:f54123765a47 | 28 | |
s1340735 | 29:f54123765a47 | 29 | void HIDScope::set(int ch, double val) |
s1340735 | 29:f54123765a47 | 30 | { |
s1340735 | 29:f54123765a47 | 31 | set(ch,(float)val); |
s1340735 | 29:f54123765a47 | 32 | } |
s1340735 | 29:f54123765a47 | 33 | |
s1340735 | 29:f54123765a47 | 34 | void HIDScope::send() |
s1340735 | 30:5d8e6f0fabc1 | 35 | { |
s1340735 | 29:f54123765a47 | 36 | for(int ch=0; ch<channelCount; ch++) |
s1340735 | 29:f54123765a47 | 37 | memcpy(&scopeData.data[ch*4], &bufferData[ch], 4); // Copy a 4 byte float to the char array |
s1340735 | 30:5d8e6f0fabc1 | 38 | |
s1340735 | 29:f54123765a47 | 39 | // Send non blocking, can be adjusted to blocking (hid.send) |
s1340735 | 29:f54123765a47 | 40 | hid.sendNB(&scopeData); |
s1340735 | 29:f54123765a47 | 41 | } |
s1340735 | 29:f54123765a47 | 42 | |
s1340735 | 29:f54123765a47 | 43 | // ****** emg filter shizzle ****** |
s1340735 | 29:f54123765a47 | 44 | |
s1340735 | 22:dc630dbb1dcd | 45 | //Define objects |
s1340735 | 22:dc630dbb1dcd | 46 | AnalogIn emgB(PTB0); //Analog input bicep |
s1340735 | 22:dc630dbb1dcd | 47 | AnalogIn emgT(PTB1); //Analog input tricep |
s1340735 | 22:dc630dbb1dcd | 48 | |
s1340735 | 23:1c51af8386c9 | 49 | float filtered_emgB; |
s1340735 | 27:24e73fd36859 | 50 | float filtered_emgT; |
s1340735 | 23:1c51af8386c9 | 51 | |
s1340735 | 23:1c51af8386c9 | 52 | MODSERIAL pc(USBTX,USBRX); |
s1340735 | 23:1c51af8386c9 | 53 | |
s1340735 | 26:b93c82fb6e1d | 54 | HIDScope scope(4);//uitgang scherm |
s1340735 | 22:dc630dbb1dcd | 55 | |
s1340735 | 22:dc630dbb1dcd | 56 | arm_biquad_casd_df1_inst_f32 lowpass; |
s1340735 | 22:dc630dbb1dcd | 57 | //constants for 50Hz lowpass |
s1340735 | 30:5d8e6f0fabc1 | 58 | float lowpass_const[] = {0.2928920553, 0.5857841107, 0.2928920554, -0, -0.17156822136};//{a0 a1 a2 -b1 -b2} van online calculator |
s1340735 | 22:dc630dbb1dcd | 59 | //state values |
s1340735 | 26:b93c82fb6e1d | 60 | float lowpass_states[4]; |
s1340735 | 22:dc630dbb1dcd | 61 | |
s1340735 | 22:dc630dbb1dcd | 62 | arm_biquad_casd_df1_inst_f32 highpass; |
s1340735 | 22:dc630dbb1dcd | 63 | //constants for 10Hz highpass |
s1340735 | 26:b93c82fb6e1d | 64 | float highpass_const[] = {0.8005910267, -1.6011820533, 0.8005910267, 1.5610153913, -0.6413487154};//{a0 a1 a2 -b1 -b2} |
s1340735 | 22:dc630dbb1dcd | 65 | //state values |
s1340735 | 26:b93c82fb6e1d | 66 | float highpass_states[4]; |
s1340735 | 22:dc630dbb1dcd | 67 | |
s1340735 | 22:dc630dbb1dcd | 68 | |
s1340735 | 22:dc630dbb1dcd | 69 | /** Looper function |
s1340735 | 22:dc630dbb1dcd | 70 | * functions used for Ticker and Timeout should be of type void <name>(void) |
s1340735 | 22:dc630dbb1dcd | 71 | * i.e. no input arguments, no output arguments. |
s1340735 | 22:dc630dbb1dcd | 72 | * if you want to change a variable that you use in other places (for example in main) |
s1340735 | 22:dc630dbb1dcd | 73 | * you will have to make that variable global in order to be able to reach it both from |
s1340735 | 22:dc630dbb1dcd | 74 | * the function called at interrupt time, and in the main function. |
s1340735 | 22:dc630dbb1dcd | 75 | * To make a variable global, define it under the includes. |
s1340735 | 22:dc630dbb1dcd | 76 | * variables that are changed in the interrupt routine (written to) should be made |
s1340735 | 22:dc630dbb1dcd | 77 | * 'volatile' to let the compiler know that those values may change outside the current context. |
s1340735 | 22:dc630dbb1dcd | 78 | * i.e.: "volatile uint16_t emg_value;" instead of "uint16_t emg_value" |
s1340735 | 22:dc630dbb1dcd | 79 | * in the example below, the variable is not re-used in the main function, and is thus declared |
s1340735 | 22:dc630dbb1dcd | 80 | * local in the looper function only. |
s1340735 | 22:dc630dbb1dcd | 81 | **/ |
s1340735 | 22:dc630dbb1dcd | 82 | |
s1340735 | 22:dc630dbb1dcd | 83 | //BICEP EMG LEZEN |
s1340735 | 22:dc630dbb1dcd | 84 | void looperB() |
s1340735 | 22:dc630dbb1dcd | 85 | { |
s1340735 | 23:1c51af8386c9 | 86 | /*variable to store value in*/ |
s1340735 | 22:dc630dbb1dcd | 87 | uint16_t emg_valueB; |
s1340735 | 30:5d8e6f0fabc1 | 88 | |
s1340735 | 22:dc630dbb1dcd | 89 | float emg_value_f32B; |
s1340735 | 22:dc630dbb1dcd | 90 | /*put raw emg value both in red and in emg_value*/ |
s1340735 | 22:dc630dbb1dcd | 91 | 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 | 92 | emg_value_f32B = emgB.read(); |
s1340735 | 22:dc630dbb1dcd | 93 | |
s1340735 | 22:dc630dbb1dcd | 94 | //process emg |
s1340735 | 22:dc630dbb1dcd | 95 | arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32B, &filtered_emgB, 1 ); |
s1340735 | 22:dc630dbb1dcd | 96 | filtered_emgB = fabs(filtered_emgB); |
s1340735 | 22:dc630dbb1dcd | 97 | arm_biquad_cascade_df1_f32(&lowpass, &filtered_emgB, &filtered_emgB, 1 ); |
s1340735 | 23:1c51af8386c9 | 98 | |
s1340735 | 22:dc630dbb1dcd | 99 | /*send value to PC. */ |
s1340735 | 22:dc630dbb1dcd | 100 | scope.set(0,emg_valueB); //uint value |
s1340735 | 22:dc630dbb1dcd | 101 | scope.set(1,filtered_emgB); //processed float |
s1340735 | 22:dc630dbb1dcd | 102 | scope.send(); |
s1340735 | 22:dc630dbb1dcd | 103 | |
s1340735 | 30:5d8e6f0fabc1 | 104 | MAF::MAF() {} |
s1340735 | 30:5d8e6f0fabc1 | 105 | float MAF::update(float filtered_emgB) { |
s1340735 | 30:5d8e6f0fabc1 | 106 | B[0]=filtered_emgB; |
s1340735 | 30:5d8e6f0fabc1 | 107 | MOVAVG_B=B[0]*0.1+B[1]*0.1+B[2]*0.1+B[3]*0.1+B[4]*0.1+B[5]*0.1+B[7]*0.1+B[8]*0.1+B[9]*0.1 |
s1340735 | 30:5d8e6f0fabc1 | 108 | B[9]=B[8]; |
s1340735 | 30:5d8e6f0fabc1 | 109 | B[8]=B[7]; |
s1340735 | 30:5d8e6f0fabc1 | 110 | B[7]=B[6]; |
s1340735 | 30:5d8e6f0fabc1 | 111 | B[6]=B[5]; |
s1340735 | 30:5d8e6f0fabc1 | 112 | B[5]=B[4]; |
s1340735 | 30:5d8e6f0fabc1 | 113 | B[4]=B[3]; |
s1340735 | 30:5d8e6f0fabc1 | 114 | B[3]=B[2]; |
s1340735 | 30:5d8e6f0fabc1 | 115 | B[2]=B[1]; |
s1340735 | 30:5d8e6f0fabc1 | 116 | B[1]=B[0]; |
s1340735 | 30:5d8e6f0fabc1 | 117 | |
s1340735 | 30:5d8e6f0fabc1 | 118 | return MOVAVG_B; |
s1340735 | 30:5d8e6f0fabc1 | 119 | } |
s1340735 | 22:dc630dbb1dcd | 120 | } |
s1340735 | 22:dc630dbb1dcd | 121 | |
s1340735 | 22:dc630dbb1dcd | 122 | void looperT() |
s1340735 | 22:dc630dbb1dcd | 123 | { |
s1340735 | 23:1c51af8386c9 | 124 | /*variable to store value in*/ |
s1340735 | 22:dc630dbb1dcd | 125 | uint16_t emg_valueT; |
s1340735 | 30:5d8e6f0fabc1 | 126 | |
s1340735 | 22:dc630dbb1dcd | 127 | float emg_value_f32T; |
s1340735 | 22:dc630dbb1dcd | 128 | /*put raw emg value both in red and in emg_value*/ |
s1340735 | 22:dc630dbb1dcd | 129 | emg_valueT = emgT.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V) |
s1340735 | 22:dc630dbb1dcd | 130 | emg_value_f32T = emgT.read(); |
s1340735 | 22:dc630dbb1dcd | 131 | |
s1340735 | 22:dc630dbb1dcd | 132 | //process emg |
s1340735 | 22:dc630dbb1dcd | 133 | arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32T, &filtered_emgT, 1 ); |
s1340735 | 22:dc630dbb1dcd | 134 | filtered_emgT = fabs(filtered_emgT); |
s1340735 | 22:dc630dbb1dcd | 135 | arm_biquad_cascade_df1_f32(&lowpass, &filtered_emgT, &filtered_emgT, 1 ); |
s1340735 | 23:1c51af8386c9 | 136 | |
s1340735 | 22:dc630dbb1dcd | 137 | /*send value to PC. */ |
s1340735 | 27:24e73fd36859 | 138 | scope.set(2,emg_valueT); //uint value |
s1340735 | 27:24e73fd36859 | 139 | scope.set(3,filtered_emgT); //processed float |
s1340735 | 22:dc630dbb1dcd | 140 | scope.send(); |
s1340735 | 22:dc630dbb1dcd | 141 | |
s1340735 | 22:dc630dbb1dcd | 142 | } |
s1340735 | 22:dc630dbb1dcd | 143 | |
s1340735 | 22:dc630dbb1dcd | 144 | int main() |
s1340735 | 22:dc630dbb1dcd | 145 | { |
s1340735 | 22:dc630dbb1dcd | 146 | Ticker log_timer; |
s1340735 | 23:1c51af8386c9 | 147 | //set up filters. Use external array for constants |
s1340735 | 22:dc630dbb1dcd | 148 | arm_biquad_cascade_df1_init_f32(&lowpass,1 , lowpass_const, lowpass_states); |
s1340735 | 22:dc630dbb1dcd | 149 | arm_biquad_cascade_df1_init_f32(&highpass,1 ,highpass_const, highpass_states); |
s1340735 | 22:dc630dbb1dcd | 150 | |
s1340735 | 22:dc630dbb1dcd | 151 | /**Here you attach the 'void looper(void)' function to the Ticker object |
s1340735 | 22:dc630dbb1dcd | 152 | * The looper() function will be called every 0.01 seconds. |
s1340735 | 22:dc630dbb1dcd | 153 | * Please mind that the parentheses after looper are omitted when using attach. |
s1340735 | 22:dc630dbb1dcd | 154 | */ |
s1340735 | 22:dc630dbb1dcd | 155 | log_timer.attach(looperB, 0.005);//?? |
s1340735 | 22:dc630dbb1dcd | 156 | log_timer.attach(looperT, 0.005);//?? |
s1340735 | 23:1c51af8386c9 | 157 | while(1) { //Loop |
s1340735 | 23:1c51af8386c9 | 158 | /*Empty!*/ |
s1340735 | 23:1c51af8386c9 | 159 | /*Everything is handled by the interrupt routine now!*/ |
s1340735 | 23:1c51af8386c9 | 160 | } |
s1340735 | 23:1c51af8386c9 | 161 | } |
s1340735 | 23:1c51af8386c9 | 162 | |
s1340735 | 22:dc630dbb1dcd | 163 | //filtered_emgB |
s1340735 | 22:dc630dbb1dcd | 164 | //filtered_emgT |
s1340735 | 22:dc630dbb1dcd | 165 | |
s1340735 | 23:1c51af8386c9 | 166 | void Antwoord() |
s1340735 | 23:1c51af8386c9 | 167 | { |
s1340735 | 23:1c51af8386c9 | 168 | float drempelwaarde=4.99; |
s1340735 | 25:cfd6db9b4b5d | 169 | int y; |
s1340735 | 30:5d8e6f0fabc1 | 170 | |
s1340735 | 30:5d8e6f0fabc1 | 171 | if (filtered_emgB > drempelwaarde) { |
s1340735 | 25:cfd6db9b4b5d | 172 | y=1; |
s1340735 | 23:1c51af8386c9 | 173 | } else { |
s1340735 | 25:cfd6db9b4b5d | 174 | y=0; |
s1340735 | 23:1c51af8386c9 | 175 | } |
s1340735 | 23:1c51af8386c9 | 176 | |
s1340735 | 25:cfd6db9b4b5d | 177 | if (y==1) { |
s1340735 | 25:cfd6db9b4b5d | 178 | pc.printf("Motor 1 beweegt\n"); |
s1340735 | 23:1c51af8386c9 | 179 | } else { |
s1340735 | 25:cfd6db9b4b5d | 180 | pc.printf("Motor 1 beweegt niet\n"); |
s1340735 | 22:dc630dbb1dcd | 181 | } |
s1340735 | 24:553707c8ebf8 | 182 | } |
s1340735 | 25:cfd6db9b4b5d | 183 | //drempelwaarde..... |