Car/Service/Src/path_plan.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 < 10; i++)
    {
        // AddPathStep(LEFT);
        // AddPathStep(LEFT);
        // AddPathStep(LEFT);

        
        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];
    }
}