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@8:7758b1db4d5b, 2013-12-06 (annotated)

Committer:
dhamilton31
Date:
Fri Dec 06 20:40:17 2013 +0000
Revision:
8:7758b1db4d5b
Parent:
7:41b5307e4c03
Child:
9:72e899c35484
LEDS WORK! WITH BUTTONS! YEAH!

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 8:7758b1db4d5b 8 //#define MAX_VIEW_X 640 // maximum X value input from camera
dhamilton31 8:7758b1db4d5b 9 //#define MAX_VIEW_Y 480 // maximum Y value input from camera
dhamilton31 8:7758b1db4d5b 10 #define MAX_VIEW_X 1600 //maximum X value input from camera
dhamilton31 8:7758b1db4d5b 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 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;
dhamilton31 8:7758b1db4d5b 63
dhamilton31 0:3ed56271dd2d 64 while(1) {
dhamilton31 2:0b362c662997 65 getXYpos();
dhamilton31 3:de12b39ad805 66 moveBot();
dhamilton31 8:7758b1db4d5b 67 changeColor();
dhamilton31 0:3ed56271dd2d 68 }
dhamilton31 0:3ed56271dd2d 69 }
dhamilton31 1:6c399fc35deb 70
dhamilton31 3:de12b39ad805 71 /**
dhamilton31 3:de12b39ad805 72 * Moves the servo to move the camera based upon where the
dhamilton31 3:de12b39ad805 73 * color is located on the reported x and y
dhamilton31 3:de12b39ad805 74 *
dhamilton31 3:de12b39ad805 75 */
dhamilton31 3:de12b39ad805 76 float moveCamera()
dhamilton31 1:6c399fc35deb 77 {
rshetty6 5:5fe2f8603be8 78 /*lcd.cls();
rshetty6 5:5fe2f8603be8 79 lcd.locate(0,0);
dhamilton31 8:7758b1db4d5b 80 lcd.printf("%d;%d;%f", xpos, ypos, servoHor.read());*/
dhamilton31 3:de12b39ad805 81 float temp = 0;
dhamilton31 3:de12b39ad805 82 if(xpos == 0) { // If we recieve a 0 for the location
dhamilton31 3:de12b39ad805 83 if(!colorLost && (++noColorCounter > NO_COLOR_MAX_COUNT)) { // Check to see if we have seen enough to consider the color lost
rchoy3 4:aa21c6609858 84 // servoHor = .5; // If the color is lost, return servo to home
dhamilton31 3:de12b39ad805 85 colorLost = true; // Set colorLost to true
dhamilton31 3:de12b39ad805 86 noColorCounter = 0; // Reset counter
dhamilton31 3:de12b39ad805 87 }
dhamilton31 3:de12b39ad805 88 } else if(!centerBox.is_touch(xpos, (MAX_VIEW_Y/2))) { // If we have a location
dhamilton31 3:de12b39ad805 89 noColorCounter = 0; // Reset counter
dhamilton31 3:de12b39ad805 90 colorLost = false; // We have found the color!
dhamilton31 3:de12b39ad805 91 temp = servoHor.read() - (centerBox.getCenterX() - xpos)/TO_SERVO_DIVISOR; // Calculate location to move servo to
rshetty6 5:5fe2f8603be8 92
rshetty6 5:5fe2f8603be8 93
dhamilton31 3:de12b39ad805 94 if(temp > 0 && temp <= 1) { // If the value is within the servo range
dhamilton31 3:de12b39ad805 95 servoHor = temp; // Set the servo equal to the position
rshetty6 5:5fe2f8603be8 96 // lcd.locate(0,0);
rshetty6 5:5fe2f8603be8 97 // lcd.printf("hello\n");
rchoy3 4:aa21c6609858 98
dhamilton31 3:de12b39ad805 99 //sprintf(serial_rx_buffer, "%f\n", temp);
dhamilton31 1:6c399fc35deb 100 }
dhamilton31 2:0b362c662997 101 /*temp = servoVer.read() + ((float)centerBox.getCenterY() - (float)ypos)/TO_SERVO_DIVISOR;
dhamilton31 1:6c399fc35deb 102 if(temp > 0 && temp <= 1) {
dhamilton31 1:6c399fc35deb 103 servoVer = temp;
dhamilton31 2:0b362c662997 104 }*/
dhamilton31 2:0b362c662997 105 }
dhamilton31 3:de12b39ad805 106 //pc.puts(serial_rx_buffer);
rchoy3 4:aa21c6609858 107
dhamilton31 3:de12b39ad805 108 return temp; // return the servo position
dhamilton31 2:0b362c662997 109 }
dhamilton31 2:0b362c662997 110
dhamilton31 3:de12b39ad805 111 // Will return the number of degrees to turn the irobot by
dhamilton31 2:0b362c662997 112 void getXYpos()
dhamilton31 2:0b362c662997 113 {
dhamilton31 3:de12b39ad805 114 char * temp;
dhamilton31 3:de12b39ad805 115 const char del = ';';
dhamilton31 3:de12b39ad805 116 if(pc.readable()) { // See if matlab has data for us
dhamilton31 2:0b362c662997 117 myled = 1;
dhamilton31 3:de12b39ad805 118 pc.gets(serial_rx_buffer, 256); // Get position data
rshetty6 5:5fe2f8603be8 119 //pc.puts(serial_rx_buffer);
dhamilton31 3:de12b39ad805 120 temp = strtok(serial_rx_buffer, &del);
dhamilton31 3:de12b39ad805 121 xpos = atoi(temp); // Convert data to xposition int
dhamilton31 3:de12b39ad805 122 temp = strtok(NULL, &del);
dhamilton31 3:de12b39ad805 123 ypos = atoi(temp);
dhamilton31 3:de12b39ad805 124 temp = strtok(NULL, &del);
dhamilton31 3:de12b39ad805 125 blobArea = atoi(temp);
dhamilton31 3:de12b39ad805 126 moveCamera(); // Move the camera
dhamilton31 2:0b362c662997 127 } else {
dhamilton31 2:0b362c662997 128 myled = 0;
dhamilton31 2:0b362c662997 129 }
rshetty6 5:5fe2f8603be8 130
dhamilton31 8:7758b1db4d5b 131 /* float irVal = irSensorFront;
dhamilton31 8:7758b1db4d5b 132 lcd.locate(0,0);
dhamilton31 8:7758b1db4d5b 133 lcd.printf("irVal: %f", irVal);
dhamilton31 8:7758b1db4d5b 134 lcd.locate(0,1);
dhamilton31 8:7758b1db4d5b 135 lcd.printf("blob: %d", blobArea);*/
dhamilton31 3:de12b39ad805 136 }
dhamilton31 3:de12b39ad805 137
dhamilton31 3:de12b39ad805 138 void moveBot()
dhamilton31 3:de12b39ad805 139 {
dhamilton31 8:7758b1db4d5b 140 // irVal = irSensorFront; aaaaaaaaaaa
rchoy3 4:aa21c6609858 141
rchoy3 4:aa21c6609858 142 //
rchoy3 4:aa21c6609858 143 // colorLost = false;
rchoy3 4:aa21c6609858 144
dhamilton31 3:de12b39ad805 145 lcd.locate(0,1);
dhamilton31 3:de12b39ad805 146 if(!colorLost) {
rchoy3 4:aa21c6609858 147 // if(irVal > COLLISION_DIST && blobArea > BLOB_CLOSE_DIST) {
rchoy3 4:aa21c6609858 148 //followMeBot.stop();
rchoy3 4:aa21c6609858 149 // lcd.printf("stop");
rchoy3 4:aa21c6609858 150 //} else if(servoIsInHome() > 0) {
rchoy3 4:aa21c6609858 151 if(servoIsInHome() > 0) {
rshetty6 5:5fe2f8603be8 152 //if(servoHor.read() > .7) {
rchoy3 4:aa21c6609858 153 //servoHor = servoHor - .1;
dhamilton31 3:de12b39ad805 154 lcd.printf("right");
rchoy3 4:aa21c6609858 155 followMeBot.right();
rshetty6 5:5fe2f8603be8 156 } else if(servoIsInHome() < 0) {
rshetty6 5:5fe2f8603be8 157 //} else if(servoHor.read() < .3) {
rchoy3 4:aa21c6609858 158
rchoy3 4:aa21c6609858 159 //servoHor = servoHor + .1;
dhamilton31 3:de12b39ad805 160 lcd.printf("left");
rchoy3 4:aa21c6609858 161 followMeBot.left();
rchoy3 4:aa21c6609858 162
rshetty6 5:5fe2f8603be8 163 } else if(servoIsInHome() == 0) {//n
dhamilton31 8:7758b1db4d5b 164
rshetty6 5:5fe2f8603be8 165 //if (irVal < COLLISION_DIST) {
rshetty6 5:5fe2f8603be8 166 if (blobArea < 180000) { //just testing aaaaaa
rshetty6 5:5fe2f8603be8 167 //lcd.cls();
rshetty6 5:5fe2f8603be8 168 //lcd.printf("forward: %f", irVal);
rshetty6 5:5fe2f8603be8 169 followMeBot.forward();
dhamilton31 8:7758b1db4d5b 170 }
dhamilton31 8:7758b1db4d5b 171 else {
rshetty6 5:5fe2f8603be8 172 followMeBot.stop();
rshetty6 5:5fe2f8603be8 173 lcd.cls();
rshetty6 5:5fe2f8603be8 174 lcd.locate(0,0);
rshetty6 5:5fe2f8603be8 175 lcd.printf("blobArea: %d ", blobArea);
rshetty6 5:5fe2f8603be8 176 //wait(1);
dhamilton31 8:7758b1db4d5b 177 // lcd.printf("stop: %f ", irVal);
dhamilton31 8:7758b1db4d5b 178 }
dhamilton31 8:7758b1db4d5b 179
rshetty6 5:5fe2f8603be8 180 /*
dhamilton31 3:de12b39ad805 181 //followMeBot.changeSpeed(SPEED_CONST/(blobArea/100));
rchoy3 4:aa21c6609858 182 followMeBot.stop();
rshetty6 5:5fe2f8603be8 183 lcd.printf("for sp: %d", (SPEED_CONST/(blobArea/100)));*/
dhamilton31 3:de12b39ad805 184 }
rchoy3 4:aa21c6609858 185 } else {
rchoy3 4:aa21c6609858 186 lcd.printf("Color Lost");
rshetty6 5:5fe2f8603be8 187 followMeBot.left();
dhamilton31 8:7758b1db4d5b 188 // followMeBot.stop();//n
dhamilton31 8:7758b1db4d5b 189 // followMeBot.right();
dhamilton31 3:de12b39ad805 190 }
rchoy3 4:aa21c6609858 191
dhamilton31 1:6c399fc35deb 192 }
dhamilton31 1:6c399fc35deb 193
dhamilton31 3:de12b39ad805 194 int servoIsInHome()
dhamilton31 3:de12b39ad805 195 {
dhamilton31 3:de12b39ad805 196 if(servoHor.read() > SERVO_HOME + SERVO_HOME_TOL) {
dhamilton31 3:de12b39ad805 197 return 1;
dhamilton31 3:de12b39ad805 198 } else if(servoHor.read() < SERVO_HOME - SERVO_HOME_TOL) {
dhamilton31 3:de12b39ad805 199 return -1;
dhamilton31 3:de12b39ad805 200 } else {
dhamilton31 3:de12b39ad805 201 return 0;
dhamilton31 3:de12b39ad805 202 }
dhamilton31 3:de12b39ad805 203 }
dhamilton31 6:e97ac3fb8ac5 204
dhamilton31 8:7758b1db4d5b 205 void changeColor(){
dhamilton31 8:7758b1db4d5b 206 int PBval = colorPB;
dhamilton31 8:7758b1db4d5b 207 if(PBval != PBPast && PBval == 1){
dhamilton31 8:7758b1db4d5b 208 color = !color;
dhamilton31 8:7758b1db4d5b 209 if(color == GREEN){
dhamilton31 8:7758b1db4d5b 210 greenLED = 1;
dhamilton31 8:7758b1db4d5b 211 redLED = 0;
dhamilton31 8:7758b1db4d5b 212 }
dhamilton31 8:7758b1db4d5b 213 else{
dhamilton31 8:7758b1db4d5b 214 greenLED = 0;
dhamilton31 8:7758b1db4d5b 215 redLED = 1;
dhamilton31 8:7758b1db4d5b 216 }
dhamilton31 6:e97ac3fb8ac5 217 }
dhamilton31 8:7758b1db4d5b 218 PBPast = PBval;
dhamilton31 6:e97ac3fb8ac5 219 }