File content as of revision 5:e82e00f12634:

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

float sonarDistance;
float servoPosition;

Mutex sonarDistance_mutex;
Mutex servoPosition_mutex;

AnalogIn sonarPin(p17);
PwmOut servoPin(p21);

C12832_LCD lcd;

void sonarSensor(void const *args){
    while(true){
        sonarDistance_mutex.lock();
            sonarDistance = sonarPin.read();
        sonarDistance_mutex.unlock();
        Thread::wait(10);
    }
}

void servoControl(void const *args){
    while(true){
        servoPosition_mutex.lock();
            servoPin.write(servoPosition);
        servoPosition_mutex.unlock();
    }
}

void logic(void const *args){
    while(true){
        float Average_4[]={0,0,0,0,0,0,0};
        float Average_Sum=0;
        
        sonarDistance_mutex.lock();
        servoPosition_mutex.lock();
            //servoPosition = (sonarDistance - servoPosition)/2 ;
            for(int i=0;i<6;i++)
                {
                 Average_Sum = Average_Sum-Average_4[i];
                 Average_4[i]= sonarDistance;
                 Average_Sum = Average_Sum + Average_4[i];
                 servoPosition = Average_Sum/6;
                // Thread::wait(50);
                }
            
        sonarDistance_mutex.unlock();
        servoPosition_mutex.unlock();
    }
}

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){
        }    
    }