Diff: main.cpp

Revision:

8:214a95be5c54

Parent:

7:cb446dbb54d9

Child:

9:fa6121cc0fff

--- a/main.cpp	Mon Mar 23 14:58:40 2015 +0000
+++ b/main.cpp	Thu Mar 26 15:38:49 2015 +0000
@@ -2,6 +2,7 @@
 #include "mbed.h"
 #include "C12832_lcd.h"
 #include "RangeFinder.h"
+#include "Servo.h"
 
 // Seeed ultrasound range finder
 RangeFinder rf(p21, 10, 5800.0, 100000);
@@ -9,7 +10,14 @@
 C12832_LCD lcd;
 
 float sonar_data;
+int base_servo_pos = 500;
 
+Servo s1(p22);
+AnalogIn ainLeft(p15);
+AnalogIn ainRight(p16);
+
+Mutex base_servoMutex;
+Mutex base_servo_Intelligence;
 
 void lcd_display(void const *args) {
     while (true) {
@@ -63,16 +71,30 @@
     }
 }
 
+void base_servo(void const *args) {
+    while (true) {
+    base_servoMutex.lock();       
+    s1.SetPosition(base_servo_pos);
+    base_servoMutex.unlock();
+    Thread::wait(250);
+    }
+}
 
 int main()  {    
     lcd.cls();
         
+    s1.Enable(500,20000);    
+        
     Thread thread1(led_display);
     Thread thread2(lcd_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_Intelligence.unlock();        
         Thread::wait(250);
     }
 }     
\ No newline at end of file