void followWall(int bumpSensorID, int outerBump, int turnAwayRotation,int turnDist, int walkDistance, int speed, int frontWheelBias, float timeOut){

  int leftd,rightd,turnAwayDist,lastDist;
  float t,time;
  int straight = FRONT_WHEEL_STRAIGHT + turnAwayRotation*frontWheelBias;
  int turnAway = FRONT_WHEEL_STRAIGHT - turnAwayRotation*2000;

  folWallDone = FALSE;
  printf("Following wall.\n");
  reset_encoder(SHAFT_LEFT);
  reset_encoder(SHAFT_RIGHT);
  leftd=0;
  rightd=0;

  /* Go straight */
  servo(SERVO_WHEEL, straight);
  sleep(FRONT_WHEEL_WAIT);
  wheelLeft(speed);
  wheelRight(speed);
  
  time=t=seconds();
  lastDist=0;

  while((read_encoder(SHAFT_LEFT)+read_encoder(SHAFT_RIGHT) < walkDistance) && (seconds()-time < timeOut)){
    leftd=read_encoder(SHAFT_LEFT);
    rightd=read_encoder(SHAFT_RIGHT);

    /* If bump into the wall */
    if(digital(ELBOW_LEFT)){
      printf("Elbow is touched\n");
      walkDistance -= (leftd+rightd); 
      ao(); sleep(0.2);
      backOff(FOL_WALL_BACK_OFF*2,100);
      turn(turnAwayRotation,FOL_WALL_TURN*3,100);
      servo(SERVO_WHEEL, straight);
      sleep(FRONT_WHEEL_WAIT);
      reset_encoder(SHAFT_LEFT);
      reset_encoder(SHAFT_RIGHT);

      
      /* Walk straight again */
      wheelLeft(speed);
      wheelRight(speed); 
    }else if(digital(bumpSensorID)){
      printf("Bump to wall\n");
      walkDistance -= (leftd+rightd); 
      ao(); sleep(0.2);
      backOff(FOL_WALL_BACK_OFF,100);
      turn(turnAwayRotation,FOL_WALL_TURN,100);
      servo(SERVO_WHEEL, straight);
      sleep(FRONT_WHEEL_WAIT);
      reset_encoder(SHAFT_LEFT);
      reset_encoder(SHAFT_RIGHT);

      
      /* Walk straight again */
      wheelLeft(speed);
      wheelRight(speed); 
    } /* end: if bump into the wall */  

    /* Stall detection 
    if(seconds()-t > STALL_TIMEOUT){
      if(read_encoder(SHAFT_LEFT)+read_encoder(SHAFT_RIGHT)-STALL_TOL <= lastDist){
	printf("STALL\n");
	backOff(STALL_BACK_OFF,100);
	turn(-turnAwayRotation,STALL_BEND,100);
	walkDistance -= (leftd+rightd);
	servo(SERVO_WHEEL, straight);
	sleep(FRONT_WHEEL_WAIT);
	reset_encoder(SHAFT_LEFT);
	reset_encoder(SHAFT_RIGHT);
	leftd=0;
	rightd=0;
	wheelLeft(speed);
	wheelRight(speed);
	lastDist=0;
      }else{
	lastDist = read_encoder(SHAFT_LEFT)+read_encoder(SHAFT_RIGHT);
      }
      t=seconds();
    }  end if stall */

    /* outer arm detection */
    if(digital(outerBump)){
      printf("Outer arm is touched\n");
      walkDistance -= (leftd+rightd);
      ao(); sleep(0.2);
      backOff(FOL_WALL_BACK_OFF*2,100);
      turn(-turnAwayRotation,FOL_WALL_TURN,100);
      servo(SERVO_WHEEL, straight);
      sleep(FRONT_WHEEL_WAIT);
      reset_encoder(SHAFT_LEFT);
      reset_encoder(SHAFT_RIGHT);
      wheelLeft(speed);
      wheelRight(speed); 
    }
  } /* end: while distance is still short */
  folWallDone = TRUE;
}









