yusuke takahashi / Mbed 2 deprecated walkROBO_3

aa

Dependencies:   HMC6352 QEI Servo mbed

Fork of walkROBO by Ryo Ogata

ping/ping.cpp@2:955cdadf5ecc, 2013-09-05 (annotated)

Committer:
yusuke_robocup
Date:
Thu Sep 05 09:57:22 2013 +0000
Revision:
2:955cdadf5ecc
Parent:
0:4644bf6bca6a 
aa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
OGA 0:4644bf6bca6a 1 #include "mbed.h"
OGA 0:4644bf6bca6a 2 #include "ping.h"
OGA 0:4644bf6bca6a 3
OGA 0:4644bf6bca6a 4 //DigitalOut myled = LED1;
OGA 0:4644bf6bca6a 5
OGA 0:4644bf6bca6a 6 extern Timer timer2;
OGA 0:4644bf6bca6a 7
OGA 0:4644bf6bca6a 8 uint16_t ultrasonicVal[ALL_ULTRASONIC];
OGA 0:4644bf6bca6a 9 double ultrasonicValue[ALL_ULTRASONIC] = {0};
OGA 0:4644bf6bca6a 10
OGA 0:4644bf6bca6a 11
OGA 0:4644bf6bca6a 12 void Ultrasonic()
OGA 0:4644bf6bca6a 13 {
OGA 0:4644bf6bca6a 14 for(int i = 0 ; i < ALL_ULTRASONIC; i++){
OGA 0:4644bf6bca6a 15
OGA 0:4644bf6bca6a 16 uint8_t flag = 0;
OGA 0:4644bf6bca6a 17
OGA 0:4644bf6bca6a 18 DigitalOut PingPinOut(ultrasonic_pin[i]);
OGA 0:4644bf6bca6a 19 PingPinOut = 1;
OGA 0:4644bf6bca6a 20 wait_us(10);
OGA 0:4644bf6bca6a 21 PingPinOut = 0;
OGA 0:4644bf6bca6a 22 DigitalIn PingPin(ultrasonic_pin[i]);
OGA 0:4644bf6bca6a 23 timer2.reset();
OGA 0:4644bf6bca6a 24 while(PingPin == 0){
OGA 0:4644bf6bca6a 25 if(timer2.read_us() > 1500){ //1.5ms以上応答なし
OGA 0:4644bf6bca6a 26 ultrasonicValue[i] = PING_ERR;
OGA 0:4644bf6bca6a 27 flag = 1;
OGA 0:4644bf6bca6a 28 break;
OGA 0:4644bf6bca6a 29 }
OGA 0:4644bf6bca6a 30 }
OGA 0:4644bf6bca6a 31
OGA 0:4644bf6bca6a 32 timer2.reset();
OGA 0:4644bf6bca6a 33 while(PingPin == 1){
OGA 0:4644bf6bca6a 34 if((timer2.read_us() > 18500) || (flag == 1)){ //18.5ms以上のパルス
OGA 0:4644bf6bca6a 35 ultrasonicValue[i] = PING_ERR;
OGA 0:4644bf6bca6a 36 flag = 1;
OGA 0:4644bf6bca6a 37 break;
OGA 0:4644bf6bca6a 38 }
OGA 0:4644bf6bca6a 39 }
OGA 0:4644bf6bca6a 40
OGA 0:4644bf6bca6a 41 if(flag == 0){
OGA 0:4644bf6bca6a 42 ultrasonicValue[i] = timer2.read_us() / 1000000.0 / 2.0 * 340.0 * 1000.0; //mm MAX:3145
OGA 0:4644bf6bca6a 43 ultrasonicVal[i] = (int)(ultrasonicValue[i] * 10.0);
OGA 0:4644bf6bca6a 44 }else{
OGA 0:4644bf6bca6a 45 ultrasonicVal[i] = PING_ERR;
OGA 0:4644bf6bca6a 46 }
OGA 0:4644bf6bca6a 47
OGA 0:4644bf6bca6a 48 }
OGA 0:4644bf6bca6a 49 }