/*!
  \file
  \brief モータ制御

  \author Satofumi KAMIMURA

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

#include "motorCtrl.h"
#include "encCtrl.h"
#ifdef SIMULATOR
enum { SMP_USEC = 1000 };
void set_mode(const unsigned char id, const int mode) {}
void set_pwm(const unsigned char id, const int duty) {}
#else
#include <7040S.H>

#include "tRunCtrl.h"
#ifndef SMP_USEC
#define SMP_USEC 1000
#endif


/*!
  \brief 指定モータ の PWM 出力を設定

  id で指定されるモータデバイスからの PWM出力を duty に設定する。duty は [0, 255] の範囲で指定し、duty == 255 のときは常に H 出力。duty == 0 の時に常に L 出力となる。

  \param id [i] モータデバイスID
  \param duty [i] 出力する PWM 比。(デューティ比) = duty / 255.0; となる。
*/
void set_pwm(const unsigned char id, const int duty)
{
  if (id == 0) {
    if (duty <= 0 || duty >= 255) {
      PFC.PECR1.WORD &= ~0x0001; /* PE8 */
      PE.DR.WORD &= ~0x0100;
      PE.DR.WORD |= (duty <= 0) ? 0 : 0x0100;
    } else {
      PFC.PECR1.WORD |= 0x0001; /* TIOC3A */
      MTU3.TGRB = duty;
    }
  } else if (id == 1) {
    if (duty <= 0 || duty >= 255) {
      PFC.PECR1.WORD &= ~0x0010; /* PE10 */
      PE.DR.WORD &= ~0x0400;
      PE.DR.WORD |= (duty <= 0) ? 0 : 0x0400;
    } else {
      PFC.PECR1.WORD |= 0x0010; /* TIOC3C */
      MTU3.TGRD = duty;
    }
  }
}


/*!
  \brief 指定モータの制御モードを設定

  モータの制御モード(MotorMode)を指定する。

  \param id [i] モータデバイスID
  \param mode [i] 制御モード
*/
void set_mode(const unsigned char id, const int mode)
{
  if (id == 0) {
    /* DIR(PE5), EN(PE7) */
    if (mode == MTR_MODE_CCW_BREAK) {
      PE.DR.WORD |= 0x0020;	/* DIR(PE5)=1 */
      PE.DR.WORD &= ~0x0080;	/* EN(PE7)=0 */
    } else if (mode == MTR_MODE_CW_BREAK) {
      PE.DR.WORD &= ~0x0020;	/* DIR(PE5)=0 */
      PE.DR.WORD &= ~0x0080;	/* EN(PE7)=0 */
    } else {			/* MTR_MODE_FREE */
      PE.DR.WORD |= 0x0080;	/* EN(PE7)=1 */
    }

  } else if (id == 1) {
    /* DIR(PE9), EN(PE11) */
    if (mode == MTR_MODE_CCW_BREAK) {
      PE.DR.WORD |= 0x0200;	/* DIR(PE9)=1 */
      PE.DR.WORD &= ~0x0800;	/* EN(PE11)=1 */
    } else if (mode == MTR_MODE_CW_BREAK) {
      PE.DR.WORD &= ~0x0200;	/* DIR(PE9)=0 */
      PE.DR.WORD &= ~0x0800;	/* EN(PE11)=1 */
    } else {			/* MTR_MODE_FREE */
      PE.DR.WORD |= 0x0800;	/* EN(PE11)=1 */
    }
  }
}


// 直接制御用
void setDirectMotorMode(unsigned char id, int mode)
{
  set_mode(id, mode);
}


void setDirectMotorPwm(unsigned char id, int duty)
{
  set_pwm(id, duty);
}


/*!
  \brief モータデバイスの初期化

  モータ制御に用いる出力端子を初期化する。
*/
void initMotor(void)
{
  int i;

  PFC.PEIOR.WORD |= 0x0500;	/* use TIOC3A, TIOC3C port */
  MTU3.TCR.BYTE = 0x20 | 0x01;	/* cycle=TGRA, duty=TGRB, cycle/4 */
  MTU3.TMDR.BYTE = 0xc2;	/* pwm mode 1, buffer disable */
  MTU3.TIOR.BYTE.H = 0x12;	/* CMA=0, CMB=1 */
  MTU3.TIOR.BYTE.L = 0x12;	/* CMC=0, CMD=1 */
  MTU3.TGRA = MTU3.TGRC = 255;	/* cycle */

  for (i = 0; i <= 1; ++i) {
    set_pwm(i, 0);
    set_mode(i, MTR_MODE_FREE);
  }

  PFC.PECR1.WORD |= 0x0011;	/* use TGRA, TGRC port */

  MTU.TSTR.BYTE |= 0xc0;	/* pwm active */

  PFC.PECR1.WORD &= ~0x0044;	/* TIOC3B, TIOC3D */
  PFC.PECR2.WORD &= ~0x4400;	/* TIOC1B, TIOC2B */
  PFC.PEIOR.WORD |= 0x0aa0;
}
#endif


