From 69f91d3b52b37eaf47b31881d4b79cc71db1f7bd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C3=91=C3=B5=D4=AD=C3=97=C3=93?= Date: Thu, 15 May 2025 20:21:34 +0800 Subject: [PATCH] =?UTF-8?q?2025.5.15=E6=B5=8B=E8=AF=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robot_test_code/PID/PID.cpp | 101 +++++++++++++++++++------------- robot_test_code/PID/PIDmain.cpp | 11 ++-- 2 files changed, 67 insertions(+), 45 deletions(-) diff --git a/robot_test_code/PID/PID.cpp b/robot_test_code/PID/PID.cpp index b047fbf..1aab213 100644 --- a/robot_test_code/PID/PID.cpp +++ b/robot_test_code/PID/PID.cpp @@ -4,24 +4,24 @@ #include"PID.h" using namespace std; -float Kp = 0.5, Ki = 0.3, Kd = 0.1; //定义PID的参数 +float Kp = 0.2, Ki = 0.1, Kd = 0.1; //定义PID的参数 float Kpid; -float Kpid_speed; -const int speed = 16; -const float Kpid_base = 0.1; -const int speed2 = 0.75*speed; -int speed1; +int Kpid_speed; +const int speed = 18; +const float Kpid_base = 0.4; +// const int speed2 = 0.75*speed; +// int speed1; //int speed2 = speed; /*---定义全局变量---*/ int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差 -const int eI_max = 30; +const int eI_max = 20; /*---定义全局常量---*/ /*---定义全局常量---*/ // const int DIST_cell = 300 ; // const int DIST_wall = 100 ; const int DIST_cell = 300; -const int DIST_wall_f = 68; -// const int DIST_wall_rl = 88; +const int DIST_wall_f = 80; +const int DIST_wall_rl = 87; /*---全局各种变量与常量定义完成---*/ @@ -49,32 +49,33 @@ void PID_AL() eI_xianfu(eI_max); //积分部分限幅 err_diff = err - err_old; //微分部分误差值 /*---打印输出PID控制的各项参数1---*/ - cout << "err:" << err << "\t" ; - cout << "err_old:" << err_old << "\t" ; - cout << "err_sum:" << err_sum << "\t" ; - cout << "err_diff:" << err_diff << "\t" ; + cout << "err:" << err << " " ; + cout << "err_old:" << 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; - if (Kpid_speed >= 8) //对pid输出值进行限制，防止过大电机过快，或者减的过小使电机不转动 + if (Kpid_speed >= 6) //对pid输出值进行限制，防止过大电机过快，或者减的过小使电机不转动 { - Kpid_speed = 8; + Kpid_speed = 6; } - else if (Kpid_speed <= -8) + else if (Kpid_speed <= -6) { - Kpid_speed = -8; + Kpid_speed = -6; } else { //error } /*---打印输出PID控制的各项参数2---*/ - cout << "PID_out:" << Kpid << endl ; + cout << "PID_out:" << Kpid_speed << " " ; /*---打印结束---*/ - MOTORDriveRaw(1,speed1); - MOTORDriveRaw(2,speed1 + Kpid_speed); //控制小车的行驶速度，角速度 + + MOTORDriveRaw(2,speed + Kpid_speed); //控制小车的行驶速度，角速度 + MOTORDriveRaw(1,speed); } /*---PID控制下的直线行驶---*/ @@ -82,35 +83,50 @@ 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; + 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; - VWGetPosition(&x_1, &y_1, &phi_1); //获取机器人在移动前的x,y,phi值 - - MOTORDriveRaw(1,speed1); - MOTORDriveRaw(2,speed1); + // 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); //读取前方和墙壁的距离 - L_PSD = PSDGetRaw(2) - 16; //读取左侧和墙壁的距离 + // 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; R_PSD = PSDGetRaw(3); //读取右侧和墙壁的距离 + // R_PSD = R_PSD * 0.4 + R_PSD_old * 0.6; + // R_PSD_old = R_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 << " " ; /*---打印结束---*/ - if (50 (DIST_cell -50)) //当移动距离只剩下最后的50mm的时候降点速度 - { - speed1 = speed2; - } - }while (DIST_move < DIST_cell && F_PSD > DIST_wall_f); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行 + // VWGetPosition(&x_2, &y_2, &phi_2); + // DIST_move = sqrt(pow(x_2-x_1,2) + pow(y_2-y_1,2)); + DIST_move_x2 = F_PSD; + DIST_move = DIST_move_x1 - DIST_move_x2; + cout << "move: " << DIST_move << endl; + // if (DIST_move > (DIST_cell -50)) //当移动距离只剩下最后的50mm的时候降点速度 + // { + // speed1 = speed2; + // } + }while (DIST_move < DIST_cell && F_PSD > DIST_wall_f); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行 + 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 d127aad..c674eab 100644 --- a/robot_test_code/PID/PIDmain.cpp +++ b/robot_test_code/PID/PIDmain.cpp @@ -8,8 +8,8 @@ using namespace std; // const int DIST_cell = 300 ; // const int DIST_wall = 100 ; const int DIST_cell = 300; -const int DIST_wall_f = 68; -// const int DIST_wall_rl = 88; +const int DIST_wall_f = 80; +const int DIST_wall_rl = 87; /*---全局各种变量与常量定义完成---*/ @@ -28,7 +28,7 @@ int main() //判断旋转方向 if (Lwall) { - VWTurn(+90,10); //向左旋转 + VWTurn(+90,15); //向左旋转 VWWait(); } else if (Fwall) @@ -37,15 +37,16 @@ int main() } else if (Rwall) { - VWTurn(-90,10); //向右旋转 + VWTurn(-90,15); //向右旋转 VWWait(); } else { - VWTurn(180,10); + VWTurn(180,15); VWWait(); } //直走 PIDStraight(); + VWWait(); } while (KEYRead() != KEY4); }