#use "createlib.ic"
#use "xbccamlib.ic"                

/* This program causes the create to spin in place until it sees an object matching the color in color
   model 0.  It then moves towards that object until a bumper, cliff or drop sensor is activated.
   At that point the robot will stop, spin 180 degrees and move 1 meter, or until a bump, 
   cliff or drop sensor becomes active.
   written by D. Miller 4/07
*/
void main()
{
    int tx, lspeed, rspeed;//tx is the coordinate of the centroid of the tracked object; if tx=174, object is centered
    init_camera(); // start up xbc camera system
    create_connect(); // start up communication with the create
    display_clear(); // clear display
    printf("Going after colored object\n");
    while(!b_button()){
        track_update(); // get a new image into the xbc
        create_sensor_update(); // get a new set of sensor values from the create
        if(gc_lbump || gc_rbump || gc_ldrop || gc_rdrop || // these are all globals set by create_sensor_update().
           gc_fdrop || gc_rcliff || gc_rfcliff || gc_lcliff || gc_lfcliff)
          {
            printf("Chase is blocked\nRun AWAY!!!!!!!!\n"); // if sensor detects obstacle, give up
            create_drive(0,1); // stop any movement
            break; // and jump out of the while loop
        }
        if (track_count(0)>0) {    //if no obstacles, are there any blobs on channel 0?
            tx=track_x(0,0); //if so, get the x coordinate of the centroid
            rspeed = 250 + (174-tx)/2;//250 is default speed but if tx is off to right, right wheel slows
            lspeed = 250 + (tx-174)/2;//and if tx is off to left, right wheel speeds up while left slows down
            create_drive_direct(rspeed,lspeed);
        }
        else create_spin_CW(50); // if no obstacle and no colored object, spin until something appears
    }
    gc_total_angle=0; // reset the Create's turn angle to 0
    while(gc_total_angle<180){ // this loop spins the robot CCW until the robot has turned 180 degrees
        create_spin_CCW(150);
        create_sensor_update(); // this will update the global gc_angle (among others)
    }
    gc_distance=0; // now that the robot is facing backwards, reset distance counter to 0
    create_drive_straight(300); //drive forward at 300mm/sec
    while(gc_distance < 1000){ // this loop terminates normally when the robot has moved 1000mm
        create_sensor_update(); // update gc_distance and other sensor globals
        if(gc_lbump || gc_rbump || gc_ldrop || gc_rdrop || //if an obstacle...
           gc_fdrop || gc_rcliff || gc_rfcliff || gc_lcliff || gc_lfcliff || b_button())
          {
            create_drive(0,1); //...stop the robot
            printf("Dangerous terrain!\nRunaway terminated\n"); //print out message that the program stopped early
            break;//get out of the while loop
        }
    }
    create_drive(0,1);//stop the robot 
    if(gc_distance >= 1000) printf("Mission Accomplished\n");//victory if the full meter was traversed
    create_disconnect(); //return XBC serial port to talking with PC instead of robot.
    //if the disconnect is not done, then the GBA has to be rebooted before the PC can talk with the XBC.
}