【算法】舵机位置式pid

s3010舵机基础上的舵机位置式pid算法实现

k60 DN 144 平台

给与舵机50hz的工作频率为获取较大的工作频率！

在智能车竞赛上测试通过。

const uint16 steering_mid=715;
int16 steer=715;
const uint16 steering_left=620;
const uint16 steering_right=800;
int16 error=0, error_nihe,lasterror=0;
int8 Max_error;

extern float Kd;   //
extern float Kp;   //
extern int8 ave;
extern int8 Center_Num;
extern uint8 CentreLineData[60];//保存得到的中线的行数 数组大小为图像行数 60

/************************************************** 
** 函数名称: void steer_control(void)
** 功能描述: 舵机打脚量控制函数
** 输    入:
** 输    出: 
** 说明：
***************************************************/
void SteerControl(void)
{                                                                      
	error = ave-40;     
	error_nihe = (int)(Kp*error+Kd*(error-lasterror)); 
	//   if(error_nihe>0)
	//   {
	//     error_nihe*=1.113;
	//   }
	steer = steering_mid - error_nihe;
	
	if(steer>=steering_right)
	{
		steer=steering_right;    
	} 
	else if(steer<=steering_left)
	{
		steer=steering_left;
	}
	
	lasterror=error;
	Max_error=error;
	
	if(Max_error>39)
	{
		Max_error=39;
	}
	else if (Max_error<-39)
	{
		Max_error=-39;
	}
	FTM_PWM_Duty(SD5_FTM,SD5_CH,steer);  
}