File content as of revision 7:0fa7430ab812:

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

//Varible
float sonarDistance;
float servoPosition;

//Mutex
Mutex sonarDistance_mutex;                                  //Init Mutex for sensor value
Mutex servoPosition_mutex;                                  //Init Mutex for servo value
Mutex LED_mutex;

//I/O    
AnalogIn sonarPin(p17);                                     //Assign pin to read sonar sensor
PwmOut servoPin(p21);                                       //Assign pin for servo output
DigitalOut myled1(LED1);
DigitalOut myled2(LED2);
DigitalOut myled3(LED3);
DigitalOut myled4(LED4);

//LCD Setup
C12832_LCD lcd;                                             //setup LCD screen

void sonarSensor(void const *args){
    while(true){
        sonarDistance_mutex.lock();                         //Mutex lock for the Servo value
            sonarDistance = sonarPin.read();                //Read analog voltage and store in variable
        sonarDistance_mutex.unlock();                       //Mutex unlock for the Servo value
        Thread::wait(10);                                   //Pause thread for 10msec
    }  //End of While loop
} //End of Thread

void servoControl(void const *args){
    while(true){
        servoPosition_mutex.lock();                         //Mutex lock for the Servo value
            servoPin.write(servoPosition);                  //Write servo value to servo pin
        servoPosition_mutex.unlock();                       //Mutex unlock for the Servo value
    }  //end of while loop
} //end of thread

void logic(void const *args)
    {
    int Size=12;
    float Average_4[12];                  //number of value in the array
    float Average_Sum=0;
    while(true)
        {

        for(int i=0;i<Size;i++)
            {
             Average_Sum = Average_Sum-Average_4[i];        //Remove the 4th oldest value from the average sum

             sonarDistance_mutex.lock();                         //Mutex lock for the Sonar value
             Average_4[i]= sonarDistance;                   //Add the new value to the array
             sonarDistance_mutex.unlock();                       //Mutex unlock for the Sonar value
             
             Average_Sum = Average_Sum + Average_4[i];      //Add the new array value to the sum
             
             servoPosition_mutex.lock();                         //Mutex lock for the servo value             
             servoPosition = Average_Sum/Size;                      //Divide the array by the number of element in the array
             servoPosition_mutex.unlock();                       //Mutex unlock for the servo value
            
            }//end for loop
            
        }//end of while loop
    }//End of thread

void display(void const *args){
    while(true){
        sonarDistance_mutex.lock();
        servoPosition_mutex.lock();
            lcd.cls();
            lcd.locate(0,0);
            lcd.printf("Sonar : %3.2f \nServo : %3.2f \nSCIENCE!",sonarDistance,servoPosition);
        sonarDistance_mutex.unlock();
        servoPosition_mutex.unlock();
        Thread::wait(200);
            
    }
}

void LED_meter(void const *args) {
    
    float LED_distance = 0;
    
    while(true) {
        sonarDistance_mutex.lock();
        LED_distance=sonarDistance;
        sonarDistance_mutex.unlock();
        
        if(LED_distance<=0.2)
            {
            myled1 = 1;
            }
        else
            {
            myled1 = 0;
            }
            
        if(LED_distance<=0.15)
            {
            myled2 = 1;
            }
        else
            {
            myled2 = 0;
            }
 
        if(LED_distance<=0.1)
            {
            myled3 = 1;
            }
        else
            {
            myled3 = 0;
            }       
       
        if(LED_distance<=0.05)
            {
            myled4 = 1;
            //s2=0.1;  
            }
        else
            {
            myled4 = 0;
            //s2=1;  
            }         
    }
}

int main(){
    sonarDistance = 0.0f;
    servoPosition = 0.0f;
    
    
    Thread sonarSensor_thread(sonarSensor);
    Thread servoControl_thread(servoControl);
    Thread logic_thread(logic);
    Thread display_thread(display);
    Thread LED_meter_thread(LED_meter);
    
    while(true)
        {
        }    
    }