//
// LineTraceDriver.cpp
//

#include "LineTraceDriver.h"

//=============================================================================
// Simple line tracing
void LineTraceDriver::update(void)
{
// 2010/06/18 コメントアウト 沢村＠UIS
/*
	if (mrLight.get() <= (mBlack + mWhite)/2)
	{
		mDriverCmd = VectorT<S16>(50, -50); // Turn right
//		mDriverCmd = VectorT<S16>(50, 50); // Turn right
	}
	else
	{
		mDriverCmd = VectorT<S16>(50,50); // Turn left
//		mDriverCmd = VectorT<S16>(50,-50); // Turn left
	}
*/
//	mLightValue = mrLight.get();
	
	
	S16 sCurve = 0;
	if(m_bCurveSet == true){
		sCurve = m_pPIDCtrl->calcVal(mrLight.get());
	}else{
		sCurve = 0;
	}
			
	mDriverCmd = VectorT<S16>(m_speed, sCurve);
	
	//mDriverCmd = VectorT<S16>(m_speed, m_pPIDCtrl->calcVal(mLightValue));

}
