Diff: main.cpp

Revision:

16:af76305da577

Parent:

10:ca6f2769964e

Child:

17:974430ee2fbb

Child:

24:de4d6a06011b

--- a/main.cpp	Mon Apr 13 13:01:02 2015 +0000
+++ b/main.cpp	Fri Apr 17 10:12:20 2015 +0000
@@ -2,27 +2,37 @@
 #include "rtos.h"
 #include "Servo.h"
 #include "C12832.h"
+#include "RangeFinder.h"// header files for sonar sensor
 
-Servo tiltServo(p21);
+Servo tiltServo(p22);
 Servo panServo(p22);
+
 Serial pc(USBTX, USBRX);
 Servo vertServo(p23);
-AnalogIn sonar(p19);
-Mutex mutexIn;
-Mutex mutexOut;
- 
-AnalogIn p1(p19);
-AnalogIn p2(p20);
+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;
+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
+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(p26, 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) {
@@ -35,12 +45,16 @@
     @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("Ver: %0.3f", corVert);
+        lcd.printf("dis: %0.2f", distance);// Display the distance in meters from the sonar
+        mutex_sonar.unlock();
         Thread::wait(250);
     }
 }
@@ -50,6 +64,7 @@
 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
@@ -58,7 +73,7 @@
             }
         }
         mutexIn.unlock();
-        Thread::wait(25);
+        Thread::wait(250);
     }
 }
 
@@ -71,16 +86,28 @@
         panServo = outHoriz;
         vertServo = outVert;
         mutexOut.unlock();
-        Thread::wait(200);
+        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
+        printf("dis: %0.2f", 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) {
+       
         wait(1);
     }
 }