#ifndef QRK_POSITION_T_H
#define QRK_POSITION_T_H

/*!
  \file
  \brief 位置推定

  \author Satofumi KAMIMURA

  $Id: position_t.h 1381 2009-10-08 19:45:31Z satofumi $
*/

#include "body_t.h"
#include "wheel_t.h"
#include "encoder_t.h"


#ifndef POSITION_T_DIR16_COUNT_MAX
#define POSITION_T_DIR16_COUNT_MAX \
  ((long)((BODY_T_TREAD >> BODY_T_TREAD_R_SHIFT) / \
          WHEEL_T_RADIUS * WHEEL_T_GEAR_RATIO * ENCODER_T_COUNT))
#endif

#ifndef POSITION_T_COUNT_PER_MM
#define POSITION_T_COUNT_PER_MM \
  ((long)((ENCODER_T_COUNT * WHEEL_T_GEAR_RATIO) / \
          (2.0 * M_PI * WHEEL_T_RADIUS)))
#endif


//! 位置
typedef struct
{
  char reset;

  // 位置の変数
  long mm[2];
  unsigned short dir16;

  // 計算用の変数
  short dir16_diff;             //!< 前回制御からの角度変位
  long dir16_count;             //!< エンコーダ値での向き
  long xy_count_high[2];        //!< X, Y 毎のエンコーダ値での位置(上位 16 bit)
  long xy_count_low[2];         //!< X, Y 毎のエンコーダ値での位置(下位 16 bit)
} position_t;

#endif /* !QRK_POSITION_T_H */
