98 lines
2.5 KiB
C
98 lines
2.5 KiB
C
#include "app_bus.h"
|
|
|
|
#include "app_line_seek.h"
|
|
#include "block_detect.h"
|
|
#include "buzzer.h"
|
|
#include "led.h"
|
|
#include "line_seek.h"
|
|
#include "motor.h"
|
|
#include "path_plan.h"
|
|
#include "syn6288.h"
|
|
|
|
typedef enum
|
|
{
|
|
RUNNING, // 按路线运营
|
|
READY_TO_PARK, // 准备进站停车（或者准备左转）
|
|
PARKING, // 进站停车
|
|
WAIT_PASSENGER, // 等候乘客
|
|
STOP // 停车
|
|
} BusState;
|
|
|
|
static BusState state;
|
|
|
|
static int current_station;
|
|
char* stations[] = {"太平园站", "西南交大站", "新业北街站", "终点站"};
|
|
|
|
void App_Bus(void)
|
|
{
|
|
// printf("bus_state:%d\n", state);
|
|
|
|
LineSeek_GetStatusStr();
|
|
|
|
switch (state)
|
|
{
|
|
case RUNNING: // 按路线运营
|
|
{
|
|
LineSeek_Start();
|
|
if (Has_Block(FRONT)) // 前方有障碍物，停车
|
|
{
|
|
state = STOP;
|
|
}
|
|
else if (Has_Block(RIGHT_FRONT)) // 右前方有障碍物，准备停车或者左转避障
|
|
{
|
|
state = READY_TO_PARK;
|
|
// state = WAIT_PASSENGER;
|
|
}
|
|
else if (Has_Block(LEFT_FRONT)) // 左前方有障碍物，右转避障
|
|
{
|
|
Direction nextDirs[] = {RIGHT, RIGHT};
|
|
ReplacePathStep(nextDirs, 2);
|
|
}
|
|
break;
|
|
}
|
|
case READY_TO_PARK: // 准备进站停车，或者准备左转避障
|
|
{
|
|
if (LineSeek_Equals("0xxx")) // 左方有路线，左转
|
|
{
|
|
Direction nextDirs[] = {LEFT, LEFT};
|
|
ReplacePathStep(nextDirs, 2);
|
|
state = RUNNING;
|
|
}
|
|
else if (LineSeek_Equals("xxx0")) // 右方有路线，右转进站
|
|
{
|
|
Direction nextDirs[] = {RIGHT, RIGHT};
|
|
ReplacePathStep(nextDirs, 2);
|
|
state = PARKING;
|
|
}
|
|
break;
|
|
}
|
|
case PARKING: // 进站停车
|
|
{
|
|
if (Has_Block(RIGHT_FRONT)) // 右前方有障碍物，停车
|
|
{
|
|
state = WAIT_PASSENGER;
|
|
}
|
|
break;
|
|
}
|
|
case WAIT_PASSENGER: // 等候乘客
|
|
{
|
|
// TODO: 添加乘客等待逻辑
|
|
LineSeek_Stop();
|
|
SYN_FrameInfo(0, stations[current_station]);
|
|
BUZZER_StartNTimes(3, 500, 500);
|
|
MOTOR_Stop();
|
|
HAL_Delay(5000);
|
|
current_station++;
|
|
state = RUNNING;
|
|
break;
|
|
}
|
|
case STOP: // 停车
|
|
{
|
|
LineSeek_Stop();
|
|
break;
|
|
}
|
|
default:
|
|
break;
|
|
}
|
|
App_LineSeek();
|
|
} |