From 47a4473330d043201a30d2ba3b9132d980b7a20f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=B0=A7=E5=8E=9F=E5=AD=90?= Date: Sat, 17 May 2025 15:12:16 +0800 Subject: [PATCH] =?UTF-8?q?2025.5.17=E7=AC=AC=E4=BA=8C=E6=AC=A1=E6=8F=90?= =?UTF-8?q?=E4=BA=A4=EF=BC=8C=E6=B7=BB=E5=8A=A0=E6=A0=B9=E6=8D=AE=E5=AE=9E?= =?UTF-8?q?=E6=9C=BA=E8=B0=83=E8=AF=95=E4=BF=AE=E6=94=B9=E7=9A=84=E7=89=88?= =?UTF-8?q?=E6=9C=AC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- bot_code/makefile | 20 +++ bot_code/maze_func.h | 47 ++++++ bot_code/maze_func_array.cpp | 42 ++++++ bot_code/maze_func_explore.cpp | 257 +++++++++++++++++++++++++++++++++ bot_code/maze_func_flood.cpp | 63 ++++++++ bot_code/maze_func_goto.cpp | 83 +++++++++++ bot_code/maze_func_output.cpp | 58 ++++++++ bot_code/maze_func_path.cpp | 51 +++++++ bot_code/maze_func_pid.cpp | 122 ++++++++++++++++ bot_code/maze_func_turn.cpp | 45 ++++++ bot_code/maze_main.cpp | 42 ++++++ bot_code/maze_parameter.cpp | 46 ++++++ bot_code/maze_parameter.h | 43 ++++++ bot_code/全局变量.txt | 16 ++ bot_code/名称解释.txt | 5 + 15 files changed, 940 insertions(+) create mode 100644 bot_code/makefile create mode 100644 bot_code/maze_func.h create mode 100644 bot_code/maze_func_array.cpp create mode 100644 bot_code/maze_func_explore.cpp create mode 100644 bot_code/maze_func_flood.cpp create mode 100644 bot_code/maze_func_goto.cpp create mode 100644 bot_code/maze_func_output.cpp create mode 100644 bot_code/maze_func_path.cpp create mode 100644 bot_code/maze_func_pid.cpp create mode 100644 bot_code/maze_func_turn.cpp create mode 100644 bot_code/maze_main.cpp create mode 100644 bot_code/maze_parameter.cpp create mode 100644 bot_code/maze_parameter.h create mode 100644 bot_code/全局变量.txt create mode 100644 bot_code/名称解释.txt diff --git a/bot_code/makefile b/bot_code/makefile new file mode 100644 index 0000000..58e56dc --- /dev/null +++ b/bot_code/makefile @@ -0,0 +1,20 @@ + +cxx := g++sim + +file_cpp := $(wildcard *.cpp) +file_o := ../eyesim/all_test01/maze.x + +$(file_o) : $(file_cpp) + @$(cxx) $^ -o $@ + +maze : $(file_o) + +clean : + @rm -rf $(file_o) + +debug : + @echo $(cxx) + @echo $(file_cpp) + @echo $(file_o) + +.PHONY : maze clean debug \ No newline at end of file diff --git a/bot_code/maze_func.h b/bot_code/maze_func.h new file mode 100644 index 0000000..9eb2fd3 --- /dev/null +++ b/bot_code/maze_func.h @@ -0,0 +1,47 @@ +#pragma once + +/*---PART1---*/ +/*---以下为递归算法探索部分涉及到的函数声明---*/ +bool check_mark(); //检查单元格各方面是否已被确认 +void maze_entry(int x, int y, int dir, int open); //maze_entry函数,用于写入到wall数组中，1：有墙 0：通路 +bool unmarked(int y, int x, int dir); //检查该方向是否已被标记 +void explore(); //递归函数 +/*---以上为递归算法探索部分涉及到的函数声明---*/ + +/*---PART2---*/ +/*---以下为pid控制和行驶部分涉及到的函数声明---*/ +void xneighbor(int x, int dir); //X坐标更新 +void yneighbor(int y, int dir); //Y坐标更新 +void eI_xianfu(int eI_max); //对PID控制的积分部分I限幅 +void pid_speed_xianfu(int Kpid_speed); //对PID控制的输出部分限幅 +void PID_AL(); //位置式PID算法 +void PIDStraight(); //PID控制下的直线行驶 +void go_to(int dir); //行驶到指定单元格 +void BOTturn(int turn); //自定义转向函数 +/*---以上为pid控制和行驶部分涉及到的函数声明---*/ + +/*---PART3---*/ +/*---以下为洪水填充算法部分涉及到的函数声明---*/ +void flood(int *map, int *copy_wall); //洪水填充算法 +/*---以上为洪水填充算法部分涉及到的函数声明---*/ + +/*---PART4---*/ +/*---以下为路径算法部分涉及到的函数声明---*/ +void build_path(int i, int j, int len, int *path, int *map, int *copy_wall); //路径算法 +void drive_path(int len, int *path); //移动到目的地 +/*---以上为路径算法部分涉及到的函数声明---*/ + +/*---PART5---*/ +/*---以下为数组构建部分涉及到的函数声明---*/ +void array_negative_one(int size, int *arr); //将数组初始化为-1 +void array_copy_mark(int size_x, int size_y, int *copy_mark); //复制一份mark数组，便于后续频繁读取提升性能 +void array_copy_wall(int size_x, int size_y, int *copy_wall); //复制一份wall数组，便于后续频繁读取提升性能 +/*---以上为数组构建部分涉及到的函数声明---*/ + +/*---PART6---*/ +/*---以下为数组打印部分涉及到的函数声明---*/ +void output_arr2D(int size_x, int size_y, int *arr); //打印所有二维数组数组 +void output_arrwall(int size_x, int size_y, int *arr); //打印wall数组 +void output_arrpath(int size, int *arr); //打印path数组 +/*---以上为数组打印部分涉及到的函数声明---*/ + diff --git a/bot_code/maze_func_array.cpp b/bot_code/maze_func_array.cpp new file mode 100644 index 0000000..a35850d --- /dev/null +++ b/bot_code/maze_func_array.cpp @@ -0,0 +1,42 @@ +#include +#include +#include +#include"eyebot++.h" +#include"maze_parameter.h" +#include"maze_func.h" +using namespace std; + +/*---从vector容器复制mark信息到数组函数---*/ +void array_copy_mark(int size_x, int size_y, int *copy_mark) +{ + for (int i = 0; i < size_x; i++) + { + for (int j = 0; j < size_y; j++) + { + copy_mark[i * size_y + j] = mark[i][j]; + } + } +} + +/*---从vector容器复制wall信息到数组函数---*/ +void array_copy_wall(int size_x, int size_y, int *copy_wall) +{ + for (int i = 0; i <= size_x; i++) + { + for (int j = 0; j <= size_y; j++) + { + for(int k = 0; k < 2; k++) + { + copy_wall[i * (size_y + 1) * 2 + j * 2 + k] = wall[i][j][k]; //注释：k = 0记录的是单元格下方的数据，1记录的是单元格左边的数据，所以转为一维数组后，i*...+j*...+k,k=1为左侧数据，k=0为下方墙壁。 + } + } + } +} + +void array_negative_one(int size, int *arr) //将数组初始化为-1 +{ + for (int i = 0; i < size; i++) + { + arr[i] = -1; + } +} \ No newline at end of file diff --git a/bot_code/maze_func_explore.cpp b/bot_code/maze_func_explore.cpp new file mode 100644 index 0000000..ce91f7b --- /dev/null +++ b/bot_code/maze_func_explore.cpp @@ -0,0 +1,257 @@ +#include +#include +#include +#include +#include"eyebot++.h" +#include"maze_parameter.h" +#include"maze_func.h" +using namespace std; + + +/*---检查单元格各方向是否已被到访---*/ +bool check_mark() +{ bool check = false; + int x, y; + x = rob_x; //机器人当前x坐标 + y = rob_y; //机器人当前y坐标 + int size_x = mark.size(); + int size_y = mark[0].size(); + + bool N_mark; + bool W_mark; + bool S_mark; + bool E_mark; + + if (x > 0) + { + W_mark = mark[x-1][y]; + } + else + { + W_mark = true; + } + + if (y > 0) + { + S_mark = mark[x][y-1]; + } + else + { + S_mark = true; + } + + if (x < size_x - 1) + { + E_mark = mark[x+1][y]; + } + else + { + E_mark = true; + } + + if (y < size_y - 1) + { + N_mark = mark[x][y+1]; + } + else + { + N_mark = true; + } + + bool N_road = (!wall[x][y+1][0] && N_mark); + bool W_road = (!wall[x][y][1] && W_mark); + bool S_road = (!wall[x][y][0] && S_mark); + bool E_road = (!wall[x+1][y][1] && E_mark); + + if (N_road && W_road && S_road && E_road) + { + check = true; + } + return check; +} + +/*---maze_entry函数,用于写入到wall数组中，1：有墙 0：通路---*/ +void maze_entry(int x, int y, int dir, int open) +{ + int maze_dir = (dir + 4) % 4; + + switch (maze_dir) + { + case 0: //0表示北(上)，表示(x，y+1)坐标单元格的下方墙壁信息 + if (y == static_cast(mark[0].size()) - 1 && open) //检测是否超出容器范围，如果超出，则扩大容器：对每一个内层尾插数据 + { + if (!mark.empty() && !wall.empty()) //插入安全性检测，确保容器非空 + { + for (size_t i = 0; i < mark.size(); i++) + { + mark[i].emplace_back(0); + } + for (size_t i = 0; i < wall.size(); i++) + { + wall[i].emplace_back(std::vector(2,0)); + } + } + else + { + cout << "error: mark or wall empty" << endl; + } + } + wall[x][y+1][0] = !open; + break; + + case 1: //1表示西(左)，表示(x，y)坐标单元格的左侧墙壁信息 + if (x == 0 && open) //检测是否超出容器范围，如果超出，则扩大容器：头插一个内层 + { + if (!mark.empty() && !wall.empty()) //插入安全性检测，确保容器非空 + { + // std::vector mark_insert(mark[0].size(), 0); + // mark.insert(mark.begin(), mark_insert); + + // std::vector> wall_insert(wall[0].size(), std::vector(2,0)); + // wall.insert(wall.begin(), wall_insert); + + mark.emplace(mark.begin(), std::vector(mark[0].size(), 0)); //在mark头部插入一个内层 + wall.emplace(wall.begin(), std::vector>(wall[0].size(), std::vector(2, 0))); //在wall头部插入一个内层 + } + else + { + cout << "error: mark or wall empty" << endl; + } + } + wall[x][y][1] = !open; + break; + + case 2: //2表示南(下)，表示(x，y)坐标单元格的下侧墙壁信息 + if (y == 0 && open) //检测是否超出容器范围，如果超出，则扩大容器：对每一个内层头插数据 + { + if (!mark.empty() && !wall.empty()) //插入安全性检测，确保容器非空 + { + for (size_t i = 0; i < mark.size(); i++) + { + mark[i].emplace(mark[i].begin(), 0); + } + for (size_t i = 0; i < wall.size(); i++) + { + wall[i].emplace(wall[i].begin(), std::vector(2,0)); + } + } + else + { + cout << "error: mark or wall empty" << endl; + } + } + wall[x][y][0] = !open; + break; + + case 3: //3表示东(右)，表示(x+1，y)坐标单元格的左侧墙壁信息 + if (x == static_cast(mark.size()) - 1 && open) //检测是否超出容器范围，如果超出，则扩大容器：尾插一个内层 + { + if (!mark.empty() && !wall.empty()) //插入安全性检测，确保容器非空 + { + mark.emplace_back(std::vector(mark[0].size(), 0)); + wall.emplace_back(std::vector>(wall[0].size(), std::vector(2, 0))); + } + else + { + cout << "error: mark or wall empty" << endl; + } + } + wall[x+1][y][1] = !open; + break; + } +} + +/*---检查该方向是否已被标记---*/ +bool unmarked(int x, int y, int dir) +{ + bool check = false; + int maze_dir = (dir + 4) % 4; + switch(maze_dir) + { + case 0: + if (y < (static_cast(mark[0].size()) - 1)) + { + if (mark[x][y+1] == 0) + { + check = true; + } + } + break; + + case 1: + if (x > 0) + { + if (mark[x-1][y] == 0) + { + check = true; + } + } + break; + + case 2: + if (y > 0) + { + if (mark[x][y-1] == 0) + { + check = true; + } + } + break; + + case 3: + if (x < (static_cast(mark.size()) - 1)) + { + if (mark[x+1][y] == 0) + { + check = true; + } + } + break; + } + return check; +} + +/*---递归函数---*/ +void explore() +{ + int F_open,L_open,R_open,old_dir; + bool check; + + mark[rob_x][rob_y] = 1; //将当前的单元格设置为已到访 + F_open = PSDGetRaw(1) > DIST_cell; //获取前侧空间状况 + L_open = PSDGetRaw(2) > DIST_cell; //获取左侧空间状况 + R_open = PSDGetRaw(3) > DIST_cell; //获取右侧空间状况 + + maze_entry(rob_x, rob_y, rob_dir, F_open); //写入前方墙壁信息 + maze_entry(rob_x, rob_y, rob_dir + 1, L_open); //写入左侧墙壁信息 + maze_entry(rob_x, rob_y, rob_dir - 1, R_open); //写入右侧墙壁信息 + check = check_mark(); //检查3个方向是否到访 + if (check) + { + goto end_explore; + } + + old_dir = rob_dir; + if (F_open && unmarked(rob_x, rob_y, old_dir)) //如果前方有通路且前方单元格未被探索 + { + go_to(old_dir); //向前移动一格 + explore(); //进行递归，在完成所有可到达单元格的访问前，一直循环在explore中 + go_to(old_dir + 2); //返回到移动前的单元格 + } + + if (L_open && unmarked(rob_x, rob_y, old_dir + 1)) //如果左侧有通路且前方单元格未被探索 + { + go_to(old_dir + 1); //向左移动一格 + explore(); //进行递归，在完成所有可到达单元格的访问前，一直循环在explore中 + go_to(old_dir - 1); //返回到移动前的单元格 + } + + if (R_open && unmarked(rob_x, rob_y, old_dir - 1)) //如果右侧有通路且前方单元格未被探索 + { + go_to(old_dir - 1); //向右移动一格 + explore(); //进行递归，在完成所有可到达单元格的访问前，一直循环在explore中 + go_to(old_dir + 1); //返回到移动前的单元格 + } + end_explore: //check_mark函数确认三个方向都被探索的话，直接跳转到此处结束此次循环 + VWWait(); +} //递归结束 diff --git a/bot_code/maze_func_flood.cpp b/bot_code/maze_func_flood.cpp new file mode 100644 index 0000000..57c8f5f --- /dev/null +++ b/bot_code/maze_func_flood.cpp @@ -0,0 +1,63 @@ +#include +#include +#include +#include"eyebot++.h" +#include"maze_parameter.h" +#include"maze_func.h" +using namespace std; + +void flood(int *map, int *copy_wall) +{ + int num = 0; + int size_x = mark.size(); + int size_y = mark[0].size(); + map[0] = 0; + + do + { + for (int i = 0; i < size_x; i++) + { + for (int j = 0; j < size_y; j++) + { + if (map[(i * size_y) + j] != -1) + { + if (i > 0) + { + if (!copy_wall[(i * (size_y + 1) * 2) + (j * 2) + 1] && map[((i-1) * size_y) + j] == -1) //左边格子 + { + map[((i-1) * size_y) + j] = map[(i * size_y) + j] + 1; + num++; + } + } + + if (i < size_x - 1) + { + if (!copy_wall[((i+1) * (size_y + 1) * 2) + (j * 2) + 1] && map[((i+1) * size_y) + j] == -1) //右边格子 + { + map[((i+1) * size_y) + j] = map[(i * size_y) + j] + 1; + num++; + } + } + + if (j > 0) + { + if (!copy_wall[(i * (size_y + 1) * 2) + (j * 2) + 0] && map[(i *size_y) + (j-1)] == -1) + { + map[(i * size_y) + (j-1)] = map[(i * size_y) + j] + 1; + num++; + } + } + + if (j < size_y - 1) + { + if (!copy_wall[(i * (size_y + 1) * 2) + ((j+1) * 2) + 0] && map[(i * size_y) + (j+1)] == -1) + { + map[(i * size_y) + (j+1)] = map[(i * size_y) + j] + 1; + num++; + } + } + } + } + } + } while (map[((target_x - 1) * size_y) + (target_y - 1)] == -1 && num < (size_x * size_y)); +} \ No newline at end of file diff --git a/bot_code/maze_func_goto.cpp b/bot_code/maze_func_goto.cpp new file mode 100644 index 0000000..a6078ca --- /dev/null +++ b/bot_code/maze_func_goto.cpp @@ -0,0 +1,83 @@ +#include +#include +#include"eyebot++.h" +#include"maze_parameter.h" +#include"maze_func.h" +using namespace std; + +/*---X坐标更新---*/ +void xneighbor(int x, int dir) +{ + int maze_dir = (dir + 4) % 4; + switch (maze_dir) + { + case 0: + break; + + case 1: + rob_x = x-1; + break; + + case 2: + break; + + case 3: + rob_x = x+1; + break; + } +} + +/*---Y坐标更新---*/ +void yneighbor(int y, int dir) +{ + int maze_dir = (dir + 4) % 4; + switch (maze_dir) + { + case 0: + rob_y = y+1; + break; + + case 1: + break; + + case 2: + rob_y = y-1; + break; + + case 3: + break; + } +} + +/*---行驶到指定单元格---*/ +void go_to(int dir) +{ + int turn; //定义转向倍率 + int maze_dir; + //static int cur_x,cur_y,cur_p; + + maze_dir = (dir + 4) % 4; //保证机器人dir方位在[0-3]之间 + turn = maze_dir - rob_dir; //旋转角度倍数 + //turn = turn == +3 ? turn = -1 : turn = turn; 三目运算符？能用吗？ + + if (turn == +3) //确保机器人是旋转90度而不是270度，减少时间 + { + turn = -1; + } + if (turn == -3) + { + turn = +1; + } + + if (turn) //如果turn不是0，则执行旋转命令 + { + BOTturn(turn); + VWWait(); + } + PIDStraight(); //使用PID算法算法，向指定方向直线行驶一格 + VWWait(); //等待下一条指令 + + rob_dir = maze_dir; //更新机器人移动到下一格之后的方向 + xneighbor(rob_x,rob_dir); //更新机器人移动到下一格之后的X坐标 + yneighbor(rob_y,rob_dir); //更新机器人移动到下一格之后的y坐标 +} \ No newline at end of file diff --git a/bot_code/maze_func_output.cpp b/bot_code/maze_func_output.cpp new file mode 100644 index 0000000..f2a98ee --- /dev/null +++ b/bot_code/maze_func_output.cpp @@ -0,0 +1,58 @@ +#include +#include +#include"eyebot++.h" +#include"maze_func.h" +using namespace std; + + +/*---打印数组检查错误---*/ +/*---打印数组mark,map,copy_map等二维数组所用的函数---*/ +void output_arr2D(int size_x, int size_y, int *arr) +{ + for (int i = size_y - 1; i >= 0; i--) + { + for (int j = 0; j < size_x; j++) + { + cout << arr[(j * size_y) + i] << " "; + } + cout << endl; + } +} + +/*---打印数组wall---*/ +void output_arrwall(int size_x, int size_y, int *arr) +{ + for (int i = size_y; i >= 0; i--) + { + for (int j = 0; j <= size_x; j++) + { + if (arr[(j * (size_y + 1) * 2) + (i * 2) + 1] == 1) + { + cout << "|" ; + } + else + { + cout << " " ; + } + + if (arr[(j * (size_y + 1) * 2) + (i * 2) + 0] == 1) + { + cout << "_" ; + } + else + { + cout << " " ; + } + } + cout << endl; + } +} + +void output_arrpath(int size, int *arr) +{ + for (int i = 0; i < size; i++) + { + cout << arr[i] << " "; + } + cout << endl; +} \ No newline at end of file diff --git a/bot_code/maze_func_path.cpp b/bot_code/maze_func_path.cpp new file mode 100644 index 0000000..ec5338d --- /dev/null +++ b/bot_code/maze_func_path.cpp @@ -0,0 +1,51 @@ +#include +#include +#include +#include"eyebot++.h" +#include"maze_parameter.h" +#include"maze_func.h" +using namespace std; + +/*---解迷宫路径到path数组函数---*/ +void build_path(int i, int j, int len, int *path, int *map, int *copy_wall) +{ + int size_x = mark.size(); + int size_y = mark[0].size(); + + for (int k = len -1; k >= 0; k--) + { + if (i > 0 && !copy_wall[(i * (size_y + 1) * 2) + (j * 2) + 1] && map[((i-1) * size_y) + j] == k) //前一个单位的东面是当前格，即当前格子的左侧为上一个格子 + { + i--; + path[k] = 3; + } + else if (i < size_x - 1 && !copy_wall[((i+1) * (size_y + 1) * 2) + (j * 2) + 1] && map[((i+1) * size_y) + j] == k) //前一个单位的西面是当前格，即当前格子的右侧为上一个格子 + { + i++; + path[k] = 1; + } + else if(j > 0 && !copy_wall[(i * (size_y + 1) * 2) + (j * 2) + 0] && map[(i * size_y) + (j-1)] == k) //前一个单位的北面是当前格，即当前格子的下侧为上一个格子 + { + j--; + path[k] = 0; + } + else if (j < size_y - 1 && !copy_wall[(i * (size_y + 1) * 2) + ((j+1) * 2) + 0] && map[(i * size_y) + (j+1)] == k) //前一个单位的南面是当前格，即当前格子的上侧为上一个格子 + { + j++; + path[k] = 2; + } + else + { + LCDPrintf("ERROR"); + } + } +} + +/*---通过path数组移动到目标点函数---*/ +void drive_path(int len, int *path) +{ + for(int i = 0; i < len; i++) + { + go_to(path[i]); + } +} diff --git a/bot_code/maze_func_pid.cpp b/bot_code/maze_func_pid.cpp new file mode 100644 index 0000000..8bbf73a --- /dev/null +++ b/bot_code/maze_func_pid.cpp @@ -0,0 +1,122 @@ +#include +#include +#include"eyebot++.h" +#include"maze_parameter.h" +#include"maze_func.h" +using namespace std; + + +/*---对PID控制的积分部分I限幅---*/ +void eI_xianfu(int eI_max) +{ + if (err_sum >= eI_max) //如果err_sum大于限幅，则直接让err_sum=限幅，防止err_sum过大，影响PID控制 + { + err_sum = eI_max; + } + else if (err_sum <= -eI_max) + { + err_sum = -eI_max; + } + else + { + //此处为空; + } +} + +void pid_speed_xianfu(int Kpid_speed) +{ + if (Kpid_speed > speed_xianfu) + { + Kpid_speed = speed_xianfu; + } + else if (Kpid_speed < -speed_xianfu) + { + Kpid_speed = -speed_xianfu; + } + else + { + //此处为空 + } +} + +/*---位置式PID算法---*/ +void PID_AL() +{ + err_sum += err; //积分累计误差值 + eI_xianfu(eI_max); //积分部分限幅 + err_diff = err - err_old; //微分部分误差值 + /*---打印输出PID控制的各项参数1---*/ + cout << "本次误差:" << err << "\t" ; + cout << "上次误差:" << err_old << "\t" ; + cout << "误差累计:" << err_sum << "\t" ; + cout << "误差变化:" << err_diff << "\t" ; + /*---打印结束---*/ + err_old = err; //将误差幅值到上一次误差 + + Kpid = Kp*err + Ki*err_sum + Kd*err_diff; //PID控制算法输出的倍率 + Kpid_speed = Kpid * Kpid_base; + + pid_speed_xianfu(Kpid_speed); + + /*---打印输出PID控制的各项参数2---*/ + cout << "PID输出值:" << Kpid_speed << "\t" ; + /*---打印结束---*/ + // VWSetSpeed(speed,Kpid*Kpid_base); //控制小车的行驶速度，角速度(模拟器可以用，实物no) + MOTORDriveRaw(1,speed1); + MOTORDriveRaw(2,speed2 + Kpid_speed); +} + +/*---PID控制下的直线行驶---*/ +void PIDStraight() +{ + int DIST_move; //定义基础角速度和速度,和移动距离 + int L_PSD, R_PSD, F_PSD; //定义左侧，右侧，前方的距离值 + int x_1,x_2, y_1,y_2, phi_1,phi_2; //VWGetPosition的参数定义 + speed1 = speed; + VWGetPosition(&x_1, &y_1, &phi_1); //获取机器人在移动前的x,y,phi值 + + MOTORDriveRaw(1,speed1); + MOTORDriveRaw(2,speed2); + + do + { + F_PSD = PSDGetRaw(1) - 5; //读取前方和墙壁的距离 + L_PSD = PSDGetRaw(2) - 8; //读取左侧和墙壁的距离 + R_PSD = PSDGetRaw(3); //读取右侧和墙壁的距离 + + /*---打印输出PSD数值---*/ + // cout << "PSD值L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << "\t" ; + /*---打印结束---*/ + + if (30 DIST_wall_F); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行 + + MOTORDriveRaw(1,0); + MOTORDriveRaw(2,0); + +} diff --git a/bot_code/maze_func_turn.cpp b/bot_code/maze_func_turn.cpp new file mode 100644 index 0000000..e4d535d --- /dev/null +++ b/bot_code/maze_func_turn.cpp @@ -0,0 +1,45 @@ +#include +#include +#include"eyebot++.h" +#include"maze_parameter.h" +#include"maze_func.h" +using namespace std; + + +void BOTturn(int turn) +{ + if (turn == 1) + { + MOTORDriveRaw(1,24); + MOTORDriveRaw(2,-22); + + OSWait(1985); + + MOTORDriveRaw(1,0); + MOTORDriveRaw(2,0); + } + else if (turn == -1) + { + MOTORDriveRaw(1,-24); + MOTORDriveRaw(2,22); + + OSWait(1985); + + MOTORDriveRaw(1,0); + MOTORDriveRaw(2,0); + } + else if (turn == 2) + { + MOTORDriveRaw(1,24); + MOTORDriveRaw(2,-22); + + OSWait(2985); + + MOTORDriveRaw(1,0); + MOTORDriveRaw(2,0); + } + else + { + cout << "errer" << endl; + } +} \ No newline at end of file diff --git a/bot_code/maze_main.cpp b/bot_code/maze_main.cpp new file mode 100644 index 0000000..cace75d --- /dev/null +++ b/bot_code/maze_main.cpp @@ -0,0 +1,42 @@ +#include +#include +#include +#include"eyebot++.h" +#include"maze_parameter.h" +#include"maze_func.h" +using namespace std; + + +/*---下面是主函数---*/ +int main() +{ + explore(); //使用递归算法构建地图 + wall[0][0][0] = 1; + + const int mazesize_x = mark.size(); //获取探索结束之后的迷宫的X轴长度 + const int mazesize_y = mark[0].size(); //获取探索结束之后的迷宫的Y轴长度 + + int copy_mark[mazesize_x * mazesize_y] = {}; //复制一份mark数组，并转换为一维数组，便于后续频繁读取提升性能并初始化为0 + int copy_wall[(mazesize_x + 1) * (mazesize_y + 1) * 2] = {}; //复制一份wall数组，并转换为一维数组，便于后续频繁读取提升性能并初始化为0 + + array_copy_mark(mazesize_x, mazesize_y, copy_mark); //复制一份mark数组，并转换为一维数组，便于后续频繁读取提升性能 + array_copy_wall(mazesize_x, mazesize_y, copy_wall); //复制一份wall数组，并转换为一维数组，便于后续频繁读取提升性能 + + output_arr2D(mazesize_x, mazesize_y, copy_mark); //打印地图mark信息 + output_arrwall(mazesize_x, mazesize_y, copy_wall); //打印墙壁wall信息 + + int map[mazesize_x * mazesize_y] = {}; //创建最短路径求解地图并初始化为0 + array_negative_one(mazesize_x * mazesize_y, map); //将map数组初始化为-1 + + flood(map, copy_wall); //洪水填充算法 + output_arr2D(mazesize_x, mazesize_y, map); //打印map的数组信息 + + const int len = map[((target_x - 1) * mazesize_y) + (target_y -1)]; //迷宫的终点（人为确定） + int path[len] = {}; //创建最短路径结果地图并初始化为0 + + build_path(target_x - 1, target_y - 1, len, path, map, copy_wall); //构建出最短路径path数组 + drive_path(len, path); //移动到指定目标点 + + output_arrpath(len, path); //打印path数组信息，及小车每一步的行驶方向 + +} diff --git a/bot_code/maze_parameter.cpp b/bot_code/maze_parameter.cpp new file mode 100644 index 0000000..b5f3c9a --- /dev/null +++ b/bot_code/maze_parameter.cpp @@ -0,0 +1,46 @@ +#include +#include +#include +#include"eyebot++.h" +#include"maze_parameter.h" +// #include"maze_func.h" +using namespace std; + +/*---定义数据---*/ +/*---定义全局常量---*/ +const int DIST_cell = 300 ; //单元格长度 +const int DIST_wall_F = 90 ; //与墙壁的距离 +extern const int DIST_wall_RL = 100; //与左右墙壁的距离 + +/*---定义全局变量---*/ +/*---容器类---*/ +std::vector> mark(1,std::vector(1,0)); //定义mark容器（二维容器），容器初始为1行1列 1=为已访问 +std::vector>> wall(2,std::vector>(2,std::vector(2,0))); //定义wall容器（三维容器），容器初始为1行1列，且每个格子内带有2个墙壁信息 1=墙壁；0=通路 + +/*---机器人坐标---*/ +int rob_x0 = 0; //机器人起点X坐标 +int rob_y0 = 0; //机器人起点Y坐标 +int rob_x = 0; //机械人当前X坐标（初始为0） +int rob_y = 0; //机器人当前Y坐标（初始为0） +int rob_dir = 0; //机器人当前的朝向（初始为0） + +/*---递归算法---*/ + + +/*---PID算法---*/ +const float Kp = 0.8, Ki = 0.015, Kd = 0.15; //定义PID的参数 +float Kpid; //定义PID计算后输出的倍率 +int Kpid_speed; +const float Kpid_base = 1; //定义基本角速度和速度 +const int speed = 18; //定义基本角速度和速度 +const int speed2 = 0.75*speed; //快到目的地时减速 +const int speed_xianfu = 3; //定义kpid限幅 +int speed1; +int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差 +const int eI_max = 200; //定义积分限幅 + +/*---目标点的坐标---*/ +const int target_x = 4; //声明目标点的X坐标 +const int target_y = 4; //声明目标点的Y坐标 + +/*---定义结束---*/ \ No newline at end of file diff --git a/bot_code/maze_parameter.h b/bot_code/maze_parameter.h new file mode 100644 index 0000000..c1a437d --- /dev/null +++ b/bot_code/maze_parameter.h @@ -0,0 +1,43 @@ +#pragma once +#include + +/*---声明数据---*/ +/*---声明全局常量---*/ +extern const int DIST_cell; //单元格长度 +extern const int DIST_wall_F; //与前面墙壁的距离 +extern const int DIST_wall_RL; //与左右墙壁的距离 + +/*---声明全局变量---*/ + +/*---声明全局容器---*/ +extern std::vector> mark; //声明mark容器（二维容器） +extern std::vector>> wall; //声明wall容器（三维容器） + +/*---声明机器人坐标---*/ +extern int rob_x0; //机器人起点X坐标 +extern int rob_y0; //机器人起点Y坐标 +extern int rob_x; //机械人当前X坐标 +extern int rob_y; //机器人当前Y坐标 +extern int rob_dir; //机器人当前的朝向 + +/*---各种全局量声明结束---*/ + +/*---递归算法---*/ + +/*---PID算法---*/ +extern const float Kp, Ki, Kd; //声明PID的参数 +extern float Kpid; //定义PID计算后输出的倍率 +extern int Kpid_speed; //定义计算后的速度 +extern const float Kpid_base; //定义速度基础倍数速度 +extern const int speed; //定义速度基础倍数速度 +extern const int speed2; //快到目的地时减速 +extern const int speed_xianfu; //kpid输出速度的限幅 +extern int speed1; +extern int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差 +extern const int eI_max; //定义积分限幅 + +/*---目标点坐标---*/ +extern const int target_x; //声明目标点的X坐标 +extern const int target_y; //声明目标点的Y坐标 + +/*---声明结束---*/ \ No newline at end of file diff --git a/bot_code/全局变量.txt b/bot_code/全局变量.txt new file mode 100644 index 0000000..8344412 --- /dev/null +++ b/bot_code/全局变量.txt @@ -0,0 +1,16 @@ +几个数组： +wall，mark，maze.... + +机器人坐标： +rob_x 机器人X坐标 +rob_y 机器人Y坐标 +rob_dir 机器人朝向 + +迷宫基础数据： +DIST_cell 单个单元格的长度 +DIST_wall PID直线的时候，检测与墙壁的距离值 + +PID： +err，err_old，err_sum，err_diff, eI_max 误差，上一次误差，积分累计误差，微分误差变化，积分限幅 +Kp, Ki, Kd, Kpid PID控制的参数P,I,D以及算法输出的倍率参数 + diff --git a/bot_code/名称解释.txt b/bot_code/名称解释.txt new file mode 100644 index 0000000..b13245e --- /dev/null +++ b/bot_code/名称解释.txt @@ -0,0 +1,5 @@ +头文件:mazetool.h +函数文件:mazetool.cpp +全局: +rob_y:机器人在迷宫地图中的Y坐标值 +rob_x:机器人在迷宫地图中的x坐标值