#ifndef MODE_CTRL_H
#define MODE_CTRL_H

/*!
  \file
  \brief 制御モード管理

  \author Satofumi KAMIMURA

  $Id: modeCtrl.h 360 2008-11-19 23:51:52Z satofumi $
*/

#include "velocityCtrl.h"


#ifndef NULL
#define NULL 0x0
#endif


/*!
  \brief モードの情報管理
*/
typedef struct {
  int ctrl;
  int straight_ctrl;
  int rotate_ctrl;
  int pre_straight_ctrl;
  int pre_rotate_ctrl;
  int follow_r;
} modeInfo_t;


enum {
  // 制御モード
  MODE_NO_CTRL = 0,		// 制御なし
  MODE_SERVO_CTRL = 1,		// サーボ制御
  MODE_SERVO_FREE,		// サーボフリー
  MODE_DIRECT_CTRL,		// モータレベルでの直接制御
  MODE_DIRECT_WHEEL_CTRL,	// 車輪レベルでの直接制御

  // 並進
  MODE_HOLD_VELOCITY_FIRST = 0x100,
  MODE_HOLD_VELOCITY,
  MODE_HOLD_POSITION_FIRST,
  MODE_HOLD_POSITION,
  MODE_STOP_FIRST,
  MODE_STOP,

  // 回転
  MODE_HOLD_ROTATE_VELOCITY_FIRST = 0x200,
  MODE_HOLD_ROTATE_VELOCITY,
  MODE_HOLD_ANGLE_FIRST,
  MODE_HOLD_ANGLE,
  MODE_ROTATE_ANGLE_FIRST,
  MODE_ROTATE_ANGLE,
  MODE_FOLLOW_LINE_FIRST,
  MODE_FOLLOW_LINE,
  MODE_FOLLOW_CIRCLE_FIRST,
  MODE_FOLLOW_CIRCLE,
  MODE_ROTATE_STOP_FIRST,
  MODE_ROTATE_STOP,
};


extern void initModeInfo(modeInfo_t *mode);
extern int calcStraightRefVel(velocityCtrlInfo_t *velCtrl, modeInfo_t *mode,
			      velocityInfo_t *velInfo,
			      coordinateInfo_t *run_crd);
extern int calcRotateRefVel(velocityCtrlInfo_t *velCtrl, modeInfo_t *mode,
			    bodyPosition_t *bodyPos, velocityInfo_t *velInfo,
			    coordinateInfo_t *run_crd);

#endif /* !MODE_CTRL_H */
