Code for Sprint 2

Dependencies:   C12832 mbed rtos RangeFinder

Revision:
9:e5d24b7a921b
Parent:
4:70090f3b1f07
Child:
10:ca6f2769964e
--- a/main.cpp	Thu Mar 12 19:57:41 2015 +0000
+++ b/main.cpp	Mon Apr 13 12:39:24 2015 +0000
@@ -2,8 +2,9 @@
 #include "rtos.h"
 #include "Servo.h"
 #include "C12832.h"
-Servo s1(p21);
-Servo s2(p22);
+
+Servo tiltServo(p21);
+Servo panServo(p22);
 Serial pc(USBTX, USBRX);
 Mutex mutexIn;
 Mutex mutexOut;
@@ -11,25 +12,16 @@
 AnalogIn p1(p19);
 AnalogIn p2(p20);
 
-// Globel variables
-char cordinates[20];
-char corHoriz[20];
-char corVert[20];
-//float corDeep;
-float outVert;
-//float outHoriz;
+// Global variables
+float corHoriz = 0;
+float corVert = 0;
 C12832 lcd(p5, p7, p6, p8, p11);// lcd is an object from class c12832 initialised by p5,p7....
 
 /* 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) {
-        mutexIn.lock();
-      //  pc.gets(cordinates,4);
-     //   cordinates = pc.putc(pc.getc());
-         pc.scanf("%s \n %s \n ",corHoriz,corVert);// read from serial port the data
-        mutexIn.unlock();
-        Thread::wait(200);
+        pc.scanf("%f,%f", &corHoriz, &corVert);// read from serial port the data
     }
 }
  
@@ -37,43 +29,22 @@
     @update inData */
 void lcd_thread(void const *args) {
     while (true) {
-        mutexIn.lock();
-        mutexOut.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:%s",corHoriz);
+        lcd.printf("Hor: %0.3f", corHoriz);
         lcd.locate(0,20);    // the location where you want your charater to be displayed
-        lcd.printf("Ver%s", corVert);
-        mutexIn.unlock();
-        mutexOut.unlock();
-        Thread::wait(25);
+        lcd.printf("Ver: %0.3f", corVert);
+        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();
-        // The control code will come here
-        if (corHoriz[1]=='1')
-            s1 = s1 + 0.25;
-        if (corHoriz[1]=='2')
-            s1 = s1 - 0.25;
-        mutexIn.unlock();
-        Thread::wait(25);
-    }
-}
-
-/* Thread Servo 4 - handles the output data from the control thread, and pass to the servo.
+/* Thread Servo 3 - handles the output data from the control thread, and pass to the servo.
     @update s1, s2 */
 void servo_thread(void const *args) {
     while (true) {
-        mutexOut.lock();
-//        s1 = outVert;
-//        s2 = outHoriz;
-        mutexOut.unlock();
+        tiltServo = corVert;
+        panServo = corHoriz;
         Thread::wait(200);
     }
 }
@@ -81,9 +52,8 @@
 int main() {
     Thread thread_1(serial_thread); // Start Serial Thread
     Thread thread_2(lcd_thread); // Start LCD Thread
-    Thread thread_3(control_thread); // Start Control Thread
-    Thread thread_4(servo_thread); // Start Servo Thread
+    Thread thread_3(servo_thread); // Start Servo Thread
     while(1) {
-        Thread::wait(10);
+        wait(1);
     }
 }