// pid.cpp : 定义控制台应用程序的入口点。
//

#include "stdafx.h"
#include<Windows.h>
#include<stdio.h>

typedef struct PID_
{
    float actual_value;      //实际值 每次反馈回来的值
    float error;           //偏差值
    float Kd, Ki, Kp;         //比例、积分、微分常数
    float P, I, D;             //比例项 积分项 微分项
    float error_pre;            //E[k-1]  //上一次误差
    float error_pre_pre;        //E[k-2]  //上上次误差
    float set_value;              //设定值 你期望系统达到的值
    float integral;         //积分值
}PID;

/*pid初始化*/
void pid_init(double kp, double ki, double kd, PID *pid)
{
    pid->actual_value = 0;
    pid->error = 0;
    pid->error_pre= 0;
    pid->error_pre_pre = 0;
    pid->Kp = kp;
    pid->Ki = ki;
    pid->Kd = kd;
    pid->set_value = 0;
    pid->integral = 0;
    pid->P = 0;
    pid->I = 0;
    pid->D = 0;
}


/*位置式pid
@param pid  要计算的pid
@param set_value 想要设定的值
返回 增量
*/
float position_pid(PID *pid)
{
    pid->error = pid->set_value - pid->actual_value;
    pid->integral += pid->error;   /*误差累积*/

    //比例项
    pid->P = pid->Kp*pid->error;

    //积分项
    pid->I = pid->Ki*pid->integral;

    //微分项
    pid->D =pid->Kd*(pid->error-pid->error_pre);

    pid->error_pre_pre = pid->error_pre;
    pid->error_pre = pid->error;

    float uk = pid->P + pid->I +pid->D;
    pid->actual_value += uk;

    return uk;

}

/*增量式PID算法
@param pid  要计算的pid
@param set_value 想要设定的值 可以作为入参
返回给执行器的输出值

*/
float incremental_pid(PID *pid)
{
    float P, I, D,increment;
    pid->error = pid->set_value - pid->actual_value;

    P = pid->Kp*(pid->error-pid->error_pre);
    I = pid->Ki*pid->error;
    D = pid->Kd*(pid->error-2*pid->error_pre + pid->error_pre_pre);
    
    increment = P + I + D;   /*pid计算得到的增量*/
    pid->actual_value += increment;     /*实际反馈值加上增量*/

    pid->error_pre_pre = pid->error_pre;
    pid->error_pre = pid->error;

    return increment;
}
int main()
{
    PID pos_pid, inc_pid;
    pid_init(0.2, 0.015, 0.2, &pos_pid);
    pid_init(0.2,0.015,0.2,&inc_pid);

    //printf("位置式:\n");
    printf("增量式:\n");
    printf("ID\t增量\t\tP\tI\tD\n");


    //设定测试值为50
    pos_pid.set_value = 50;
    inc_pid.set_value = 50;

    //模拟传感器采集到的值  不是实际值 
    float test[] = {40,45,46,47,48,50,51,52,50,53,56,60,50};
    for (int i = 0; i <=sizeof(test)/sizeof(float); i++)
    {
        
        /*特别说明：这里只是单纯的算法实现，并没有实际的设备反馈回来的实际值
        是能说是模拟，具体还的看设备的反馈值
        实际情况下每执行一次position_pid（） actual_value 应该都要变化 
        */
        pos_pid.actual_value = test[i];      
        //得到pid增量
        //float uk = position_pid(&pos_pid);
        float uk = incremental_pid(&pos_pid);
        printf("%d\t%f\t%f\t%f\t%f\t\n",i,uk,pos_pid.P, pos_pid.I, pos_pid.D);
    }
    system("pause");

    return 0;
}