//
// Driver.cpp
//

#include "Driver.h"
#include "Vector.h"

//=============================================================================
Driver::Driver(LightSensor* pLightSensor, GyroSensor* pGyroSensor, Motor* pMotorL, Motor* pMotorR, Nxt* pNxt, Lcd* pLcd, TailControl* pTailControl){
	m_pPIDCtrl = new PIDCtrl(pLightSensor, 590, 0.7, 0.5, 13.0);
	m_pPositionControl = new  PositionControl(pMotorL, pMotorR, pLcd);
	m_pStandControl = new StandControl(pGyroSensor, pMotorL, pMotorR, pNxt, m_pPositionControl);
	m_pTailControl = pTailControl;
	m_speed = 70;
}

Driver::~Driver(){
	delete m_pPIDCtrl;
	delete m_pStandControl;
}

void Driver::run(){
	m_pTailControl->tail_control(TAIL_ANGLE_DRIVE);
	S16 PIDCalcValue = m_pPIDCtrl->calcVal();
	VectorT<S16> command = VectorT<S16>(m_speed, PIDCalcValue);
	m_pStandControl->standRun(command);
}

