File content as of revision 9:72e899c35484:

#include "mbed.h"
#include "iRobot.h"
#include "Servo.h"
#include "Rectangle.h"
#include "TextLCD.h"

// Macros/Constants
#define MAX_VIEW_X 640 // maximum X value input from camera
#define MAX_VIEW_Y 480 // 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 100 // Size of our box
#define TO_SERVO_DIVISOR 4000.0 // Value to divide by to get the amount to move the servo by
#define NO_COLOR_MAX_COUNT 10
#define COLLISION_DIST .37
#define BLOB_CLOSE_DIST 120000
#define SERVO_HOME .5
#define SERVO_HOME_TOL .2
#define SPEED_CONST 65535 // Used with the blob area to determine how fast the robot should move toward the target
#define GREEN 0
#define RED 1

// Hardware sensors and devices
DigitalOut myled(LED1);
DigitalOut myled2(LED2);
DigitalIn colorPB(p30);
DigitalOut greenLED(p29);
DigitalOut redLED(p28);
iRobot followMeBot(p9, p10);
Servo servoHor(p22);
Servo servoVer(p21);
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

//float irVal = irSensorFront;

float irVal;

// Software variables
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 = true; // boolean to represent if the color is confirmed "lost" (aka noColorCounter > NO_COLOR_MAX_COUNT)

// Function Prototypes
void getXYpos();
float moveCamera();
void moveBot();
int servoIsInHome();
void changeColor();
char color = 0;
int PBPast;

int main()
{
    followMeBot.start();
    servoHor = .5;
   // pc.printf("%d\n", GREEN);

    while(1) {
        getXYpos();
        moveBot();
        changeColor();
    }
}

/**
*   Moves the servo to move the camera based upon where the
*   color is located on the reported x and y
*
*/
float moveCamera()
{
    /*lcd.cls();
    lcd.locate(0,0);
  lcd.printf("%d;%d;%f", xpos, ypos, servoHor.read());*/
    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
            // lcd.locate(0,0);
            // lcd.printf("hello\n");

            //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);

    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()) { // See if matlab has data for us
        myled = 1;
        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);
        moveCamera(); // Move the camera
    } else {
        myled = 0;
    }
    lcd.locate(0,0);
    lcd.printf("x: %d y: %d", xpos, ypos);

  /*  float irVal = irSensorFront;
    lcd.locate(0,0);
    lcd.printf("irVal: %f", irVal);
    lcd.locate(0,1);
    lcd.printf("blob: %d", blobArea);*/
}

void moveBot()
{
   // irVal = irSensorFront;                aaaaaaaaaaa

    //
    // colorLost = false;

    lcd.locate(0,1);
    if(!colorLost) {
        // if(irVal > COLLISION_DIST && blobArea > BLOB_CLOSE_DIST) {
        //followMeBot.stop();
        //  lcd.printf("stop");
        //} else if(servoIsInHome() > 0) {
        if(servoIsInHome() > 0) {
            //if(servoHor.read() > .7) {
            //servoHor = servoHor - .1;
            lcd.printf("right");
            followMeBot.right();
        } else if(servoIsInHome() < 0) {
            //} else if(servoHor.read() < .3) {

            //servoHor = servoHor + .1;
            lcd.printf("left");
            followMeBot.left();

        } else if(servoIsInHome() == 0) {//n
        
            //if (irVal < COLLISION_DIST) {
            if (blobArea < 3000000) {              //just testing          aaaaaa
                //lcd.cls();
                //lcd.printf("forward: %f", irVal);
                followMeBot.forward();
            }
            else {
                followMeBot.stop();
                lcd.cls();
                //lcd.locate(0,0);
               //lcd.printf("blobArea: %d ", blobArea);
                //wait(1);
               // lcd.printf("stop: %f ", irVal);
            } 
            
            /*
            //followMeBot.changeSpeed(SPEED_CONST/(blobArea/100));
            followMeBot.stop();
            lcd.printf("for sp: %d", (SPEED_CONST/(blobArea/100)));*/
        }
    } else {
        lcd.printf("Color Lost");
        followMeBot.left();
      //  followMeBot.stop();//n
       // followMeBot.right();
    }

}

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;
    }
}

void changeColor(){
    int PBval = colorPB;
    if(PBval != PBPast && PBval == 1){
        color = !color;
        if(color == GREEN){
            greenLED = 1;
            redLED = 0;
        }
        else{
            greenLED = 0;
            redLED = 1;        
        }
            pc.printf("%d\n", color);
            servoHor = SERVO_HOME;
    }
    PBPast = PBval;
}