/*6.270 Robot Code; Team 27 (Lego My Eggo): Johanna Mathieu, Rikky Muller, 
Kyle Gilpin*/


/*threshold values for 38-600 tables only*/
#define PRESET_THRESHOLD_back_left_cds_value      35
#define PRESET_THRESHOLD_front_right_cds_value    55
#define PRESET_THRESHOLD_front_left_cds_value     56
#define PRESET_THRESHOLD_back_right_cds_value     59

/*calibrated servo positions: S0=steering wheel, S1=left claw, S2=right 
claw*/
#define S0_VERTICAL                               1930
#define S0_HORIZONTAL                             150
#define S1_CLOSED                                 0
#define S2_CLOSED                                 3900
#define S1_OPEN                                   2100
#define S2_OPEN                                   1700
#define S1_TRAPPING                               370
#define S2_TRAPPING                               3620
#define S1_STRAIGHT                               1600
#define S2_STRAIGHT                               2300
#define S1_ALL_OPEN                               3900
#define S2_ALL_OPEN                               100

/*colors*/
#define BLACK                                     1
#define WHITE                                     0

/*side_of_board*/
#define LEFT                                      1
#define RIGHT                                     0

/*boolean*/
#define TRUE                                      1
#define FALSE                                     0

/*wheels*/
#define LEFT_WHEEL                                0
#define RIGHT_WHEEL                               1

/*turning*/
#define CW                                        1
#define CCW                                       2
#define TICKS_180                                 230
#define TICKS_90                                  108

/*motion control*/
#define INCHES_PER_CLICK                          0.03095
#define WHEEL_BASE                                165.0
#define K_CLICKS                                  (584.0 / 100.0)
#define K_INT                                     2.0
#define K_PRO                                     0.5
#define ACCELERATION_FACTOR                       35.0

/*block location*/
#define OUTSIDE_TOP                               0
#define OUTSIDE_BOTTOM                            1
#define INSIDE                                    2
#define INSIDE_BOTTOM                             3
#define INCONCLUSIVE                              -1


                /* PORT DEFINITIONS */
/*motor ports*/
#define LEFT_MOTOR_A                              0
#define LEFT_MOTOR_B                              1
#define RIGHT_MOTOR_A                             2
#define RIGHT_MOTOR_B                             3

/*servo ports*/
#define STEERING_WHEEL                            0
#define LEFT_CLAW                                 1
#define RIGHT_CLAW                                2

/*digital output*/
#define IR_BEACON                                 8

/*analog ports*/
#define LEFT_DISTANCE_SENSOR                      4
#define RIGHT_DISTANCE_SENSOR                     5
#define MIDDLE_CENTER_CDS                         16
#define FRONT_LEFT_CDS                            17
#define FRONT_RIGHT_CDS                           18
#define BACK_LEFT_CDS                             19
#define BACK_RIGHT_CDS                            20

/*digital ports*/
#define LEFT_CLAW_FRONT_BUMP                      2
#define RIGHT_CLAW_FRONT_BUMP                     3
#define LEFT_BACK_BUMP                            10
#define RIGHT_BACK_BUMP                           11
#define LEFT_LIP_BUMP                             12
#define RIGHT_LIP_BUMP                            13
#define LEFT_CLAW_SIDE_BUMP                       14
#define RIGHT_CLAW_SIDE_BUMP                      15


/* GLOBAL VARIABLES */
/*cds readings*/
int back_left_cds_value;
int front_right_cds_value;
int front_left_cds_value;
int middle_center_cds_value;
int back_right_cds_value;

/*decisions*/
int location_of_block;
int side_of_board;

/*motor control*/
int pid_pid;
int pid_enable = FALSE;
int still_moving = FALSE;
int measure_distance = FALSE;
int measure_angle = FALSE;
int mtr_pwr[2] = {0,0};
int side_scan_pid;
float desired_clicks = 0.0;
float bias = 0.0;
float desired_distance = 0.0;
float integral = 0.0;
float theta = 0.0;
float delta_y = 0.0;
float delta_x = 0.0;
float mod_desired_clicks = 0.0;
float drive_start_time;
float top_dist_avg;
float bottom_dist_avg;
float bottom_measurements;
float top_measurements;


