File content as of revision 1:a22303e03da3:

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

C12832 lcd(p5, p7, p6, p8, p11);
Serial pc(USBTX, USBRX);
AnalogIn aIn(p17);

float userX, userY;
float servoOutX, servoOutY;
float sensorIn;

Servo servoPan(p21);
Servo servoTilt(p22);

typedef struct {
    float x;
    float y;
} Position;

Mail<Position, 16> rrInput;

static float clamp(float value, float min, float max){
    return (value < min)? min : (value > max)? max : value;    
}

bool withinDelta(float a, float b, float delta) {
    return fabs(a - b) < delta;
}

void getUserPosition(void const *name){
    float x;
    float y;
    float lastX;
    float lastY;
    pc.scanf("%f,%f", &x, &y);
    lastX = x;
    lastY = y;
    bool firstTime = true;
    while(1){
        pc.scanf("%f,%f", &x, &y);
        if(firstTime || !withinDelta(x, lastX, 0.02) || !withinDelta(y, lastY, 0.02)) {
            firstTime = false;
            Position* pos = rrInput.alloc();
            if(pos != NULL) { // Can't have nmore than 16 items in the queue, alloc() returns null if queue is full.
                pos->x = x;
                pos->y = y;
                rrInput.put(pos);
            }
            lastX = x;
            lastY = y;
        }

    }
}

void getSensorValue(void const *name){
    while(1){
        sensorIn = aIn.read();
        Thread::wait(50);
    }
}

void logic(void const *name){
    while(1){
        osEvent evt = rrInput.get();
        if(evt.status == osEventMail) {
            Position* reading = (Position*)evt.value.p;
            lcd.cls();
            lcd.locate(0, 0);
            servoOutX = clamp(reading->x, 0, 1);
            servoOutY = clamp(reading->y, 0, 1);
            
            //Displays debug data
            lcd.cls();
            lcd.locate(0,0);
            lcd.printf("InX: %.3f, OutX: %.3f", reading->x, servoOutX);
            lcd.locate(0,15);
            lcd.printf("InY: %.3f, OutY: %.3f", reading->y, servoOutY);
            
            rrInput.free(reading);
            Thread::wait(50);
        }
    }
}

//Sets the servos to the given data
void setServoValue(void const *name){
    while(1){
        
        //Set the servos to the set values
        servoPan = servoOutX;
        servoTilt = servoOutY;
        
        Thread::wait(50);
    }
}
 
int main(){
    //Start each of the threads
    Thread t1(getUserPosition, (void *)"Th 1");
    Thread t2(getSensorValue, (void *)"Th 2");
    Thread t3(logic, (void *)"Th 3");
    Thread t4(setServoValue, (void *) "Th 4");
    
    //Stop the main process from dying
    while(1)wait(10000);
}