#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;
    }
}