main.cpp

00001 #include "mbed.h"
00002 #include "sub.hpp"
00003 #include "notch.hpp"
00004 
00005 int main(){
00006     common_setup();
00007     int Lissajous = 50;
00008     xbee.baud(38400);
00009     xbee.attach(&rx_fifoset, xbee.RxIrq);
00010     timer_setup.attach_us(&timer_interrupt, 5); // 5usec
00011     while(1){
00012         tx_fifo_check();
00013         if(timer_value[1] > 19){ // 0.1msec sampling
00014             timer_value[1] = 0;
00015             data1 = (float)gain * (analog_value2.read() - 0.5f);  // A/D in (3)
00016             if(data1 < 0) data1 = -data1; // always detection ON
00017             if(notch_1 != 0) data1 = notch_filter1(data1);
00018             data2 = (float)gain * (analog_value3.read() - 0.5f);  // A/D in (4)
00019             if(data2 < 0) data2 = -data2; // always detection ON
00020             if(notch_2 != 0) data2 = notch_filter2(data2);
00021             data3 = (float)Lissajous / 10.0 * (data1 - data2) * sqrt( data1*data1 + data2*data2 );
00022         }
00023         if(timer_value[2] > 999){  // 5msec
00024             timer_value[2] = 0;
00025             if(max_count != 0) data3 = move_mean_calc2(data3);
00026             tx_message(0x400000 + ((uint16_t)((data3 + 1.0f) * 2047)<<4));
00027         }
00028         if(timer_value[0] > 199999){ // 1000msec
00029             timer_value[0] = 0;
00030             myled = !myled;
00031         }
00032         if(rx_fifo_check() == 1){
00033             int sum = 0;
00034             for (int i=0; i<6; i++) sum += conv_hex(raw_data[i])<<(4*(5-i));
00035             tx_message(sum); // Echo Back 
00036             if(sum>>16 == 0x80){
00037                 switch((sum & 0xff00)>>8){
00038                     case 0x00:
00039 //                        fir_lpf = sum & 0x01;
00040                         Lissajous = sum & 0x7f;
00041                         break;
00042                     case 0x01:
00043                         gain = sum & 0x0f;
00044                         break;
00045                     case 0x02:
00046                         max_count = sum & 0x7f;
00047                         if (max_count>100) max_count = 100;
00048                         sum_clear();
00049                         break;
00050                     case 0x03:
00051                         notch_1 = sum & 0x01;
00052                         break;
00053                     case 0x04:
00054                         notch_2 = sum & 0x01;
00055                         break;
00056                     case 0x05:
00057                         coef_set(sum & 0x3f);
00058                         break;
00059                     case 0x06:
00060 //                        emergence = sum & 0x01;
00061                         break;
00062                 }
00063             }
00064         }        
00065     }
00066 }