52 lines
990 B
C
52 lines
990 B
C
|
|
||
|
#include "path_plan.h"
|
||
|
|
||
|
#include "stdio.h"
|
||
|
|
||
|
// 全局路径规划器
|
||
|
PathPlanner pathPlanner;
|
||
|
|
||
|
// 初始化路径规划器
|
||
|
void PathPlanner_Init()
|
||
|
{
|
||
|
pathPlanner.currentStep = -1;
|
||
|
pathPlanner.path = NULL;
|
||
|
pathPlanner.pathLength = 0;
|
||
|
SetPath();
|
||
|
}
|
||
|
|
||
|
// 设置自定义路径
|
||
|
void SetPath()
|
||
|
{
|
||
|
int i;
|
||
|
for (i = 0; i < 100; i++)
|
||
|
{
|
||
|
AddPathStep(LEFT);
|
||
|
AddPathStep(RIGHT);
|
||
|
AddPathStep(STRAIGHT);
|
||
|
}
|
||
|
}
|
||
|
|
||
|
// 获取当前方向
|
||
|
Direction GetCurrentDirection()
|
||
|
{
|
||
|
return pathPlanner.path[pathPlanner.currentStep];
|
||
|
}
|
||
|
|
||
|
// 获取下一步方向
|
||
|
Direction GetNextDirection()
|
||
|
{
|
||
|
if (pathPlanner.currentStep >= pathPlanner.pathLength)
|
||
|
{
|
||
|
pathPlanner.currentStep = -1;
|
||
|
}
|
||
|
return pathPlanner.path[++pathPlanner.currentStep];
|
||
|
}
|
||
|
|
||
|
// 添加路径
|
||
|
void AddPathStep(Direction step)
|
||
|
{
|
||
|
pathPlanner.path = realloc(pathPlanner.path, ++pathPlanner.pathLength * sizeof(Direction));
|
||
|
pathPlanner.path[pathPlanner.pathLength - 1] = step;
|
||
|
}
|