diff --git a/robot_test_code/PID/PID.cpp b/robot_test_code/PID/PID.cpp index e9173f7..b047fbf 100644 --- a/robot_test_code/PID/PID.cpp +++ b/robot_test_code/PID/PID.cpp @@ -4,20 +4,24 @@ #include"PID.h" using namespace std; -float Kp = 1, Ki = 0, Kd = 0.5; //定义PID的参数 +float Kp = 0.5, Ki = 0.3, Kd = 0.1; //定义PID的参数 float Kpid; float Kpid_speed; -const int Kpid_base = 1, speed = 200; -const int speed2 = 0.25*speed; +const int speed = 16; +const float Kpid_base = 0.1; +const int speed2 = 0.75*speed; int speed1; //int speed2 = speed; /*---定义全局变量---*/ int err, err_old, err_sum, err_diff; //定义误差,上一次误差,积分误差,微分误差 -const int eI_max = 50; +const int eI_max = 30; /*---定义全局常量---*/ /*---定义全局常量---*/ -const int DIST_cell = 300 ; -const int DIST_wall = 100 ; +// 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; /*---全局各种变量与常量定义完成---*/ @@ -28,6 +32,14 @@ void eI_xianfu(int eI_max) { err_sum = eI_max; } + else if (err_sum <= -eI_max) + { + err_sum = -eI_max; + } + else + { + //此处为空; + } } /*---位置式PID算法---*/ @@ -37,19 +49,32 @@ void PID_AL() 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" ; + cout << "err:" << err << "\t" ; + cout << "err_old:" << err_old << "\t" ; + cout << "err_sum:" << err_sum << "\t" ; + cout << "err_diff:" << err_diff << "\t" ; /*---打印结束---*/ err_old = err; //将误差幅值到上一次误差 Kpid = Kp*err + Ki*err_sum + Kd*err_diff; //PID控制算法输出的倍率 Kpid_speed = Kpid * Kpid_base; + if (Kpid_speed >= 8) //对pid输出值进行限制,防止过大电机过快,或者减的过小使电机不转动 + { + Kpid_speed = 8; + } + else if (Kpid_speed <= -8) + { + Kpid_speed = -8; + } + else + { + //error + } /*---打印输出PID控制的各项参数2---*/ - // cout << "PID输出值:" << Kpid << endl ; + cout << "PID_out:" << Kpid << endl ; /*---打印结束---*/ - MOTORDriveRaw(2,speed1+Kpid_speed); //控制小车的行驶速度,角速度 + MOTORDriveRaw(1,speed1); + MOTORDriveRaw(2,speed1 + Kpid_speed); //控制小车的行驶速度,角速度 } /*---PID控制下的直线行驶---*/ @@ -67,25 +92,25 @@ void PIDStraight() do { - L_PSD = PSDGetRaw(2); //读取左侧和墙壁的距离 - R_PSD = PSDGetRaw(3); //读取右侧和墙壁的距离 F_PSD = PSDGetRaw(1); //读取前方和墙壁的距离 + 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 (100 DIST_wall); //当移动到指定距离,或者检测到小车前方的距离小于检测距离的时候,停止运行 + }while (DIST_move < DIST_cell && F_PSD > DIST_wall_f); //当移动到指定距离,或者检测到小车前方的距离小于检测距离的时候,停止运行 //VWSetSpeed(0,0); } diff --git a/robot_test_code/PID/PIDmain.cpp b/robot_test_code/PID/PIDmain.cpp index d9278bd..d127aad 100644 --- a/robot_test_code/PID/PIDmain.cpp +++ b/robot_test_code/PID/PIDmain.cpp @@ -5,8 +5,11 @@ using namespace std; /*---定义全局常量---*/ -const int DIST_cell = 300 ; -const int DIST_wall = 100 ; +// 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; /*---全局各种变量与常量定义完成---*/ @@ -25,7 +28,7 @@ int main() //判断旋转方向 if (Lwall) { - VWTurn(+90,45); //向左旋转 + VWTurn(+90,10); //向左旋转 VWWait(); } else if (Fwall) @@ -34,12 +37,12 @@ int main() } else if (Rwall) { - VWTurn(-90,45); //向右旋转 + VWTurn(-90,10); //向右旋转 VWWait(); } else { - VWTurn(180,45); + VWTurn(180,10); VWWait(); } //直走 diff --git a/robot_test_code/PID/makefile b/robot_test_code/PID/makefile new file mode 100644 index 0000000..77dd6c8 --- /dev/null +++ b/robot_test_code/PID/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