/*!
  \file
  \brief 筐体制御

  \author Satofumi KAMIMURA

  $Id: body_control.c 1209 2009-08-05 13:11:59Z satofumi $
*/

#include "body_control.h"
#include "robot_macro.h"


void body_initialize(body_t *body)
{
  body->reset = 0;
  body->rotate_coeffecient = BODY_T_ROTATE_COEFFECIENT;
}


void body_reset(body_t *body)
{
  // 実装内容なし
  (void)body;
}


void body_calculateWheelVelocity(long wheel_velocity[],
                                 long mm_per_sec, long dir16,
                                 const body_t *body)
{
  // 回転時の移動距離 = トレッド * 回転速度
  long difference =
    (dir16 * body->rotate_coeffecient) >> (BODY_T_TREAD_R_SHIFT + 1 + 16);

  // 右の車輪速 = 並進速度 + (回転速度 * トレッド / 2)
  wheel_velocity[RightId] = mm_per_sec + difference;

  // 左の車輪速 = 並進速度 - (回転速度 * トレッド / 2)
  wheel_velocity[LeftId] = mm_per_sec - difference;
}
