51 lines
1.3 KiB
C
51 lines
1.3 KiB
C
|
#include "app_ultrasonic.h"
|
||
|
#include "servo.h"
|
||
|
#include "hcsr04.h"
|
||
|
#include "main.h"
|
||
|
#include "led.h"
|
||
|
|
||
|
#define BLOCK_DISTANCE 150
|
||
|
|
||
|
int APP_Find_Direction(void)
|
||
|
{
|
||
|
uint32_t left_block,right_block,straight_block;
|
||
|
|
||
|
// 测定左侧障碍物距离
|
||
|
SERVO_Rotate(50);
|
||
|
HAL_Delay(300);
|
||
|
left_block = sonar_mm();
|
||
|
|
||
|
// 测定正前侧障碍物距离
|
||
|
SERVO_Rotate(90);
|
||
|
HAL_Delay(300);
|
||
|
straight_block = sonar_mm();
|
||
|
|
||
|
// 测定右侧障碍物距离
|
||
|
SERVO_Rotate(150);
|
||
|
HAL_Delay(300);
|
||
|
right_block = sonar_mm();
|
||
|
|
||
|
LED_Start(7);
|
||
|
|
||
|
if(left_block > BLOCK_DISTANCE && right_block > BLOCK_DISTANCE && straight_block > BLOCK_DISTANCE)// 前左右均无障碍物,直行
|
||
|
{
|
||
|
return STRAIGHT;
|
||
|
}
|
||
|
else if(left_block < BLOCK_DISTANCE && right_block < BLOCK_DISTANCE && straight_block < BLOCK_DISTANCE)// 前左右均有障碍物,停止并鸣笛
|
||
|
{
|
||
|
LED_SetDuty(255,0,0);
|
||
|
return STOP;
|
||
|
}
|
||
|
else if(straight_block < BLOCK_DISTANCE && left_block < BLOCK_DISTANCE)// 前左有障碍物,右转
|
||
|
{
|
||
|
return RIGHT;
|
||
|
}
|
||
|
else if (straight_block < BLOCK_DISTANCE && right_block < BLOCK_DISTANCE)// 前右有障碍物,左转
|
||
|
{
|
||
|
return LEFT;
|
||
|
}
|
||
|
else
|
||
|
{
|
||
|
return STRAIGHT;
|
||
|
}
|
||
|
}
|