Diff: main.cpp

Revision:

20:5dfebb54cdbd

Parent:

17:4730277de7c2

Child:

21:32270b453137

--- a/main.cpp	Mon Apr 20 12:02:45 2015 +0000
+++ b/main.cpp	Mon Apr 20 13:34:28 2015 +0000
@@ -9,12 +9,14 @@
 Servo vertServo(p23);
 
 AnalogIn sonar(p19); // temporary changed to potmeter
+AnalogIn corVert(p20); // temporary
 Mutex mutexIn;
 Mutex mutexOut;
 
 // Global variables
 float corHoriz = 0.5; // horizontal variable arrives from OpenCV
-float corVert = 0.5; // vertical variable arrives from OpenCV
+//float corVert = 0.5; // vertical variable arrives from OpenCV
+float corVert1 = 0.5; // temporary
 float distance = 0.5; // distance from the sonar sensor
 float outVert; // output to vertical servo
 float outTilt; // output to tilt servo
@@ -27,7 +29,7 @@
 void serial_thread(void const *args)
 {
     while (true) {
-        pc.scanf("%f,%f", &corHoriz, &corVert);// read from serial port the data
+        pc.scanf("%f,%f", &corHoriz, &corVert1);// read from serial port the data
     }
 }
 
@@ -38,10 +40,12 @@
     while (true) {
         // 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.locate(0,1);    // the location where you want your charater to be displayed
         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);
+        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
+        //lcd.printf("Sonar: %0.3f", sonar.read());
         Thread::wait(250);
     }
 }
@@ -53,13 +57,13 @@
     while (true) {
         mutexIn.lock();
         //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                
+        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 * (1 - differ);
+            outVert = (corVert * (1 - differ)) + .5;
             // tilt down by the fraction of the difference from the middle
-            outTilt = corVert * differ;  /*
-        } else if (0 < corVert < 0.3) {
+            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)
@@ -67,8 +71,8 @@
             outVert = 1;
             outTilt = corVert * 1.1; */
         } else { // Else this is the case when there is no input from the OpenCV
-            outVert = .5;
-            outTilt = corVert * differ;
+            outVert = corVert;
+            outTilt = corVert;
             // TODO Pan search code here. (Searching personality.)
         }
         mutexIn.unlock();