Rahul Shetty / Mbed 2 deprecated FollowMeBot3

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

main.cpp@3:de12b39ad805, 2013-11-25 (annotated)

Committer:
dhamilton31
Date:
Mon Nov 25 19:55:12 2013 +0000
Revision:
3:de12b39ad805
Parent:
2:0b362c662997
Child:
4:aa21c6609858
Added additional servo control and printing commands to lcd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dhamilton31 0:3ed56271dd2d 1 #include "mbed.h"
dhamilton31 0:3ed56271dd2d 2 #include "iRobot.h"
dhamilton31 0:3ed56271dd2d 3 #include "Servo.h"
dhamilton31 1:6c399fc35deb 4 #include "Rectangle.h"
dhamilton31 3:de12b39ad805 5 #include "TextLCD.h"
dhamilton31 0:3ed56271dd2d 6
dhamilton31 1:6c399fc35deb 7 // Macros/Constants
dhamilton31 3:de12b39ad805 8 #define MAX_VIEW_X 1600 // maximum X value input from camera
dhamilton31 3:de12b39ad805 9 #define MAX_VIEW_Y 1200 // maximum Y value input from camera
dhamilton31 2:0b362c662997 10 #define CENTER_BOX_TOLLERANCE 200 // Size of our box
dhamilton31 3:de12b39ad805 11 #define TO_SERVO_DIVISOR 3000.0 // Value to divide by to get the amount to move the servo by
dhamilton31 3:de12b39ad805 12 #define NO_COLOR_MAX_COUNT 10
dhamilton31 3:de12b39ad805 13 #define COLLISION_DIST .4
dhamilton31 3:de12b39ad805 14 #define BLOB_CLOSE_DIST 200
dhamilton31 3:de12b39ad805 15 #define SERVO_HOME .5
dhamilton31 3:de12b39ad805 16 #define SERVO_HOME_TOL .2
dhamilton31 3:de12b39ad805 17 #define SPEED_CONST 65535
dhamilton31 0:3ed56271dd2d 18
dhamilton31 1:6c399fc35deb 19 // Hardware sensors and devices
dhamilton31 1:6c399fc35deb 20 DigitalOut myled(LED1);
dhamilton31 2:0b362c662997 21 DigitalOut myled2(LED2);
dhamilton31 1:6c399fc35deb 22 iRobot followMeBot(p9, p10);
dhamilton31 1:6c399fc35deb 23 Servo servoHor(p22);
dhamilton31 1:6c399fc35deb 24 Servo servoVer(p21);
dhamilton31 3:de12b39ad805 25 AnalogIn irSensorFront(p15);
dhamilton31 3:de12b39ad805 26 //AnalogIn irSensorLeft(p19);
dhamilton31 3:de12b39ad805 27 //AnalogIn irSensorRight(p18);
dhamilton31 1:6c399fc35deb 28 Serial pc(USBTX, USBRX); // tx, rx
dhamilton31 3:de12b39ad805 29 TextLCD lcd(p14, p16, p17, p18, p19, p20); // rs, e, d4-d7
dhamilton31 1:6c399fc35deb 30
dhamilton31 1:6c399fc35deb 31 // Software variables
dhamilton31 3:de12b39ad805 32 char serial_rx_buffer[256]; // Input buffer for data from the PC
dhamilton31 3:de12b39ad805 33 int xpos, ypos, blobArea; // x and y positions read from matlab
dhamilton31 3:de12b39ad805 34 Rectangle centerBox((MAX_VIEW_X/2)-CENTER_BOX_TOLLERANCE, (MAX_VIEW_Y/2)-CENTER_BOX_TOLLERANCE,
dhamilton31 3:de12b39ad805 35 (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
dhamilton31 3:de12b39ad805 36 int noColorCounter; // Counts how long it has been since we have seen a color to follow
dhamilton31 3:de12b39ad805 37 bool colorLost; // boolean to represent if the color is confirmed "lost" (aka noColorCounter > NO_COLOR_MAX_COUNT)
dhamilton31 1:6c399fc35deb 38
dhamilton31 2:0b362c662997 39 // Function Prototypes
dhamilton31 2:0b362c662997 40 void getXYpos();
dhamilton31 3:de12b39ad805 41 float moveCamera();
dhamilton31 3:de12b39ad805 42 void moveBot();
dhamilton31 3:de12b39ad805 43 int servoIsInHome();
dhamilton31 2:0b362c662997 44
dhamilton31 1:6c399fc35deb 45 int main()
dhamilton31 1:6c399fc35deb 46 {
dhamilton31 2:0b362c662997 47 //followMeBot.start();
dhamilton31 3:de12b39ad805 48 //wait(1);
dhamilton31 0:3ed56271dd2d 49 while(1) {
dhamilton31 2:0b362c662997 50 getXYpos();
dhamilton31 3:de12b39ad805 51 moveBot();
dhamilton31 0:3ed56271dd2d 52 }
dhamilton31 0:3ed56271dd2d 53 }
dhamilton31 1:6c399fc35deb 54
dhamilton31 3:de12b39ad805 55 /**
dhamilton31 3:de12b39ad805 56 * Moves the servo to move the camera based upon where the
dhamilton31 3:de12b39ad805 57 * color is located on the reported x and y
dhamilton31 3:de12b39ad805 58 *
dhamilton31 3:de12b39ad805 59 */
dhamilton31 3:de12b39ad805 60 float moveCamera()
dhamilton31 1:6c399fc35deb 61 {
dhamilton31 3:de12b39ad805 62 float temp = 0;
dhamilton31 3:de12b39ad805 63 if(xpos == 0) { // If we recieve a 0 for the location
dhamilton31 3:de12b39ad805 64 if(!colorLost && (++noColorCounter > NO_COLOR_MAX_COUNT)) { // Check to see if we have seen enough to consider the color lost
dhamilton31 3:de12b39ad805 65 servoHor = .5; // If the color is lost, return servo to home
dhamilton31 3:de12b39ad805 66 colorLost = true; // Set colorLost to true
dhamilton31 3:de12b39ad805 67 noColorCounter = 0; // Reset counter
dhamilton31 3:de12b39ad805 68 }
dhamilton31 3:de12b39ad805 69 } else if(!centerBox.is_touch(xpos, (MAX_VIEW_Y/2))) { // If we have a location
dhamilton31 3:de12b39ad805 70 noColorCounter = 0; // Reset counter
dhamilton31 3:de12b39ad805 71 colorLost = false; // We have found the color!
dhamilton31 3:de12b39ad805 72 temp = servoHor.read() - (centerBox.getCenterX() - xpos)/TO_SERVO_DIVISOR; // Calculate location to move servo to
dhamilton31 3:de12b39ad805 73 if(temp > 0 && temp <= 1) { // If the value is within the servo range
dhamilton31 3:de12b39ad805 74 servoHor = temp; // Set the servo equal to the position
dhamilton31 3:de12b39ad805 75 //sprintf(serial_rx_buffer, "%f\n", temp);
dhamilton31 1:6c399fc35deb 76 }
dhamilton31 2:0b362c662997 77 /*temp = servoVer.read() + ((float)centerBox.getCenterY() - (float)ypos)/TO_SERVO_DIVISOR;
dhamilton31 1:6c399fc35deb 78 if(temp > 0 && temp <= 1) {
dhamilton31 1:6c399fc35deb 79 servoVer = temp;
dhamilton31 2:0b362c662997 80 }*/
dhamilton31 2:0b362c662997 81 }
dhamilton31 3:de12b39ad805 82 //pc.puts(serial_rx_buffer);
dhamilton31 3:de12b39ad805 83 return temp; // return the servo position
dhamilton31 2:0b362c662997 84 }
dhamilton31 2:0b362c662997 85
dhamilton31 3:de12b39ad805 86 // Will return the number of degrees to turn the irobot by
dhamilton31 2:0b362c662997 87 void getXYpos()
dhamilton31 2:0b362c662997 88 {
dhamilton31 3:de12b39ad805 89 char * temp;
dhamilton31 3:de12b39ad805 90 const char del = ';';
dhamilton31 3:de12b39ad805 91 if(pc.readable()) { // See if matlab has data for us
dhamilton31 2:0b362c662997 92 myled = 1;
dhamilton31 3:de12b39ad805 93 pc.gets(serial_rx_buffer, 256); // Get position data
dhamilton31 3:de12b39ad805 94 pc.puts(serial_rx_buffer);
dhamilton31 3:de12b39ad805 95 temp = strtok(serial_rx_buffer, &del);
dhamilton31 3:de12b39ad805 96 xpos = atoi(temp); // Convert data to xposition int
dhamilton31 3:de12b39ad805 97 temp = strtok(NULL, &del);
dhamilton31 3:de12b39ad805 98 ypos = atoi(temp);
dhamilton31 3:de12b39ad805 99 temp = strtok(NULL, &del);
dhamilton31 3:de12b39ad805 100 blobArea = atoi(temp);
dhamilton31 3:de12b39ad805 101 lcd.cls();
dhamilton31 3:de12b39ad805 102 lcd.locate(0,0);
dhamilton31 3:de12b39ad805 103 lcd.printf("%d;%d", xpos, ypos);
dhamilton31 3:de12b39ad805 104 moveCamera(); // Move the camera
dhamilton31 2:0b362c662997 105 } else {
dhamilton31 2:0b362c662997 106 myled = 0;
dhamilton31 2:0b362c662997 107 }
dhamilton31 3:de12b39ad805 108 }
dhamilton31 3:de12b39ad805 109
dhamilton31 3:de12b39ad805 110 void moveBot()
dhamilton31 3:de12b39ad805 111 {
dhamilton31 3:de12b39ad805 112 float irVal = irSensorFront;
dhamilton31 3:de12b39ad805 113 lcd.locate(0,1);
dhamilton31 3:de12b39ad805 114 if(!colorLost) {
dhamilton31 3:de12b39ad805 115 if(irVal > COLLISION_DIST && blobArea > BLOB_CLOSE_DIST) {
dhamilton31 3:de12b39ad805 116 //followMeBot.stop();
dhamilton31 3:de12b39ad805 117 lcd.printf("stop");
dhamilton31 3:de12b39ad805 118 } else if(servoIsInHome() > 0) {
dhamilton31 3:de12b39ad805 119 //followMeBot.right();
dhamilton31 3:de12b39ad805 120 lcd.printf("right");
dhamilton31 3:de12b39ad805 121 } else if(servoIsInHome() < 0) {
dhamilton31 3:de12b39ad805 122 //followMeBot.left();
dhamilton31 3:de12b39ad805 123 lcd.printf("left");
dhamilton31 3:de12b39ad805 124 } else {
dhamilton31 3:de12b39ad805 125 //followMeBot.changeSpeed(SPEED_CONST/(blobArea/100));
dhamilton31 3:de12b39ad805 126 //followMeBot.forward();
dhamilton31 3:de12b39ad805 127 lcd.printf("for sp: %d", (SPEED_CONST/(blobArea/100)));
dhamilton31 3:de12b39ad805 128 }
dhamilton31 3:de12b39ad805 129 }
dhamilton31 3:de12b39ad805 130 else{
dhamilton31 3:de12b39ad805 131 lcd.printf("Color Lost");
dhamilton31 1:6c399fc35deb 132 }
dhamilton31 1:6c399fc35deb 133 }
dhamilton31 1:6c399fc35deb 134
dhamilton31 3:de12b39ad805 135 int servoIsInHome()
dhamilton31 3:de12b39ad805 136 {
dhamilton31 3:de12b39ad805 137 if(servoHor.read() > SERVO_HOME + SERVO_HOME_TOL) {
dhamilton31 3:de12b39ad805 138 return 1;
dhamilton31 3:de12b39ad805 139 } else if(servoHor.read() < SERVO_HOME - SERVO_HOME_TOL) {
dhamilton31 3:de12b39ad805 140 return -1;
dhamilton31 3:de12b39ad805 141 } else {
dhamilton31 3:de12b39ad805 142 return 0;
dhamilton31 3:de12b39ad805 143 }
dhamilton31 3:de12b39ad805 144 }