Diff: main.cpp

Revision:

22:5f6df7ae19a3

Parent:

21:32270b453137

Child:

23:f098ffcd192a

--- a/main.cpp	Mon Apr 20 14:07:31 2015 +0000
+++ b/main.cpp	Thu Apr 23 10:40:09 2015 +0000
@@ -21,7 +21,7 @@
 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.
+float search = .9; // set current position of pan servo to find the movement direction
 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.
@@ -29,7 +29,7 @@
 void serial_thread(void const *args)
 {
     while (true) {
-        pc.scanf("%f,%f", &corHoriz, &corVert1);// read from serial port the data
+        pc.scanf("%f,%f", &corHoriz, &corVert1);// read from serial port the data // temporary Vert1
     }
 }
 
@@ -41,7 +41,7 @@
         // Display values on the LCD screen
         lcd.cls();          // clear the display
         lcd.locate(0,1);    // the location where you want your charater to be displayed
-        lcd.printf("differ: %0.3f, OutTilt: %0.3f", differ, outTilt);
+        lcd.printf("Sonar: %0.3f, OutTilt: %0.3f", sonar.read(), outTilt);
         lcd.locate(0,9);    // the location where you want your charater to be displayed
         lcd.printf("Vert: %0.3f, OutVert: %0.3f", corVert.read(), outVert);
         //lcd.locate(0,18);    // the location where you want your charater to be displayed
@@ -56,24 +56,26 @@
 {
     while (true) {
         mutexIn.lock();
-        //float differ;
-        differ = ((exp(corVert + sonar * outTilt) / (1 + exp(corVert + sonar * outTilt))) - .49 ) * 2.5;
         if (corVert > 0 && corVert < 1) { // if corVert is valid (between 0 - 1) then do movements                
             // moves lamp down by the fraction of the difference from the middle
-            outVert = (corVert * 2 * (1 - differ));
-            // tilt down by the fraction of the difference from the middle
-            outTilt = (corVert * differ) + .15;
-        /*} 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) {
+            if (corVert >= .5) {
+                outVert = corVert - ((corVert - .5) * sonar);
+            } else {
+                outVert = corVert + ((.5 - corVert) * sonar);
+            }
+            outTilt = corVert;
+            outHoriz = corHoriz;
+        } else { // Else this is the case when there is no input from the OpenCV
             outVert = 1;
-            outTilt = corVert * 1.1; */
-        } else { // Else this is the case when there is no input from the OpenCV
-            outVert = corVert;
             outTilt = corVert;
             // TODO Pan search code here. (Searching personality.)
+            /*if (search < outHoriz && search < .9) {
+                outHoriz = outHoriz + .05;
+                search = search + .05;
+            } else if (search > outHoriz && search > .1) {
+                outHoriz = 
+                search = 
+            }*/
         }
         mutexIn.unlock();
         Thread::wait(25);