#include <stdio.h>
/************************************/
//位置式PID
/*************************************/

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

void PID_Init(void)
{
    printf("PID_Value_Init_Begin \n");
    
    PID.SetSpeed = 0;
    PID.ActualSpeed = 0;
    PID.Err = 0;        
    PID.Err_last = 0;   
    PID.Voltage = 0;    
    PID.Intergral = 0; 
    PID.KP = 0.1;
    PID.KI = 0.15;
    PID.KD = 0.1;   
    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;
    PID.Intergral += PID.Err;
    PID.Voltage = PID.KP*PID.Err + PID.KI*PID.Intergral + PID.KD*(PID.Err-PID.Err_last);
    PID.Err_last = PID.Err;
    PID.ActualSpeed = PID.Voltage;
    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;
}