sasasa
Dependencies: HMC6352 PID eeprom mbed
Fork of ver1_2_2_1 by
ultrasonic.cpp
- Committer:
- yusuke_robocup
- Date:
- 2013-04-18
- Revision:
- 3:b4fb2b5365a7
- Parent:
- 2:09fabba6c00d
File content as of revision 3:b4fb2b5365a7:
#include "mbed.h" #include "ultrasonic.h" extern Timer timer2; extern Serial pc; // tx, rx extern float now_compass; double ultrasonicVal[ALL_ULTRASONIC] = {0}; void Ultrasonic() { for(uint8_t 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() > 1000){ //1ms以上応答なし ultrasonicVal[i] = PING_ERR; flag = 1; break; } } timer2.reset(); while(PingPin == 1){ if((timer2.read_us() > 5000) || (flag == 1)){ //18.5ms以上のパルス ultrasonicVal[i] = PING_ERR; flag = 1; break; } } if(flag == 0){ ultrasonicVal[i] = timer2.read_us() / 1000000.0 / 2.0 * 340.0 * 1000.0; //cm } } //pc.printf("%f\n",ultrasonicVal[1]); //pc.printf("compass.sample = %f\n",compass.sample() / 1.0); //pc.printf("%f\n",); }