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@4:aa21c6609858, 2013-12-02 (annotated)

Committer:
rchoy3
Date:
Mon Dec 02 17:20:53 2013 +0000
Revision:
4:aa21c6609858
Parent:
3:de12b39ad805
Child:
5:5fe2f8603be8
turning

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