/* servo.c */

/* servo controller code */

int servo_center[3] = { 1860, 2200, 550 };
float servo_90[3] = { -1890.0, -1890.0, 1700.0 };
int servo_min[3] = { 0, 0, 0 };
int servo_max[3] = { 4000, 4000, 4000 };
float servo_pos[3];

/* initialize servos */
int init_servos()
{
  int i;

  enable_servos();

  for (i=0; i<3; i++)
  {
    servo(i, servo_center[i]);
    servo_pos[i] = 0.0;
  }
  return 0;
}

float get_servo_pos(int servo_id)
{
  if (servo_id < 0 || servo_id > 2)
  {
#ifdef DEBUG
    printf("get_servo_pos: invalid id %d\n", servo_id);
#endif
    return _2PI;
  }

  return servo_pos[servo_id];
}

int set_servo_pos(int servo_id, float angle)
{
  int val;

  if (servo_id < 0 || servo_id > 2)
  {
#ifdef DEBUG
    printf("set_servo_pos: invalid id %d\n", servo_id);
#endif
    return 1;
  }

  val = servo_center[servo_id] +
    (int)(angle * servo_90[servo_id] * (1.0 / PI_2));

  if (val < servo_min[servo_id] || val > servo_max[servo_id])
  {
#ifdef DEBUG
    printf("set_servo: %d to %d\n", servo_id, val);
#endif
    return 2;
  }

  servo_pos[servo_id] = angle;
  servo(servo_id, val);

  return 0;
}