金牌会员
- 积分
- 1167
- 威望
- 646
- 贡献
- 255
- 兑换币
- 327
- 注册时间
- 2015-11-16
- 在线时间
- 133 小时
- 毕业学校
- 光明
|
void Steer_Out()
{
short int Angle_D_error = 0,Angle_error = 0,Angle_last_error = 0,steer_out = 0;
uint8 i,n = 0;
uint8 steer_p = 8,steer_d = 0;
for(i = 59;i > 0;i --)
{
Angle_error += (center -(CAMERA_W>>1));
n ++;
}
Angle_error /= n;
Angle_D_error = Angle_error - Angle_last_error;
steer_out = steer_p*Angle_error + steer_d*Angle_D_error;
Angle_last_error = Angle_error;
if(steer_out > 2000)
steer_out = 2000;
if(steer_out < -2000)
steer_out = -2000;
ftm_pwm_init(FTM1, FTM_CH0,100, 2800 + steer_out);
}
|
|