Revision 29:e8f7b891b5c1, committed 2015-04-27

Comitter:

Argensis

Date:

Mon Apr 27 12:55:56 2015 +0000

Parent:

28:e04fb7a2a51e

Commit message:

Adjusted limits so servos work and adjusted states

Changed in this revision

--- a/main.cpp	Fri Apr 24 10:29:27 2015 +0000
+++ b/main.cpp	Mon Apr 27 12:55:56 2015 +0000
@@ -10,12 +10,10 @@
 Mutex mutexIn;// protect global variables
 Mutex mutexOut;// protect global variables
 Mutex mutex_sonar;
-AnalogIn sonar(p17); // correct is p17. TEMPORARY changed to potmeter p19
-//AnalogIn corVert(p20); // TO REMOVE. Temporary changed to potmeter
 
 // Global variables
-float corHoriz = -1; // horizontal variable arrives from OpenCV
-float corVert = -1; // vertical variable arrives from OpenCV
+float cvHoriz = -1; // horizontal variable arrives from OpenCV
+float cvVert = -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
@@ -29,7 +27,7 @@
     retreating
 } LampState;
 
-LampState currentTask = retreating;
+LampState currentTask = searching;
 
 time_t lastSearchMovementTime = time(NULL);
 time_t lastVisionEvent = time(NULL);
@@ -52,11 +50,11 @@
         //   mutexOut.lock();
 
         //if not range finding
-        //mutex_sonar.lock();
+        mutex_sonar.lock();
         if(pc.readable())
-            pc.scanf("%f,%f", &corHoriz, &corVert);// read from serial port the data
+            pc.scanf("%f,%f", &cvHoriz, &cvVert);// read from serial port the data
 
-        //mutex_sonar.unlock();
+        mutex_sonar.unlock();
         //mutexIn.unlock();
         //mutexOut.unlock();
         Thread::wait(1000);
@@ -73,12 +71,26 @@
 
         // lcd.cls();          // clear the display
         lcd.locate(0,0);    // the location where you want your charater to be displayed
-        lcd.printf("tilt: %0.3f, pan: %0.3f", tiltServo.read(), panServo.read());
+        lcd.printf("tilt: %0.3f, dsrd: %0.3f", tiltServo.read(), 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", vertServo.read());
+        //lcd.printf("Vert: %0.3f, OutVert: %0.3f", cvVert, outVert);
+        //lcd.printf("vert: %0.3f", vertServo.read());
+        switch(currentTask) {
+            case retreating:
+                lcd.printf("retreating");
+                break;
+            case tracking:
+                lcd.printf("tracking");
+                break;
+            case searching:
+                lcd.printf("searching");
+                break;
+            default:
+                lcd.printf("Invalid Mode");
+                break;
+        }
 
         //lcd.cls();          // clear the display
         lcd.locate(0,20);    // the location where you want your charater to be displayed
@@ -91,18 +103,18 @@
 
 void followPerson() {
     // moves lamp down by the fraction of the difference from the middle
-    if (corVert >= .5) {
-        outVert = corVert - ((corVert - .5) * sonar);
+    if (cvVert >= .5) {
+        outVert = cvVert - ((cvVert - .5) * norm);
     } else {
-        outVert = corVert + ((.5 - corVert) * sonar);
+        outVert = cvVert + ((.5 - cvVert) * norm);
     }
-    outTilt = corVert;
-    outHoriz = corHoriz;
+    outTilt = cvVert;
+    outHoriz = cvHoriz;
 }
 
 void searchPerson() {
     time_t currentTime = time(NULL);
-    if((currentTime - lastSearchMovementTime) > 10) {
+    if((currentTime - lastSearchMovementTime) > 2) {
         if(outHoriz < 0.5) {
             outHoriz = 0.8;
         } else {
@@ -113,13 +125,14 @@
 }
 
 void shooPerson() {
+    outHoriz = 0.5;
     outVert = 0.2;
     outTilt = 0.2;
-    Thread::wait(5000);
+    Thread::wait(250);
     outTilt = 0.8;
-    Thread::wait(5000);
+    Thread::wait(250);
     outTilt = 0.2;
-    Thread::wait(5000);
+    Thread::wait(250);
     outTilt = 0.8;
 }
 
@@ -130,12 +143,12 @@
     while (true) {
         mutexIn.lock();
         time_t currentTime = time(NULL);
-        if((currentTime - lastVisionEvent) > 200) {
+        if(distance <= 0.5) {
+            currentTask = retreating;
+        } else if((currentTime - lastVisionEvent) < 200) {
+            currentTask = tracking;
+        } else {
             currentTask = searching;
-        } else if(distance <= 0.5) {
-            currentTask = retreating;
-        } else {
-            currentTask = tracking;
         }
         switch(currentTask) {
             case tracking:
@@ -149,7 +162,7 @@
                 break;   
         }
         mutexIn.unlock();
-        Thread::wait(250);
+        Thread::wait(50);
     }
 }
 
@@ -170,9 +183,9 @@
     while (true) {
         mutexOut.lock();
         
-        tiltServo = clamp(0.01, 0.1, outTilt);
-        panServo = clamp(0.01, 0.1, outHoriz);
-        vertServo = clamp(0.01, 0.1, outVert);
+        tiltServo = clamp(0.055, 0.09, outTilt);
+        panServo = clamp(0.03, 0.12, outHoriz);
+        vertServo = clamp(0.0, 0.1, outVert);
         mutexOut.unlock();
         Thread::wait(250);
     }