/* 6.270 Robot Test Program 1/19/01 
 * Group 53: J.Xu, C.Law, A.Ramakrishnan 
 */

/* Motor Constants */
#define LEFT_MOTOR1 0
#define LEFT_MOTOR2 1
#define RIGHT_MOTOR1 2
#define RIGHT_MOTOR2 3
#define FRONT_GATE 4
#define BACK_GATE 5

/* Global Constants */
/* Speed Constants */
#define FAST 100
#define MEDIUM_FAST 83
#define MEDIUM_SLOW 66
#define SLOW 50
#define SLOWER 33
#define VERY_SLOW 16
#define STOP 0

/* Direction Constants */
#define WEST 16
#define SOUTH 17
#define EAST 18
#define NORTH 19
#define FRONT NORTH
#define BACK SOUTH
#define LEFT WEST
#define RIGHT EAST

/* LED output port and Photo resistor ports */
#define LEDDIGITAL 7  /* Digital Output Port 7 */
#define FRONT_RIGHT_PR 2
#define BACK_RIGHT_PR 3
#define BACK_LEFT_PR 4
#define CENTER_PR 5

/***************************/
/*** Digital Input Ports ***/
/***************************/

/* Shaft Encoder Constants */
#define LEFT_SHAFT_ENCODER 7
#define RIGHT_SHAFT_ENCODER 8

/* Gate Push Button Sensors */
#define FRONT_BUTTON1 9
#define FRONT_BUTTON2 10

/* Corner Push Button Sensors */
#define SIDE_LEFT_BUTTON 11
#define SIDE_RIGHT_BUTTON 12
#define BACK_RIGHT_BUTTON 13
#define BACK_LEFT_BUTTON 14

/* Angle calibrations for shaft encoder ticks */
#define QTR_ROTATION 54
#define QTR_ROTATION_R 50
#define HALF_ROTATION 125
#define HALF_ROTATION_R 120
#define FULL_ROTATION 280
#define FULL_ROTATION_R 275

/* Color Constants */
#define DARK 0
#define LIGHT 1

/* Gate State Constants */
#define OPEN 1
#define CLOSED 0

/* Servo constants */
#define TURN 0
#define DRIVE 1
#define ARM_SERVO 0
#define THIRD_WHEEL_SERVO 1

/* Global time variable */
float start_time, time;
int floor_color;

/* Global gate state variable */
int front_gate = OPEN;
int back_gate = OPEN;

/* Global variables for floor color */
int fr_color, bl_color, br_color, c_color;
int our_side;
int led_read = 0;

/* Global third wheel state. Could be TURN or DRIVE */
int third_wheel = TURN;

/*********************/
/*** main function ***/
/*********************/
void main(){
  ir_transmit_off();
  printf("**Robot On**\n");
  sleep(2.0);

  /* Main Program Stuff */ 
  clear_digital_out(LEDDIGITAL);  
  start_machine(CENTER_PR);
  setup(); 
  orient();
  drive(FAST, 0.5);
  close_gate(FRONT_GATE);
  extend_arm();
  drive(-FAST, 1.5);
  retract_arm();
  raise_gate(FRONT_GATE);
  start_process(check_front_ball());
  
  first_pass();
  second_pass();
  last_attempt();
  
  /* calibration functions */
  /* read_floor_colors(); */
  /* hard_right(get_tick_value(), 60.0); */
  /* start_process(check_front_ball());
     raise_gate(FRONT_GATE);
     drive_until_gate(FAST, 5.0, &front_gate);
     drive_backwards_until_wall(FAST, 10.0);
     */
  /*   drive_until_gate(SLOW, 5.0, &front_gate);
       raise_gate(FRONT_GATE);
       while(!start_button()){}
       drive_until_gate(FAST, 5.0, &front_gate);
       while(!start_button()){}
       drive_until_gate(-SLOW, 5.0, &front_gate);
       raise_gate(FRONT_GATE);
       while(!start_button()){}
       drive_until_gate(-FAST, 5.0, &front_gate);
       shut_down();
       */
}

