/*!
  \file
  \brief PWM 制御

  \author Satofumi KAMIMURA

  $Id: pwm_control.c 1677 2010-02-07 13:59:59Z satofumi $
*/

#include "pwm_control.h"
#include <stdbool.h>
#include <7125S.H>


void pwm_initialize(int id)
{
  (void)id;

  // TCNT の停止
  MTU2.TSTR.BYTE &= ~0xc0;

  // 出力設定
  MTU2.TOER.BYTE |= 0x12;

  // 周期を TGRA に設定
  MTU23.TCR.BYTE = 0x25;
  MTU24.TCR.BYTE = 0x25;
  MTU23.TGRA = 0x00ff;
  MTU24.TGRA = 0x00ff;

  // コンペアマッチ出力が TGRA と同じになるよう TGRC も設定する
  MTU23.TGRC = 0x00ff;
  MTU24.TGRC = 0x00ff;

  // duty 出力端子の初期化
  MTU23.TIOR.BYTE.H = 0x16;     // TGRB, TGRA
  MTU23.TIOR.BYTE.L = 0x16;     // TGRD, TGRC
  MTU24.TIOR.BYTE.H = 0x16;     // TGRB, TGRA
  MTU24.TIOR.BYTE.L = 0x16;     // TGRD, TGRC

  MTU23.TCNT = 0;
  MTU24.TCNT = 0;

  // PWM モード 1
  MTU23.TMDR.BYTE = 0x02;
  MTU24.TMDR.BYTE = 0x02;

  // TIOC4C(PE14), TIOC4A(PE12), TIOC3C(PE10), TIOC3A(PE8)
  PFC.PEIORL.WORD |= 0x5500;

  // 初期状態では、サーボフリーにする
  int i;
  for (i = 0; i < 2; ++i) {
    pwm_set(i, PwmMode_Free, 0);
  }

  // TCNT のスタート
  MTU2.TSTR.BYTE |= 0xc0;
}


static void PE8_set(bool on)
{
  PFC.PECRL3.WORD &= ~0x0007;
  PFC.PEIORL.WORD |= 0x0100;
  if (on) {
    PE.DRL.WORD |= 0x0100;
  } else {
    PE.DRL.WORD &= ~0x0100;
  }
}


static void PE10_set(bool on)
{
  PFC.PECRL3.WORD &= ~0x0700;
  PFC.PEIORL.WORD |= 0x0400;
  if (on) {
    PE.DRL.WORD |= 0x0400;
  } else {
    PE.DRL.WORD &= ~0x0400;
  }
}


static void PE12_set(bool on)
{
  PFC.PECRL4.WORD &= ~0x0007;
  PFC.PEIORL.WORD |= 0x1000;
  if (on) {
    PE.DRL.WORD |= 0x1000;
  } else {
    PE.DRL.WORD &= ~0x1000;
  }
}


static void PE14_set(bool on)
{
  PFC.PECRL4.WORD &= ~0x0700;
  PFC.PEIORL.WORD |= 0x4000;
  if (on) {
    PE.DRL.WORD |= 0x4000;
  } else {
    PE.DRL.WORD &= ~0x4000;
  }
}


static void pwm_setDuty(int id, unsigned char duty)
{
  if (id == 0) {
    MTU23.TGRB = duty;
    MTU23.TGRD = duty;

  } else if (id == 1) {
    MTU24.TGRB = duty;
    MTU24.TGRD = duty;
  }
}


void pwm_set(int id, PwmMode mode, unsigned char duty)
{
  // In1, In2 端子を設定する

  if (id == 0) {
    switch (mode) {
    case PwmMode_CW:
      // PWM, L
      if (duty == 0) {
        PE8_set(false);
      } else if (duty == 255) {
        PE8_set(true);
      } else {
        PFC.PECRL3.WORD |= 0x0001;
      }
      PE10_set(false);
      pwm_setDuty(id, duty);
      break;

    case PwmMode_CCW:
      // L, PWM
      PE8_set(false);
      if (duty == 0) {
        PE10_set(false);
      } else if (duty == 255) {
        PE10_set(true);
      } else {
        PFC.PECRL3.WORD |= 0x0100;
      }
      pwm_setDuty(id, duty);
      break;

    case PwmMode_Stop:
      // H, H
      PE8_set(true);
      PE10_set(true);
      break;

    case PwmMode_Free:
      // L, L
      PE8_set(false);
      PE10_set(false);
      break;
    }
  } else if (id == 1) {
    switch (mode) {
    case PwmMode_CW:
      // PWM, L
      if (duty == 0) {
        PE12_set(false);
      } else if (duty == 255) {
        PE12_set(true);
      } else {
        PFC.PECRL4.WORD |= 0x0001;
      }
      PE14_set(false);
      pwm_setDuty(id, duty);
      break;

    case PwmMode_CCW:
      // L, PWM
      PE12_set(false);
      if (duty == 0) {
        PE14_set(false);
      } else if (duty == 255) {
        PE14_set(true);
      } else {
        PFC.PECRL4.WORD |= 0x0100;
      }
      pwm_setDuty(id, duty);
      break;

    case PwmMode_Stop:
      // H, H
      PE12_set(true);
      PE14_set(true);
      break;

    case PwmMode_Free:
      // L, L
      PE12_set(false);
      PE14_set(false);
      break;
    }
  }
}
