ooooooooga
Dependencies: ColorSensor1 HMC6352 PID QEI Servo TextLCD mbed
ping/ping.cpp
- Committer:
- OGA
- Date:
- 2013-10-01
- Revision:
- 0:e6bb0bcba274
File content as of revision 0:e6bb0bcba274:
#include "mbed.h" #include "ping.h" //DigitalOut myled = LED1; extern Timer timer2; uint16_t ultrasonicVal[ALL_ULTRASONIC]; double ultrasonicValue[ALL_ULTRASONIC] = {0}; void Ultrasonic() { for(int i = 0 ; i < ALL_ULTRASONIC; i++){ uint8_t flag = 0; DigitalOut PingPinOut(ultrasonic_pin[i]); PingPinOut = 1; wait_us(10); PingPinOut = 0; DigitalIn PingPin(ultrasonic_pin[i]); timer2.reset(); while(PingPin == 0){ if(timer2.read_us() > 1500){ //1.5ms以上応答なし ultrasonicValue[i] = PING_ERR; flag = 1; break; } } timer2.reset(); while(PingPin == 1){ if((timer2.read_us() > 18500) || (flag == 1)){ //18.5ms以上のパルス ultrasonicValue[i] = PING_ERR; flag = 1; break; } } if(flag == 0){ ultrasonicValue[i] = timer2.read_us() / 1000000.0 / 2.0 * 340.0 * 1000.0; //mm MAX:3145 ultrasonicVal[i] = (int)(ultrasonicValue[i] * 10.0); }else{ ultrasonicVal[i] = PING_ERR; } } }