File content as of revision 14:f6d4980c48d6:

#include "mbed.h"
#include "rtos.h"
#include "Servo.h"
#include "C12832.h"
Servo sPan(p21);
Servo sTilt(p22);
Serial pc(USBTX, USBRX);
Servo sVert(p23);
AnalogIn distance(p19);
Mutex mutexIn;
Mutex mutexOut;

// Global variables
float corHoriz; // horizontal variable arrives from OpenCV
float corVert; // vertical variable arrives from OpenCV
float distance; // distance measured by the sonar
float outVert; //
float outTilt;
float 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.gets(cordinates,4);
        //   cordinates = pc.putc(pc.getc());
        pc.scanf("%s \n %s \n ",corHoriz,corVert);// read from serial port the data
        mutexIn.unlock();
        Thread::wait(200);
    }
}

/* 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) {
        mutexIn.lock();
        mutexOut.lock();
        // 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("Hor:%s",corHoriz);
        lcd.locate(0,20);    // the location where you want your charater to be displayed
        lcd.printf("Ver%s", corVert);
        mutexIn.unlock();
        mutexOut.unlock();
        Thread::wait(25);
    }
}

/* 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) / (1 + exp(corVert + sonar * outTilt));
        if (corVert > .45) { // check if face is below the half of the camera view
            if (outTilt > .45) { // check if lamp head is facing down
            // moves lamp down by the fraction of the difference from the middle
                outVert = outVert + 
                outTilt = outTilt + 
            } else if (outTilt < .55) {  // check if lamp head is facing up
                //
            }
           
        } else if (corVert < .55) {
           // moves lamp up by the fraction of the difference from the middle
        }
    }
    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();
        sTi1t = outTilt;
        sPan = outHoriz;
        sVert = 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 Control Thread
    Thread thread_4(servo_thread); // Start Servo Thread
    while(1) {
        Thread::wait(10);
    }
}