File content as of revision 8:214a95be5c54:

#include "rtos.h"
#include "mbed.h"
#include "C12832_lcd.h"
#include "RangeFinder.h"
#include "Servo.h"

// Seeed ultrasound range finder
RangeFinder rf(p21, 10, 5800.0, 100000);
BusOut myleds(LED1, LED2, LED3, LED4);
C12832_LCD lcd;

float sonar_data;
int base_servo_pos = 500;

Servo s1(p22);
AnalogIn ainLeft(p15);
AnalogIn ainRight(p16);

Mutex base_servoMutex;
Mutex base_servo_Intelligence;

void lcd_display(void const *args) {
    while (true) {
        lcd.locate(0,0);
        if (sonar_data == -1.0)  {
            lcd.printf("Timeout Error.\n");   
        } else if (sonar_data > 5.0) {  
            lcd.printf("No object within detection range.\n");         
        } else  {
            lcd.printf("Distance = %f m.\n", sonar_data);
        }
        Thread::wait(250);
    }
}

void led_display(void const *args) {
    while (true) {
            float led_m = 0.790474;
        if (sonar_data <= led_m)  
            {
             myleds = 15;
             }
        else if (sonar_data > led_m && sonar_data <= 2*led_m ) 
            {
            myleds = 7;      
            }
        else if (sonar_data > 2*led_m && sonar_data <= 3*led_m )  
            {
            myleds = 3;      
            }
        else  
            {
            myleds = 1;      
            }
    
        Thread::wait(250);
    }
}

void serial_display(void const *args) {
    while (true) {
        lcd.locate(0,0);
        if (sonar_data == -1.0)  {
            printf("Timeout Error.\n");   
        } else if (sonar_data > 5.0) {  
            printf("No object within detection range.\n");         
        } else  {
            printf("Distance = %f m.\n", sonar_data);
        }
        Thread::wait(250);
    }
}

void base_servo(void const *args) {
    while (true) {
    base_servoMutex.lock();       
    s1.SetPosition(base_servo_pos);
    base_servoMutex.unlock();
    Thread::wait(250);
    }
}

int main()  {    
    lcd.cls();
        
    s1.Enable(500,20000);    
        
    Thread thread1(led_display);
    Thread thread2(lcd_display);
    Thread thread3(serial_display);
    Thread thread4(base_servo);
    
    while (1)   {
        sonar_data = rf.read_m();
        base_servo_Intelligence.lock();
        base_servo_pos = 500 +((1000*sonar_data)/3);         
        base_servo_Intelligence.unlock();        
        Thread::wait(250);
    }
}