Diff: main.cpp

Revision:

17:4730277de7c2

Parent:

16:74733a28eb80

Child:

18:974430ee2fbb

Child:

20:5dfebb54cdbd

--- a/main.cpp	Thu Apr 16 16:16:10 2015 +0000
+++ b/main.cpp	Mon Apr 20 12:02:45 2015 +0000
@@ -7,17 +7,19 @@
 Servo panServo(p25);
 Serial pc(USBTX, USBRX);
 Servo vertServo(p23);
+
 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; // distance from the sonar sensor
+float corHoriz = 0.5; // horizontal variable arrives from OpenCV
+float corVert = 0.5; // vertical variable arrives from OpenCV
+float distance = 0.5; // distance from the sonar sensor
 float outVert; // output to vertical servo
 float outTilt; // output to tilt servo
 float outHoriz; // output to horizontal servo
+float differ; // temporary global. Can be local.
 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.
@@ -37,7 +39,7 @@
         // 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("Sonar: %0.3f, OutTilt: %0.3f", sonar.read(), outTilt);
+        lcd.printf("differ: %0.3f, OutTilt: %0.3f", differ, outTilt);
         lcd.locate(0,20);    // the location where you want your charater to be displayed
         lcd.printf("Vert: %0.3f, OutVert: %0.3f", corVert, outVert);
         Thread::wait(250);
@@ -50,22 +52,23 @@
 {
     while (true) {
         mutexIn.lock();
-        float differ;
-        differ = exp(corVert + sonar * outTilt) / (1 + exp(corVert + sonar * outTilt));
-        if (0.3 < corVert < 0.7) { // if corVert is valid (between 0 - 1) then do movements                
+        //float differ;
+        differ = exp(corVert + sonar * (outTilt / 20)) / (1 + exp(corVert + sonar * (outTilt / 20)));
+        if (corVert > 0.1 && corVert < 0.9) { // if corVert is valid (between 0 - 1) then do movements                
             // moves lamp down by the fraction of the difference from the middle
-            outVert = outVert + ((outVert - .5) * differ);
+            outVert = corVert * (1 - differ);
             // tilt down by the fraction of the difference from the middle
-            outTilt = outTilt + ((outVert - .5) * (1 - differ));  
+            outTilt = corVert * 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;
+            outTilt = corVert * 1.1; */
         } else { // Else this is the case when there is no input from the OpenCV
             outVert = .5;
+            outTilt = corVert * differ;
             // TODO Pan search code here. (Searching personality.)
         }
         mutexIn.unlock();