yyz/bk_bishe_pi
1
0
Fork 0
mirror of https://gitee.com/yyz_o/bk_bishe_pi.git synced 2025-09-07 23:21:26 +00:00
Code Issues Packages Projects Releases Wiki Activity

2025.5.15第三次提交，继续修改

Browse Source
This commit is contained in:
氧原子
2025-05-15 22:40:18 +08:00
parent 75f7336601
commit a81594bf68
8 changed files with 63 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
maze_code/maze_func_array.cpp
View File

@@ -27,7 +27,7 @@ void array_copy_wall(int size_x, int size_y, int *copy_wall)
{ {
for(int k = 0; k < 2; k++) 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];
} }
} }
} }

30
maze_code/maze_func_flood.cpp
View File

@@ -25,36 +25,36 @@ void flood(int *map, int *copy_map, int *copy_wall)
{ {
if (i > 0) 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++; num++;
} }
} }
if (i < size_x - 1) 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++; num++;
} }
} }
if (j > 0) 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++; num++;
} }
} }
if (j < size_y - 1) 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++; 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 i = 0; i < size_x; i++)
{ // {
for (int j = 0; j < size_y; j++) // for (int j = 0; j < size_y; j++)
{ // {
map[(i * size_y) + j] = copy_map[(i * 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)); } while (map[(target_x * size_y) + target_y] == -1 && num < (size_x * size_y));
} }

8
maze_code/maze_func_output.cpp
View File

@@ -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) 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 << "|" ; cout << "|" ;
} }
@@ -39,7 +39,7 @@ void output_arrwall(int size_x, int size_y, int *arr)
cout << " " ; cout << " " ;
} }
if (arr[(j * size_y * 2) + (i * 2) + 0] == 1) if (arr[(j * (size_y + 1) * 2) + (i * 2) + 0] == 1)
{ {
cout << "_" ; cout << "_" ;
} }

8
maze_code/maze_func_path.cpp
View File

@@ -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--) 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--; i--;
path[k] = 3; 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++; i++;
path[k] = 1; 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--; j--;
path[k] = 0; 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++; j++;
path[k] = 2; path[k] = 2;

33
maze_code/maze_func_pid.cpp
View File

@@ -31,17 +31,17 @@ void PID_AL()
eI_xianfu(eI_max); //积分部分限幅 eI_xianfu(eI_max); //积分部分限幅
err_diff = err - err_old; //微分部分误差值 err_diff = err - err_old; //微分部分误差值
/*---打印输出PID控制的各项参数1---*/ /*---打印输出PID控制的各项参数1---*/
// cout << "本次误差:" << err << "\t" ; cout << "本次误差:" << err << "\t" ;
// cout << "上次误差:" << err_old << "\t" ; cout << "上次误差:" << err_old << "\t" ;
// cout << "误差累计:" << err_sum << "\t" ; cout << "误差累计:" << err_sum << "\t" ;
// cout << "误差变化:" << err_diff << "\t" ; cout << "误差变化:" << err_diff << "\t" ;
/*---打印结束---*/ /*---打印结束---*/
err_old = err; //将误差幅值到上一次误差 err_old = err; //将误差幅值到上一次误差
Kpid = Kp*err + Ki*err_sum + Kd*err_diff; //PID控制算法输出的倍率 Kpid = Kp*err + Ki*err_sum + Kd*err_diff; //PID控制算法输出的倍率
Kpid_speed = Kpid * Kpid_base; Kpid_speed = Kpid * Kpid_base;
/*---打印输出PID控制的各项参数2---*/ /*---打印输出PID控制的各项参数2---*/
// cout << "PID输出值:" << Kpid << endl ; cout << "PID输出值:" << Kpid_speed << "\t" ;
/*---打印结束---*/ /*---打印结束---*/
// VWSetSpeed(speed,Kpid*Kpid_base); //控制小车的行驶速度，角速度(模拟器可以用，实物no) // VWSetSpeed(speed,Kpid*Kpid_base); //控制小车的行驶速度，角速度(模拟器可以用，实物no)
MOTORDriveRaw(1,speed1); MOTORDriveRaw(1,speed1);
@@ -70,19 +70,19 @@ void PIDStraight()
// cout << "PSD值L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << "\t" ; // cout << "PSD值L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << "\t" ;
/*---打印结束---*/ /*---打印结束---*/
if (100<L_PSD && L_PSD<180 && 100<R_PSD && R_PSD<180) //如果两侧都有墙的情况 if (30<L_PSD && L_PSD<180 && 30<R_PSD && R_PSD<180) //如果两侧都有墙的情况
{ {
err = L_PSD - R_PSD; err = L_PSD - R_PSD;
PID_AL(); PID_AL();
} }
else if (100<L_PSD && L_PSD<180) //只有左侧有墙的情况 else if (30<L_PSD && L_PSD<180) //只有左侧有墙的情况
{ {
err = L_PSD - DIST_wall; err = L_PSD - DIST_wall_RL;
PID_AL(); PID_AL();
} }
else if (100<R_PSD && R_PSD<180) //只有右侧有墙的情况 else if (30<R_PSD && R_PSD<180) //只有右侧有墙的情况
{ {
err = DIST_wall - R_PSD; err = DIST_wall_RL - R_PSD;
PID_AL(); PID_AL();
} }
else else
@@ -94,9 +94,12 @@ void PIDStraight()
} }
VWGetPosition(&x_2, &y_2, &phi_2); VWGetPosition(&x_2, &y_2, &phi_2);
DIST_move = sqrt(pow(x_2-x_1,2) + pow(y_2-y_1,2)); DIST_move = sqrt(pow(x_2-x_1,2) + pow(y_2-y_1,2));
if (DIST_move > (DIST_cell -50)) //当移动距离只剩下最后的50mm的时候降点速度 cout << "move:" << DIST_move << endl;
{ // if (DIST_move > (DIST_cell -50)) //当移动距离只剩下最后的50mm的时候降点速度
speed1 = speed2; // {
} // speed1 = speed2;
}while (DIST_move < DIST_cell && F_PSD > DIST_wall); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行 // }
}while (DIST_move < DIST_cell && F_PSD > DIST_wall_F); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
MOTORDriveRaw(1,0);
MOTORDriveRaw(2,0);
} }

