From 6879dec68c7568a0bea4f36a7a0bc492c1906ff9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=B0=A7=E5=8E=9F=E5=AD=90?= Date: Thu, 15 May 2025 14:56:30 +0800 Subject: [PATCH] =?UTF-8?q?2025.5.15=E7=AC=AC=E4=B8=80=E6=AC=A1=E6=8F=90?= =?UTF-8?q?=E4=BA=A4=EF=BC=8C=E4=BF=AE=E5=A4=8D=E5=A4=A7=E9=87=8F=E5=BA=94?= =?UTF-8?q?=E5=88=87=E6=8D=A2vector=E9=80=A0=E6=88=90=E7=9A=84bug=EF=BC=8C?= =?UTF-8?q?=E5=B9=B6=E6=B7=BB=E5=8A=A0=E5=AE=9E=E6=9C=BA=E6=B5=8B=E8=AF=95?= =?UTF-8?q?=E6=96=87=E4=BB=B6=E5=A4=B9=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- maze_code/maze_func_explore.cpp | 174 ++++++++++------------ maze_code/maze_func_flood.cpp | 1 + maze_code/maze_func_path.cpp | 1 + maze_code/maze_main.cpp | 1 + maze_code/maze_parameter.cpp | 3 +- robot_test_code/LCD输出测试/lcd_test1.cpp | 10 ++ robot_test_code/PID/PID.cpp | 106 +++++++++++++ robot_test_code/PID/PID.h | 5 + robot_test_code/PID/PIDmain.cpp | 48 ++++++ robot_test_code/实机测试/test_run.cpp | 19 +++ 10 files changed, 273 insertions(+), 95 deletions(-) create mode 100644 robot_test_code/LCD输出测试/lcd_test1.cpp create mode 100644 robot_test_code/PID/PID.cpp create mode 100644 robot_test_code/PID/PID.h create mode 100644 robot_test_code/PID/PIDmain.cpp create mode 100644 robot_test_code/实机测试/test_run.cpp diff --git a/maze_code/maze_func_explore.cpp b/maze_code/maze_func_explore.cpp index 6ebcd9b..ce91f7b 100644 --- a/maze_code/maze_func_explore.cpp +++ b/maze_code/maze_func_explore.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include"eyebot++.h" #include"maze_parameter.h" #include"maze_func.h" @@ -21,89 +22,40 @@ bool check_mark() bool S_mark; bool E_mark; - if (x>0 && y>0) //情况1 + if (x > 0) { - 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]; - } + W_mark = mark[x-1][y]; } - else if (x==0 && y>0) //情况2 + else { - 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]; - } + W_mark = true; } - else if (x>0 && y==0) //情况3 + + if (y > 0) { - 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]; - } + S_mark = mark[x][y-1]; } - else //(x==0 && y==0) + else { - 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]; - } + S_mark = true; + } + + if (x < size_x - 1) + { + E_mark = mark[x+1][y]; + } + else + { + E_mark = true; + } + + if (y < size_y - 1) + { + N_mark = mark[x][y+1]; + } + else + { + N_mark = true; } bool N_road = (!wall[x][y+1][0] && N_mark); @@ -126,15 +78,22 @@ void maze_entry(int x, int y, int dir, int open) switch (maze_dir) { case 0: //0表示北(上)，表示(x，y+1)坐标单元格的下方墙壁信息 - if (y == mark[0].size() - 1 && open) //检测是否超出容器范围，如果超出，则扩大容器：对每一个内层尾插数据 + if (y == static_cast(mark[0].size()) - 1 && open) //检测是否超出容器范围，如果超出，则扩大容器：对每一个内层尾插数据 { - for (int i = 0; i < mark.size() - 1; i++) + if (!mark.empty() && !wall.empty()) //插入安全性检测，确保容器非空 { - mark[i].emplace_back(0); + for (size_t i = 0; i < mark.size(); i++) + { + mark[i].emplace_back(0); + } + for (size_t i = 0; i < wall.size(); i++) + { + wall[i].emplace_back(std::vector(2,0)); + } } - for (int i = 0; i < wall.size() - 1; i++) + else { - wall[i].emplace_back(vector(2,0)); + cout << "error: mark or wall empty" << endl; } } wall[x][y+1][0] = !open; @@ -143,8 +102,21 @@ void maze_entry(int x, int y, int dir, int open) 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)); + if (!mark.empty() && !wall.empty()) //插入安全性检测，确保容器非空 + { + // std::vector mark_insert(mark[0].size(), 0); + // mark.insert(mark.begin(), mark_insert); + + // std::vector> wall_insert(wall[0].size(), std::vector(2,0)); + // wall.insert(wall.begin(), wall_insert); + + mark.emplace(mark.begin(), std::vector(mark[0].size(), 0)); //在mark头部插入一个内层 + wall.emplace(wall.begin(), std::vector>(wall[0].size(), std::vector(2, 0))); //在wall头部插入一个内层 + } + else + { + cout << "error: mark or wall empty" << endl; + } } wall[x][y][1] = !open; break; @@ -152,23 +124,37 @@ void maze_entry(int x, int y, int dir, int open) case 2: //2表示南(下)，表示(x，y)坐标单元格的下侧墙壁信息 if (y == 0 && open) //检测是否超出容器范围，如果超出，则扩大容器：对每一个内层头插数据 { - for (int i = 0; i < mark.size() - 1; i++) + if (!mark.empty() && !wall.empty()) //插入安全性检测，确保容器非空 { - mark[i].emplace(mark[i].begin(), 0); + for (size_t i = 0; i < mark.size(); i++) + { + mark[i].emplace(mark[i].begin(), 0); + } + for (size_t i = 0; i < wall.size(); i++) + { + wall[i].emplace(wall[i].begin(), std::vector(2,0)); + } } - for (int i = 0; i < wall.size() - 1; i++) + else { - wall[i].emplace(wall[i].begin(), vector(2,0)); + cout << "error: mark or wall empty" << endl; } } wall[x][y][0] = !open; break; case 3: //3表示东(右)，表示(x+1，y)坐标单元格的左侧墙壁信息 - if (x == (mark.size() - 1) && open) //检测是否超出容器范围，如果超出，则扩大容器：尾插一个内层 + if (x == static_cast(mark.size()) - 1 && open) //检测是否超出容器范围，如果超出，则扩大容器：尾插一个内层 { - mark.emplace_back(mark[0].size(), 0); - wall.emplace_back(wall[0].size(), vector(2,0)); + if (!mark.empty() && !wall.empty()) //插入安全性检测，确保容器非空 + { + mark.emplace_back(std::vector(mark[0].size(), 0)); + wall.emplace_back(std::vector>(wall[0].size(), std::vector(2, 0))); + } + else + { + cout << "error: mark or wall empty" << endl; + } } wall[x+1][y][1] = !open; break; @@ -183,7 +169,7 @@ bool unmarked(int x, int y, int dir) switch(maze_dir) { case 0: - if (y < mark[0].size() - 2) + if (y < (static_cast(mark[0].size()) - 1)) { if (mark[x][y+1] == 0) { @@ -193,7 +179,7 @@ bool unmarked(int x, int y, int dir) break; case 1: - if (x > 1) + if (x > 0) { if (mark[x-1][y] == 0) { @@ -203,7 +189,7 @@ bool unmarked(int x, int y, int dir) break; case 2: - if (y > 1) + if (y > 0) { if (mark[x][y-1] == 0) { @@ -213,7 +199,7 @@ bool unmarked(int x, int y, int dir) break; case 3: - if (x < mark.size() - 2) + if (x < (static_cast(mark.size()) - 1)) { if (mark[x+1][y] == 0) { diff --git a/maze_code/maze_func_flood.cpp b/maze_code/maze_func_flood.cpp index 03cf529..927b38c 100644 --- a/maze_code/maze_func_flood.cpp +++ b/maze_code/maze_func_flood.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include"eyebot++.h" #include"maze_parameter.h" #include"maze_func.h" diff --git a/maze_code/maze_func_path.cpp b/maze_code/maze_func_path.cpp index d819aed..fe4883b 100644 --- a/maze_code/maze_func_path.cpp +++ b/maze_code/maze_func_path.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include"eyebot++.h" #include"maze_parameter.h" #include"maze_func.h" diff --git a/maze_code/maze_main.cpp b/maze_code/maze_main.cpp index 2d8916d..7ae81dc 100644 --- a/maze_code/maze_main.cpp +++ b/maze_code/maze_main.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include"eyebot++.h" #include"maze_parameter.h" #include"maze_func.h" diff --git a/maze_code/maze_parameter.cpp b/maze_code/maze_parameter.cpp index e409c7e..77b66c6 100644 --- a/maze_code/maze_parameter.cpp +++ b/maze_code/maze_parameter.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include"eyebot++.h" #include"maze_parameter.h" #include"maze_func.h" @@ -14,7 +15,7 @@ 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=通路 +vector>> wall(2,vector>(2,vector(2,0))); //定义wall容器（三维容器），容器初始为1行1列，且每个格子内带有2个墙壁信息 1=墙壁；0=通路 /*---机器人坐标---*/ int rob_x0 = 0; //机器人起点X坐标 diff --git a/robot_test_code/LCD输出测试/lcd_test1.cpp b/robot_test_code/LCD输出测试/lcd_test1.cpp new file mode 100644 index 0000000..fdec667 --- /dev/null +++ b/robot_test_code/LCD输出测试/lcd_test1.cpp @@ -0,0 +1,10 @@ +#include +#include +#include"eyebot++.h" +using namespace std; + + +int main() +{ + +} \ No newline at end of file diff --git a/robot_test_code/PID/PID.cpp b/robot_test_code/PID/PID.cpp new file mode 100644 index 0000000..e9173f7 --- /dev/null +++ b/robot_test_code/PID/PID.cpp @@ -0,0 +1,106 @@ +#include +#include +#include"eyebot++.h" +#include"PID.h" +using namespace std; + +float Kp = 1, Ki = 0, Kd = 0.5; //定义PID的参数 +float Kpid; +float Kpid_speed; +const int Kpid_base = 1, speed = 200; +const int speed2 = 0.25*speed; +int speed1; +//int speed2 = speed; +/*---定义全局变量---*/ +int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差 +const int eI_max = 50; +/*---定义全局常量---*/ +/*---定义全局常量---*/ +const int DIST_cell = 300 ; +const int DIST_wall = 100 ; +/*---全局各种变量与常量定义完成---*/ + + +/*---对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控制算法输出的倍率 + Kpid_speed = Kpid * Kpid_base; + /*---打印输出PID控制的各项参数2---*/ + // cout << "PID输出值:" << Kpid << endl ; + /*---打印结束---*/ + 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 + { + L_PSD = PSDGetRaw(2); //读取左侧和墙壁的距离 + R_PSD = PSDGetRaw(3); //读取右侧和墙壁的距离 + F_PSD = PSDGetRaw(1); //读取前方和墙壁的距离 + /*---打印输出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); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行 + //VWSetSpeed(0,0); +} diff --git a/robot_test_code/PID/PID.h b/robot_test_code/PID/PID.h new file mode 100644 index 0000000..42c1ec4 --- /dev/null +++ b/robot_test_code/PID/PID.h @@ -0,0 +1,5 @@ +#pragma once + +void eI_xianfu(int eI_max); //积分部分限幅 +void PID_AL(); //位置式PID算法 +void PIDStraight(); //PID控制下的直线行驶 \ No newline at end of file diff --git a/robot_test_code/PID/PIDmain.cpp b/robot_test_code/PID/PIDmain.cpp new file mode 100644 index 0000000..d9278bd --- /dev/null +++ b/robot_test_code/PID/PIDmain.cpp @@ -0,0 +1,48 @@ +#include +#include"PID.h" +#include"eyebot++.h" +using namespace std; + + +/*---定义全局常量---*/ +const int DIST_cell = 300 ; +const int DIST_wall = 100 ; +/*---全局各种变量与常量定义完成---*/ + + +/*---下面是主函数---*/ +int main() +{ + int Fwall,Lwall,Rwall; + LCDMenu("","","","END"); + + do + { //检测墙面 + Fwall = PSDGetRaw(1) > DIST_cell; //检测前面侧是否有墙 + Lwall = PSDGetRaw(2) > DIST_cell; //检测左面 + Rwall = PSDGetRaw(3) > DIST_cell; //检测右面 + + //判断旋转方向 + if (Lwall) + { + VWTurn(+90,45); //向左旋转 + VWWait(); + } + else if (Fwall) + { + //不旋转 + } + else if (Rwall) + { + VWTurn(-90,45); //向右旋转 + VWWait(); + } + else + { + VWTurn(180,45); + VWWait(); + } + //直走 + PIDStraight(); + } while (KEYRead() != KEY4); +} diff --git a/robot_test_code/实机测试/test_run.cpp b/robot_test_code/实机测试/test_run.cpp new file mode 100644 index 0000000..4d150b5 --- /dev/null +++ b/robot_test_code/实机测试/test_run.cpp @@ -0,0 +1,19 @@ +#include +#include +#include"eyebot++.h" +using namespace std; + + + +int main() +{ + int Fwall,Lwall,Rwall; + LCDMenu("","","","END"); + + do + { //检测墙面 + MOTORDriveRaw(1,50); + MOTORDriveRaw(2,50); + + } while (KEYRead() != KEY4); +} \ No newline at end of file