/***********************************************************************/
/** @name servos.c 
    Contains functions to init, release and move servos for camera and
    kicker.
    
    @author Birgit Graf, UWA, 1998
    @author Andrew Berry, UWA, 1999
*/
/***********************************************************************/

/*@{*/

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

enum {
  KICKER_DOWN =   0,
  KICKER_UP   = 225
};

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


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

void InitServos()
{
  /* init servos */
  if (!(camera = SERVOInit(SERVO11))) 
    error("CameraSerInit!");
  
  if (!(kicker = SERVOInit(SERVO12)))
    error("KickerSerInit!");  

  /* set to starting positions */
  LCDSetPos(4, 10);
  LCDPrintf("\n");
  SERVOSet(camera, 0);
  cam_pos = DOWN;
  OSWait(10);
  SERVOSet(kicker, KICKER_DOWN);
}


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

void ReleaseServos()
{
  SERVORelease(camera);
  SERVORelease(kicker);
}


/***********************************************************************/
/** Move camera.
    Move camera up or down 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
*/
/***********************************************************************/
   

void move_camera(int new_pos)
{
  if (new_pos != cam_pos)	/* need to change camera position */
  {
    LCDSetPos(4, 10);
    switch (new_pos)
    {
    case DOWN:
      LCDPrintf("D\n");
      SERVOSet(camera, 0);
      break;
    case MIDDLE:
      LCDPrintf("M\n");
      SERVOSet(camera, 150);
      break;
    case UP:
      LCDPrintf("U\n");
      SERVOSet(camera, 225);
      break;
    }
    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()
{
  SERVOSet(kicker, KICKER_UP);
  OSWait(20);
  SERVOSet(kicker, KICKER_DOWN);
}
/***********************************************************************/
/** Kick ball variable.
    Move kicker forward to kick ball which should be in front of robot, with
    a varying servo final position between 0 and 225.
    
    Called by drive_to_goal().
    @see drive_to_goal()
*/
/***********************************************************************/


void kick_ball_variable(int angle)
{
  if (angle>225 || angle < 0)
    OSPanic("invalid kick angle\n");
  SERVOSet(kicker, angle);
  OSWait(20);
  SERVOSet(kicker, KICKER_DOWN);
}
/*@}*/