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

1

Browse Source
This commit is contained in:
Ñõԭ×Ó
2025-05-27 14:57:30 +08:00
parent 00e0d9366d
commit 9d96af2ff3
6 changed files with 45 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
bot_code_turn/maze_func_flood.cpp
View File

@@ -15,6 +15,7 @@ void flood(int *map, int *copy_wall)
do do
{ {
num++;
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++)
@@ -26,7 +27,7 @@ void flood(int *map, int *copy_wall)
if (!copy_wall[(i * (size_y + 1) * 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) //左边格子
{ {
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++;
} }
} }
@@ -35,7 +36,7 @@ void flood(int *map, int *copy_wall)
if (!copy_wall[((i+1) * (size_y + 1) * 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) //右边格子
{ {
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++;
} }
} }
@@ -44,7 +45,7 @@ void flood(int *map, int *copy_wall)
if (!copy_wall[(i * (size_y + 1) * 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)
{ {
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++;
} }
} }
@@ -53,11 +54,11 @@ void flood(int *map, int *copy_wall)
if (!copy_wall[(i * (size_y + 1) * 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)
{ {
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++;
} }
} }
} }
} }
} }
} while (map[((target_x - 1) * size_y) + (target_y - 1)] == -1 && num < (size_x * size_y)); } while (map[((target_x - 1) * size_y) + (target_y - 1)] == -1 && num < (size_x * size_y));
} }

3
bot_code_turn/maze_func_goto.cpp
View File

@@ -85,6 +85,9 @@ void go_to(int dir)
//~ VWWait(); //等待下一条指令 //~ VWWait(); //等待下一条指令
MOTORDriveRaw(1,0); MOTORDriveRaw(1,0);
MOTORDriveRaw(2,0); MOTORDriveRaw(2,0);
ENCODERReset(1);
ENCODERReset(2);
OSWait(800);
rob_dir = maze_dir; //更新机器人移动到下一格之后的方向 rob_dir = maze_dir; //更新机器人移动到下一格之后的方向
xneighbor(rob_x,rob_dir); //更新机器人移动到下一格之后的X坐标 xneighbor(rob_x,rob_dir); //更新机器人移动到下一格之后的X坐标

28
bot_code_turn/maze_func_pid.cpp
View File

