ultrasonic.cpp

00001 
00002 #include "mbed.h"
00003 #include "ultrasonic.h"
00004  
00005 extern Timer timer2;
00006 
00007 uint16_t ultrasonicVal[4];
00008 double ultrasonicValue[ALL_ULTRASONIC] = {0};
00009 
00010 
00011 void Ultrasonic()
00012 {
00013     for(uint8_t i = 0 ; i < ALL_ULTRASONIC ; i++){
00014         uint8_t flag = 0;
00015     
00016         DigitalOut PingPinOut(ultrasonic_pin[i]);
00017         PingPinOut = 1;
00018         wait_us(10);
00019         PingPinOut = 0;
00020         DigitalIn PingPin(ultrasonic_pin[i]);
00021         timer2.reset();
00022         while(PingPin == 0){
00023             if(timer2.read_us() > 1500){   //1.5ms以上応答なし
00024                 ultrasonicValue[i] =  PING_ERR;
00025                 flag = 1;
00026                 break;
00027             }
00028         } 
00029         timer2.reset();
00030         while(PingPin == 1){
00031             if((timer2.read_us() > 18500) || (flag == 1)){  //18.5ms以上のパルス
00032                 ultrasonicValue[i] =  PING_ERR;
00033                 flag = 1;
00034                 break;
00035             }
00036         }
00037         if(flag == 0){
00038             ultrasonicValue[i] = timer2.read_us() / 1000000.0 / 2.0 * 340.0 * 1000.0; //mm  MAX:3145
00039             ultrasonicVal[i] = (int)(ultrasonicValue[i] * 10.0);
00040         }else{
00041             ultrasonicVal[i] = PING_ERR;
00042         }
00043     }
00044 }