这是一个简单的pid控制程序

				#include "app.h"
				
				#define P_center	180    			// 角度偏差与速度偏差系数之比,
				 									// 将弧度误差转换为位置 1度对应2.6毫米
				fp32   PROP=10;
				fp32   Ti=10000;
				fp32   Td=0;
				
				static fp32  	EP1 = 0.0;
				static fp32  	EP2 = 0.0;
				static int16 	AmendV = 0;
				
				/*
				==================================================================
				* 
				*函数名：		PIDControl	
				*
				*参数：			CtrlError:			误差	
				* 
				*功能描述：		pid增量式控制
				*
				*返回值：		调整量AmendV
				*
				*创建人：		g s，2007年3月30日
				*
				*版本号：		beta1.0
				*
				*修改记录：
				*   
				=================================================================== 
				*/
				int16 PIDControl(uint8 dir,fp32 CtrlError,fp32 PROP,fp32 Ti,fp32 Td)
				{
					fp32  a0 = 0.0;
					fp32  a1 = 0.0;
					fp32  a2 = 0.0;
					fp32  Vd;
				       
					a0 = ( PROP * ( 1 + 1 / Ti + Td ));     
					a1 = ( -PROP * ( 1 + 2 * Td ));         
					a2 = ( PROP * Td );    	
				   
					Vd = a0 * CtrlError + a1 * EP1 + a2 * EP2;  //增量式pid
				   
					AmendV = AmendV + Vd;
					
				    if(Int16Abs(AmendV)				      AmendV = 0;
				      
					if(dir==Front)
					{
					    if(AmendV > 0x1000)
						{
							AmendV = 0x1000;
						}
						if(AmendV < -0x1000)
						{
							AmendV = -0x1000;
						}//限幅
					}
					if(dir==Back)
					{      
					    if(AmendV > 0x400)
						{
							AmendV = 0x400;
						}
						if(AmendV < -0x400)
						{
							AmendV = -0x400;
						}//限幅
					}
				  
					EP2 = EP1;
					EP1 = CtrlError; 
				
					return AmendV;  	
				}
				
				/*
				==================================================================
				* 
				*函数名：		PIDClear	
				*
				*参数：			none
				* 
				*功能描述：		清空本文件静态全局变量EP1 ，EP2 ，amend_V ，
				*				应该在一段PID控制结束后调用此函数，以备下一个PID控制用。
				*				
				*
				*返回值：		成功返回TRUE
				*
				*创建人：		rc，2008年1月11日
				*
				*版本号：		beta1.0
				*
				*修改记录：
				*   
				=================================================================== 
				*/
				uint8 PIDClear( void )
				{
					EP1 = 0.0 ;
					EP2 = 0.0 ;
					AmendV = 0 ;
					
					return TRUE ;
				}
				
				/*
				==================================================================
				* 
				*函数名：		CombineError	
				*
				*参数：			AngleError:				角度误差	
				*				CenterError:		   	位置误差
				* 
				*功能描述：		将角度误差和位置误差综合成一个，便于校正
				*
				*返回值：		综合误差error
				*
				*创建人：		rc，2008年1月11日
				*
				*版本号：		1.0
				*
				*修改记录：
				*   
				=================================================================== 
				*/
				fp32 CombineError( fp32 AngleError , fp32 CenterError )
				{
					return CenterError+ AngleError * P_center ;
				}