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

/** @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

    @author Andrew Berry, UWA, 1999



*/

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



/*@{*/



#include "global.h"

#include "general.h"

#include "image.h"

#include "servos.h"

#include "sensors.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

#define epsilon2 0.1

#define epsilonphi PI/5



/** 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





int patrol_leg = 1; /* which part of the patrol circuit it has reached */



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

/** Check Goal

    waits for the image process to check for the goal



    Called by Check_goal_position()



    @author Andrew Berry, UWA, 1999



*/

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



void check_goal()

{

  look_for_goal_flag = TRUE;

  found_goal_flag = 0;

  while (found_goal_flag == 0)

  {

    OSReschedule();

    LCDSetPos(0,0);

    LCDClear();

    /*wait for image processing */

  }



}



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

/** Check Goal Position

    checks to see if the goal is where it should be



    Called by PPdrive_field()

    @see PPdrive_field()



    @author Andrew Berry, UWA, 1999



 */

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



int Check_goal_position()

{

  PositionType pos;



  /* rotate to 0 degrees */

  VWGetPosition(vw,&pos);

  VWDriveTurn(vw,-pos.phi,ROT_SPEED);

  while(!VWDriveDone(vw))

      OSReschedule();



  /*look to see if the goal is where it should be*/



  check_goal();

  if (found_goal_flag == 2)

    {

      /*  it there*/

      return TRUE;

    }

  else

    {

      /*  it is lost*/

      return FALSE;

    }

}







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

/** Reset position

    resets the angle based on the target goal. Called when the goal isn't where

    it is suppost to be .



    Called by PPdrive_field()



    @author Andrew Berry, UWA, 1999



 */

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



int reset_angle()

{



  PositionType pos;

  float rot_count=0.0;



  LCDClear();



  /* turn pi/8 radians and look for goal. Repeat until it is found of

   you've turned 360 degrees. */



  while ( (found_goal_flag != 2) && (rot_count < 2*PI))

  {

    LCDSetPos(1,10);

    LCDPrintf("g srch\n");

    found_goal_flag = 0;



    VWDriveTurn(vw,PI2/4,ROT_SPEED);

    move_camera(UP);

    check_goal();

    while (!VWDriveDone(vw))

    {

      LCDClear();

      OSReschedule();

    }

    rot_count += PI2/4;



  }



  if (rot_count != 2*PI)

  {



    VWDriveTurn(vw,-PI2/8,ROT_SPEED);

    while (!VWDriveDone(vw))

    {

      /*do nothing */

    }

    VWGetPosition(vw,&pos);

    VWSetPosition(vw,pos.x,pos.y,0.0);

    LCDClear();

    return 1;

  }

  else

  {

    LCDClear();

    return 0;

  }

}









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

/** 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;

    default:

      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, 2.0, 0.05);

        break;

      case 1:

        ROT_SPEED = set_fparam("Rot. speed", 0.0, ROT_SPEED, 2.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).



    Also resets the x or y co-ord when the obstacle is a wall.



    Called by PPdrive_field().

    @see PPdrive_field()

    @author Andrew Berry, UWA, 1999





*/

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



void avoid_obstacle()

