Yoichi Nagashima
/
CQ_nagasm_10_Lissajous
NucleoF401RE EMG seneor with Lissajous Analysis
sub.hpp@1:42b57bfcc750, 2014-12-20 (annotated)
- Committer:
- nagasm
- Date:
- Sat Dec 20 02:09:52 2014 +0000
- Revision:
- 1:42b57bfcc750
- Parent:
- 0:418c739a67df
revised
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nagasm | 0:418c739a67df | 1 | unsigned char rxFIFO[256], txFIFO[256], raw_data[6]; |
nagasm | 0:418c739a67df | 2 | unsigned char rx_top, rx_end, tx_top, tx_end, phase; |
nagasm | 0:418c739a67df | 3 | float mean_sum1, mean_sum2, ad_data1[101], ad_data2[101]; |
nagasm | 0:418c739a67df | 4 | int timer_value[6], max_count, gain, ad_pointer1, ad_pointer2; |
nagasm | 0:418c739a67df | 5 | float a1, a2, b1, c0, y1[3], y2[3]; |
nagasm | 0:418c739a67df | 6 | int notch_1, notch_2; |
nagasm | 0:418c739a67df | 7 | float data1, data2, data3; |
nagasm | 0:418c739a67df | 8 | const float _60Hz_a1 = 1.936768, _60Hz_a2 = -0.939101, _60Hz_b1 = -1.998579, _60Hz_c0 = 1.642174; |
nagasm | 0:418c739a67df | 9 | const float _50Hz_a1 = 1.937188, _50Hz_a2 = -0.939101, _50Hz_b1 = -1.999013, _50Hz_c0 = 1.938304; |
nagasm | 0:418c739a67df | 10 | |
nagasm | 0:418c739a67df | 11 | RawSerial xbee(PA_2, PA_3); |
nagasm | 0:418c739a67df | 12 | Ticker timer_setup; |
nagasm | 0:418c739a67df | 13 | AnalogIn analog_value0(A0); |
nagasm | 0:418c739a67df | 14 | AnalogIn analog_value1(A1); |
nagasm | 0:418c739a67df | 15 | AnalogIn analog_value2(A2); |
nagasm | 0:418c739a67df | 16 | AnalogIn analog_value3(A3); |
nagasm | 0:418c739a67df | 17 | DigitalOut myled(LED1); |
nagasm | 0:418c739a67df | 18 | |
nagasm | 0:418c739a67df | 19 | void sum_clear(){ |
nagasm | 0:418c739a67df | 20 | int i; |
nagasm | 0:418c739a67df | 21 | for (i=0; i<101; i++) ad_data1[i] = ad_data2[i] = 0; |
nagasm | 0:418c739a67df | 22 | ad_pointer1 = ad_pointer2 = mean_sum1 = mean_sum2 = 0; |
nagasm | 0:418c739a67df | 23 | } |
nagasm | 0:418c739a67df | 24 | |
nagasm | 0:418c739a67df | 25 | void coef_set(int herz){ |
nagasm | 0:418c739a67df | 26 | if(herz < 55){ |
nagasm | 0:418c739a67df | 27 | a1 = _50Hz_a1; |
nagasm | 0:418c739a67df | 28 | a2 = _50Hz_a2; |
nagasm | 0:418c739a67df | 29 | b1 = _50Hz_b1; |
nagasm | 0:418c739a67df | 30 | c0 = _50Hz_c0; |
nagasm | 0:418c739a67df | 31 | } |
nagasm | 0:418c739a67df | 32 | else{ |
nagasm | 0:418c739a67df | 33 | a1 = _60Hz_a1; |
nagasm | 0:418c739a67df | 34 | a2 = _60Hz_a2; |
nagasm | 0:418c739a67df | 35 | b1 = _60Hz_b1; |
nagasm | 0:418c739a67df | 36 | c0 = _60Hz_c0; |
nagasm | 0:418c739a67df | 37 | } |
nagasm | 0:418c739a67df | 38 | for (int i=0; i<3; i++) y1[i] = y2[i] = 0; |
nagasm | 0:418c739a67df | 39 | } |
nagasm | 0:418c739a67df | 40 | void common_setup(){ |
nagasm | 0:418c739a67df | 41 | rx_top = rx_end = tx_top = tx_end = phase = 0; |
nagasm | 0:418c739a67df | 42 | max_count = 100; |
nagasm | 0:418c739a67df | 43 | gain = 5; |
nagasm | 0:418c739a67df | 44 | sum_clear(); |
nagasm | 1:42b57bfcc750 | 45 | notch_1 = notch_2 = 1; |
nagasm | 0:418c739a67df | 46 | coef_set(60); |
nagasm | 0:418c739a67df | 47 | } |
nagasm | 0:418c739a67df | 48 | |
nagasm | 0:418c739a67df | 49 | void timer_interrupt(){ |
nagasm | 0:418c739a67df | 50 | int i; |
nagasm | 0:418c739a67df | 51 | for (i=0; i<6; i++) timer_value[i]++; |
nagasm | 0:418c739a67df | 52 | } |
nagasm | 0:418c739a67df | 53 | |
nagasm | 0:418c739a67df | 54 | void tx_fifo_check(){ |
nagasm | 0:418c739a67df | 55 | if(xbee.writeable() == 1){ |
nagasm | 0:418c739a67df | 56 | if(tx_top != tx_end){ |
nagasm | 0:418c739a67df | 57 | xbee.putc(txFIFO[tx_end]); |
nagasm | 0:418c739a67df | 58 | ++tx_end &= 255; |
nagasm | 0:418c739a67df | 59 | } |
nagasm | 0:418c739a67df | 60 | } |
nagasm | 0:418c739a67df | 61 | } |
nagasm | 0:418c739a67df | 62 | |
nagasm | 0:418c739a67df | 63 | int rx_fifo_check(){ |
nagasm | 0:418c739a67df | 64 | unsigned char data; |
nagasm | 0:418c739a67df | 65 | if(rx_top != rx_end){ |
nagasm | 0:418c739a67df | 66 | data = rxFIFO[rx_end]; |
nagasm | 0:418c739a67df | 67 | ++rx_end &= 255; |
nagasm | 0:418c739a67df | 68 | if (data < 33){ |
nagasm | 0:418c739a67df | 69 | phase = 0; |
nagasm | 0:418c739a67df | 70 | return(1); |
nagasm | 0:418c739a67df | 71 | } |
nagasm | 0:418c739a67df | 72 | raw_data[phase] = data; |
nagasm | 0:418c739a67df | 73 | if(++phase > 5) phase = 0; |
nagasm | 0:418c739a67df | 74 | return(0); |
nagasm | 0:418c739a67df | 75 | } |
nagasm | 0:418c739a67df | 76 | return(0); |
nagasm | 0:418c739a67df | 77 | } |
nagasm | 0:418c739a67df | 78 | |
nagasm | 0:418c739a67df | 79 | void rx_fifoset(void){ |
nagasm | 0:418c739a67df | 80 | rxFIFO[rx_top] = xbee.getc(); |
nagasm | 0:418c739a67df | 81 | ++rx_top &= 255; |
nagasm | 0:418c739a67df | 82 | } |
nagasm | 0:418c739a67df | 83 | |
nagasm | 0:418c739a67df | 84 | void tx_fifoset(unsigned char data){ |
nagasm | 0:418c739a67df | 85 | txFIFO[tx_top] = data; |
nagasm | 0:418c739a67df | 86 | ++tx_top &= 255; |
nagasm | 0:418c739a67df | 87 | } |
nagasm | 0:418c739a67df | 88 | |
nagasm | 0:418c739a67df | 89 | unsigned char hex_conv(unsigned char data){ |
nagasm | 0:418c739a67df | 90 | data &= 15; |
nagasm | 0:418c739a67df | 91 | if(data < 10) return(data+48); |
nagasm | 0:418c739a67df | 92 | else return(data+55); |
nagasm | 0:418c739a67df | 93 | } |
nagasm | 0:418c739a67df | 94 | |
nagasm | 0:418c739a67df | 95 | unsigned char conv_hex(unsigned char data){ |
nagasm | 0:418c739a67df | 96 | if((data > 47) && (data < 58)) return(data-48); |
nagasm | 0:418c739a67df | 97 | else if((data > 64) && (data < 71)) return(data-55); |
nagasm | 0:418c739a67df | 98 | return(0); |
nagasm | 0:418c739a67df | 99 | } |
nagasm | 0:418c739a67df | 100 | |
nagasm | 0:418c739a67df | 101 | void tx_message(int data){ |
nagasm | 0:418c739a67df | 102 | int i; |
nagasm | 0:418c739a67df | 103 | for (i=0; i<6; i++) tx_fifoset(hex_conv((data>>(4*(5-i))) & 15)); |
nagasm | 0:418c739a67df | 104 | tx_fifoset(13); |
nagasm | 0:418c739a67df | 105 | } |
nagasm | 0:418c739a67df | 106 | |
nagasm | 0:418c739a67df | 107 | float move_mean_calc1(float data){ |
nagasm | 0:418c739a67df | 108 | mean_sum1 = mean_sum1 - ad_data1[ad_pointer1] + data; |
nagasm | 0:418c739a67df | 109 | ad_data1[ad_pointer1] = data; |
nagasm | 0:418c739a67df | 110 | ad_pointer1++; |
nagasm | 0:418c739a67df | 111 | if(ad_pointer1 == max_count) ad_pointer1 = 0; |
nagasm | 0:418c739a67df | 112 | return(mean_sum1 / (float)max_count); |
nagasm | 0:418c739a67df | 113 | } |
nagasm | 0:418c739a67df | 114 | |
nagasm | 0:418c739a67df | 115 | float move_mean_calc2(float data){ |
nagasm | 0:418c739a67df | 116 | mean_sum2 = mean_sum2 - ad_data2[ad_pointer2] + data; |
nagasm | 0:418c739a67df | 117 | ad_data2[ad_pointer2] = data; |
nagasm | 0:418c739a67df | 118 | ad_pointer2++; |
nagasm | 0:418c739a67df | 119 | if(ad_pointer2 == max_count) ad_pointer2 = 0; |
nagasm | 0:418c739a67df | 120 | return(mean_sum2 / (float)max_count); |
nagasm | 0:418c739a67df | 121 | } |