/***********************************/
/****** Strategic Scripting ********/
/***********************************/
void first_pass(){
  /* in the beginning of the first pass, we are situated in the
     start position facing towards opponent side */
  drive_until_gate(FAST, 7.0, &front_gate);
  if((front_gate==CLOSED) && (time<60.0) ){
    /* captured the first ball */
    printf("GATE CLOSED\n");
    drive_until_colors(-FAST, 8.0, our_side);
    drive(-FAST, 2.0);
    if ((fr_color==our_side)&&(br_color==our_side)
	&&(bl_color==our_side)&&(c_color==our_side)&&(time<60.0)){
      /* we are indeed on our side ok to release */
      raise_gate(FRONT_GATE);
      hard_left(HALF_ROTATION, 10.0);
      hard_right(HALF_ROTATION_R, 4.0); /* manual adjustment */
    }
  }
  else{
    printf("DIDN'T GET BALL\n");
    /* didn't get the first ball. Turn to get second one */
    if(time<60.0){
      close_gate(FRONT_GATE);
      drive_until_center_color(-SLOW, 3.0, our_side);
      hard_left(QTR_ROTATION, 10.0);
      /* hard_left(7, 1.0); manual adjustment */
      raise_gate(FRONT_GATE);
      drive_until_gate(FAST, 7.0, &front_gate);
      close_gate(FRONT_GATE);
      drive_backwards_until_wall(FAST, 7.0);
      drive(FAST, 0.3); /* manual adjustment */
      hard_left(QTR_ROTATION, 4.0);
      drive_until_colors(FAST, 8.0, our_side);
      drive(FAST, 2.0);
      if ((fr_color==our_side)&&(br_color==our_side)
	  &&(bl_color==our_side)&&(c_color==our_side)&&(time<60.0)){
	/* we are indeed on our side ok to release */
	raise_gate(FRONT_GATE);
	drive(-FAST, 1.0);
	hard_right(HALF_ROTATION_R, 10.0);
      }
    }
  }
}

void second_pass(){
  /* In the beginning of the second pass, we are situated
     on our side with balls already dropped out of the cabin
     we are facing opponent's side and need to do another run */
  if(time<60.0){
    drive(FAST, 0.4); /* manual adjustment */
    hard_left(15, 1.0); /* manual adjustment */
    drive_until_colors(FAST, 7.0, !our_side);
    close_gate(FRONT_GATE);
    extend_arm();
    drive_until_colors(-FAST, 7.0, our_side);
    retract_arm();
    raise_gate(FRONT_GATE);
    drive_until_gate(FAST, 7.0, &front_gate);
    close_gate(FRONT_GATE);
    drive_until_center_color(-SLOW, 3.0, our_side);
    hard_left(QTR_ROTATION, 4.0);
    hard_left(7, 1.0); /* manual adjustment */
    raise_gate(FRONT_GATE);
    drive(FAST, 7.0);
    close_gate(FRONT_GATE);
    drive_backwards_until_wall(FAST, 6.0);
    drive(FAST, 0.3); /* manual adjustment */
    hard_left(QTR_ROTATION, 3.0);
    drive_until_colors(FAST, 10.0, our_side);
    drive(FAST, 2.0);
    if ((fr_color==our_side)&&(br_color==our_side)
	&&(bl_color==our_side)&&(c_color==our_side)&&(time<60.0)){
      /* we are indeed on our side ok to release */
      raise_gate(FRONT_GATE);
    }
  }
}

void last_attempt(){
  /* In the beginning of the last_attempt, we are situated 
     on our side with balls already dropped out of the cabin
     from the end of the second pass. We do a last attempt 
     at swinging all center balls */
  drive_until_back_right_color(-FAST, 4.0, !our_side);
  extend_arm();
  while(time<55.0)
    hard_left(FULL_ROTATION, 60.0);
  retract_arm();
}

/*******************/
/*** Threads *******/
/*******************/
/* timer thread */
void timer(){
  start_time = seconds();
  while(1){
    time = seconds()-start_time;
  }
}

/* checking whether we're within the 60 seconds */
int check_time(){
  if(time < 60.0)
    return 1;
  else
    return 0;
}

/* check for front ball thread */
void check_front_ball(){
  while((!digital(FRONT_BUTTON1))&&(!digital(FRONT_BUTTON2))){
    /* do nothing */
  }
  close_gate(FRONT_GATE);
}

/* check for LED values */
void led_thread(){
  int fron, bron, blon, con;
  int fr_threashold = 12; 
  int br_threashold = 46;
  int bl_threashold = 84;
  int c_threashold = 31;
  set_digital_out(LEDDIGITAL);  
  sleep(0.25);
  while(1){
    /* reading values */
    fron = analog(FRONT_RIGHT_PR); 
    bron = analog(BACK_RIGHT_PR);
    blon = analog(BACK_LEFT_PR); 
    con = analog(CENTER_PR);
    
    if (fron > fr_threashold)
      fr_color = DARK;
    else
      fr_color = LIGHT;
    if (bron > br_threashold)
      br_color = DARK;
    else 
      br_color = LIGHT;
    if (blon > bl_threashold)
      bl_color = DARK;
    else
      bl_color = LIGHT;
    if (con > c_threashold)
      c_color = DARK;
    else
      c_color = LIGHT;
    led_read = 1;
  }
}

