Diff: main.cpp

Revision:

12:de4d6a06011b

Parent:

11:af76305da577

--- a/main.cpp	Fri Apr 17 10:12:20 2015 +0000
+++ b/main.cpp	Mon Apr 20 13:06:40 2015 +0000
@@ -26,18 +26,22 @@
 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.
+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(p26, 10, 5800.0, 100000);
+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
+       
+       
     }
 }
  
@@ -45,7 +49,7 @@
     @update inData */
 void lcd_thread(void const *args) {
     while (true) {
-        mutex_sonar.lock();
+      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
@@ -81,33 +85,52 @@
     @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 (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);
-    }
+    while (1) {
+       
 }
+ }
 int main() {
-    Thread thread_1(serial_thread); // Start Serial Thread
-    Thread thread_2(lcd_thread); // Start LCD Thread
+ //  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);
+        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);
     }
 }