void followWallAggressive(int bumpSensorID, int outerBump, int turnAwayRotation,int turnDist, int walkDistance, int speed, int frontWheelBias, float timeOut){

  int leftd,rightd,turnAwayDist,lastDist;
  float t,time;
  int straight = FRONT_WHEEL_STRAIGHT + turnAwayRotation*frontWheelBias;
  int turnAway = FRONT_WHEEL_STRAIGHT - turnAwayRotation*2000;

  int nBump =0;

  folWallDone = FALSE;
  printf("Following wall.\n");
  reset_encoder(SHAFT_LEFT);
  reset_encoder(SHAFT_RIGHT);
  leftd=0;
  rightd=0;

  /* Go straight */
  servo(SERVO_WHEEL, straight);
  sleep(FRONT_WHEEL_WAIT);
  wheelLeft(speed);
  wheelRight(speed);
  
  time=t=seconds();
  lastDist=0;

  while((read_encoder(SHAFT_LEFT)+read_encoder(SHAFT_RIGHT) < walkDistance) && (seconds()-time < timeOut)){
    leftd=read_encoder(SHAFT_LEFT);
    rightd=read_encoder(SHAFT_RIGHT);

    if( (nBump>10) || (digital(ARM_BUMP_LEFT) && digital(ARM_BUMP_RIGHT))){
      ao();
      ram();
      return;
    }


    if(digital(ELBOW_LEFT)){
      printf("Elbow is touched\n");
      walkDistance -= (leftd+rightd); 
      ao(); sleep(0.2);
      backOff(FOL_WALL_BACK_OFF*2,100);
      turn(turnAwayRotation,FOL_WALL_TURN*3,100);
      servo(SERVO_WHEEL, straight);
      sleep(FRONT_WHEEL_WAIT);
      reset_encoder(SHAFT_LEFT);
      reset_encoder(SHAFT_RIGHT);

      
      /* Walk straight again */
      wheelLeft(speed);
      wheelRight(speed); 
    }else if(digital(bumpSensorID)){
      printf("Bump to wall\n");
      walkDistance -= (leftd+rightd); 
      ao(); sleep(0.2);
      backOff(FOL_WALL_BACK_OFF,100);
      turn(turnAwayRotation,FOL_WALL_TURN,100);
      servo(SERVO_WHEEL, straight);
      sleep(FRONT_WHEEL_WAIT);
      reset_encoder(SHAFT_LEFT);
      reset_encoder(SHAFT_RIGHT);

      
      /* Walk straight again */
      wheelLeft(speed);
      wheelRight(speed); 
    } /* end: if bump into the wall */  

    /* outer arm detection */
    else if(digital(outerBump)){
      printf("Outer arm is touched\n");
      walkDistance -= (leftd+rightd);
      ao(); sleep(0.2);
      backOff(FOL_WALL_BACK_OFF*2,100);
      turn(-turnAwayRotation,FOL_WALL_TURN,100);
      servo(SERVO_WHEEL, straight);
      sleep(FRONT_WHEEL_WAIT);
      reset_encoder(SHAFT_LEFT);
      reset_encoder(SHAFT_RIGHT);
      wheelLeft(speed);
      wheelRight(speed); 
    }
  } /* end: while distance is still short */
  folWallDone = TRUE;
}




void ram(){
  int i;
  straightFront();
  for(i=0;i<3;i++){
    backOff(200,100);
    ao(); sleep(0.3);
    backOff(250,-100);
    ao(); sleep(0.3);
  }
}







/*
void aggressive(int pid){
  int i;
  int nBump = 0;
  while(!folWallDone){
    if(digital(ARM_BUMP_LEFT)) nBump++;
    if(digital(ARM_BUMP_RIGHT)) nBump++;
    if((nBump>10) || (digital(ARM_BUMP_LEFT) && digital(ARM_BUMP_RIGHT))){
      printf("Ramming\n");
      kill_process(pid);
      ao();
      straightFront();
      for(i=0;i<3;i++){
	backOff(200,100);
	ao(); sleep(0.3);
	backOff(250,-100);
	ao(); sleep(0.3);
      }
      return;
    }
  }
}
*/