#include "vtor_pid.h"

#ifdef __VTOR_PID__

void VtorPid_Clear(VtorPid* pid)
{
	pid->totalErr = 0;
	pid->outi = 0;
}

void VtorPid_Calc(VtorPid* pid, float curTime)
{
	pid->intervalTime = curTime - pid->lastTime;
	pid->err = pid->exp - pid->cur;
	pid->err = pid->err * pid->kerr;
	pid->totalErr += pid->err * pid->intervalTime;
	
	pid->outp = pid->kp * pid->err * pid->intervalTime;
	pid->outi = pid->ki * pid->totalErr;
	pid->outi = pid->outi < pid->outiMax ? pid->outi : pid->outiMax;
	pid->outi = pid->outi > pid->outiMin ? pid->outi : pid->outiMin;
	pid->outd = pid->kd * (pid->err - pid->lastErr) / pid->intervalTime;

	pid->out = pid->outp + pid->outi + pid->outd;
	pid->out = pid->out < pid->outMax ? pid->out : pid->outMax;
	pid->out = pid->out > pid->outMin ? pid->out : pid->outMin;

	pid->lastTime = curTime;
	pid->lastErr = pid->err;
}

void VtorPid_SetExp(VtorPid* pid, float exp)
{
	pid->exp = exp;
}

void VtorPid_SetCur(VtorPid* pid, float cur)
{
	pid->cur = cur;
}

#endif // __VTOR_PID__