2024-07-17 17:39:06 +08:00
|
|
|
#include "app_ultrasonic.h"
|
|
|
|
#include "servo.h"
|
|
|
|
#include "hcsr04.h"
|
|
|
|
#include "main.h"
|
|
|
|
#include "led.h"
|
2024-07-17 22:14:32 +08:00
|
|
|
#include "path_plan.h"
|
2024-07-17 17:39:06 +08:00
|
|
|
|
|
|
|
#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;
|
|
|
|
}
|
|
|
|
}
|