File content as of revision 6:4c62f9c91b1d:

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

float sonarDistance;
float servoPosition;

Mutex sonarDistance_mutex;                                  //Init Mutex for sensor value
Mutex servoPosition_mutex;                                  //Init Mutex for servo value
    
AnalogIn sonarPin(p17);                                     //Assign pin to read sonar sensor
PwmOut servoPin(p21);                                       //Assign pin for servo output

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)
    {
    while(true)
        {
        float Average_4[]={0,0,0,0,0,0,0};                  //number of value in the array
        float Average_Sum=0;
        
        sonarDistance_mutex.lock();                         //Mutex lock for the Sonar value
        servoPosition_mutex.lock();                         //Mutex lock for the servo value
            
        for(int i=0;i<6;i++)
            {
             Average_Sum = Average_Sum-Average_4[i];        //Remove the 4th oldest value from the average sum
             Average_4[i]= sonarDistance;                   //Add the new value to the array
             Average_Sum = Average_Sum + Average_4[i];      //Add the new array value to the sum
             servoPosition = Average_Sum/6;                 //Divide the array by the number of element in the array
            }//end for loop
            
        sonarDistance_mutex.unlock();                       //Mutex unlock for the Sonar value
        servoPosition_mutex.unlock();                       //Mutex unlock for the servo value
        }//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);
            
    }
}

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