位置式PID

//	单环PID参数
float Position_KP = 0.0180, Position_KI = 0.0013, Position_KD = 0.11425; 	/* 位置式PID系数 */

/**************************************************************************
 * @brief  位置式PID控制器
 * @param  实际位置reality，目标位置target
 * @retval 电机PWM
 * @attention
 * @describe 根据位置式离散PID公式
				pwm=Kp*e(k)+Ki*∑e(k)+Kd[e（k）-e(k-1)]
				e(k)代表本次偏差
				e(k-1)代表上一次的偏差
				∑e(k)代表e(k)以及之前的偏差的累积和;其中k为1,2,...,k;
 * @matters pwm代表输出
**************************************************************************/
long Position_PID_Left(long reality, long target)
{
	static long Bias, Last_Bias;   // 偏差Bias，上次偏差Last_Bias
	static long Integral_bias = 0; // 偏差的积分
	static long PWM_OUT;
 
	Bias = target - reality; /* 计算偏差 */
	Integral_bias += Bias;	 /* 偏差累积 */
 
	if (Integral_bias > 5000)
		Integral_bias = 5000; /* 积分限幅 */
	if (Integral_bias < -5000)
		Integral_bias = -5000;
 
	PWM_OUT = (Position_KP * Bias)					/* 比例环节 */
			  + (Position_KI * Integral_bias)		/* 积分环节 */
			  + (Position_KD * (Bias - Last_Bias)); /* 微分环节 */
 
	Last_Bias = Bias; /* 保存上次偏差 */
 
	return PWM_OUT; /* 输出结果 */
}
//右轮的位置式PID --- 写两个函数，是为了编码器数据不干扰
long Position_PID_Right(long reality, long target)
{
	static long Bias, Last_Bias;   // 偏差Bias，上次偏差Last_Bias
	static long Integral_bias = 0; // 偏差的积分
	static long PWM_OUT;
 
	Bias = target - reality; /* 计算偏差 */
	Integral_bias += Bias;	 /* 偏差累积 */
 
	if (Integral_bias > 5000)
		Integral_bias = 5000; /* 积分限幅 */
	if (Integral_bias < -5000)
		Integral_bias = -5000;
 
	PWM_OUT = (Position_KP * Bias)					/* 比例环节 */
			  + (Position_KI * Integral_bias)		/* 积分环节 */
			  + (Position_KD * (Bias - Last_Bias)); /* 微分环节 */
 
	Last_Bias = Bias; /* 保存上次偏差 */
	return PWM_OUT; /* 输出结果 */
}