#include "pid.h"

 
//用于初始化pid参数的函数
void PID_Init(PID *pid, float p, float i, float d, float maxI, float maxOut, float minOut)
{
    pid->kp = p;
    pid->ki = i;
    pid->kd = d;
    pid->maxIntegral = maxI;
    pid->maxOutput = maxOut;
		pid->minOutput = minOut;
	
		pid->error = 0;
		pid->error1 = 0;
		pid->error2 = 0;
		pid->integral = 0;
		pid->output = 0;
}
 

//位置式PID
//pwm=Kp*e(k)+Ki*∑e(k)+Kd[e（k）-e(k-1)]
//setvalue : 设置值（期望值）
//actualvalue: 实际值
//由于全量输出，每次输出均与过去状态有关，计算时要对ek累加，计算量大

float PID_Location(PID *pid, float setvalue, float feedback, float dt)
{
 	//更新数据
    pid->error1 = pid->error; //将旧error存起来
    pid->error = setvalue - feedback; //计算新error
	
    //计算微分
    float dout = (pid->error - pid->error1)/dt * pid->kd;
    //计算比例
    float pout = pid->error * pid->kp;
    //计算积分
    pid->integral += pid->error*dt * pid->ki;
	
    //积分限幅
    if(pid->integral > pid->maxIntegral) 
			pid->integral = pid->maxIntegral;
    else if(pid->integral < -pid->maxIntegral) 
			pid->integral = -pid->maxIntegral;
		
    //计算输出
    pid->output = pout+dout + pid->integral;
	
    //输出限幅
    if(pid->output > pid->maxOutput) 
			pid->output = pid->maxOutput;
    if(pid->output < pid->minOutput) 
			pid->output = pid->minOutput;
		
		return pid->output;
		
}

//增量式PID
//pidout+=Kp[e（k）-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
//setvalue : 设置值（期望值）
//feedback: 实际值
float PID_Increment(PID *pid, float setvalue, float feedback)
{
	 	//更新数据
    pid->error2 = pid->error1; //将旧error1存起来
		pid->error1 = pid->error; //将旧error存起来
	
    pid->error = setvalue - feedback; //计算新error
	
		//计算微分
    float dout = (pid->error - 2*pid->error1 + pid->error2) * pid->kd;
    //计算比例
    float pout = (pid->error - pid->error1 )* pid->kp;
    //计算积分
    pid->integral = pid->error * pid->ki;

		//计算输出
    pid->output += (pout+dout + pid->integral);
	
    //输出限幅
//    if(pid->output > pid->maxOutput) 
//			pid->output = pid->maxOutput;
//    if(pid->output < pid->minOutput) 
//			pid->output = pid->minOutput;

		return pid->output;

}