Yoichi Nagashima
/
CQ_nagasm_11_FFT1
NucleoF401RE EMG sensor with 2channel 16band FFT, mixed 7band XBee send, only test
main.cpp@0:5e2a4b964485, 2014-12-20 (annotated)
- Committer:
- nagasm
- Date:
- Sat Dec 20 05:22:00 2014 +0000
- Revision:
- 0:5e2a4b964485
NucleoF401RE EMG sensor with 2channel 16band FFT, mixed 7band XBee send, only test
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nagasm | 0:5e2a4b964485 | 1 | #include "mbed.h" |
nagasm | 0:5e2a4b964485 | 2 | #include "sub.hpp" |
nagasm | 0:5e2a4b964485 | 3 | #include "notch.hpp" |
nagasm | 0:5e2a4b964485 | 4 | #include "fft1.hpp" |
nagasm | 0:5e2a4b964485 | 5 | |
nagasm | 0:5e2a4b964485 | 6 | float in1[16], out1[16], in2[16], out2[16]; |
nagasm | 0:5e2a4b964485 | 7 | |
nagasm | 0:5e2a4b964485 | 8 | int main(){ |
nagasm | 0:5e2a4b964485 | 9 | common_setup(); |
nagasm | 0:5e2a4b964485 | 10 | int fft_counter1 = 0; |
nagasm | 0:5e2a4b964485 | 11 | int fft_counter2 = 0; |
nagasm | 0:5e2a4b964485 | 12 | xbee.baud(38400); |
nagasm | 0:5e2a4b964485 | 13 | xbee.attach(&rx_fifoset, xbee.RxIrq); |
nagasm | 0:5e2a4b964485 | 14 | timer_setup.attach_us(&timer_interrupt, 5); // 5usec |
nagasm | 0:5e2a4b964485 | 15 | while(1){ |
nagasm | 0:5e2a4b964485 | 16 | tx_fifo_check(); |
nagasm | 0:5e2a4b964485 | 17 | if(timer_value[1] > 19){ // 0.1msec sampling |
nagasm | 0:5e2a4b964485 | 18 | timer_value[1] = 0; |
nagasm | 0:5e2a4b964485 | 19 | data1 = (float)gain * (analog_value2.read() - 0.5f); // A/D in (3) |
nagasm | 0:5e2a4b964485 | 20 | data2 = (float)gain * (analog_value3.read() - 0.5f); // A/D in (4) |
nagasm | 0:5e2a4b964485 | 21 | if(data1 < 0) data1 = -data1; // always detection ON |
nagasm | 0:5e2a4b964485 | 22 | if(notch_1 != 0) data1 = notch_filter1(data1); |
nagasm | 0:5e2a4b964485 | 23 | if(max_count != 0) data1 = move_mean_calc1(data1); |
nagasm | 0:5e2a4b964485 | 24 | if(data2 < 0) data2 = -data2; // always detection ON |
nagasm | 0:5e2a4b964485 | 25 | if(notch_2 != 0) data2 = notch_filter2(data2); |
nagasm | 0:5e2a4b964485 | 26 | if(max_count != 0) data2 = move_mean_calc2(data2); |
nagasm | 0:5e2a4b964485 | 27 | } |
nagasm | 0:5e2a4b964485 | 28 | if(timer_value[2] > 199){ // 1msec sampling |
nagasm | 0:5e2a4b964485 | 29 | timer_value[2] = 0; |
nagasm | 0:5e2a4b964485 | 30 | in1[fft_counter1] = data1; |
nagasm | 0:5e2a4b964485 | 31 | if(++fft_counter1 > 15){ |
nagasm | 0:5e2a4b964485 | 32 | fft_counter1 = 0; |
nagasm | 0:5e2a4b964485 | 33 | fft1(in1, out1); |
nagasm | 0:5e2a4b964485 | 34 | int summ = 0; |
nagasm | 0:5e2a4b964485 | 35 | for(int i=0; i<7; i++) { summ = summ + (uint16_t)((out1[2*i]+out1[2*i+1]) * 7.9) << ((6-i)*3); } |
nagasm | 0:5e2a4b964485 | 36 | tx_message(0x000000 + summ); |
nagasm | 0:5e2a4b964485 | 37 | } |
nagasm | 0:5e2a4b964485 | 38 | } |
nagasm | 0:5e2a4b964485 | 39 | if(timer_value[3] > 199){ // 1msec sampling |
nagasm | 0:5e2a4b964485 | 40 | timer_value[3] = 0; |
nagasm | 0:5e2a4b964485 | 41 | in2[fft_counter2] = data2; |
nagasm | 0:5e2a4b964485 | 42 | if(++fft_counter2 > 15){ |
nagasm | 0:5e2a4b964485 | 43 | fft_counter2 = 0; |
nagasm | 0:5e2a4b964485 | 44 | fft1(in2, out2); |
nagasm | 0:5e2a4b964485 | 45 | int summ = 0; |
nagasm | 0:5e2a4b964485 | 46 | for(int i=0; i<7; i++) { summ = summ + (uint16_t)((out2[2*i]+out2[2*i+1]) * 7.9) << ((6-i)*3); } |
nagasm | 0:5e2a4b964485 | 47 | tx_message(0x400000 + summ); |
nagasm | 0:5e2a4b964485 | 48 | } |
nagasm | 0:5e2a4b964485 | 49 | } |
nagasm | 0:5e2a4b964485 | 50 | if(timer_value[0] > 199999){ // 1000msec |
nagasm | 0:5e2a4b964485 | 51 | timer_value[0] = 0; |
nagasm | 0:5e2a4b964485 | 52 | myled = !myled; |
nagasm | 0:5e2a4b964485 | 53 | } |
nagasm | 0:5e2a4b964485 | 54 | if(rx_fifo_check() == 1){ |
nagasm | 0:5e2a4b964485 | 55 | int sum = 0; |
nagasm | 0:5e2a4b964485 | 56 | for (int i=0; i<6; i++) sum += conv_hex(raw_data[i])<<(4*(5-i)); |
nagasm | 0:5e2a4b964485 | 57 | tx_message(sum); // Echo Back |
nagasm | 0:5e2a4b964485 | 58 | if(sum>>16 == 0x80){ |
nagasm | 0:5e2a4b964485 | 59 | switch((sum & 0xff00)>>8){ |
nagasm | 0:5e2a4b964485 | 60 | case 0x00: |
nagasm | 0:5e2a4b964485 | 61 | break; |
nagasm | 0:5e2a4b964485 | 62 | case 0x01: |
nagasm | 0:5e2a4b964485 | 63 | gain = sum & 0x0f; |
nagasm | 0:5e2a4b964485 | 64 | break; |
nagasm | 0:5e2a4b964485 | 65 | case 0x02: |
nagasm | 0:5e2a4b964485 | 66 | max_count = sum & 0x7f; |
nagasm | 0:5e2a4b964485 | 67 | if (max_count>100) max_count = 100; |
nagasm | 0:5e2a4b964485 | 68 | sum_clear(); |
nagasm | 0:5e2a4b964485 | 69 | break; |
nagasm | 0:5e2a4b964485 | 70 | case 0x03: |
nagasm | 0:5e2a4b964485 | 71 | notch_1 = sum & 0x01; |
nagasm | 0:5e2a4b964485 | 72 | break; |
nagasm | 0:5e2a4b964485 | 73 | case 0x04: |
nagasm | 0:5e2a4b964485 | 74 | notch_2 = sum & 0x01; |
nagasm | 0:5e2a4b964485 | 75 | break; |
nagasm | 0:5e2a4b964485 | 76 | case 0x05: |
nagasm | 0:5e2a4b964485 | 77 | coef_set(sum & 0x3f); |
nagasm | 0:5e2a4b964485 | 78 | break; |
nagasm | 0:5e2a4b964485 | 79 | case 0x06: |
nagasm | 0:5e2a4b964485 | 80 | break; |
nagasm | 0:5e2a4b964485 | 81 | } |
nagasm | 0:5e2a4b964485 | 82 | } |
nagasm | 0:5e2a4b964485 | 83 | } |
nagasm | 0:5e2a4b964485 | 84 | } |
nagasm | 0:5e2a4b964485 | 85 | } |