#ifndef WHEEL_CTRL_H
#define WHEEL_CTRL_H

/*!
  \file
  \brief 移動速度を実現するための車輪制御

  \author Satofumi KAMIMURA

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

#include "motorCtrl.h"
#include "encCtrl.h"
#include "math_util.h"


#ifndef WHL_DIAMETER_RIGHT
#define WHL_DIAMETER_RIGHT 84.5
#endif
#ifndef WHL_DIAMETER_LEFT
#define WHL_DIAMETER_LEFT 84.5
#endif

#ifndef WHL_GEAR_RATIO
#define WHL_GEAR_RATIO (3249.0/169.0 * 45.0/16.0)
#endif

#define WHL_CNT_PER_MM_RIGHT \
((WHL_GEAR_RATIO * ENC_PULSE) / (WHL_DIAMETER_RIGHT * M_PI))
#define WHL_CNT_PER_MM_LEFT \
((WHL_GEAR_RATIO * ENC_PULSE) / (WHL_DIAMETER_LEFT * M_PI))


enum {
  WHL_RIGHT = 0, WHL_LEFT = 1,
  WHL_VEL2CNT_16SHIFT_RIGHT = (int)(((1 << 16) * WHL_CNT_PER_MM_RIGHT / 1000)),
  WHL_VEL2CNT_16SHIFT_LEFT = (int)(((1 << 16) * WHL_CNT_PER_MM_LEFT / 1000)),

  WHL_CNT2VEL_16SHIFT_RIGHT = (int)((1 << 16) * 1000 / WHL_CNT_PER_MM_RIGHT),
  WHL_CNT2VEL_16SHIFT_LEFT = (int)((1 << 16) * 1000 / WHL_CNT_PER_MM_LEFT),

  WHL_CNT_PER_M_RIGHT = (int)(1000.0 * WHL_CNT_PER_MM_RIGHT),
  WHL_CNT_PER_M_LEFT = (int)(1000.0 * WHL_CNT_PER_MM_LEFT),
};


/*!
  \brief 車輪の情報管理
*/
typedef struct {
  motorInfo_t *mtr;
  encInfo_t *enc;
  int next_add;
  int vel2cnt_const;
  int cnt2vel_const;
  int cnt_per_m;
} wheelInfo_t;


extern void initWheelInfo(wheelInfo_t *whl,
			  motorInfo_t *mtr, encInfo_t *enc,
			  const unsigned char id);
extern int change_wheelVel2Cnt(wheelInfo_t *whl, int mm_sec_vel);
extern int change_wheelCnt2Vel(wheelInfo_t *whl, int cnt_msec_vel);
extern void setWheelFree(wheelInfo_t *whl);
extern void setWheelMoveVelocity(int mm_sec_vel, wheelInfo_t *whl);

#endif /* !WHEEL_CTRL_H */
