ping.cpp

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