Rahul Shetty / Mbed 2 deprecated FollowMeBot

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 FollowMeBot3 by Rahul Shetty

main.cpp@5:5fe2f8603be8, 2013-12-06 (annotated)

Committer:
rshetty6
Date:
Fri Dec 06 16:59:07 2013 +0000
Revision:
5:5fe2f8603be8
Parent:
4:aa21c6609858
Child:
6:e97ac3fb8ac5
friday

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