File content as of revision 0:ac150fd4158e:

#ifndef SONAR_SENSOR_SONAR_H
#define SONAR_SENSOR_SONAR_H

#include "mbed.h"
#include "hcsr04.h"
//#include "Print.h"
#include <string>

Serial pc(USBTX, USBRX); // tx, rx


//trigger is bigger number and echo is smaller
HCSR04  frontCenter(p26, p9);
HCSR04  frontRight(p26, p8);
HCSR04  frontLeft(p26, p7);
HCSR04  rear(p26, p6);

const unsigned int waitDuration = 55; //ms
const unsigned int MaxDistance = 40;

unsigned int sensorDistance[4] = {9999, 9999 ,9999, 9999};

void getDistance(){
    for(int i = 0; i<4; ++i)
        sensorDistance[i] = 9999;

    frontRight.start();

    wait_ms(waitDuration);

    sensorDistance[0] = frontCenter.get_dist_cm();
    sensorDistance[1] = frontRight.get_dist_cm();
    sensorDistance[2] = frontLeft.get_dist_cm();
    sensorDistance[3] = rear.get_dist_cm();
}

void printDistance(){
    const string SENSOR[] = {"CENTER", "RIGHT", "LEFT", "REAR"};
    
    for(int i = 0; i<4; ++i)
        pc.printf("%s: %dcm\r\n", SENSOR[i] ,sensorDistance[i]);
    
    pc.printf("\n\r");
}

void printDistance2(int maxSensor){
    const string SENSOR[] = {"CENTER", "RIGHT", "LEFT", "REAR"};
    if(maxSensor>3)
        maxSensor = 3;
        
    for(int i = 0; i<maxSensor; ++i)
        pc.printf("%s: %dcm\r\n", SENSOR[i] ,sensorDistance[i]);
    pc.printf("\n\r");    
}

//return true if obstacle is to close and car needs to dodge
bool avoidObstacle(const unsigned int &distance){
    return distance<MaxDistance;
}

#endif //SONAR_SENSOR_SONAR_H