//
// PIDCtrl.cpp
//
#include "PIDCtrl.h"
#include "LightSensor.h"

//=============================================================================
// Constructor
PIDCtrl::PIDCtrl(LightSensor* pLightSensor, float tgtVal, float kp, float ki, float kd)
	: TGT_VAL(tgtVal), KP(kp) , KI(ki) , KD(kd) ,m_prevDiff(0) 
{
	m_pMyLightSensor = pLightSensor;
	m_Black = DEFAULT_BLACK;
	m_White = DEFAULT_WHITE;
}

PIDCtrl::~PIDCtrl() {
	// pMyLightSensor の削除は行わない
}

void PIDCtrl::setColorRef(S16 black, S16 white)
{
	m_Black = black;
	m_White = white;
}

S16 PIDCtrl::calcVal() {
	S16 lightValue = m_pMyLightSensor->get();
	float diff = TGT_VAL - lightValue;		//偏差
	return (S16)(calcPVal(diff) + calcIVal(diff) + calcDVal(diff));
}

float PIDCtrl::calcPVal(float diff) {
	return KP * diff;
}

float PIDCtrl::calcIVal(float diff) {
	return 0;
}
	
float PIDCtrl::calcDVal(float diff) {
	float dValue = KD * (diff - m_prevDiff);
	m_prevDiff = diff;
	return dValue;
}
