/*!
  \file
  \brief 車輪制御

  \author Satofumi KAMIMURA

  $Id: wheel_control.c 1217 2009-08-10 22:38:36Z satofumi $
*/

#include "wheel_control.h"

#include <stdio.h>


void wheel_initialize(wheel_t *wheel)
{
  wheel->reset = 0;
  wheel->coefficient = WheelCoefficient;
}


void wheel_reset(wheel_t *wheel)
{
  (void)wheel;

  // !!! 微小な速度指定を実現するための変数、をクリアする
}


long wheel_calculateMotorVelocity(int mm_per_sec, const wheel_t *wheel)
{
  // 車輪の回転速度を最大速で制限する
  if (mm_per_sec > WHEEL_MAX_MM) {
    mm_per_sec = WHEEL_MAX_MM;
  } else if (mm_per_sec < -WHEEL_MAX_MM) {
    mm_per_sec = -WHEEL_MAX_MM;
  }

  // 1 [msec] あたりのモータ回転数 = 係数 * mm_per_sec
  // 係数 = (GEAR_RATIO << 16) / (2.0 * M_PI * WHEEL_RADIUS) / 1000.0;

  // !!! 微少な速度指示も制御できるようにする
  // !!! 計算誤差を加算していき、次の計算時に評価する

  return (mm_per_sec * wheel->coefficient) >> 10;
}