{

  PositionType pos, newpos;

  float delta_x,delta_y;

  float wall_dist;



  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)){}

  }





  check_wall_flag = TRUE;

  is_wall_flag = FALSE;



  OSReschedule(); /* switch threads */



  wall_dist = GetPSDValue();



  /* if it is a wall, reset position of robot */

  if (is_wall_flag)

  {

    if ( ( pos.phi > PI/4) && (pos.phi < 3*PI/4))

      /* i.e. facing a left side wall */

    {

      LCDSetPos(0,0);

      LCDPrintf("left wall\n");

      OSWait(50);



      newpos.y = WIDTH2 - wall_dist;

      newpos.x = pos.x;

      newpos.phi = pos.phi;

    }

    else

      if ( ( pos.phi < -PI/4) && (pos.phi >- 3*PI/4) )

        /* i.e. facing a right side wall */

      {

        LCDSetPos(0,0);

        LCDPrintf("rght wall\n");

        OSWait(50);



        newpos.y = -WIDTH2 + wall_dist;

        newpos.x = pos.x;

        newpos.phi = pos.phi;

      }

      else

        if ( ( (pos.phi <= PI/4) && (pos.phi >=0))

             ||( (pos.phi<=0) && (pos.phi >= PI/4)  ) )

          /* i.e. facing target goal */

        {

          LCDSetPos(0,0);

          LCDPrintf("my   goal\n");

          OSWait(50);



          newpos.x = -LENGTH + wall_dist;

          newpos.y = pos.y;

          newpos.phi = pos.phi;

        }

        else

          /* facing defending goal */

        {

          LCDSetPos(0,0);

          LCDPrintf("ther goal\n");

          OSWait(50);



          newpos.x = LENGTH - wall_dist;

          newpos.y = pos.y;

          newpos.phi = pos.phi;

        }



    /* reset original home position */

    VWSetPosition(vw,newpos.x,newpos.y,newpos.phi);

    delta_x = pos.x - newpos.x;

    delta_y = pos.y - newpos.y;



    reset_pos.x -=delta_x;

    reset_pos.y -=delta_y;

    is_wall_flag = FALSE;

    found_wall_flag = TRUE;

    AUBeep();

    AUBeep();

  }



  /* drive back from obstacle */



  LCDSetPos(0,0);

  LCDPrintf("back off\n");

  VWDriveStraight(vw, -0.1, LIN_SPEED);

  while(!VWDriveDone(vw))

  {

    OSReschedule();

    if (see_ball_flag == TRUE)

    {

      VWSetSpeed(vw,0,0);

    }

  }

  LCDSetPos(0,0);

  LCDPrintf("turn    \n");

  if((rand()%2)==0)

    VWDriveTurn(vw,1.57,0.6);

  else

    VWDriveTurn(vw,-1.57,0.6);



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

    {

      OSReschedule();

      if (see_ball_flag == TRUE)

        VWSetSpeed(vw,0,0);

    }



  VWDriveStraight(vw, 0.1, LIN_SPEED);

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

  {

    OSReschedule();

    if (see_ball_flag == TRUE)

      {

        VWSetSpeed(vw,0,0);

      }

  }



  move_camera(UP);

  obstacle_flag = FALSE;

  got_ball_flag = FALSE;

  see_ball_flag = FALSE;                /* lost ball while driving

                                           back -> search again */

  OSReschedule();

}







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

/** 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 -PI and PI */

  if (diffphi >= PI)

    diffphi -= 2.0 * PI;

  if (diffphi <= -PI)

    diffphi += 2.0 * PI;



  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;



  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;





  PositionType pos;

  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;

    }

  }

}



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

/** Drives to a point on the field

    Moves the robot in a straight line to a point, and gives it the

    desired facing.

    Will stop early if it spots the ball.



    @param TargetX the desired x co-ord

    @param TargetY the desired y co-ord

    @param TargetPhi the desired facing angle



    Returns 1 if completed successfully

            0 if stopped early



    Called by PPdrive_field()

    @see PPdrive_field()

    @author Andrew Berry

*/

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



int DriveTo(float TargetX, float TargetY, float TargetPhi)

{

  PositionType start_pos;

  float deltaX,deltaY,deltaPhi;





  /* determine direction to travel in */

  VWGetPosition(vw,&start_pos);

  deltaX = TargetX - start_pos.x;

  deltaY = TargetY - start_pos.y;

  if( (fabs(deltaX)>epsilon2) || (fabs(deltaY)>epsilon2))

  {

    OSReschedule();

    deltaPhi = get_angle((float)deltaX,(float)deltaY,start_pos.phi);

    OSReschedule();



    /* turn to face the direction of travel */

    VWDriveTurn(vw,deltaPhi,ROT_SPEED);

    OSReschedule();

    while(!VWDriveDone(vw))

    {

      OSReschedule();

      if (see_ball_flag == TRUE || obstacle_flag == TRUE)

      {

        UpdateFlags();

        VWSetSpeed(vw,0,0);

        return 0;

      }

    }

    OSReschedule();



    /* drive to point */

    VWDriveStraight(vw,hypot(deltaX,deltaY),LIN_SPEED);

    OSReschedule();

    while(!VWDriveDone(vw))

    {

      OSReschedule();

      if (see_ball_flag == TRUE || obstacle_flag == TRUE )

      {

        UpdateFlags();

        VWSetSpeed(vw,0,0);

        return 0;

      }

    }

    OSReschedule();

  }

  VWGetPosition(vw,&start_pos);

  OSReschedule();

  deltaPhi = TargetPhi-start_pos.phi;



  /* turn to the final facing */



  if (fabs(deltaPhi)>epsilonphi)

  {

    OSReschedule();

    if (deltaPhi>PI)

      deltaPhi = -(deltaPhi - PI);

    VWDriveTurn(vw,deltaPhi,ROT_SPEED);

    OSReschedule();

    while(!VWDriveDone(vw))

    {

      if (see_ball_flag == TRUE || obstacle_flag == TRUE)

      {

        UpdateFlags();

        VWSetSpeed(vw,0,0);

        return 0;

      }

      OSReschedule();



    }

  }

  OSReschedule();

  LCDSetPos(1, 10);

  return 1;

}







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