void main() {                                                              
/*MAIN FUNCTION*/

    ir_transmit_off();

    enable_servos();

    /*initalize servos positions*/
    servo(STEERING_WHEEL, S0_VERTICAL);
    servo(LEFT_CLAW, S1_CLOSED);
    servo(RIGHT_CLAW, S2_CLOSED);

    ao();

    calibration();

    /*start of competition!*/
    start_machine2(MIDDLE_CENTER_CDS, 10);
    enable_servos();

    /*start pid*/
    pid_pid=start_process(pid());
    drive(0,0.0);

    /*figure out orientation and turn toward center of board*/
    orient();

    /*first drive sequence: catch first 2 balls*/
    first_drive_sequence();

    /*second drive sequence: catch next 4 balls*/
    if (location_of_block==OUTSIDE_TOP) {

         outside_top_routine();
         }
    else if (location_of_block == OUTSIDE_BOTTOM) {

         outside_bottom_routine();
         }
    else {

        inside_routine_3_balls();

        inside_routine_last_ball();
        }
}

void first_drive_sequence(){                                               
/*FIRST DRIVE SEQUENCE FUNCTION*/
     float top_avg_dist;
     float bottom_avg_dist;
     int bottom_readings;
     int top_readings;

     int distance_sensor;

     /* printf("\nFirst Drive Sequence.");
     printf("\nCatch first ball"); */

     servo(LEFT_CLAW, S1_OPEN);
     servo(RIGHT_CLAW, S2_OPEN);

     turbo_eggo(); /* burn some rubber */
     bump_into_lip(5.0);
     servo(LEFT_CLAW, S1_CLOSED);
     servo(RIGHT_CLAW, S2_CLOSED);
     drive(-75, 3.0);
     while (still_moving==TRUE)
        ;
     turn((TICKS_180+ 15), 50, CW);
     back_into_wall(12.0);
     drive(100,0.0);
     servo(LEFT_CLAW, S1_STRAIGHT);
     servo(RIGHT_CLAW, S2_STRAIGHT);

     if (side_of_board == RIGHT)
        distance_sensor = LEFT_DISTANCE_SENSOR;
     else
        distance_sensor = RIGHT_DISTANCE_SENSOR;

     bottom_avg_dist = 0.0;
     top_avg_dist = 0.0;
     bottom_readings = 0;
     top_readings = 0;

     while (delta_x < (12.0 / INCHES_PER_CLICK))
        ;

     while ( (delta_x >= (12.0 / INCHES_PER_CLICK)) && (delta_x < (21.0 / 
INCHES_PER_CLICK)) ) {
        bottom_avg_dist += (float)analog(distance_sensor);
        bottom_readings++;
     }

     while ( (delta_x >= (21.0 / INCHES_PER_CLICK)) && (delta_x < (24.0 / 
INCHES_PER_CLICK)) )
        ;

     while ( (delta_x >= (24.0 / INCHES_PER_CLICK)) && 
(!digital(LEFT_CLAW_FRONT_BUMP)) && (!digital(RIGHT_CLAW_FRONT_BUMP)) ) {
        top_avg_dist += (float)analog(distance_sensor);
        top_readings++;
     }

     drive(0,0.0);


     /* printf("\nCatch second ball."); */
     servo(LEFT_CLAW, S1_TRAPPING);
     servo(RIGHT_CLAW, S2_TRAPPING);
     drive(-75,1.5);
     while (still_moving==TRUE)
         ;
     if (side_of_board == RIGHT)
        turn(TICKS_180, 50, CCW);
     else
        turn(TICKS_180, 50, CW);

     back_into_wall(5.0);
     drive(100,0.0);

     if (side_of_board == RIGHT)
        distance_sensor = RIGHT_DISTANCE_SENSOR;
     else
        distance_sensor = LEFT_DISTANCE_SENSOR;

     while (delta_x < (12.0 / INCHES_PER_CLICK))
        ;

     while ( (delta_x >= (12.0 / INCHES_PER_CLICK)) && (delta_x < (21.0 / 
INCHES_PER_CLICK)) ) {
        top_avg_dist += (float)analog(distance_sensor);
        top_readings++;
     }

     while ( (delta_x >= (21.0 / INCHES_PER_CLICK)) && (delta_x < (24.0 / 
INCHES_PER_CLICK)) )
        ;

     while ( (delta_x >= (24.0 / INCHES_PER_CLICK)) && 
(!digital(LEFT_LIP_BUMP)) && (!digital(RIGHT_LIP_BUMP)) ) {
        bottom_avg_dist += (float)analog(distance_sensor);
        bottom_readings++;
     }

     bottom_avg_dist /= (float)bottom_readings;
     top_avg_dist /= (float)top_readings;

     if ( (top_avg_dist < 50.0) && (bottom_avg_dist < 50.0) ) {
        if ( (fabs(top_avg_dist - bottom_avg_dist)) > 15.0 ) {
            if (top_avg_dist > bottom_avg_dist)
                location_of_block = OUTSIDE_TOP;
            else
                location_of_block = OUTSIDE_BOTTOM;
        }
        else
            location_of_block = INSIDE;
     }
     else {
        if ( (fabs(top_avg_dist - bottom_avg_dist)) > 15.0 ) {
            if (top_avg_dist > bottom_avg_dist)
                location_of_block = OUTSIDE_TOP;
            else
                location_of_block = OUTSIDE_BOTTOM;
        }
        else if ( (top_avg_dist >= 50.0) && (bottom_avg_dist < 50.0) )
            location_of_block = OUTSIDE_TOP;
        else if ( (bottom_avg_dist >= 50.0) && (bottom_avg_dist < 50.0) )
            location_of_block = OUTSIDE_BOTTOM;
        else if ( (top_avg_dist >= 50.0) && (bottom_avg_dist >= 50.0) && 
(fabs(top_avg_dist - bottom_avg_dist) > 5.5) ){
            if (top_avg_dist > bottom_avg_dist)
                location_of_block = OUTSIDE_TOP;
            else
                location_of_block = OUTSIDE_BOTTOM;
        }
        else
            location_of_block = INSIDE;
     }

     printf("%f, %d, %f, %d", top_avg_dist, top_readings, bottom_avg_dist, 
bottom_readings);

     bump_into_lip(5.0);
     servo(LEFT_CLAW, S1_OPEN);
     servo(RIGHT_CLAW, S2_OPEN);
     sleep(.5);
     servo(LEFT_CLAW, S1_CLOSED);
     servo(RIGHT_CLAW, S2_CLOSED);
}


