软件设计（舵机电机的PID控制）

首先定义 pid结构体：

typedef struct PID
{
	long SumError;	//误差累计
	int32 LastError;	//Error[-1]
	int32 PrevError;	//Error[-2]
	int32 LastSpeed;	//Speed[-1]
    float KP;                              //比例常数 Proportional Const
    float KI;                                //积分常数 Integral Const
    float KD;                              //微分常数 Derivative Const
} PID;

‘
在舵机，电机，编码器中，为了在面对不同的赛道的变化时做出相应的精确变化，通常采用PID控制，可以使它们在面对不同的赛道类型时达到闭环控制的效果。其中，主要用到2种PID。

代码如下：

float PlacePID_Control(PID *sprt, int32 NowPiont, int32 SetPoint)
{       
        int iError;	//当前误差      
	float	Kp=0,Actual=0;		//动态P
	
	iError = SetPoint - NowPiont;	//计算当前误差
	Kp = 1.0 * (iError*iError) / S3010[Set][T] + S3010[Set][P];	//P值与差值成二次函数关系
	
	Actual = Kp * iError + S3010[Set][D] * (iError - sprt->LastError);//只用PD
	
	sprt->LastError = iError;		//更新上次误差
		
	return  S3010_MID - Actual;

}

代码如下：

int32 PID_Realize(PID *sptr, int32 ActualSpeed, int32 SetSpeed)
{
  	int32 iError,Increase;	    //当前误差     最后得出的实际增量
  	
	iError = SetSpeed - ActualSpeed;//计算当前误差

	Increase = sptr->KP * (iError - sptr->LastError)
			 + sptr->KI * iError
			 + sptr->KD * (iError - 2 * sptr->LastError + sptr->PrevError);
	sptr->PrevError = sptr->LastError;	//更新前次误差
	sptr->LastError = iError;		  	//更新上次误差
	
	return Increase;
}