3
maze_code/maze_main.cpp
View File

@@ -12,6 +12,7 @@ using namespace std;
int main() int main()
{ {
explore(); //使用递归算法构建地图 explore(); //使用递归算法构建地图
wall[0][0][0] = 1;
const int mazesize_x = mark.size(); //获取探索结束之后的迷宫的X轴长度 const int mazesize_x = mark.size(); //获取探索结束之后的迷宫的X轴长度
const int mazesize_y = mark[0].size(); //获取探索结束之后的迷宫的Y轴长度 const int mazesize_y = mark[0].size(); //获取探索结束之后的迷宫的Y轴长度
@@ -23,7 +24,7 @@ int main()
array_copy_wall(mazesize_x, mazesize_y, copy_wall); //复制一份wall数组，并转换为一维数组，便于后续频繁读取提升性能 array_copy_wall(mazesize_x, mazesize_y, copy_wall); //复制一份wall数组，并转换为一维数组，便于后续频繁读取提升性能
output_arr2D(mazesize_x, mazesize_y, copy_mark); //打印地图mark信息 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 map[mazesize_x * mazesize_y] = {}; //创建最短路径求解地图并初始化为0
int copy_map[mazesize_x * mazesize_y] = {}; //创建copy复制地图并初始化为0 int copy_map[mazesize_x * mazesize_y] = {}; //创建copy复制地图并初始化为0

16
maze_code/maze_parameter.cpp
View File

@@ -9,13 +9,14 @@ using namespace std;
/*---定义数据---*/ /*---定义数据---*/
/*---定义全局常量---*/ /*---定义全局常量---*/
const int DIST_cell = 360 ; //单元格长度 const int DIST_cell = 300 ; //单元格长度
const int DIST_wall = 130 ; //与墙壁的距离 const int DIST_wall_F = 90 ; //与墙壁的距离
extern const int DIST_wall_RL = 100; //与左右墙壁的距离
/*---定义全局变量---*/ /*---定义全局变量---*/
/*---容器类---*/ /*---容器类---*/
vector<vector<int>> mark(1,vector<int>(1,0)); //定义mark容器（二维容器），容器初始为1行1列 1=为已访问 std::vector<std::vector<int>> mark(1,std::vector<int>(1,0)); //定义mark容器（二维容器），容器初始为1行1列 1=为已访问
vector<vector<vector<int>>> wall(2,vector<vector<int>>(2,vector<int>(2,0))); //定义wall容器（三维容器），容器初始为1行1列，且每个格子内带有2个墙壁信息 1=墙壁；0=通路 std::vector<std::vector<std::vector<int>>> wall(2,std::vector<std::vector<int>>(2,std::vector<int>(2,0))); //定义wall容器（三维容器），容器初始为1行1列，且每个格子内带有2个墙壁信息 1=墙壁；0=通路
/*---机器人坐标---*/ /*---机器人坐标---*/
int rob_x0 = 0; //机器人起点X坐标 int rob_x0 = 0; //机器人起点X坐标
@@ -28,10 +29,11 @@ int rob_dir = 0; //机器人当前的朝向（初始为0）
/*---PID算法---*/ /*---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; //定义PID计算后输出的倍率
float Kpid_speed; int Kpid_speed;
const int Kpid_base = 2, speed = 24; //定义基本角速度和速度 const float Kpid_base = 0.1; //定义基本角速度和速度
const int speed = 50; //定义基本角速度和速度
const int speed2 = 0.75*speed; //快到目的地时减速 const int speed2 = 0.75*speed; //快到目的地时减速
int speed1; int speed1;
int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差 int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差

8
maze_code/maze_parameter.h
View File

@@ -4,7 +4,8 @@
/*---声明数据---*/ /*---声明数据---*/
/*---声明全局常量---*/ /*---声明全局常量---*/
extern const int DIST_cell; //单元格长度 extern const int DIST_cell; //单元格长度
extern const int DIST_wall; //与墙壁的距离 extern const int DIST_wall_F; //与前面墙壁的距离
extern const int DIST_wall_RL; //与左右墙壁的距离
/*---声明全局变量---*/ /*---声明全局变量---*/
@@ -26,8 +27,9 @@ extern int rob_dir; //机器人当前的朝向
/*---PID算法---*/ /*---PID算法---*/
extern const float Kp, Ki, Kd; //声明PID的参数 extern const float Kp, Ki, Kd; //声明PID的参数
extern float Kpid; //定义PID计算后输出的倍率 extern float Kpid; //定义PID计算后输出的倍率
extern float Kpid_speed; //定义计算后的速度 extern int Kpid_speed; //定义计算后的速度
extern const int Kpid_base, speed; //定义基本角速度和速度 extern const float Kpid_base; //定义速度基础倍数速度
extern const int speed; //定义速度基础倍数速度
extern const int speed2; //快到目的地时减速 extern const int speed2; //快到目的地时减速
extern int speed1; extern int speed1;
extern int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差 extern int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差