触须避障程序（改进篇）

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