102 lines
2.2 KiB
C
102 lines
2.2 KiB
C
|
|
#include "path_plan.h"
|
|
|
|
#include "stdio.h"
|
|
#include "stdlib.h"
|
|
|
|
// 定义路径规划器结构
|
|
typedef struct
|
|
{
|
|
int currentStep;
|
|
Direction* path;
|
|
int pathLength;
|
|
} PathPlanner;
|
|
|
|
// 路径规划器
|
|
static PathPlanner pathPlanner;
|
|
|
|
void SetPath();
|
|
|
|
// 初始化路径规划器
|
|
void PathPlanner_Init()
|
|
{
|
|
pathPlanner.currentStep = -1;
|
|
pathPlanner.path = NULL;
|
|
pathPlanner.pathLength = 0;
|
|
SetPath();
|
|
}
|
|
|
|
// 设置自定义路径
|
|
void SetPath()
|
|
{
|
|
int i;
|
|
for (i = 0; i < 100; i++)
|
|
{
|
|
AddPathStep(STRAIGHT);
|
|
AddPathStep(STRAIGHT);
|
|
AddPathStep(STRAIGHT);
|
|
AddPathStep(RIGHT);
|
|
AddPathStep(LEFT);
|
|
AddPathStep(STRAIGHT);
|
|
AddPathStep(STRAIGHT);
|
|
AddPathStep(LEFT);
|
|
AddPathStep(STRAIGHT);
|
|
AddPathStep(STRAIGHT);
|
|
AddPathStep(STRAIGHT);
|
|
AddPathStep(LEFT);
|
|
AddPathStep(LEFT);
|
|
AddPathStep(LEFT);
|
|
// AddPathStep(STRAIGHT);
|
|
// AddPathStep(STRAIGHT);
|
|
// AddPathStep(STRAIGHT);
|
|
// AddPathStep(LEFT);
|
|
// AddPathStep(LEFT);
|
|
// AddPathStep(STRAIGHT);
|
|
// AddPathStep(STRAIGHT);
|
|
// AddPathStep(STRAIGHT);
|
|
// AddPathStep(RIGHT);
|
|
// AddPathStep(STRAIGHT);
|
|
// AddPathStep(STRAIGHT);
|
|
|
|
// AddPathStep(STRAIGHT);
|
|
// AddPathStep(STRAIGHT);
|
|
// AddPathStep(LEFT);
|
|
// AddPathStep(LEFT);
|
|
// AddPathStep(STRAIGHT);
|
|
// 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;
|
|
}
|
|
|
|
// 替换接下来的n个路径方向
|
|
void ReplacePathStep(Direction* step, int n)
|
|
{
|
|
int i;
|
|
for (i = 0; i < n; i++)
|
|
{
|
|
pathPlanner.path[(pathPlanner.currentStep + 1 + i) % pathPlanner.pathLength] = step[i];
|
|
}
|
|
}
|