2025.5.18第一次提交，包括17号和今天上午对实机的调试

bot_code/makefile

@@ -1,8 +1,8 @@
 
-cxx := g++sim
+cxx := gccarm
 
 file_cpp := $(wildcard *.cpp)
-file_o := ../eyesim/all_test01/maze.x
+file_o := /home/pi/usr/maze.x
 
 $(file_o) : $(file_cpp)
 	@$(cxx) $^ -o $@

bot_code/maze_func.h

@@ -12,8 +12,8 @@ void explore();                                     //递归函数
 /*---以下为pid控制和行驶部分涉及到的函数声明---*/
 void xneighbor(int x, int dir);     //X坐标更新
 void yneighbor(int y, int dir);     //Y坐标更新
-void eI_xianfu(int eI_max);         //对PID控制的积分部分I限幅
+void eI_xianfu();         //对PID控制的积分部分I限幅
-void pid_speed_xianfu(int Kpid_speed);  //对PID控制的输出部分限幅
+void pid_speed_xianfu();  //对PID控制的输出部分限幅
 void PID_AL();                      //位置式PID算法
 void PIDStraight();                 //PID控制下的直线行驶
 void go_to(int dir);                //行驶到指定单元格

bot_code/maze_func_goto.cpp

@@ -68,13 +68,20 @@ void go_to(int dir)
   {
     turn = +1;
   }
+  if (turn == -2)
+  {
+    turn = 2;
+  }
   
   if (turn) //如果turn不是0，则执行旋转命令
   {
+    OSWait(1000);
     BOTturn(turn);
+    OSWait(200);
     VWWait();
   }
   PIDStraight();   //使用PID算法算法，向指定方向直线行驶一格
+  //~ OSWait(200);
   VWWait();   //等待下一条指令
   
   rob_dir = maze_dir;  //更新机器人移动到下一格之后的方向

bot_code/maze_func_pid.cpp

@@ -7,7 +7,7 @@ using namespace std;
 
 
 /*---对PID控制的积分部分I限幅---*/
-void eI_xianfu(int eI_max)
+void eI_xianfu()
 {
   if (err_sum >= eI_max)    //如果err_sum大于限幅，则直接让err_sum=限幅，防止err_sum过大，影响PID控制
   {
@@ -23,13 +23,13 @@ void eI_xianfu(int eI_max)
   }
 }
 
