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
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?
User | Revision | Line number | New 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 | } |