From 5ce7796b14483098fe8352f15a92242900593017 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C3=91=C3=B5=D4=AD=C3=97=C3=93?= Date: Sat, 17 May 2025 15:04:27 +0800 Subject: [PATCH] =?UTF-8?q?2025.5.17=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=AC16=E5=8F=B7=E5=92=8C?= =?UTF-8?q?=E4=BB=8A=E5=A4=A9=EF=BC=8C=E5=9C=A8=E5=AE=9E=E6=9C=BA=E4=B8=8A?= =?UTF-8?q?=EF=BC=8C=E5=AF=B9=E4=BB=A3=E7=A0=81=E4=BF=AE=E6=94=B9=E5=90=8E?= =?UTF-8?q?=E7=9A=84=E7=BB=93=E6=9E=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robot_test_code/LCD输出测试/makefile | 20 +++++ robot_test_code/PID/PID.cpp | 105 ++++++++++++++---------- robot_test_code/PID/PIDmain.cpp | 69 ++++++++++------ robot_test_code/实机测试/makefile | 26 ++++++ robot_test_code/实机测试/test_psd.cpp | 22 +++++ robot_test_code/实机测试/test_run.cpp | 26 ++++-- robot_test_code/实机测试/test_turn.cpp | 106 +++++++++++++++++++++++++ 7 files changed, 300 insertions(+), 74 deletions(-) create mode 100644 robot_test_code/LCD输出测试/makefile create mode 100644 robot_test_code/实机测试/makefile create mode 100644 robot_test_code/实机测试/test_psd.cpp create mode 100644 robot_test_code/实机测试/test_turn.cpp diff --git a/robot_test_code/LCD输出测试/makefile b/robot_test_code/LCD输出测试/makefile new file mode 100644 index 0000000..77dd6c8 --- /dev/null +++ b/robot_test_code/LCD输出测试/makefile @@ -0,0 +1,20 @@ + +cxx := gccarm + +file_cpp := $(wildcard *.cpp) +file_o := /home/pi/usr/pid.x + +$(file_o) : $(file_cpp) + @$(cxx) $^ -o $@ + +maze : $(file_o) + +clean : + @rm -rf $(file_o) + +debug : + @echo $(cxx) + @echo $(file_cpp) + @echo $(file_o) + +.PHONY : maze clean debug \ No newline at end of file diff --git a/robot_test_code/PID/PID.cpp b/robot_test_code/PID/PID.cpp index 1aab213..4213cdc 100644 --- a/robot_test_code/PID/PID.cpp +++ b/robot_test_code/PID/PID.cpp @@ -4,24 +4,27 @@ #include"PID.h" using namespace std; -float Kp = 0.2, Ki = 0.1, Kd = 0.1; //定义PID的参数 +float Kp = 0.79, Ki = 0.009, Kd = 0.14; //定义PID的参数 float Kpid; int Kpid_speed; -const int speed = 18; -const float Kpid_base = 0.4; +const int speed_l = 17; +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 = 20; +const int eI_max = 200; /*---定义全局常量---*/ /*---定义全局常量---*/ // const int DIST_cell = 300 ; // const int DIST_wall = 100 ; -const int DIST_cell = 300; -const int DIST_wall_f = 80; -const int DIST_wall_rl = 87; +const int DIST_cell = 310; +const int DIST_wall_f = 120; +const int DIST_wall_rl = 90; /*---全局各种变量与常量定义完成---*/ @@ -58,13 +61,13 @@ void PID_AL() Kpid = Kp*err + Ki*err_sum + Kd*err_diff; //PID控制算法输出的倍率 Kpid_speed = Kpid * Kpid_base; - if (Kpid_speed >= 6) //对pid输出值进行限制，防止过大电机过快，或者减的过小使电机不转动 + if (Kpid_speed >= speed_err) //对pid输出值进行限制，防止过大电机过快，或者减的过小使电机不转动 { - Kpid_speed = 6; + Kpid_speed = speed_err; } - else if (Kpid_speed <= -6) + else if (Kpid_speed <= -speed_err) { - Kpid_speed = -6; + Kpid_speed = -speed_err; } else { @@ -73,9 +76,10 @@ void PID_AL() /*---打印输出PID控制的各项参数2---*/ cout << "PID_out:" << Kpid_speed << " " ; /*---打印结束---*/ + + MOTORDriveRaw(2,Kpid_speed + speed_r); //控制小车的行驶速度，角速度 + MOTORDriveRaw(1,speed_l); - MOTORDriveRaw(2,speed + Kpid_speed); //控制小车的行驶速度，角速度 - MOTORDriveRaw(1,speed); } /*---PID控制下的直线行驶---*/ @@ -83,48 +87,56 @@ void PIDStraight() { int DIST_move; //定义基础角速度和速度,和移动距离 int L_PSD, R_PSD, F_PSD; //定义左侧，右侧，前方的距离值 - int DIST_move_x1; - int DIST_move_x2; - // int L_PSD_old, R_PSD_old, F_PSD_old; - // int x_1,x_2, y_1,y_2, phi_1,phi_2; //VWGetPosition的参数定义 + //~ int DIST_move_x1; + //~ int DIST_move_x2; + //~ int L_PSD_old, R_PSD_old, F_PSD_old; + int x_1,x_2, y_1,y_2, phi_1,phi_2; //VWGetPosition的参数定义 // speed1 = speed; - F_PSD = PSDGetRaw(1); - // F_PSD_old = F_PSD; - // L_PSD = PSDGetRaw(2) - 18; - // L_PSD_old = L_PSD; - // R_PSD = PSDGetRaw(3); - // R_PSD_old = R_PSD; + + //~ F_PSD = PSDGetRaw(1); + //~ F_PSD_old = F_PSD; + + //~ L_PSD = PSDGetRaw(2) - 8; + //~ L_PSD_old = L_PSD; + + //~ R_PSD = PSDGetRaw(3); + //~ R_PSD_old = R_PSD; + + VWGetPosition(&x_1, &y_1, &phi_1); //获取机器人在移动前的x,y,phi值 + //~ DIST_move_x1 = F_PSD; + + MOTORDriveRaw(2,speed_r); + MOTORDriveRaw(1,speed_l); - // VWGetPosition(&x_1, &y_1, &phi_1); //获取机器人在移动前的x,y,phi值 - DIST_move_x1 = F_PSD; - MOTORDriveRaw(1,speed); - MOTORDriveRaw(2,speed); do { - F_PSD = PSDGetRaw(1); //读取前方和墙壁的距离 - // F_PSD = F_PSD * 0.4 + F_PSD_old * 0.6; - // F_PSD_old = F_PSD; - L_PSD = PSDGetRaw(2) - 18; //读取左侧和墙壁的距离 - // L_PSD = L_PSD * 0.4 + L_PSD_old * 0.6; - // L_PSD_old = L_PSD; + F_PSD = PSDGetRaw(1) - 5; //读取前方和墙壁的距离 + //~ F_PSD = PSDGetRaw(1) * 0.3 + F_PSD_old * 0.7; + //~ F_PSD_old = F_PSD; + + L_PSD = PSDGetRaw(2) - 10; //读取左侧和墙壁的距离 + //~ L_PSD = PSDGetRaw(2) * 0.3 + L_PSD_old * 0.7; + //~ L_PSD_old = L_PSD; + R_PSD = PSDGetRaw(3); //读取右侧和墙壁的距离 - // R_PSD = R_PSD * 0.4 + R_PSD_old * 0.6; - // R_PSD_old = R_PSD; + //~ R_PSD = PSDGetRaw(3) * 0.3 + R_PSD_old * 0.7; + //~ R_PSD_old = R_PSD; + /*---打印输出PSD数值---*/ cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << " " ; /*---打印结束---*/ - if (25 (DIST_cell -50)) //当移动距离只剩下最后的50mm的时候降点速度 // { // speed1 = speed2; // } + }while (DIST_move < DIST_cell && F_PSD > DIST_wall_f); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行 + //~ }while (F_PSD > DIST_wall_f); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行 + //~ MOTORDriveRaw(1,-15); + //~ MOTORDriveRaw(2,-15); MOTORDriveRaw(1,0); MOTORDriveRaw(2,0); //VWSetSpeed(0,0); diff --git a/robot_test_code/PID/PIDmain.cpp b/robot_test_code/PID/PIDmain.cpp index c674eab..1854b9c 100644 --- a/robot_test_code/PID/PIDmain.cpp +++ b/robot_test_code/PID/PIDmain.cpp @@ -8,45 +8,62 @@ using namespace std; // const int DIST_cell = 300 ; // const int DIST_wall = 100 ; const int DIST_cell = 300; -const int DIST_wall_f = 80; -const int DIST_wall_rl = 87; +const int DIST_wall_f = 120; +const int DIST_wall_rl = 90; /*---全局各种变量与常量定义完成---*/ /*---下面是主函数---*/ int main() { - int Fwall,Lwall,Rwall; + //~ int Fwall,Lwall,Rwall; + int F_wall; LCDMenu("","","","END"); - + int num; + do { //检测墙面 - Fwall = PSDGetRaw(1) > DIST_cell; //检测前面侧是否有墙 - Lwall = PSDGetRaw(2) > DIST_cell; //检测左面 - Rwall = PSDGetRaw(3) > DIST_cell; //检测右面 - - //判断旋转方向 - if (Lwall) + + //~ Fwall = PSDGetRaw(1) > DIST_cell; //检测前面侧是否有墙 + //~ Lwall = PSDGetRaw(2) > DIST_cell; //检测左面 + //~ Rwall = PSDGetRaw(3) > DIST_cell; //检测右面 + F_wall = PSDGetRaw(1); + + if (F_wall < DIST_wall_f) { - VWTurn(+90,15); //向左旋转 - VWWait(); + goto end_main; } - else if (Fwall) + //~ //判断旋转方向 + //~ if (Lwall) + //~ { + //~ VWTurn(+90,15); //向左旋转 + //~ VWWait(); + //~ } + //~ else if (Fwall) + //~ { + //~ //不旋转 + //~ } + //~ else if (Rwall) + //~ { + //~ VWTurn(-90,15); //向右旋转 + //~ VWWait(); + //~ } + //~ else + //~ { + //~ VWTurn(180,15); + //~ VWWait(); + //~ } + //~ //直走 + + cin >> num ; + if (num) { - //不旋转 - } - else if (Rwall) - { - VWTurn(-90,15); //向右旋转 - VWWait(); - } - else - { - VWTurn(180,15); - VWWait(); - } - //直走 PIDStraight(); + } + //~ PIDStraight(); VWWait(); + } while (KEYRead() != KEY4); + end_main: + cout << "over" << endl; } diff --git a/robot_test_code/实机测试/makefile b/robot_test_code/实机测试/makefile new file mode 100644 index 0000000..f43ef4c --- /dev/null +++ b/robot_test_code/实机测试/makefile @@ -0,0 +1,26 @@ + +cxx := gccarm + +#~ file_cpp := $(wildcard *.cpp) +file_cpp := ./test_turn.cpp +#~ file_cpp := ./test_run.cpp +#~ file_cpp := ./test_psd.cpp + +#~ file_o := /home/pi/usr/move.x +file_o := /home/pi/usr/turn.x +#~ file_o := /home/pi/usr/psd.x + +$(file_o) : $(file_cpp) + @$(cxx) $^ -o $@ + +maze : $(file_o) + +clean : + @rm -rf $(file_o) + +debug : + @echo $(cxx) + @echo $(file_cpp) + @echo $(file_o) + +.PHONY : maze clean debug diff --git a/robot_test_code/实机测试/test_psd.cpp b/robot_test_code/实机测试/test_psd.cpp new file mode 100644 index 0000000..010d5be --- /dev/null +++ b/robot_test_code/实机测试/test_psd.cpp @@ -0,0 +1,22 @@ +#include +#include +#include"eyebot++.h" +using namespace std; + + + +int main() +{ + int Fwall,Lwall,Rwall,bwall; + LCDMenu("","","","END"); + do + { + Fwall = PSDGetRaw(1); //检测前面侧是否有墙 + Lwall = PSDGetRaw(2); //检测左面 + Rwall = PSDGetRaw(3); //检测右面 + bwall = PSDGetRaw(4); // + cout << "PSD L:" << Lwall << " R:" << Rwall << " F:" << Fwall << " b:" << bwall << endl; ; + + } while (KEYRead() != KEY4); + +} diff --git a/robot_test_code/实机测试/test_run.cpp b/robot_test_code/实机测试/test_run.cpp index 4d150b5..30a2a83 100644 --- a/robot_test_code/实机测试/test_run.cpp +++ b/robot_test_code/实机测试/test_run.cpp @@ -7,13 +7,27 @@ using namespace std; int main() { - int Fwall,Lwall,Rwall; LCDMenu("","","","END"); - + //~ int num; do - { //检测墙面 - MOTORDriveRaw(1,50); - MOTORDriveRaw(2,50); + { + MOTORDriveRaw(1,16); + MOTORDriveRaw(2,14); + //~ cin>>num; + //~ if(num) + //~ { + //~ MOTORDriveRaw(1,25); + //~ MOTORDriveRaw(2,-24); + //~ OSWait(1650); + + //~ MOTORDriveRaw(1,18); + //~ MOTORDriveRaw(2,-16); + //~ OSWait(3500); + + //~ MOTORDriveRaw(1,0); + //~ MOTORDriveRaw(2,0); + //~ } } while (KEYRead() != KEY4); -} \ No newline at end of file + +} diff --git a/robot_test_code/实机测试/test_turn.cpp b/robot_test_code/实机测试/test_turn.cpp new file mode 100644 index 0000000..9fe5062 --- /dev/null +++ b/robot_test_code/实机测试/test_turn.cpp @@ -0,0 +1,106 @@ +#include +#include +#include"eyebot++.h" +using namespace std; + + + +int main() +{ + //~ int f_wall, r_wall, l_wall, b_wall; + //~ int old_f_wall, old_r_wall, old_l_wall, old_b_wall; + int b_wall; + int old_b_wall; + int turn_check; + LCDMenu("","","","END"); + int num; + do + { + cin>>num; + if(num == 1) + { + b_wall = PSDGetRaw(4) - 2; + do + { + old_b_wall = b_wall; + MOTORDriveRaw(1,19); + MOTORDriveRaw(2,-17); + //~ MOTORDriveRaw(1,24); + //~ MOTORDriveRaw(2,-22); + OSWait(850); + //~ OSWait(1980); + MOTORDriveRaw(1,0); + MOTORDriveRaw(2,0); + b_wall = (PSDGetRaw(4) - 2) * 0.7 + old_b_wall * 0.3; //减小突变 + //~ b_wall = PSDGetRaw(4) - 2; + turn_check = b_wall - old_b_wall; + /*---打印输出PSD数值---*/ + //~ cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << " " ; + cout << "b_wall:" << b_wall << " check:" << turn_check << endl; + /*---打印结束---*/ + OSWait(50); + } while ( turn_check < -3 || b_wall > 130); + } + else if (num == 3) + { + b_wall = PSDGetRaw(4) - 2; + do + { + old_b_wall = b_wall; + MOTORDriveRaw(1,-19); + MOTORDriveRaw(2,17); + //~ MOTORDriveRaw(1,24); + //~ MOTORDriveRaw(2,-22); + OSWait(850); + //~ OSWait(1980); + MOTORDriveRaw(1,0); + MOTORDriveRaw(2,0); + b_wall = (PSDGetRaw(4) - 2) * 0.7 + old_b_wall * 0.3; //减小突变 + //~ b_wall = PSDGetRaw(4) - 2; + turn_check = b_wall - old_b_wall; + /*---打印输出PSD数值---*/ + //~ cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << " " ; + cout << "b_wall:" << b_wall << " check:" << turn_check << endl; + /*---打印结束---*/ + OSWait(50); + } while (turn_check < -3 || b_wall > 130); + } + //~ if(num == 1) + //~ { + //~ MOTORDriveRaw(1,19); + //~ MOTORDriveRaw(2,-17); + //~ MOTORDriveRaw(1,24); + //~ MOTORDriveRaw(2,-22); + //~ OSWait(1500); + //~ OSWait(1980); + //~ MOTORDriveRaw(1,0); + //~ MOTORDriveRaw(2,0); + //~ } + //~ else if(num == 2) + //~ { + //~ MOTORDriveRaw(1,24); + //~ MOTORDriveRaw(2,-22); + //~ MOTORDriveRaw(1,24); + //~ MOTORDriveRaw(2,-22); + //~ OSWait(2700); + //~ OSWait(3250); + //~ MOTORDriveRaw(1,0); + //~ MOTORDriveRaw(2,0); + //~ } + //~ else if(num == 3) + //~ { + //~ MOTORDriveRaw(1,26); + //~ MOTORDriveRaw(2,-24); + //~ MOTORDriveRaw(1,-24); + //~ MOTORDriveRaw(2,22); + //~ OSWait(1815); + //~ OSWait(2025); + //~ MOTORDriveRaw(1,0); + //~ MOTORDriveRaw(2,0); + //~ } + else + { + } + } while (KEYRead() != KEY4); + +}