/*******************************************/
/**** Testing and Calibration Functions ****/
/*******************************************/
/*** Testing turning via shaft encoder tick calibration ***/
int get_tick_value(){
  int value =0;
  printf("Tick Value: %d\n", value);
  while(!stop_button()){
    sleep(0.25);
    if(start_button()){
      value+=5;
      printf("Tick Value: %d\n", value);
    }
  }
  return value;
}

/* testing corner sensors */
void test_corner_sensors(){
  while(1){
    printf("fl:%d fr:%d br:%d, bl%d\n", digital(SIDE_LEFT_BUTTON), 
	   digital(SIDE_RIGHT_BUTTON), 
	   digital(BACK_RIGHT_BUTTON), 
	   digital(BACK_LEFT_BUTTON));
  }
}

/* testing servo */
void test_servo(){
  enable_servos();
  servo(0, 2900);
  sleep(2.0);
  servo(0, 0);
  /* printf("Turning 0\n");
     servo(0, 0);
     sleep(2.0);
     printf("Turning 4000\n");
     servo(0, 4000); */
}

/* Calibrating floor sensors */
void read_floor_colors(){
  int fron, bron, blon, con, froff, broff, bloff, coff;
  while(1){
    set_digital_out(LEDDIGITAL);
    sleep(0.25);
    /* reading values */
    fron = analog(FRONT_RIGHT_PR); 
    bron = analog(BACK_RIGHT_PR);
    blon = analog(BACK_LEFT_PR); 
    con = analog(CENTER_PR);
    clear_digital_out(LEDDIGITAL);
    sleep(0.25);
    froff = analog(FRONT_RIGHT_PR);
    broff = analog(BACK_RIGHT_PR);
    bloff = analog(BACK_LEFT_PR);
    coff = analog(CENTER_PR);
    printf("%d:%d:%d:%d %d:%d:%d:%d\n", fron, bron, blon, con, froff, broff, bloff, coff);
    /* printf("fr:%d br:%d bl:%d c:%d\n", froff-fron, broff-bron, bloff-blon, coff-con); */
  }
}

/***********************/
/*** Motor Functions ***/
/***********************/
/* power left motors */
void power_left_motors(int speed){
  motor(LEFT_MOTOR1, speed);
  motor(LEFT_MOTOR2, speed);
}

/* power right motors */
void power_right_motors(int speed){
  motor(RIGHT_MOTOR1, speed);
  motor(RIGHT_MOTOR2, speed);
}

/* gate commands */
/* raising gate */
void raise_gate(int gate){
  /* positive front */
  if (gate == FRONT_GATE){
    motor(FRONT_GATE, FAST);
    sleep(0.4);
    front_gate = OPEN;
  }
  /* negative back */
  else if(gate == BACK_GATE){
    motor(BACK_GATE, -FAST);
    sleep(0.4);
    back_gate = OPEN;
  }
  motor(gate, STOP);
}

/* closing gate */
void close_gate(int gate){
  /* negative front */
  if (gate == FRONT_GATE){
    motor(FRONT_GATE, -FAST);
    sleep(0.5);
    front_gate = CLOSED;
  }
  /* positive back  */
  else if (gate == BACK_GATE){
    motor(BACK_GATE, FAST);
    sleep(0.5);
    back_gate = CLOSED;
  }
  motor(gate, STOP);
}


