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

  \author Satofumi KAMIMURA

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

#include "motor_control.h"
#include "pwm_control.h"


void motor_initialize(int id, motor_t *motor)
{
  enum {
    MotorGain_P = 30,
    MotorGain_I = 4,
  };

  motor->reset = 0;
  motor->id = id;
  motor->gain_p = MotorGain_P;
  motor->gain_i = MotorGain_I;
  motor->i_total = 0;

  pwm_initialize(id);
}


void motor_reset(motor_t *motor)
{
  (void)motor;
  // !!!
}


void motor_set(const motor_t *motor, MotorMode mode, unsigned char duty)
{
  char id = motor->id;

  switch (mode) {
  case MotorMode_CW:
    pwm_set(id, PwmMode_CW, duty);
    break;

  case MotorMode_CCW:
    pwm_set(id, PwmMode_CCW, duty);
    break;

  case MotorMode_Free:
    pwm_set(id, PwmMode_Free, duty);
    break;
  }
}


void motor_setVelocity(motor_t *motor,
                       int count_per_msec, int encoder_count)
{
  // PI 制御で目標電流値を決定
  long diff = encoder_count - count_per_msec;
  long current_ref = motor->gain_p * diff + motor->gain_i * motor->i_total;
  motor->i_total += diff;

  // I 値を上限で抑える
  enum { I_TotalMax = 4096 };
  if (motor->i_total > +I_TotalMax) {
    motor->i_total = +I_TotalMax;
  } else if (motor->i_total < -I_TotalMax) {
    motor->i_total = -I_TotalMax;
  }

  // 目標電流値を duty 比に変換
  // !!! とりあえず、線形に増加するとして duty に変換してしまう
  long duty_8bit = current_ref >> 5;
  if (duty_8bit >= 0xff) {
    duty_8bit = 0xff;
  } else if (duty_8bit <= -0xff) {
    duty_8bit = -0xff;
  }

  unsigned char duty;
  PwmMode mode;
  if (duty_8bit >= 0) {
    duty = duty_8bit & 0xff;
    mode = PwmMode_CW;
  } else {
    duty = -duty_8bit & 0xff;
    mode = PwmMode_CCW;
  }

  // PWM 出力を更新
  char id = motor->id;
  pwm_set(id, mode, duty);
}
