/* Simple code to control a server attached to the uCsimm with the PWM. */

#include <stdio.h>
#include <time.h>
#include <sys/time.h>
#include <asm/MC68EZ328.h>
#include <string.h>
#include <stdlib.h>
#include "servo.h"

void
servo_init(void)
{
  /* Setup the PWMO pin to be PWMO */
  PBSEL &= ~0x80;

  /* Set the PWM timer to have a period of ~20ms.  The uCsimm's clock
     is 16580608 Hz; set the prescaler to divide by 80, set CLKSEL to
     divide by 16, and program the preiod register with 0xFE to divide
     by 256.  This gives us a period of about 19.7ms. */
  PWMC = PWMC_EN;
  PWMC = (80 << PWMC_PRESCALER_SHIFT) |
    PWMC_EN |
    (3 << PWMC_CLKSEL_SHIFT);
  PWMP = 0xfe;

  /* Now, the PWM sample register will enable the PWMO pin for
     77us * PWM sample value.  So, since the valid ranges for a
     servo are from 0.5ms to 1.5ms, valid values are 6 to 19.
     13 will get you to the middle. */
  PWMS = (13 << 8) | 13;
}

/* Set the servo to point to a particular angle.  Valid angles are
   from -45 to 45. */
void
servo_set(int angle)
{
  int pwm_sample;

  /* Scale from angle (-45 to 45) to PWM sample (6 to 19)
     [Note: the second '+ 45' below is for rounding] */
  pwm_sample = (((angle + 45) * 13) + 45) / 90 + 6;
  if (pwm_sample < 1)
    pwm_sample = 1;

#ifdef SERVO_TEST
  printf ("Setting PWMS value to %d\n", pwm_sample);
#endif
  PWMS = (pwm_sample << 8) | pwm_sample;
}

#ifdef SERVO_TEST
int main(void)
{
  int angle;
  char inbuf[80];

  servo_init();

  while (1)
    {
      printf ("Angle: ");
      fflush(stdout);
      fgets(inbuf, 80, stdin);
      printf("\n");
      fflush(stdout);
      if ((strcmp (inbuf, "q") == 0) ||
	  (strcmp (inbuf, "quit") == 0))
	return 0;

      angle = atoi(inbuf);
      servo_set(angle);
    }
}
#endif