2025.5.15测试

maze_code/maze_func_explore.cpp

@@ -1,6 +1,7 @@
 #include<iostream>
 #include<string>
 #include<vector>
+#include<cstddef>
 #include"eyebot++.h"
 #include"maze_parameter.h"
 #include"maze_func.h"
@@ -21,90 +22,41 @@ bool check_mark()
   bool S_mark;
   bool E_mark;
 
-  if (x>0  &&  y>0)   //情况1
+  if (x > 0)
   {
-    if (y == size_y - 1)  //N
-    {
-      N_mark = true;
+    W_mark = mark[x-1][y];
   }
   else
   {
-      N_mark = mark[x][y+1];
+    W_mark = true;
   }
-    W_mark = mark[x-1][y];  //W
-    S_mark = mark[x][y-1];  //S
-    if (x == size_x -1) //E
+
+  if (y > 0)
   {
-      E_mark = true;
+    S_mark = mark[x][y-1];
   }
   else
+  {
+    S_mark = true;
+  }
+
+  if (x < size_x - 1)
   {
     E_mark = mark[x+1][y];
   }
-  }
-  else if (x==0  &&  y>0)   //情况2
-  {
-    if (y == size_y - 1)  //N
-    {
-      N_mark = true;
-    }
   else
-    {
-      N_mark = mark[x][y+1];
-    }
-    W_mark = true;  //W
-    S_mark = mark[x][y-1];  //S
-    if (x == size_x - 1)  //E
   {
     E_mark = true;
   }
-    else
-    {
-      E_mark = mark[x+1][y];
-    }
-  }
-  else if (x>0  &&  y==0)   //情况3
-  {
-    if (y == size_y - 1)  //N
-    {
-      N_mark = true;
-    }
-    else
+
+  if (y < size_y - 1)
   {
     N_mark = mark[x][y+1];
   }
-    W_mark = mark[x-1][y];  //W
-    S_mark = true;  //S
-    if (x == size_x -1) //E
-    {
-      E_mark = true;
-    }
   else
-    {
-      E_mark = mark[x+1][y];
-    }
-  }
-  else //(x==0  &&  y==0)
-  {
-    if (y == size_y - 1)  //N
   {
     N_mark = true;
   }
-    else
-    {
-      N_mark = mark[x][y+1];
-    }
-    W_mark = true;  //W
-    S_mark = true;  //S
-    if (x == size_x -1) //E
-    {
-      E_mark = true;
-    }
-    else
-    {
-      E_mark = mark[x+1][y];
-    }
-  }
 
   bool N_road = (!wall[x][y+1][0]  &&  N_mark);
   bool W_road = (!wall[x][y][1]  &&  W_mark);
