65 lines
1.5 KiB
C
65 lines
1.5 KiB
C
|
#include "block_detect.h"
|
||
|
|
|
||
|
|
#include "bluetooth.h"
|
||
|
|
#include "buzzer.h"
|
||
|
|
#include "hcsr04.h"
|
||
|
|
#include "infrared.h"
|
||
|
|
#include "syscalls.h"
|
||
|
|
#include "timer.h"
|
||
|
|
|
||
|
|
int has_block_front;
|
||
|
|
int has_block_left;
|
||
|
|
int has_block_right;
|
||
|
|
static uint8_t timer_event_id = -1;
|
||
|
|
static uint8_t timer_event_id2 = -1;
|
||
|
|
|
||
|
|
void INFRARED_Detect();
|
||
|
|
void Sonar_Detect();
|
||
|
|
|
||
|
|
// 避障检测初始化
|
||
|
|
void BlockDetect_Init()
|
||
|
|
{
|
||
|
|
timer_event_id = TIMER_AddInfiniteLoopEvent(timer_event_id, INFRARED_Detect, 20);
|
||
|
|
timer_event_id2 = TIMER_AddInfiniteLoopEvent(timer_event_id2, Sonar_Detect, 150);
|
||
|
|
}
|
||
|
|
|
||
|
|
// 每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);
|
||
|
|
return has_block_front;
|
||
|
|
break;
|
||
|
|
case LEFT_FRONT:
|
||
|
|
BUZZER_StartTimed(2);
|
||
|
|
return has_block_left;
|
||
|
|
break;
|
||
|
|
case RIGHT_FRONT:
|
||
|
|
BUZZER_StartTimed(2);
|
||
|
|
return has_block_right;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
return 0;
|
||
|
|
}
|