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@11:503f845d2766, 2013-12-07 (annotated)

Committer:
rshetty6
Date:
Sat Dec 07 04:06:40 2013 +0000
Revision:
11:503f845d2766
Parent:
10:d53a8ef068c0 
Final commit

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