void outside_top_routine(){                                                
/*OUTSIDE TOP ROUTINE FUNCTION*/
    drive(-100, 7.75);
    while (still_moving==TRUE)
        ;
    if (side_of_board==LEFT)
           turn(TICKS_90, 50, CCW);
    else
           turn(TICKS_90, 50, CW);
    while (still_moving==TRUE)
        ;
    back_into_wall(12.0);
    if (side_of_board==LEFT){
        servo(RIGHT_CLAW, S2_ALL_OPEN);
        servo(LEFT_CLAW, S1_OPEN);
        }
    else {
        servo(LEFT_CLAW, S1_ALL_OPEN);
        servo(RIGHT_CLAW, S2_OPEN);
        }
    drive(50, 10.5);
    check_front_bumps();
    if (side_of_board==LEFT) {
        turn((TICKS_90+20), 98, CW);
        while (still_moving)
            ;
    }
    else {
        turn((TICKS_90+20), 98, CCW);
        while (still_moving)
            ;
    }


    servo(LEFT_CLAW, S1_ALL_OPEN);
    servo(RIGHT_CLAW, S2_ALL_OPEN);

    drive(100, 0.0);
    bump_into_lip(4.0);
    sleep(.3);

    servo(LEFT_CLAW, S1_CLOSED);
    servo(RIGHT_CLAW, S2_CLOSED);

    drive(-100, 3.0);
    while (still_moving)
        ;


    if (side_of_board == RIGHT)
        turn(TICKS_90, 50, CW);
    else
        turn(TICKS_90, 50, CCW);

    drive(100, 16.0);
    while (still_moving)
        ;

    if (side_of_board == RIGHT)
        turn(TICKS_90, 50, CW);
    else
        turn(TICKS_90, 50, CCW);

    back_into_wall(3.0);

    servo(LEFT_CLAW, S1_OPEN);
    servo(RIGHT_CLAW, S2_OPEN);


    drive(70, 6.75);

    while ((delta_x * INCHES_PER_CLICK) < 5.5)
        ;

    servo(LEFT_CLAW, S1_TRAPPING);
    servo(RIGHT_CLAW, S2_TRAPPING);

    while(still_moving)
        ;

    turn(265, 50, CW); /* special turn for two balls trapped */

    drive(100, 0.0);

    while ((!digital(LEFT_LIP_BUMP)) && (!digital(RIGHT_LIP_BUMP)))
        ;

    servo(LEFT_CLAW, S1_ALL_OPEN);
    servo(RIGHT_CLAW, S2_ALL_OPEN);

    sleep(.5);

    servo(LEFT_CLAW, S1_CLOSED);
    servo(RIGHT_CLAW, S2_CLOSED);
    drive(-100,3.0);
    while (still_moving==TRUE)
        ;
    turn(TICKS_180, 50, CW);
    back_into_wall(5.0);
    drive(100,0.0);
    sleep(1.0);

    if (side_of_board == RIGHT) {
        servo(LEFT_CLAW, S1_STRAIGHT);
        servo(RIGHT_CLAW, S2_STRAIGHT);
    }
    else {
        servo(LEFT_CLAW, S1_STRAIGHT);
        servo(RIGHT_CLAW, S2_STRAIGHT);
    }

    while ((!digital(RIGHT_CLAW_FRONT_BUMP)) && 
(!digital(LEFT_CLAW_FRONT_BUMP)))
        ;
    drive(0,0.0);

    servo(LEFT_CLAW, S1_TRAPPING);
    servo(RIGHT_CLAW, S2_TRAPPING);

    drive(-100, 3.5);
    while(still_moving)
        ;

    if (side_of_board == RIGHT)
        turn(TICKS_180, 50, CW);
    else
        turn(TICKS_180, 50, CCW);

    turbo_eggo();
    /* check_front_bumps(); */

    servo(LEFT_CLAW, S1_ALL_OPEN);
    servo(RIGHT_CLAW, S2_ALL_OPEN);

}


