void configure() {
  balance_config();
  /*
  color_sensor_config();
  */
  cs_nw_w=0; cs_nw_b=30;
  cs_ne_w=0; cs_ne_b=30;
  cs_sw_w=0; cs_sw_b=30;
  cs_se_w=0; cs_se_b=30;
  gyro_config();
}

float desired_angle;

void main() {
  int pid, turnleft, ourballs, final_dir;
  long start_time;
  float dist, turn_angle;

  printf("\nWelcome, Team 4!\n");
  rf_team = 4;
  rf_enable();

  configure();

  printf("\nHit START to arm\n");
  button(START);

  start_machine();

  start_time = mseconds();

  start_process(update_angle());
  start_process(detect_motion());

  orient();

  desired_angle = theta_deg;

  /* This is for qualifying only.
     turn_around();
  */

  start_time = mseconds();

  /* This is for qualifying only.
    go_forward_straight(desired_angle, 80, 5000L, 1);
    return;
  */
    
  if ((MYCOLOR == BLACK && rf_vote_winner == RED) ||
      (MYCOLOR == WHITE && rf_vote_winner == GREEN)) {
    turnleft = 1;
    final_dir = 1;
    turn_angle = LEFT_TURN;
  } else {
    turnleft = 0;
    final_dir = -1;
    turn_angle = RIGHT_TURN;
  }
  ourballs = rf_vote_winner;

  /* back into balls */
  go_forward_straight(desired_angle, 80, 4000L, 0);

  /* move forward */
  dist = (float)lsb_ms_per_deg;
  go_forward_straight(desired_angle, 80, 1000L, 1);

  desired_angle += turn_angle;
  if (turnleft) {
    turn_left();
  } else {
    turn_right();
  }

  /* Go to the side wall */
  go_forward_straight(desired_angle, 80, 2000L, 1);

  desired_angle -= turn_angle;
  if (!turnleft)
    turn_left();
  else
    turn_right();

  /* Go to the far corner */
  go_forward_straight(desired_angle, 80, 8000L, 1);

  desired_angle -= turn_angle;
  if (!turnleft)
    turn_left();
  else
    turn_right();

  /* Go to the enemy's center scoring area */
  /* final_dir = scoring[1-MYCOLOR][1][0] - rf_x_us(); */
  go_forward_straight(desired_angle, 80, 2000L, 1);

  /* wait for time to run out */
  while (mseconds() - start_time < 57000L)
    defer();

  if (ourballs == rf_vote_winner) {
    /* We have the winning balls. Move them to our areas. */
    if (isign(final_dir) == isign(rf_x_them() - rf_x_us())) {
      go_forward = 0;
    } else {
      go_forward = 1;
    }
    go_forward_straight(desired_angle, 80, 3000L, go_forward);
  }

}