/*!
  \brief モータ制御パラメータの初期化

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

  \param id [i] モータデバイスID
  \param mtr [o] モータ制御用の構造体
*/
void initMotorInfo(const unsigned char id, motorInfo_t *mtr)
{
  mtr->id = id;
  if (id == 0) {
    mtr->gain_p = MTR_GAIN_P;
    mtr->gain_i = MTR_GAIN_I;
  } else if (id == 1) {
    mtr->gain_p = -MTR_GAIN_P;
    mtr->gain_i = -MTR_GAIN_I;
  }
  mtr->i_value = 0;

  mtr->r_per_v = (int)((1<<MTR_I_BIT_WIDTH) * MTR_OHM / MTR_POWER);
  mtr->ke_per_v = (int)(1.0 / MTR_RPM_PER_V
			* 60 * 1000 / (SMP_USEC / 1000)
			* (1 << MTR_I_BIT_WIDTH) / (MTR_POWER * ENC_PULSE));
}


/*!
  \brief 回転の目標値を実現する PWM比 を計算する

  モータ制御用の構造体に基づき、エンコーダの目標回転数を実現する。

  - \f$ V_0 \f$ : モータに与える電圧 \f$ [V] \f$
  - \f$ R \f$ : 巻き線抵抗 \f$ [\Omega] \f$
  - \f$ I^{ref} \f$ : 目標電流値 \f$ [A] \f$
  - \f$ K_e \f$ : 逆起電力定数 \f$ [V/rpm] \f$
  - \f$ N \f$ : モータの回転数 \f$ [cnt] \f$
  - \f$ N^{ref} \f$ : モータの目標回転数 \f$ [cnt] \f$
  - \f$ K_P \f$ : P制御のゲイン \f$ [A/cnt] \f$
  - \f$ diff \f$ : モータの目標回転数との差分 \f$ [cnt] \f$
  - \f$ diff^{sum} \f$ : I制御における差分の積分値 \f$ [cnt] \f$
  - \f$ PWM \f$ : 出力するPWM比 \f$ [1] \f$
  - \f$ T^{smp} \f$ : 制御周期 \f$ [msec] \f$

  目標回転数を実現する PWM比の計算式
  \f[ PWM = \frac{1}{V_0} { RI^{ref}(t) + K_e N(t) \times (T^{smp} \times 60000)} \f]

  上記の式を整数型の計算によって実装し、符号が回転方向、比率の絶対値([0, 255]) が出力すべき PWM比を示す値を返す。

  \param ref_cnt [i] エンコーダにおける目標の回転数
  \param cnt_diff [i] エンコーダの回転数
  \param mtr [i/o] モータ制御用の構造体

  \retval pwm 符号が CW(+)、CCW(-) を示し、範囲は[-255, +255] となる。
*/
static int calcGainOutput(const int ref_cnt, const int cnt_diff,
			  motorInfo_t *mtr)
{
  int diff;
  int output;

  diff = ref_cnt - cnt_diff;
  mtr->i_value += diff;
  if (mtr->i_value > I_VALUE_MAX) {
    mtr->i_value = I_VALUE_MAX;
  } else if (mtr->i_value < -I_VALUE_MAX) {
    mtr->i_value = -I_VALUE_MAX;
  }
  output = (mtr->gain_p * diff) + (mtr->gain_i * mtr->i_value);
  output <<= (MTR_I_BIT_WIDTH - 10);

  return output;
}


// モータに対して、MTR_I_BIT_SHIF シフトされた電流[A] を流すように設定する
static int calcInvertedCtrlPwm(const int output, const int cnt_diff,
			       motorInfo_t *mtr)
{
  int sign;
  int pwm;

  pwm = (mtr->r_per_v * output) + (mtr->ke_per_v * cnt_diff);
  sign = (pwm < 0) ? -1 : +1;
  pwm = (sign * pwm) >> ((MTR_I_BIT_WIDTH << 1) - 8);
  if (pwm > 0xff) {
    pwm = 0xff;
  }
  return (sign * pwm);
}


/*!
  \brief モータをサーボフリー状態にする

  この関数は、外部レイヤーからモータの制御モードを直接サーボフリーにするために存在している。

  \param mtr [o] モータ制御用の構造体
*/
void setMotorFree(motorInfo_t *mtr) {
  set_mode(mtr->id, MTR_MODE_FREE);
}


/*!
  \brief モータの目標回転速度を設定する

  モータの目標回転速度を、制御周期あたりのエンコーダの回転数にて指定する。結果として、指定された目標回転速度を実現する PWM比と回転方向をモータデバイスに対して設定する。

  \param ref_cnt [i] エンコーダにおける目標回転数
  \param cnt_diff [i] エンコーダの回転数
  \param mtr [i/o] モータ制御用の構造体

  \retval 設定した PWM比 [-255, 255]
*/
int setMotorRevolution(const int ref_cnt, const int cnt_diff,
		       motorInfo_t *mtr) {
  int ref_pwm;
  int mode;
  int pwm;
  int output;

  output = calcGainOutput(ref_cnt, cnt_diff, mtr);
  ref_pwm = calcInvertedCtrlPwm(output, cnt_diff, mtr);
  mode = (ref_pwm >= 0) ? MTR_MODE_CW_BREAK : MTR_MODE_CCW_BREAK;
  pwm = (ref_pwm >= 0) ? ref_pwm : -ref_pwm;

  set_pwm(mtr->id, pwm);
  set_mode(mtr->id, mode);

  return ref_pwm;
}
