/***********************************************************************/

/** @name drive.c 

    Contains functions to init and release motors, control overall

    robot movement and set and read robot position.    

    

    @author Birgit Graf, UWA, 1998

*/

/***********************************************************************/



/*@{*/





#include "global.h"

#include "general.h"

#include "image.h"

#include "servos.h"



/** values for RoBIOS PI controller (used in vw interface) */

#define v_lin 7.0

#define t_lin 0.3

#define v_rot 10.0

#define t_rot 0.1



/** accuracy parameter for robot movement (in m), angle < epsilon ->

    don't move */

#define epsilon 0.01



/** flag to indicate location of ball (1 -> turn left, -1 -> turn right) */

int direction = 1;



/** radius of circle for goalie (> GOALWIDTH/2) */

#define radius  0.55



/** distance goal middle to circle middle */

#define goaldist 0.5



/** goalie: maximum angle to ball where robot is still allowed to

    drive ( atan (goaldist / GOALWIDTH/2 */

#define anglemax 1.12



/* distance beneath/behind ball where robot should drive */

#define arounddist 0.1







/***********************************************************************/

/** Init VW interface.



    Called by main().

    @see main()

*/

/***********************************************************************/



int InitDrive()

{

  if (!(vw = VWInit(VW_DRIVE, 1)))

    error("VWInit!\n");



  if (VWStartControl(vw, v_lin, t_lin , v_rot, t_rot) == -1)

    error("VWStart!\n");



  switch(player_pos)

    {

    case 1: /* set default position */

      VWSetPosition(vw, 0.1, 0.0, -PI/2); /* goalie */

      break;

    case 2:

      VWSetPosition(vw, 0.225, 0.5, 0.0); /* lb */

      break;

    case 3:

      VWSetPosition(vw, 0.225, -0.5, 0.0); /* rb */

      break;

    case 4:

      VWSetPosition(vw, LENGTH-0.225, 0.5, -PI); /* lf */

      break;

   case 5:

      VWSetPosition(vw, LENGTH-0.225, -0.5, PI); /* rf */

      break;

    }

return 0;

}







/***********************************************************************/

/** Change parameters.

    Changes thresholds which are used to select ball/goal coloured

    pixels from others and size of ball/goal.



    Called by main().

    @see  main()

*/

/***********************************************************************/



void set_drv_parameters()

{

  int ind = 0;

  int end_proc = FALSE;



  LCDClear();

  LCDMenu("CHG", "NXT", " ", "END");



  while (!end_proc)

  {

    LCDSetPos(0, 0);

    LCDPrintf("Driving param.\n");



    LCDSetPos(2, 0);

    LCDPrintf("Lin speed\n");

    LCDSetPos(3, 0);

    LCDPrintf("Rot speed\n");

    if (!I_am_goalie())

    {

      LCDSetPos(4, 0);

      LCDPrintf("Player pos.\n");

    }

    LCDSetChar(2 + ind, 15, '*');



    switch (KEYGet())

    {

    case KEY1:

      switch(ind)

      {

      case 0:

	LIN_SPEED = set_fparam("Lin speed", 0.0, LIN_SPEED, 1.0, 0.05);

	break;

      case 1:

	ROT_SPEED = set_fparam("Rot. speed", 0.0, ROT_SPEED, 1.0, 0.05);

	break;

      case 2:

	player_pos = set_iparam("Player pos.\n(2=lb, 3=rb,\n4=lf, 5=rf)",

				2, player_pos, 5, 1);	  

	break;

      default: break;

      }

      

      LCDMenu("CHG", "NXT", " ", "END");

      break;



    case KEY2:

      LCDSetChar(2 + ind, 15, ' ');

      ind ++;

      if (I_am_goalie())

	if (ind > 1) ind = 0;

      if (ind > 2) ind = 0;

      break;



    case KEY4:

      LCDClear(); end_proc = TRUE; break;



    default: break;

    }

  }

}





/***********************************************************************/

/** Print robot position.

    Print x- and y-coordiantes in cm and orientation in degrees on LCD.



    Called by set_coordinates().

    @see set_coordinates()

*/

/***********************************************************************/



void print_pos()

