yusuke takahashi / Mbed 2 deprecated ver1_2_2_1

sasasa

Dependencies:   HMC6352 PID eeprom mbed

Fork of ver1_2_2_1 by ryo seki

ultrasonic.cpp@3:b4fb2b5365a7, 2013-04-18 (annotated)

Committer:
yusuke_robocup
Date:
Thu Apr 18 08:42:17 2013 +0000
Revision:
3:b4fb2b5365a7
Parent:
2:09fabba6c00d 
new new new

Who changed what in which revision?

UserRevisionLine numberNew contents of line
akudohune 0:74bf4953c0d1 1
akudohune 0:74bf4953c0d1 2 #include "mbed.h"
akudohune 0:74bf4953c0d1 3 #include "ultrasonic.h"
akudohune 0:74bf4953c0d1 4
akudohune 0:74bf4953c0d1 5
akudohune 0:74bf4953c0d1 6 extern Timer timer2;
yusuke_robocup 2:09fabba6c00d 7 extern Serial pc; // tx, rx
yusuke_robocup 2:09fabba6c00d 8 extern float now_compass;
akudohune 0:74bf4953c0d1 9
akudohune 1:89408fff7cc9 10 double ultrasonicVal[ALL_ULTRASONIC] = {0};
akudohune 0:74bf4953c0d1 11
akudohune 0:74bf4953c0d1 12
akudohune 0:74bf4953c0d1 13 void Ultrasonic()
akudohune 0:74bf4953c0d1 14 {
yusuke_robocup 2:09fabba6c00d 15
akudohune 0:74bf4953c0d1 16 for(uint8_t i = 0 ; i < ALL_ULTRASONIC ; i++){
akudohune 0:74bf4953c0d1 17 uint8_t flag = 0;
akudohune 0:74bf4953c0d1 18
akudohune 0:74bf4953c0d1 19 DigitalOut PingPinOut(ultrasonic_pin[i]);
akudohune 0:74bf4953c0d1 20 PingPinOut = 1;
akudohune 0:74bf4953c0d1 21 wait_us(10);
akudohune 0:74bf4953c0d1 22 PingPinOut = 0;
akudohune 0:74bf4953c0d1 23 DigitalIn PingPin(ultrasonic_pin[i]);
akudohune 0:74bf4953c0d1 24 timer2.reset();
akudohune 0:74bf4953c0d1 25 while(PingPin == 0){
akudohune 0:74bf4953c0d1 26 if(timer2.read_us() > 1000){ //1ms以上応答なし
akudohune 0:74bf4953c0d1 27 ultrasonicVal[i] = PING_ERR;
akudohune 0:74bf4953c0d1 28 flag = 1;
akudohune 0:74bf4953c0d1 29 break;
akudohune 0:74bf4953c0d1 30 }
akudohune 0:74bf4953c0d1 31 }
akudohune 0:74bf4953c0d1 32 timer2.reset();
akudohune 0:74bf4953c0d1 33 while(PingPin == 1){
yusuke_robocup 2:09fabba6c00d 34 if((timer2.read_us() > 5000) || (flag == 1)){ //18.5ms以上のパルス
akudohune 0:74bf4953c0d1 35 ultrasonicVal[i] = PING_ERR;
akudohune 0:74bf4953c0d1 36 flag = 1;
akudohune 0:74bf4953c0d1 37 break;
akudohune 0:74bf4953c0d1 38 }
akudohune 0:74bf4953c0d1 39 }
akudohune 0:74bf4953c0d1 40 if(flag == 0){
akudohune 1:89408fff7cc9 41 ultrasonicVal[i] = timer2.read_us() / 1000000.0 / 2.0 * 340.0 * 1000.0; //cm
akudohune 0:74bf4953c0d1 42 }
akudohune 0:74bf4953c0d1 43 }
yusuke_robocup 2:09fabba6c00d 44 //pc.printf("%f\n",ultrasonicVal[1]);
akudohune 1:89408fff7cc9 45 //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
yusuke_robocup 2:09fabba6c00d 46 //pc.printf("%f\n",);
yusuke_robocup 2:09fabba6c00d 47
akudohune 0:74bf4953c0d1 48 }