Diff: main.cpp

Revision:

4:aa21c6609858

Parent:

3:de12b39ad805

Child:

5:5fe2f8603be8

--- a/main.cpp	Mon Nov 25 19:55:12 2013 +0000
+++ b/main.cpp	Mon Dec 02 17:20:53 2013 +0000
@@ -8,7 +8,7 @@
 #define MAX_VIEW_X 1600 // maximum X value input from camera
 #define MAX_VIEW_Y 1200 // maximum Y value input from camera
 #define CENTER_BOX_TOLLERANCE 200 // Size of our box
-#define TO_SERVO_DIVISOR 3000.0 // Value to divide by to get the amount to move the servo by
+#define TO_SERVO_DIVISOR 5000.0 // Value to divide by to get the amount to move the servo by
 #define NO_COLOR_MAX_COUNT 10
 #define COLLISION_DIST .4
 #define BLOB_CLOSE_DIST 200
@@ -34,7 +34,7 @@
 Rectangle centerBox((MAX_VIEW_X/2)-CENTER_BOX_TOLLERANCE, (MAX_VIEW_Y/2)-CENTER_BOX_TOLLERANCE,
                     (MAX_VIEW_X/2)+CENTER_BOX_TOLLERANCE,(MAX_VIEW_Y/2)+CENTER_BOX_TOLLERANCE); // Creates a box to examine if the camera is well enough centered
 int noColorCounter; // Counts how long it has been since we have seen a color to follow
-bool colorLost; // boolean to represent if the color is confirmed "lost" (aka noColorCounter > NO_COLOR_MAX_COUNT)
+bool colorLost = true; // boolean to represent if the color is confirmed "lost" (aka noColorCounter > NO_COLOR_MAX_COUNT)
 
 // Function Prototypes
 void getXYpos();
@@ -44,8 +44,9 @@
 
 int main()
 {
-    //followMeBot.start();
-    //wait(1);
+    followMeBot.start();
+    servoHor = .5;
+
     while(1) {
         getXYpos();
         moveBot();
@@ -62,7 +63,7 @@
     float temp = 0;
     if(xpos == 0) { // If we recieve a 0 for the location
         if(!colorLost && (++noColorCounter > NO_COLOR_MAX_COUNT)) { // Check to see if we have seen enough to consider the color lost
-            servoHor = .5; // If the color is lost, return servo to home
+//            servoHor = .5; // If the color is lost, return servo to home
             colorLost = true; // Set colorLost to true
             noColorCounter = 0; // Reset counter
         }
@@ -72,6 +73,7 @@
         temp = servoHor.read() - (centerBox.getCenterX() - xpos)/TO_SERVO_DIVISOR; // Calculate location to move servo to
         if(temp > 0 && temp <= 1) { // If the value is within the servo range
             servoHor = temp; // Set the servo equal to the position
+
             //sprintf(serial_rx_buffer, "%f\n", temp);
         }
         /*temp = servoVer.read() + ((float)centerBox.getCenterY() - (float)ypos)/TO_SERVO_DIVISOR;
@@ -80,6 +82,11 @@
         }*/
     }
     //pc.puts(serial_rx_buffer);
+
+    lcd.cls();
+    lcd.locate(0,0);
+    lcd.printf("%d;%d;%f", xpos, ypos, servoHor.read());
+
     return temp; // return the servo position
 }
 
@@ -98,9 +105,7 @@
         ypos = atoi(temp);
         temp = strtok(NULL, &del);
         blobArea = atoi(temp);
-        lcd.cls();
-        lcd.locate(0,0);
-        lcd.printf("%d;%d", xpos, ypos);
+
         moveCamera(); // Move the camera
     } else {
         myled = 0;
@@ -110,26 +115,39 @@
 void moveBot()
 {
     float irVal = irSensorFront;
+
+    //
+    // colorLost = false;
+
     lcd.locate(0,1);
     if(!colorLost) {
-        if(irVal > COLLISION_DIST && blobArea > BLOB_CLOSE_DIST) {
-            //followMeBot.stop();
-            lcd.printf("stop");
-        } else if(servoIsInHome() > 0) {
-            //followMeBot.right();
+        // if(irVal > COLLISION_DIST && blobArea > BLOB_CLOSE_DIST) {
+        //followMeBot.stop();
+        //  lcd.printf("stop");
+        //} else if(servoIsInHome() > 0) {
+        if(servoIsInHome() > 0) {
+        //if(servoHor.read() > .7) {
+            //servoHor = servoHor - .1;
             lcd.printf("right");
-        } else if(servoIsInHome() < 0) {
-            //followMeBot.left();
+            followMeBot.right();
+           } else if(servoIsInHome() < 0) {
+        //} else if(servoHor.read() < .3) {
+
+            //servoHor = servoHor + .1;
             lcd.printf("left");
-        } else {
+            followMeBot.left();
+
+        } else if(servoIsInHome() == 0) {
             //followMeBot.changeSpeed(SPEED_CONST/(blobArea/100));
             //followMeBot.forward();
+            followMeBot.stop();
             lcd.printf("for sp: %d", (SPEED_CONST/(blobArea/100)));
         }
+    } else {
+        lcd.printf("Color Lost");
+        followMeBot.right();
     }
-    else{
-        lcd.printf("Color Lost");
-    }
+
 }
 
 int servoIsInHome()