/**************************/
/*** Movement Functions ***/
/**************************/
/* moving straight */
int drive(int speed, float duration){
  float begin_time;
  int left, right;
  if(third_wheel!= DRIVE){
    set_third_wheel(DRIVE);
  }
  printf("Driving at: %d\n", speed);
  enable_encoder(LEFT_SHAFT_ENCODER);
  enable_encoder(RIGHT_SHAFT_ENCODER);
  reset_encoder(LEFT_SHAFT_ENCODER);
  reset_encoder(RIGHT_SHAFT_ENCODER);
  begin_time = seconds();
  power_left_motors(speed);
  power_right_motors(speed);
  while((seconds()-begin_time < duration)&&(time<60.0)){
    left = read_encoder(LEFT_SHAFT_ENCODER);
    right = read_encoder(RIGHT_SHAFT_ENCODER);
    if((left+1 > right) && (time<60.0)){
      power_left_motors(STOP);
      left = read_encoder(LEFT_SHAFT_ENCODER);
      right = read_encoder(RIGHT_SHAFT_ENCODER);
      while((left>right) && (time<60.0)){
	  left = read_encoder(LEFT_SHAFT_ENCODER);
	  right = read_encoder(RIGHT_SHAFT_ENCODER);
      }
      power_left_motors(speed);
    }
    else if ((left < right+1) && (time<60.0)){
      power_right_motors(STOP);
      left = read_encoder(LEFT_SHAFT_ENCODER);
      right = read_encoder(RIGHT_SHAFT_ENCODER);
      while((left<right) && (time<60.0)){
	left = read_encoder(LEFT_SHAFT_ENCODER);
        right = read_encoder(RIGHT_SHAFT_ENCODER);
      }
      power_right_motors(speed);
    }
    printf("%d, %d\n", read_encoder(LEFT_SHAFT_ENCODER), read_encoder(RIGHT_SHAFT_ENCODER));
  }
  power_left_motors(STOP);
  power_right_motors(STOP);
  left = read_encoder(LEFT_SHAFT_ENCODER);
  right = read_encoder(RIGHT_SHAFT_ENCODER);
  disable_encoder(LEFT_SHAFT_ENCODER);
  disable_encoder(RIGHT_SHAFT_ENCODER);
  
  return((left+right)/2);
}

/* drive until hit wall */
int drive_forward_until_wall(int speed, float duration){
  float begin_time;
  int left, right;
  int done = 0;
  if(third_wheel!= DRIVE){
    set_third_wheel(DRIVE);
  }
  printf("Driving at: %d\n", speed);
  enable_encoder(LEFT_SHAFT_ENCODER);
  enable_encoder(RIGHT_SHAFT_ENCODER);
  reset_encoder(LEFT_SHAFT_ENCODER);
  reset_encoder(RIGHT_SHAFT_ENCODER);
  begin_time = seconds();
  power_left_motors(speed);
  power_right_motors(speed);
  while((seconds()-begin_time < duration)&&(time<60.0)){
    left = read_encoder(LEFT_SHAFT_ENCODER);
    right = read_encoder(RIGHT_SHAFT_ENCODER);
    if((left+1 > right) && (time<60.0)){
      power_left_motors(STOP);
      left = read_encoder(LEFT_SHAFT_ENCODER);
      right = read_encoder(RIGHT_SHAFT_ENCODER);
      while((left>right) && (time<60.0)){
	  left = read_encoder(LEFT_SHAFT_ENCODER);
	  right = read_encoder(RIGHT_SHAFT_ENCODER);
      }
      power_left_motors(speed);
    }
    else if ((left < right+1) && (time<60.0)){
      power_right_motors(STOP);
      left = read_encoder(LEFT_SHAFT_ENCODER);
      right = read_encoder(RIGHT_SHAFT_ENCODER);
      while((left<right) && (time<60.0)){
	left = read_encoder(LEFT_SHAFT_ENCODER);
        right = read_encoder(RIGHT_SHAFT_ENCODER);
      }
      power_right_motors(speed);
    }
    printf("%d, %d\n", read_encoder(LEFT_SHAFT_ENCODER), read_encoder(RIGHT_SHAFT_ENCODER));
  }

  power_left_motors(STOP);
  power_right_motors(STOP);
  left = read_encoder(LEFT_SHAFT_ENCODER);
  right = read_encoder(RIGHT_SHAFT_ENCODER);
  disable_encoder(LEFT_SHAFT_ENCODER);
  disable_encoder(RIGHT_SHAFT_ENCODER);
  
  return((left+right)/2);
}

