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@2:0b362c662997, 2013-11-21 (annotated)

Committer:
dhamilton31
Date:
Thu Nov 21 00:02:05 2013 +0000
Revision:
2:0b362c662997
Parent:
1:6c399fc35deb
Child:
3:de12b39ad805
It controls the servo!

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 0:3ed56271dd2d 5
dhamilton31 1:6c399fc35deb 6 // Macros/Constants
dhamilton31 2:0b362c662997 7 #define MAX_VIEW_X 1176 // maximum X value input from camera
dhamilton31 2:0b362c662997 8 #define MAX_VIEW_Y 711 // maximum Y value input from camera
dhamilton31 2:0b362c662997 9 #define CENTER_BOX_TOLLERANCE 200 // Size of our box
dhamilton31 2:0b362c662997 10 #define TO_SERVO_DIVISOR 1176.0 // Value to divide by to get the amount to move the servo by
dhamilton31 0:3ed56271dd2d 11
dhamilton31 1:6c399fc35deb 12 // Hardware sensors and devices
dhamilton31 1:6c399fc35deb 13 DigitalOut myled(LED1);
dhamilton31 2:0b362c662997 14 DigitalOut myled2(LED2);
dhamilton31 1:6c399fc35deb 15 iRobot followMeBot(p9, p10);
dhamilton31 1:6c399fc35deb 16 Servo servoHor(p22);
dhamilton31 1:6c399fc35deb 17 Servo servoVer(p21);
dhamilton31 1:6c399fc35deb 18 AnalogIn irSensorFront(p20);
dhamilton31 1:6c399fc35deb 19 AnalogIn irSensorLeft(p19);
dhamilton31 1:6c399fc35deb 20 AnalogIn irSensorRight(p18);
dhamilton31 1:6c399fc35deb 21 Serial pc(USBTX, USBRX); // tx, rx
dhamilton31 1:6c399fc35deb 22
dhamilton31 1:6c399fc35deb 23 // Software variables
dhamilton31 1:6c399fc35deb 24 char serial_rx_buffer[128]; // Input buffer for data from the PC
dhamilton31 1:6c399fc35deb 25 int xpos, ypos; // x and y positions read from matlab
dhamilton31 1:6c399fc35deb 26 Rectangle centerBox((MAX_VIEW_X/2)+CENTER_BOX_TOLLERANCE, (MAX_VIEW_Y/2)+CENTER_BOX_TOLLERANCE,
dhamilton31 1:6c399fc35deb 27 (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 1:6c399fc35deb 28
dhamilton31 2:0b362c662997 29 // Function Prototypes
dhamilton31 2:0b362c662997 30 void getXYpos();
dhamilton31 2:0b362c662997 31 void moveCamera();
dhamilton31 2:0b362c662997 32
dhamilton31 1:6c399fc35deb 33 int main()
dhamilton31 1:6c399fc35deb 34 {
dhamilton31 2:0b362c662997 35 //followMeBot.start();
dhamilton31 1:6c399fc35deb 36
dhamilton31 0:3ed56271dd2d 37 while(1) {
dhamilton31 2:0b362c662997 38 getXYpos();
dhamilton31 2:0b362c662997 39 wait(.5);
dhamilton31 0:3ed56271dd2d 40 }
dhamilton31 0:3ed56271dd2d 41 }
dhamilton31 1:6c399fc35deb 42
dhamilton31 1:6c399fc35deb 43 void moveCamera()
dhamilton31 1:6c399fc35deb 44 {
dhamilton31 2:0b362c662997 45 if(xpos == 0) {
dhamilton31 2:0b362c662997 46 servoHor = .5;
dhamilton31 2:0b362c662997 47 } else if(!centerBox.is_touch(xpos, ypos)) {
dhamilton31 1:6c399fc35deb 48 float temp;
dhamilton31 2:0b362c662997 49 //printf("Servo at: %f\n", servoHor.read());
dhamilton31 2:0b362c662997 50 temp = servoHor.read() - (centerBox.getCenterX() - xpos)/TO_SERVO_DIVISOR;
dhamilton31 2:0b362c662997 51 //printf("move servo by: %f\n", (centerBox.getCenterX() - xpos)/TO_SERVO_DIVISOR);
dhamilton31 1:6c399fc35deb 52 if(temp > 0 && temp <= 1) {
dhamilton31 1:6c399fc35deb 53 servoHor = temp;
dhamilton31 2:0b362c662997 54 sprintf(serial_rx_buffer, "%f\n", temp);
dhamilton31 1:6c399fc35deb 55 }
dhamilton31 2:0b362c662997 56 /*temp = servoVer.read() + ((float)centerBox.getCenterY() - (float)ypos)/TO_SERVO_DIVISOR;
dhamilton31 1:6c399fc35deb 57 if(temp > 0 && temp <= 1) {
dhamilton31 1:6c399fc35deb 58 servoVer = temp;
dhamilton31 2:0b362c662997 59 }*/
dhamilton31 2:0b362c662997 60 }
dhamilton31 2:0b362c662997 61 pc.puts(serial_rx_buffer);
dhamilton31 2:0b362c662997 62 }
dhamilton31 2:0b362c662997 63
dhamilton31 2:0b362c662997 64 void getXYpos()
dhamilton31 2:0b362c662997 65 {
dhamilton31 2:0b362c662997 66 //char * temp;
dhamilton31 2:0b362c662997 67 //const char del = ';';
dhamilton31 2:0b362c662997 68
dhamilton31 2:0b362c662997 69 if(pc.readable()) {
dhamilton31 2:0b362c662997 70 myled = 1;
dhamilton31 2:0b362c662997 71 pc.gets(serial_rx_buffer, 128);
dhamilton31 2:0b362c662997 72 xpos = atoi(serial_rx_buffer);
dhamilton31 2:0b362c662997 73 //sprintf(serial_rx_buffer, "%d\n", xpos);
dhamilton31 2:0b362c662997 74 //pc.puts(serial_rx_buffer);
dhamilton31 2:0b362c662997 75 //temp = strtok(serial_rx_buffer, &del);
dhamilton31 2:0b362c662997 76 moveCamera();
dhamilton31 2:0b362c662997 77 } else {
dhamilton31 2:0b362c662997 78 myled = 0;
dhamilton31 2:0b362c662997 79 }
dhamilton31 2:0b362c662997 80 if(xpos > 500) {
dhamilton31 2:0b362c662997 81 myled2 = 1;
dhamilton31 2:0b362c662997 82 } else {
dhamilton31 2:0b362c662997 83 myled2 = 0;
dhamilton31 1:6c399fc35deb 84 }
dhamilton31 1:6c399fc35deb 85 }
dhamilton31 1:6c399fc35deb 86
dhamilton31 1:6c399fc35deb 87