用arduino ide写一个A*寻路算法

好的，以下是一个简单的 A* 寻路算法的实现，使用 Arduino IDE 编写：

#include <iostream>
#include <vector>
#include <queue>
#include <algorithm>

using namespace std;

// 定义节点结构体
struct Node {
    int x, y; // 节点坐标
    int f, g, h; // f = g + h
    Node* parent; // 父节点指针

    Node(int _x, int _y) : x(_x), y(_y), f(0), g(0), h(0), parent(nullptr) {}

    // 重载小于运算符，用于优先队列排序
    bool operator<(const Node& other) const {
        return f > other.f;
    }
};

// 判断节点是否在地图范围内
bool isValid(int x, int y, int n, int m) {
    return x >= 0 && x < n && y >= 0 && y < m;
}

// 判断节点是否是障碍物
bool isObstacle(int x, int y, vector<vector<int>>& grid) {
    return grid[x][y] == 1;
}

// 计算两个节点之间的曼哈顿距离
int manhattanDistance(Node* a, Node* b) {
    return abs(a->x - b->x) + abs(a->y - b->y);
}

// A* 寻路算法
vector<Node*> aStarSearch(vector<vector<int>>& grid, Node* start, Node* end) {
    int n = grid.size(), m = grid[0].size();
    vector<Node*> path;

    // 定义开启列表和关闭列表
    priority_queue<Node*> openList;
    vector<Node*> closedList;

    // 将起点加入开启列表
    openList.push(start);

    while (!openList.empty()) {
        // 取出开启列表中 f 值最小的节点
        Node* current = openList.top();
        openList.pop();

        // 如果当前节点是终点，返回路径
        if (current->x == end->x && current->y == end->y) {
            Node* node = current;
            while (node != nullptr) {
                path.push_back(node);
                node = node->parent;
            }
            reverse(path.begin(), path.end());
            break;
        }

        // 将当前节点加入关闭列表
        closedList.push_back(current);

        // 遍历当前节点周围的节点
        for (int dx = -1; dx <= 1; dx++) {
            for (int dy = -1; dy <= 1; dy++) {
                if (dx == 0 && dy == 0) continue; // 忽略当前节点
                int x = current->x + dx, y = current->y + dy;
                if (!isValid(x, y, n, m) || isObstacle(x, y, grid)) continue; // 节点不在地图范围内或是障碍物

                // 计算节点的 f, g, h 值
                int g = current->g + 1;
                int h = manhattanDistance(new Node(x, y), end);
                int f = g + h;

                // 如果节点已经在关闭列表中，忽略
                bool inClosedList = false;
                for (auto node : closedList) {
                    if (node->x == x && node->y == y) {
                        inClosedList = true;
                        break;
                    }
                }
                if (inClosedList) continue;

                // 如果节点已经在开启列表中，更新节点的 f 值和父节点指针
                bool inOpenList = false;
                for (auto node : openList) {
                    if (node->x == x && node->y == y) {
                        inOpenList = true;
                        if (f < node->f) {
                            node->f = f;
                            node->g = g;
                            node->h = h;
                            node->parent = current;
                        }
                        break;
                    }
                }
                if (inOpenList) continue;

                // 将节点加入开启列表
                Node* node = new Node(x, y);
                node->f = f;
                node->g = g;
                node->h = h;
                node->parent = current;
                openList.push(node);
            }
        }
    }

    return path;
}

// 测试
int main() {
    vector<vector<int>> grid = {
        {0, 0, 0, 0, 0},
        {0, 1, 0, 1, 0},
        {0, 0, 0, 0, 0},
        {0, 1, 0, 1, 0},
        {0, 0, 0, 0, 0}
    };
    Node* start = new Node(0, 0);
    Node* end = new Node(4, 4);
    vector<Node*> path = aStarSearch(grid, start, end);
    for (auto node : path) {
        cout << "(" << node->x << ", " << node->y << ") ";
    }
    cout << endl;
    return 0;
}

希望这个代码能够帮到你！