{

  int boc[3];

  PositionType pos;



  VWGetPosition(vw,&pos);

  boc[0] = (int)(100.0 * pos.x);

  boc[1] = (int)(100.0 * pos.y);

  boc[2] = (int)(180.0 * pos.phi / PI);



  LCDSetPos(1, 0);

  LCDPrintf("Robot Coord.:\n");



  LCDSetPos(2, 0);

  LCDPrintf("x  : % 4d cm\n", boc[0]);

  LCDSetPos(3, 0);

  LCDPrintf("y  : % 4d cm \n", boc[1]);

  LCDSetPos(4, 0);

  LCDPrintf("phi: % 4d deg\n", boc[2]);

}





/***********************************************************************/

/** Set robot coordinates.

    Set robot x and y coordinaes. (0,0) = middle of own goal,

    looking towards opponents goal, y goes positive to left,

    x positive forwards, angle is positive to right, negative to left.



    Called by main().

    @see main()

*/

/***********************************************************************/



void set_coordinates()

{

  PositionType pos;

  int end_proc = FALSE, end_proc2 = FALSE;



  LCDClear();

  if (I_am_goalie())

    LCDMenu("GOAL", " ", "INFO", "END");

  else

    LCDMenu("LBC", "RBC", "...", "END");

  while (!end_proc)

  {

    VWGetPosition(vw, &pos);

    print_pos();



    switch (KEYRead())

    {

    case KEY1:

      if (I_am_goalie())

	VWSetPosition(vw, 0.1, 0.0, -PI/2);

      else

	VWSetPosition(vw, 0.225, 0.5, 0.0);

      break;

    case KEY2:

      if (!I_am_goalie())

	VWSetPosition(vw, 0.225, -0.5, 0.0);

      break;

    case KEY3:

      if (I_am_goalie())

      {

	LCDClear();

	LCDMenu(" "," "," ","OK");

	LCDPutString("Put robot 10cm  in front of     middle of our   ");

	LCDPutString("goal (GOAL) ");

	KEYWait(KEY4);

	LCDClear();

	LCDMenu("GOAL", " ", "INFO", "END");

      }

      else

      {

	LCDClear();

	LCDMenu("LFC", "RFC", "MID", "END");

	while (!end_proc2)

	  {

	    VWGetPosition(vw, &pos);

	    print_pos();

	    

	    switch (KEYRead())

	      {

	      case KEY1:

		VWSetPosition(vw, LENGTH-0.225, 0.5, -PI);

		break;

	      case KEY2:

		VWSetPosition(vw, LENGTH-0.225, -0.5, PI);

		break;

	      case KEY3:

		VWSetPosition(vw, LENGTH/2, 0.0, 0.0);

		break;

	      case KEY4:

		LCDClear(); end_proc2 = TRUE; break;

	      default: break;

	      }

	  }

      }	

    case KEY4:

      LCDClear(); end_proc = TRUE; break;

    default: break;

    }

  }

}







/***********************************************************************/

/** Avoid obstacles.

    Function makes robot drive back in case there is an obstacle in front

    of it (wheels stalled or front-PSD-distance below obstacle_thresh).



    Called by PPdrive_field().

    @see PPdrive_field()

*/

/***********************************************************************/



void avoid_obstacle()

{

  PositionType pos;



  VWGetPosition(vw, &pos);



  /* ----- drive away from obstacle ----- */

  if (got_ball_flag) 

  {

    direction = fsign(pos.y);

    

    /* got ball -> turn to kick it towards opponent's goal */

      VWDriveTurn(vw, (float) -direction * PI/4, 5.0 * ROT_SPEED); 



    while(!VWDriveDone(vw) && !(KEYRead() == KEY3)){}

  }

  

  /* drive back from wall */

  VWDriveStraight(vw, -0.05, LIN_SPEED);	

  while(!VWDriveDone(vw) && !(KEYRead() == KEY3) && !VWStalled(vw)){}

  

  got_ball_flag = FALSE;

  see_ball_flag = FALSE;		/* lost ball while driving

					   back -> search again */

}







/***********************************************************************/

/** Search goal and drive to it.

    If ball is caught in front of robot, drive curve with fixed angle

    (don't want to loose ball) until facing goal, then kick it

    there. Kick ball as well as soon as goal is seen.



    Called by PPdrive_field().

    @see PPdrive_field()

*/

/***********************************************************************/



void drive_to_goal()