void outside_bottom_routine(){                                             
/*OUTSIDE BOTTOM ROUTINE FUNCTION*/

    drive(-100, 24.5);
    while(still_moving == TRUE)
        ;

    if (side_of_board == RIGHT) {
        turn(TICKS_90, 50, CW);
        back_into_wall(5.0);
        servo(LEFT_CLAW, S1_STRAIGHT);
        servo(RIGHT_CLAW, S2_OPEN);
    }
    else {
        turn(TICKS_90, 50, CCW);
        back_into_wall(5.0);
        servo(LEFT_CLAW, S1_OPEN);
        servo(RIGHT_CLAW, S2_STRAIGHT);
    }

    drive(75, 10.75);
    check_front_bumps();

    while (still_moving==TRUE)
        ;
    servo(LEFT_CLAW, S1_TRAPPING);
    servo(RIGHT_CLAW, S2_TRAPPING);

    if (side_of_board == RIGHT)
        turn(TICKS_90, 50, CCW);
    else
        turn(TICKS_90, 50, CW);

    back_into_wall(5.0);

    drive(50, 1.25);
    while (still_moving == TRUE)
        ;
    if (side_of_board == RIGHT)
        turn(TICKS_90, 50, CW);
    else
        turn(TICKS_90, 50, CCW);

    drive(50, 17.0);
    while(still_moving == TRUE)
        ;
    if (side_of_board == RIGHT)
        turn(TICKS_90, 50, CCW);
    else
        turn(TICKS_90, 50, CW);

    back_into_wall(6.0);

    if (side_of_board == RIGHT) {
        servo(LEFT_CLAW, S1_STRAIGHT);
        servo(RIGHT_CLAW, S2_OPEN);
    }
    else {
        servo(LEFT_CLAW, S1_OPEN);
        servo(RIGHT_CLAW, S2_STRAIGHT);
    }

    drive(100, 0.0);
    check_front_bumps();

    drive(0,0.0);

    controlled_release();
}