/* drive backwards until the wall */
int drive_backwards_until_wall(int speed, float duration){
  float begin_time;
  int left, right;
  int done = 0;
  speed = -speed;
  if(third_wheel!= DRIVE){
    set_third_wheel(DRIVE);
  }
  printf("Driving at: %d\n", speed);
  enable_encoder(LEFT_SHAFT_ENCODER);
  enable_encoder(RIGHT_SHAFT_ENCODER);
  reset_encoder(LEFT_SHAFT_ENCODER);
  reset_encoder(RIGHT_SHAFT_ENCODER);
  begin_time = seconds();
  power_left_motors(speed);
  power_right_motors(speed);
  while((seconds()-begin_time < duration)&&(time<60.0)&&(!digital(BACK_LEFT_BUTTON) && !digital(BACK_RIGHT_BUTTON))){
    left = read_encoder(LEFT_SHAFT_ENCODER);
    right = read_encoder(RIGHT_SHAFT_ENCODER);
    if((left+1 > right) && (time<60.0)){
      power_left_motors(STOP);
      left = read_encoder(LEFT_SHAFT_ENCODER);
      right = read_encoder(RIGHT_SHAFT_ENCODER);
      while((left>right) && (time<60.0)){
	  left = read_encoder(LEFT_SHAFT_ENCODER);
	  right = read_encoder(RIGHT_SHAFT_ENCODER);
      }
      power_left_motors(speed);
    }
    else if ((left < right+1) && (time<60.0)){
      power_right_motors(STOP);
      left = read_encoder(LEFT_SHAFT_ENCODER);
      right = read_encoder(RIGHT_SHAFT_ENCODER);
      while((left<right) && (time<60.0)){
	left = read_encoder(LEFT_SHAFT_ENCODER);
        right = read_encoder(RIGHT_SHAFT_ENCODER);
      }
      power_right_motors(speed);
    }
    /* printf("%d, %d\n", read_encoder(LEFT_SHAFT_ENCODER), read_encoder(RIGHT_SHAFT_ENCODER));  */
  }

  while((digital(BACK_LEFT_BUTTON) || digital(BACK_RIGHT_BUTTON))&&(seconds()-begin_time < duration)&&(time<60.0)){
    if(digital(BACK_LEFT_BUTTON)&&digital(BACK_RIGHT_BUTTON)){
      done = 1;
      break;
    }
    if(digital(BACK_LEFT_BUTTON)){
      printf("BACK LEFT PRESSED\n");
      power_left_motors(STOP);
    }
    else if(digital(BACK_RIGHT_BUTTON)){
      printf("BACK RIGHT PRESSED\n");
      power_right_motors(STOP);
    }
  }
  if (!done&&(seconds()-begin_time < duration)&&(time<60.0)){
    /* we hit the ball and not the wall */
    drive_backwards_until_wall(-speed, (duration-(time-begin_time)));
  }

  power_left_motors(STOP);
  power_right_motors(STOP);
  left = read_encoder(LEFT_SHAFT_ENCODER);
  right = read_encoder(RIGHT_SHAFT_ENCODER);
  disable_encoder(LEFT_SHAFT_ENCODER);
  disable_encoder(RIGHT_SHAFT_ENCODER);
  
  return((left+right)/2);
}


/* drive until center color is a certain value */
int drive_until_center_color(int speed, float duration, int color){
  float begin_time;
  int left, right;
  if(third_wheel!= DRIVE){
    set_third_wheel(DRIVE);
  }
  printf("Driving at: %d\n", speed);
  enable_encoder(LEFT_SHAFT_ENCODER);
  enable_encoder(RIGHT_SHAFT_ENCODER);
  reset_encoder(LEFT_SHAFT_ENCODER);
  reset_encoder(RIGHT_SHAFT_ENCODER);
  begin_time = seconds();
  power_left_motors(speed);
  power_right_motors(speed);
  while((seconds()-begin_time < duration)&&(time<60.0)&&(c_color!=color)){
    left = read_encoder(LEFT_SHAFT_ENCODER);
    right = read_encoder(RIGHT_SHAFT_ENCODER);
    if((left+1 > right) && (time<60.0)){
      power_left_motors(STOP);
      left = read_encoder(LEFT_SHAFT_ENCODER);
      right = read_encoder(RIGHT_SHAFT_ENCODER);
      while((left>right) && (time<60.0)){
	  left = read_encoder(LEFT_SHAFT_ENCODER);
	  right = read_encoder(RIGHT_SHAFT_ENCODER);
      }
      power_left_motors(speed);
    }
    else if ((left < right+1) && (time<60.0)){
      power_right_motors(STOP);
      left = read_encoder(LEFT_SHAFT_ENCODER);
      right = read_encoder(RIGHT_SHAFT_ENCODER);
      while((left<right) && (time<60.0)){
	left = read_encoder(LEFT_SHAFT_ENCODER);
        right = read_encoder(RIGHT_SHAFT_ENCODER);
      }
      power_right_motors(speed);
    }
    printf("%d, %d\n", read_encoder(LEFT_SHAFT_ENCODER), read_encoder(RIGHT_SHAFT_ENCODER));
  }
  power_left_motors(STOP);
  power_right_motors(STOP);
  left = read_encoder(LEFT_SHAFT_ENCODER);
  right = read_encoder(RIGHT_SHAFT_ENCODER);
  disable_encoder(LEFT_SHAFT_ENCODER);
  disable_encoder(RIGHT_SHAFT_ENCODER);
  
  return((left+right)/2);
}

