sub.hpp

00001 unsigned char rxFIFO[256], txFIFO[256], raw_data[6];
00002 unsigned char rx_top, rx_end, tx_top, tx_end, phase;
00003 float mean_sum1, mean_sum2, ad_data1[101], ad_data2[101];
00004 int timer_value[6], max_count, gain, ad_pointer1, ad_pointer2;
00005 float a1, a2, b1, c0, y1[3], y2[3];
00006 int notch_1, notch_2;
00007 float data1, data2, data3;
00008 const float _60Hz_a1 = 1.936768, _60Hz_a2 = -0.939101, _60Hz_b1 = -1.998579, _60Hz_c0 = 1.642174;
00009 const float _50Hz_a1 = 1.937188, _50Hz_a2 = -0.939101, _50Hz_b1 = -1.999013, _50Hz_c0 = 1.938304;
00010 
00011 RawSerial xbee(PA_2, PA_3);
00012 Ticker timer_setup;
00013 AnalogIn analog_value0(A0);
00014 AnalogIn analog_value1(A1);
00015 AnalogIn analog_value2(A2);
00016 AnalogIn analog_value3(A3);
00017 DigitalOut myled(LED1);
00018 
00019 void sum_clear(){
00020     for (int i=0; i<101; i++) ad_data1[i] = ad_data2[i] = 0;
00021     ad_pointer1 = ad_pointer2 = mean_sum1 = mean_sum2 = 0;
00022 }
00023 
00024 void coef_set(int herz){
00025     if(herz < 55){
00026         a1 = _50Hz_a1;
00027         a2 = _50Hz_a2;
00028         b1 = _50Hz_b1;
00029         c0 = _50Hz_c0;
00030     }
00031     else{
00032         a1 = _60Hz_a1;
00033         a2 = _60Hz_a2;
00034         b1 = _60Hz_b1;
00035         c0 = _60Hz_c0;
00036     }
00037     for (int i=0; i<3; i++) y1[i] = y2[i] = 0;
00038 }
00039 void common_setup(){
00040     rx_top = rx_end = tx_top = tx_end = phase = 0;
00041     max_count = 100;
00042     gain = 7;
00043     sum_clear();
00044     notch_1 = notch_2 = 1;
00045     coef_set(60);
00046     for(int i=0; i<6; i++) timer_value[i] = 0;
00047     timer_value[3] = 100;
00048 }
00049 
00050 void timer_interrupt(){
00051     for(int i=0; i<6; i++) timer_value[i]++;
00052 }
00053 
00054 void tx_fifo_check(){
00055     if(xbee.writeable() == 1){
00056         if(tx_top != tx_end){
00057             xbee.putc(txFIFO[tx_end]);
00058             ++tx_end &= 255;
00059         }
00060     }
00061 }
00062 
00063 int rx_fifo_check(){
00064     if(rx_top != rx_end){
00065         unsigned char data = rxFIFO[rx_end];
00066         ++rx_end &= 255;
00067         if (data < 33){
00068             phase = 0;
00069             return(1);
00070         }
00071         raw_data[phase] = data;
00072         if(++phase > 5) phase = 0;
00073         return(0);
00074     }
00075     return(0);
00076 }
00077 
00078 void rx_fifoset(void){
00079     rxFIFO[rx_top] = xbee.getc();
00080     ++rx_top &= 255;
00081 }
00082 
00083 void tx_fifoset(unsigned char data){
00084     txFIFO[tx_top] = data;
00085     ++tx_top &= 255;
00086 }
00087 
00088 unsigned char hex_conv(unsigned char data){
00089     data &= 15;
00090     if(data < 10) return(data+48);
00091     else return(data+55);
00092 }
00093 
00094 unsigned char conv_hex(unsigned char data){
00095     if((data > 47) && (data < 58)) return(data-48);
00096     else if((data > 64) && (data < 71)) return(data-55);
00097     return(0);
00098 }
00099 
00100 void tx_message(int data){
00101     for (int i=0; i<6; i++) tx_fifoset(hex_conv((data>>(4*(5-i))) & 15));
00102     tx_fifoset(13);
00103 }
00104 
00105 float move_mean_calc1(float data){
00106     mean_sum1 = mean_sum1 - ad_data1[ad_pointer1] + data;
00107     ad_data1[ad_pointer1] = data;
00108     ad_pointer1++;
00109     if(ad_pointer1 == max_count) ad_pointer1 = 0;
00110     return(mean_sum1 / (float)max_count);
00111 }
00112 
00113 float move_mean_calc2(float data){
00114     mean_sum2 = mean_sum2 - ad_data2[ad_pointer2] + data;
00115     ad_data2[ad_pointer2] = data;
00116     ad_pointer2++;
00117     if(ad_pointer2 == max_count) ad_pointer2 = 0;
00118     return(mean_sum2 / (float)max_count);
00119 }