diff --git a/编写代码/maze_func.h b/编写代码/maze_func.h new file mode 100644 index 0000000..2212a87 --- /dev/null +++ b/编写代码/maze_func.h @@ -0,0 +1,23 @@ +#pragma once + +/*---PART1---*/ +/*---以下为递归算法探索部分涉及到的函数声明---*/ +bool check_mark(); //检查单元格各方面是否已被确认 +void maze_entry(int x, int y, int dir, int open); //maze_entry函数,用于写入到wall数组中，1：有墙 0：通路 +void xneighbor(int x, int dir); //X坐标更新 +void yneighbor(int y, int dir); //Y坐标更新 +void eI_xianfu(int eI_max); //对PID控制的积分部分I限幅 +void PID_AL(); //位置式PID算法 +void PIDStraight(); //PID控制下的直线行驶 +void go_to(int dir); //行驶到指定单元格 +bool unmarked(int y, int x, int dir); //检查该方向是否已被标记 +void explore(); //递归函数 +void output_mark(); //打印mark数组检查错误 +void output_wall(); //打印wall数组检查错误 +/*---以上为递归算法探索部分涉及到的函数声明---*/ + +/*---PART2---*/ +/*---以下为洪水填充算法设涉及到的函数声明---*/ + + + diff --git a/编写代码/maze_func_explore.cpp b/编写代码/maze_func_explore.cpp new file mode 100644 index 0000000..5fc6fc1 --- /dev/null +++ b/编写代码/maze_func_explore.cpp @@ -0,0 +1,327 @@ +#include +#include +#include"eyebot++.h" +#include"maze_func.h" +#include"maze_parameter.h" +using namespace std; + + +/*---检查单元格各方向是否已被到访---*/ +bool check_mark() +{ bool check = false; + int x, y; + x = rob_x; //机器人当前x坐标 + y = rob_y; //机器人当前y坐标 + + bool N_road = ((wall[x][y+1][0] + 1) % 2 && mark[x][y+1]); + bool W_road = ((wall[x][y][1] + 1) % 2 && mark[x-1][y]); + bool S_road = ((wall[x][y][0] + 1) % 2 && mark[x][y-1]); + bool E_road = ((wall[x+1][y][1] + 1) % 2 && mark[x+1][y]); + + if (x>0 && y>0) //情况1 + { + if (N_road && W_road && S_road && E_road) + { + check = true; + } + } + else if (x==0 && y>0) //情况2 + { + if (N_road && S_road && E_road) + { + check = true; + } + } + else if (x>0 && y==0) //情况3 + { + if (N_road && W_road && E_road) + { + check = true; + } + } + else //情况4 + { + if (N_road && E_road) + { + check = true; + } + } + // if (check) + // { + // goto end_explore; + // } + return check; +} + +/*---maze_entry函数,用于写入到wall数组中，1：有墙 0：通路---*/ +void maze_entry(int x, int y, int dir, int open) +{ + int maze_dir = (dir + 4) % 4; + + if (open) //获取到的open信息为是否有路径信息，转换为墙壁信息 + { + open = 0; + } + else + { + open = 1; + } + + switch (maze_dir) + { + case 0: //0表示北(上)，表示(x，y+1)坐标单元格的下方墙壁信息 + wall[x][y+1][0] = open; + break; + + case 1: //1表示西(左)，表示(x，y)坐标单元格的左侧墙壁信息 + wall[x][y][1] = open; + break; + + case 2: //2表示南(下)，表示(x，y)坐标单元格的下侧墙壁信息 + wall[x][y][0] = open; + break; + + case 3: //3表示东(右)，表示(x+1，y)坐标单元格的左侧墙壁信息 + wall[x+1][y][1] = open; + break; + } +} + +/*---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; + } +} + +/*---对PID控制的积分部分I限幅---*/ +void eI_xianfu(int eI_max) +{ + if (err_sum >= eI_max) //如果err_sum大于限幅，则直接让err_sum=限幅，防止err_sum过大，影响PID控制 + { + err_sum = eI_max; + } +} + +/*---位置式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控制算法输出的倍率 + /*---打印输出PID控制的各项参数2---*/ + // cout << "PID输出值:" << Kpid << endl ; + /*---打印结束---*/ + VWSetSpeed(SPEED,Kpid*Alpha); //控制小车的行驶速度，角速度 +} + +/*---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的参数定义 + SPEED = 200; + VWGetPosition(&x_1, &y_1, &phi_1); //获取机器人在移动前的x,y,phi值 + + do + { + L_PSD = PSDGet(PSD_LEFT); //读取左侧和墙壁的距离 + R_PSD = PSDGet(PSD_RIGHT); //读取右侧和墙壁的距离 + F_PSD = PSDGet(PSD_FRONT); //读取前方和墙壁的距离 + /*---打印输出PSD数值---*/ + // cout << "PSD值L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << "\t" ; + /*---打印结束---*/ + + if (100 (DIST_cell -50)) //当移动距离只剩下最后的50mm的时候降点速度 + { + SPEED = SPEED2; + } + }while (DIST_move < DIST_cell && F_PSD > DIST_wall); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行 +} + +/*---行驶到指定单元格---*/ +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，则执行旋转命令 + { + VWTurn(turn*90,45); + VWWait(); + } + PIDStraight(); //使用PID算法算法，向指定方向直线行驶一格 + VWWait(); //等待下一条指令 + + rob_dir = maze_dir; //更新机器人移动到下一格之后的方向 + xneighbor(rob_x,rob_dir); //更新机器人移动到下一格之后的X坐标 + yneighbor(rob_y,rob_dir); //更新机器人移动到下一格之后的y坐标 +} + +/*---检查该方向是否已被标记---*/ +bool unmarked(int x, int y, int dir) +{ + bool check = false; + int maze_dir = (dir + 4) % 4; + switch(maze_dir) + { + case 0: + if (mark[x][y+1] == 0) + { + check = true; + } + break; + + case 1: + if (mark[x-1][y] == 0) + { + check = true; + } + break; + + case 2: + if (mark[x][y-1] == 0) + { + check = true; + } + break; + + case 3: + 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; //将当前的单元格设置为已到访 + L_open = PSDGet(PSD_LEFT) > DIST_cell; //获取左侧空间状况 + F_open = PSDGet(PSD_FRONT) > DIST_cell; //获取前侧空间状况 + R_open = PSDGet(PSD_RIGHT) > 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/编写代码/maze_func_flood.cpp b/编写代码/maze_func_flood.cpp new file mode 100644 index 0000000..1a08d5b --- /dev/null +++ b/编写代码/maze_func_flood.cpp @@ -0,0 +1,64 @@ +#include +#include +#include"eyebot++.h" +#include"maze_func.h" +#include"maze_parameter.h" +using namespace std; + +void flood() +{ + int num = 0; + + do + { + num ++; + for (i = 1; i < mazesize_x - 1; i++) + { + for (j = 1; j < mazesize_y - 1; j++) + { + if (map[i][j] == -1) + { + if (i > 1) + { + if (! wall[i][j][1] && map[i-1][j] != -1) + { + copy_map[i][j] = map[i-1][j] + 1; + } + } + + if (i < mazesize_x - 2) + { + if (! wall[i+1][j][1] && map[i+1][j] != -1) + { + copy_map[i][j] = map[i+1][j] = 1; + } + } + + if (j > 1) + { + if (! wall[i][j][0] && map[i][j-1] != -1) + { + copy_map[i][j] = map[i][j-1] + 1; + } + } + + if (j < mazesize_y - 2) + { + if (! wall[i][j+1][0] && map[i][j+1] != -1) + { + copy_map[i][j] = map[i][j+1] + 1; + } + } + } + } + } + + for (i = 0; i < mazesize_x; i++) + { + for (j = 0; j < mazesize_y; j++) + { + map[i][j] = copy_map[i][j]; + } + } + } while (map[target_x][target_y] == -1 && num < ((mazesize_x - 2) * (mazesize_y - 2))) +} \ No newline at end of file diff --git a/编写代码/maze_func_output.cpp b/编写代码/maze_func_output.cpp new file mode 100644 index 0000000..3dbf17b --- /dev/null +++ b/编写代码/maze_func_output.cpp @@ -0,0 +1,52 @@ +#include +#include +#include"eyebot++.h" +#include"maze_func.h" +#include"maze_parameter.h" +using namespace std; + + +/*---打印数组检查错误---*/ +/*---打印数组mark---*/ +void output_mark() +{ + for (int i = 5; i >= 0; i--) + { + for (int j = 0; j < 6; j++) + { + cout << mark[j][i] ; + } + cout << endl; + } +} + +/*---打印数组wall---*/ +void output_wall() +{ + for (int i = 6; i >= 0; i--) + { + for (int j = 0; j < 7; j++) + { + if (wall[j][i][1] == 1) + { + cout << "|" ; + } + else + { + cout << " " ; + } + + if (wall[j][i][0] == 1) + { + cout << "_" ; + } + else + { + cout << " " ; + } + } + cout << endl; + } +} + +/*---打印数组map---*/ \ No newline at end of file diff --git a/编写代码/maze_func_path.cpp b/编写代码/maze_func_path.cpp new file mode 100644 index 0000000..bec5b02 --- /dev/null +++ b/编写代码/maze_func_path.cpp @@ -0,0 +1,6 @@ +#include +#include +#include"eyebot++.h" +#include"maze_func.h" +#include"maze_parameter.h" +using namespace std; \ No newline at end of file diff --git a/编写代码/maze_main.cpp b/编写代码/maze_main.cpp new file mode 100644 index 0000000..6dc7e58 --- /dev/null +++ b/编写代码/maze_main.cpp @@ -0,0 +1,13 @@ +#include +#include"maze_func.h" +#include"maze_parameter.h" +using namespace std; + + + +/*---下面是主函数---*/ +int main() +{ + explore(); + output(); +} diff --git a/编写代码/maze_parameter.cpp b/编写代码/maze_parameter.cpp new file mode 100644 index 0000000..bb3f65d --- /dev/null +++ b/编写代码/maze_parameter.cpp @@ -0,0 +1,42 @@ +#include +#include +#include"eyebot++.h" +#include"maze_func.h" +#include"maze_parameter.h" +using namespace std; + + +/*---定义全局常量---*/ +const int DIST_cell = 360 ; +const int DIST_wall = 130 ; +// const int mazesize_x = 6 ; +// const int mazesize_y = 6 ; +/*---全局各种常量定义完成---*/ + +/*---定义全局变量---*/ +/*---数组类---*/ +int mark[6][6] = {}; //1=为已访问 +int wall[7][7][2] = {}; //1=墙壁；0=通路 +int map[6][6] = {}; //最短路径求解地图 +int copy_map[6][6] = {}; //copy复制地图 +int path[6][6] = {}; //最短路径结果地图 +// int mark[mazesize_x][mazesize_y] = {}; //1=为已访问 +// int wall[mazesize_x + 1][mazesize_y + 1][2] = {}; //1=墙壁；0=通路 +//wall[1][1][0] = 1; //定义迷宫的初始小车背后为墙 +// int map[mazesize_x][mazesize_y]; //最短路径求解地图 +// int copy_map[mazesize_x][mazesize_y]; //copy复制地图 +// int path[mazesize_x][mazesize_y]; //最短路径结果地图 +/*---机器人坐标---*/ +int rob_x = 1 ; //机械人初始的X坐标 +int rob_y = 1 ; //机器人初始的Y坐标 +int rob_dir = 0 ; //机器人初始的朝向 + +/*---递归算法---*/ +/*---PID算法---*/ +const float Kp = 1, Ki = 0, Kd = 0.5; //定义PID的参数 +float Kpid; //定义PID计算后输出的倍率 +const int Alpha = 2, SPEED = 200; //定义基本角速度和速度 +const int SPEED2 = 0.25*SPEED; //快到目的地时减速 +int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差 +const int eI_max = 20; //定义积分限幅 +/*---全局各种变量定义完成---*/ \ No newline at end of file diff --git a/编写代码/maze_parameter.h b/编写代码/maze_parameter.h new file mode 100644 index 0000000..45f68f9 --- /dev/null +++ b/编写代码/maze_parameter.h @@ -0,0 +1,34 @@ +#pragma once + +/*---声明数据---*/ +/*---声明全局常量---*/ +extern const int DIST_cell; //单元格长度 +extern const int DIST_wall; //与墙壁的距离 +extern const int mazesize_x; //迷宫X轴长度 +extern const int mazesize_y; //迷宫Y轴高度 +/*---声明全局变量---*/ +/*---声明数组类---*/ +extern int mark[6][6]; //迷宫单元格是否访问数组 +extern int wall[7][7][2]; //迷宫墙壁信息数组 +extern int map[6][6]; //迷宫地图数组 +extern int copy_map[6][6]; //迷宫地图复制数组 +extern int path[6][6]; +/*---声明机器人坐标---*/ +extern int rob_x; +extern int rob_y; +extern int rob_dir; +/*---各种全局量声明结束---*/ + +/*---递归算法---*/ +/*---PID算法---*/ +extern const float Kp, Ki, Kd; //声明PID的参数 +extern float Kpid; //定义PID计算后输出的倍率 +extern const int Alpha, SPEED; //定义基本角速度和速度 +extern const int SPEED2; //快到目的地时减速 +extern int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差 +extern const int eI_max = 20; //定义积分限幅 + + + + +/*---全局各种变量声明完成---*/ \ No newline at end of file diff --git a/编写代码/全局变量.txt b/编写代码/全局变量.txt new file mode 100644 index 0000000..8344412 --- /dev/null +++ b/编写代码/全局变量.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/编写代码/名称解释.txt b/编写代码/名称解释.txt new file mode 100644 index 0000000..b13245e --- /dev/null +++ b/编写代码/名称解释.txt @@ -0,0 +1,5 @@ +头文件:mazetool.h +函数文件:mazetool.cpp +全局: +rob_y:机器人在迷宫地图中的Y坐标值 +rob_x:机器人在迷宫地图中的x坐标值