#include <stdio.h>
/************************************/
//增量式PID
/*************************************/

//初始化变量
struct _PID{
    float SetSpeed;       //设定值
    float ActualSpeed;    //检测值
    float Err;            //当前误差
    float Err_last;       //上一次误差
    float Err_llast;      //上上次的误差
    float KP,KI,KD;       //PID参数
}PID;

void PID_Init(void)
{
    printf("PID_Value_Init_Begin \n");
    
    PID.SetSpeed = 0.0;
    PID.ActualSpeed = 0.0;
    PID.Err = 0.0;        
    PID.Err_last = 0.0;   
    PID.Err_llast = 0.0; 
    PID.KP = 0.2;
    PID.KI = 0.015;
    PID.KD = 0.2;   
    printf("PID_Init_End \n");
}

void delay(int count)
{
    int i;
    for(i = 1;i <= count;i++);
}

float PID_Realize(float Speed)
{
    PID.SetSpeed = Speed;
    PID.Err = PID.SetSpeed - PID.ActualSpeed;
    float Increment_Speed = PID.KP * (PID.Err - PID.Err_last) + PID.KI * PID.Err + PID.KD * (PID.Err - 2 * PID.Err_last + PID.Err_llast);
    PID.ActualSpeed += Increment_Speed;
    PID.Err_llast = PID.Err_last;
    PID.Err_last = PID.Err;
    return PID.ActualSpeed;
}

int main() 
{
    int count = 0;
    
	printf("Please begin \n");
	PID_Init();
	
	while(count < 1000)
	{
	    float Speed = PID_Realize(1000.0);
	    
	    printf("Value is %f\n",Speed);
	    count++;
	    delay(3000);
	}
	
	return 0;
}