93 lines
2.0 KiB
C
93 lines
2.0 KiB
C
#include "block_detect.h"
|
|
|
|
#include "bluetooth.h"
|
|
#include "buzzer.h"
|
|
#include "hcsr04.h"
|
|
#include "infrared.h"
|
|
#include "syscalls.h"
|
|
#include "timer.h"
|
|
|
|
#define MIN_BLOCK_TIME 500
|
|
|
|
int has_block_front;
|
|
int has_block_left;
|
|
int has_block_right;
|
|
int block_time;
|
|
|
|
void INFRARED_Detect();
|
|
void Sonar_Detect();
|
|
|
|
void Block_Time_Inc()
|
|
{
|
|
block_time++;
|
|
}
|
|
|
|
// 避障检测初始化
|
|
void BlockDetect_Init()
|
|
{
|
|
TIMER_AddLoopEvent(EVENT_INFRARED, INFRARED_Detect, 5);
|
|
TIMER_AddLoopEvent(EVENT_SONAR, Sonar_Detect, 150);
|
|
TIMER_AddLoopEvent(EVENT_BLOCK, Block_Time_Inc, 1);
|
|
}
|
|
|
|
// 每20ms红外检测一次障碍物
|
|
void INFRARED_Detect()
|
|
{
|
|
has_block_left = INFRARED_GetL() == INFRARED_MEET;
|
|
has_block_right = INFRARED_GetR() == INFRARED_MEET;
|
|
}
|
|
|
|
// 每100ms超声波检测一次障碍物
|
|
void Sonar_Detect()
|
|
{
|
|
if (sonor_state == 0) sonar_mm();
|
|
}
|
|
|
|
// 超声波测距完毕回调
|
|
void Sonar_CP_Callback()
|
|
{
|
|
// BLUETOOTH_Send("f: %d, l: %d, r: %d\n sonar distance = %d (mm)\n", has_block_front, has_block_left, has_block_right, sonor_distance);
|
|
has_block_front = sonor_distance < 200;
|
|
}
|
|
|
|
int Has_Block(BlockDirection direction)
|
|
{
|
|
switch (direction)
|
|
{
|
|
case FRONT:
|
|
// BUZZER_StartTimed(10);
|
|
if (has_block_front)
|
|
{
|
|
if (block_time > MIN_BLOCK_TIME)
|
|
{
|
|
block_time = 0;
|
|
return 1;
|
|
}
|
|
}
|
|
break;
|
|
case LEFT_FRONT:
|
|
// BUZZER_StartTimed(2);
|
|
if (has_block_left)
|
|
{
|
|
if (block_time > MIN_BLOCK_TIME)
|
|
{
|
|
block_time = 0;
|
|
return 1;
|
|
}
|
|
}
|
|
break;
|
|
case RIGHT_FRONT:
|
|
// BUZZER_StartTimed(2);
|
|
if (has_block_right)
|
|
{
|
|
if (block_time > MIN_BLOCK_TIME)
|
|
{
|
|
block_time = 0;
|
|
return 1;
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
return 0;
|
|
}
|