增量式PID

/**************************************************************************
 * @brief  增量式PID控制器
 * @param  实际值reality，目标值target
 * @retval 电机PWM
 * @attention
 * @describe 根据增量式离散PID公式 
				pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
				e(k)代表本次偏差 
				e(k-1)代表上一次的偏差  以此类推 
 * @matters pwm代表输出
**************************************************************************/
int Incremental_PID_Left(int reality,int target)
{ 	
	 static float Bias,Pwm,Last_bias=0,Prev_bias=0;
    
	 Bias=target-reality;                                   /* 计算偏差 */
    
	 Pwm += (Incremental_KP*(Bias-Last_bias))               /* 比例环节 */
           +(Incremental_KI*Bias)                           /* 积分环节 */
           +(Incremental_KD*(Bias - 2 * Last_bias + Prev_bias));  /* 微分环节 */ 
    
     Prev_bias=Last_bias;                                   /* 保存上上次偏差 */
	 Last_bias=Bias;	                                    /* 保存上一次偏差 */
    
	 return Pwm;                                            /* 输出结果 */
}
//右轮 增量式PID计算
int Incremental_PID_Right(int reality,int target)
{ 	
	 static float Bias,Pwm,Last_bias=0,Prev_bias=0;
    
	 Bias=target-reality;                                   /* 计算偏差 */
    
	 Pwm += (Incremental_KP*(Bias-Last_bias))               /* 比例环节 */
           +(Incremental_KI*Bias)                           /* 积分环节 */
           +(Incremental_KD*(Bias - 2 * Last_bias + Prev_bias));  /* 微分环节 */ 
    
     Prev_bias=Last_bias;                                   /* 保存上上次偏差 */
	 Last_bias=Bias;	                                    /* 保存上一次偏差 */
    
	 return Pwm;                                            /* 输出结果 */
}