-void pid_speed_xianfu(int Kpid_speed)
+void pid_speed_xianfu()
 {
-  if (Kpid_speed > speed_xianfu)
+  if (Kpid_speed >= speed_xianfu)
   {
     Kpid_speed = speed_xianfu;
   }
-  else if (Kpid_speed < -speed_xianfu)
+  else if (Kpid_speed <= -speed_xianfu)
   {
     Kpid_speed = -speed_xianfu;
   }
@@ -42,81 +42,84 @@ void pid_speed_xianfu(int Kpid_speed)
 /*---位置式PID算法---*/
 void PID_AL()
 {
-  err_sum += err;   //积分累计误差值
+  //~ err_sum += err;   //积分累计误差值
-  eI_xianfu(eI_max);   //积分部分限幅
+  //~ eI_xianfu();   //积分部分限幅
   err_diff = err - err_old;   //微分部分误差值
   /*---打印输出PID控制的各项参数1---*/
-    cout << "本次误差:" << err << "\t" ;
+    //~ cout << "err:" << err << " " ;
-    cout << "上次误差:" << err_old << "\t" ;
+    //~ cout << "old_err:" << err_old << " " ;
-    cout << "误差累计:" << err_sum << "\t" ;
+    //~ cout << "err_sum:" << err_sum << " " ;
-    cout << "误差变化:" << err_diff << "\t" ;
+    //~ cout << "err_diff:" << err_diff << " " ;
   /*---打印结束---*/
   err_old = err;    //将误差幅值到上一次误差
   
-  Kpid = Kp*err + Ki*err_sum + Kd*err_diff;   //PID控制算法输出的倍率
+  //~ Kpid = Kp*err + Ki*err_sum + Kd*err_diff;   //PID控制算法输出的倍率
-  Kpid_speed = Kpid * Kpid_base;
+  //~ Kpid_speed = (Kp*err + Ki*err_sum + Kd*err_diff) * Kpid_base;
+  Kpid_speed = (Kp*err + Kd*err_diff) * Kpid_base;
+  
+  pid_speed_xianfu();
+  speed_r = speed_l + Kpid_speed;
 
-  pid_speed_xianfu(Kpid_speed);
 
   /*---打印输出PID控制的各项参数2---*/
-    cout << "PID输出值:" << Kpid_speed << "\t" ;
+    cout << "PID_out:" << Kpid_speed << endl ;
   /*---打印结束---*/
   // VWSetSpeed(speed,Kpid*Kpid_base);   //控制小车的行驶速度，角速度(模拟器可以用，实物no)
-  MOTORDriveRaw(1,speed1);
+ 
-  MOTORDriveRaw(2,speed2 + Kpid_speed);
+  MOTORDriveRaw(2,speed_r);
+  MOTORDriveRaw(1,speed_l);
 }
 
 /*---PID控制下的直线行驶---*/
 void PIDStraight()
 {
-  int DIST_move;    //定义基础角速度和速度,和移动距离
+  int bot_move;    //定义基础角速度和速度,和移动距离
   int L_PSD, R_PSD, F_PSD;    //定义左侧，右侧，前方的距离值
   int x_1,x_2, y_1,y_2, phi_1,phi_2;    //VWGetPosition的参数定义
-  speed1 = speed;
   VWGetPosition(&x_1, &y_1, &phi_1);    //获取机器人在移动前的x,y,phi值
   
-  MOTORDriveRaw(1,speed1);
+  MOTORDriveRaw(2,speed_r);
-  MOTORDriveRaw(2,speed2);
+  MOTORDriveRaw(1,speed_l);
+
 
   do 
   {
-    F_PSD = PSDGetRaw(1) - 5;    //读取前方和墙壁的距离
+    F_PSD = PSDGetRaw(1) - 2;    //读取前方和墙壁的距离
-    L_PSD = PSDGetRaw(2) - 8;   //读取左侧和墙壁的距离
+    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 (30<L_PSD && L_PSD<180  &&  30<R_PSD && R_PSD<180)   //如果两侧都有墙的情况
+    if (20<L_PSD && L_PSD<140  &&  20<R_PSD && R_PSD<140)   //如果两侧都有墙的情况
     {
       err = L_PSD - R_PSD;
       PID_AL();
     }
-    else if (30<L_PSD && L_PSD<180)    //只有左侧有墙的情况
+    else if (20<L_PSD && L_PSD<140)    //只有左侧有墙的情况
     {
       err = L_PSD - DIST_wall_RL;
       PID_AL();
     }
-    else if (30<R_PSD && R_PSD<180)    //只有右侧有墙的情况
+    else if (20<R_PSD && R_PSD<140)    //只有右侧有墙的情况
     {
       err = DIST_wall_RL - R_PSD;
       PID_AL();
     }
     else
     {
-      L_PSD = L_PSD % DIST_cell;
+      L_PSD = L_PSD % 300;
-      R_PSD = R_PSD % DIST_cell;
+      R_PSD = R_PSD % 300;
       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));
+    bot_move = sqrt(pow(x_2-x_1,2) + pow(y_2-y_1,2));
-    cout << "move:" << DIST_move << endl;
+    //~ cout << "move:" << bot_move << endl;
 
-  }while (DIST_move < DIST_cell && F_PSD > DIST_wall_F);    //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
+  }while (bot_move < DIST_move  &&  F_PSD > DIST_wall_F);    //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
 
   MOTORDriveRaw(1,0);
   MOTORDriveRaw(2,0);
-  
 }

bot_code/maze_func_turn.cpp

