|
|
void app_ultrasonic_mode(void)
{
int Len = 0;
Len = bsp_getUltrasonicDistance();
//printf("CSB:%d", Len);
if(Len < 25)//數(shù)值為碰到障礙物的距離，可以按實(shí)際情況設(shè)置
{
Len = (u16)bsp_getUltrasonicDistance();
while(Len < 25)//再次判斷是否有障礙物，若有則轉(zhuǎn)動(dòng)方向后，繼續(xù)判斷
{
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)//當(dāng)遇到障礙物時(shí)
{
Car_Stop();//停下來做測距
Angle_J1 = 180; // 左邊
delay_ms(500); //等待舵機(jī)到位
Len = bsp_getUltrasonicDistance();
LeftDistance = Len;
Angle_J1 = 0; // 右邊
delay_ms(500); //等待舵機(jī)到位
Len = bsp_getUltrasonicDistance();
RightDistance = Len;
Angle_J1 = 90; //歸位
delay_ms(500); //等待舵機(jī)到位
if((LeftDistance < 22 ) &&( RightDistance < 22 ))//當(dāng)左右兩側(cè)均有障礙物靠得比較近
{
Car_SpinRight(6000, 5000);//旋轉(zhuǎn)掉頭
delay_ms(500); //等待舵機(jī)到位
}
else if(LeftDistance >= RightDistance)//左邊比右邊空曠
{
Car_SpinLeft(6000, 5000);//左轉(zhuǎn)
delay_ms(500); //等待舵機(jī)到位
}
else//右邊比左邊空曠
{
Car_SpinRight(6000, 5000); //右轉(zhuǎn)
delay_ms(500); //等待舵機(jī)到位
}
}
else if(Len > 30)//當(dāng)遇到障礙物時(shí)
{
Car_Run(5000); //無障礙物，直行
}
}
|
|