/* drive until all pr's equal to indicated color */
int drive_until_colors(int speed, float duration, int color){
  float begin_time;
  int left, right;
  if(third_wheel!= DRIVE){
    set_third_wheel(DRIVE);
  }
  printf("Driving at: %d\n", speed);
  enable_encoder(LEFT_SHAFT_ENCODER);
  enable_encoder(RIGHT_SHAFT_ENCODER);
  reset_encoder(LEFT_SHAFT_ENCODER);
  reset_encoder(RIGHT_SHAFT_ENCODER);
  begin_time = seconds();
  power_left_motors(speed);
  power_right_motors(speed);
  while((seconds()-begin_time < duration)&&(time<60.0)
	&&((fr_color!=color)||(br_color!=color)||(bl_color!=color)||(c_color!=color))){
    left = read_encoder(LEFT_SHAFT_ENCODER);
    right = read_encoder(RIGHT_SHAFT_ENCODER);
    if((left+1 > right) && (time<60.0)){
      power_left_motors(STOP);
      left = read_encoder(LEFT_SHAFT_ENCODER);
      right = read_encoder(RIGHT_SHAFT_ENCODER);
      while((left>right) && (time<60.0)){
	  left = read_encoder(LEFT_SHAFT_ENCODER);
	  right = read_encoder(RIGHT_SHAFT_ENCODER);
      }
      power_left_motors(speed);
    }
    else if ((left < right+1) && (time<60.0)){
      power_right_motors(STOP);
      left = read_encoder(LEFT_SHAFT_ENCODER);
      right = read_encoder(RIGHT_SHAFT_ENCODER);
      while((left<right) && (time<60.0)){
	left = read_encoder(LEFT_SHAFT_ENCODER);
        right = read_encoder(RIGHT_SHAFT_ENCODER);
      }
      power_right_motors(speed);
    }
    printf("%d, %d\n", read_encoder(LEFT_SHAFT_ENCODER), read_encoder(RIGHT_SHAFT_ENCODER));
  }
  power_left_motors(STOP);
  power_right_motors(STOP);
  left = read_encoder(LEFT_SHAFT_ENCODER);
  right = read_encoder(RIGHT_SHAFT_ENCODER);
  disable_encoder(LEFT_SHAFT_ENCODER);
  disable_encoder(RIGHT_SHAFT_ENCODER);
  
  return((left+right)/2);
}

/* drive until corner color */
int drive_until_back_right_color(int speed, float duration, int color){
  float begin_time;
  int left, right;
  if(third_wheel!= DRIVE){
    set_third_wheel(DRIVE);
  }
  printf("Driving at: %d\n", speed);
  enable_encoder(LEFT_SHAFT_ENCODER);
  enable_encoder(RIGHT_SHAFT_ENCODER);
  reset_encoder(LEFT_SHAFT_ENCODER);
  reset_encoder(RIGHT_SHAFT_ENCODER);
  begin_time = seconds();
  power_left_motors(speed);
  power_right_motors(speed);
  while((seconds()-begin_time < duration)&&(time<60.0)&&(br_color!=color)){
    left = read_encoder(LEFT_SHAFT_ENCODER);
    right = read_encoder(RIGHT_SHAFT_ENCODER);
    if((left > right) && (time<60.0)){
      power_left_motors(STOP);
      left = read_encoder(LEFT_SHAFT_ENCODER);
      right = read_encoder(RIGHT_SHAFT_ENCODER);
      while((left+1>right) && (time<60.0)){
	  left = read_encoder(LEFT_SHAFT_ENCODER);
	  right = read_encoder(RIGHT_SHAFT_ENCODER);
      }
      power_left_motors(speed);
    }
    else if ((left < right+1) && (time<60.0)){
      power_right_motors(STOP);
      left = read_encoder(LEFT_SHAFT_ENCODER);
      right = read_encoder(RIGHT_SHAFT_ENCODER);
      while((left<right) && (time<60.0)){
	left = read_encoder(LEFT_SHAFT_ENCODER);
        right = read_encoder(RIGHT_SHAFT_ENCODER);
      }
      power_right_motors(speed);
    }
    printf("%d, %d\n", read_encoder(LEFT_SHAFT_ENCODER), read_encoder(RIGHT_SHAFT_ENCODER));
  }
  power_left_motors(STOP);
  power_right_motors(STOP);
  left = read_encoder(LEFT_SHAFT_ENCODER);
  right = read_encoder(RIGHT_SHAFT_ENCODER);
  disable_encoder(LEFT_SHAFT_ENCODER);
  disable_encoder(RIGHT_SHAFT_ENCODER);
  
  return((left+right)/2);
}