void inside_routine_3_balls(){                                             
/*INSIDE ROUTINE 3 BALLS FUNCTION*/

        back_into_wall(12.0);
        drive(25, 2.5);
        while (still_moving==TRUE)
            ;
        if (side_of_board==RIGHT)
            turn(TICKS_90, 50, CW);
        else
            turn(TICKS_90, 50, CCW);
        while (still_moving==TRUE)
            ;

        back_into_wall(5.0);
        drive(100, 14.5);
        while (still_moving==TRUE)
            ;
        drive(0,0.0);

        if (side_of_board==RIGHT)
            turn(TICKS_90, 50, CCW);
        else
            turn(TICKS_90, 50, CW);

        back_into_wall(5.0);

        if (side_of_board == RIGHT) {
            servo(LEFT_CLAW, S1_OPEN);
            servo(RIGHT_CLAW, S2_STRAIGHT);
        }
        else {
            servo(LEFT_CLAW, S1_STRAIGHT);
            servo(RIGHT_CLAW, S2_OPEN);
        }


        drive(100,0.0);

        check_front_bumps();

        bump_into_lip(5.0);
        servo(LEFT_CLAW, S1_ALL_OPEN);
        servo(RIGHT_CLAW, S2_ALL_OPEN);

        sleep(.2);


}

void inside_routine_last_ball(){                                           
/*INSIDE ROUTINE LAST BALL FUNCTION*/
    int i;
    float dist_reading;

    servo(LEFT_CLAW, S1_CLOSED);
    servo(RIGHT_CLAW, S2_CLOSED);

    drive(-100, 3.0);
    while (still_moving == TRUE)
        ;

    if (side_of_board == RIGHT) {
        dist_reading = (float)analog(RIGHT_DISTANCE_SENSOR);

        for (i = 0; i <= 3; i++) {
            drive(-100, 1.0);
            while (still_moving)
                ;
            dist_reading += (float)analog(RIGHT_DISTANCE_SENSOR);
        }

        dist_reading /= 4.0;

        if (dist_reading > 75.0)
            location_of_block = INSIDE_BOTTOM;


        if (location_of_block != INSIDE_BOTTOM) {
            drive(100, 2.5);
            while(still_moving)
                ;

            servo(RIGHT_CLAW, S2_OPEN);
            servo(LEFT_CLAW, S1_OPEN);
            turn(TICKS_90, 50, CW);
            drive(100, 9.0);
            while ((delta_x*INCHES_PER_CLICK)<7.5)
                ;

            servo(LEFT_CLAW, S1_TRAPPING);
            servo(RIGHT_CLAW, S2_TRAPPING);

        }
        else {
            servo(RIGHT_CLAW, S2_CLOSED);
            drive(-100, 7.75);
            while (still_moving == TRUE)
                ;
            turn(TICKS_90, 50, CW);
            servo(RIGHT_CLAW, S2_OPEN);
            servo(LEFT_CLAW, S1_STRAIGHT);
            drive(100, 9.0);
            while ((delta_x*INCHES_PER_CLICK)<7.5)
                ;
            servo(LEFT_CLAW, S1_TRAPPING);
            servo(RIGHT_CLAW, S2_TRAPPING);

        }
        while (still_moving)
            ;

        back_into_wall(12.0);
        drive (100, 4.0);
        while (still_moving)
            ;

        if (side_of_board == RIGHT)
            turn(TICKS_90, 50, CCW);
        else
            turn(TICKS_90, 50, CW);

        turbo_eggo();

        if (side_of_board == RIGHT) {
            servo(LEFT_CLAW, S1_ALL_OPEN);
            servo(RIGHT_CLAW, S2_STRAIGHT);
        }
        else {
            servo(LEFT_CLAW, S1_STRAIGHT);
            servo(RIGHT_CLAW, S2_ALL_OPEN);
        }
    }
    else {
        dist_reading = (float)analog(LEFT_DISTANCE_SENSOR);

        for (i = 1; i <= 3; i++) {
            drive(-100, 1.0);
            while(still_moving)
                ;
            dist_reading += (float)analog(LEFT_DISTANCE_SENSOR);
        }

        dist_reading /= 4.0;

        if (dist_reading > 75.0)
            location_of_block = INSIDE_BOTTOM;

        if (location_of_block != INSIDE_BOTTOM) {
            drive(0, 2.5);
            while (still_moving)
                ;
            servo(RIGHT_CLAW, S2_OPEN);
            servo(LEFT_CLAW, S1_OPEN);
            turn(TICKS_90, 50, CCW);
            drive(100, 9.0);
            while ((delta_x*INCHES_PER_CLICK)<7.5)
                ;
            servo(LEFT_CLAW, S1_TRAPPING);
            servo(RIGHT_CLAW, S2_TRAPPING);
        }
        else {
            servo(LEFT_CLAW, S1_CLOSED);
            drive(-100, 7.75);
            while (still_moving == TRUE)
                ;
            turn(TICKS_90, 50, CCW);
            servo(RIGHT_CLAW, S2_STRAIGHT);
            servo(LEFT_CLAW, S1_OPEN);
            drive(100, 9.0);
            while ( (delta_x*INCHES_PER_CLICK) < 7.5)
                ;
            servo(LEFT_CLAW, S1_TRAPPING);
            servo(RIGHT_CLAW, S2_TRAPPING);
            }
        while (still_moving)
            ;

        back_into_wall(12.0);
        drive (100, 4.0);
        while (still_moving)
            ;

        if (side_of_board == RIGHT)
            turn(TICKS_90, 50, CCW);
        else
            turn(TICKS_90, 50, CW);

        turbo_eggo();

        if (side_of_board == RIGHT) {
            servo(LEFT_CLAW, S1_STRAIGHT);
            servo(RIGHT_CLAW, S2_ALL_OPEN);
        }
        else {
            servo(LEFT_CLAW, S1_ALL_OPEN);
            servo(RIGHT_CLAW, S2_STRAIGHT);
        }
    }
}


