void app_ultrasonic_mode(void)
{
int Len = 0;
Len = bsp_getUltrasonicDistance();
//printf("CSB:%d", Len);
if(Len < 25)//數值為碰到障礙物的距離，可以按實際情況設置
{
Len = (u16)bsp_getUltrasonicDistance();
while(Len < 25)//再次判斷是否有障礙物，若有則轉動方向后，繼續判斷
{
Car_Stop();//停車
Car_SpinRight(3600, 3600);
delay_ms(300);
Len = (u16)bsp_getUltrasonicDistance();
}
}
else
{
Car_Run(3600); //無障礙物，直行
}
}
/**
* Function app_ultrasonic_servo_mode
* @author liusen
* @date 2017.07.20
* @brief 超聲波避障
* @param[in] void
* @param[out] void
* @retval void
* @par History 無
*/
void app_ultrasonic_servo_mode(void)
{
int Len = 0;
int LeftDistance = 0, RightDistance = 0;
Len = (u16)bsp_getUltrasonicDistance();
if(Len <= 30)//當遇到障礙物時
{
Car_Stop();//停下來做測距
Angle_J1 = 180; // 左邊
delay_ms(500); //等待舵機到位
Len = bsp_getUltrasonicDistance();
LeftDistance = Len;
Angle_J1 = 0; // 右邊
delay_ms(500); //等待舵機到位
Len = bsp_getUltrasonicDistance();
RightDistance = Len;
Angle_J1 = 90; //歸位
delay_ms(500); //等待舵機到位
if((LeftDistance < 22 ) &&( RightDistance < 22 ))//當左右兩側均有障礙物靠得比較近
{
Car_SpinRight(6000, 5000);//旋轉掉頭
delay_ms(500); //等待舵機到位
}
else if(LeftDistance >= RightDistance)//左邊比右邊空曠
{
Car_SpinLeft(6000, 5000);//左轉
delay_ms(500); //等待舵機到位
}
else//右邊比左邊空曠
{
Car_SpinRight(6000, 5000); //右轉
delay_ms(500); //等待舵機到位
}
}
else if(Len > 30)//當遇到障礙物時
{
Car_Run(5000); //無障礙物，直行
}
}
|