{

  PositionType pos;

  int x_middle = 0, y_middle = 0, goal_size = 0; /* parameters of goal */



  /* angle between robot orientation and goal middle */

  float angle;			

  

  if (use_motors)

    VWDriveStraight(vw, 0.2, LIN_SPEED / 4.0);

  

  /* look for goal */

  VWGetPosition(vw, &pos);

  angle = DiscAtan((int)(1000 * pos.y), (int)(1000.0 * (LENGTH - pos.x))) + pos.phi;

  if(((fabs(angle) < PI2) && (pos.x < WIDTH2) )|| /* facing opponent's goal */

     ((fabs(angle) < PI / 4) && (pos.x > WIDTH2)))

  {

    if (use_motors)

      VWDriveStraight(vw, 0.2, LIN_SPEED);

    kick_ball();

    got_ball_flag = FALSE;

    see_ball_flag = FALSE;

  }

  else			/* not facing opponent's goal */

  {

    move_camera(UP);

    if (find_goal(&x_middle, &y_middle, &goal_size)) /* seeing opponent's goal */

    {

      LCDSetPos(1, 10);

      LCDPrintf("GOAL!\n");

      LCDSetPos(2, 10);

      LCDPrintf("     \n");

      LCDSetPos(3, 10);

      LCDPrintf("     \n");

      

      kick_ball();

      got_ball_flag = FALSE;

      see_ball_flag = FALSE;

    }

    else

      if (use_motors)

	VWSetSpeed(vw, LIN_SPEED / 4.0, -fsign(angle) * LIN_SPEED); 

    move_camera(DOWN);	/* make sure ball is still there */

  }

}









/***********************************************************************/

/** Get angle.

    Get angle between robot orientation phi and line to point (diffx, diffy).



    Called by start_curve_to_ball(), drive_home() and PPdrive_field().

    

    @see start_curve_to_ball()

    @see drive_home()

    @see PPdrive_field()

    @param diffx, diffxy difference between coordinates of point to

    current robot position

    @param phi robot heading

    @return diffphi angle between robot heading and point (x, y)

*/

/***********************************************************************/



float get_angle(float diffx, float diffy, float phi)

{

  float diffphi;



  /* angle to desired position */

  if (fabs(diffx) < epsilon && fabs(diffy) < epsilon)

    diffphi = phi;              /* point reached -> no angle difference */

  else

    diffphi = DiscAtan((int)(1000.0 * diffy), (int) (1000.0 * diffx));

  

  /* difference to current heading */

  diffphi -= phi;

  

  /* angle always between 0 and 2*PI */

  if (diffphi >= 2.0 * PI)

    diffphi -= 2.0 * PI;

  if (diffphi <= 0)

    diffphi += 2.0 * PI;

  

  /* angle always between -PI and PI */

  if (diffphi >=  PI)

    diffphi -= 2.0 * PI;

  

/*   if (fabs(diffphi) > PI/2)  */

/*     AUTone(1200,200); */

  return diffphi;

}





/***********************************************************************/

/** Drive curve directly to ball.

    Drive directly to ball if it is close enough.

    

    Called by PPdrive_field().



    @see PPdrive_field()

    @param pos robot position

    @param ball structure containing coordinates of ball and

    angle from ball position to opponent's goal

    @param turn flag to indicate whether robot is supposed to turn at

    small distances

*/

/***********************************************************************/



void start_curve_to_ball(PositionType pos, PositionType ball, int turn)

{

  /* difference in coordinates from robot to ball */

  float diff_x, diff_y;

  /* distance and angle from robot to ball */

  float ball_angle, ball_dist;



  /* speed depending on distance of ball */

  float drive_speed, drive_dist;



  diff_x = ball.x - pos.x;	/* difference to ball position */

  diff_y = ball.y - pos.y;



  /* angle between robot orientation and line from robot middle

     through ball */

  ball_angle = get_angle(diff_x, diff_y, pos.phi);



  /* distance to ball in a straight line */

  ball_dist = hypot(diff_x, diff_y);



  if (fabs(ball_angle) == 0)

    drive_dist = ball_dist;

  else if (fabs(ball_angle) == PI)

    drive_dist = 0.5 * ball_dist * PI;

  else

    drive_dist = ball_dist * ball_angle / sin(ball_angle);



  drive_speed = 2.0 * drive_dist * LIN_SPEED;

  if (drive_speed < LIN_SPEED / 4.0)

    drive_speed = LIN_SPEED / 4.0;

  if (drive_speed > LIN_SPEED)

    drive_speed = LIN_SPEED;



/*    if ((ball_dist < 0.07) || !turn) */

/*      VWDriveStraight(vw, 0.1, drive_speed); */

/*    else */

    /* drive curve to ball, angle = ball_angle, distance on curve =

       straight line distance * angle * sin(angle) */

    VWDriveCurve(vw, drive_dist, 2.0 * ball_angle, drive_speed);

  

  while (!VWDriveDone(vw) && !see_ball_flag && !end_flag && !obstacle_flag)

    OSReschedule();

}





