//
// 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:
	Motor* m_pMotorL;
	Motor* m_pMotorR;
	
	float m_dX;
	float m_dY;
	float m_dW;
	float m_dPI;
	float m_dWRadius; 		// ŽÔ—Ö”¼Œa(m)
	float m_dWInterval;		// ŽÔ—ÖŠÔŠu(m)
	long m_lSampling;
	long m_lLastLRotAngle;
	long m_lLastRRotAngle;
	
	//for Debug
	Lcd* m_pLcd;
};

#endif
