main.cpp

00001 #include "mbed.h"
00002 #include "TextLCD.h"
00003 #include "main.h"
00004 
00005 int ave5point(int source)
00006 {
00007     static int tmp[5] = {0};
00008     static int sum = 0;
00009     
00010     sum -= tmp[4];
00011     sum += source;
00012     tmp[4] = tmp[3];
00013     tmp[3] = tmp[2];
00014     tmp[2] = tmp[1];
00015     tmp[1] = tmp[0];
00016     tmp[0] = source;
00017     
00018     for(uint8_t i = 0; i < 5; i++){
00019         if((tmp[i] > 180) || (tmp[i] < 10)){
00020             return 0xFF;
00021         }
00022     }
00023     
00024     return (int)(sum / 5.0);
00025 
00026 }
00027 
00028 void init()
00029 {
00030     device.baud(BAUD_RATE);
00031     device.printf("START");
00032     device2.printf("START"); 
00033     device.attach(&dev_tx,Serial::TxIrq);
00034     device.attach(&dev_rx,Serial::RxIrq);
00035     device2.attach(&dev2_tx,Serial::TxIrq);
00036     device2.attach(&dev2_rx,Serial::RxIrq);
00037     Button.mode(PullUp);
00038     
00039     timer2.start();
00040     
00041     lcd.cls();
00042 }
00043 
00044 int main() 
00045 {
00046     uint8_t state = 0;
00047 
00048     init();
00049     
00050     for(;;){
00051         if((!Button) && (state == 0)){
00052             wait(0.3);
00053             state = 1;
00054         }else if((!Button) && (state == 1)){
00055             wait(0.3);
00056             state = 2;
00057         }else if((!Button) &&(state == 2)){
00058             wait(0.3);
00059             state = 0;
00060         }
00061         
00062         mbedleds = 0;
00063         
00064         Ultrasonic();
00065         IR_Position();
00066         AveDistance = ave5point(Distance);
00067         
00068         lcd.cls();
00069         
00070         if(state == 0){
00071             lcd.locate(0,0);
00072             lcd.printf("R:%d,F:%d",(int)ultrasonicValue[1],(int)ultrasonicValue[0]);
00073             lcd.locate(0,1);
00074             lcd.printf("L:%d,B:%d",(int)ultrasonicValue[3],(int)ultrasonicValue[2]);
00075         }else if(state == 1){
00076             lcd.locate(0,0);
00077             lcd.printf("%d,%d,%d",direction,data[2],(data[0] << 8) + data[1]);//data[2]
00078             lcd.locate(0,1);
00079             lcd.printf("%d,%d",AveDistance,data2[0]);
00080         }else if(state == 2){
00081             lcd.locate(0,0);
00082             lcd.printf("%d,%d,%d",irDistance[0],irDistance[1],irDistance[2]);//data[2]
00083             lcd.locate(0,1);
00084             lcd.printf("%d,%d,%d",irDistance[3],irDistance[4],irDistance[5]);
00085         }
00086         
00087         //pc.printf("%d\n",direction);
00088         /*
00089         pc.printf("R:%d,\tF:%d\t",(int)ultrasonicValue[1],(int)ultrasonicValue[0]);
00090         pc.printf("L:%d,\tB:%d\n",(int)ultrasonicValue[3],(int)ultrasonicValue[2]);
00091         */
00092         /*
00093         lcd.locate(0,0);
00094         lcd.printf("%d",(data[0] << 8) + data[1]);
00095         lcd.locate(0,1);
00096         lcd.printf("%d",data[2]);
00097         */
00098         /*
00099         lcd.locate(0,0);
00100         lcd.printf("%d,%d",direction,data[3]);//data[2]
00101         lcd.locate(0,1);
00102         lcd.printf("%d",AveDistance);
00103         */
00104         /*
00105         lcd.locate(0,0);
00106         lcd.printf("%d",data[0]);
00107         lcd.locate(0,1);
00108         lcd.printf("%d",data[1]);*/
00109     }
00110 }