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@7:41b5307e4c03, 2013-12-06 (annotated)

Committer:
dhamilton31
Date:
Fri Dec 06 19:06:50 2013 +0000
Revision:
7:41b5307e4c03
Parent:
6:e97ac3fb8ac5
Child:
8:7758b1db4d5b
added red and green serial printing

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