#include <xc.h>
#include <stdint.h>
#include "mcr.h"
#include "servo.h"
#include "io.h"

static void delay_usec_by_tmr(uint16_t t_usec);

/*-------------------------------------------------------
 * サーボモータへパルスを送信
 *   16ビットタイマを使用 (e.g., TMR1)
 *-------------------------------------------------------*/
void servo_pulse(uint16_t pos)
{
  /* サーボモータパルス幅の計算 [uSec] */
  uint16_t pulse_width = PULSE_WIDTH_NEUTRAL;
  switch(sw_degree_range) {
  case DEGREE_RANGE_90:   /* 可動範囲:  90度 */
    pulse_width = PULSE_WIDTH_90(pos);
    if        (pulse_width < PULSE_WIDTH_90_MIN) {
      pulse_width = PULSE_WIDTH_90_MIN;
    } else if (pulse_width > PULSE_WIDTH_90_MAX) {
      pulse_width = PULSE_WIDTH_90_MAX;
    }
    break;
  case DEGREE_RANGE_180:  /* 可動範囲: 180度 */
    pulse_width = PULSE_WIDTH_180(pos);
    if        (pulse_width < PULSE_WIDTH_180_MIN) {
      pulse_width = PULSE_WIDTH_180_MIN;
    } else if (pulse_width > PULSE_WIDTH_180_MAX) {
      pulse_width = PULSE_WIDTH_180_MAX;
    }
    break;
  }
  /* サーボモータへ１パルスを出力 */
  OUT_SERVO_PULSE_ON();
  delay_usec_by_tmr(pulse_width);
  OUT_SERVO_PULSE_OFF();
  delay_usec_by_tmr(PULSE_CYCLE - pulse_width);
}

/*-------------------------------------------------------
 * 遅延関数
 *   16ビットタイマを使用 (e.g., TMR1)
 *-------------------------------------------------------*/
static void delay_usec_by_tmr(uint16_t t_usec)
{
  /* 設定より: タイマ1周 (0x10000カウント)は 32.768 [mSec] */
  /* t_usec [uSec] の経過 = (2*t_usec) [カウント] の経過 */
  /* 20 [mSec] 間で 40000 [カウント] */
  /* 1 [uSec] で 2 [カウント] (= 0x100000 / 32768 = 2) */
  uint16_t tmr1_val = 0 - (t_usec << 1);
  TMR1_Stop();
  TMR1_Write(tmr1_val);
  TMR1_Start();
  for (;;) {
    if (TMR1_IsExpired()) {
      break;
    }
  }
  TMR1_Stop();
}