@@ -75,6 +75,7 @@ void PID_AL()
MOTORDriveRaw(2,speed_r + Kpid_speed); MOTORDriveRaw(2,speed_r + Kpid_speed);
//~ MOTORDriveRaw(2,speed_r); //~ MOTORDriveRaw(2,speed_r);
MOTORDriveRaw(1,speed_l - Kpid_speed); MOTORDriveRaw(1,speed_l - Kpid_speed);
//~ MOTORDriveRaw(1,speed_l);
} }
/*---PID控制下的直线行驶---*/ /*---PID控制下的直线行驶---*/
@@ -85,9 +86,9 @@ void PIDStraight()
int x_1,x_2, y_1,y_2, phi_1,phi_2; //VWGetPosition的参数定义 int x_1,x_2, y_1,y_2, phi_1,phi_2; //VWGetPosition的参数定义
VWGetPosition(&x_1, &y_1, &phi_1); //获取机器人在移动前的x,y,phi值 VWGetPosition(&x_1, &y_1, &phi_1); //获取机器人在移动前的x,y,phi值
F_PSD = PSDGetRaw(1) - 2; //读取前方和墙壁的距离 //~ F_PSD = PSDGetRaw(1) - 2; //读取前方和墙壁的距离
L_PSD = PSDGetRaw(2) - 16; //读取左侧和墙壁的距离 //~ L_PSD = PSDGetRaw(2) - 16; //读取左侧和墙壁的距离
R_PSD = PSDGetRaw(3); //读取右侧和墙壁的距离 //~ R_PSD = PSDGetRaw(3); //读取右侧和墙壁的距离
MOTORDriveRaw(2,speed_r); MOTORDriveRaw(2,speed_r);
MOTORDriveRaw(1,speed_l); MOTORDriveRaw(1,speed_l);
@@ -95,13 +96,13 @@ void PIDStraight()
do do
{ {
//~ F_PSD = PSDGetRaw(1) - 2; //读取前方和墙壁的距离 F_PSD = PSDGetRaw(1) - 2; //读取前方和墙壁的距离
//~ L_PSD = PSDGetRaw(2) - 16; //读取左侧和墙壁的距离 L_PSD = PSDGetRaw(2) - 16; //读取左侧和墙壁的距离
//~ R_PSD = PSDGetRaw(3); //读取右侧和墙壁的距离 R_PSD = PSDGetRaw(3); //读取右侧和墙壁的距离
F_PSD = (PSDGetRaw(1) - 2) * 0.6 + F_PSD * 0.4; //读取前方和墙壁的距离 //~ F_PSD = (PSDGetRaw(1) - 2) * 0.6 + F_PSD * 0.4; //读取前方和墙壁的距离
L_PSD = (PSDGetRaw(2) - 16) * 0.6 + L_PSD * 0.4; //读取左侧和墙壁的距离 //~ L_PSD = (PSDGetRaw(2) - 16) * 0.6 + L_PSD * 0.4; //读取左侧和墙壁的距离
R_PSD = (PSDGetRaw(3)) * 0.6 + R_PSD * 0.4; //读取右侧和墙壁的距离 //~ R_PSD = (PSDGetRaw(3)) * 0.6 + R_PSD * 0.4; //读取右侧和墙壁的距离
/*---打印输出PSD数值---*/ /*---打印输出PSD数值---*/
//~ cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << "\t" ; //~ cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << "\t" ;
@@ -124,9 +125,12 @@ void PIDStraight()
} }
else else
{ {
L_PSD = L_PSD % 100; //~ L_PSD = L_PSD % 200;
R_PSD = R_PSD % 100; //~ R_PSD = R_PSD % 200;
err = L_PSD - R_PSD; //~ err = L_PSD - R_PSD;
err = 0;
err_old = 0;
err_sum = 0;
PID_AL(); PID_AL();
} }
VWGetPosition(&x_2, &y_2, &phi_2); VWGetPosition(&x_2, &y_2, &phi_2);

16
bot_code_turn/maze_func_turn.cpp
View File

@@ -14,8 +14,8 @@ void BOTturn(int turn)
ENCODERReset(2); ENCODERReset(2);
do do
{ {
MOTORDriveRaw(1,22); MOTORDriveRaw(1,20);
MOTORDriveRaw(2,-20); MOTORDriveRaw(2,-18);
} while (abs(ENCODERRead(2)) < 250); } while (abs(ENCODERRead(2)) < 250);
MOTORDriveRaw(1,0); MOTORDriveRaw(1,0);
MOTORDriveRaw(2,0); MOTORDriveRaw(2,0);
@@ -26,9 +26,9 @@ void BOTturn(int turn)
ENCODERReset(2); ENCODERReset(2);
do do
{ {
MOTORDriveRaw(1,-22); MOTORDriveRaw(1,-20);
MOTORDriveRaw(2,20); MOTORDriveRaw(2,18);
} while (abs(ENCODERRead(2)) < 245); } while (abs(ENCODERRead(2)) < 250);
MOTORDriveRaw(1,0); MOTORDriveRaw(1,0);
MOTORDriveRaw(2,0); MOTORDriveRaw(2,0);
} }
@@ -38,10 +38,10 @@ void BOTturn(int turn)
ENCODERReset(2); ENCODERReset(2);
do do
{ {
MOTORDriveRaw(1,22); MOTORDriveRaw(1,20);
MOTORDriveRaw(2,-20); MOTORDriveRaw(2,-18);
} while (abs(ENCODERRead(2)) < 500); } while (abs(ENCODERRead(2)) < 580);
MOTORDriveRaw(1,0); MOTORDriveRaw(1,0);
MOTORDriveRaw(2,0); MOTORDriveRaw(2,0);
} }