void controlled_release() {                                                
/*CONTROLLED RELEASE FUNCTION*/
    int k;

    if (side_of_board == RIGHT) {
        servo(RIGHT_CLAW, S2_STRAIGHT);
        for (k = 1; k <= 4; k++) {
            servo(LEFT_CLAW, S1_CLOSED);
            sleep(.45);
            servo(LEFT_CLAW, (S1_OPEN+200));
            sleep(.18);
        }
        servo(LEFT_CLAW, S1_ALL_OPEN);
    }
    else {
        servo(LEFT_CLAW, S1_STRAIGHT);
        for (k = 1; k <= 4; k++) {
            servo(RIGHT_CLAW, S2_CLOSED);
            sleep(.45);
            servo(RIGHT_CLAW, (S2_OPEN-200));
            sleep(.18);
        }
        servo(RIGHT_CLAW, S2_ALL_OPEN);
    }
}

void back_into_wall(float time_out) {                                      
/*BACK INTO WALL FUNCTION*/
     float start_time;

     servo(STEERING_WHEEL, S0_VERTICAL);
     drive(-100,0.0);
     start_time=seconds();
     while 
((!digital(LEFT_BACK_BUMP))&&(!digital(RIGHT_BACK_BUMP))&&((seconds() - 
start_time)<time_out))
	    ;
     drive(0, 0.0);
	 kill_process(pid_pid);
     while 
(((!digital(LEFT_BACK_BUMP))||(!digital(RIGHT_BACK_BUMP)))&&(((seconds() - 
start_time)<time_out))) {

        sleep(.2);
        if ((!digital(LEFT_BACK_BUMP))&&(!digital(RIGHT_BACK_BUMP))){
	       /* Servo0 Horizontal command removed from here and below */
	       motor(LEFT_MOTOR_A, -50);
	       motor(LEFT_MOTOR_B, -50);
	       motor(RIGHT_MOTOR_A, -50);
	       motor(RIGHT_MOTOR_B, -50);
           ;
        }
        else if ((!digital(LEFT_BACK_BUMP))&&(digital(RIGHT_BACK_BUMP))){

	        motor(LEFT_MOTOR_A, -50);
            motor(LEFT_MOTOR_B, -50);
            motor(RIGHT_MOTOR_A, 70);
            motor(RIGHT_MOTOR_B, 70);
            ;
        }
        else if ((digital(LEFT_BACK_BUMP))&&(!digital(RIGHT_BACK_BUMP))){
            motor(LEFT_MOTOR_A, 70);
            motor(LEFT_MOTOR_B, 70);
            motor(RIGHT_MOTOR_A, -50);
            motor(RIGHT_MOTOR_B, -50);
            ;
        }
     }
     ao();
     servo(STEERING_WHEEL, S0_VERTICAL);
     pid_pid = start_process(pid());
     drive(0,0.0);
}


