2025.5.17第一次提交，包括16号和今天，在实机上，对代码修改后的结果

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

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 int speed_l = 17;
-const float Kpid_base = 0.4;
+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_cell = 310;
-const int DIST_wall_f = 80;
+const int DIST_wall_f = 120;
-const int DIST_wall_rl = 87;
+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_x1;
-  int DIST_move_x2;
+  //~ int DIST_move_x2;
-  // int L_PSD_old, R_PSD_old, F_PSD_old;
+  //~ 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 x_1,x_2, y_1,y_2, phi_1,phi_2;    //VWGetPosition的参数定义
   // speed1 = speed;
-  F_PSD = PSDGetRaw(1);
+  
-  // F_PSD_old = F_PSD;
+  //~ F_PSD = PSDGetRaw(1);
-  // L_PSD = PSDGetRaw(2) - 18;
+  //~ F_PSD_old = F_PSD;
-  // L_PSD_old = L_PSD;
+  
-  // R_PSD = PSDGetRaw(3);
+  //~ L_PSD = PSDGetRaw(2) - 8;
-  // R_PSD_old = R_PSD;
+  //~ 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 = PSDGetRaw(1) - 5;    //读取前方和墙壁的距离
-    // F_PSD = F_PSD * 0.4  +  F_PSD_old * 0.6;
+    //~ F_PSD = PSDGetRaw(1) * 0.3 + F_PSD_old * 0.7;
-    // F_PSD_old = F_PSD;
+    //~ F_PSD_old = F_PSD;
-    L_PSD = PSDGetRaw(2) - 18;   //读取左侧和墙壁的距离
+    
-    // L_PSD = L_PSD * 0.4  +  L_PSD_old * 0.6;
+    L_PSD = PSDGetRaw(2) - 10;   //读取左侧和墙壁的距离
-    // L_PSD_old = L_PSD;
+    //~ 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 = PSDGetRaw(3) * 0.3 + R_PSD_old * 0.7;
-    // R_PSD_old = R_PSD;
+    //~ R_PSD_old = R_PSD;
+    
     /*---打印输出PSD数值---*/
     cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << " " ;
     /*---打印结束---*/
-    if (25<L_PSD && L_PSD<180  &&  50<R_PSD && R_PSD<180)   //如果两侧都有墙的情况
+    if (20<L_PSD && L_PSD<180  &&  50<R_PSD && R_PSD<180)   //如果两侧都有墙的情况
     {
       err = L_PSD - R_PSD;
       PID_AL();
     }
-    else if (25<L_PSD && L_PSD<180)    //只有左侧有墙的情况
+    else if (20<L_PSD && L_PSD<180)    //只有左侧有墙的情况
     {
       err = L_PSD - DIST_wall_rl;
       PID_AL();
     }
-    else if (25<R_PSD && R_PSD<180)    //只有右侧有墙的情况
+    else if (20<R_PSD && R_PSD<180)    //只有右侧有墙的情况
     {
       err = DIST_wall_rl - R_PSD;
       PID_AL();
@@ -136,16 +148,25 @@ void PIDStraight()
       err = L_PSD - R_PSD;
       PID_AL();
     }
-    // VWGetPosition(&x_2, &y_2, &phi_2);
+    
-    // DIST_move = sqrt(pow(x_2-x_1,2) + pow(y_2-y_1,2));
+    VWGetPosition(&x_2, &y_2, &phi_2);
-    DIST_move_x2 = F_PSD;
+    DIST_move = sqrt(pow(x_2-x_1,2) + pow(y_2-y_1,2));
-    DIST_move = DIST_move_x1 - DIST_move_x2;
+    
+    //~ 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);    //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
+  //~ }while (F_PSD > DIST_wall_f);    //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
+  //~ MOTORDriveRaw(1,-15);
+  //~ MOTORDriveRaw(2,-15);
   MOTORDriveRaw(1,0);
   MOTORDriveRaw(2,0);
   //VWSetSpeed(0,0);

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_f = 120;
-const int DIST_wall_rl = 87;
+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;    //检测左面
+        //~ Fwall = PSDGetRaw(1) > DIST_cell;   //检测前面侧是否有墙
-        Rwall = PSDGetRaw(3) > DIST_cell;   //检测右面
+        //~ Lwall = PSDGetRaw(2) > DIST_cell;    //检测左面
-
+        //~ Rwall = PSDGetRaw(3) > DIST_cell;   //检测右面
-        //判断旋转方向
+        F_wall = PSDGetRaw(1);
-        if (Lwall)
+        
+        if (F_wall < DIST_wall_f)
         {
-            VWTurn(+90,15);     //向左旋转
+            goto end_main;
-            VWWait();
         }
-        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;
 }

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

robot_test_code/实机测试/test_psd.cpp

@@ -0,0 +1,22 @@
+#include<iostream>
+#include<string>
+#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);
+    
+}

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(1,16);
-        MOTORDriveRaw(2,50);
+        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);
-}
+    
+}

robot_test_code/实机测试/test_turn.cpp

@@ -0,0 +1,106 @@
+#include<iostream>
+#include<string>
+#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);
+    
+}