4
bot_code_turn/maze_main.cpp
View File

@@ -11,7 +11,9 @@ using namespace std;
int main() int main()
{ {
explore(); //使用递归算法构建地图 explore(); //使用递归算法构建地图
wall[0][0][0] = 1; //~ wall[0][0][0] = 1;
explore();
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轴长度

18
bot_code_turn/maze_parameter.cpp
View File

@@ -9,8 +9,8 @@ using namespace std;
/*---定义数据---*/ /*---定义数据---*/
/*---定义全局常量---*/ /*---定义全局常量---*/
const int DIST_cell = 240; //单元格长度 const int DIST_cell = 240; //单元格长度
const int DIST_move = 340; //移动的单元格距离 const int DIST_move = 330; //移动的单元格距离
const int DIST_wall_F = 85; //与墙壁的距离 const int DIST_wall_F = 95; //与墙壁的距离
//~ const int DIST_wall_RL = 90; //与左右墙壁的距离 1 //~ const int DIST_wall_RL = 90; //与左右墙壁的距离 1
const int DIST_wall_RL = 87; //与左右墙壁的距离 const int DIST_wall_RL = 87; //与左右墙壁的距离
@@ -32,16 +32,16 @@ int rob_dir = 0; //机器人当前的朝向（初始为0）
/*---PID算法---*/ /*---PID算法---*/
/*--成功过的方案--*/ /*--成功过的方案--*/
//~ const float Kp = 2.8, Ki = 0.006, Kd = 0.08; //定义PD的参数16-2此方案成功过，就是有点摇摇晃晃 墙壁为1 //~ const float Kp = 2.8, Ki = 0.006, Kd = 0.08; //定义PD的参数16-2此方案成功过，就是有点摇摇晃晃 墙壁为1
//~ const float Kp = 0.2, Ki =0, Kd = 1; //定义PD的参数16-2
/*--在尝试的方案--*/ /*--在尝试的方案--*/
const float Kp = 2.82, Ki =0.006, Kd = 0.08; //定义PD的参数16-2
//~ const float Kp = 5, Ki = 0, Kd = 0.1; //定义PD的参数16-3 const float Kp = 0.22, Ki =0.004, Kd = 1; //定义PD的参数16-2
//~ const float Kp = 2.4, Ki = 0.006, Kd = 0.125; //定义PID的参数16-2
//~ const float Kp = 2.6, Ki = 0.006, Kd = 0.12; //定义PD的参数?
//~ const float Kp = 2.5, Ki = 0.01, Kd = 0.2; //定义PID的参数16-3
//~ float Kpid; //定义PID计算后输出的倍率 //~ float Kpid; //定义PID计算后输出的倍率
int Kpid_speed; int Kpid_speed;
const float Kpid_base = 1; //定义基本角速度和速度 const float Kpid_base = 1; //定义基本角速度和速度
const int speed_l = 15; //定义基本左轮速度 const int speed_l = 16; //定义基本左轮速度
const int speed_r = speed_l - 1; //定义基本右轮速度 const int speed_r = speed_l - 1; //定义基本右轮速度
//~ int speed_r = speed_l - 1; //定义基本右轮速度 //~ int speed_r = speed_l - 1; //定义基本右轮速度
const int speed_xianfu = 2; //定义kpid限幅 const int speed_xianfu = 2; //定义kpid限幅
@@ -49,7 +49,7 @@ int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分
const int eI_max = 200; //定义积分限幅 const int eI_max = 200; //定义积分限幅
/*---目标点的坐标---*/ /*---目标点的坐标---*/
const int target_x = 4; //声明目标点的X坐标 const int target_x = 3; //声明目标点的X坐标
const int target_y = 4; //声明目标点的Y坐标 const int target_y = 4; //声明目标点的Y坐标
/*---定义结束---*/ /*---定义结束---*/