//
// PositionControl.h
//

#ifndef POSITIONCONTROL_H_
#define POSITIONCONTROL_H_

#include "Motor.h"
#include "Vector.h"
using namespace ecrobot;

//for Debug
#include "Lcd.h"

class PositionControl
{
public:

	//
	// Constructor
	//
	PositionControl(Motor* pMotorL, Motor* pMotorR, Lcd* pLcd);
	
	~PositionControl();
	
	//
	// 現在位置更新
	//
	void updateCurrentPosition();
	
	//
	// 現在位置取得
	//
	VectorT<S32> getCurrentPosition();

	//
	//角度取得
	//
	
	float get_dW(){return m_dW;}
	
	float calcDirectTarget(float dTPosX, float dTPosY, bool& bRightTurn);

	bool isTurnExec(float fTargetAngle);
	
	void reset();
	void setPosition(float dX, float dY, float dW);

private:
	
	//
	// 車輪半径(m)
	//
	float m_dWRadius;
	
	//
	// 車輪間隔(m)
	//
	float m_dWInterval;
	
	//
	// X座標
	//
	float m_dX;
	
	//
	// Y座標
	//
	float m_dY;
	
	float m_dW;
	
	float m_dPI;
	
	//
	// 計測間隔
	//
	long m_lSampling;
	
	long m_lLastLRotAngle;
	
	long m_lLastRRotAngle;
	
	Motor* m_pMotorL;
	
	Motor* m_pMotorR;

	//for Debug
	Lcd* m_pLcd;

#ifdef ENABLE_BT_LOG
	S8 m_iLogCnt;
	S32 m_dsx0;
	S32 m_dsy0;
#endif

};

#endif
