File content as of revision 29:e8f7b891b5c1:

#include "mbed.h"
#include "rtos.h"
#include "C12832.h"
#include "RangeFinder.h"// header files for sonar sensor

PwmOut tiltServo(p24);
PwmOut panServo(p25);
PwmOut vertServo(p23);
Serial pc(USBTX, USBRX);
Mutex mutexIn;// protect global variables
Mutex mutexOut;// protect global variables
Mutex mutex_sonar;

// Global variables
float cvHoriz = -1; // horizontal variable arrives from OpenCV
float cvVert = -1; // vertical variable arrives from OpenCV
float distance = 0.5;// variable holds the distance in meters 0 to 3.3
float norm=0;      // variable holds the normalised values form the sonar sensor
float outVert; // output to vertical servo
float outTilt; // output to tilt servo
float outHoriz; // output to horizontal servo
C12832 lcd(p5, p7, p6, p8, p11);// lcd is an object from class c12832 initialised by p5,p7....

typedef enum {
    searching,
    tracking,
    retreating
} LampState;

LampState currentTask = searching;

time_t lastSearchMovementTime = time(NULL);
time_t lastVisionEvent = time(NULL);
time_t initialVisionTime = lastVisionEvent;

/* parallax ultrasound range finder
p21 pin the range finder is connected to.
10 is Time of pulse to send to the rangefinder to trigger a measurement, in microseconds.
5800 is   Scaling of the range finder's output pulse from microseconds to metres.
100000 Time to wait for a pulse from the range finder before giving up */

RangeFinder rf(p21, 10, 5800.0, 100000);
/* 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();
        //   mutexOut.lock();

        //if not range finding
        mutex_sonar.lock();
        if(pc.readable())
            pc.scanf("%f,%f", &cvHoriz, &cvVert);// read from serial port the data

        mutex_sonar.unlock();
        //mutexIn.unlock();
        //mutexOut.unlock();
        Thread::wait(1000);
    }
}

/* 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) {
        mutex_sonar.lock();
        // Display values on the LCD screen

        // lcd.cls();          // clear the display
        lcd.locate(0,0);    // the location where you want your charater to be displayed
        lcd.printf("tilt: %0.3f, dsrd: %0.3f", tiltServo.read(), outTilt);

        // lcd.cls();          // clear the display
        lcd.locate(0,10);    // the location where you want your charater to be displayed
        //lcd.printf("Vert: %0.3f, OutVert: %0.3f", cvVert, outVert);
        //lcd.printf("vert: %0.3f", vertServo.read());
        switch(currentTask) {
            case retreating:
                lcd.printf("retreating");
                break;
            case tracking:
                lcd.printf("tracking");
                break;
            case searching:
                lcd.printf("searching");
                break;
            default:
                lcd.printf("Invalid Mode");
                break;
        }

        //lcd.cls();          // clear the display
        lcd.locate(0,20);    // the location where you want your charater to be displayed
        lcd.printf("dis: %0.2f\n\r", distance);// Display the distance in meters from the sonar
        mutex_sonar.unlock();
        Thread::wait(250);
        mutex_sonar.unlock();
    }
}

void followPerson() {
    // moves lamp down by the fraction of the difference from the middle
    if (cvVert >= .5) {
        outVert = cvVert - ((cvVert - .5) * norm);
    } else {
        outVert = cvVert + ((.5 - cvVert) * norm);
    }
    outTilt = cvVert;
    outHoriz = cvHoriz;
}

void searchPerson() {
    time_t currentTime = time(NULL);
    if((currentTime - lastSearchMovementTime) > 2) {
        if(outHoriz < 0.5) {
            outHoriz = 0.8;
        } else {
            outHoriz = 0.3;
        }
        lastSearchMovementTime = currentTime;
    }
}

void shooPerson() {
    outHoriz = 0.5;
    outVert = 0.2;
    outTilt = 0.2;
    Thread::wait(250);
    outTilt = 0.8;
    Thread::wait(250);
    outTilt = 0.2;
    Thread::wait(250);
    outTilt = 0.8;
}

/* 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();
        time_t currentTime = time(NULL);
        if(distance <= 0.5) {
            currentTask = retreating;
        } else if((currentTime - lastVisionEvent) < 200) {
            currentTask = tracking;
        } else {
            currentTask = searching;
        }
        switch(currentTask) {
            case tracking:
                followPerson();
                break;   
            case searching:
                searchPerson();
                break;
            case retreating:
                shooPerson();
                break;   
        }
        mutexIn.unlock();
        Thread::wait(50);
    }
}

float clamp(float min, float max, float scale) {
    if(scale > 1) {
        scale = 1;
    }
    if(scale < 0) {
        scale = 0;
    }
    return ((max - min) * scale) + min;
}

/* 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 = clamp(0.055, 0.09, outTilt);
        panServo = clamp(0.03, 0.12, outHoriz);
        vertServo = clamp(0.0, 0.1, outVert);
        mutexOut.unlock();
        Thread::wait(250);
    }
}

/* Thread sonar 5 - handles the sonar values which can be in meter or normailsed value to one */
void sonar_thread(void const *args)
{
    while (true) {
        mutex_sonar.lock();
        distance = rf.read_m(); // read the distance from the sonar sensor in meter
        norm= distance/3.3;     // normalised value from the sonar sensor
        // lcd.cls();          // clear the display
        // lcd.locate(0,5);    // the location where you want your charater to be displayed
        printf("dis: %0.2f\n\r", distance);// Display the distance in meters from the sonar
        mutex_sonar.unlock();
        Thread::wait(250);
    }
}

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
    Thread thread_5(sonar_thread); // Start Servo Thread
    while(1) {
        Thread::wait(1);
    }
}