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

/* create-orbit.ic This program causes the create to spin in place until it 
   sees an object matching the color in color
   model 0.  It then attempts to orbit the object in a 1m radius circle.
   If the object is moved, the trajectory will tighten or loosen as needed to recenter the object
   as the robot circles. The robot plays the NBC logo theme if it 
   runs into something.
   written by D. Miller 4/07
*/
void main()
{
    int tx, radius;//tx is the coordinate of the centroid of the tracked object; if tx=178, object is centered
    init_camera(); // start up xbc camera system
    create_connect(); // start up communication with the create
    display_clear(); // clear display
    gc_song_array[1][0]=3; //load the crash tones
    gc_song_array[1][1]=79;
    gc_song_array[1][2]=45;
    gc_song_array[1][3]=88;
    gc_song_array[1][4]=45;
    gc_song_array[1][5]=84;
    gc_song_array[1][6]=45;
    create_load_song(1);
    printf("song loaded\n");
    printf("Attempting Orbit\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("Space Junk\nPlease clear the orbital path!\n"); // if sensor detects obstacle, give up
            create_drive(0,1); // stop any movement
            create_play_song(1); // play crash tones
            sleep(3.); //wait 3s and hope path is magically cleared
        }
        if (track_count(0)>0) {    //if no obstacles, are there any blobs on channel 0?
            display_clear();
            printf("Orbit achieved\n");
            tx=track_x(0,0); //if so, get the x coordinate of the centroid
            radius = 1000 - 2*(178-tx);//set the nominal orbit size
            create_drive(200,radius); //orbital speed is 0.2m/second
        }
        else create_spin_CW(50); // if no obstacle and no colored object, spin until something appears
    }
    create_drive(0,1);//stop the robot 
    printf("Mission Terminated\n");//parachuting to safety??
    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.
}