void bump_into_lip(float time_out) {                                       
/*BUMP INTO LIP FUNCTION*/
    float start_time;

    start_time=seconds();
    while 
((!digital(LEFT_LIP_BUMP))&&(!digital(RIGHT_LIP_BUMP))&&((seconds()-start_time)<time_out))
	    ;
    kill_process(pid_pid);
    servo(STEERING_WHEEL, S0_VERTICAL);
    start_time=seconds();
    while 
((!digital(LEFT_LIP_BUMP))||(!digital(RIGHT_LIP_BUMP))&&((seconds()-start_time)<time_out)){
        if ((!digital(LEFT_LIP_BUMP))&&(!digital(RIGHT_LIP_BUMP))){
            motor(LEFT_MOTOR_A, 50);
            motor(LEFT_MOTOR_B, 50);
            motor(RIGHT_MOTOR_A, 50);
            motor(RIGHT_MOTOR_B, 50);
        }
        else if ((!digital(LEFT_LIP_BUMP))&&(digital(RIGHT_LIP_BUMP))){
            motor(LEFT_MOTOR_A, 50);
            motor(LEFT_MOTOR_B, 50);
            motor(RIGHT_MOTOR_A, -70);
            motor(RIGHT_MOTOR_B, -70);
        }
        else if ((digital(LEFT_LIP_BUMP))&&(!digital(RIGHT_LIP_BUMP))){
            motor(LEFT_MOTOR_A, -70);
            motor(LEFT_MOTOR_B, -70);
            motor(RIGHT_MOTOR_A, 50);
            motor(RIGHT_MOTOR_B, 50);
        }
    }

    ao();
    servo(STEERING_WHEEL,S0_VERTICAL);
	pid_pid = start_process(pid());
	drive(0,0.0);
}

void check_front_bumps () {                                                
/*CHECK FRONT BUMPS FUNCTION*/

    float corrected = 1.0;
    float dist_traveled = 0.0;
    float total_desired_dist;
    int previous_speed;
    int previous_servo_position;

    total_desired_dist=desired_distance * INCHES_PER_CLICK;

    while (corrected < 5.0) {
        if (digital(LEFT_CLAW_SIDE_BUMP)) {
            servo(LEFT_CLAW, (servo1 - 250));
            corrected++;
        }

        if (digital(RIGHT_CLAW_SIDE_BUMP)) {
            servo(RIGHT_CLAW, (servo2 + 250));
            corrected++;
        }

        if (digital(LEFT_CLAW_FRONT_BUMP)){

            previous_speed = (int)(desired_clicks / K_CLICKS);
            previous_servo_position = servo2;
            servo(RIGHT_CLAW, S2_STRAIGHT);

            dist_traveled += (delta_x * INCHES_PER_CLICK);

            drive(-50,0.75);
            servo(LEFT_CLAW,(servo1-450));
            servo(RIGHT_CLAW, previous_servo_position);

            while 
((still_moving==TRUE)&&(!digital(LEFT_BACK_BUMP))&&(!digital(RIGHT_BACK_BUMP)))
                ;

            if (total_desired_dist != 0.0)
                drive((previous_speed + 5),(total_desired_dist - 
dist_traveled + (2.0*(corrected))));
            else
                drive((previous_speed + 5),0.0);

    	    corrected++;
        }

        if (digital(RIGHT_CLAW_FRONT_BUMP)){

            previous_speed = (int)(desired_clicks / K_CLICKS);
            previous_servo_position = servo1;
            servo(LEFT_CLAW, S2_STRAIGHT);

            dist_traveled += (delta_x * INCHES_PER_CLICK);

            drive(-50,0.75);
            servo(LEFT_CLAW,previous_servo_position);
            servo(RIGHT_CLAW, (servo2+450));

            while 
((still_moving==TRUE)&&(!digital(LEFT_BACK_BUMP))&&(!digital(RIGHT_BACK_BUMP)))
                ;

            if (total_desired_dist != 0.0)
                drive((previous_speed + 5),(total_desired_dist - 
dist_traveled + (2.0*(corrected))));
            else
                drive((previous_speed + 5),0.0);

       	    corrected++;
        }

    	if ((digital(LEFT_LIP_BUMP))||(digital(RIGHT_LIP_BUMP)))
            corrected=100.0;

        if (still_moving==FALSE)
            corrected=100.0;

        sleep(.1);
    }
}