/***********************************************************
功能:触须避障程序
说明:左触须接P1_4,右触须接P2_3)
时间:2012.12.14
作者:荆轩道
************************************************************/
#include<BoeBot.h>
#include<uart.h>
int P1_4state(void)//获取P1_4的状态,右胡须
{
return (P1&0x10)?1:0;
}
int P2_3state(void)//获取P2_3的状态,左胡须
{
return (P2&0x08)?1:0;
}
void Forward(void)
{
P1_1=1;
delay_nus(1700);
P1_1=0;
P1_0=1;
delay_nus(1300);
P1_0=0;
delay_nms(20);
}
void Left_Turn(void)
{
int i;
for(i=1;i<=26;i++)
{
P1_1=1;
delay_nus(1300);
P1_1=0;
P1_0=1;
delay_nus(1300);
P1_0=0;
delay_nms(20);
}
}
void Right_Turn(void)
{
int i;
for(i=1;i<=26;i++)
{
P1_1=1;
delay_nus(1700);
P1_1=0;
P1_0=1;
delay_nus(1700);
P1_0=0;
delay_nms(20);
}
}
void Backward(void)
{
int i;
for(i=1;i<=65;i++)
{
P1_1=1;
delay_nus(1300);
P1_1=0;
P1_0=1;
delay_nus(1700);
P1_0=0;
delay_nms(20);
}
}
int main(void)
{
uart_Init();
printf("Program Running!\n");
while(1)
{
if((P1_4state()==0)&&(P2_3state()==0))
{
Backward(); //向后
Left_Turn();//向左
Left_Turn();//向左
}
else if(P1_4state()==0)
{
Backward();//向后
Left_Turn();//向左
}
else if(P2_3state()==0)
{
Backward();//向后
Right_Turn();//向右
}
else
Forward();//向前
}
}