File content as of revision 5:4354b585dcab:

#include "mbed.h"
#include "rtos.h"
#include "Servo.h"
#include "C12832.h"
Servo sHoriz(p21);
Servo sVert(p22);
Serial pc(USBTX, USBRX);
Mutex mutexIn;
Mutex mutexOut;

// Globel variables
float corHoriz, corVert, outVert, outHoriz;
C12832 lcd(p5, p7, p6, p8, p11);// lcd is an object from class c12832 initialised by p5,p7....

/* Thread Serial 1 - handles the output data from the control thread, and pass to the servo.
    @update s1, s2 */
void serial_thread(void const *args) {
    while (true) {
        mutexIn.lock();
        pc.scanf("%f,%f", &corHoriz, &corVert);
        mutexIn.unlock();
        Thread::wait(50);
    }
}
 
/* Thread Calc and LCD 2 - handles the input data from the sonar sensor, and display on the LCD screen.
    @update inData */
void lcd_thread(void const *args) {
    while (true) {
        mutexIn.lock();
        mutexOut.lock();
        // Calculate servo movement data arrived from openCV
        // This case openCV send 5+ if need to increase the value of cordinates
        outHoriz = outHoriz + ((corHoriz - 5) / 1000);
        outVert = outVert + ((corVert - 5) / 1000);
        // Display values on the LCD screen
        lcd.cls();          // clear the display
        lcd.locate(0,3);    // the location where you want your charater to be displayed
        lcd.printf("Cor Hor: %.3f, Vert: %.3f", corHoriz, corVert);
        lcd.printf("Out Hor: %.3f, Vert: %.3f", outHoriz, outVert);
        lcd.printf("Servo Hor: %f, Vert: %f", sHoriz, sVert);
        mutexIn.unlock();
        mutexOut.unlock();
        Thread::wait(50);
    }
}

/* Thread Servo 3 - handles the output data from the control thread, and pass to the servo.
    @update s1, s2 */
void servo_thread(void const *args) {
    while (true) {
        mutexOut.lock();
        sVert = outVert;
        sHoriz = outHoriz;
        mutexOut.unlock();
        Thread::wait(50);
    }
}

int main() {
    Thread thread_1(serial_thread); // Start Serial Thread
    Thread thread_2(lcd_thread); // Start LCD Thread
    Thread thread_3(servo_thread); // Start Servo Thread
    while(1) {
        Thread::wait(10);
    }
}