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

  \author Satofumi KAMIMURA

  $Id: encoder_control.c 1932 2010-10-02 21:46:01Z satofumi $
*/

#include "encoder_control.h"
#include <7125S.H>


void encoder_initialize(int id, encoder_t *encoder)
{
  if (id == 0) {
    MTU2.TSTR.BYTE &= ~0x02;
    MTU21.TMDR.BYTE = 0x04;

    // TCOKB(PA7), TCLKA(PA6)
    PFC.PACRL2.WORD &= ~0xff00;
    PFC.PACRL2.WORD |= 0x1100;

    MTU2.TSTR.BYTE |= 0x02;

  } else if (id == 1) {
    MTU2.TSTR.BYTE &= ~0x04;
    MTU22.TMDR.BYTE = 0x04;

    // TCLKD(PA9), TCLKC(PA8)
    PFC.PACRL3.WORD &= ~0x00ff;
    PFC.PACRL3.WORD |= 0x0011;

    MTU2.TSTR.BYTE |= 0x04;
  }

  encoder->reset = 0;
  encoder->id = id;
  encoder->difference = 0;

  encoder_reset(encoder);
}


void encoder_reset(encoder_t *encoder)
{
  int id = encoder->id;

  if (id == 0) {
    MTU2.TSTR.BYTE &= ~0x02;
    MTU21.TCNT = 0;
    MTU2.TSTR.BYTE |= 0x02;

  } else if (id == 1) {
    MTU2.TSTR.BYTE &= ~0x04;
    MTU22.TCNT = 0;
    MTU2.TSTR.BYTE |= 0x04;
  }
  encoder->previous_count = 0;
}


void encoder_update(encoder_t *encoder)
{
  int id = encoder->id;

  short current_count = encoder_count(id);
  encoder->difference = current_count - encoder->previous_count;
  encoder->previous_count = current_count;
}


short encoder_difference(const encoder_t *encoder)
{
  // myrobot のモータ回転方向とエンコーダの向きが逆なのを
  // 負号を付けることで吸収している
  return -encoder->difference;
}


unsigned long encoder_count(int id)
{
  if (id == 0) {
    return MTU21.TCNT;

  } else if (id == 1) {
    return MTU22.TCNT;

  } else {
    return 0;
  }
}
