Diff: main.cpp

Revision:

3:de12b39ad805

Parent:

2:0b362c662997

Child:

4:aa21c6609858

diff -r 0b362c662997 -r de12b39ad805 main.cpp
--- a/main.cpp	Thu Nov 21 00:02:05 2013 +0000
+++ b/main.cpp	Mon Nov 25 19:55:12 2013 +0000
@@ -2,12 +2,19 @@
 #include "iRobot.h"
 #include "Servo.h"
 #include "Rectangle.h"
+#include "TextLCD.h"
 
 // Macros/Constants
-#define MAX_VIEW_X 1176 // maximum X value input from camera
-#define MAX_VIEW_Y 711 // 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 200 // Size of our box
-#define TO_SERVO_DIVISOR 1176.0 // Value to divide by to get the amount to move the servo by
+#define TO_SERVO_DIVISOR 3000.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 SERVO_HOME .5
+#define SERVO_HOME_TOL .2
+#define SPEED_CONST 65535
 
 // Hardware sensors and devices
 DigitalOut myled(LED1);
@@ -15,73 +22,123 @@
 iRobot followMeBot(p9, p10);
 Servo servoHor(p22);
 Servo servoVer(p21);
-AnalogIn irSensorFront(p20);
-AnalogIn irSensorLeft(p19);
-AnalogIn irSensorRight(p18);
+AnalogIn irSensorFront(p15);
+//AnalogIn irSensorLeft(p19);
+//AnalogIn irSensorRight(p18);
 Serial pc(USBTX, USBRX); // tx, rx
+TextLCD lcd(p14, p16, p17, p18, p19, p20); // rs, e, d4-d7
 
 // Software variables
-char serial_rx_buffer[128]; // Input buffer for data from the PC
-int xpos, ypos; // x and y positions read from matlab
-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
+char serial_rx_buffer[256]; // Input buffer for data from the PC
+int xpos, ypos, blobArea; // x and y positions read from matlab
+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)
 
 // Function Prototypes
 void getXYpos();
-void moveCamera();
+float moveCamera();
+void moveBot();
+int servoIsInHome();
 
 int main()
 {
     //followMeBot.start();
-
+    //wait(1);
     while(1) {
         getXYpos();
-        wait(.5);
+        moveBot();
     }
 }
 
-void moveCamera()
+/**
+*   Moves the servo to move the camera based upon where the
+*   color is located on the reported x and y
+*
+*/
+float moveCamera()
 {
-    if(xpos == 0) {
-        servoHor = .5;
-    } else if(!centerBox.is_touch(xpos, ypos)) {
-        float temp;
-        //printf("Servo at: %f\n", servoHor.read());
-        temp = servoHor.read() - (centerBox.getCenterX() - xpos)/TO_SERVO_DIVISOR;
-        //printf("move servo by: %f\n", (centerBox.getCenterX() - xpos)/TO_SERVO_DIVISOR);
-        if(temp > 0 && temp <= 1) {
-            servoHor = temp;
-          sprintf(serial_rx_buffer, "%f\n", temp);
+    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
+            colorLost = true; // Set colorLost to true
+            noColorCounter = 0; // Reset counter
+        }
+    } else if(!centerBox.is_touch(xpos, (MAX_VIEW_Y/2))) { // If we have a location
+        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
+            //sprintf(serial_rx_buffer, "%f\n", temp);
         }
         /*temp = servoVer.read() + ((float)centerBox.getCenterY() - (float)ypos)/TO_SERVO_DIVISOR;
         if(temp > 0 && temp <= 1) {
             servoVer = temp;
         }*/
     }
-    pc.puts(serial_rx_buffer);
+    //pc.puts(serial_rx_buffer);
+    return temp; // return the servo position
 }
 
+// Will return the number of degrees to turn the irobot by
 void getXYpos()
 {
-    //char * temp;
-    //const char del = ';';
-
-    if(pc.readable()) {
+    char * temp;
+    const char del = ';';
+    if(pc.readable()) { // See if matlab has data for us
         myled = 1;
-        pc.gets(serial_rx_buffer, 128);
-        xpos = atoi(serial_rx_buffer);
-        //sprintf(serial_rx_buffer, "%d\n", xpos);
-        //pc.puts(serial_rx_buffer);
-        //temp = strtok(serial_rx_buffer, &del);
-        moveCamera();
+        pc.gets(serial_rx_buffer, 256); // Get position data
+        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);
+        lcd.cls();
+        lcd.locate(0,0);
+        lcd.printf("%d;%d", xpos, ypos);
+        moveCamera(); // Move the camera
     } else {
         myled = 0;
     }
-    if(xpos > 500) {
-        myled2 = 1;
-    } else {
-        myled2 = 0;
+}
+
+void moveBot()
+{
+    float irVal = irSensorFront;
+    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();
+            lcd.printf("right");
+        } else if(servoIsInHome() < 0) {
+            //followMeBot.left();
+            lcd.printf("left");
+        } else {
+            //followMeBot.changeSpeed(SPEED_CONST/(blobArea/100));
+            //followMeBot.forward();
+            lcd.printf("for sp: %d", (SPEED_CONST/(blobArea/100)));
+        }
+    }
+    else{
+        lcd.printf("Color Lost");
     }
 }
 
-
+int servoIsInHome()
+{
+    if(servoHor.read() > SERVO_HOME + SERVO_HOME_TOL) {
+        return 1;
+    } else if(servoHor.read() < SERVO_HOME - SERVO_HOME_TOL) {
+        return -1;
+    } else {
+        return 0;
+    }
+}