File content as of revision 4:429d9a63eb79:

#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();
    }
}

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

void logic(void const *args){
    while(true){
        sonarDistance_mutex.lock();
        servoPosition_mutex.lock();
            servoPosition = (sonarDistance - servoPosition) / 2;
        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){
        }    
    }