/***********************************************************************/
/** @name servos.c 
    Contains functions to init, release and move servos for camera and
    kicker.
    
    @author Birgit Graf, UWA, 1998, modified 2000 (Mk3/4)
*/
/***********************************************************************/

/*@{*/

#include "global.h"
#include "general.h"
#include "servos.h"


/** simulate servos ? */
int sim_camera = FALSE;
int sim_kicker = FALSE;


/** handle for servos (camera and kicker) */
ServoHandle camera, kicker;


#define  KICKER_DOWN 220
#define  KICKER_UP   130


/* Todo: Robot specific parameters! Move to HDT! */
#define  CAMERA_ANGLE  180  /* maximum camera angle */
#define  CAMERA_MAX   255   /* maximum camera value */

int  CAMERA_MID_POS= 120; /* value at middle position (robot B)*/


/* in case of badly adjusted camera: new conversion factors */
int cam_new_max, cam_min, cam_new_angle;   

#define SERVO_CAM SERVO11
#define SERVO_KICK SERVO12



/***********************************************************************/
/** Init servos.
 */
/***********************************************************************/

void Init_Servos()
{
  /* init servos */
  if (!sim_camera)
  {
    if (!(camera = SERVOInit(SERVO_CAM))) 
    {
			LCDClear();
			LCDMenu("", "", "", "OK");
			LCDPutString("CameraSerInit \nerror!\nSwitching to\nsim mode  ");
			KEYWait(KEY4);
			LCDClear();
			
			sim_camera = TRUE;
    }
    else
    {
			/* factors for adjusting camera */
			if (2 * CAMERA_MID_POS > CAMERA_MAX)    /* servo shifted to pos. side */
			{
				cam_new_max = 2 * (CAMERA_MAX - CAMERA_MID_POS);
				cam_min = CAMERA_MAX - cam_new_max;
			}
			else	  /* servo shifted to neg. side */
			{
				cam_new_max = 2 * CAMERA_MID_POS;
				cam_min = 0;
			}
			cam_new_angle = CAMERA_ANGLE - CAMERA_ANGLE * cam_min / CAMERA_MAX; /* max extension in degrees */
			SERVOSet(camera,  CAMERA_MID_POS);
    }
  }
	
  if (!sim_kicker)
  {
    if (!(kicker = SERVOInit(SERVO_KICK)))
    {
			LCDClear();
			LCDMenu("", "", "", "OK");
			LCDPutString("KickerSerInit \nerror!\nSwitching to\nsim mode  ");
			KEYWait(KEY4);
			LCDClear();
			
			sim_kicker = TRUE;
    }
    else
			SERVOSet(kicker, KICKER_DOWN);
  }

	/* set camera middle position depending on robot */
  switch(OSMachineID())
		{
	case 1:
		CAMERA_MID_POS=120;
		break;
	case 2:
		CAMERA_MID_POS=136;
		break;
	case 3:
		CAMERA_MID_POS=120;
		break;
	default:
		CAMERA_MID_POS=120;
		break;
	}
  cam_pos = 0;		/* position in degrees from middle */
}


/***********************************************************************/
/** Release servos.
 */
/***********************************************************************/

void End_Servos()
{
  if (!sim_camera)
    SERVORelease(camera);
  if (!sim_kicker)
    SERVORelease(kicker);
}


/***********************************************************************/
/** Move camera.
    Move camera left or right and set cam_pos (=position of camera).

    Called by drive_to_goal() and PPball_test().
    @see drive_to_goal()
    @see ball_test()
    @param new_pos desired position of camera (degrees)
*/
/***********************************************************************/
   

void move_camera(int new_pos)
{
  if (new_pos != cam_pos && !sim_camera)	/* need to change camera position */
  {
    int servo_value;
    servo_value = new_pos * cam_new_max / cam_new_angle + CAMERA_MID_POS;
    SERVOSet(camera, servo_value);

    OSWait(10);
    cam_pos = new_pos;
  }
}


/***********************************************************************/
/** Kick ball.
    Move kicker forward to kick ball which should be in front of robot.

    Called by drive_to_goal().
    @see drive_to_goal()
*/
/***********************************************************************/


void kick_ball()
{  
  if (!sim_kicker)
  {
    SERVOSet(kicker, KICKER_UP);
    OSWait(20);
    SERVOSet(kicker, KICKER_DOWN);
  }
}

/*@}*/