Revision 10:ca6f2769964e, committed 2015-04-13

Comitter:

Argensis

Date:

Mon Apr 13 13:01:02 2015 +0000

Parent:

8:fe434a018d96

Parent:

9:e5d24b7a921b

Child:

11:015d9e5487b5

Child:

13:4ef3d9835b13

Child:

16:af76305da577

Commit message:

Merge Andreas movement and Tony's OpenCV/Serial code.

Changed in this revision

--- a/Servo.lib	Mon Apr 13 12:43:43 2015 +0000
+++ b/Servo.lib	Mon Apr 13 13:01:02 2015 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/jdenkers/code/Servo/#352133517ccc
+http://developer.mbed.org/users/simon/code/Servo/#36b69a7ced07

--- a/main.cpp	Mon Apr 13 12:43:43 2015 +0000
+++ b/main.cpp	Mon Apr 13 13:01:02 2015 +0000
@@ -2,11 +2,12 @@
 #include "rtos.h"
 #include "Servo.h"
 #include "C12832.h"
-Servo sPan(p21);
-Servo sTilt(p22);
+
+Servo tiltServo(p21);
+Servo panServo(p22);
 Serial pc(USBTX, USBRX);
-Servo sVert(p23);
-Sonar distance(p24);
+Servo vertServo(p23);
+AnalogIn sonar(p19);
 Mutex mutexIn;
 Mutex mutexOut;
  
@@ -14,9 +15,9 @@
 AnalogIn p2(p20);
 
 // Global variables
-float corHoriz; // horizontal variable arrives from OpenCV
-float corVert; // vertical variable arrives from OpenCV
-float distance; // distance measured by the sonar
+float corHoriz = 0; // horizontal variable arrives from OpenCV
+float corVert = 0; // vertical variable arrives from OpenCV
+float distance = 0;
 float outVert; // rr
 float outTilt;
 float outHoriz;
@@ -26,12 +27,7 @@
     @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
     }
 }
  
@@ -39,17 +35,13 @@
     @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);
     }
 }
 
@@ -63,6 +55,8 @@
         if (corVert > .5) { // check if face is below the half of the camera view
             if (outTilt > .5) { // check if lamp head is facing down
                 // moves lamp down by the fraction of the difference from the middle
+            }
+        }
         mutexIn.unlock();
         Thread::wait(25);
     }
@@ -73,9 +67,9 @@
 void servo_thread(void const *args) {
     while (true) {
         mutexOut.lock();
-        sTi1t = outTilt;
-        sPan = outHoriz;
-        sVert = outVert;
+        tiltServo = outTilt;
+        panServo = outHoriz;
+        vertServo = outVert;
         mutexOut.unlock();
         Thread::wait(200);
     }
@@ -84,9 +78,9 @@
 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_3(control_thread); // Start Servo Thread
     Thread thread_4(servo_thread); // Start Servo Thread
     while(1) {
-        Thread::wait(10);
+        wait(1);
     }
 }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp.orig	Mon Apr 13 13:01:02 2015 +0000
@@ -0,0 +1,92 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "Servo.h"
+#include "C12832.h"
+Servo sPan(p21);
+Servo sTilt(p22);
+Serial pc(USBTX, USBRX);
+Servo sVert(p23);
+Sonar distance(p24);
+Mutex mutexIn;
+Mutex mutexOut;
+ 
+AnalogIn p1(p19);
+AnalogIn p2(p20);
+
+// Global variables
+float corHoriz; // horizontal variable arrives from OpenCV
+float corVert; // vertical variable arrives from OpenCV
+float distance; // distance measured by the sonar
+float outVert; // rr
+float outTilt;
+float 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.
+    @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);
+    }
+}
+ 
+/* Thread 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();
+        // 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.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);
+    }
+}
+
+/* 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();
+        float differ;
+        differ = exp(corVert + sonar * outTilt) / (1 + exp(corVert + sonar * outTilt));
+        if (corVert > .5) { // check if face is below the half of the camera view
+            if (outTilt > .5) { // check if lamp head is facing down
+                // moves lamp down by the fraction of the difference from the middle
+        mutexIn.unlock();
+        Thread::wait(25);
+    }
+}
+
+/* Thread Servo 4 - 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();
+        sTi1t = outTilt;
+        sPan = outHoriz;
+        sVert = outVert;
+        mutexOut.unlock();
+        Thread::wait(200);
+    }
+}
+
+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
+    while(1) {
+        Thread::wait(10);
+    }
+}