/* drive until gate closes */
int drive_until_gate(int speed, float duration, int *gate){
  float begin_time;
  int left, right;
  if(third_wheel!= DRIVE){
    set_third_wheel(DRIVE);
  }
  printf("Driving at: %d\n", speed);
  enable_encoder(LEFT_SHAFT_ENCODER);
  enable_encoder(RIGHT_SHAFT_ENCODER);
  reset_encoder(LEFT_SHAFT_ENCODER);
  reset_encoder(RIGHT_SHAFT_ENCODER);
  begin_time = seconds();
  power_left_motors(speed);
  power_right_motors(speed);
  while((seconds()-begin_time < duration) && (*gate == OPEN) && (time<60.0)){
    left = read_encoder(LEFT_SHAFT_ENCODER);
    right = read_encoder(RIGHT_SHAFT_ENCODER);
    if((left+1 > right) && (time<60.0)){
      power_left_motors(STOP);
      left = read_encoder(LEFT_SHAFT_ENCODER);
      right = read_encoder(RIGHT_SHAFT_ENCODER);
      while((left>right) && (time<60.0)){
	  left = read_encoder(LEFT_SHAFT_ENCODER);
	  right = read_encoder(RIGHT_SHAFT_ENCODER);
      }
      power_left_motors(speed);
    }
    else if ((left < right+1) && (time<60.0)){
      power_right_motors(STOP);
      left = read_encoder(LEFT_SHAFT_ENCODER);
      right = read_encoder(RIGHT_SHAFT_ENCODER);
      while((left<right) && (time<60.0)){
	left = read_encoder(LEFT_SHAFT_ENCODER);
        right = read_encoder(RIGHT_SHAFT_ENCODER);
      }
      power_right_motors(speed);
    }
    printf("%d, %d\n", read_encoder(LEFT_SHAFT_ENCODER), read_encoder(RIGHT_SHAFT_ENCODER));
  }
  power_left_motors(STOP);
  power_right_motors(STOP);
  left = read_encoder(LEFT_SHAFT_ENCODER);
  right = read_encoder(RIGHT_SHAFT_ENCODER);
  disable_encoder(LEFT_SHAFT_ENCODER);
  disable_encoder(RIGHT_SHAFT_ENCODER);
  
  return((left+right)/2);
}

/* turning right */
void hard_right(int ticks, float duration){
  int sum_value = 0;
  int difference = 0;
  int before_turn_left, before_turn_right;
  float begin_time;
  if(third_wheel!=TURN)
    set_third_wheel(TURN);
  enable_encoder(LEFT_SHAFT_ENCODER);
  enable_encoder(RIGHT_SHAFT_ENCODER);
  reset_encoder(LEFT_SHAFT_ENCODER);
  reset_encoder(RIGHT_SHAFT_ENCODER);
  begin_time = seconds();
  power_right_motors(-FAST);
  power_left_motors(FAST);
  while((sum_value < ticks) && (time<60.0) && (seconds()-begin_time < duration )){
    /* printf("Turning Right\n"); */
    if(digital(SIDE_RIGHT_BUTTON)||digital(BACK_LEFT_BUTTON)){
      power_left_motors(STOP);
      power_right_motors(STOP);
      before_turn_left = read_encoder(LEFT_SHAFT_ENCODER);
      before_turn_right = read_encoder(RIGHT_SHAFT_ENCODER);
      set_third_wheel(DRIVE);
      power_left_motors(FAST);
      power_right_motors(FAST);
      sleep(0.25);
      power_left_motors(STOP);
      power_right_motors(STOP);
      difference += ((read_encoder(LEFT_SHAFT_ENCODER)+read_encoder(RIGHT_SHAFT_ENCODER))-(before_turn_left+before_turn_right));
      set_third_wheel(TURN);
      power_left_motors(FAST);
      power_right_motors(-FAST);
    }
    sum_value = read_encoder(LEFT_SHAFT_ENCODER) + read_encoder(RIGHT_SHAFT_ENCODER);
  }
  power_right_motors(STOP);
  power_left_motors(STOP);
  disable_encoder(LEFT_SHAFT_ENCODER);
  disable_encoder(RIGHT_SHAFT_ENCODER);
}

