File content as of revision 1:4808f55970e8:

#include "mbed.h"
#include "HCSR04.h"
#include "AutomationElements.h"

InterruptIn EchoL(PA_0);
InterruptIn EchoF(PA_1);
InterruptIn EchoR(PA_4);

DigitalOut TriggerL(PB_3);
DigitalOut TriggerF(PB_4);
DigitalOut TriggerR(PB_5);

HCSR04 sensor_Left(EchoL, TriggerL);
HCSR04 sensor_Front(EchoF, TriggerF);
HCSR04 sensor_Right(EchoR, TriggerR);

float distance;
float filteredDistance;
int Glob_Position;

void calc()
{
    switch(Glob_Position) {
        case 1:
            sensor_Left.startMeasurement();
            break;

        case 2:
            sensor_Front.startMeasurement();
            break;

        case 3:
            sensor_Right.startMeasurement();
            break;

        default:
            break;
    }
}

int readSensor(int Position)
{
    Glob_Position = Position;
    //PT1 filter(1, 2, sampleTime);

    sensor_Left.setRanges(2, 400);
    sensor_Front.setRanges(2,400);
    sensor_Right.setRanges(2, 400);
    //ticker_sens.attach(callback(this, &ReadSensor::calc, sampleTime));
    while(true) {
        switch(Position) {
            case 1:
                while(!sensor_Left.isNewDataReady()) {
                    // wait for new data
                }
                distance = sensor_Left.getDistance_cm();
                break;

            case 2:
                while(!sensor_Front.isNewDataReady()) {
                    // wait for new data
                }
                distance = sensor_Front.getDistance_cm();
                break;

            case 3:
                while(!sensor_Right.isNewDataReady()) {
                    // wait for new data
                }
                distance = sensor_Right.getDistance_cm();
                break;

            default:
                return true;
                break;
        }

        if(distance <= 9.00) {
            //printf("t%f\n",filteredDistance);
            printf("t%f\n",distance);
            return 1;

        } else {
            //printf("f%f\n",filteredDistance);
            printf("t%f\n",distance);
            return 0;

        }

    }
}

float readSensorValue(int Position)
{
    Glob_Position = Position;
    float distancetot = 0;
    int i = 0;

    sensor_Left.setRanges(2, 400);
    sensor_Front.setRanges(2,400);
    sensor_Right.setRanges(2, 400);
    while(true) {
        for(i = 0; i < 2; i++) {
            switch(Position) {
                case 1:
                    while(!sensor_Left.isNewDataReady()) {
                        // wait for new data
                    }
                    distance = sensor_Left.getDistance_cm();
                    break;

                case 2:
                    while(!sensor_Front.isNewDataReady()) {
                        // wait for new data
                    }
                    distance = sensor_Front.getDistance_cm();
                    break;

                case 3:
                    while(!sensor_Right.isNewDataReady()) {
                        // wait for new data
                    }
                    distance = sensor_Right.getDistance_cm();
                    break;

                default:
                    return true;
                    break;
            }
            distancetot = distancetot + distance;
        }
        //ticker_sens.detach();
        distancetot = distancetot / (i);
        printf("%f\n",distancetot);
        return distancetot;
        //filter.in(distance);
        //filteredDistance = filter.out();


    }
}