触须避障程序

来源：互联网发布：java写文件换行编辑：程序博客网时间：2024/05/01 05:01

/***********************************************************
功能：触须避障程序
说明：左触须接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();//向前
}
}