sasasa
Dependencies: HMC6352 PID eeprom mbed
Fork of ver1_2_2_1 by
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?
User | Revision | Line number | New 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 | } |