File content as of revision 24:de4d6a06011b:

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

Servo tiltServo(p22);
Servo panServo(p22);

Serial pc(USBTX, USBRX);
Servo vertServo(p23);
AnalogIn sonar(p19); //not any more used

Mutex mutexIn;// protect globel variables
Mutex mutexOut;// protect globel variables
Mutex mutex_sonar;

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

/*parallax ultrasound range finder
p23 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(p23, 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) {
      // I have problem in this thread the sonar reading is not correct if this thread is used 
      
        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) {
      mutex_sonar.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: %0.3f", corHoriz);
        lcd.locate(0,10);    // the location where you want your charater to be displayed
        lcd.printf("Ver: %0.3f", corVert);
        lcd.locate(0,20);    // the location where you want your charater to be displayed
        lcd.printf("dis: %0.2f", distance);// Display the distance in meters from the sonar
        mutex_sonar.unlock();
        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) / (1 + exp(corVert + sonar * outTilt));
        if (corVert > .5) { // check if face is below the half of the camera view
            if (outTilt > .5) { // check if lamp head is facing down
                // moves lamp down by the fraction of the difference from the middle
            }
        }
        mutexIn.unlock();
        Thread::wait(250);
    }
}

/* 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) {
       //  mutex_sonar.lock();
        mutexOut.lock();
        tiltServo = outTilt;
        panServo = outHoriz;
        vertServo = outVert;
        
        /* if the distance more than 3 meters the servo will mover full range CW and CCW*/
        for(int i=0; i<100; i++) {
             if (distance>=2)
             {
             tiltServo = i/100.0;
             Thread::wait(25);
             }
         }
         for(int i=100; i>0; i--) {
             if (distance>=2)
             {
             tiltServo = i/100.0;
            Thread:: wait(25);
            }
         }
         tiltServo.position(0.0);
       // mutex_sonar.unlock();
        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 (1) {
       
}
 }
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) {
        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
        mutex_sonar.unlock();
        Thread::wait(25);
    }
}