/*************************************************************************/

/** Drive behind ball.

    Drive straight line, then turn to get behind ball.



    @param side side of ball where robot is driving to

    @param diff_x, diff_y way to middle of circle

    @return TRUE if robot drove all the way behind the ball

*/

/*************************************************************************/



int drive_around(float diff_x, float diff_y, int side, float myphi)

{

  float diff_angle, diff_dist;

  float turn_angle;

  

  int done = FALSE;			/* finished first driving operation */

  

  /* angle between line through middle of circle and robot and 0 */

  diff_angle = atan2(diff_y, diff_x);



  /* distance between robot and middle of circle */

  diff_dist = hypot(diff_x, diff_y);

  

  /* turn back phi, forth angle to get to middle of circle and angle

     to circular path */

  turn_angle = -myphi + diff_angle + side * asin(arounddist / 2 / diff_dist);

  if (fabs(turn_angle) > PI)

    turn_angle -= 2 * fsign(turn_angle) * PI;



  VWDriveTurn(vw, turn_angle, ROT_SPEED);

  while (!VWDriveDone(vw) && !end_flag && !obstacle_flag)

    OSReschedule();

  

  if (VWDriveDone(vw) && !see_ball_flag && !end_flag && !obstacle_flag)

  {

    VWDriveStraight(vw, hypot(diff_dist, arounddist), LIN_SPEED);

    while (!(done = VWDriveDone(vw)) && !see_ball_flag && !end_flag && !obstacle_flag)

      OSReschedule();

  }

  return done;

}







/***********************************************************************/

/** Turn and drive back to home position.

    

    Called by PPdrive_field().

    @see PPdrive_field()

*/

/***********************************************************************/



void drive_home()

{

  /* difference in coordinates from robot to ball */

  float diff_x, diff_y;     

  /* distance and angle from robot to ball */

  float home_angle, home_dist;

  

  /* speed depending on distance of home position */

  float drive_speed;



  /*int done;*/			/* drove turn */



  PositionType pos;

  /*PositionType ball;*/

  VWGetPosition(vw, &pos);



  diff_x = home[player_pos-2][next_home].x - pos.x;	/* difference to home position */

  diff_y = home[player_pos-2][next_home].y - pos.y;

  

  /* angle between robot orientation and line from robot middle

     through home position */

  home_angle = get_angle(diff_x, diff_y, pos.phi);

  

  /* distance to home position in a straight line */

  home_dist = hypot(diff_x, diff_y);



  if (start_flag)		/* just starting to go -> take

				   shortest way */

    direction = home_angle;

  

  if (fsign(home_angle) == direction)

    VWDriveTurn(vw, home_angle, ROT_SPEED);

  else

    VWDriveTurn(vw, direction * 2.0 * PI + home_angle, ROT_SPEED);

  

  while (!VWDriveDone(vw) && !see_ball_flag && !end_flag && !obstacle_flag)

    OSReschedule();



  start_flag = FALSE;

  

  if (VWDriveDone(vw) && !see_ball_flag && !end_flag && !obstacle_flag)

  {

    drive_speed = 2.0 * home_dist * LIN_SPEED;

    if (drive_speed < LIN_SPEED / 4.0)

      drive_speed = LIN_SPEED / 4.0;

    if (drive_speed > LIN_SPEED)

      drive_speed = LIN_SPEED;

    

    VWDriveStraight(vw, home_dist, drive_speed); /* drive home */

    while (!VWDriveDone(vw) && !see_ball_flag && !end_flag && !obstacle_flag)    

      OSReschedule();

    if (see_ball_flag)

      VWSetSpeed(vw, 0.0, 0.0);

    if (VWDriveDone(vw))

      {

	next_home++;

	if (next_home > 2)

	  next_home = 0;

      }

  }

}





/***********************************************************************/

