yusuke takahashi / Mbed 2 deprecated PIDsync_0_2_2

aaaaa

Dependencies:   HMC6352 PID QEI Servo mbed

ping/ping.cpp@0:1be472d79ae9, 2013-09-30 (annotated)

Committer:
yusuke_robocup
Date:
Mon Sep 30 09:01:37 2013 +0000
Revision:
0:1be472d79ae9
PIDsync

Who changed what in which revision?

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