Diff: main.cpp

Revision:

5:4354b585dcab

Parent:

0:5edc27224a37

Child:

6:dc51d320b369

--- a/main.cpp	Wed Feb 25 17:33:41 2015 +0000
+++ b/main.cpp	Fri Mar 13 22:51:36 2015 +0000
@@ -2,19 +2,14 @@
 #include "rtos.h"
 #include "Servo.h"
 #include "C12832.h"
-Servo s1(p21);
-Servo s2(p22);
+Servo sHoriz(p21);
+Servo sVert(p22);
 Serial pc(USBTX, USBRX);
 Mutex mutexIn;
 Mutex mutexOut;
 
 // Globel variables
-float cordinates[3];
-float corHoriz;
-float corVertic;
-float corDeep;
-float outVert;
-float outHoriz;
+float corHoriz, corVert, outVert, outHoriz;
 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.
@@ -22,60 +17,50 @@
 void serial_thread(void const *args) {
     while (true) {
         mutexIn.lock();
-//        cordinates = pc.putc(pc.getc());
-        corHoriz = cordinates[0];
-        corVertic = cordinates[1];
-        corDeep = cordinates[2];
+        pc.scanf("%f,%f", &corHoriz, &corVert);
         mutexIn.unlock();
-        Thread::wait(200);
+        Thread::wait(50);
     }
 }
  
-/* Thread LCD 2 - handles the input data from the sonar sensor, and display on the LCD screen.
+/* Thread Calc and LCD 2 - handles the input data from the sonar sensor, and display on the LCD screen.
     @update inData */
 void lcd_thread(void const *args) {
     while (true) {
         mutexIn.lock();
         mutexOut.lock();
+        // Calculate servo movement data arrived from openCV
+        // This case openCV send 5+ if need to increase the value of cordinates
+        outHoriz = outHoriz + ((corHoriz - 5) / 1000);
+        outVert = outVert + ((corVert - 5) / 1000);
         // Display values on the LCD screen
         lcd.cls();          // clear the display
         lcd.locate(0,3);    // the location where you want your charater to be displayed
-        lcd.printf("Hor: %f, Vert: %f, Deep: %f", corHoriz, corVertic, corDeep);
+        lcd.printf("Cor Hor: %.3f, Vert: %.3f", corHoriz, corVert);
+        lcd.printf("Out Hor: %.3f, Vert: %.3f", outHoriz, outVert);
+        lcd.printf("Servo Hor: %f, Vert: %f", sHoriz, sVert);
         mutexIn.unlock();
         mutexOut.unlock();
-        Thread::wait(25);
+        Thread::wait(50);
     }
 }
 
-/* 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
-        
-        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;
+        sVert = outVert;
+        sHoriz = outHoriz;
         mutexOut.unlock();
-        Thread::wait(200);
+        Thread::wait(50);
     }
 }
 
 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);
     }