@@ -126,15 +78,22 @@ void maze_entry(int x, int y, int dir, int open)
   switch (maze_dir)
   {
     case 0:  //0表示北(上)，表示(x，y+1)坐标单元格的下方墙壁信息
-    if (y == mark[0].size() - 1  &&  open)  //检测是否超出容器范围，如果超出，则扩大容器：对每一个内层尾插数据
+    if (y == static_cast<int>(mark[0].size()) - 1  &&  open)  //检测是否超出容器范围，如果超出，则扩大容器：对每一个内层尾插数据
     {
-      for (int i = 0; i < mark.size() - 1; i++)
+      if (!mark.empty()  &&  !wall.empty()) //插入安全性检测，确保容器非空
+      {
+        for (size_t i = 0; i < mark.size(); i++)
         {
           mark[i].emplace_back(0);
         }
-      for (int i = 0; i < wall.size() - 1; i++)
+        for (size_t i = 0; i < wall.size(); i++)
         {
-        wall[i].emplace_back(vector<int>(2,0));
+          wall[i].emplace_back(std::vector<int>(2,0));
+        }
+      }
+      else 
+      {
+        cout << "error: mark or wall empty" << endl;
       }
     }
     wall[x][y+1][0] = !open;
@@ -143,8 +102,21 @@ void maze_entry(int x, int y, int dir, int open)
     case 1:  //1表示西(左)，表示(x，y)坐标单元格的左侧墙壁信息
     if (x == 0  &&  open)  //检测是否超出容器范围，如果超出，则扩大容器：头插一个内层
     {
-      mark.emplace(mark.begin(), mark[0].size(), 0);
-      wall.emplace(wall.begin(), wall[0].size(), vector<int>(2,0));
+      if (!mark.empty()  &&  !wall.empty()) //插入安全性检测，确保容器非空
+      {
+        // std::vector<int> mark_insert(mark[0].size(), 0);
+        // mark.insert(mark.begin(), mark_insert);
+
+        // std::vector<std::vector<int>> wall_insert(wall[0].size(), std::vector<int>(2,0));
+        // wall.insert(wall.begin(), wall_insert);
+
+        mark.emplace(mark.begin(), std::vector<int>(mark[0].size(), 0));  //在mark头部插入一个内层
+        wall.emplace(wall.begin(), std::vector<std::vector<int>>(wall[0].size(), std::vector<int>(2, 0)));  //在wall头部插入一个内层
+      }
+      else 
+      {
+        cout << "error: mark or wall empty" << endl;
+      }
     }
     wall[x][y][1] = !open;
     break;
@@ -152,23 +124,37 @@ void maze_entry(int x, int y, int dir, int open)
     case 2:  //2表示南(下)，表示(x，y)坐标单元格的下侧墙壁信息
     if (y == 0  &&  open)  //检测是否超出容器范围，如果超出，则扩大容器：对每一个内层头插数据
     {
-      for (int i = 0; i < mark.size() - 1; i++)
+      if (!mark.empty()  &&  !wall.empty()) //插入安全性检测，确保容器非空
+      {
+        for (size_t i = 0; i < mark.size(); i++)
         {
           mark[i].emplace(mark[i].begin(), 0);
         }
-      for (int i = 0; i < wall.size() - 1; i++)
+        for (size_t i = 0; i < wall.size(); i++)
         {
-        wall[i].emplace(wall[i].begin(), vector<int>(2,0));
+          wall[i].emplace(wall[i].begin(), std::vector<int>(2,0));
+        }
+      }
+      else 
+      {
+        cout << "error: mark or wall empty" << endl;
       }
     }
     wall[x][y][0] = !open;
     break;
     
     case 3:  //3表示东(右)，表示(x+1，y)坐标单元格的左侧墙壁信息
-    if (x == (mark.size() - 1)  &&  open)  //检测是否超出容器范围，如果超出，则扩大容器：尾插一个内层
+    if (x == static_cast<int>(mark.size()) - 1  &&  open)  //检测是否超出容器范围，如果超出，则扩大容器：尾插一个内层
     {
-      mark.emplace_back(mark[0].size(), 0);
-      wall.emplace_back(wall[0].size(), vector<int>(2,0));
+      if (!mark.empty()  &&  !wall.empty()) //插入安全性检测，确保容器非空
+      {
+        mark.emplace_back(std::vector<int>(mark[0].size(), 0));
+        wall.emplace_back(std::vector<std::vector<int>>(wall[0].size(), std::vector<int>(2, 0)));
+      }
+      else 
+      {
+        cout << "error: mark or wall empty" << endl;
+      }
     }
     wall[x+1][y][1] = !open;
     break;
@@ -183,7 +169,7 @@ bool unmarked(int x, int y, int dir)
   switch(maze_dir)
   {
     case 0:
-    if (y < mark[0].size() - 2)
+    if (y < (static_cast<int>(mark[0].size()) - 1))
     {
       if (mark[x][y+1] == 0)
       {
@@ -193,7 +179,7 @@ bool unmarked(int x, int y, int dir)
     break;
     
     case 1:
-    if (x > 1)
+    if (x > 0)
     {
       if (mark[x-1][y] == 0)
       {
@@ -203,7 +189,7 @@ bool unmarked(int x, int y, int dir)
     break;
     
     case 2:
-    if (y > 1)
+    if (y > 0)
     {
       if (mark[x][y-1] == 0)
       {
@@ -213,7 +199,7 @@ bool unmarked(int x, int y, int dir)
     break;
     
     case 3:
-    if (x < mark.size() - 2)
+    if (x < (static_cast<int>(mark.size()) - 1))
     {
       if (mark[x+1][y] == 0)
       {

maze_code/maze_func_flood.cpp

@@ -1,6 +1,7 @@
 #include<iostream>
 #include<string>
 #include<vector>
+#include<cstddef>
 #include"eyebot++.h"
 #include"maze_parameter.h"
 #include"maze_func.h"

maze_code/maze_func_path.cpp

@@ -1,6 +1,7 @@
 #include<iostream>
 #include<string>
 #include<vector>
+#include<cstddef>
 #include"eyebot++.h"
 #include"maze_parameter.h"
 #include"maze_func.h"

maze_code/maze_main.cpp

@@ -1,6 +1,7 @@
 #include<iostream>
 #include<string>
 #include<vector>
+#include<cstddef>
 #include"eyebot++.h"
 #include"maze_parameter.h"
 #include"maze_func.h"

maze_code/maze_parameter.cpp

@@ -1,6 +1,7 @@
 #include<iostream>
 #include<string>
 #include<vector>
+#include<cstddef>
 #include"eyebot++.h"
 #include"maze_parameter.h"
 #include"maze_func.h"
@@ -14,7 +15,7 @@ const int DIST_wall = 130 ;     //与墙壁的距离
 /*---定义全局变量---*/
 /*---容器类---*/
 vector<vector<int>> mark(1,vector<int>(1,0));                                   //定义mark容器（二维容器），容器初始为1行1列    1=为已访问
-vector<vector<vector<int>>> wall(1,vector<vector<int>>(1,vector<int>(2,0)));    //定义wall容器（三维容器），容器初始为1行1列，且每个格子内带有2个墙壁信息   1=墙壁；0=通路
+vector<vector<vector<int>>> wall(2,vector<vector<int>>(2,vector<int>(2,0)));    //定义wall容器（三维容器），容器初始为1行1列，且每个格子内带有2个墙壁信息   1=墙壁；0=通路
 
 /*---机器人坐标---*/
 int rob_x0 = 0;     //机器人起点X坐标

robot_test_code/LCD输出测试/lcd_test1.cpp

@@ -0,0 +1,10 @@
+#include<iostream>
+#include<string>
+#include"eyebot++.h"
+using namespace std;
+
+
+int main()
+{
+    
+}

robot_test_code/PID/PID.cpp

@@ -0,0 +1,152 @@
+#include<iostream>
+#include<string>
+#include"eyebot++.h"
+#include"PID.h"
+using namespace std;
+
+float Kp = 0.2, Ki = 0.1, Kd = 0.1;   //定义PID的参数
+float Kpid;
+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 = 20;
+/*---定义全局常量---*/
+/*---定义全局常量---*/
+// 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_rl = 87;
+/*---全局各种变量与常量定义完成---*/
+
+
+/*---对PID控制的积分部分I限幅---*/
+void eI_xianfu(int eI_max)
+{
+  if (err_sum >= eI_max)    //如果err_sum大于限幅，则直接让err_sum=限幅，防止err_sum过大，影响PID控制
+  {
+    err_sum = eI_max;
+  }
+  else if (err_sum <= -eI_max)
+  {
+    err_sum = -eI_max;
+  }
+  else
+  {
+    //此处为空;
+  }
+}
+
+/*---位置式PID算法---*/
+void PID_AL()
+{
+  err_sum += err;   //积分累计误差值
+  eI_xianfu(eI_max);   //积分部分限幅
+  err_diff = err - err_old;   //微分部分误差值
+  /*---打印输出PID控制的各项参数1---*/
+  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 >= 6)  //对pid输出值进行限制，防止过大电机过快，或者减的过小使电机不转动
+  {
+    Kpid_speed = 6;
+  }
+  else if (Kpid_speed <= -6)
+  {
+    Kpid_speed = -6;
+  }
+  else 
+  {
+    //error
+  }
+  /*---打印输出PID控制的各项参数2---*/
+  cout << "PID_out:" << Kpid_speed << " " ;
+  /*---打印结束---*/
+
+  MOTORDriveRaw(2,speed + Kpid_speed);   //控制小车的行驶速度，角速度
+  MOTORDriveRaw(1,speed);
+}
+
+/*---PID控制下的直线行驶---*/
+void PIDStraight()
+{
+  int DIST_move;    //定义基础角速度和速度,和移动距离
+  int L_PSD, R_PSD, F_PSD;    //定义左侧，右侧，前方的距离值
+  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值
+  DIST_move_x1 = F_PSD;
+  MOTORDriveRaw(1,speed);
+  MOTORDriveRaw(2,speed);
+
+  do 
+  {
+    F_PSD = PSDGetRaw(1);    //读取前方和墙壁的距离
+    // 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 << " " ;
+    /*---打印结束---*/
+    if (25<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)    //只有左侧有墙的情况
+    {
+      err = L_PSD - DIST_wall_rl;
+      PID_AL();
+    }
+    else if (25<R_PSD && R_PSD<180)    //只有右侧有墙的情况
+    {
+      err = DIST_wall_rl - R_PSD;
+      PID_AL();
+    }
+    else
+    {
+      L_PSD = L_PSD % DIST_cell;
+      R_PSD = R_PSD % DIST_cell;
+      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));
+    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);
+}

robot_test_code/PID/PID.h

@@ -0,0 +1,5 @@
+#pragma once
+
+void eI_xianfu(int eI_max);   //积分部分限幅
+void PID_AL();    //位置式PID算法
+void PIDStraight();   //PID控制下的直线行驶

robot_test_code/PID/PIDmain.cpp

@@ -0,0 +1,52 @@
+#include<iostream>
+#include"PID.h"
+#include"eyebot++.h"
+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_rl = 87;
+/*---全局各种变量与常量定义完成---*/
+
+
+/*---下面是主函数---*/
+int main()
+{
+    int Fwall,Lwall,Rwall;
+    LCDMenu("","","","END");
+
+    do
+    {   //检测墙面
+        Fwall = PSDGetRaw(1) > DIST_cell;   //检测前面侧是否有墙
+        Lwall = PSDGetRaw(2) > DIST_cell;    //检测左面
+        Rwall = PSDGetRaw(3) > DIST_cell;   //检测右面
+
+        //判断旋转方向
+        if (Lwall)
+        {
+            VWTurn(+90,15);     //向左旋转
+            VWWait();
+        }
+        else if (Fwall)     
+        {
+        //不旋转
+        }
+        else if (Rwall)
+        {
+            VWTurn(-90,15);     //向右旋转
+            VWWait();
+        }
+        else 
+        {
+            VWTurn(180,15);
+            VWWait();
+        }
+        //直走
+        PIDStraight();
+        VWWait();
+    } while (KEYRead() != KEY4);
+}

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

robot_test_code/实机测试/test_run.cpp

@@ -0,0 +1,19 @@
+#include<iostream>
+#include<string>
+#include"eyebot++.h"
+using namespace std;
+
+
+
+int main()
+{
+    int Fwall,Lwall,Rwall;
+    LCDMenu("","","","END");
+
+    do
+    {   //检测墙面
+        MOTORDriveRaw(1,50);
+        MOTORDriveRaw(2,50);
+        
+    } while (KEYRead() != KEY4);
+}