Yoichi Nagashima
/
CQ_nagasm_09_emergency
NucleoF401RE EMG sensor with Artifact Filter, Notch Filter, etc
main.cpp@1:7bccad3c277f, 2014-12-20 (annotated)
- Committer:
- nagasm
- Date:
- Sat Dec 20 02:09:12 2014 +0000
- Revision:
- 1:7bccad3c277f
- Parent:
- 0:805cf4bfa472
revised
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nagasm | 0:805cf4bfa472 | 1 | #include "mbed.h" |
nagasm | 0:805cf4bfa472 | 2 | #include "sub.hpp" |
nagasm | 0:805cf4bfa472 | 3 | #include "FIR_LPF.hpp" |
nagasm | 0:805cf4bfa472 | 4 | #include "notch.hpp" |
nagasm | 0:805cf4bfa472 | 5 | |
nagasm | 0:805cf4bfa472 | 6 | int main(){ |
nagasm | 0:805cf4bfa472 | 7 | common_setup(); |
nagasm | 0:805cf4bfa472 | 8 | xbee.baud(38400); |
nagasm | 0:805cf4bfa472 | 9 | xbee.attach(&rx_fifoset, xbee.RxIrq); |
nagasm | 0:805cf4bfa472 | 10 | timer_setup.attach_us(&timer_interrupt, 5); // 5usec |
nagasm | 0:805cf4bfa472 | 11 | while(1){ |
nagasm | 0:805cf4bfa472 | 12 | tx_fifo_check(); |
nagasm | 0:805cf4bfa472 | 13 | if(timer_value[1] > 19){ // 0.1msec sampling |
nagasm | 0:805cf4bfa472 | 14 | timer_value[1] = 0; |
nagasm | 0:805cf4bfa472 | 15 | float data1 = (float)gain * (analog_value2.read() - 0.5f); // A/D in (3) |
nagasm | 0:805cf4bfa472 | 16 | if(emergence!=0 && data1<0.05 && data1>0.95){ |
nagasm | 0:805cf4bfa472 | 17 | data1 = old_1; |
nagasm | 0:805cf4bfa472 | 18 | if(++em_count1 > 10){ |
nagasm | 0:805cf4bfa472 | 19 | em_count1 = 0; |
nagasm | 0:805cf4bfa472 | 20 | tx_message(0xff0000); // Emergency 1 |
nagasm | 0:805cf4bfa472 | 21 | } |
nagasm | 0:805cf4bfa472 | 22 | } |
nagasm | 0:805cf4bfa472 | 23 | if(data1 < 0) data1 = -data1; // always detection ON |
nagasm | 0:805cf4bfa472 | 24 | if(fir_lpf != 0) data1 = FIR_calc1(data1); // FIR calc (1) call |
nagasm | 0:805cf4bfa472 | 25 | if(notch_1 != 0) data1 = notch_filter1(data1); |
nagasm | 0:805cf4bfa472 | 26 | old_1 = data1; |
nagasm | 0:805cf4bfa472 | 27 | if(timer_value[2] > 2999){ // 15msec |
nagasm | 0:805cf4bfa472 | 28 | timer_value[2] = 0; |
nagasm | 0:805cf4bfa472 | 29 | if(max_count != 0) data1 = move_mean_calc1(data1); |
nagasm | 0:805cf4bfa472 | 30 | tx_message((uint16_t)((data1 + 1.0f) * 2047)<<4); |
nagasm | 0:805cf4bfa472 | 31 | } |
nagasm | 0:805cf4bfa472 | 32 | } |
nagasm | 0:805cf4bfa472 | 33 | if(timer_value[3] > 19){ // 0.1msec sampling |
nagasm | 0:805cf4bfa472 | 34 | timer_value[3] = 0; |
nagasm | 0:805cf4bfa472 | 35 | float data2 = (float)gain * (analog_value3.read() - 0.5f); // A/D in (4) |
nagasm | 0:805cf4bfa472 | 36 | if(emergence!=0 && data2<0.05 && data2>0.95){ |
nagasm | 0:805cf4bfa472 | 37 | data2 = old_2; |
nagasm | 0:805cf4bfa472 | 38 | if(++em_count2 > 10){ |
nagasm | 0:805cf4bfa472 | 39 | em_count2 = 0; |
nagasm | 0:805cf4bfa472 | 40 | tx_message(0xff0001); // Emergency 2 |
nagasm | 0:805cf4bfa472 | 41 | } |
nagasm | 0:805cf4bfa472 | 42 | } |
nagasm | 0:805cf4bfa472 | 43 | if(data2 < 0) data2 = -data2; // always detection ON |
nagasm | 0:805cf4bfa472 | 44 | if(fir_lpf != 0) data2 = FIR_calc2(data2); // FIR calc (1) call |
nagasm | 0:805cf4bfa472 | 45 | if(notch_2 != 0) data2 = notch_filter2(data2); |
nagasm | 0:805cf4bfa472 | 46 | old_2 = data2; |
nagasm | 0:805cf4bfa472 | 47 | if(timer_value[4] > 2999){ // 15msec |
nagasm | 0:805cf4bfa472 | 48 | timer_value[4] = 0; |
nagasm | 0:805cf4bfa472 | 49 | if(max_count != 0) data2 = move_mean_calc2(data2); |
nagasm | 0:805cf4bfa472 | 50 | tx_message(0x400000 + ((uint16_t)((data2 + 1.0f) * 2047)<<4)); |
nagasm | 0:805cf4bfa472 | 51 | } |
nagasm | 0:805cf4bfa472 | 52 | } |
nagasm | 0:805cf4bfa472 | 53 | if(timer_value[0] > 199999){ // 1000msec |
nagasm | 0:805cf4bfa472 | 54 | timer_value[0] = 0; |
nagasm | 0:805cf4bfa472 | 55 | myled = !myled; |
nagasm | 0:805cf4bfa472 | 56 | } |
nagasm | 0:805cf4bfa472 | 57 | if(rx_fifo_check() == 1){ |
nagasm | 0:805cf4bfa472 | 58 | int sum = 0; |
nagasm | 0:805cf4bfa472 | 59 | for (int i=0; i<6; i++) sum += conv_hex(raw_data[i])<<(4*(5-i)); |
nagasm | 0:805cf4bfa472 | 60 | tx_message(sum); // Echo Back |
nagasm | 0:805cf4bfa472 | 61 | if(sum>>16 == 0x80){ |
nagasm | 0:805cf4bfa472 | 62 | switch((sum & 0xff00)>>8){ |
nagasm | 0:805cf4bfa472 | 63 | case 0x00: |
nagasm | 0:805cf4bfa472 | 64 | fir_lpf = sum & 0x01; |
nagasm | 0:805cf4bfa472 | 65 | break; |
nagasm | 0:805cf4bfa472 | 66 | case 0x01: |
nagasm | 0:805cf4bfa472 | 67 | gain = sum & 0x0f; |
nagasm | 0:805cf4bfa472 | 68 | break; |
nagasm | 0:805cf4bfa472 | 69 | case 0x02: |
nagasm | 0:805cf4bfa472 | 70 | max_count = sum & 0x7f; |
nagasm | 0:805cf4bfa472 | 71 | if (max_count>100) max_count = 100; |
nagasm | 0:805cf4bfa472 | 72 | sum_clear(); |
nagasm | 0:805cf4bfa472 | 73 | break; |
nagasm | 0:805cf4bfa472 | 74 | case 0x03: |
nagasm | 0:805cf4bfa472 | 75 | notch_1 = sum & 0x01; |
nagasm | 0:805cf4bfa472 | 76 | break; |
nagasm | 0:805cf4bfa472 | 77 | case 0x04: |
nagasm | 0:805cf4bfa472 | 78 | notch_2 = sum & 0x01; |
nagasm | 0:805cf4bfa472 | 79 | break; |
nagasm | 0:805cf4bfa472 | 80 | case 0x05: |
nagasm | 0:805cf4bfa472 | 81 | coef_set(sum & 0x3f); |
nagasm | 0:805cf4bfa472 | 82 | break; |
nagasm | 0:805cf4bfa472 | 83 | case 0x06: |
nagasm | 0:805cf4bfa472 | 84 | emergence = sum & 0x01; |
nagasm | 0:805cf4bfa472 | 85 | break; |
nagasm | 0:805cf4bfa472 | 86 | } |
nagasm | 0:805cf4bfa472 | 87 | } |
nagasm | 0:805cf4bfa472 | 88 | } |
nagasm | 0:805cf4bfa472 | 89 | } |
nagasm | 0:805cf4bfa472 | 90 | } |