//
// StandControl.cpp
//

#include "StandControl.h"
//=============================================================================
StandControl::StandControl(GyroSensor* pGyroSensor, Motor* pMotorL, Motor* pMotorR, Nxt* pNxt, PositionControl* pPositionControl)
	: m_pGyroSensor(pGyroSensor), 
	  m_pMotorL(pMotorL), 
	  m_pMotorR(pMotorR), 
	  m_pNxt(pNxt),
	  m_pPositionControl(pPositionControl)
{
	calGyroOffset();
}

StandControl::~StandControl(){
}

void StandControl::standRun(VectorT<S16> command){

	VectorT<S8> motorPwm = this->calcPWM(command);

	m_pMotorL->setPWM(motorPwm.mX);
	m_pMotorR->setPWM(motorPwm.mY);
	
	// 現在位置を更新する
	m_pPositionControl->updateCurrentPosition();
}

VectorT<S8> StandControl::calcPWM(VectorT<S16> cmd){

	S16 gyroData = m_pGyroSensor->get();
	S16 battMv = m_pNxt->getBattMv();
	VectorT<S32> motorCount;
	motorCount.mX = m_pMotorL->getCount();
	motorCount.mY = m_pMotorR->getCount();

	VectorT<S8> pwm;

	balance_control(
		static_cast<F32>(cmd.mX),
		static_cast<F32>(cmd.mY),
		static_cast<F32>(gyroData),
		static_cast<F32>(m_offset),
		static_cast<F32>(motorCount.mX),
		static_cast<F32>(motorCount.mY),
		static_cast<F32>(battMv),
		&pwm.mX,
		&pwm.mY);

	return pwm;
}

void StandControl::calGyroOffset(void)
{
	U32 calGyroOffset = 0;
	SINT numOfAvg = 0;
	while (numOfAvg < 100) // it takes approximately 4*100 = 400msec
	{
		calGyroOffset += m_pGyroSensor->get();
		numOfAvg++;
		U32 tic = systick_get_ms();
		while (systick_get_ms() < 4 + tic); // wait for 4msec
	}

	m_offset = static_cast<S16>(calGyroOffset/numOfAvg);
}
