Bandpass, notch, abs en laagdoorlaat 3H
Dependencies: HIDScope MODSERIAL TextLCD mbed-dsp mbed
Fork of EMGmeten by
EMGmeten.cpp@8:4b275b1667d8, 2014-10-31 (annotated)
- Committer:
- lauradeheus
- Date:
- Fri Oct 31 17:15:22 2014 +0000
- Revision:
- 8:4b275b1667d8
- Parent:
- 7:0e76120eb7ad
Werkend_EMG plus pieken meten;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
lauradeheus | 0:b68a6d0451b8 | 1 | #include "mbed.h" |
lauradeheus | 0:b68a6d0451b8 | 2 | #include "HIDScope.h" |
lauradeheus | 2:5d64e4c95f5c | 3 | #include "arm_math.h" |
lauradeheus | 2:5d64e4c95f5c | 4 | #include "MODSERIAL.h" |
lauradeheus | 4:7150ed6ce984 | 5 | #include "TextLCD.h" |
lauradeheus | 4:7150ed6ce984 | 6 | |
lauradeheus | 4:7150ed6ce984 | 7 | #define EMG_treshhold 0,05 |
lauradeheus | 0:b68a6d0451b8 | 8 | |
lauradeheus | 8:4b275b1667d8 | 9 | //Serial pc(USBTX,USBRX); |
lauradeheus | 0:b68a6d0451b8 | 10 | AnalogIn emg(PTB1); //Analog input |
lauradeheus | 8:4b275b1667d8 | 11 | HIDScope scope(3); // Twee kanalen op de HIDScope |
lauradeheus | 4:7150ed6ce984 | 12 | DigitalIn knop(PTD4); /*Digital input pin (knop) definieren*/ |
lauradeheus | 4:7150ed6ce984 | 13 | TextLCD lcd(PTD2, PTA12, PTB2, PTB3, PTC2, PTA13, TextLCD::LCD16x2); |
lauradeheus | 0:b68a6d0451b8 | 14 | |
lauradeheus | 4:7150ed6ce984 | 15 | arm_biquad_casd_df1_inst_f32 lowpass_1; //2e orde lowpass biquad butterworthfilter 99Hz |
lauradeheus | 4:7150ed6ce984 | 16 | arm_biquad_casd_df1_inst_f32 lowpass_2; //2e orde lowpass biquad butterworthfilter 3Hz |
lauradeheus | 4:7150ed6ce984 | 17 | arm_biquad_casd_df1_inst_f32 highpass; //2e orde highpass biquad butterworthfilter 20Hz |
lauradeheus | 4:7150ed6ce984 | 18 | arm_biquad_casd_df1_inst_f32 notch; //2e orde lowpass biquad butterworthfilter 50Hz |
lauradeheus | 1:b7a9fcf50776 | 19 | float lowpass_1_const[] = {0.978030479206560 , 1.956060958413119 , 0.978030479206560 , -1.955578240315036 , -0.956543676511203}; |
lauradeheus | 1:b7a9fcf50776 | 20 | float lowpass_1_states[4]; |
lauradeheus | 4:7150ed6ce984 | 21 | float lowpass_2_const[] = {0.002080567135492 , 0.004161134270985 , 0.002080567135492 , 1.866892279711715 , -0.875214548253684}; //3Hz |
lauradeheus | 4:7150ed6ce984 | 22 | //{0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , 1.911196288237583 , -0.914975054072353};//2Hz |
lauradeheus | 4:7150ed6ce984 | 23 | //{0.00024135899874854145 , 0.0004827179974970829 , 0.00024135899874854145 , 1.9555778328194147 , -0.9565432688144089}; 1Hz |
lauradeheus | 1:b7a9fcf50776 | 24 | float lowpass_2_states[4]; |
lauradeheus | 1:b7a9fcf50776 | 25 | float highpass_const[] = {0.638945525159022 , -1.277891050318045 , 0.638945525159022 , 1.142980502539901 , -0.412801598096189}; |
lauradeheus | 0:b68a6d0451b8 | 26 | float highpass_states[4]; |
lauradeheus | 2:5d64e4c95f5c | 27 | float notch_const[] = {0.978048948305681 , 0.000000000000000 , 0.978048948305681 , 0.000000000000000 , -0.956097896611362}; |
lauradeheus | 2:5d64e4c95f5c | 28 | float notch_states[4]; |
lauradeheus | 8:4b275b1667d8 | 29 | float emg_filtered; |
lauradeheus | 8:4b275b1667d8 | 30 | float emg_max=0.15; |
lauradeheus | 8:4b275b1667d8 | 31 | float emg_treshhold_laag = 0.3*emg_max; |
lauradeheus | 8:4b275b1667d8 | 32 | float emg_treshhold_hoog = 0.7*emg_max; |
lauradeheus | 4:7150ed6ce984 | 33 | |
lauradeheus | 4:7150ed6ce984 | 34 | bool aanspan; |
lauradeheus | 4:7150ed6ce984 | 35 | int aantal_pieken; |
lauradeheus | 5:f93b7068c202 | 36 | int doel; |
lauradeheus | 4:7150ed6ce984 | 37 | |
lauradeheus | 8:4b275b1667d8 | 38 | void emg_filtering(); |
lauradeheus | 4:7150ed6ce984 | 39 | void pieken_tellen(); |
lauradeheus | 8:4b275b1667d8 | 40 | void emg_max_meting(); |
lauradeheus | 8:4b275b1667d8 | 41 | |
lauradeheus | 8:4b275b1667d8 | 42 | //void emg_max_meting(){ |
lauradeheus | 8:4b275b1667d8 | 43 | // if (emg_filtered>=emg_max) |
lauradeheus | 8:4b275b1667d8 | 44 | // { |
lauradeheus | 8:4b275b1667d8 | 45 | // emg_max=emg_filtered; |
lauradeheus | 8:4b275b1667d8 | 46 | // } |
lauradeheus | 8:4b275b1667d8 | 47 | // emg_treshhold_laag = 0.3*emg_max; |
lauradeheus | 8:4b275b1667d8 | 48 | // emg_treshhold_hoog = 0.7*emg_max; |
lauradeheus | 8:4b275b1667d8 | 49 | //} |
lauradeheus | 8:4b275b1667d8 | 50 | |
lauradeheus | 8:4b275b1667d8 | 51 | void emg_filtering() { |
lauradeheus | 8:4b275b1667d8 | 52 | uint16_t emg_value; |
lauradeheus | 8:4b275b1667d8 | 53 | float emg_value_f32; |
lauradeheus | 8:4b275b1667d8 | 54 | emg_value = emg.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V) |
lauradeheus | 8:4b275b1667d8 | 55 | emg_value_f32 = emg.read(); |
lauradeheus | 8:4b275b1667d8 | 56 | |
lauradeheus | 8:4b275b1667d8 | 57 | arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32, &emg_filtered, 1 ); |
lauradeheus | 8:4b275b1667d8 | 58 | arm_biquad_cascade_df1_f32(&lowpass_1, &emg_filtered, &emg_filtered, 1 ); |
lauradeheus | 8:4b275b1667d8 | 59 | arm_biquad_cascade_df1_f32(¬ch, &emg_filtered, &emg_filtered, 1); |
lauradeheus | 8:4b275b1667d8 | 60 | emg_filtered = fabs(emg_filtered); |
lauradeheus | 8:4b275b1667d8 | 61 | arm_biquad_cascade_df1_f32(&lowpass_2, &emg_filtered, &emg_filtered, 1 ); |
lauradeheus | 8:4b275b1667d8 | 62 | |
lauradeheus | 8:4b275b1667d8 | 63 | pieken_tellen(); |
lauradeheus | 8:4b275b1667d8 | 64 | doel = aantal_pieken-((aantal_pieken/3)*3)+1; |
lauradeheus | 8:4b275b1667d8 | 65 | |
lauradeheus | 8:4b275b1667d8 | 66 | scope.set(0,emg_value); //uint value |
lauradeheus | 8:4b275b1667d8 | 67 | scope.set(1,emg_filtered); //processed float |
lauradeheus | 8:4b275b1667d8 | 68 | scope.set(2,doel); |
lauradeheus | 8:4b275b1667d8 | 69 | scope.send(); |
lauradeheus | 8:4b275b1667d8 | 70 | |
lauradeheus | 8:4b275b1667d8 | 71 | lcd.cls(); |
lauradeheus | 8:4b275b1667d8 | 72 | lcd.printf("Hallo"); |
lauradeheus | 8:4b275b1667d8 | 73 | } |
lauradeheus | 8:4b275b1667d8 | 74 | |
lauradeheus | 8:4b275b1667d8 | 75 | void pieken_tellen(){ |
lauradeheus | 8:4b275b1667d8 | 76 | if (emg_filtered>=emg_treshhold_hoog) |
lauradeheus | 8:4b275b1667d8 | 77 | { |
lauradeheus | 8:4b275b1667d8 | 78 | aanspan=true; //maak een variabele waarin je opslaat dat het signaal hoog is. |
lauradeheus | 8:4b275b1667d8 | 79 | } |
lauradeheus | 8:4b275b1667d8 | 80 | if (aanspan==true && emg_filtered<=emg_treshhold_laag)//== ipv =, anders wordt aanspan true gemaakt |
lauradeheus | 8:4b275b1667d8 | 81 | { |
lauradeheus | 8:4b275b1667d8 | 82 | aanspan=false; |
lauradeheus | 8:4b275b1667d8 | 83 | aantal_pieken++; |
lauradeheus | 8:4b275b1667d8 | 84 | doel = aantal_pieken-((aantal_pieken/3)*3)+1; |
lauradeheus | 8:4b275b1667d8 | 85 | } |
lauradeheus | 8:4b275b1667d8 | 86 | } |
lauradeheus | 4:7150ed6ce984 | 87 | //char *lcd_r1 = new char[16]; |
lauradeheus | 4:7150ed6ce984 | 88 | //char *lcd_r2 = new char[16]; |
lauradeheus | 0:b68a6d0451b8 | 89 | |
lauradeheus | 8:4b275b1667d8 | 90 | /* |
lauradeheus | 8:4b275b1667d8 | 91 | void emg_filtering() |
lauradeheus | 0:b68a6d0451b8 | 92 | { |
lauradeheus | 0:b68a6d0451b8 | 93 | uint16_t emg_value; |
lauradeheus | 0:b68a6d0451b8 | 94 | float emg_value_f32; |
lauradeheus | 0:b68a6d0451b8 | 95 | emg_value = emg.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V) |
lauradeheus | 0:b68a6d0451b8 | 96 | emg_value_f32 = emg.read(); |
lauradeheus | 0:b68a6d0451b8 | 97 | |
lauradeheus | 8:4b275b1667d8 | 98 | arm_biquad_cascade_df1_f32(&highpass, &emg_value_f32, &emg_filtered, 1 ); |
lauradeheus | 8:4b275b1667d8 | 99 | arm_biquad_cascade_df1_f32(&lowpass_1, &emg_filtered, &emg_filtered, 1 ); |
lauradeheus | 8:4b275b1667d8 | 100 | arm_biquad_cascade_df1_f32(¬ch, &emg_filtered, &emg_filtered, 1); |
lauradeheus | 8:4b275b1667d8 | 101 | emg_filtered = fabs(emg_filtered); |
lauradeheus | 2:5d64e4c95f5c | 102 | //emg_value_f32 = fabs(emg_value_f32); |
lauradeheus | 8:4b275b1667d8 | 103 | arm_biquad_cascade_df1_f32(&lowpass_2, &emg_filtered, &emg_filtered, 1 ); |
lauradeheus | 0:b68a6d0451b8 | 104 | |
lauradeheus | 6:740f08fad2c3 | 105 | //EMG_max_meting(); |
lauradeheus | 6:740f08fad2c3 | 106 | pieken_tellen(); |
lauradeheus | 6:740f08fad2c3 | 107 | doel = aantal_pieken-((aantal_pieken/3)*3)+1; |
lauradeheus | 8:4b275b1667d8 | 108 | //pc.printf("%d",doel); |
lauradeheus | 6:740f08fad2c3 | 109 | |
lauradeheus | 0:b68a6d0451b8 | 110 | scope.set(0,emg_value); //uint value |
lauradeheus | 8:4b275b1667d8 | 111 | scope.set(1,emg_filtered); //processed float |
lauradeheus | 8:4b275b1667d8 | 112 | scope.set(2,doel); |
lauradeheus | 0:b68a6d0451b8 | 113 | scope.send(); |
lauradeheus | 0:b68a6d0451b8 | 114 | } |
lauradeheus | 0:b68a6d0451b8 | 115 | |
lauradeheus | 8:4b275b1667d8 | 116 | void emg_max_meting() |
lauradeheus | 6:740f08fad2c3 | 117 | { |
lauradeheus | 7:0e76120eb7ad | 118 | //int i=0; // tijdspad |
lauradeheus | 7:0e76120eb7ad | 119 | //lcd.cls(); |
lauradeheus | 7:0e76120eb7ad | 120 | //lcd.printf("Maximale\nEMGmeting"); |
lauradeheus | 7:0e76120eb7ad | 121 | //for(i=0; i<300; i++) |
lauradeheus | 7:0e76120eb7ad | 122 | //{ |
lauradeheus | 7:0e76120eb7ad | 123 | // wait(0.1); |
lauradeheus | 8:4b275b1667d8 | 124 | if (emg_filtered>=emg_max) |
lauradeheus | 6:740f08fad2c3 | 125 | { |
lauradeheus | 8:4b275b1667d8 | 126 | emg_max=emg_filtered; |
lauradeheus | 6:740f08fad2c3 | 127 | } |
lauradeheus | 7:0e76120eb7ad | 128 | //} |
lauradeheus | 7:0e76120eb7ad | 129 | //lcd.cls(); |
lauradeheus | 7:0e76120eb7ad | 130 | //lcd.printf("Maximale EMG\nbepaald"); |
lauradeheus | 7:0e76120eb7ad | 131 | //wait(3); |
lauradeheus | 8:4b275b1667d8 | 132 | emg_treshhold_laag = 0.3*emg_max; |
lauradeheus | 8:4b275b1667d8 | 133 | emg_treshhold_hoog = 0.7*emg_max; |
lauradeheus | 6:740f08fad2c3 | 134 | } |
lauradeheus | 6:740f08fad2c3 | 135 | |
lauradeheus | 6:740f08fad2c3 | 136 | void pieken_tellen() |
lauradeheus | 6:740f08fad2c3 | 137 | { |
lauradeheus | 8:4b275b1667d8 | 138 | if (emg_filtered>=emg_treshhold_hoog) |
lauradeheus | 6:740f08fad2c3 | 139 | { |
lauradeheus | 6:740f08fad2c3 | 140 | aanspan=true; //maak een variabele waarin je opslaat dat het signaal hoog is. |
lauradeheus | 6:740f08fad2c3 | 141 | } |
lauradeheus | 8:4b275b1667d8 | 142 | if (aanspan==true && emg_filtered<=emg_treshhold_laag)//== ipv =, anders wordt aanspan true gemaakt |
lauradeheus | 6:740f08fad2c3 | 143 | { |
lauradeheus | 6:740f08fad2c3 | 144 | aanspan=false; |
lauradeheus | 6:740f08fad2c3 | 145 | aantal_pieken++; |
lauradeheus | 6:740f08fad2c3 | 146 | } |
lauradeheus | 6:740f08fad2c3 | 147 | } |
lauradeheus | 8:4b275b1667d8 | 148 | */ |
lauradeheus | 8:4b275b1667d8 | 149 | |
lauradeheus | 6:740f08fad2c3 | 150 | |
lauradeheus | 0:b68a6d0451b8 | 151 | int main() |
lauradeheus | 0:b68a6d0451b8 | 152 | { |
lauradeheus | 0:b68a6d0451b8 | 153 | Ticker log_timer; |
lauradeheus | 0:b68a6d0451b8 | 154 | |
lauradeheus | 1:b7a9fcf50776 | 155 | arm_biquad_cascade_df1_init_f32(&lowpass_1,1 , lowpass_1_const, lowpass_1_states); |
lauradeheus | 0:b68a6d0451b8 | 156 | arm_biquad_cascade_df1_init_f32(&highpass,1 , highpass_const, highpass_states); |
lauradeheus | 2:5d64e4c95f5c | 157 | arm_biquad_cascade_df1_init_f32(¬ch,1 , notch_const, notch_states); |
lauradeheus | 3:c82170d8b6c8 | 158 | arm_biquad_cascade_df1_init_f32(&lowpass_2,1 , lowpass_2_const, lowpass_2_states); |
lauradeheus | 0:b68a6d0451b8 | 159 | |
lauradeheus | 8:4b275b1667d8 | 160 | log_timer.attach(emg_filtering, 0.005); |
lauradeheus | 0:b68a6d0451b8 | 161 | |
lauradeheus | 0:b68a6d0451b8 | 162 | while(1) //Loop |
lauradeheus | 0:b68a6d0451b8 | 163 | { |
lauradeheus | 8:4b275b1667d8 | 164 | //lcd.cls(); |
lauradeheus | 8:4b275b1667d8 | 165 | //lcd.printf("Te raken doel =\n%d", doel); |
lauradeheus | 8:4b275b1667d8 | 166 | //pc.baud(9600); |
lauradeheus | 8:4b275b1667d8 | 167 | //(0.02); |
lauradeheus | 0:b68a6d0451b8 | 168 | /*Empty!*/ |
lauradeheus | 0:b68a6d0451b8 | 169 | /*Everything is handled by the interrupt routine now!*/ |
lauradeheus | 2:5d64e4c95f5c | 170 | //pc.baud(9600); |
lauradeheus | 0:b68a6d0451b8 | 171 | } |
lauradeheus | 6:740f08fad2c3 | 172 | } |