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

Committer:
dhamilton31
Date:
Fri Dec 06 18:04:28 2013 +0000
Revision:
6:e97ac3fb8ac5
Parent:
5:5fe2f8603be8
Child:
7:41b5307e4c03
added pushbuttons and leds

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