/** Set field player movement.

    Move robot and output text (drive status) on LCD.

    Activate robot movement according to flags end_flag,

    obstacle_flag, got_ball_flag and see_ball_flag, concerning

    different priorities.



    Process started by main().

    @see main()

*/

/***********************************************************************/



void PPdrive_field()

{

  PositionType pos, ball;

  /*float ball_dist;*/

  int ball_count = 0;

  int new_ball;

  int done = FALSE;			/* flag to show whether robot

				   succesfully drove around ball */

  

  float angle_after_turn;	/* heading if robot would drive curve

				   to the ball */

  float ball_angle;		/* angle to the ball */

  

  float middle_x, middle_y, diff_x, diff_y, mid_dist, side;

  while(!end_flag)

  {

/*     LCDPrintf("control\n"); */

    

    if (start_flag1 && attack_flag)

      {

	if (attack_flag == 1)

	  OSWait(100);

	if((player_pos == 4) || (player_pos ==5))

	  {

	    VWDriveStraight(vw, 0.2, 1.5 * LIN_SPEED);

	    while (!VWDriveDone(vw)) 

	      {

		kick_ball();

		OSWait(50);

	      }

	  }

	

	if((player_pos == 2) || (player_pos == 3))

	  {

	    kick_ball();

	    VWDriveStraight(vw, -0.1, 1.5 * LIN_SPEED);

	    while (!VWDriveDone(vw)) {}

	  }

	start_flag1 = FALSE;

      }

    

    /* double check if ball is in front of robot */

    if (see_ball_flag)

    {

      VWGetPosition(vw, &pos);

      if (hypot(ball.x - pos.x, ball.y - pos.y) < 0.05)

      {

	move_camera(DOWN);

	got_ball_flag = TRUE;

      }

    }

    

    LCDSetPos(0, 10);

    LCDPrintf("o%ds%dg%d\n", obstacle_flag, see_ball_flag, got_ball_flag);



    if (obstacle_flag)		/* ran into obtstacle -> avoid it */

    {

      LCDSetPos(1, 10);

      LCDPrintf("Wall!\n");

      LCDSetPos(2, 10);

      LCDPrintf("     \n");

      LCDSetPos(3, 10);

      LCDPrintf("     \n");

      

      obstacle_flag = FALSE;

      if (use_motors)

	avoid_obstacle();

    }

    else

      if (got_ball_flag) /* if ball is caught

			    -> drive it to goal */

      {

	LCDSetPos(1, 10);

	LCDPrintf("GOT  \n");

	LCDSetPos(2, 10);

	LCDPrintf("BALL \n");

        LCDSetPos(3, 10);

        LCDPrintf("     \n");

	

	new_ball = FALSE;

	drive_to_goal();

	AUBeep(); AUBeep(); AUBeep(); AUBeep(); AUBeep();

      }

      else

        if (see_ball_flag && (ball_count < 3))

        {

	  AUBeep();		  

	  VWGetPosition(vw, &pos); /* robot position */

          get_ball_coord(&ball); /* ball position */

	  

          LCDSetPos(1, 10);

          LCDPrintf("Ball:\n");

          LCDSetPos(2, 10);

          LCDPrintf("(%d, \n", (int)(100 * ball.x));

          LCDSetPos(3, 10);

          LCDPrintf("% 2d) \n", (int)(100 * ball.y));



	  new_ball = FALSE;

	  see_ball_flag = FALSE; /* driving to current ball position

				    -> don't consider it again */

	  

	  if (use_motors)

	  {

	    if ((((ball.x < home[player_pos-2][0].x + LENGTH4) && /* ball outside region? */

		(ball.x > home[player_pos-2][0].x - LENGTH4)) )||

		((player_pos > 3) && fabs(pos.phi) < PI/8)) /* striker facing opponent's goal */

	    {

	      ball_count = 0;

	      

	      /* angle to ball */

	      ball_angle = get_angle(ball.x - pos.x, ball.y - pos.y, pos.phi);



	      /* angle in which robot would hit the ball after turning to it */

	      angle_after_turn = pos.phi - 2 * ball_angle;

	      if (fabs(angle_after_turn) > PI)

                angle_after_turn -= 2 * fsign(angle_after_turn) * PI;



	      /* drive in/turn to right position to drive curve to ball */

	      if (fabs(angle_after_turn) > PI2) /* turn to own goal */

	      {

		LCDSetPos(4, 12);

		LCDPrintf("be1\n");

		

		/* side of ball where robot has to drive */

		diff_y = ball.y - pos.y;

		side = fsign(diff_y);



		ball.x -= arounddist; /* drive behind ball */



		/* position of middle of circle: used to check whether robot is too

		   close to ball */

		middle_x = ball.x - side * arounddist / 2 * sin(ball.phi);

		middle_y = ball.y - side * arounddist / 2 * cos(ball.phi);

		

		/* difference of position robot - middle of circle */

		diff_x = middle_x - pos.x;

		diff_y = middle_y - pos.y;

		

		mid_dist = hypot(diff_x, diff_y);

		

		done = FALSE;

		if(mid_dist > arounddist)

		  done = drive_around(diff_x, diff_y, side, pos.phi);

		else

		  VWDriveTurn(vw, side * PI, ROT_SPEED);

	

		if (done)

		{

		  VWGetPosition(vw, &pos);

		  /* angle to ball */

		  ball_angle = get_angle(ball.x - pos.x, ball.y - pos.y, pos.phi);

		  

		  /* angle in which robot would hit the ball after turning to it */

		  angle_after_turn = pos.phi - 2 * ball_angle;

		  if (fabs(angle_after_turn) > PI)

		    angle_after_turn -= 2 * fsign(angle_after_turn) * PI;

		  

		  LCDSetPos(4, 12);

		  LCDPrintf("be2\n");

		    

		  if (fabs(angle_after_turn) < PI / 4)

		  {

		    start_curve_to_ball(pos, ball, TRUE); /* drive behind ball */

		    

		    if (VWDriveDone(vw) && !see_ball_flag)

		      move_camera(DOWN); /* lost ball - maybe already in

					    front of robot -> check */

		  }

		  else

		    VWDriveTurn(vw, -side * PI, ROT_SPEED);

		}

		ball.x += arounddist; /* real ball position */

	      }

	      else

	      {

		LCDSetPos(4, 12);

		LCDPrintf("frn\n");

		VWGetPosition(vw, &pos);

		start_curve_to_ball(pos, ball, FALSE); /* drive behind ball */

	      }

	    }

	   }

	    else

	    {

	      ball_count ++;

	    } 

	}

	else                    /* no ball in sight */

        {

          LCDSetPos(1, 10);

          LCDPrintf("     \n");

          LCDSetPos(2, 10);

          LCDPrintf("     \n");

          LCDSetPos(3, 10);

          LCDPrintf("     \n");

	  LCDSetPos(4, 12);

	  LCDPrintf("   \n");

	  

	  ball_count = 0;

	  VWSetSpeed(vw, 0.0, 0.0);

	  

	  if (use_motors && (cam_pos == UP))

	    {

	      drive_home();

	      move_camera(DOWN);

	    }

	  new_ball = see_ball_flag;

	}

    if (!new_ball)

      OSReschedule();

  }

  end_flag += 1;

  OSKill(0);

  }







