Diff: main.cpp

Revision:

9:fa6121cc0fff

Parent:

8:214a95be5c54

Child:

10:5a9a3a5704ef

diff -r 214a95be5c54 -r fa6121cc0fff main.cpp
--- a/main.cpp	Thu Mar 26 15:38:49 2015 +0000
+++ b/main.cpp	Fri Apr 24 10:16:05 2015 +0000
@@ -4,15 +4,18 @@
 #include "RangeFinder.h"
 #include "Servo.h"
 
+#define WAITT   Thread::wait(50);
+
 // Seeed ultrasound range finder
-RangeFinder rf(p21, 10, 5800.0, 100000);
+RangeFinder rf(p22, 10, 5800.0, 100000);
 BusOut myleds(LED1, LED2, LED3, LED4);
 C12832_LCD lcd;
 
 float sonar_data;
-int base_servo_pos = 500;
+int base_servo_pos = 1000;
 
-Servo s1(p22);
+
+Servo s1(p21);
 AnalogIn ainLeft(p15);
 AnalogIn ainRight(p16);
 
@@ -28,73 +31,50 @@
             lcd.printf("No object within detection range.\n");         
         } else  {
             lcd.printf("Distance = %f m.\n", sonar_data);
+            lcd.locate(1,0);
+            lcd.printf("\nvalue = %d m.\n", base_servo_pos);
         }
-        Thread::wait(250);
-    }
-}
-
-void led_display(void const *args) {
-    while (true) {
-            float led_m = 0.790474;
-        if (sonar_data <= led_m)  
-            {
-             myleds = 15;
-             }
-        else if (sonar_data > led_m && sonar_data <= 2*led_m ) 
-            {
-            myleds = 7;      
-            }
-        else if (sonar_data > 2*led_m && sonar_data <= 3*led_m )  
-            {
-            myleds = 3;      
-            }
-        else  
-            {
-            myleds = 1;      
-            }
-    
-        Thread::wait(250);
-    }
-}
-
-void serial_display(void const *args) {
-    while (true) {
-        lcd.locate(0,0);
-        if (sonar_data == -1.0)  {
-            printf("Timeout Error.\n");   
-        } else if (sonar_data > 5.0) {  
-            printf("No object within detection range.\n");         
-        } else  {
-            printf("Distance = %f m.\n", sonar_data);
-        }
-        Thread::wait(250);
+       WAITT;
     }
 }
 
 void base_servo(void const *args) {
     while (true) {
-    base_servoMutex.lock();       
+    base_servoMutex.lock();
+           
     s1.SetPosition(base_servo_pos);
     base_servoMutex.unlock();
-    Thread::wait(250);
+    WAITT;
     }
 }
 
+float ajusting_sonardata(float data)
+{
+    if(data>0.3)
+    {
+        return 0.3;
+        }
+    else{
+        return data;
+        }
+}
+
+
 int main()  {    
     lcd.cls();
         
-    s1.Enable(500,20000);    
+    s1.Enable(1000,20000);    
         
-    Thread thread1(led_display);
+//    Thread thread1(led_display);
     Thread thread2(lcd_display);
-    Thread thread3(serial_display);
+//    Thread thread3(serial_display);
     Thread thread4(base_servo);
     
     while (1)   {
         sonar_data = rf.read_m();
         base_servo_Intelligence.lock();
-        base_servo_pos = 500 +((1000*sonar_data)/3);         
+        base_servo_pos = 2200-((1400*ajusting_sonardata(sonar_data)/0.3));         
         base_servo_Intelligence.unlock();        
-        Thread::wait(250);
+        WAITT;
     }
 }     
\ No newline at end of file