ultrasonic.cpp

00001 
00002 #include "mbed.h"
00003 #include "ultrasonic.h"
00004 
00005 
00006 extern Timer timer2;
00007 extern Serial pc; // tx, rx
00008 extern float now_compass; 
00009 
00010 double ultrasonicVal[ALL_ULTRASONIC] = {0};
00011 
00012 
00013 void Ultrasonic()
00014 {
00015     
00016     for(uint8_t i = 0 ; i < ALL_ULTRASONIC ; i++){
00017         uint8_t flag = 0;
00018     
00019         DigitalOut PingPinOut(ultrasonic_pin[i]);
00020         PingPinOut = 1;
00021         wait_us(10);
00022         PingPinOut = 0;
00023         DigitalIn PingPin(ultrasonic_pin[i]);
00024         timer2.reset();
00025         while(PingPin == 0){
00026             if(timer2.read_us() > 1000){   //1ms以上応答なし
00027                 ultrasonicVal[i] =  PING_ERR;
00028                 flag = 1;
00029                 break;
00030             }
00031         } 
00032         timer2.reset();
00033         while(PingPin == 1){
00034             if((timer2.read_us() > 5000) || (flag == 1)){  //18.5ms以上のパルス
00035                 ultrasonicVal[i] =  PING_ERR;
00036                 flag = 1;
00037                 break;
00038             }
00039         }
00040         if(flag == 0){
00041             ultrasonicVal[i] = timer2.read_us() / 1000000.0 / 2.0 * 340.0 * 1000.0; //cm
00042         }
00043     }
00044     //pc.printf("%f\n",ultrasonicVal[1]);
00045     //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
00046     //pc.printf("%f\n",);
00047 
00048 }