中级会员
- 积分
- 414
- 威望
- 233
- 贡献
- 121
- 兑换币
- 140
- 注册时间
- 2013-12-1
- 在线时间
- 30 小时
- 毕业学校
- 安徽财经大学
|
2#
楼主 |
发表于 2014-5-16 08:28:22
|
只看该作者
//////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////// 方向控制 //////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////
int dir_mid=2800;
int dir_left=3450;
int dir_right=2150;
int dir_p=6;
//3250左值,2350右值,2800中值
int abs(int a)
{
if(a<0)
return -a;
else
return a;
}
int dir1() {
int leftnum,rightnum,left,right,i;
int output;
left=0;
right=0;
for(i=64;i>=5;i--)
{
if(abs(ADV[i-2]-ADV[i])>=20)
{
left=1; //找到左边黑线位置
leftnum=i-2;
break;
}
}
for(i=64;i<=102;i++)
{
if(abs(ADV[i+2]-ADV[i])>=20)
{
right=1; //找到右边黑线位置
rightnum=i+2;
break;
}
}
if(right==1&&left==1)
{
rightnum=abs(rightnum-64);
leftnum=abs(64-leftnum);
if(rightnum>leftnum)
output=dir_mid-(rightnum-leftnum)*dir_p;
else if(rightnum<leftnum)
output=dir_mid+(leftnum-rightnum)*dir_p;
else output=dir_mid;
}
else if(right==1)
output=dir_mid+rightnum*dir_p;
else if(left==1)
output=dir_mid-leftnum*dir_p;
if(output>dir_left)
output=dir_left;
if(output<dir_right)
output=dir_right;
PWM_Output(PWM_PIN_PTA10,output);
return output;
} |
|