/***********************************************************************/

/** GOALIE: Drive to ball on circle near goal line.

    Drive curve near goal line while always facing the ball.

        

    Called by PPdrive_goal().



    @see PPdrive_goal()

    @param ball structure containing coordinates of ball and

    angle from ball position to opponent's goal

*/

/***********************************************************************/



  void start_goal_line(PositionType ball)

{

  float robot_angle, diff_angle, correct_angle;

  float drive_speed;

  

  PositionType pos;

  

  VWGetPosition(vw, &pos);

  ball.phi = atan2(ball.y, goaldist + ball.x); /* ball angle */

  robot_angle = atan2(pos.y, goaldist + pos.x); /* goal angle */



  /* angle between lines middle of circle to robot and middle of

     circle to ball */

  diff_angle = ball.phi - robot_angle;

  

  see_ball_flag = FALSE;        /* already driving to current ball position */



  /* ------------------------ correct heading ------------------------- */

  /* correct heading if robot is standing at the corner of the goal

     and has to drive away: turn until robot is standing perpendicular

     to line to middle of circle */

  if ((fsign(diff_angle) != fsign(pos.y)) && (fabs(pos.y) >= GOALWIDTH / 3.0))

    VWDriveTurn(vw, -PI2 - pos.phi + robot_angle, ROT_SPEED); 

  while (!VWDriveDone(vw) && !see_ball_flag && !end_flag)

    OSReschedule();

  /* ------------------------------------------------------------------ */



  if (!end_flag && !obstacle_flag)

  {

    if (fabs(ball.phi) > anglemax)

      diff_angle = fsign(ball.phi) * anglemax - robot_angle;



    /* ------------------------ drive to ball ------------------------ */

    if ((fabs(diff_angle) > PI / 100.0)) /* don't react on little changes or if 

					 already facing ball */

    {

      drive_speed = 4.0 * fabs(diff_angle) * LIN_SPEED;

      if (drive_speed < LIN_SPEED / 4.0)

	drive_speed = LIN_SPEED / 4.0;

      if (drive_speed > LIN_SPEED)

	drive_speed = LIN_SPEED;

  

      VWDriveCurve(vw, -diff_angle * radius, -fabs(diff_angle), drive_speed);  

      while (!VWDriveDone(vw) && 

             ((fsign(diff_angle) != fsign(pos.y)) || 

              (fabs(pos.y) < GOALWIDTH / 3.0)) && 

             !see_ball_flag && !obstacle_flag && !end_flag)

      {

        OSReschedule();

        VWGetPosition(vw, &pos);

      }

    }

    VWSetSpeed(vw, 0.0, 0.0);



    if (!see_ball_flag)

    {

      /* ------------------------ correct heading ------------------------- */

      /* turn to look at ball in case robot is standing at the corner

	 of its goal */

      correct_angle = PI2 + pos.phi - ball.phi;

      if (fabs(correct_angle) > PI / 200.0)

        VWDriveTurn(vw, -correct_angle, ROT_SPEED / 2.0); 

    }

  }

}



    

