//
// 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);
	
	// 
	// 傾く
	// 
	void lean(VectorT<S16> cmd);
	
	//
	// 傾き走行
	//
	void leanRun(VectorT<S16> command);
	
	//
	// 傾き走行
	//
	void leanRunToGoal(VectorT<S16> command);

	//
	// 向きを変更
	//
	void changeDirection(S8 pwm);

	void calGyroOffset(void);

private:
	
	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
