From 9d96af2ff30a5b68e8863d75868b671dd129bf39 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C3=91=C3=B5=D4=AD=C3=97=C3=93?= Date: Tue, 27 May 2025 14:57:30 +0800 Subject: [PATCH] 1 --- bot_code_turn/maze_func_flood.cpp | 11 ++++++----- bot_code_turn/maze_func_goto.cpp | 3 +++ bot_code_turn/maze_func_pid.cpp | 28 ++++++++++++++++------------ bot_code_turn/maze_func_turn.cpp | 16 ++++++++-------- bot_code_turn/maze_main.cpp | 4 +++- bot_code_turn/maze_parameter.cpp | 18 +++++++++--------- 6 files changed, 45 insertions(+), 35 deletions(-) diff --git a/bot_code_turn/maze_func_flood.cpp b/bot_code_turn/maze_func_flood.cpp index 57c8f5f..521f98d 100644 --- a/bot_code_turn/maze_func_flood.cpp +++ b/bot_code_turn/maze_func_flood.cpp @@ -15,6 +15,7 @@ void flood(int *map, int *copy_wall) do { + num++; for (int i = 0; i < size_x; i++) { 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) //左边格子 { 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) //右边格子 { 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) { 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) { 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)); -} \ No newline at end of file +} diff --git a/bot_code_turn/maze_func_goto.cpp b/bot_code_turn/maze_func_goto.cpp index 217423a..b9f66e2 100644 --- a/bot_code_turn/maze_func_goto.cpp +++ b/bot_code_turn/maze_func_goto.cpp @@ -85,6 +85,9 @@ void go_to(int dir) //~ VWWait(); //等待下一条指令 MOTORDriveRaw(1,0); MOTORDriveRaw(2,0); + ENCODERReset(1); + ENCODERReset(2); + OSWait(800); rob_dir = maze_dir; //更新机器人移动到下一格之后的方向 xneighbor(rob_x,rob_dir); //更新机器人移动到下一格之后的X坐标 diff --git a/bot_code_turn/maze_func_pid.cpp b/bot_code_turn/maze_func_pid.cpp index 663a043..ae6b7d7 100644 --- a/bot_code_turn/maze_func_pid.cpp +++ b/bot_code_turn/maze_func_pid.cpp @@ -75,6 +75,7 @@ void PID_AL() MOTORDriveRaw(2,speed_r + Kpid_speed); //~ MOTORDriveRaw(2,speed_r); MOTORDriveRaw(1,speed_l - Kpid_speed); + //~ MOTORDriveRaw(1,speed_l); } /*---PID控制下的直线行驶---*/ @@ -85,9 +86,9 @@ void PIDStraight() int x_1,x_2, y_1,y_2, phi_1,phi_2; //VWGetPosition的参数定义 VWGetPosition(&x_1, &y_1, &phi_1); //获取机器人在移动前的x,y,phi值 - F_PSD = PSDGetRaw(1) - 2; //读取前方和墙壁的距离 - L_PSD = PSDGetRaw(2) - 16; //读取左侧和墙壁的距离 - R_PSD = PSDGetRaw(3); //读取右侧和墙壁的距离 + //~ F_PSD = PSDGetRaw(1) - 2; //读取前方和墙壁的距离 + //~ L_PSD = PSDGetRaw(2) - 16; //读取左侧和墙壁的距离 + //~ R_PSD = PSDGetRaw(3); //读取右侧和墙壁的距离 MOTORDriveRaw(2,speed_r); MOTORDriveRaw(1,speed_l); @@ -95,13 +96,13 @@ void PIDStraight() do { - //~ F_PSD = PSDGetRaw(1) - 2; //读取前方和墙壁的距离 - //~ L_PSD = PSDGetRaw(2) - 16; //读取左侧和墙壁的距离 - //~ R_PSD = PSDGetRaw(3); //读取右侧和墙壁的距离 + F_PSD = PSDGetRaw(1) - 2; //读取前方和墙壁的距离 + L_PSD = PSDGetRaw(2) - 16; //读取左侧和墙壁的距离 + R_PSD = PSDGetRaw(3); //读取右侧和墙壁的距离 - F_PSD = (PSDGetRaw(1) - 2) * 0.6 + F_PSD * 0.4; //读取前方和墙壁的距离 - L_PSD = (PSDGetRaw(2) - 16) * 0.6 + L_PSD * 0.4; //读取左侧和墙壁的距离 - R_PSD = (PSDGetRaw(3)) * 0.6 + R_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; //读取左侧和墙壁的距离 + //~ R_PSD = (PSDGetRaw(3)) * 0.6 + R_PSD * 0.4; //读取右侧和墙壁的距离 /*---打印输出PSD数值---*/ //~ cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << "\t" ; @@ -124,9 +125,12 @@ void PIDStraight() } else { - L_PSD = L_PSD % 100; - R_PSD = R_PSD % 100; - err = L_PSD - R_PSD; + //~ L_PSD = L_PSD % 200; + //~ R_PSD = R_PSD % 200; + //~ err = L_PSD - R_PSD; + err = 0; + err_old = 0; + err_sum = 0; PID_AL(); } VWGetPosition(&x_2, &y_2, &phi_2); diff --git a/bot_code_turn/maze_func_turn.cpp b/bot_code_turn/maze_func_turn.cpp index 150564e..449e9dd 100644 --- a/bot_code_turn/maze_func_turn.cpp +++ b/bot_code_turn/maze_func_turn.cpp @@ -14,8 +14,8 @@ void BOTturn(int turn) ENCODERReset(2); do { - MOTORDriveRaw(1,22); - MOTORDriveRaw(2,-20); + MOTORDriveRaw(1,20); + MOTORDriveRaw(2,-18); } while (abs(ENCODERRead(2)) < 250); MOTORDriveRaw(1,0); MOTORDriveRaw(2,0); @@ -26,9 +26,9 @@ void BOTturn(int turn) ENCODERReset(2); do { - MOTORDriveRaw(1,-22); - MOTORDriveRaw(2,20); - } while (abs(ENCODERRead(2)) < 245); + MOTORDriveRaw(1,-20); + MOTORDriveRaw(2,18); + } while (abs(ENCODERRead(2)) < 250); MOTORDriveRaw(1,0); MOTORDriveRaw(2,0); } @@ -38,10 +38,10 @@ void BOTturn(int turn) ENCODERReset(2); do { - MOTORDriveRaw(1,22); - MOTORDriveRaw(2,-20); + MOTORDriveRaw(1,20); + MOTORDriveRaw(2,-18); - } while (abs(ENCODERRead(2)) < 500); + } while (abs(ENCODERRead(2)) < 580); MOTORDriveRaw(1,0); MOTORDriveRaw(2,0); } diff --git a/bot_code_turn/maze_main.cpp b/bot_code_turn/maze_main.cpp index cace75d..869b445 100644 --- a/bot_code_turn/maze_main.cpp +++ b/bot_code_turn/maze_main.cpp @@ -11,7 +11,9 @@ using namespace std; int main() { explore(); //使用递归算法构建地图 - wall[0][0][0] = 1; + //~ wall[0][0][0] = 1; + explore(); + const int mazesize_x = mark.size(); //获取探索结束之后的迷宫的X轴长度 const int mazesize_y = mark[0].size(); //获取探索结束之后的迷宫的Y轴长度 diff --git a/bot_code_turn/maze_parameter.cpp b/bot_code_turn/maze_parameter.cpp index 71611c9..692fd6c 100644 --- a/bot_code_turn/maze_parameter.cpp +++ b/bot_code_turn/maze_parameter.cpp @@ -9,8 +9,8 @@ using namespace std; /*---定义数据---*/ /*---定义全局常量---*/ const int DIST_cell = 240; //单元格长度 -const int DIST_move = 340; //移动的单元格距离 -const int DIST_wall_F = 85; //与墙壁的距离 +const int DIST_move = 330; //移动的单元格距离 +const int DIST_wall_F = 95; //与墙壁的距离 //~ const int DIST_wall_RL = 90; //与左右墙壁的距离 1 const int DIST_wall_RL = 87; //与左右墙壁的距离 @@ -32,16 +32,16 @@ int rob_dir = 0; //机器人当前的朝向（初始为0） /*---PID算法---*/ /*--成功过的方案--*/ //~ 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 = 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 + +const float Kp = 0.22, Ki =0.004, Kd = 1; //定义PD的参数16-2 + + //~ float Kpid; //定义PID计算后输出的倍率 int Kpid_speed; const float Kpid_base = 1; //定义基本角速度和速度 -const int speed_l = 15; //定义基本左轮速度 +const int speed_l = 16; //定义基本左轮速度 const int speed_r = speed_l - 1; //定义基本右轮速度 //~ int speed_r = speed_l - 1; //定义基本右轮速度 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 target_x = 4; //声明目标点的X坐标 +const int target_x = 3; //声明目标点的X坐标 const int target_y = 4; //声明目标点的Y坐标 /*---定义结束---*/