#include "PIController.h"
#include "FocConfig.h"
/**
 * @brief   pi计算
 * @param   target目标值  real实际值
 * @return  out输出值
 */
void PIControllerStep(PIController_t *pi)
{
		float integ,out;
		pi->err = pi->target - pi->real;
	  integ = pi->integral;
		integ += pi->ki * pi->err * FOC_PWM_PERIOD; 
		out = integ + pi->kp * pi->err;
		//防止积分饱和，限幅处理
		if(out > pi->maxValue)
		{
			out = pi->maxValue;
		}
		else if(out < pi->minValue )	
		{
			out = pi->minValue;
		}
		else
		{
			
		}
//		if(pi->integSat < integ)
//		{
//			integ =	pi->integSat; 
//		}
//		else if(-pi->integSat > integ)
//		{
//			integ =	-pi->integSat; 
//		}
		pi->integral = integ;
		pi->out = out;
 }

 /**
 * @brief   pi计算
 * @param   target目标值  real实际值
 * @return  
 */
 void SpeedLoopStep(PIController_t *pi)
 {
		float integ,out;
		pi->err = pi->target - pi->real;
	  integ = pi->integral;
		integ += pi->ki * pi->err; 
		out = integ + pi->kp * pi->err;
		//防止积分饱和，限幅处理

		if(pi->integSat < integ)
		{
			integ =	pi->integSat; 
		}
		else if(-pi->integSat > integ)
		{
			integ =	-pi->integSat; 
		}
		pi->integral = integ;
		pi->out = out + pi->lastout;
		if(pi->out > pi->maxValue)
		{
			pi->out = pi->maxValue;
		}
		else if(pi->out < pi->minValue )	
		{
			pi->out = pi->minValue;
		}
		else
		{
			
		}
		pi->lastout = pi->out;		
 }