Diff: main.cpp

Revision:

5:5fe2f8603be8

Parent:

4:aa21c6609858

Child:

6:e97ac3fb8ac5

--- a/main.cpp	Mon Dec 02 17:20:53 2013 +0000
+++ b/main.cpp	Fri Dec 06 16:59:07 2013 +0000
@@ -5,16 +5,18 @@
 #include "TextLCD.h"
 
 // Macros/Constants
-#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 5000.0 // Value to divide by to get the amount to move the servo by
+#define MAX_VIEW_X 640 // maximum X value input from camera
+#define MAX_VIEW_Y 480 // maximum Y value input from camera
+//#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 100 // Size of our box
+#define TO_SERVO_DIVISOR 4000.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
+#define COLLISION_DIST .37
+#define BLOB_CLOSE_DIST 120000
 #define SERVO_HOME .5
 #define SERVO_HOME_TOL .2
-#define SPEED_CONST 65535
+#define SPEED_CONST 65535 // Used with the blob area to determine how fast the robot should move toward the target
 
 // Hardware sensors and devices
 DigitalOut myled(LED1);
@@ -28,6 +30,10 @@
 Serial pc(USBTX, USBRX); // tx, rx
 TextLCD lcd(p14, p16, p17, p18, p19, p20); // rs, e, d4-d7
 
+//float irVal = irSensorFront;
+
+float irVal;
+
 // Software variables
 char serial_rx_buffer[256]; // Input buffer for data from the PC
 int xpos, ypos, blobArea; // x and y positions read from matlab
@@ -60,6 +66,9 @@
 */
 float moveCamera()
 {
+    /*lcd.cls();
+    lcd.locate(0,0);
+  lcd.printf("%d;%d;%f", xpos, ypos, servoHor.read());*/
     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
@@ -71,8 +80,12 @@
         noColorCounter = 0; // Reset counter
         colorLost = false; // We have found the color!
         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
+            // lcd.locate(0,0);
+            // lcd.printf("hello\n");
 
             //sprintf(serial_rx_buffer, "%f\n", temp);
         }
@@ -83,10 +96,6 @@
     }
     //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,23 +107,28 @@
     if(pc.readable()) { // See if matlab has data for us
         myled = 1;
         pc.gets(serial_rx_buffer, 256); // Get position data
-        pc.puts(serial_rx_buffer);
+        //pc.puts(serial_rx_buffer);
         temp = strtok(serial_rx_buffer, &del);
         xpos = atoi(temp); // Convert data to xposition int
         temp = strtok(NULL, &del);
         ypos = atoi(temp);
         temp = strtok(NULL, &del);
         blobArea = atoi(temp);
-
         moveCamera(); // Move the camera
     } else {
         myled = 0;
     }
+
+  /*  float irVal = irSensorFront;
+    lcd.locate(0,0);
+    lcd.printf("irVal: %f", irVal);
+    lcd.locate(0,1);
+    lcd.printf("blob: %d", blobArea);*/
 }
 
 void moveBot()
 {
-    float irVal = irSensorFront;
+   // irVal = irSensorFront;                aaaaaaaaaaa
 
     //
     // colorLost = false;
@@ -126,26 +140,44 @@
         //  lcd.printf("stop");
         //} else if(servoIsInHome() > 0) {
         if(servoIsInHome() > 0) {
-        //if(servoHor.read() > .7) {
+            //if(servoHor.read() > .7) {
             //servoHor = servoHor - .1;
             lcd.printf("right");
             followMeBot.right();
-           } else if(servoIsInHome() < 0) {
-        //} else if(servoHor.read() < .3) {
+        } else if(servoIsInHome() < 0) {
+            //} else if(servoHor.read() < .3) {
 
             //servoHor = servoHor + .1;
             lcd.printf("left");
             followMeBot.left();
 
-        } else if(servoIsInHome() == 0) {
+        } else if(servoIsInHome() == 0) {//n
+        
+            //if (irVal < COLLISION_DIST) {
+            if (blobArea < 180000) {              //just testing          aaaaaa
+                //lcd.cls();
+                //lcd.printf("forward: %f", irVal);
+                followMeBot.forward();
+            }
+            else {
+                followMeBot.stop();
+                lcd.cls();
+                lcd.locate(0,0);
+                lcd.printf("blobArea: %d ", blobArea);
+                //wait(1);
+               // lcd.printf("stop: %f ", irVal);
+            } 
+            
+            /*
             //followMeBot.changeSpeed(SPEED_CONST/(blobArea/100));
-            //followMeBot.forward();
             followMeBot.stop();
-            lcd.printf("for sp: %d", (SPEED_CONST/(blobArea/100)));
+            lcd.printf("for sp: %d", (SPEED_CONST/(blobArea/100)));*/
         }
     } else {
         lcd.printf("Color Lost");
-        followMeBot.right();
+        followMeBot.left();
+      //  followMeBot.stop();//n
+       // followMeBot.right();
     }
 
 }