From a81594bf68d6b963d29b706242b55b68d54f4a81 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=B0=A7=E5=8E=9F=E5=AD=90?= Date: Thu, 15 May 2025 22:40:18 +0800 Subject: [PATCH] =?UTF-8?q?2025.5.15=E7=AC=AC=E4=B8=89=E6=AC=A1=E6=8F=90?= =?UTF-8?q?=E4=BA=A4=EF=BC=8C=E7=BB=A7=E7=BB=AD=E4=BF=AE=E6=94=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- maze_code/maze_func_array.cpp | 2 +- maze_code/maze_func_flood.cpp | 30 ++++++++++++++--------------- maze_code/maze_func_output.cpp | 8 ++++---- maze_code/maze_func_path.cpp | 8 ++++---- maze_code/maze_func_pid.cpp | 35 ++++++++++++++++++---------------- maze_code/maze_main.cpp | 5 +++-- maze_code/maze_parameter.cpp | 18 +++++++++-------- maze_code/maze_parameter.h | 12 +++++++----- 8 files changed, 63 insertions(+), 55 deletions(-) diff --git a/maze_code/maze_func_array.cpp b/maze_code/maze_func_array.cpp index a9c9414..acb6fa6 100644 --- a/maze_code/maze_func_array.cpp +++ b/maze_code/maze_func_array.cpp @@ -27,7 +27,7 @@ void array_copy_wall(int size_x, int size_y, int *copy_wall) { for(int k = 0; k < 2; k++) { - copy_wall[i * size_y * 2 + j * 2 + k] = wall[i][j][k]; + copy_wall[i * (size_y + 1) * 2 + j * 2 + k] = wall[i][j][k]; } } } diff --git a/maze_code/maze_func_flood.cpp b/maze_code/maze_func_flood.cpp index 927b38c..705adbc 100644 --- a/maze_code/maze_func_flood.cpp +++ b/maze_code/maze_func_flood.cpp @@ -25,36 +25,36 @@ void flood(int *map, int *copy_map, int *copy_wall) { if (i > 0) { - if (!copy_wall[(i * size_y * 2) + (j * 2) + 1] && map[((i-1) * size_y) + j] == -1) //左边格子 + if (!copy_wall[(i * (size_y + 1) * 2) + (j * 2) + 1] && map[((i-1) * size_y) + j] == -1) //左边格子 { - copy_map[((i-1) * size_y) + j] = map[(i * 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 * 2) + (j * 2) + 1] && map[((i+1) * size_y) + j] == -1) //右边格子 + if (!copy_wall[((i+1) * (size_y + 1) * 2) + (j * 2) + 1] && map[((i+1) * size_y) + j] == -1) //右边格子 { - copy_map[((i+1) * size_y) + j] = map[(i * 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 * 2) + (j * 2) + 0] && map[(i *size_y) + (j-1)] == -1) + if (!copy_wall[(i * (size_y + 1) * 2) + (j * 2) + 0] && map[(i *size_y) + (j-1)] == -1) { - copy_map[(i * size_y) + (j-1)] = map[(i * size_y) + j] + 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 * 2) + ((j+1) * 2) + 0] && map[(i * size_y) + (j+1)] == -1) + if (!copy_wall[(i * (size_y + 1) * 2) + ((j+1) * 2) + 0] && map[(i * size_y) + (j+1)] == -1) { - copy_map[(i * size_y) + (j+1)] = map[(i * size_y) + j] + 1; + map[(i * size_y) + (j+1)] = map[(i * size_y) + j] + 1; num++; } } @@ -62,12 +62,12 @@ void flood(int *map, int *copy_map, int *copy_wall) } } - for (int i = 0; i < size_x; i++) - { - for (int j = 0; j < size_y; j++) - { - map[(i * size_y) + j] = copy_map[(i * size_y) + j]; - } - } + // for (int i = 0; i < size_x; i++) + // { + // for (int j = 0; j < size_y; j++) + // { + // map[(i * size_y) + j] = copy_map[(i * size_y) + j]; + // } + // } } while (map[(target_x * size_y) + target_y] == -1 && num < (size_x * size_y)); } \ No newline at end of file diff --git a/maze_code/maze_func_output.cpp b/maze_code/maze_func_output.cpp index 726bccc..ab714ae 100644 --- a/maze_code/maze_func_output.cpp +++ b/maze_code/maze_func_output.cpp @@ -26,11 +26,11 @@ void output_arr2D(int size_x, int size_y, int *arr) void output_arrwall(int size_x, int size_y, int *arr) { - for (int i = size_y - 1; i >= 0; i--) + for (int i = size_y; i >= 0; i--) { - for (int j = 0; j < size_x; j++) + for (int j = 0; j <= size_x; j++) { - if (arr[(j * size_y * 2) + (i * 2) + 1] == 1) + if (arr[(j * (size_y + 1) * 2) + (i * 2) + 1] == 1) { cout << "|" ; } @@ -39,7 +39,7 @@ void output_arrwall(int size_x, int size_y, int *arr) cout << " " ; } - if (arr[(j * size_y * 2) + (i * 2) + 0] == 1) + if (arr[(j * (size_y + 1) * 2) + (i * 2) + 0] == 1) { cout << "_" ; } diff --git a/maze_code/maze_func_path.cpp b/maze_code/maze_func_path.cpp index fe4883b..fb3ae2e 100644 --- a/maze_code/maze_func_path.cpp +++ b/maze_code/maze_func_path.cpp @@ -15,22 +15,22 @@ void build_path(int i, int j, int len, int *path, int *map, int *copy_wall) for (int k = len -1; k >= 0; k--) { - if (i > 0 && !copy_wall[(i * size_y * 2) + (j * 2) + 1] && map[((i-1) * size_y) + j] == 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 * 2) + (j * 2) + 1] && map[((i+1) * size_y) + j] == k) //前一个单位的西面是当前格，即当前格子的右侧为上一个格子 + 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 * 2) + (j * 2) + 0] && map[(i * size_y) + (j-1)] == k) //前一个单位的北面是当前格，即当前格子的下侧为上一个格子 + 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 * 2) + ((j+1) * 2) + 0] && map[(i * size_y) + (j+1)] == k) //前一个单位的南面是当前格，即当前格子的上侧为上一个格子 + 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; diff --git a/maze_code/maze_func_pid.cpp b/maze_code/maze_func_pid.cpp index 2d3af39..367fbac 100644 --- a/maze_code/maze_func_pid.cpp +++ b/maze_code/maze_func_pid.cpp @@ -31,21 +31,21 @@ void PID_AL() 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" ; + 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控制的各项参数2---*/ - // cout << "PID输出值:" << Kpid << endl ; + cout << "PID输出值:" << Kpid_speed << "\t" ; /*---打印结束---*/ // VWSetSpeed(speed,Kpid*Kpid_base); //控制小车的行驶速度，角速度(模拟器可以用，实物no) MOTORDriveRaw(1,speed1); - MOTORDriveRaw(2,speed1+Kpid_speed); + MOTORDriveRaw(2,speed1 + Kpid_speed); } /*---PID控制下的直线行驶---*/ @@ -70,19 +70,19 @@ void PIDStraight() // cout << "PSD值L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << "\t" ; /*---打印结束---*/ - if (100 (DIST_cell -50)) //当移动距离只剩下最后的50mm的时候降点速度 - { - speed1 = speed2; - } - }while (DIST_move < DIST_cell && F_PSD > DIST_wall); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行 + cout << "move:" << DIST_move << endl; + // if (DIST_move > (DIST_cell -50)) //当移动距离只剩下最后的50mm的时候降点速度 + // { + // speed1 = speed2; + // } + }while (DIST_move < DIST_cell && F_PSD > DIST_wall_F); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行 + MOTORDriveRaw(1,0); + MOTORDriveRaw(2,0); } diff --git a/maze_code/maze_main.cpp b/maze_code/maze_main.cpp index 7ae81dc..10bc1e0 100644 --- a/maze_code/maze_main.cpp +++ b/maze_code/maze_main.cpp @@ -12,7 +12,8 @@ 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轴长度 @@ -23,7 +24,7 @@ int main() array_copy_wall(mazesize_x, mazesize_y, copy_wall); //复制一份wall数组，并转换为一维数组，便于后续频繁读取提升性能 output_arr2D(mazesize_x, mazesize_y, copy_mark); //打印地图mark信息 - output_arrwall(mazesize_x + 1, mazesize_y + 1, copy_wall); //打印墙壁wall信息 + output_arrwall(mazesize_x, mazesize_y, copy_wall); //打印墙壁wall信息 int map[mazesize_x * mazesize_y] = {}; //创建最短路径求解地图并初始化为0 int copy_map[mazesize_x * mazesize_y] = {}; //创建copy复制地图并初始化为0 diff --git a/maze_code/maze_parameter.cpp b/maze_code/maze_parameter.cpp index 77b66c6..1379df3 100644 --- a/maze_code/maze_parameter.cpp +++ b/maze_code/maze_parameter.cpp @@ -9,13 +9,14 @@ using namespace std; /*---定义数据---*/ /*---定义全局常量---*/ -const int DIST_cell = 360 ; //单元格长度 -const int DIST_wall = 130 ; //与墙壁的距离 +const int DIST_cell = 300 ; //单元格长度 +const int DIST_wall_F = 90 ; //与墙壁的距离 +extern const int DIST_wall_RL = 100; //与左右墙壁的距离 /*---定义全局变量---*/ /*---容器类---*/ -vector> mark(1,vector(1,0)); //定义mark容器（二维容器），容器初始为1行1列 1=为已访问 -vector>> wall(2,vector>(2,vector(2,0))); //定义wall容器（三维容器），容器初始为1行1列，且每个格子内带有2个墙壁信息 1=墙壁；0=通路 +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坐标 @@ -28,11 +29,12 @@ int rob_dir = 0; //机器人当前的朝向（初始为0） /*---PID算法---*/ -const float Kp = 1, Ki = 0, Kd = 0.5; //定义PID的参数 +const float Kp = 0.5, Ki = 0.2, Kd = 0.5; //定义PID的参数 float Kpid; //定义PID计算后输出的倍率 -float Kpid_speed; -const int Kpid_base = 2, speed = 24; //定义基本角速度和速度 -const int speed2 = 0.75*speed; //快到目的地时减速 +int Kpid_speed; +const float Kpid_base = 0.1; //定义基本角速度和速度 +const int speed = 50; //定义基本角速度和速度 +const int speed2 = 0.75*speed; //快到目的地时减速 int speed1; int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差 const int eI_max = 20; //定义积分限幅 diff --git a/maze_code/maze_parameter.h b/maze_code/maze_parameter.h index 7100aa0..2684aa9 100644 --- a/maze_code/maze_parameter.h +++ b/maze_code/maze_parameter.h @@ -4,13 +4,14 @@ /*---声明数据---*/ /*---声明全局常量---*/ extern const int DIST_cell; //单元格长度 -extern const int DIST_wall; //与墙壁的距离 +extern const int DIST_wall_F; //与前面墙壁的距离 +extern const int DIST_wall_RL; //与左右墙壁的距离 /*---声明全局变量---*/ /*---声明全局容器---*/ -extern std::vector> mark; //声明mark容器（二维容器） -extern std::vector>> wall; //声明wall容器（三维容器） +extern std::vector> mark; //声明mark容器（二维容器） +extern std::vector>> wall; //声明wall容器（三维容器） /*---声明机器人坐标---*/ extern int rob_x0; //机器人起点X坐标 @@ -26,8 +27,9 @@ extern int rob_dir; //机器人当前的朝向 /*---PID算法---*/ extern const float Kp, Ki, Kd; //声明PID的参数 extern float Kpid; //定义PID计算后输出的倍率 -extern float Kpid_speed; //定义计算后的速度 -extern const int Kpid_base, speed; //定义基本角速度和速度 +extern int Kpid_speed; //定义计算后的速度 +extern const float Kpid_base; //定义速度基础倍数速度 +extern const int speed; //定义速度基础倍数速度 extern const int speed2; //快到目的地时减速 extern int speed1; extern int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差