Diff: main.cpp

Revision:

16:74733a28eb80

Parent:

15:4ef3d9835b13

Child:

17:4730277de7c2

--- a/main.cpp	Mon Apr 13 13:19:24 2015 +0000
+++ b/main.cpp	Thu Apr 16 16:16:10 2015 +0000
@@ -3,22 +3,22 @@
 #include "Servo.h"
 #include "C12832.h"
 
-Servo tiltServo(p21);
-Servo panServo(p22);
+Servo tiltServo(p24);
+Servo panServo(p25);
 Serial pc(USBTX, USBRX);
 Servo vertServo(p23);
-AnalogIn sonar(p19);
+AnalogIn sonar(p19); // temporary changed to potmeter
 Mutex mutexIn;
 Mutex mutexOut;
 
 // Global variables
 float corHoriz = 0; // horizontal variable arrives from OpenCV
 float corVert = 0; // vertical variable arrives from OpenCV
-float distance = 0;
-float outVert; // 
-float outTilt;
-float outHoriz;
-C12832 lcd(p5, p7, p6, p8, p11);// lcd is an object from class c12832 initialised by p5,p7....
+float distance = 0; // distance from the sonar sensor
+float outVert; // output to vertical servo
+float outTilt; // output to tilt servo
+float outHoriz; // output to horizontal servo
+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 */
@@ -37,9 +37,9 @@
         // 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: %0.3f", corHoriz);
+        lcd.printf("Sonar: %0.3f, OutTilt: %0.3f", sonar.read(), outTilt);
         lcd.locate(0,20);    // the location where you want your charater to be displayed
-        lcd.printf("Ver: %0.3f", corVert);
+        lcd.printf("Vert: %0.3f, OutVert: %0.3f", corVert, outVert);
         Thread::wait(250);
     }
 }
@@ -52,23 +52,25 @@
         mutexIn.lock();
         float differ;
         differ = exp(corVert + sonar * outTilt) / (1 + exp(corVert + sonar * outTilt));
-        if (corVert > .45) { // check if face is below the half of the camera view
-            if (outTilt > .45) { // check if lamp head is facing down
+        if (0.3 < corVert < 0.7) { // if corVert is valid (between 0 - 1) then do movements                
             // moves lamp down by the fraction of the difference from the middle
-                outVert = outVert + 
-                outTilt = outTilt + 
-            } else if (outTilt < .55) {  // check if lamp head is facing up
-                //
-            }           
-        } else if (corVert < .55) {
-           // moves lamp up by the fraction of the difference from the middle
+            outVert = outVert + ((outVert - .5) * differ);
+            // tilt down by the fraction of the difference from the middle
+            outTilt = outTilt + ((outVert - .5) * (1 - differ));  
+        } else if (0 < corVert < 0.3) {
+            outVert = 0;
+            outTilt = corVert * .9; // Follow the low movement only with the tilt servo. 
+                                           // (.9 is the correction of the side of the screen at OpenCV)
+        } else if (corVert > 0.7 && corVert < 1) {
+            outVert = 1;
+            outTilt = corVert * 1.1;
+        } else { // Else this is the case when there is no input from the OpenCV
+            outVert = .5;
+            // TODO Pan search code here. (Searching personality.)
         }
         mutexIn.unlock();
         Thread::wait(25);
     }
-    mutexIn.unlock();
-    Thread::wait(25);
-}
 }
 
 /* Thread Servo 4 - handles the output data from the control thread, and pass to the servo.