main.cpp

00001 #include "mbed.h"
00002 #include "iRobot.h"
00003 #include "Servo.h"
00004 #include "Rectangle.h"
00005 #include "TextLCD.h"
00006 
00007 // Macros/Constants
00008 #define MAX_VIEW_X 1600 // maximum X value input from camera
00009 #define MAX_VIEW_Y 1200 // maximum Y value input from camera
00010 #define CENTER_BOX_TOLLERANCE 200 // Size of our box
00011 #define TO_SERVO_DIVISOR 5000.0 // Value to divide by to get the amount to move the servo by
00012 #define NO_COLOR_MAX_COUNT 10
00013 #define COLLISION_DIST .4
00014 #define BLOB_CLOSE_DIST 200
00015 #define SERVO_HOME .5
00016 #define SERVO_HOME_TOL .2
00017 #define SPEED_CONST 65535
00018 
00019 // Hardware sensors and devices
00020 DigitalOut myled(LED1);
00021 DigitalOut myled2(LED2);
00022 iRobot followMeBot(p9, p10);
00023 Servo servoHor(p22);
00024 Servo servoVer(p21);
00025 AnalogIn irSensorFront(p15);
00026 //AnalogIn irSensorLeft(p19);
00027 //AnalogIn irSensorRight(p18);
00028 Serial pc(USBTX, USBRX); // tx, rx
00029 TextLCD lcd(p14, p16, p17, p18, p19, p20); // rs, e, d4-d7
00030 
00031 // Software variables
00032 char serial_rx_buffer[256]; // Input buffer for data from the PC
00033 int xpos, ypos, blobArea; // x and y positions read from matlab
00034 Rectangle centerBox((MAX_VIEW_X/2)-CENTER_BOX_TOLLERANCE, (MAX_VIEW_Y/2)-CENTER_BOX_TOLLERANCE,
00035                     (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
00036 int noColorCounter; // Counts how long it has been since we have seen a color to follow
00037 bool colorLost = true; // boolean to represent if the color is confirmed "lost" (aka noColorCounter > NO_COLOR_MAX_COUNT)
00038 
00039 // Function Prototypes
00040 void getXYpos();
00041 float moveCamera();
00042 void moveBot();
00043 int servoIsInHome();
00044 
00045 int main()
00046 {
00047     followMeBot.start();
00048     servoHor = .5;
00049 
00050     while(1) {
00051         getXYpos();
00052         moveBot();
00053     }
00054 }
00055 
00056 /**
00057 *   Moves the servo to move the camera based upon where the
00058 *   color is located on the reported x and y
00059 *
00060 */
00061 float moveCamera()
00062 {
00063     float temp = 0;
00064     if(xpos == 0) { // If we recieve a 0 for the location
00065         if(!colorLost && (++noColorCounter > NO_COLOR_MAX_COUNT)) { // Check to see if we have seen enough to consider the color lost
00066 //            servoHor = .5; // If the color is lost, return servo to home
00067             colorLost = true; // Set colorLost to true
00068             noColorCounter = 0; // Reset counter
00069         }
00070     } else if(!centerBox.is_touch(xpos, (MAX_VIEW_Y/2))) { // If we have a location
00071         noColorCounter = 0; // Reset counter
00072         colorLost = false; // We have found the color!
00073         temp = servoHor.read() - (centerBox.getCenterX() - xpos)/TO_SERVO_DIVISOR; // Calculate location to move servo to
00074         if(temp > 0 && temp <= 1) { // If the value is within the servo range
00075             servoHor = temp; // Set the servo equal to the position
00076 
00077             //sprintf(serial_rx_buffer, "%f\n", temp);
00078         }
00079         /*temp = servoVer.read() + ((float)centerBox.getCenterY() - (float)ypos)/TO_SERVO_DIVISOR;
00080         if(temp > 0 && temp <= 1) {
00081             servoVer = temp;
00082         }*/
00083     }
00084     //pc.puts(serial_rx_buffer);
00085 
00086     lcd.cls();
00087     lcd.locate(0,0);
00088     lcd.printf("%d;%d;%f", xpos, ypos, servoHor.read());
00089 
00090     return temp; // return the servo position
00091 }
00092 
00093 // Will return the number of degrees to turn the irobot by
00094 void getXYpos()
00095 {
00096     char * temp;
00097     const char del = ';';
00098     if(pc.readable()) { // See if matlab has data for us
00099         myled = 1;
00100         pc.gets(serial_rx_buffer, 256); // Get position data
00101         pc.puts(serial_rx_buffer);
00102         temp = strtok(serial_rx_buffer, &del);
00103         xpos = atoi(temp); // Convert data to xposition int
00104         temp = strtok(NULL, &del);
00105         ypos = atoi(temp);
00106         temp = strtok(NULL, &del);
00107         blobArea = atoi(temp);
00108 
00109         moveCamera(); // Move the camera
00110     } else {
00111         myled = 0;
00112     }
00113 }
00114 
00115 void moveBot()
00116 {
00117     float irVal = irSensorFront;
00118 
00119     //
00120     // colorLost = false;
00121 
00122     lcd.locate(0,1);
00123     if(!colorLost) {
00124         // if(irVal > COLLISION_DIST && blobArea > BLOB_CLOSE_DIST) {
00125         //followMeBot.stop();
00126         //  lcd.printf("stop");
00127         //} else if(servoIsInHome() > 0) {
00128         if(servoIsInHome() > 0) {
00129         //if(servoHor.read() > .7) {
00130             //servoHor = servoHor - .1;
00131             lcd.printf("right");
00132             followMeBot.right();
00133            } else if(servoIsInHome() < 0) {
00134         //} else if(servoHor.read() < .3) {
00135 
00136             //servoHor = servoHor + .1;
00137             lcd.printf("left");
00138             followMeBot.left();
00139 
00140         } else if(servoIsInHome() == 0) {
00141             //followMeBot.changeSpeed(SPEED_CONST/(blobArea/100));
00142             //followMeBot.forward();
00143             followMeBot.stop();
00144             lcd.printf("for sp: %d", (SPEED_CONST/(blobArea/100)));
00145         }
00146     } else {
00147         lcd.printf("Color Lost");
00148         followMeBot.right();
00149     }
00150 
00151 }
00152 
00153 int servoIsInHome()
00154 {
00155     if(servoHor.read() > SERVO_HOME + SERVO_HOME_TOL) {
00156         return 1;
00157     } else if(servoHor.read() < SERVO_HOME - SERVO_HOME_TOL) {
00158         return -1;
00159     } else {
00160         return 0;
00161     }
00162 }