test
Dependencies: HIDScope MODSERIAL mbed-dsp mbed
Fork of emg_filter2 by
EMGfilter.cpp@26:b93c82fb6e1d, 2014-10-17 (annotated)
- Committer:
- s1340735
- Date:
- Fri Oct 17 08:32:39 2014 +0000
- Revision:
- 26:b93c82fb6e1d
- Parent:
- 25:cfd6db9b4b5d
- Child:
- 27:24e73fd36859
filter coefficienten;
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 | 22:dc630dbb1dcd | 6 | //Define objects |
s1340735 | 22:dc630dbb1dcd | 7 | AnalogIn emgB(PTB0); //Analog input bicep |
s1340735 | 22:dc630dbb1dcd | 8 | AnalogIn emgT(PTB1); //Analog input tricep |
s1340735 | 22:dc630dbb1dcd | 9 | |
s1340735 | 23:1c51af8386c9 | 10 | float filtered_emgB; |
s1340735 | 23:1c51af8386c9 | 11 | |
s1340735 | 23:1c51af8386c9 | 12 | MODSERIAL pc(USBTX,USBRX); |
s1340735 | 23:1c51af8386c9 | 13 | |
s1340735 | 26:b93c82fb6e1d | 14 | HIDScope scope(4);//uitgang scherm |
s1340735 | 22:dc630dbb1dcd | 15 | |
s1340735 | 22:dc630dbb1dcd | 16 | arm_biquad_casd_df1_inst_f32 lowpass; |
s1340735 | 22:dc630dbb1dcd | 17 | //constants for 50Hz lowpass |
s1340735 | 26:b93c82fb6e1d | 18 | float lowpass_const[] = {0.2928920553, 0.5857841107, 0.2928920554, -0, -0.17156822136};//{a0 a1 a2 -b1 -b2} van online calculator |
s1340735 | 22:dc630dbb1dcd | 19 | //state values |
s1340735 | 26:b93c82fb6e1d | 20 | float lowpass_states[4]; |
s1340735 | 22:dc630dbb1dcd | 21 | |
s1340735 | 22:dc630dbb1dcd | 22 | arm_biquad_casd_df1_inst_f32 highpass; |
s1340735 | 22:dc630dbb1dcd | 23 | //constants for 10Hz highpass |
s1340735 | 26:b93c82fb6e1d | 24 | float highpass_const[] = {0.8005910267, -1.6011820533, 0.8005910267, 1.5610153913, -0.6413487154};//{a0 a1 a2 -b1 -b2} |
s1340735 | 22:dc630dbb1dcd | 25 | //state values |
s1340735 | 26:b93c82fb6e1d | 26 | float highpass_states[4]; |
s1340735 | 22:dc630dbb1dcd | 27 | |
s1340735 | 22:dc630dbb1dcd | 28 | |
s1340735 | 22:dc630dbb1dcd | 29 | /** Looper function |
s1340735 | 22:dc630dbb1dcd | 30 | * functions used for Ticker and Timeout should be of type void <name>(void) |
s1340735 | 22:dc630dbb1dcd | 31 | * i.e. no input arguments, no output arguments. |
s1340735 | 22:dc630dbb1dcd | 32 | * if you want to change a variable that you use in other places (for example in main) |
s1340735 | 22:dc630dbb1dcd | 33 | * you will have to make that variable global in order to be able to reach it both from |
s1340735 | 22:dc630dbb1dcd | 34 | * the function called at interrupt time, and in the main function. |
s1340735 | 22:dc630dbb1dcd | 35 | * To make a variable global, define it under the includes. |
s1340735 | 22:dc630dbb1dcd | 36 | * variables that are changed in the interrupt routine (written to) should be made |
s1340735 | 22:dc630dbb1dcd | 37 | * 'volatile' to let the compiler know that those values may change outside the current context. |
s1340735 | 22:dc630dbb1dcd | 38 | * i.e.: "volatile uint16_t emg_value;" instead of "uint16_t emg_value" |
s1340735 | 22:dc630dbb1dcd | 39 | * in the example below, the variable is not re-used in the main function, and is thus declared |
s1340735 | 22:dc630dbb1dcd | 40 | * local in the looper function only. |
s1340735 | 22:dc630dbb1dcd | 41 | **/ |
s1340735 | 22:dc630dbb1dcd | 42 | |
s1340735 | 22:dc630dbb1dcd | 43 | //BICEP EMG LEZEN |
s1340735 | 22:dc630dbb1dcd | 44 | void looperB() |
s1340735 | 22:dc630dbb1dcd | 45 | { |
s1340735 | 23:1c51af8386c9 | 46 | /*variable to store value in*/ |
s1340735 | 22:dc630dbb1dcd | 47 | uint16_t emg_valueB; |
s1340735 | 23:1c51af8386c9 | 48 | |
s1340735 | 22:dc630dbb1dcd | 49 | float emg_value_f32B; |
s1340735 | 22:dc630dbb1dcd | 50 | /*put raw emg value both in red and in emg_value*/ |
s1340735 | 22:dc630dbb1dcd | 51 | 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 | 52 | emg_value_f32B = emgB.read(); |
s1340735 | 22:dc630dbb1dcd | 53 | |
s1340735 | 22:dc630dbb1dcd | 54 | //process emg |
s1340735 | 22:dc630dbb1dcd | 55 | arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32B, &filtered_emgB, 1 ); |
s1340735 | 22:dc630dbb1dcd | 56 | filtered_emgB = fabs(filtered_emgB); |
s1340735 | 22:dc630dbb1dcd | 57 | arm_biquad_cascade_df1_f32(&lowpass, &filtered_emgB, &filtered_emgB, 1 ); |
s1340735 | 23:1c51af8386c9 | 58 | |
s1340735 | 22:dc630dbb1dcd | 59 | /*send value to PC. */ |
s1340735 | 22:dc630dbb1dcd | 60 | scope.set(0,emg_valueB); //uint value |
s1340735 | 22:dc630dbb1dcd | 61 | scope.set(1,filtered_emgB); //processed float |
s1340735 | 22:dc630dbb1dcd | 62 | scope.send(); |
s1340735 | 22:dc630dbb1dcd | 63 | |
s1340735 | 22:dc630dbb1dcd | 64 | } |
s1340735 | 22:dc630dbb1dcd | 65 | |
s1340735 | 22:dc630dbb1dcd | 66 | void looperT() |
s1340735 | 22:dc630dbb1dcd | 67 | { |
s1340735 | 23:1c51af8386c9 | 68 | /*variable to store value in*/ |
s1340735 | 22:dc630dbb1dcd | 69 | uint16_t emg_valueT; |
s1340735 | 22:dc630dbb1dcd | 70 | float filtered_emgT; |
s1340735 | 22:dc630dbb1dcd | 71 | float emg_value_f32T; |
s1340735 | 22:dc630dbb1dcd | 72 | /*put raw emg value both in red and in emg_value*/ |
s1340735 | 22:dc630dbb1dcd | 73 | 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 | 74 | emg_value_f32T = emgT.read(); |
s1340735 | 22:dc630dbb1dcd | 75 | |
s1340735 | 22:dc630dbb1dcd | 76 | //process emg |
s1340735 | 22:dc630dbb1dcd | 77 | arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32T, &filtered_emgT, 1 ); |
s1340735 | 22:dc630dbb1dcd | 78 | filtered_emgT = fabs(filtered_emgT); |
s1340735 | 22:dc630dbb1dcd | 79 | arm_biquad_cascade_df1_f32(&lowpass, &filtered_emgT, &filtered_emgT, 1 ); |
s1340735 | 23:1c51af8386c9 | 80 | |
s1340735 | 22:dc630dbb1dcd | 81 | /*send value to PC. */ |
s1340735 | 22:dc630dbb1dcd | 82 | scope.set(0,emg_valueT); //uint value |
s1340735 | 22:dc630dbb1dcd | 83 | scope.set(1,filtered_emgT); //processed float |
s1340735 | 22:dc630dbb1dcd | 84 | scope.send(); |
s1340735 | 22:dc630dbb1dcd | 85 | |
s1340735 | 22:dc630dbb1dcd | 86 | } |
s1340735 | 22:dc630dbb1dcd | 87 | |
s1340735 | 22:dc630dbb1dcd | 88 | int main() |
s1340735 | 22:dc630dbb1dcd | 89 | { |
s1340735 | 22:dc630dbb1dcd | 90 | Ticker log_timer; |
s1340735 | 23:1c51af8386c9 | 91 | //set up filters. Use external array for constants |
s1340735 | 22:dc630dbb1dcd | 92 | arm_biquad_cascade_df1_init_f32(&lowpass,1 , lowpass_const, lowpass_states); |
s1340735 | 22:dc630dbb1dcd | 93 | arm_biquad_cascade_df1_init_f32(&highpass,1 ,highpass_const, highpass_states); |
s1340735 | 22:dc630dbb1dcd | 94 | |
s1340735 | 22:dc630dbb1dcd | 95 | /**Here you attach the 'void looper(void)' function to the Ticker object |
s1340735 | 22:dc630dbb1dcd | 96 | * The looper() function will be called every 0.01 seconds. |
s1340735 | 22:dc630dbb1dcd | 97 | * Please mind that the parentheses after looper are omitted when using attach. |
s1340735 | 22:dc630dbb1dcd | 98 | */ |
s1340735 | 22:dc630dbb1dcd | 99 | log_timer.attach(looperB, 0.005);//?? |
s1340735 | 22:dc630dbb1dcd | 100 | log_timer.attach(looperT, 0.005);//?? |
s1340735 | 23:1c51af8386c9 | 101 | while(1) { //Loop |
s1340735 | 23:1c51af8386c9 | 102 | /*Empty!*/ |
s1340735 | 23:1c51af8386c9 | 103 | /*Everything is handled by the interrupt routine now!*/ |
s1340735 | 23:1c51af8386c9 | 104 | } |
s1340735 | 23:1c51af8386c9 | 105 | } |
s1340735 | 23:1c51af8386c9 | 106 | |
s1340735 | 22:dc630dbb1dcd | 107 | //filtered_emgB |
s1340735 | 22:dc630dbb1dcd | 108 | //filtered_emgT |
s1340735 | 22:dc630dbb1dcd | 109 | |
s1340735 | 23:1c51af8386c9 | 110 | void Antwoord() |
s1340735 | 23:1c51af8386c9 | 111 | { |
s1340735 | 23:1c51af8386c9 | 112 | float drempelwaarde=4.99; |
s1340735 | 25:cfd6db9b4b5d | 113 | int y; |
s1340735 | 25:cfd6db9b4b5d | 114 | |
s1340735 | 25:cfd6db9b4b5d | 115 | if (filtered_emgB > drempelwaarde) |
s1340735 | 25:cfd6db9b4b5d | 116 | { |
s1340735 | 25:cfd6db9b4b5d | 117 | y=1; |
s1340735 | 23:1c51af8386c9 | 118 | } else { |
s1340735 | 25:cfd6db9b4b5d | 119 | y=0; |
s1340735 | 23:1c51af8386c9 | 120 | } |
s1340735 | 23:1c51af8386c9 | 121 | |
s1340735 | 25:cfd6db9b4b5d | 122 | if (y==1) { |
s1340735 | 25:cfd6db9b4b5d | 123 | pc.printf("Motor 1 beweegt\n"); |
s1340735 | 23:1c51af8386c9 | 124 | } else { |
s1340735 | 25:cfd6db9b4b5d | 125 | pc.printf("Motor 1 beweegt niet\n"); |
s1340735 | 22:dc630dbb1dcd | 126 | } |
s1340735 | 24:553707c8ebf8 | 127 | } |
s1340735 | 25:cfd6db9b4b5d | 128 | //drempelwaarde..... |