void turn(int direction, int circum, int speed){
  int leftd=0,rightd=0;
  float time;


  servo(SERVO_WHEEL,FRONT_WHEEL_STRAIGHT-direction*2000);
  sleep(FRONT_WHEEL_WAIT);
  reset_encoder(SHAFT_LEFT);
  reset_encoder(SHAFT_RIGHT);
  time=seconds();
  wheelRight(speed*direction);
  wheelLeft(-speed*direction);
  while((leftd+rightd < circum) && (seconds()-time < TURN_TIMEOUT)){
    leftd=read_encoder(SHAFT_LEFT);
    rightd=read_encoder(SHAFT_RIGHT);
  }
  ao();
}


void turnCareful(int direction, int circum, int speed, int outerFrontBumper, float timeOut){
  int leftd=0, rightd=0;
  float t,time;
  int lastDist;


  servo(SERVO_WHEEL,FRONT_WHEEL_STRAIGHT-direction*2000);
  sleep(FRONT_WHEEL_WAIT);
  reset_encoder(SHAFT_LEFT);
  reset_encoder(SHAFT_RIGHT);
  wheelRight(speed*direction);
  wheelLeft(-speed*direction);

  time=t=seconds();
  lastDist = 0;

  while((read_encoder(SHAFT_LEFT)+read_encoder(SHAFT_RIGHT) < circum) && (seconds()-time < timeOut)){
    leftd=read_encoder(SHAFT_LEFT);
    rightd=read_encoder(SHAFT_RIGHT);
    /* Stuck */
    if(digital(outerFrontBumper)){
      straightFront();
      backOff(TURN_CAREFUL_BACK_OFF,100);
      servo(SERVO_WHEEL,FRONT_WHEEL_STRAIGHT-direction*2000);
      sleep(FRONT_WHEEL_WAIT);
      circum -= (leftd+rightd);
      reset_encoder(SHAFT_LEFT);
      reset_encoder(SHAFT_RIGHT);
      leftd=0;
      rightd=0;      
      wheelRight(speed*direction);
      wheelLeft(-speed*direction);
    }

    /* Stall Detection 
    if(seconds()-t > STALL_TIMEOUT){
      if(read_encoder(SHAFT_LEFT)+read_encoder(SHAFT_RIGHT)-STALL_TOL <= lastDist){
	backOff(STALL_BACK_OFF,100);
	turn(direction,STALL_BEND,100);
	circum -= (leftd+rightd);
	straightFront();
	reset_encoder(SHAFT_LEFT);
	reset_encoder(SHAFT_RIGHT);
	leftd=0;
	rightd=0;
	wheelRight(speed*direction);
	wheelLeft(-speed*direction);
	lastDist=0;
      }else{
	lastDist = read_encoder(SHAFT_LEFT)+read_encoder(SHAFT_RIGHT);
      }
      t=seconds();
    } end if stall */

  }
  ao();
}




void turnSpecial(){
  int count;
  printf("Turn Special\n");

  servo(SERVO_WHEEL,0);
  sleep(0.9);

  turnSpecialHelper(CIRCUM_FIRST_2/8);
  sleep(0.9);
  turnSpecialHelper(CIRCUM_FIRST_2/8);
  sleep(0.9);
  turnSpecialHelper(CIRCUM_FIRST_2/8);
  
  reset_encoder(SHAFT_LEFT);
  reset_encoder(SHAFT_RIGHT);
  servo(SERVO_WHEEL,1000);
  sleep(0.8);
  wheelRight(100);
  while(read_encoder(SHAFT_RIGHT)+read_encoder(SHAFT_LEFT) < 175){}
  ao();
  
  
}


void turnSpecialHelper(int circum){
  float time;
  reset_encoder(SHAFT_LEFT);
  reset_encoder(SHAFT_RIGHT);
  time=seconds();
  wheelRight(100*CCW);
  wheelLeft(-100*CCW);
  while((read_encoder(SHAFT_LEFT)+read_encoder(SHAFT_RIGHT) < circum) && (seconds()-time < TURN_TIMEOUT)){
    sleep(0.02);
  }
  ao();
}