//	单环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_Biasstatic 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_Biasstatic 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; /* 输出结果 */
}