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 640 // maximum X value input from camera
00009 #define MAX_VIEW_Y 480 // maximum Y value input from camera
00010 //#define MAX_VIEW_X 1600 //maximum X value input from camera
00011 //#define MAX_VIEW_Y 1200 // maximum Y value input from camera
00012 #define CENTER_BOX_TOLLERANCE 100 // Size of our box
00013 #define TO_SERVO_DIVISOR 4000.0 // Value to divide by to get the amount to move the servo by
00014 #define NO_COLOR_MAX_COUNT 5
00015 #define COLLISION_DIST .5
00016 #define BLOB_CLOSE_DIST 120000
00017 #define SERVO_HOME .5
00018 #define SERVO_HOME_TOL .2
00019 #define SPEED_CONST 65535 // Used with the blob area to determine how fast the robot should move toward the target
00020 #define GREEN 0
00021 #define RED 1
00022 
00023 // Hardware sensors and devices
00024 DigitalOut myled(LED1);
00025 DigitalOut myled2(LED2);
00026 DigitalIn colorPB(p30);
00027 DigitalOut greenLED(p29);
00028 DigitalOut redLED(p28);
00029 iRobot followMeBot(p9, p10);
00030 Servo servoHor(p22);
00031 Servo servoVer(p21);
00032 AnalogIn irSensorFront(p15);
00033 //AnalogIn irSensorLeft(p19);
00034 //AnalogIn irSensorRight(p18);
00035 Serial pc(USBTX, USBRX); // tx, rx
00036 TextLCD lcd(p14, p16, p17, p18, p19, p20); // rs, e, d4-d7
00037 
00038 //float irVal = irSensorFront;
00039 
00040 float irVal;
00041 
00042 // Software variables
00043 char serial_rx_buffer[256]; // Input buffer for data from the PC
00044 int xpos, ypos, blobArea; // x and y positions read from matlab
00045 Rectangle centerBox((MAX_VIEW_X/2)-CENTER_BOX_TOLLERANCE, (MAX_VIEW_Y/2)-CENTER_BOX_TOLLERANCE,
00046                     (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
00047 int noColorCounter; // Counts how long it has been since we have seen a color to follow
00048 bool colorLost = true; // boolean to represent if the color is confirmed "lost" (aka noColorCounter > NO_COLOR_MAX_COUNT)
00049 
00050 // Function Prototypes
00051 void getXYpos();
00052 float moveCamera();
00053 void moveBot();
00054 int servoIsInHome();
00055 void changeColor();
00056 char color = 0;
00057 int PBPast;
00058 
00059 int main()
00060 {
00061     followMeBot.start();
00062     servoHor = .5;
00063    // pc.printf("%d\n", GREEN);
00064 
00065     while(1) {
00066         getXYpos();
00067         moveBot();
00068         changeColor();
00069     }
00070 }
00071 
00072 /**
00073 *   Moves the servo to move the camera based upon where the
00074 *   color is located on the reported x and y
00075 *
00076 */
00077 float moveCamera()
00078 {
00079     /*lcd.cls();
00080     lcd.locate(0,0);
00081   lcd.printf("%d;%d;%f", xpos, ypos, servoHor.read());*/
00082     float temp = 0;
00083     if(xpos == 0) { // If we recieve a 0 for the location
00084         if(!colorLost && (++noColorCounter > NO_COLOR_MAX_COUNT)) { // Check to see if we have seen enough to consider the color lost
00085 //            servoHor = .5; // If the color is lost, return servo to home
00086             colorLost = true; // Set colorLost to true
00087             noColorCounter = 0; // Reset counter
00088         }
00089     } else if(!centerBox.is_touch(xpos, (MAX_VIEW_Y/2))) { // If we have a location
00090         noColorCounter = 0; // Reset counter
00091         colorLost = false; // We have found the color!
00092         temp = servoHor.read() - (centerBox.getCenterX() - xpos)/TO_SERVO_DIVISOR; // Calculate location to move servo to
00093 
00094 
00095         if(temp > 0 && temp <= 1) { // If the value is within the servo range
00096             servoHor = temp; // Set the servo equal to the position
00097             // lcd.locate(0,0);
00098             // lcd.printf("hello\n");
00099 
00100             //sprintf(serial_rx_buffer, "%f\n", temp);
00101         }
00102         /*temp = servoVer.read() + ((float)centerBox.getCenterY() - (float)ypos)/TO_SERVO_DIVISOR;
00103         if(temp > 0 && temp <= 1) {
00104             servoVer = temp;
00105         }*/
00106     }
00107     //pc.puts(serial_rx_buffer);
00108 
00109     return temp; // return the servo position
00110 }
00111 
00112 // Will return the number of degrees to turn the irobot by
00113 void getXYpos()
00114 {
00115     char * temp;
00116     const char del = ';';
00117     if(pc.readable()) { // See if matlab has data for us
00118         myled = 1;
00119         pc.gets(serial_rx_buffer, 256); // Get position data
00120         //pc.puts(serial_rx_buffer);
00121         temp = strtok(serial_rx_buffer, &del);
00122         xpos = atoi(temp); // Convert data to xposition int
00123         temp = strtok(NULL, &del);
00124         ypos = atoi(temp);
00125         temp = strtok(NULL, &del);
00126         blobArea = atoi(temp);
00127         moveCamera(); // Move the camera
00128     } else {
00129         myled = 0;
00130     }
00131     //lcd.locate(0,0);
00132     //lcd.printf("x: %d y: %d", xpos, ypos);
00133 
00134     //float irVal = irSensorFront;
00135     lcd.locate(0,0);
00136     lcd.printf("irVal: %f", irVal);
00137     lcd.locate(0,1);
00138     lcd.printf("blob: %d", blobArea);
00139 }
00140 
00141 void moveBot()
00142 {
00143     irVal = irSensorFront;      
00144 
00145     //
00146     // colorLost = false;
00147 
00148     lcd.locate(0,1);
00149     if(!colorLost) {
00150         // if(irVal > COLLISION_DIST && blobArea > BLOB_CLOSE_DIST) {
00151         //followMeBot.stop();
00152         //  lcd.printf("stop");
00153         //} else if(servoIsInHome() > 0) {
00154         if(servoIsInHome() > 0) {
00155             //if(servoHor.read() > .7) {
00156             //servoHor = servoHor - .1;
00157             //lcd.printf("right");
00158             followMeBot.right();
00159         } else if(servoIsInHome() < 0) {
00160             //} else if(servoHor.read() < .3) {
00161 
00162             //servoHor = servoHor + .1;
00163             //lcd.printf("left");
00164             followMeBot.left();
00165 
00166         } else if(servoIsInHome() == 0) {//n
00167         
00168             if (irVal < COLLISION_DIST) {
00169             //if (blobArea < 180000) {              //just testing          aaaaaa
00170                 //lcd.cls();
00171                 //lcd.printf("forward: %f", irVal);
00172                 followMeBot.forward();
00173             }
00174             else {
00175                 followMeBot.stop();
00176                 lcd.cls();
00177                 //lcd.locate(0,0);
00178                //lcd.printf("blobArea: %d ", blobArea);
00179                 //wait(1);
00180                // lcd.printf("stop: %f ", irVal);
00181             } 
00182             
00183             /*
00184             //followMeBot.changeSpeed(SPEED_CONST/(blobArea/100));
00185             followMeBot.stop();
00186             lcd.printf("for sp: %d", (SPEED_CONST/(blobArea/100)));*/
00187         }
00188     } else {
00189         //lcd.printf("Color Lost");
00190         followMeBot.left();
00191       //  followMeBot.stop();//n
00192        // followMeBot.right();
00193     }
00194 
00195 }
00196 
00197 int servoIsInHome()
00198 {
00199     if(servoHor.read() > SERVO_HOME + SERVO_HOME_TOL) {
00200         return 1;
00201     } else if(servoHor.read() < SERVO_HOME - SERVO_HOME_TOL) {
00202         return -1;
00203     } else {
00204         return 0;
00205     }
00206 }
00207 
00208 void changeColor(){
00209     int PBval = colorPB;
00210     if(PBval != PBPast && PBval == 1){
00211         color = !color;
00212         if(color == GREEN){
00213             greenLED = 1;
00214             redLED = 0;
00215         }
00216         else{
00217             greenLED = 0;
00218             redLED = 1;        
00219         }
00220             pc.printf("%d\n", color);
00221             servoHor = SERVO_HOME;
00222     }
00223     PBPast = PBval;
00224 }