@@ -8,33 +8,44 @@ using namespace std;
 
 void BOTturn(int turn)
 {
-    if (turn == 1)
+    if (turn == -1)  //右转
     {
-        MOTORDriveRaw(1,24);
+        ENCODERReset(1);
-        MOTORDriveRaw(2,-22);
+        ENCODERReset(2);
+        do 
+		{
+			MOTORDriveRaw(1,21);
+			MOTORDriveRaw(2,-19);
             
-        OSWait(1985);
+		} while (abs(ENCODERRead(2)) < 218);
         
 		MOTORDriveRaw(1,0);
 		MOTORDriveRaw(2,0);
     }
-    else if (turn == -1)
+    else if (turn == 1)    //左转
     {
-        MOTORDriveRaw(1,-24);
+        ENCODERReset(1);
-        MOTORDriveRaw(2,22);
+		ENCODERReset(2);
+		do 
+		{
+			MOTORDriveRaw(1,-21);
+			MOTORDriveRaw(2,19);
 
-        OSWait(1985);
+		} while (abs(ENCODERRead(2)) < 240);
 		
 		MOTORDriveRaw(1,0);
 		MOTORDriveRaw(2,0);
 	}
-    else if (turn == 2)
+    else if (turn == 2)     //掉头
     {
-        MOTORDriveRaw(1,24);
+        ENCODERReset(1);
-        MOTORDriveRaw(2,-22);
+		ENCODERReset(2);
-
+		do 
-        OSWait(2985);
+		{
+			MOTORDriveRaw(1,21);
+			MOTORDriveRaw(2,-19);
 		
+		} while (abs(ENCODERRead(2)) < 500);
 		MOTORDriveRaw(1,0);
 		MOTORDriveRaw(2,0);
     }

bot_code/maze_parameter.cpp

@@ -8,9 +8,10 @@ using namespace std;
 
 /*---定义数据---*/
 /*---定义全局常量---*/
-const int DIST_cell = 300 ;     //单元格长度
+const int DIST_cell = 240;     	//单元格长度
-const int DIST_wall_F = 90 ;     //与墙壁的距离
+const int DIST_move = 292;		//移动的单元格距离
-extern const int DIST_wall_RL = 100;      //与左右墙壁的距离
+const int DIST_wall_F = 90;     //与墙壁的距离
+const int DIST_wall_RL = 88;      //与左右墙壁的距离
 
 /*---定义全局变量---*/
 /*---容器类---*/
@@ -28,14 +29,16 @@ int rob_dir = 0;    //机器人当前的朝向（初始为0）
 
 
 /*---PID算法---*/
-const float Kp = 0.8, Ki = 0.015, Kd = 0.15;   //定义PID的参数
+const float Kp = 0.4, Ki = 0, Kd = 0.25;   //定义PID的参数17
-float Kpid;                             //定义PID计算后输出的倍率
+//~ const float Kp = 0.825, Ki = 0.0081, Kd = 0.175;   //定义PID的参数17
+//~ const float Kp = 0.80, Ki = 0.0076, Kd = 0.2;   //定义PID的参数18
+//~ float Kpid;                             //定义PID计算后输出的倍率
 int Kpid_speed;
 const float Kpid_base = 1;				//定义基本角速度和速度
-const int speed = 18;                   //定义基本角速度和速度
+const int speed_l = 16;					//定义基本左轮速度
-const int speed2 = 0.75*speed;          //快到目的地时减速
+//~ const int speed_r = speed_l - 2;		//定义基本右轮速度
-const int speed_xianfu = 3;             //定义kpid限幅
+int speed_r = speed_l - 2;		//定义基本右轮速度
-int speed1;
+const int speed_xianfu = 2;				//定义kpid限幅
 int err, err_old, err_sum, err_diff;	//定义误差，上一次误差，积分误差，微分误差
 const int eI_max = 200;                 //定义积分限幅
 

bot_code/maze_parameter.h

@@ -4,6 +4,7 @@
 /*---声明数据---*/
 /*---声明全局常量---*/
 extern const int DIST_cell;         //单元格长度
+extern const int DIST_move;			//移动的单元格距离
 extern const int DIST_wall_F;		//与前面墙壁的距离
 extern const int DIST_wall_RL;      //与左右墙壁的距离
 
@@ -26,13 +27,13 @@ extern int rob_dir; //机器人当前的朝向
 
 /*---PID算法---*/
 extern const float Kp, Ki, Kd;                  //声明PID的参数
-extern float Kpid;                              //定义PID计算后输出的倍率
+//~ extern float Kpid;                              //定义PID计算后输出的倍率
 extern int Kpid_speed;                          //定义计算后的速度
 extern const float Kpid_base;					//定义速度基础倍数速度
-extern const int speed;                         //定义速度基础倍数速度
+extern const int speed_l;						//定义左轮基本速度
-extern const int speed2;                        //快到目的地时减速
+//~ extern const int speed_r;						//定义右轮基本速度
+extern int speed_r;						//定义右轮基本速度
 extern const int speed_xianfu;                  //kpid输出速度的限幅
-extern int speed1;
 extern int err, err_old, err_sum, err_diff;     //定义误差，上一次误差，积分误差，微分误差
 extern const int eI_max;                        //定义积分限幅
 

robot_test_code/PID/PID.cpp

@@ -4,26 +4,21 @@
 #include"PID.h"
 using namespace std;
 
-float Kp = 0.79, Ki = 0.009, Kd = 0.14;   //定义PID的参数
+float Kp = 0.815, Ki = 0.0083, Kd = 0.167;   //定义PID的参数
 float Kpid;
 int Kpid_speed;
-const int speed_l = 17;
+const int speed_l = 18;
 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 = 200;
 /*---定义全局常量---*/
 /*---定义全局常量---*/
-// const int DIST_cell = 300 ;
-// const int DIST_wall = 100 ;
 const int DIST_cell = 310;
-const int DIST_wall_f = 120;
+const int DIST_wall_f = 75;
 const int DIST_wall_rl = 90;
 /*---全局各种变量与常量定义完成---*/
 
@@ -126,7 +121,7 @@ void PIDStraight()
     /*---打印输出PSD数值---*/
     cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << " " ;
     /*---打印结束---*/
-    if (20<L_PSD && L_PSD<180  &&  50<R_PSD && R_PSD<180)   //如果两侧都有墙的情况
+    if (20<L_PSD && L_PSD<180  &&  20<R_PSD && R_PSD<180)   //如果两侧都有墙的情况
     {
       err = L_PSD - R_PSD;
       PID_AL();

robot_test_code/PID/PIDmain.cpp

@@ -7,8 +7,8 @@ using namespace std;
 /*---定义全局常量---*/
 // 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 = 120;
+const int DIST_wall_f = 75;
 const int DIST_wall_rl = 90;
 /*---全局各种变量与常量定义完成---*/
 

robot_test_code/实机测试/makefile

@@ -1,14 +1,16 @@
 
 cxx := gccarm
+#~ cxx := gpparm
 
 #~ file_cpp := $(wildcard *.cpp)
-file_cpp := ./test_turn.cpp
+#~ file_cpp := ./test_turn.cpp
+#~ file_cpp := ./test_turn2.cpp
 #~ file_cpp := ./test_run.cpp
-#~ file_cpp := ./test_psd.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/turn.x
-#~ file_o := /home/pi/usr/psd.x
+file_o := /home/pi/usr/psd.x
 
 $(file_o) : $(file_cpp)
 	@$(cxx) $^ -o $@

robot_test_code/实机测试/test_psd.cpp

@@ -11,8 +11,8 @@ int main()
     LCDMenu("","","","END");
     do
     {
-        Fwall = PSDGetRaw(1);   //检测前面侧是否有墙
+        Fwall = PSDGetRaw(1)-2;   //检测前面侧是否有墙
-        Lwall = PSDGetRaw(2);   //检测左面
+        Lwall = PSDGetRaw(2)-16;   //检测左面
         Rwall = PSDGetRaw(3);   //检测右面
         bwall = PSDGetRaw(4);   //
         cout << "PSD L:" << Lwall << " R:" << Rwall << " F:" << Fwall << " b:" << bwall << endl; ;

robot_test_code/实机测试/test_turn.cpp

@@ -7,10 +7,8 @@ using namespace std;
 
 int main()
 {
-    //~ int f_wall, r_wall, l_wall, b_wall;
+    int f_wall, b_wall;
-    //~ int old_f_wall, old_r_wall, old_l_wall, old_b_wall;
+    int old_f_wall, old_b_wall;
-    int b_wall;
-    int old_b_wall;
     int turn_check;
     LCDMenu("","","","END");
     int num;
@@ -65,6 +63,34 @@ int main()
                 OSWait(50);
             } while (turn_check < -3  ||  b_wall > 130);
         }
+        else if (num == 2)
+        {
+            b_wall = PSDGetRaw(4) - 2;
+            f_wall = PSDGetRaw(1) - 2;
+            do 
+            {
+                old_b_wall = b_wall;
+                old_f_wall = f_wall;
+                MOTORDriveRaw(1,19);
+                MOTORDriveRaw(2,-17);
+                //~ MOTORDriveRaw(1,24);
+                //~ MOTORDriveRaw(2,-22);
+                OSWait(1000);
+                //~ OSWait(1980);
+                MOTORDriveRaw(1,0);
+                MOTORDriveRaw(2,0);
+                b_wall = (PSDGetRaw(4) - 2) * 0.7  +  old_b_wall * 0.3;     //减小突变
+                f_wall = (PSDGetRaw(1) - 2) * 0.7  +  old_f_wall * 0.3;     //减小突变
+                //~ b_wall = PSDGetRaw(4) - 2;
+                //~ f_wall = PSDGetRaw(1) - 2;
+                turn_check = b_wall - old_b_wall;
+                /*---打印输出PSD数值---*/
+                //~ cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << " " ;
+                cout << "f_wall: " << f_wall << "\t" << "b_wall: " << b_wall << "\t" << "check: " << turn_check << endl;
+                /*---打印结束---*/
+                OSWait(50);
+            } while (f_wall < 200 || turn_check < -4  ||  b_wall > 130);
+        }
         //~ if(num == 1)
         //~ {
             //~ MOTORDriveRaw(1,19);

robot_test_code/实机测试/test_turn2.cpp

@@ -0,0 +1,79 @@
+#include<iostream>
+#include<string>
+#include<cmath>
+//~ #include"eyebot++.h"
+#include"/home/pi/eyebot/include/eyebot++.h"
+using namespace std;
+
+int main()
+{
+	
+	LCDMenu("","","","END");
+	int num;
+	//~ int pwm1, pwm2;
+	do
+	{
+		cin>>num;
+		if (num == 1)
+		{
+			ENCODERReset(1);
+			ENCODERReset(2);
+			do 
+			{
+				MOTORDriveRaw(1,21);
+				MOTORDriveRaw(2,-19);
+			} while (abs(ENCODERRead(2)) < 217);
+			MOTORDriveRaw(1,0);
+			MOTORDriveRaw(2,0);
+		}
+		
+		else if (num == 2)
+		{
+			ENCODERReset(1);
+			ENCODERReset(2);
+			do 
+			{
+				MOTORDriveRaw(1,21);
+				MOTORDriveRaw(2,-19);
+			} while (abs(ENCODERRead(2)) < 500);
+			MOTORDriveRaw(1,0);
+			MOTORDriveRaw(2,0);
+		}
+		
+		else if (num == 3)
+		{
+			ENCODERReset(1);
+			ENCODERReset(2);
+			do 
+			{
+				MOTORDriveRaw(1,-21);
+				MOTORDriveRaw(2,19);
+			} while (abs(ENCODERRead(2)) < 242);
+			MOTORDriveRaw(1,0);
+			MOTORDriveRaw(2,0);
+		}
+		
+		else if (num == 4)
+		{
+			ENCODERReset(1);
+			ENCODERReset(2);
+			do 
+			{
+				MOTORDriveRaw(1,-21);
+			} while (abs(ENCODERRead(1)) < 285);
+			MOTORDriveRaw(1,0);
+			do 
+			{
+				MOTORDriveRaw(2,19);
+			} while (abs(ENCODERRead(2)) < 285);
+			MOTORDriveRaw(2,0);
+		}
+		
+		else 
+		{
+			cout << " num error " << endl;
+		}
+		
+	} while (KEYRead() != KEY4);
+	
+}

文件/eyebot函数.cpp

@@ -6,3 +6,5 @@ VWSetSpeed();
 VWGetPosition();
 VWSetPosition();
 VWWait();
+ENCODERReset();
+ENCODERRead();