//
// 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();

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;
};

#endif
