From df42d6ce7f22c2c524967ccd3a033f64c0496525 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C3=91=C3=B5=D4=AD=C3=97=C3=93?= Date: Sun, 18 May 2025 11:35:02 +0800 Subject: [PATCH] =?UTF-8?q?2025.5.18=E7=AC=AC=E4=B8=80=E6=AC=A1=E6=8F=90?= =?UTF-8?q?=E4=BA=A4=EF=BC=8C=E5=8C=85=E6=8B=AC17=E5=8F=B7=E5=92=8C?= =?UTF-8?q?=E4=BB=8A=E5=A4=A9=E4=B8=8A=E5=8D=88=E5=AF=B9=E5=AE=9E=E6=9C=BA?= =?UTF-8?q?=E7=9A=84=E8=B0=83=E8=AF=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- bot_code/makefile | 4 +- bot_code/maze_func.h | 4 +- bot_code/maze_func_goto.cpp | 9 ++- bot_code/maze_func_pid.cpp | 67 +++++++++++---------- bot_code/maze_func_turn.cpp | 61 +++++++++++-------- bot_code/maze_parameter.cpp | 29 +++++---- bot_code/maze_parameter.h | 15 ++--- robot_test_code/PID/PID.cpp | 13 ++-- robot_test_code/PID/PIDmain.cpp | 4 +- robot_test_code/实机测试/makefile | 10 ++-- robot_test_code/实机测试/test_psd.cpp | 4 +- robot_test_code/实机测试/test_turn.cpp | 34 +++++++++-- robot_test_code/实机测试/test_turn2.cpp | 79 +++++++++++++++++++++++++ 文件/eyebot函数.cpp | 2 + 14 files changed, 232 insertions(+), 103 deletions(-) create mode 100644 robot_test_code/实机测试/test_turn2.cpp diff --git a/bot_code/makefile b/bot_code/makefile index 58e56dc..be8bad0 100644 --- a/bot_code/makefile +++ b/bot_code/makefile @@ -1,8 +1,8 @@ -cxx := g++sim +cxx := gccarm file_cpp := $(wildcard *.cpp) -file_o := ../eyesim/all_test01/maze.x +file_o := /home/pi/usr/maze.x $(file_o) : $(file_cpp) @$(cxx) $^ -o $@ diff --git a/bot_code/maze_func.h b/bot_code/maze_func.h index 9eb2fd3..5f97fb3 100644 --- a/bot_code/maze_func.h +++ b/bot_code/maze_func.h @@ -12,8 +12,8 @@ void explore(); //递归函数 /*---以下为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_speed_xianfu(int Kpid_speed); //对PID控制的输出部分限幅 +void eI_xianfu(); //对PID控制的积分部分I限幅 +void pid_speed_xianfu(); //对PID控制的输出部分限幅 void PID_AL(); //位置式PID算法 void PIDStraight(); //PID控制下的直线行驶 void go_to(int dir); //行驶到指定单元格 diff --git a/bot_code/maze_func_goto.cpp b/bot_code/maze_func_goto.cpp index a6078ca..f3c2cff 100644 --- a/bot_code/maze_func_goto.cpp +++ b/bot_code/maze_func_goto.cpp @@ -68,16 +68,23 @@ void go_to(int dir) { turn = +1; } + if (turn == -2) + { + turn = 2; + } if (turn) //如果turn不是0，则执行旋转命令 { + OSWait(1000); BOTturn(turn); + OSWait(200); VWWait(); } PIDStraight(); //使用PID算法算法，向指定方向直线行驶一格 + //~ OSWait(200); 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/bot_code/maze_func_pid.cpp b/bot_code/maze_func_pid.cpp index 8bbf73a..0278569 100644 --- a/bot_code/maze_func_pid.cpp +++ b/bot_code/maze_func_pid.cpp @@ -7,7 +7,7 @@ using namespace std; /*---对PID控制的积分部分I限幅---*/ -void eI_xianfu(int eI_max) +void eI_xianfu() { if (err_sum >= eI_max) //如果err_sum大于限幅，则直接让err_sum=限幅，防止err_sum过大，影响PID控制 { @@ -23,13 +23,13 @@ void eI_xianfu(int eI_max) } } -void pid_speed_xianfu(int Kpid_speed) +void pid_speed_xianfu() { - if (Kpid_speed > speed_xianfu) + if (Kpid_speed >= speed_xianfu) { Kpid_speed = speed_xianfu; } - else if (Kpid_speed < -speed_xianfu) + else if (Kpid_speed <= -speed_xianfu) { Kpid_speed = -speed_xianfu; } @@ -42,81 +42,84 @@ void pid_speed_xianfu(int Kpid_speed) /*---位置式PID算法---*/ void PID_AL() { - err_sum += err; //积分累计误差值 - eI_xianfu(eI_max); //积分部分限幅 + //~ err_sum += err; //积分累计误差值 + //~ eI_xianfu(); //积分部分限幅 err_diff = err - err_old; //微分部分误差值 /*---打印输出PID控制的各项参数1---*/ - cout << "本次误差:" << err << "\t" ; - cout << "上次误差:" << err_old << "\t" ; - cout << "误差累计:" << err_sum << "\t" ; - cout << "误差变化:" << err_diff << "\t" ; + //~ cout << "err:" << err << " " ; + //~ cout << "old_err:" << err_old << " " ; + //~ cout << "err_sum:" << err_sum << " " ; + //~ cout << "err_diff:" << err_diff << " " ; /*---打印结束---*/ err_old = err; //将误差幅值到上一次误差 - Kpid = Kp*err + Ki*err_sum + Kd*err_diff; //PID控制算法输出的倍率 - Kpid_speed = Kpid * Kpid_base; + //~ Kpid = Kp*err + Ki*err_sum + Kd*err_diff; //PID控制算法输出的倍率 + //~ Kpid_speed = (Kp*err + Ki*err_sum + Kd*err_diff) * Kpid_base; + Kpid_speed = (Kp*err + Kd*err_diff) * Kpid_base; + + pid_speed_xianfu(); + speed_r = speed_l + Kpid_speed; - pid_speed_xianfu(Kpid_speed); /*---打印输出PID控制的各项参数2---*/ - cout << "PID输出值:" << Kpid_speed << "\t" ; + cout << "PID_out:" << Kpid_speed << endl ; /*---打印结束---*/ // VWSetSpeed(speed,Kpid*Kpid_base); //控制小车的行驶速度，角速度(模拟器可以用，实物no) - MOTORDriveRaw(1,speed1); - MOTORDriveRaw(2,speed2 + Kpid_speed); + + MOTORDriveRaw(2,speed_r); + MOTORDriveRaw(1,speed_l); } /*---PID控制下的直线行驶---*/ void PIDStraight() { - int DIST_move; //定义基础角速度和速度,和移动距离 + int bot_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,speed2); + MOTORDriveRaw(2,speed_r); + MOTORDriveRaw(1,speed_l); + do { - F_PSD = PSDGetRaw(1) - 5; //读取前方和墙壁的距离 - L_PSD = PSDGetRaw(2) - 8; //读取左侧和墙壁的距离 + F_PSD = PSDGetRaw(1) - 2; //读取前方和墙壁的距离 + L_PSD = PSDGetRaw(2) - 16; //读取左侧和墙壁的距离 R_PSD = PSDGetRaw(3); //读取右侧和墙壁的距离 /*---打印输出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" ; /*---打印结束---*/ - if (30 DIST_wall_F); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行 + }while (bot_move < DIST_move && F_PSD > DIST_wall_F); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行 MOTORDriveRaw(1,0); MOTORDriveRaw(2,0); - } diff --git a/bot_code/maze_func_turn.cpp b/bot_code/maze_func_turn.cpp index e4d535d..e492615 100644 --- a/bot_code/maze_func_turn.cpp +++ b/bot_code/maze_func_turn.cpp @@ -8,38 +8,49 @@ using namespace std; void BOTturn(int turn) { - if (turn == 1) + if (turn == -1) //右转 { - MOTORDriveRaw(1,24); - MOTORDriveRaw(2,-22); - - OSWait(1985); - - MOTORDriveRaw(1,0); - MOTORDriveRaw(2,0); + ENCODERReset(1); + ENCODERReset(2); + do + { + MOTORDriveRaw(1,21); + MOTORDriveRaw(2,-19); + + } while (abs(ENCODERRead(2)) < 218); + + MOTORDriveRaw(1,0); + MOTORDriveRaw(2,0); } - else if (turn == -1) + else if (turn == 1) //左转 { - MOTORDriveRaw(1,-24); - MOTORDriveRaw(2,22); + ENCODERReset(1); + ENCODERReset(2); + do + { + MOTORDriveRaw(1,-21); + MOTORDriveRaw(2,19); - OSWait(1985); - - MOTORDriveRaw(1,0); - MOTORDriveRaw(2,0); - } - else if (turn == 2) + } while (abs(ENCODERRead(2)) < 240); + + MOTORDriveRaw(1,0); + MOTORDriveRaw(2,0); + } + else if (turn == 2) //掉头 { - MOTORDriveRaw(1,24); - MOTORDriveRaw(2,-22); - - OSWait(2985); - - MOTORDriveRaw(1,0); - MOTORDriveRaw(2,0); + ENCODERReset(1); + ENCODERReset(2); + do + { + MOTORDriveRaw(1,21); + MOTORDriveRaw(2,-19); + + } while (abs(ENCODERRead(2)) < 500); + MOTORDriveRaw(1,0); + MOTORDriveRaw(2,0); } else { cout << "errer" << endl; } -} \ No newline at end of file +} diff --git a/bot_code/maze_parameter.cpp b/bot_code/maze_parameter.cpp index b5f3c9a..af89c6c 100644 --- a/bot_code/maze_parameter.cpp +++ b/bot_code/maze_parameter.cpp @@ -8,9 +8,10 @@ using namespace std; /*---定义数据---*/ /*---定义全局常量---*/ -const int DIST_cell = 300 ; //单元格长度 -const int DIST_wall_F = 90 ; //与墙壁的距离 -extern const int DIST_wall_RL = 100; //与左右墙壁的距离 +const int DIST_cell = 240; //单元格长度 +const int DIST_move = 292; //移动的单元格距离 +const int DIST_wall_F = 90; //与墙壁的距离 +const int DIST_wall_RL = 88; //与左右墙壁的距离 /*---定义全局变量---*/ /*---容器类---*/ @@ -28,19 +29,21 @@ int rob_dir = 0; //机器人当前的朝向（初始为0） /*---PID算法---*/ -const float Kp = 0.8, Ki = 0.015, Kd = 0.15; //定义PID的参数 -float Kpid; //定义PID计算后输出的倍率 +const float Kp = 0.4, Ki = 0, Kd = 0.25; //定义PID的参数17 +//~ const float Kp = 0.825, Ki = 0.0081, Kd = 0.175; //定义PID的参数17 +//~ const float Kp = 0.80, Ki = 0.0076, Kd = 0.2; //定义PID的参数18 +//~ float Kpid; //定义PID计算后输出的倍率 int Kpid_speed; -const float Kpid_base = 1; //定义基本角速度和速度 -const int speed = 18; //定义基本角速度和速度 -const int speed2 = 0.75*speed; //快到目的地时减速 -const int speed_xianfu = 3; //定义kpid限幅 -int speed1; -int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差 -const int eI_max = 200; //定义积分限幅 +const float Kpid_base = 1; //定义基本角速度和速度 +const int speed_l = 16; //定义基本左轮速度 +//~ const int speed_r = speed_l - 2; //定义基本右轮速度 +int speed_r = speed_l - 2; //定义基本右轮速度 +const int speed_xianfu = 2; //定义kpid限幅 +int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差 +const int eI_max = 200; //定义积分限幅 /*---目标点的坐标---*/ const int target_x = 4; //声明目标点的X坐标 const int target_y = 4; //声明目标点的Y坐标 -/*---定义结束---*/ \ No newline at end of file +/*---定义结束---*/ diff --git a/bot_code/maze_parameter.h b/bot_code/maze_parameter.h index c1a437d..ed0510a 100644 --- a/bot_code/maze_parameter.h +++ b/bot_code/maze_parameter.h @@ -4,7 +4,8 @@ /*---声明数据---*/ /*---声明全局常量---*/ extern const int DIST_cell; //单元格长度 -extern const int DIST_wall_F; //与前面墙壁的距离 +extern const int DIST_move; //移动的单元格距离 +extern const int DIST_wall_F; //与前面墙壁的距离 extern const int DIST_wall_RL; //与左右墙壁的距离 /*---声明全局变量---*/ @@ -26,13 +27,13 @@ extern int rob_dir; //机器人当前的朝向 /*---PID算法---*/ extern const float Kp, Ki, Kd; //声明PID的参数 -extern float Kpid; //定义PID计算后输出的倍率 +//~ extern float Kpid; //定义PID计算后输出的倍率 extern int Kpid_speed; //定义计算后的速度 -extern const float Kpid_base; //定义速度基础倍数速度 -extern const int speed; //定义速度基础倍数速度 -extern const int speed2; //快到目的地时减速 +extern const float Kpid_base; //定义速度基础倍数速度 +extern const int speed_l; //定义左轮基本速度 +//~ extern const int speed_r; //定义右轮基本速度 +extern int speed_r; //定义右轮基本速度 extern const int speed_xianfu; //kpid输出速度的限幅 -extern int speed1; extern int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差 extern const int eI_max; //定义积分限幅 @@ -40,4 +41,4 @@ 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/robot_test_code/PID/PID.cpp b/robot_test_code/PID/PID.cpp index 4213cdc..b04f2df 100644 --- a/robot_test_code/PID/PID.cpp +++ b/robot_test_code/PID/PID.cpp @@ -4,26 +4,21 @@ #include"PID.h" using namespace std; -float Kp = 0.79, Ki = 0.009, Kd = 0.14; //定义PID的参数 +float Kp = 0.815, Ki = 0.0083, Kd = 0.167; //定义PID的参数 float Kpid; int Kpid_speed; -const int speed_l = 17; +const int speed_l = 18; const int speed_r = speed_l - 2; const int speed_err = 2; const float Kpid_base = 1; -// const int speed2 = 0.75*speed; -// int speed1; -//int speed2 = speed; /*---定义全局变量---*/ int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差 const int eI_max = 200; /*---定义全局常量---*/ /*---定义全局常量---*/ -// const int DIST_cell = 300 ; -// const int DIST_wall = 100 ; const int DIST_cell = 310; -const int DIST_wall_f = 120; +const int DIST_wall_f = 75; const int DIST_wall_rl = 90; /*---全局各种变量与常量定义完成---*/ @@ -126,7 +121,7 @@ void PIDStraight() /*---打印输出PSD数值---*/ cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << " " ; /*---打印结束---*/ - if (20 130); } + else if (num == 2) + { + b_wall = PSDGetRaw(4) - 2; + f_wall = PSDGetRaw(1) - 2; + do + { + old_b_wall = b_wall; + old_f_wall = f_wall; + MOTORDriveRaw(1,19); + MOTORDriveRaw(2,-17); + //~ MOTORDriveRaw(1,24); + //~ MOTORDriveRaw(2,-22); + OSWait(1000); + //~ OSWait(1980); + MOTORDriveRaw(1,0); + MOTORDriveRaw(2,0); + b_wall = (PSDGetRaw(4) - 2) * 0.7 + old_b_wall * 0.3; //减小突变 + f_wall = (PSDGetRaw(1) - 2) * 0.7 + old_f_wall * 0.3; //减小突变 + //~ b_wall = PSDGetRaw(4) - 2; + //~ f_wall = PSDGetRaw(1) - 2; + turn_check = b_wall - old_b_wall; + /*---打印输出PSD数值---*/ + //~ cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << " " ; + cout << "f_wall: " << f_wall << "\t" << "b_wall: " << b_wall << "\t" << "check: " << turn_check << endl; + /*---打印结束---*/ + OSWait(50); + } while (f_wall < 200 || turn_check < -4 || b_wall > 130); + } //~ if(num == 1) //~ { //~ MOTORDriveRaw(1,19); diff --git a/robot_test_code/实机测试/test_turn2.cpp b/robot_test_code/实机测试/test_turn2.cpp new file mode 100644 index 0000000..4dee7d5 --- /dev/null +++ b/robot_test_code/实机测试/test_turn2.cpp @@ -0,0 +1,79 @@ +#include +#include +#include +//~ #include"eyebot++.h" +#include"/home/pi/eyebot/include/eyebot++.h" +using namespace std; + +int main() +{ + + LCDMenu("","","","END"); + int num; + //~ int pwm1, pwm2; + do + { + cin>>num; + if (num == 1) + { + ENCODERReset(1); + ENCODERReset(2); + do + { + MOTORDriveRaw(1,21); + MOTORDriveRaw(2,-19); + } while (abs(ENCODERRead(2)) < 217); + MOTORDriveRaw(1,0); + MOTORDriveRaw(2,0); + } + + else if (num == 2) + { + ENCODERReset(1); + ENCODERReset(2); + do + { + MOTORDriveRaw(1,21); + MOTORDriveRaw(2,-19); + } while (abs(ENCODERRead(2)) < 500); + MOTORDriveRaw(1,0); + MOTORDriveRaw(2,0); + } + + else if (num == 3) + { + ENCODERReset(1); + ENCODERReset(2); + do + { + MOTORDriveRaw(1,-21); + MOTORDriveRaw(2,19); + } while (abs(ENCODERRead(2)) < 242); + MOTORDriveRaw(1,0); + MOTORDriveRaw(2,0); + } + + else if (num == 4) + { + ENCODERReset(1); + ENCODERReset(2); + do + { + MOTORDriveRaw(1,-21); + } while (abs(ENCODERRead(1)) < 285); + MOTORDriveRaw(1,0); + do + { + MOTORDriveRaw(2,19); + } while (abs(ENCODERRead(2)) < 285); + MOTORDriveRaw(2,0); + } + + else + { + cout << " num error " << endl; + } + + } while (KEYRead() != KEY4); + +} diff --git a/文件/eyebot函数.cpp b/文件/eyebot函数.cpp index dacb526..0c2bf3c 100644 --- a/文件/eyebot函数.cpp +++ b/文件/eyebot函数.cpp @@ -6,3 +6,5 @@ VWSetSpeed(); VWGetPosition(); VWSetPosition(); VWWait(); +ENCODERReset(); +ENCODERRead();