//
// StandControl.h
//

#ifndef STANDCONTROL_H_
#define STANDCONTROL_H_


#include "GyroSensor.h"
#include "Motor.h"
#include "Nxt.h"
#include "PositionControl.h"
#include "Vector.h"
using namespace ecrobot;

extern "C"
{
	#include "balancer.h"
};


class StandControl
{
public:

	/**
	 * Constructor.
	 */
	StandControl(GyroSensor* pGyroSensor, Motor* pMotorL, Motor* pMotorR, Nxt* pNxt, PositionControl* pPositionControl);
	
	~StandControl();
	
	void standRun(VectorT<S16> command);

private:
	void calGyroOffset(void);
	VectorT<S8> calcPWM(VectorT<S16> cmd);
	void tail_control(signed int angle);

	GyroSensor* m_pGyroSensor;
	Motor* m_pMotorL;
	Motor* m_pMotorR;
	Nxt* m_pNxt;
	PositionControl* m_pPositionControl;
	S16 m_offset;
};

#endif