/** Find_Lost_Ball()

    Attempt to find out what happened to the ball that was just lost by turning

    in the direction the ball was traveling in.



    @return true if the ball is found, false if it is not

    @Called by PPDriveField()

    @author Andrew Berry

*/

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

int Find_Lost_Ball()

{



  float turn_angle;



  AUBeep();



  /* which direction do we need to go ? */

  if (BallImagePos == LEFT_EDGE)

  {

    turn_angle = PI2;

    LCDSetPos(4,0);

    LCDPrintf("Lost:Left \n");

  }

  else

  {

    turn_angle = -PI2;

    LCDSetPos(4,0);

    LCDPrintf("Lost:Right \n");

  }



  /* turn and look */



  VWDriveTurn(vw,turn_angle,ROT_SPEED);

  while(!VWDriveDone(vw))

    {

      if ((see_ball_flag == TRUE) || (obstacle_flag == TRUE))

      {

        OSReschedule();

        VWSetSpeed(vw,0,0);

        return 0;

      }

      OSReschedule();

    }

  return 1;

}





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

/** AttackBall

    Robot drives to the balls position.

    @Called by PPDriveField()

    @author Andrew Berry

*/

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



int AttackBall()

{

  PositionType pos, ball;

  int ball_count = 0;

  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;



    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) ))  ||

         ( ((MyRole == ATTACKER) && 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 ++;

      }

    return 1;

}



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

/** Procedure for handling the receiving of messages and what to do with the message



    'r' means reset

    'b # #' is the ball co-ordinates

    'g' means go



    @Called by PPDriveField()

   @author Andrew Berry

*/

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



void RecvMesg()

{



  BYTE  buffer[64],*bufferpointer;

  BYTE  SenderID;

  int   bytesReceived;

  float f1,f2;



  while(RADIOCheck()!=0)

  {

    if(RADIORecv(&SenderID,&bytesReceived,buffer))

      OSPanic("Can't receive message\n");





    switch(buffer[1]){

    case 'r':

      reset_flag = TRUE;

      break;

    case 'b':

      if (OSMachineID() != SenderID )

      {

        bufferpointer = strchr(buffer,' ');

        f1=atof(bufferpointer);

        bufferpointer = strrchr(bufferpointer,' ');

        f2=atof(bufferpointer);

        if ((fabs(ToldPos.x - f1)>epsilon2) || (fabs(ToldPos.y - f2)>epsilon2))

        {

          /*update only if it's a new pos */

          ToldPos.x = f1;

          ToldPos.y = f2;

          told_where_flag = TRUE;

        }

      }

      break;

    case 'g':

      play_flag = TRUE;

      break;

    default:{}

    }

  }

  free(buffer);

}



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

/** Patrol

    has the robot randomly patrol around through a set looking for the ball.

    Returns 1 if the ball is found, 0 if no ball is found.



    @author Andrew Berry



    Called by PPdrive_field()

    @see  PPdrive_field()

*/

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

int Patrol()