/* turning left */
void hard_left(int ticks, float duration){
  int sum_value = 0;
  int difference = 0;
  int before_turn_left, before_turn_right;
  float begin_time;
  if(third_wheel!=TURN)
    set_third_wheel(TURN);
  enable_encoder(LEFT_SHAFT_ENCODER);
  enable_encoder(RIGHT_SHAFT_ENCODER);
  reset_encoder(LEFT_SHAFT_ENCODER);
  reset_encoder(RIGHT_SHAFT_ENCODER);
  begin_time = seconds();
  power_right_motors(FAST);
  power_left_motors(-FAST);
  while((sum_value < ticks) && (time<60.0)&&(seconds()-begin_time < duration)){
    /* printf("Turning Left\n"); */
    if(digital(SIDE_RIGHT_BUTTON)||digital(BACK_LEFT_BUTTON)){
      power_left_motors(STOP);
      power_right_motors(STOP);
      before_turn_left = read_encoder(LEFT_SHAFT_ENCODER);
      before_turn_right = read_encoder(RIGHT_SHAFT_ENCODER);
      set_third_wheel(DRIVE);
      power_left_motors(FAST);
      power_right_motors(FAST);
      sleep(0.25);
      power_left_motors(STOP);
      power_right_motors(STOP);
      difference += ((read_encoder(LEFT_SHAFT_ENCODER)+read_encoder(RIGHT_SHAFT_ENCODER))-(before_turn_left+before_turn_right));
      set_third_wheel(TURN);
      power_left_motors(-FAST);
      power_right_motors(FAST);
    }
    sum_value = read_encoder(LEFT_SHAFT_ENCODER) + read_encoder(RIGHT_SHAFT_ENCODER);
  }
  power_right_motors(STOP);
  power_left_motors(STOP);
  disable_encoder(LEFT_SHAFT_ENCODER);
  disable_encoder(RIGHT_SHAFT_ENCODER);
}


/*************************/
/**** Servo Functions ****/
/*************************/
/* mecanical arm extend and slap */
void extend_arm(){
  /* extend the arm out */
  servo(ARM_SERVO, 0);
  sleep(0.5);
}
void retract_arm(){
  servo(ARM_SERVO, 2700);
}

/* Third Wheel movements */
void set_third_wheel(int state){
  if(state == TURN){
    servo(THIRD_WHEEL_SERVO, 2025);
  }
  else if(state == DRIVE){
    servo(THIRD_WHEEL_SERVO, 260);
  }
  sleep(0.5);
  third_wheel = state;
}


/**********************************************/
/**** Sensor Commands and Error Detection *****/
/**********************************************/
/* ground sensor returns dark or light */
int ground_sensor(){
  /* returns dark or light */
}


/*************************************/
/***  Setup and Shutdown Functions ***/
/*************************************/
/* setting up right after light goes off beginning of 60 secs */
void setup(){
  start_process(timer());
  ir_transmit_on();
  enable_servos();
  retract_arm();
  set_third_wheel(TURN);
  start_process(led_thread());
}

/* orient ourselves right after the match starts */
void orient(){
  while(!led_read){}
  /* figure out what side we're on */
  if ((fr_color+br_color+bl_color) > 1)
    our_side = LIGHT;
  else
    our_side = DARK;
  
  if (our_side)
    printf("LIGHT");
  else
    printf("DARK");
  
  printf("fr:%d br:%d bl:%d\n", fr_color, br_color, bl_color);

  /* orient ourselves correctly */
  if(fr_color!=our_side){
    hard_right(QTR_ROTATION_R, 3.0);
  }
  else if(br_color!=our_side){
    hard_left(HALF_ROTATION, 4.0);
  }
  else if(bl_color!=our_side){
    hard_left(QTR_ROTATION, 3.0);
  }

  return;
}

/* shut down procedure after 60 seconds */
void shut_down(){
  int i;
  for(i=0;i<6;i++){
    motor(i, 0);
  }
  /* disable_servos(); */
  while(time < 60.0) {}
  ir_transmit_off();
  printf("**Robot Done**\n");
}