Diff: main.cpp

Revision:

27:b1653e9bc81c

Parent:

26:71288f42dbc6

Child:

28:e04fb7a2a51e

--- a/main.cpp	Thu Apr 23 14:46:50 2015 +0000
+++ b/main.cpp	Thu Apr 23 16:50:27 2015 +0000
@@ -15,8 +15,8 @@
 //AnalogIn corVert(p20); // TO REMOVE. Temporary changed to potmeter
 
 // Global variables
-float corHoriz = 0.5; // horizontal variable arrives from OpenCV
-float corVert = 0.5; // vertical variable arrives from OpenCV
+float corHoriz = -1; // horizontal variable arrives from OpenCV
+float corVert = -1; // vertical variable arrives from OpenCV
 float distance = 0.5;// variable holds the distance in meters 0 to 3.3
 float norm=0;      // variable holds the normalised values form the sonar sensor
 float outVert; // output to vertical servo
@@ -30,7 +30,11 @@
     retreating
 } LampState;
 
-LampState lamp;
+LampState currentTask = retreating;
+
+time_t lastSearchMovementTime = time(NULL);
+time_t lastVisionEvent = time(NULL);
+time_t initialVisionTime = lastVisionEvent;
 
 /* parallax ultrasound range finder
 p21 pin the range finder is connected to.
@@ -70,11 +74,12 @@
 
         // lcd.cls();          // clear the display
         lcd.locate(0,0);    // the location where you want your charater to be displayed
-        lcd.printf("differ: %0.3f, OutTilt: %0.3f", differ, outTilt);
+        lcd.printf("outTilt: %0.3f", outTilt);
 
         // lcd.cls();          // clear the display
         lcd.locate(0,10);    // the location where you want your charater to be displayed
-        lcd.printf("Vert: %0.3f, OutVert: %0.3f", corVert, outVert);
+        //lcd.printf("Vert: %0.3f, OutVert: %0.3f", corVert, outVert);
+        lcd.printf("OutHoriz: %0.3f", outHoriz);
 
         //lcd.cls();          // clear the display
         lcd.locate(0,20);    // the location where you want your charater to be displayed
@@ -85,32 +90,64 @@
     }
 }
 
+void followPerson() {
+    // moves lamp down by the fraction of the difference from the middle
+    if (corVert >= .5) {
+        outVert = corVert - ((corVert - .5) * sonar);
+    } else {
+        outVert = corVert + ((.5 - corVert) * sonar);
+    }
+    outTilt = corVert;
+    outHoriz = corHoriz;
+}
+
+void searchPerson() {
+    time_t currentTime = time(NULL);
+    if((currentTime - lastSearchMovementTime) > 10) {
+        if(outHoriz < 0.5) {
+            outHoriz = 0.8;
+        } else {
+            outHoriz = 0.3;
+        }
+        lastSearchMovementTime = currentTime;
+    }
+}
+
+void shooPerson() {
+    outVert = 0.1;
+    outTilt = 0.1;
+    Thread::wait(5000);
+    outTilt = 1;
+    Thread::wait(5000);
+    outTilt = 0.1;
+    Thread::wait(5000);
+    outTilt = 1;
+}
+
 /* Thread Control 3 - handles the input data from the sonar sensor, and display on the LCD screen.
     @update inData */
 void control_thread(void const *args)
 {
     while (true) {
         mutexIn.lock();
-        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
-            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;
-            // 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 =
-            }*/
+        time_t currentTime = time(NULL);
+        if((currentTime - lastVisionEvent) > 200) {
+            currentTask = searching;
+        } else if(distance <= 0.5) {
+            currentTask = retreating;
+        } else {
+            currentTask = tracking;
+        }
+        switch(currentTask) {
+            case tracking:
+                followPerson();
+                break;   
+            case searching:
+                searchPerson();
+                break;
+            case retreating:
+                shooPerson();
+                break;   
         }
         mutexIn.unlock();
         Thread::wait(250);