{

  static PositionType target_pos,pos;



  if (patrol_leg == 0 )

    patrol_leg = 1;

  VWGetPosition(vw,&pos);



  patrol_leg = rand()%9;



  if (MyRole == DEFENDER)

    switch(patrol_leg){

    case 0:

      target_pos.x = 0.4;

      target_pos.y = -0.3;

      target_pos.phi = 0;

      break;

    case 1:

      target_pos.x = 0.4;

      target_pos.y = 0.3;

      target_pos.phi = 0;

      break;

    case 2:

      target_pos.x = 0.6;

      target_pos.y = 0.3;

      target_pos.phi = 0;

      break;

    case 3:

      target_pos.x = 0.6;

      target_pos.y = -0.3;

      target_pos.phi = 0;

      break;

    case 4:

      target_pos.x = 1.2;

      target_pos.y = -0.3;

      target_pos.phi = 0;

    case 5:

      target_pos.x = 1.2;

      target_pos.y = 0.3;

      target_pos.phi = 0;

    case 6:

      target_pos.x = 0.4;

      target_pos.y = 0;

      target_pos.phi = 0;

    case 7:

      target_pos.x = 0.6;

      target_pos.y = 0;

      target_pos.phi = 0;

    case 8:

      target_pos.x = 1.2;

      target_pos.y = 0;

      target_pos.phi = 0;

    default:

      {}

    }

  else /*MyRole = ATTACKER */

     switch(patrol_leg){

  case 0:

      target_pos.x = 1.4;

      target_pos.y = -0.3;

      target_pos.phi = 0;

      break;

    case 1:

      target_pos.x = 1.4;

      target_pos.y = 0.3;

      target_pos.phi = 0;

      break;

    case 2:

      target_pos.x = 1.6;

      target_pos.y = 0.3;

      target_pos.phi = 0;

      break;

    case 3:

      target_pos.x = 1.6;

      target_pos.y = -0.3;

      target_pos.phi = 0;

      break;

    case 4:

      target_pos.x = 2.0;

      target_pos.y = -0.3;

      target_pos.phi = 0;

    case 5:

      target_pos.x = 2.0;

      target_pos.y = 0.3;

      target_pos.phi = 0;

    case 6:

      target_pos.x = 1.4;

      target_pos.y = 0;

      target_pos.phi = 0;

    case 7:

      target_pos.x = 1.6;

      target_pos.y = 0;

      target_pos.phi = 0;

    case 8:

      target_pos.x = 2.0;

      target_pos.y = 0;

      target_pos.phi = 0;

    default:

      {}

     }



  return  DriveTo(target_pos.x,target_pos.y,target_pos.phi);

}



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

/** FindLoc()

    This procedure has the robot try to re-determine it's position on

    the field. It can't correct the angle, just x & y.



    Called by main() (not currently used)

    @see main()



    @author Andrew Berry

*/

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

int FindLoc()

