robocup

Dependencies:   HMC6352 PID mbed

ultrasonic.cpp

Committer:
akudohune
Date:
2013-03-08
Revision:
0:13ab960fc61f

File content as of revision 0:13ab960fc61f:


#include "mbed.h"
#include "ultrasonic.h"


extern Timer timer2;
extern Serial pc; // tx, rx 

uint16_t ultrasonicVal[ALL_ULTRASONIC] = {0};


void Ultrasonic()
{
    for(uint8_t i = 0 ; i < ALL_ULTRASONIC ; i++){
        uint8_t flag = 0;
    
        DigitalOut PingPinOut(ultrasonic_pin[i]);
        PingPinOut = 1;
        wait_us(10);
        PingPinOut = 0;
        DigitalIn PingPin(ultrasonic_pin[i]);
        timer2.reset();
        while(PingPin == 0){
            if(timer2.read_us() > 1000){   //1ms以上応答なし
                ultrasonicVal[i] =  PING_ERR;
                flag = 1;
                break;
            }
        } 
        timer2.reset();
        while(PingPin == 1){
            if((timer2.read_us() > 18500) || (flag == 1)){  //18.5ms以上のパルス
                ultrasonicVal[i] =  PING_ERR;
                flag = 1;
                break;
            }
        }
        if(flag == 0){
            ultrasonicVal[i] = timer2.read_us();
        }
        //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
    }
    //pc.printf("%d\n",ultrasonicVal[2]);
    
}