歩行ロボ
Dependencies: HMC6352 QEI Servo mbed
ping/ping.cpp@1:f465d89a26b0, 2013-08-01 (annotated)
- Committer:
- OGA
- Date:
- Thu Aug 01 09:05:23 2013 +0000
- Revision:
- 1:f465d89a26b0
- Parent:
- 0:4644bf6bca6a
????
Who changed what in which revision?
User | Revision | Line number | New 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 | } |