Yoichi Nagashima
/
CQ_nagasm_10_Lissajous
NucleoF401RE EMG seneor with Lissajous Analysis
main.cpp@0:418c739a67df, 2014-12-19 (annotated)
- Committer:
- nagasm
- Date:
- Fri Dec 19 06:37:45 2014 +0000
- Revision:
- 0:418c739a67df
NucleoF401RE EMG seneor with Lissajous Analysis
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nagasm | 0:418c739a67df | 1 | #include "mbed.h" |
nagasm | 0:418c739a67df | 2 | #include "sub.hpp" |
nagasm | 0:418c739a67df | 3 | #include "notch.hpp" |
nagasm | 0:418c739a67df | 4 | |
nagasm | 0:418c739a67df | 5 | int main(){ |
nagasm | 0:418c739a67df | 6 | common_setup(); |
nagasm | 0:418c739a67df | 7 | int Lissajous = 50; |
nagasm | 0:418c739a67df | 8 | xbee.baud(38400); |
nagasm | 0:418c739a67df | 9 | xbee.attach(&rx_fifoset, xbee.RxIrq); |
nagasm | 0:418c739a67df | 10 | timer_setup.attach_us(&timer_interrupt, 5); // 5usec |
nagasm | 0:418c739a67df | 11 | while(1){ |
nagasm | 0:418c739a67df | 12 | tx_fifo_check(); |
nagasm | 0:418c739a67df | 13 | if(timer_value[1] > 19){ // 0.1msec sampling |
nagasm | 0:418c739a67df | 14 | timer_value[1] = 0; |
nagasm | 0:418c739a67df | 15 | data1 = (float)gain * (analog_value2.read() - 0.5f); // A/D in (3) |
nagasm | 0:418c739a67df | 16 | if(data1 < 0) data1 = -data1; // always detection ON |
nagasm | 0:418c739a67df | 17 | if(notch_1 != 0) data1 = notch_filter1(data1); |
nagasm | 0:418c739a67df | 18 | data2 = (float)gain * (analog_value3.read() - 0.5f); // A/D in (4) |
nagasm | 0:418c739a67df | 19 | if(data2 < 0) data2 = -data2; // always detection ON |
nagasm | 0:418c739a67df | 20 | if(notch_2 != 0) data2 = notch_filter2(data2); |
nagasm | 0:418c739a67df | 21 | data3 = (float)Lissajous / 10.0 * (data1 - data2) * sqrt( data1*data1 + data2*data2 ); |
nagasm | 0:418c739a67df | 22 | } |
nagasm | 0:418c739a67df | 23 | if(timer_value[2] > 999){ // 5msec |
nagasm | 0:418c739a67df | 24 | timer_value[2] = 0; |
nagasm | 0:418c739a67df | 25 | if(max_count != 0) data3 = move_mean_calc2(data3); |
nagasm | 0:418c739a67df | 26 | tx_message(0x400000 + ((uint16_t)((data3 + 1.0f) * 2047)<<4)); |
nagasm | 0:418c739a67df | 27 | } |
nagasm | 0:418c739a67df | 28 | if(timer_value[0] > 199999){ // 1000msec |
nagasm | 0:418c739a67df | 29 | timer_value[0] = 0; |
nagasm | 0:418c739a67df | 30 | myled = !myled; |
nagasm | 0:418c739a67df | 31 | } |
nagasm | 0:418c739a67df | 32 | if(rx_fifo_check() == 1){ |
nagasm | 0:418c739a67df | 33 | int sum = 0; |
nagasm | 0:418c739a67df | 34 | for (int i=0; i<6; i++) sum += conv_hex(raw_data[i])<<(4*(5-i)); |
nagasm | 0:418c739a67df | 35 | tx_message(sum); // Echo Back |
nagasm | 0:418c739a67df | 36 | if(sum>>16 == 0x80){ |
nagasm | 0:418c739a67df | 37 | switch((sum & 0xff00)>>8){ |
nagasm | 0:418c739a67df | 38 | case 0x00: |
nagasm | 0:418c739a67df | 39 | // fir_lpf = sum & 0x01; |
nagasm | 0:418c739a67df | 40 | Lissajous = sum & 0x7f; |
nagasm | 0:418c739a67df | 41 | break; |
nagasm | 0:418c739a67df | 42 | case 0x01: |
nagasm | 0:418c739a67df | 43 | gain = sum & 0x0f; |
nagasm | 0:418c739a67df | 44 | break; |
nagasm | 0:418c739a67df | 45 | case 0x02: |
nagasm | 0:418c739a67df | 46 | max_count = sum & 0x7f; |
nagasm | 0:418c739a67df | 47 | if (max_count>100) max_count = 100; |
nagasm | 0:418c739a67df | 48 | sum_clear(); |
nagasm | 0:418c739a67df | 49 | break; |
nagasm | 0:418c739a67df | 50 | case 0x03: |
nagasm | 0:418c739a67df | 51 | notch_1 = sum & 0x01; |
nagasm | 0:418c739a67df | 52 | break; |
nagasm | 0:418c739a67df | 53 | case 0x04: |
nagasm | 0:418c739a67df | 54 | notch_2 = sum & 0x01; |
nagasm | 0:418c739a67df | 55 | break; |
nagasm | 0:418c739a67df | 56 | case 0x05: |
nagasm | 0:418c739a67df | 57 | coef_set(sum & 0x3f); |
nagasm | 0:418c739a67df | 58 | break; |
nagasm | 0:418c739a67df | 59 | case 0x06: |
nagasm | 0:418c739a67df | 60 | // emergence = sum & 0x01; |
nagasm | 0:418c739a67df | 61 | break; |
nagasm | 0:418c739a67df | 62 | } |
nagasm | 0:418c739a67df | 63 | } |
nagasm | 0:418c739a67df | 64 | } |
nagasm | 0:418c739a67df | 65 | } |
nagasm | 0:418c739a67df | 66 | } |