{

  PositionType pos;





  LCDSetPos(1,10);

  LCDPrintf("findlc\n");



  VWGetPosition(vw,&pos);

  VWSetPosition(vw,pos.x,pos.y,0);



  /* set the y */

  VWDriveTurn(vw,PI2,ROT_SPEED);

  while(!VWDriveDone(vw));



  found_wall_flag = FALSE;

  while(found_wall_flag == FALSE)

  {

    if (GetPSDValue()>0.08)

    {

      VWDriveStraight(vw,0.15,LIN_SPEED);

      if (GetLeftPSDValue() < 0.08)

      {

        VWSetSpeed(vw,0,0);

        VWDriveTurn(vw,-PI/24,ROT_SPEED);

        while(!VWDriveDone(vw));

        VWDriveStraight(vw,0.15,LIN_SPEED);

      }

      if (GetRightPSDValue() < 0.08)

      {

        VWSetSpeed(vw,0,0);

        VWDriveTurn(vw,PI/24,ROT_SPEED);

        while(!VWDriveDone(vw));

        VWDriveStraight(vw,0.15,LIN_SPEED);

      }

    }

    else

    {

      VWSetSpeed(vw,0,0);

      AUBeep();AUBeep();

      avoid_obstacle();

    }

  }



  VWDriveStraight(vw,-0.1,LIN_SPEED);

  while(!VWDriveDone(vw));





  /* set the x */



  VWDriveTurn(vw,-PI2,ROT_SPEED);

  while(!VWDriveDone(vw));



  found_wall_flag = FALSE;

  while(found_wall_flag == FALSE)

  {

    if (GetPSDValue()>0.08)

    {

      VWDriveStraight(vw,0.15,LIN_SPEED);

      if (GetLeftPSDValue() < 0.08)

      {

        VWSetSpeed(vw,0,0);

        VWDriveTurn(vw,-PI/24,ROT_SPEED);

        while(!VWDriveDone(vw));

        VWDriveStraight(vw,0.15,LIN_SPEED);

      }

      if (GetRightPSDValue() < 0.08)

      {

        VWSetSpeed(vw,0,0);

        VWDriveTurn(vw,PI/24,ROT_SPEED);

        while(!VWDriveDone(vw));

        VWDriveStraight(vw,0.15,LIN_SPEED);

      }

    }

    else

    {

      VWSetSpeed(vw,0,0);

      AUBeep();AUBeep();

      avoid_obstacle();

    }

  }



  DriveTo(reset_pos.x,reset_pos.y,reset_pos.phi);

  LCDClear();

  return 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;*/



  BYTE mesg_buffer[64];        /* the message buffer */



  int kick_off = TRUE;

  int check_goal = FALSE;



  int need_to_check_counter = 0; /* counter to tell when to check angle */



  play_flag = FALSE;

  reset_flag = TRUE;





  while(!end_flag)

  {

    LCDMenu("Go  ","RSET","STOP","   ");

    LCDSetPos(1,0);

    LCDPrintf("here    \n");

    if (!see_ball_flag)

      got_ball_flag = FALSE;



    if (kick_off && play_flag)

      {

        kick_off = FALSE;

        kick_ball();

        VWDriveStraight(vw,0.2,LIN_SPEED);

      }





    need_to_check_counter++;





    OSReschedule();

    UpdateFlags();



    if (comms_flag ) /* I have a message */

      {

        LCDSetPos(1,0);

        LCDPrintf("chk mail\n");

        if( RADIOCheck()!=0)

          {

            AUBeep();

            LCDSetPos(1, 10);

            LCDPrintf("Mesg  \n");

            RecvMesg();

            OSReschedule();

          }

        else

          {

            LCDSetPos(1, 10);

            LCDPrintf("NoMesg\n");

          }

      }



    if (obstacle_flag && !see_ball_flag && !got_ball_flag) /* check obstacles */

      {

        LCDSetPos(0,0);

        LCDPrintf("obst dect\n");



        LCDSetPos(1,10);

        LCDPrintf("Obst!\n");

        move_camera(DOWN);

        OSReschedule();

        if (!see_ball_flag)

          avoid_obstacle();

        OSReschedule();

      }

    else

      if (find_loc_flag) /* find my location */

        {

          FindLoc();

          find_loc_flag = FALSE;

        }

      else

        if (reset_flag) /* time to reset to starting position */

          {

            UpdateFlags();

            LCDSetPos(1, 10);

            LCDPrintf("reset\n");

            play_flag = FALSE;

            while(!DriveTo(reset_pos.x,reset_pos.y,reset_pos.phi))

              {

                /* keep driving to the reset position */

                reset_flag = TRUE;

                OSReschedule();

              }

            reset_flag = FALSE;

            see_ball_flag = FALSE;

            told_where_flag = FALSE;

            just_lost_flag = FALSE;



          }

        else

          if (play_flag == TRUE) /*time to play */

          {

            LCDSetPos(1,0);

            LCDPrintf("playing   \n");

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

            if (see_ball_flag)

            {

              LCDSetPos(1,0);

              LCDPrintf("I see ball\n");

              VWGetPosition(vw, &pos);

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

              {

                move_camera(DOWN);

                got_ball_flag = TRUE;

              }

              LCDSetPos(1,0);

              LCDPrintf("looked down\n");

            }



            if(start_flag) /* go go go */

            {

              LCDSetPos(1,0);

              LCDPrintf("kick off  \n");

              UpdateFlags();

              if (MyRole == ATTACKER)

                kick_ball();





            /* start code here */

              start_flag = FALSE;

              reset_flag = FALSE;

            }



            else

              if (got_ball_flag) /* I have the ball in front of me */

              {

                LCDSetPos(1,0);

                LCDPrintf("got ball  \n");

                AUBeep();

                told_where_flag = FALSE;

                UpdateFlags();



                LCDSetPos(1,10);

                LCDPrintf("Got    \n");

                LCDSetPos(2,10);

                LCDPrintf("Ball   \n");



                new_ball = FALSE;

                move_camera(DOWN);



                if (MyRole == ATTACKER)

                {

                 drive_to_goal();

                }

                else /* I'm defending */

                {

                  drive_to_goal();

                }

              }

              else

                if (see_ball_flag) /* I see the ball */

                {

                  AUBeep();

                  AUBeep();



                  LCDSetPos(1,0);

                  LCDPrintf("seen ball \n");

                  told_where_flag = FALSE;

                  UpdateFlags();

                  LCDSetPos(4,0);

                  LCDPrintf("          \n");

                  LCDSetPos(1,10);

                  LCDPrintf("Ball  \n");

                  get_ball_coord(&ball);

                  LCDSetPos(2,10);

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

                  LCDSetPos(3,10);

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



                  /* tell everyone about the ball */



                  LCDSetPos(1,0);

                  LCDPrintf("tell about\n");

                  sprintf(mesg_buffer,"b %g %g ",ball.x,ball.y);

                  RADIOSend(BROADCAST,strlen(mesg_buffer)+1,mesg_buffer);



                  /* which edge info update */

                  if (BallImagePos == LEFT_EDGE)

                  {

                    LCDSetPos(3,0);

                    LCDPrintf("l edge\n");

                  }



                  if (BallImagePos == RIGHT_EDGE)

                  {

                    LCDSetPos(3,0);

                    LCDPrintf("r edge\n");

                  }



                  LCDSetPos(1,0);

                  LCDPrintf("attack!!!!\n");

                  if (MyRole == ATTACKER)

                  {

                    AttackBall();

                  }

                  else /* I'm defending */

                  {

                    AttackBall();

                  }

                }

                else

                  if (told_where_flag) /* someone else sees the ball */

                  {

                    LCDSetPos(1,0);

                    LCDPrintf("told where\n");

                    UpdateFlags();

                    /* for testing - go to where ball was last seen */

                    DriveTo(ToldPos.x,ToldPos.y,reset_pos.phi);

                    /* look behind you */

                    DriveTo(ToldPos.x,ToldPos.y,-reset_pos.phi);

                    /* otherwise give up & go somewhere else */

                    told_where_flag = FALSE;

                  }



                  else

                    if (just_lost_flag) /* I had it, now it's gone */

                    {

                      UpdateFlags();

                      if (BallImagePos == LEFT_EDGE)

                      {

                        LCDSetPos(3,0);

                        LCDPrintf("l edge\n");

                      }



                      if (BallImagePos == RIGHT_EDGE)

                      {

                        LCDSetPos(3,0);

                        LCDPrintf("r edge\n");

                      }



                      LCDSetPos(1,10);

                      LCDPrintf("LB!   \n");

                      Find_Lost_Ball();

                    }

                    else

                      if (teammate_has_ball) /* space for future teamwork stuff - does nothing now */

                      {

                        if (MyRole == ATTACKER)

                        {

                          /* attack stuff here */

                        }

                        else /* I'm defending */

                        {

                          /* defender stuff here */

                        }

                      }

                      else

                        /* default search activity */

                      {

                        if (need_to_check_counter > 25)

                          check_goal = TRUE;

                        if (check_goal)

                        {

                          LCDSetPos(1,10);

                          LCDPrintf("chk g\n");

                          need_to_check_counter = 0;

                          if(!Check_goal_position())

                          {

                            LCDSetPos(1,10);

                            LCDPrintf("fnd g\n");

                            reset_angle();

                          }

                          LCDPrintf("done\n");

                          check_goal = FALSE;

                        }



                        LCDSetPos(1,0);

                        LCDPrintf("patrol    \n");

                        LCDSetPos(1,10);

                        LCDPrintf("Pat  %d\n",patrol_leg);

                        UpdateFlags();

                        if(VWDriveDone(vw))

                          Patrol();

                      }

          }

  }

  end_flag += 1;

  OSReschedule();

  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;

  BYTE mesg_buffer[64];

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

  int factor = 0;



  LCDMenu("RESET", "GO", "STOP", "  ");





  AUBeep();

  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);



         /* tell everyone about the ball */

        sprintf(mesg_buffer,"b %g %g ",ball.x,ball.y);

        RADIOSend(BROADCAST,strlen(mesg_buffer)+1,mesg_buffer);



        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);

}



/*@}*/























