#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 chases that object until it runs into it, at which point it spins 180
   goes a few inches and then resumes the search and chase

   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_s(); // start up communication with the create
    display_clear(); // clear display
    while(!b_button()){// keep up the chase until the B button on the XBC is pressed
        printf("The chase is a foot!\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("Tag!!!!!\n"); // if sensor detects obstacle, give the tagee a chance to escape
                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(100); // 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 && b_button()==0){ // 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 < 100){ // this loop terminates normally when the robot has moved 100mm
            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!\n"); //print out message that there was an unexpected obstacle
                break;//get out of the while loop
            }
        }
        create_drive(0,1);//stop the robot 
    }
    printf("The Chase has been canceled\n");
    create_disconnect_s(); //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.
}