/*!
  \file
  \brief エンコーダ制御

  \author Satofumi KAMIMURA

  $Id: encCtrl.c 286 2008-10-20 09:40:22Z satofumi $
*/

#include <7040S.H>
#include "encCtrl.h"


/*!
  \brief エンコーダデバイスの初期化

  エンコーダ制御に用いる入力端子を初期化する。
*/
void initEnc(void) {
  MTU1.TMDR.BYTE = 0xc4;	/* encoder mode 1 */
  MTU2.TMDR.BYTE = 0xc4;	/* encoder mode 1 */
  MTU.TSTR.BYTE |= 0x06;	/* start count up */

  PFC.PECR2.WORD |= 0x1100;	/* TIOC1A, TIOC2A */
  MTU1.TIOR.BYTE = 0x0a;	/* input capture */
  MTU2.TIOR.BYTE = 0x0a;	/* input capture */

  PFC.PEIOR.WORD &= ~0x0050;	/* use TIOC1A, TIOC2A port */

  PFC.PACRL2.WORD &= ~0xf000;	/* TCLKA, TCLKB */
  PFC.PACRL2.WORD |= 0x5000;
  PFC.PACRL1.WORD &= ~0x000f;	/* TCLKC, TCLKD */
  PFC.PACRL1.WORD |= 0x0005;

  MTU1.TCNT = 0;		/* counter=0 */
  MTU2.TCNT = 0;
  MTU1.TGRA = 0;		/* buffer=0 */
  MTU2.TGRA = 0;
}


/*!
  \brief 指定デバイスID のエンコーダ値を読み出す

  id で指定されたエンコーダデバイスのカウント値(エンコーダ値)を読み出す。

  \param id [i] エンコーダデバイスID
  \retval エンコーダ値
*/
static unsigned short getEncValue(const unsigned char id) {
  unsigned short cnt = -1;

  if (id == 0) {
    cnt = MTU1.TGRA;
  } else if (id == 1) {
    cnt = MTU2.TGRA;
  }
  return cnt;
}


/*!
  \brief エンコーダ制御用の構造体を初期化

  エンコーダ制御用の構造体のメンバ変数を初期化する。

  \param id [i] エンコーダデバイスID
  \param enc [o] エンコーダ制御用の構造体
*/
void initEncInfo(const unsigned char id, encInfo_t *enc)
{
  enc->prev = getEncValue(id);
  enc->id = id;
  if (id == 0) {
    enc->enc_direction = ENC_CW_TO_PLUSE;

  } else if (id == 1) {
    enc->enc_direction = ENC_CCW_TO_PLUSE;
  }
  enc->diff = 0;
  enc->total = 0;
}


/*!
  \brief エンコーダ値を更新する

  エンコーダ値の変位量を更新する

  \param enc [i/o] エンコーダ制御用の構造体

  \attention 制御周期毎に１度だけ読み出すべき
*/
void updateEncDiff(encInfo_t *enc)
{
  short now = getEncValue(enc->id);
  enc->diff = enc->enc_direction * (now - enc->prev);
  enc->total += enc->diff;
  enc->prev = now;
}


/*!
  \brief エンコーダ値の変位を取得する

  前回の制御周期におけるエンコーダ値の変位量をエンコーダ制御用の構造体に格納すると同時に、返り値として返す。

  \param enc [i/o] エンコーダ制御用の構造体
  \retval エンコーダの変位量
*/
int getEncDiff(encInfo_t *enc) {
  return enc->diff;
}
