Diff: main.cpp

Revision:

8:fe434a018d96

Parent:

7:24d62ef1ed34

Child:

10:ca6f2769964e

Child:

14:f6d4980c48d6

--- a/main.cpp	Mon Mar 23 18:07:16 2015 +0000
+++ b/main.cpp	Mon Apr 13 12:43:43 2015 +0000
@@ -2,23 +2,24 @@
 #include "rtos.h"
 #include "Servo.h"
 #include "C12832.h"
-//Servo sPan(p21);
+Servo sPan(p21);
 Servo sTilt(p22);
 Serial pc(USBTX, USBRX);
-Servo sVert(p21); // temporary assigned 21 for testing
+Servo sVert(p23);
+Sonar distance(p24);
 Mutex mutexIn;
 Mutex mutexOut;
  
 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; // 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.
@@ -57,14 +58,11 @@
 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;
+        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 (outVert > .5) { // check if lamp head is facing down
-                sVert = ; // moves lamp down by the fraction of the difference from the middle
+            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);
     }
@@ -75,8 +73,9 @@
 void servo_thread(void const *args) {
     while (true) {
         mutexOut.lock();
-//        s1 = outVert;
-//        s2 = outHoriz;
+        sTi1t = outTilt;
+        sPan = outHoriz;
+        sVert = outVert;
         mutexOut.unlock();
         Thread::wait(200);
     }