/***********************************************************************/

/** GOALIE: Set movement.

    Move robot and output text (drive status) on LCD.

    Activate robot movement according to flags end_flag,

    obstacle_flag, got_ball_flag and see_ball_flag, concerning

    different priorities.



    Process started by main().

    @see main() */

/***********************************************************************/



void PPdrive_goal()

{

  PositionType ball, last, pos;

  int last_flag = FALSE; /* flag whether last time ball has been detected */

  int factor = 0;



  while(!end_flag)

  {

/*     LCDPrintf("control\n"); */



    LCDSetPos(0, 10);

    LCDPrintf("o%ds%d\n", obstacle_flag, see_ball_flag);



    if (obstacle_flag)

    {

      LCDSetPos(1, 10);

      LCDPrintf("Wall!\n");

      LCDSetPos(2, 10);

      LCDPrintf("     \n");

      LCDSetPos(3, 10);

      LCDPrintf("     \n");

      

      last_flag = FALSE;

      VWSetSpeed(vw, 0.0, 0.0);

    }

    else 

      if (see_ball_flag)

      {

	AUBeep();



	get_ball_coord(&ball);

	LCDSetPos(1, 10);

        LCDPrintf("Ball:\n");

        LCDSetPos(2, 10);

        LCDPrintf("(%d,\n", (int)(100 * ball.x));

        LCDSetPos(3, 10);

        LCDPrintf("% 2d) \n", (int)(100 * ball.y));

        

	factor = 3;		/* allow factor pictures without ball 

				   after driving to it */



	if ((ball.x < 0.3) || last_flag) /* use last ball coordinates

					    if available */

	{

	  if (last_flag)	/* propable future ball position */

	  {

	    ball.x -= last.x - ball.x;

	    ball.y -= last.y - ball.y;

	  }

	  VWGetPosition(vw, &pos);

	  if (hypot(ball.x - pos.x, ball.y - pos.y) < 0.05)

	    kick_ball();    

	  start_goal_line(ball);

	}

	last = ball;

	last_flag = TRUE;

      }

      else                      /* no ball in sight */

      {

        LCDSetPos(1, 10);

        LCDPrintf("     \n");

        LCDSetPos(2, 10);

        LCDPrintf("     \n");

        LCDSetPos(3, 10);

        LCDPrintf("     \n");



	last_flag = FALSE;

        if (factor)

          factor --;

        else

        {

	  ball.x = 0.1;

	  ball.y = 0.0;

	  start_goal_line(ball);   /* lost ball -> drive back to middle of goal */

          if (VWDriveDone(vw))

          {

            LCDSetPos(2, 10);

            LCDPrintf("MID  \n");

          }

        }

      }

    OSReschedule();

  }

  end_flag += 1;

  OSKill(0);

}



/*@}*/