121 lines
3.2 KiB
C
121 lines
3.2 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, // 等候乘客
|
|
EXIT_STATION, // 出站
|
|
STOP // 停车
|
|
} BusState;
|
|
|
|
#define MIN_CROSS_TIME 10
|
|
static BusState state;
|
|
static cross_time;
|
|
|
|
static int current_station;
|
|
char* stations[] = {"太平园站", "西南交大站", "新业北街站", "终点站"};
|
|
|
|
void App_Bus(void)
|
|
{
|
|
// printf("bus_state:%d\n", state);
|
|
|
|
LineSeek_GetStatusStr();
|
|
|
|
switch (state)
|
|
{
|
|
case RUNNING: // 按路线运营
|
|
{
|
|
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};
|
|
ReplacePathStep(nextDirs, 1);
|
|
cross_time = 0;
|
|
state = PARKING;
|
|
}
|
|
break;
|
|
}
|
|
case PARKING: // 进站停车
|
|
{
|
|
if (LineSeek_Equals("0000")) // 左方有路线,左转
|
|
{
|
|
Direction nextDirs[] = {LEFT, LEFT};
|
|
ReplacePathStep(nextDirs, 2);
|
|
state = RUNNING;
|
|
}
|
|
|
|
if (Has_Block(RIGHT_FRONT)) // 右前方有障碍物,停车
|
|
{
|
|
state = WAIT_PASSENGER;
|
|
}
|
|
break;
|
|
}
|
|
case WAIT_PASSENGER: // 等候乘客
|
|
{
|
|
// TODO: 添加乘客等待逻辑
|
|
LineSeek_Stop();
|
|
SYN_FrameInfo(0, stations[current_station]);
|
|
MOTOR_Stop();
|
|
LED_StartNTimes(5, 400, 200, 0, 255, 255);
|
|
HAL_Delay(2000);
|
|
current_station++;
|
|
state = EXIT_STATION;
|
|
break;
|
|
}
|
|
case EXIT_STATION: // 出站
|
|
{
|
|
LineSeek_Start();
|
|
if (LineSeek_Equals("xxx0")) // 右方有路线,右转出站
|
|
{
|
|
Direction nextDirs[] = {RIGHT};
|
|
ReplacePathStep(nextDirs, 1);
|
|
state = RUNNING;
|
|
}
|
|
break;
|
|
}
|
|
case STOP: // 停车
|
|
{
|
|
LineSeek_Stop();
|
|
BUZZER_StartNTimes(10, 1000, 500);
|
|
LED_StartNTimes(10, 1000, 500, 255, 0, 0);
|
|
break;
|
|
}
|
|
default:
|
|
break;
|
|
}
|
|
App_LineSeek();
|
|
} |