2025.5.15第二次提交，按照实际场景初步修改代码。

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<L_PSD && L_PSD<180  &&  100<R_PSD && R_PSD<180)   //如果两侧都有墙的情况
+    if (50<L_PSD && L_PSD<180  &&  50<R_PSD && R_PSD<180)   //如果两侧都有墙的情况
     {
       err = L_PSD - R_PSD;
       PID_AL();
     }
-    else if (100<L_PSD && L_PSD<180)    //只有左侧有墙的情况
+    else if (50<L_PSD && L_PSD<180)    //只有左侧有墙的情况
     {
-      err = L_PSD - DIST_wall;
+      err = L_PSD - DIST_wall_f;
       PID_AL();
     }
-    else if (100<R_PSD && R_PSD<180)    //只有右侧有墙的情况
+    else if (50<R_PSD && R_PSD<180)    //只有右侧有墙的情况
     {
-      err = DIST_wall - R_PSD;
+      err = DIST_wall_f - R_PSD;
       PID_AL();
     }
     else
@@ -101,6 +126,6 @@ void PIDStraight()
     {
       speed1 = speed2;
     }
-  }while (DIST_move < DIST_cell && F_PSD > DIST_wall);    //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
+  }while (DIST_move < DIST_cell && F_PSD > DIST_wall_f);    //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
   //VWSetSpeed(0,0);
 }

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();
         }
         //直走

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