File content as of revision 17:4730277de7c2:

#include "mbed.h"
#include "rtos.h"
#include "Servo.h"
#include "C12832.h"

Servo tiltServo(p24);
Servo panServo(p25);
Serial pc(USBTX, USBRX);
Servo vertServo(p23);

AnalogIn sonar(p19); // temporary changed to potmeter
Mutex mutexIn;
Mutex mutexOut;

// Global variables
float corHoriz = 0.5; // horizontal variable arrives from OpenCV
float corVert = 0.5; // vertical variable arrives from OpenCV
float distance = 0.5; // distance from the sonar sensor
float outVert; // output to vertical servo
float outTilt; // output to tilt servo
float outHoriz; // output to horizontal servo
float differ; // temporary global. Can be local.
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) {
        pc.scanf("%f,%f", &corHoriz, &corVert);// read from serial port the data
    }
}

/* Thread 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) {
        // Display values on the LCD screen
        lcd.cls();          // clear the display
        lcd.locate(0,5);    // the location where you want your charater to be displayed
        lcd.printf("differ: %0.3f, OutTilt: %0.3f", differ, outTilt);
        lcd.locate(0,20);    // the location where you want your charater to be displayed
        lcd.printf("Vert: %0.3f, OutVert: %0.3f", corVert, outVert);
        Thread::wait(250);
    }
}

/* Thread Control 3 - handles the input data from the sonar sensor, and display on the LCD screen.
    @update inData */
void control_thread(void const *args)
{
    while (true) {
        mutexIn.lock();
        //float differ;
        differ = exp(corVert + sonar * (outTilt / 20)) / (1 + exp(corVert + sonar * (outTilt / 20)));
        if (corVert > 0.1 && corVert < 0.9) { // if corVert is valid (between 0 - 1) then do movements                
            // moves lamp down by the fraction of the difference from the middle
            outVert = corVert * (1 - differ);
            // tilt down by the fraction of the difference from the middle
            outTilt = corVert * differ;  /*
        } else if (0 < corVert < 0.3) {
            outVert = 0;
            outTilt = corVert * .9; // Follow the low movement only with the tilt servo. 
                                           // (.9 is the correction of the side of the screen at OpenCV)
        } else if (corVert > 0.7 && corVert < 1) {
            outVert = 1;
            outTilt = corVert * 1.1; */
        } else { // Else this is the case when there is no input from the OpenCV
            outVert = .5;
            outTilt = corVert * differ;
            // TODO Pan search code here. (Searching personality.)
        }
        mutexIn.unlock();
        Thread::wait(25);
    }
}

/* Thread Servo 4 - 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();
        tiltServo = outTilt;
        panServo = outHoriz;
        vertServo = outVert;
        mutexOut.unlock();
        Thread::wait(200);
    }
}

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