#use "roombalib.ic"

/* This program uses the Roomba sensors and three sonars hooked into the XBC to 
   guide the Roomba through a maze of obstacles.  Most of the time the sonars will
   keep the Roomba out of trouble, but occasionally the bumpers will be needed.
   The cliff and drop sensors are used to terminate the program (when robot is picked up).
   The XBC v3 can turn the roomba on and off, so all that is needed is the serial line to
   be connected between the two. If an earlier XBC is used, the the Roomba must be 
   turned on before starting the program.

   written by Dave Miller 4/2007
*/ 

void main ()
{
    int ls, cs, rs;
    roomba_connect(); // start up communication with the roomba
    sleep(1.);
    display_clear(); // clear display
    roomba_dirt_led(1); // turn on some LEDs so we know comm is working
    roomba_status_led(3);
    printf("Avoiding Obstacles\n");
    while(!gr_ldrop && !gr_rdrop && //if an obstacle...
          !gr_fdrop && !gr_rcliff && !gr_rfcliff && !gr_lcliff && !gr_lfcliff && !b_button())
      {
        ls=sonar(14); //update the sonar values for left
        cs=sonar(12); // center
        rs=sonar(10); // and right sonars
        roomba_sensor_update(); //update bumpers etc
        if(gr_lbump){ // if sonars failed and there was a collision on the left turn right for a seond
            roomba_spin_CW(200);
            sleep(1.);
        }
        else {
            if(gr_rbump){// if a bump on the right turn left
                roomba_drive(0,1);
                roomba_spin_CCW(220);
                sleep(1.2); // different turn times get ou out of oscillations, eventually
            }
            else{               
                printf("Sonars L: %d C: %d R: %d\n", ls , cs, rs); // debug info
                if(ls > 450 && cs > 450 && rs > 450){//check inf anything is too close
                    roomba_drive(250,-30000); // my roomba curves slightly left, so this is a correction
                }
                else {
                    if(ls > rs){
                        roomba_drive(150,50); // do a tight turn to avoid sonar detected obstacle
                        printf("Avoiding Obstacles on right\n");
                    }
                    else{
                        roomba_drive(160,-50); // do a turn to the right if left sonar is shorter than right
                        printf("Avoiding Obstacles on Left\n");
                    }
                }
            }
        }
    }
    roomba_drive_straight(0); //if roomba is picked up or hits cliff, stop and shutdown
    roomba_disconnect();
    printf("Whee I'm flying!\n");
}