/************************************************************************/
/*									*/
/* "servo.c" for the 6.270 'bot		         			*/
/*	servo support code						*/
/* by Anne Wright							*/
/* Sat Jan 11								*/
/*									*/
/************************************************************************/
/* these were chosen experimentally...*/
int MIN_SERVO_WAVETIME = 800;
int MAX_SERVO_WAVETIME = 4700;
int SERVO_RANGE =(MAX_SERVO_WAVETIME-MIN_SERVO_WAVETIME);

float rexcursion = 3.14159;
float dexcursion = 180.;

void servo_on()
{
  asm_servo_on(0);
}
void servo_off()
{
  asm_servo_off(0);
}
/************************************************************************/
/* Servo movement commands                                              */
int servo(int period) /* argument in clock cycles of pulse, moves servo */
{
  if(period>MAX_SERVO_WAVETIME)
    return (servo_pulse_wavetime=MAX_SERVO_WAVETIME);
  else if(period<MIN_SERVO_WAVETIME)
    return (servo_pulse_wavetime=MIN_SERVO_WAVETIME);
  else
    return(servo_pulse_wavetime=period);
}
int servo_rad(float angle) /* argument in radians, moves servo */
{
  return servo(radian_to_pulse(angle));
}
int servo_deg(float angle) /* argument in degrees, moves servo */
{
  return servo(degree_to_pulse(angle));
}
/************************************************************************/
/* Pulse width calculations                                             */
int radian_to_pulse(float angle) /* argument in radians, returns pulse width */
{
  return ((int)(angle*((float)SERVO_RANGE)/rexcursion)+MIN_SERVO_WAVETIME);
}

int degree_to_pulse(float angle) /* argument in degrees, returns pulse width */
{
  return ((int)((angle*((float)SERVO_RANGE))/dexcursion)+MIN_SERVO_WAVETIME);
}