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

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

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;         //积分值
}pos_pid,inc_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,float set_value)
{
    pid->set_value = set_value;
    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_pre-pid->error_pre_pre);

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

    pid->actual_value += pid->P + pid->I +pid->D;
    return pid->actual_value;

}

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

*/
float incremental_pid(PID *pid, float set_value)
{
    float P, I, D,increment;
    pid->set_value = set_value;
    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 pid->actual_value;
}



int main()
{

    pid_init(0.2, 0.015, 0.2, &pos_pid);
    pid_init(0.2,0.015,0.2,&inc_pid);

    char *p = "位置式PID\n上一次\tP\tI\tD\t本次\n";
    printf(p);

    for (int i = 1; i <=100; i++)
    {
        
        /*特别说明：这里只是单纯的算法实现，并没有实际的设备反馈回来的实际值
        是能说是模拟，具体还的看设备的反馈值
        实际情况下每执行一次position_pid（） actual_value 应该都要变化 
        */
        pos_pid.actual_value = 35;         
        printf("%d\t%f\t%f\t%f\t%f\t",i, pos_pid.actual_value,pos_pid.P, pos_pid.P, pos_pid.D);
        position_pid(&pos_pid, 50);
        printf("%f\n", pos_pid.actual_value);
       
    }

    
    system("pause");

    return 0;
}