Follow me bot application!
Dependencies: Rectangle Servo TextLCD mbed
Diff: main.cpp
- Revision:
- 3:de12b39ad805
- Parent:
- 2:0b362c662997
diff -r 0b362c662997 -r de12b39ad805 main.cpp --- a/main.cpp Thu Nov 21 00:02:05 2013 +0000 +++ b/main.cpp Mon Nov 25 19:55:12 2013 +0000 @@ -2,12 +2,19 @@ #include "iRobot.h" #include "Servo.h" #include "Rectangle.h" +#include "TextLCD.h" // Macros/Constants -#define MAX_VIEW_X 1176 // maximum X value input from camera -#define MAX_VIEW_Y 711 // maximum Y value input from camera +#define MAX_VIEW_X 1600 // maximum X value input from camera +#define MAX_VIEW_Y 1200 // maximum Y value input from camera #define CENTER_BOX_TOLLERANCE 200 // Size of our box -#define TO_SERVO_DIVISOR 1176.0 // Value to divide by to get the amount to move the servo by +#define TO_SERVO_DIVISOR 3000.0 // Value to divide by to get the amount to move the servo by +#define NO_COLOR_MAX_COUNT 10 +#define COLLISION_DIST .4 +#define BLOB_CLOSE_DIST 200 +#define SERVO_HOME .5 +#define SERVO_HOME_TOL .2 +#define SPEED_CONST 65535 // Hardware sensors and devices DigitalOut myled(LED1); @@ -15,73 +22,123 @@ iRobot followMeBot(p9, p10); Servo servoHor(p22); Servo servoVer(p21); -AnalogIn irSensorFront(p20); -AnalogIn irSensorLeft(p19); -AnalogIn irSensorRight(p18); +AnalogIn irSensorFront(p15); +//AnalogIn irSensorLeft(p19); +//AnalogIn irSensorRight(p18); Serial pc(USBTX, USBRX); // tx, rx +TextLCD lcd(p14, p16, p17, p18, p19, p20); // rs, e, d4-d7 // Software variables -char serial_rx_buffer[128]; // Input buffer for data from the PC -int xpos, ypos; // x and y positions read from matlab -Rectangle centerBox((MAX_VIEW_X/2)+CENTER_BOX_TOLLERANCE, (MAX_VIEW_Y/2)+CENTER_BOX_TOLLERANCE, - (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 +char serial_rx_buffer[256]; // Input buffer for data from the PC +int xpos, ypos, blobArea; // x and y positions read from matlab +Rectangle centerBox((MAX_VIEW_X/2)-CENTER_BOX_TOLLERANCE, (MAX_VIEW_Y/2)-CENTER_BOX_TOLLERANCE, + (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 +int noColorCounter; // Counts how long it has been since we have seen a color to follow +bool colorLost; // boolean to represent if the color is confirmed "lost" (aka noColorCounter > NO_COLOR_MAX_COUNT) // Function Prototypes void getXYpos(); -void moveCamera(); +float moveCamera(); +void moveBot(); +int servoIsInHome(); int main() { //followMeBot.start(); - + //wait(1); while(1) { getXYpos(); - wait(.5); + moveBot(); } } -void moveCamera() +/** +* Moves the servo to move the camera based upon where the +* color is located on the reported x and y +* +*/ +float moveCamera() { - if(xpos == 0) { - servoHor = .5; - } else if(!centerBox.is_touch(xpos, ypos)) { - float temp; - //printf("Servo at: %f\n", servoHor.read()); - temp = servoHor.read() - (centerBox.getCenterX() - xpos)/TO_SERVO_DIVISOR; - //printf("move servo by: %f\n", (centerBox.getCenterX() - xpos)/TO_SERVO_DIVISOR); - if(temp > 0 && temp <= 1) { - servoHor = temp; - sprintf(serial_rx_buffer, "%f\n", temp); + float temp = 0; + if(xpos == 0) { // If we recieve a 0 for the location + if(!colorLost && (++noColorCounter > NO_COLOR_MAX_COUNT)) { // Check to see if we have seen enough to consider the color lost + servoHor = .5; // If the color is lost, return servo to home + colorLost = true; // Set colorLost to true + noColorCounter = 0; // Reset counter + } + } else if(!centerBox.is_touch(xpos, (MAX_VIEW_Y/2))) { // If we have a location + noColorCounter = 0; // Reset counter + colorLost = false; // We have found the color! + temp = servoHor.read() - (centerBox.getCenterX() - xpos)/TO_SERVO_DIVISOR; // Calculate location to move servo to + if(temp > 0 && temp <= 1) { // If the value is within the servo range + servoHor = temp; // Set the servo equal to the position + //sprintf(serial_rx_buffer, "%f\n", temp); } /*temp = servoVer.read() + ((float)centerBox.getCenterY() - (float)ypos)/TO_SERVO_DIVISOR; if(temp > 0 && temp <= 1) { servoVer = temp; }*/ } - pc.puts(serial_rx_buffer); + //pc.puts(serial_rx_buffer); + return temp; // return the servo position } +// Will return the number of degrees to turn the irobot by void getXYpos() { - //char * temp; - //const char del = ';'; - - if(pc.readable()) { + char * temp; + const char del = ';'; + if(pc.readable()) { // See if matlab has data for us myled = 1; - pc.gets(serial_rx_buffer, 128); - xpos = atoi(serial_rx_buffer); - //sprintf(serial_rx_buffer, "%d\n", xpos); - //pc.puts(serial_rx_buffer); - //temp = strtok(serial_rx_buffer, &del); - moveCamera(); + pc.gets(serial_rx_buffer, 256); // Get position data + pc.puts(serial_rx_buffer); + temp = strtok(serial_rx_buffer, &del); + xpos = atoi(temp); // Convert data to xposition int + temp = strtok(NULL, &del); + ypos = atoi(temp); + temp = strtok(NULL, &del); + blobArea = atoi(temp); + lcd.cls(); + lcd.locate(0,0); + lcd.printf("%d;%d", xpos, ypos); + moveCamera(); // Move the camera } else { myled = 0; } - if(xpos > 500) { - myled2 = 1; - } else { - myled2 = 0; +} + +void moveBot() +{ + float irVal = irSensorFront; + lcd.locate(0,1); + if(!colorLost) { + if(irVal > COLLISION_DIST && blobArea > BLOB_CLOSE_DIST) { + //followMeBot.stop(); + lcd.printf("stop"); + } else if(servoIsInHome() > 0) { + //followMeBot.right(); + lcd.printf("right"); + } else if(servoIsInHome() < 0) { + //followMeBot.left(); + lcd.printf("left"); + } else { + //followMeBot.changeSpeed(SPEED_CONST/(blobArea/100)); + //followMeBot.forward(); + lcd.printf("for sp: %d", (SPEED_CONST/(blobArea/100))); + } + } + else{ + lcd.printf("Color Lost"); } } - +int servoIsInHome() +{ + if(servoHor.read() > SERVO_HOME + SERVO_HOME_TOL) { + return 1; + } else if(servoHor.read() < SERVO_HOME - SERVO_HOME_TOL) { + return -1; + } else { + return 0; + } +}