diff --git a/maze_code/makefile b/maze_code/makefile new file mode 100644 index 0000000..300ce18 --- /dev/null +++ b/maze_code/makefile @@ -0,0 +1,19 @@ + +cxx := gccarm +file_cpp := $(wildcard *.cpp) +file_o := maze.x + +$(file_o) : $(file_cpp) + @$(cxx) $^ -O $@ + +maze : $(file_o) + + +debug : + @echo $(cxx) + @echo $(file_cpp) + @echo $(file_o) + + + +.PHONY : maze debug \ No newline at end of file diff --git a/maze_code/maze_func.h b/maze_code/maze_func.h new file mode 100644 index 0000000..9962a32 --- /dev/null +++ b/maze_code/maze_func.h @@ -0,0 +1,45 @@ +#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_AL(); //位置式PID算法 +void PIDStraight(); //PID控制下的直线行驶 +void go_to(int dir); //行驶到指定单元格 +/*---以上为pid控制和行驶部分涉及到的函数声明---*/ + +/*---PART3---*/ +/*---以下为洪水填充算法部分涉及到的函数声明---*/ +void flood(int *map, int *copy_map, int *copy_wall); //洪水填充算法 +/*---以上为洪水填充算法部分涉及到的函数声明---*/ + +/*---PART4---*/ +/*---以下为路径算法部分涉及到的函数声明---*/ +void build_path(int i, int j, int len, int *path, int *copy_wall); //路径算法 +void drive_path(int len, int *path); //移动到目的地 +/*---以上为路径算法部分涉及到的函数声明---*/ + +/*---PART5---*/ +/*---以下为数组构建部分涉及到的函数声明---*/ +void array_negative_one(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 *arr); //打印所有二维数组数组 +void output_arrwall(int *arr); //打印wall数组 +void output_arrpath(int *arr); //打印path数组 +/*---以上为数组打印部分涉及到的函数声明---*/ + diff --git a/maze_code/maze_func_array.cpp b/maze_code/maze_func_array.cpp new file mode 100644 index 0000000..b1dec65 --- /dev/null +++ b/maze_code/maze_func_array.cpp @@ -0,0 +1,49 @@ +#include +#include +#include +#include"eyebot++.h" +#include"maze_func.h" +#include"maze_parameter.h" +using namespace std; + +/*---将二维数组初始化为-1---*/ +void array_negative_one(int *arr) +{ + int rows = sizeof(arr) / sizeof(arr[0]); //获取X轴长度 + int cols = sizeof(arr[0]) / sizeof(arr[0][0]); //获取Y轴长度 + + for (i = 0; i < rows; i++) + { + for (j = 0; j < cols; j++) + { + arr[i][j] = -1; + } + } +} + +/*---从vector容器复制mark信息到数组函数---*/ +void array_copy_mark(int size_x, int size_y, int *copy_mark) +{ + for (i = 0; i < size_x; i++) + { + for (j = 0; j < size_y; j++) + { + copy_mark[i][j] = mark[i][j]; + } + } +} + +/*---从vector容器复制wall信息到数组函数---*/ +void array_copy_wall(int size_x, int size_y, int *copy_wall) +{ + for (i = 0; i <= size_x; i++) + { + for (j = 0; j <= size_y; j++) + { + for(k = 0; k < 2; k++) + { + copy_wall[i][j][k] = wall[i][j][k]; + } + } + } +} diff --git a/maze_code/maze_func_explore.cpp b/maze_code/maze_func_explore.cpp new file mode 100644 index 0000000..13b12ea --- /dev/null +++ b/maze_code/maze_func_explore.cpp @@ -0,0 +1,271 @@ +#include +#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坐标 + 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 && y>0) //情况1 + { + if (y == size_y - 1) //N + { + N_mark = true; + } + else + { + N_mark = mark[x][y+1]; + } + W_mark = mark[x-1][y]; //W + S_mark = mark[x][y-1]; //S + if (x == size_x -1) //E + { + E_mark = true; + } + else + { + E_mark = mark[x+1][y]; + } + } + else if (x==0 && y>0) //情况2 + { + if (y == size_y - 1) //N + { + N_mark = true; + } + else + { + N_mark = mark[x][y+1]; + } + W_mark = true; //W + S_mark = mark[x][y-1]; //S + if (x == size_x - 1) //E + { + E_mark = true; + } + else + { + E_mark = mark[x+1][y]; + } + } + else if (x>0 && y==0) //情况3 + { + if (y == size_y - 1) //N + { + N_mark = true; + } + else + { + N_mark = mark[x][y+1]; + } + W_mark = mark[x-1][y]; //W + S_mark = true; //S + if (x == size_x -1) //E + { + E_mark = true; + } + else + { + E_mark = mark[x+1][y]; + } + } + else //(x==0 && y==0) + { + if (y == size_y - 1) //N + { + N_mark = true; + } + else + { + N_mark = mark[x][y+1]; + } + W_mark = true; //W + S_mark = true; //S + if (x == size_x -1) //E + { + E_mark = true; + } + else + { + E_mark = mark[x+1][y]; + } + } + + 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 == mark[0].size() - 1 && open) //检测是否超出容器范围，如果超出，则扩大容器：对每一个内层尾插数据 + { + for (i = 0; i < mark.size() - 1; i++) + { + mark[i].emplace_back(0); + } + for (i = 0; i < wall.size() - 1; i++) + { + wall[i].emplace_back(vector(2,0)); + } + } + wall[x][y+1][0] = ! open; + break; + + case 1: //1表示西(左)，表示(x，y)坐标单元格的左侧墙壁信息 + if (x == 0 && open) //检测是否超出容器范围，如果超出，则扩大容器：头插一个内层 + { + mark.emplace(mark.begin(), mark[0].size(), 0); + wall.emplace(wall.begin(), wall[0].size(), vector(2,0)); + } + wall[x][y][1] = ! open; + break; + + case 2: //2表示南(下)，表示(x，y)坐标单元格的下侧墙壁信息 + if (y == 0 && open) //检测是否超出容器范围，如果超出，则扩大容器：对每一个内层头插数据 + { + for (i = 0; i < mark.size() - 1; i++) + { + mark[i].emplace(mark[i].begin(), 0); + } + for (i = 0; i < wall.size() - 1; i++) + { + wall[i].emplace(wall[i].begin(), vector(2,0)); + } + } + wall[x][y][0] = ! open; + break; + + case 3: //3表示东(右)，表示(x+1，y)坐标单元格的左侧墙壁信息 + if (x == mark.size() - 1 && open) //检测是否超出容器范围，如果超出，则扩大容器：尾插一个内层 + { + mark.emplace_back(mark[0].size(), 0); + wall.emplace_back(wall[0].size(), vector(2,0)); + } + 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 < mark[0].size() - 2) + { + if (mark[x][y+1] == 0) + { + check = true; + } + } + break; + + case 1: + if (x > 1) + { + if (mark[x-1][y] == 0) + { + check = true; + } + } + break; + + case 2: + if (y > 1) + { + if (mark[x][y-1] == 0) + { + check = true; + } + } + break; + + case 3: + if (x < mark.size() - 2) + { + 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/maze_code/maze_func_flood.cpp b/maze_code/maze_func_flood.cpp new file mode 100644 index 0000000..98e7b31 --- /dev/null +++ b/maze_code/maze_func_flood.cpp @@ -0,0 +1,72 @@ +#include +#include +#include +#include"eyebot++.h" +#include"maze_func.h" +#include"maze_parameter.h" +using namespace std; + +void flood(int *map, int *copy_map, int *copy_wall) +{ + int num = 0; + int size_x = mark.size(); + int size_y = mark[0].size(); + map[0][0] = 0; + copy_map[0][0] = 0; + + do + { + for (i = 0; i < size_x; i++) + { + for (j = 0; j < size_y; j++) + { + if (! map[i][j] == -1) + { + if (i > 0) + { + if (! copy_wall[i][j][1] && map[i-1][j] = -1) //左边格子 + { + copy_map[i-1][j] = map[i][j] + 1; + num++; + } + } + + if (i < size_x - 1) + { + if (! copy_wall[i+1][j][1] && map[i+1][j] = -1) //右边格子 + { + copy_map[i+1][j] = map[i][j] + 1; + num++; + } + } + + if (j > 0) + { + if (! copy_wall[i][j][0] && map[i][j-1] = -1) + { + copy_map[i][j-1] = map[i][j] + 1; + num++; + } + } + + if (j < size_y - 1) + { + if (! copy_wall[i][j+1][0] && map[i][j+1] = -1) + { + copy_map[i][j+1] = map[i][j] + 1; + num++; + } + } + } + } + } + + for (i = 0; i < size_x; i++) + { + for (j = 0; j < size_y; j++) + { + map[i][j] = copy_map[i][j]; + } + } + } while (map[target_x][target_y] == -1 && num < (size_x * size_y)) +} \ No newline at end of file diff --git a/maze_code/maze_func_goto.cpp b/maze_code/maze_func_goto.cpp new file mode 100644 index 0000000..22a93ff --- /dev/null +++ b/maze_code/maze_func_goto.cpp @@ -0,0 +1,84 @@ +#include +#include +#include +#include"eyebot++.h" +#include"maze_func.h" +#include"maze_parameter.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，则执行旋转命令 + { + VWTurn(turn*90,45); + 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/maze_code/maze_func_output.cpp b/maze_code/maze_func_output.cpp new file mode 100644 index 0000000..82d9edb --- /dev/null +++ b/maze_code/maze_func_output.cpp @@ -0,0 +1,69 @@ +#include +#include +#include +#include"eyebot++.h" +#include"maze_func.h" +#include"maze_parameter.h" +using namespace std; + + +/*---打印数组检查错误---*/ +/*---打印数组mark,map,copy_map等二维数组所用的函数---*/ +void output_arr2D(int *arr) +{ + int rows = sizeof(arr) / sizeof(arr[0]); + int cols = sizeof(arr[0]) / sizeof(arr[0][0]); + + for (int i = cols - 1; i >= 0; i--) + { + for (int j = 0; j < rows; j++) + { + cout << arr[j][i] << " "; + } + cout << endl; + } +} + +/*---打印数组wall---*/ +void output_arrwall(int *arr) +{ + int rows = sizeof(arr) / sizeof(arr[0]); + int cols = sizeof(arr[0]) / sizeof(arr[0][0]); + + for (int i = cols - 1; i >= 0; i--) + { + for (int j = 0; j < rows; j++) + { + if (arr[j][i][1] == 1) + { + cout << "|" ; + } + else + { + cout << " " ; + } + + if (arr[j][i][0] == 1) + { + cout << "_" ; + } + else + { + cout << " " ; + } + } + cout << endl; + } +} + +/*---打印数组path---*/ +void output_arrpath(int *arr) +{ + int size = sizeof(arr) / sizeof(arr[0]); + + for (i = 0; i < size; i++) + { + cout << arr[i] << " "; + } + cout << endl; +} \ No newline at end of file diff --git a/maze_code/maze_func_path.cpp b/maze_code/maze_func_path.cpp new file mode 100644 index 0000000..5a199ed --- /dev/null +++ b/maze_code/maze_func_path.cpp @@ -0,0 +1,52 @@ +#include +#include +#include +#include"eyebot++.h" +#include"maze_func.h" +#include"maze_parameter.h" +using namespace std; + +/*---解迷宫路径到path数组函数---*/ +void build_path(int i, int j, int len, int *path, int *copy_wall) +{ + int k; + int size_x = mark.size(); + int size_y = mark[0].size(); + + for (k = len -1; k >= 0; k--) + { + if (i > 0 && ! copy_wall[i][j][1] && map[i-1][j] == k) //前一个单位的东面是当前格，即当前格子的左侧为上一个格子 + { + i--; + path[k] = 3; + } + else if (i < size_x - 1 && ! copy_wall[i+1][j][1] && map[i+1][j] == k) //前一个单位的西面是当前格，即当前格子的右侧为上一个格子 + { + i++; + path[k] = 1; + } + else if(j > 0 && ! copy_wall[i][j][0] && map[i][j-1] == k) //前一个单位的北面是当前格，即当前格子的下侧为上一个格子 + { + j--; + path[k] = 0; + } + else if (j < size_y - 1 && ! copy_wall[i][j+1][0] && map[i][j+1] == k) //前一个单位的南面是当前格，即当前格子的上侧为上一个格子 + { + j++; + path[k] = 2; + } + else + { + LCDPrintf("ERROR"); + } + } +} + +/*---通过path数组移动到目标点函数---*/ +void drive_path(int len, int *path) +{ + for(i = 0; i < len; i++) + { + go_to(path[i]); + } +} diff --git a/maze_code/maze_func_pid.cpp b/maze_code/maze_func_pid.cpp new file mode 100644 index 0000000..7a91f54 --- /dev/null +++ b/maze_code/maze_func_pid.cpp @@ -0,0 +1,102 @@ +#include +#include +#include +#include"eyebot++.h" +#include"maze_func.h" +#include"maze_parameter.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 + { + //此处为空; + } +} + +/*---位置式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控制的各项参数2---*/ + // cout << "PID输出值:" << Kpid << endl ; + /*---打印结束---*/ + // VWSetSpeed(speed,Kpid*Kpid_base); //控制小车的行驶速度，角速度(模拟器可以用，实物no) + MOTORDriveRaw(1,speed1); + MOTORDriveRaw(2,speed1+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,speed1); + + do + { + F_PSD = PSDGetRaw(1); //读取前方和墙壁的距离 + L_PSD = PSDGetRaw(2); //读取左侧和墙壁的距离 + R_PSD = PSDGetRaw(3); //读取右侧和墙壁的距离 + + /*---打印输出PSD数值---*/ + // 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); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行 +} diff --git a/maze_code/maze_main.cpp b/maze_code/maze_main.cpp new file mode 100644 index 0000000..c852ca6 --- /dev/null +++ b/maze_code/maze_main.cpp @@ -0,0 +1,45 @@ +#include +#include +#include +#include"eyebot++.h" +#include"maze_func.h" +#include"maze_parameter.h" +using namespace std; + + +/*---下面是主函数---*/ +int main() +{ + explore(); //使用递归算法构建地图 + + 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] = {}; //复制一份wall数组，便于后续频繁读取提升性能并初始化为0 + + array_copy_wall(mazesize_x, mazesize_y, copy_mark); //复制一份mark数组，便于后续频繁读取提升性能 + array_copy_wall(mazesize_x, mazesize_y, copy_wall); //复制一份wall数组，便于后续频繁读取提升性能 + + output_arr2D(copy_mark); //打印地图mark信息 + output_arrwall(copy_wall); //打印墙壁wall信息 + + int map[mazesize_x][mazesize_y] = {}; //创建最短路径求解地图并初始化为0 + int copy_map[mazesize_x][mazesize_y] = {}; //创建copy复制地图并初始化为0 + + array_negative_one(map); //将map数组初始化为-1 + array_negative_one(copy_map); //将copy_map数组初始化为-1 + + flood(map, copy_map, copy_mark, copy_wall); //洪水填充算法 + + output_arr2D(map); //打印map的数组信息 + + const int len = map[target_x][target_y]; //迷宫的终点（人为确定） + int path[len] = {}; //创建最短路径结果地图并初始化为0 + + build_path(target_x, target_y, len, path, copy_wall); //构建出最短路径path数组 + drive_path(len, path); //移动到指定目标点 + + output_arrpath(path); //打印path数组信息，及小车每一步的行驶方向 + +} diff --git a/maze_code/maze_parameter.cpp b/maze_code/maze_parameter.cpp new file mode 100644 index 0000000..9f5f539 --- /dev/null +++ b/maze_code/maze_parameter.cpp @@ -0,0 +1,54 @@ +#include +#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 ; //与墙壁的距离 + +/*---定义全局变量---*/ +/*---容器类---*/ +vector> mark(1,vector(1,0)); //定义mark容器（二维容器），容器初始为1行1列 1=为已访问 +vector>> wall(1,vector>(1,vector(2,0))); //定义wall容器（三维容器），容器初始为1行1列，且每个格子内带有2个墙壁信息 1=墙壁；0=通路 + +/*---数组类---*/ +// int mark[4][4] = {}; //1=为已访问 +// int wall[5][5][2] = {}; //1=墙壁；0=通路 +// int map[4][4] = {}; //最短路径求解地图 +// int copy_map[4][4] = {}; //copy复制地图 +// int path[4][4] = {}; //最短路径结果地图 +// 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_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 = 1, Ki = 0, Kd = 0.5; //定义PID的参数 +float Kpid; //定义PID计算后输出的倍率 +const int Kpid_base = 2, speed = 24; //定义基本角速度和速度 +const int speed2 = 0.75*speed; //快到目的地时减速 +int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差 +const int eI_max = 20; //定义积分限幅 + +/*---目标点的坐标---*/ +const int target_x = 4; //声明目标点的X坐标 +const int target_y = 4; //声明目标点的Y坐标 + +/*---定义结束---*/ \ No newline at end of file diff --git a/maze_code/maze_parameter.h b/maze_code/maze_parameter.h new file mode 100644 index 0000000..0ff8f0f --- /dev/null +++ b/maze_code/maze_parameter.h @@ -0,0 +1,46 @@ +#pragma once +#include + +/*---声明数据---*/ +/*---声明全局常量---*/ +extern const int DIST_cell; //单元格长度 +extern const int DIST_wall; //与墙壁的距离 + +/*---声明全局变量---*/ + +/*---声明全局容器---*/ +extern vector> mark; //声明mark容器（二维容器） +extern vector>> wall; //声明wall容器（三维容器） + +/*---声明数组类---*/ +// 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_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 float Kpid_speed; //定义计算后的速度 +extern const int Kpid_base, speed; //定义基本角速度和速度 +extern const int speed1,speed2; //快到目的地时减速 +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/maze_code/全局变量.txt b/maze_code/全局变量.txt new file mode 100644 index 0000000..8344412 --- /dev/null +++ b/maze_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/maze_code/名称解释.txt b/maze_code/名称解释.txt new file mode 100644 index 0000000..b13245e --- /dev/null +++ b/maze_code/名称解释.txt @@ -0,0 +1,5 @@ +头文件:mazetool.h +函数文件:mazetool.cpp +全局: +rob_y:机器人在迷宫地图中的Y坐标值 +rob_x:机器人在迷宫地图中的x坐标值