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

/** @name driveglobal.c 

    Contains global functions to init and release motors, control overall

    robot movement and set and read robot position.    

    

    @author Birgit Graf, UWA, 1998, modified 2000 (Mk3/4)

*/

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



/*@{*/





#include "driveglobal.h"

#include "general.h"





/** simulate motors (position is still being read)? */

int sim_motors = FALSE;



/** Handle for motors */

VWHandle vw;



/** maximum driving speeds */

meterPerSec LIN_SPEED = 0.2;

radPerSec ROT_SPEED = 0.6;



/* meterPerSec LIN_SPEED = 0.1;

radPerSec ROT_SPEED = 0.3; */



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



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

#define arounddist 0.1



/* distance below which robot should drive straight instead of curve to ball */

#define STRAIGHT_DISTANCE 0.1





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

/** Init VW interface.



    Called by main().

    @see main()

*/

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



void Init_Drive()

{

  if (!(vw = VWInit(VW_DRIVE, 1)))	/* initialize motors and encoders */

    error("VWInit!\n");



  if (sim_motors)  /* only need encoder readings, no motion */

		return;



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

	{

		LCDClear();

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

		LCDPutString("VWStart \nerror!\nSwitching to\nsim mode  ");

		KEYWait(KEY4);

		LCDClear();



		sim_motors = TRUE;

	}



	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:

		VWSetPosition(vw, 0, 0, 0); /* 0 position */

		break;

  }

}





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

/** Stop VW interface.

*/

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



void End_Drive()

{

  VWRelease(vw);

}





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

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

  if (sim_motors)

  {

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

    LCDPrintf("Motors \ndeactivated!");

    KEYWait(KEY4);

    LCDClear();

    return;

  }

  

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

    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(1=goalie, \n2=lb, 3=rb,\n4=lf, 5=rf)",

							1, player_pos, 5, 1);	  

				break;

      default: break;

      }

      

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

      break;



    case KEY2:

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

      ind ++;

			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 position.

    Interface function to vw.

*/

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



void set_pos(meter x, meter y, radians phi)

{

  VWSetPosition(vw, x, y, phi);

}





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

/** Return robot position in referenced parameter.

    Interface function to vw.

*/

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



void get_pos(PositionType* req_pos)

{

  VWGetPosition(vw, req_pos);

}





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

/** Set robot coordinates.

    Set robot x and y coordinates. (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", "0", "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, 0.0, 0.0, 0.0);

				break;

	    case KEY4:

				LCDClear(); end_proc2 = TRUE; break;

	    default: break;

	    }

	  }

      }	

    case KEY4:

      LCDClear(); end_proc = TRUE; break;

    default: break;

    }

  }

}





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

/** Stop.

    Interface function to vw.

*/

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



void drive_stop()

{

/*	LCDPrintf("Stop"); */

	VWSetSpeed(vw, 0.0, 0.0);

}





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

/** Did robot wheels stall?

    Interface function to vw.

*/

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



int wheels_stalled()

{

  return VWStalled(vw);

}





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

/** Get drive speed.

    Get drive speed depending on distance to goal

    @param dist distance to current goal

    @return speed

*/

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



float get_drive_speed(float dist)

{

	float speed = 2.0 * dist * LIN_SPEED;

  if (speed < LIN_SPEED / 2.0)

    speed = LIN_SPEED / 2.0;

  if (speed > LIN_SPEED)

    speed = LIN_SPEED;



	return speed;

}





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

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

/*	if (USE_AUDIO) */

/*     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=0, diff_y=0;

  /* distance and angle from robot to ball */

  float ball_angle=0, ball_dist=0;



  /* speed depending on distance of ball */

  float drive_speed=0, drive_dist=0;



  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 = get_drive_speed(drive_dist);



/*	KEYWait(ANYKEY);

	LCDClear();

	LCDPrintf("d:%f\na:%f\ns:%f\nt:%d\n", ball_dist, ball_angle, drive_speed, turn);

	KEYWait(ANYKEY);

	LCDClear();

*/

  if ((ball_dist < STRAIGHT_DISTANCE) && !turn)

    VWDriveStraight(vw, ball_dist, 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;

}









/*@}*/





