//
// PIDCtrl.h
//

#ifndef PIDCTRL_H_
#define PIDCTRL_H_

#include "LightSensor.h"
using namespace ecrobot;

/**
 * @brief PID制御クラス
 * PID操作量を算出します。
 *
 * I操作量の算出は省略してます。
 *
 * @author teshima
 */
class PIDCtrl
{
public:

	/**
	 * Default light sensor value for white color
	 */
	static const S16 DEFAULT_BLACK = 700;

	/**
	 * Default light sensor value for black color
	 */
	static const S16 DEFAULT_WHITE = 520;

	/**
	 * コンストラクタ
	 * @param tgtVal 目標値
	 * @param kp Pゲイン値
	 * @param ki Iゲイン値
	 * @param kd Dゲイン値
	 */
	PIDCtrl(LightSensor* pLightSensor, float tgtVal, float kp, float ki, float kd);
 
	~PIDCtrl();

	/**
	 * PID操作量算出
	 * @retval S16 PID操作量
	 */
	S16 calcVal();
	
	/**
	 * 光センサのオフセット値を設定する
	 * @param black Light sensor value for black
	 * @param white Light sensor value for white
	 */
	void setColorRef(S16 black, S16 white);

private:
	const float TGT_VAL;	//目標値
	const float KP;			//Pゲイン値
	const float KI;			//Iゲイン値
	const float KD;			//Dゲイン値
	float m_prevDiff;		//前回偏差
	LightSensor* m_pMyLightSensor;	//光センサ
	S16 m_Black;
	S16 m_White;
	
	/**
	 * P操作量算出
	 * @param diff 現在値
	 * @retval float P操作量
	 */
	float calcPVal(float diff);

	/**
	 * I操作量算出
	 * @brief 実装なし
	 */
	float calcIVal(float diff);
	
	/**
	 * D操作量算出
	 * @param diff 現在値
	 * @retval float D操作量
	 */
	float calcDVal(float diff);
};

#endif /*PIDCTRL_H_*/
