The FollowMeBot follows the user based on the color of his or her shirt. The device hosts a webcam on a servo to find the object and orient the robot. The color is chosen through the user interface with push buttons. Currently, the MATLAB code supports red and green detection. The purpose of FollowMeBot is to be able to follow the user in order to be helpful for daily tasks.

Dependencies:   Rectangle Servo TextLCD mbed

Fork of FollowMeBot by richard choy

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;
+    }
+}