File content as of revision 13:e9b99635aa2d:

#include "mbed.h"
#include "rtos.h"
#include "hardware.h"


void front_sensor(void);
void back_sensor(void);

void semaphore_thread(void const *name){
    pc.printf("Starting semaphore thread\n\r");
    while (true) {
        _semaphore.wait();
        //pc.printf("%s\n\r", (const char*)name);
        if(strstr((const char*)name, "frontSensor") != NULL)
            front_sensor();    
        if(strstr((const char*)name, "backSensor") != NULL)
            back_sensor();    
        
        Thread::wait(100);
        _semaphore.release();
    }

} 

void front_sensor(){
    DigitalOut trig(p7);
    DigitalIn echo(p8);
    bool tooLong = false;
    Timer timer;
    
    timer.start();
    trig=1;
    wait_us(10);
    trig=0;
    while(!echo){
        if(timer.read_us() > 5000)
            tooLong = true;            
    }
    if(tooLong == false){
        timer.reset();
        while(echo){
           if(timer.read_us() > 5000)
                break;    
        }
        _frontDistance = timer.read_us() / 58;
        //_frontWall = true;
        //pc.printf("front distance %d \n\n\r", _frontDistance);
    }
}

void back_sensor(){
    DigitalOut trig(p19);
    DigitalIn echo(p20);
    bool tooLong = false;
    Timer timer;
    
    //printf("left sensor started \n\r");
    timer.start();
    trig=1;
    wait_us(10);
    trig=0;
    while(!echo){
        if(timer.read_us() > 5000)
            tooLong = true;            
    }
    timer.reset();
    if(tooLong == false){
        timer.reset();
        while(echo){
           if(timer.read_us() > 5000)
                break;    
        }
        _backDistance = timer.read_us() / 58;
        //_backWall = true;
        //pc.printf("                          back distance %d \n\n\r", _backDistance);
    }

}

/*
void left_sensor(){
    DigitalOut trig(p19);
    DigitalIn echo(p20);
    bool tooLong = false;
    Timer timer;
    //printf("left sensor started \n\r");
    timer.start();
    //while(1){
        trig=1;   //  trigger high
        //printf("trig\n\r");
        wait_us(10);
        //printf("wait\n\r");
        trig=0;  // trigger low
        // start pulseIN
        while(!echo){
            if(timer.read_us() > 5000)
                tooLong = true;            
        }
        if(tooLong == false){
            timer.reset();
            while(echo){
               if(timer.read_us() > 5000)
                    break;    
            }
            _leftDistance = timer.read_us() / 58;
            if(_leftDistance <= 30)
                _leftWall = true;
            //pc.printf("left distance %d \n\n\r", _leftDistance);
        }
        //Thread::wait(100);
    //}
    
}

void right_sensor(){
    DigitalOut trig(p17);
    DigitalIn echo(p18);
    
    Timer timer;
    //printf("left sensor started \n\r");
    timer.start();
    //while(1){
        trig=1;   //  trigger high
        //printf("trig\n\r");
        wait_us(10);
        //printf("wait\n\r");
        trig=0;  // trigger low
        // start pulseIN
        while(!echo);
        timer.reset();
        while(echo);
        _rightDistance = timer.read_us() / 58;
        if(_rightDistance <= 40)
            _rightWall = true;
        pc.printf("right distance %d \n\n\r", _rightDistance);
        
        //Thread::wait(100);
    //}
    
}
*/