2025.5.18第二次提交，重新写一份复杂的，慢一些的turn函数

2025.5.18第一次提交，包括17号和今天上午对实机的调试
2025-09-07 23:21:26 +00:00 · 2025-05-18 11:55:43 +08:00 · 2025-05-18 11:35:02 +08:00
29 changed files with 1237 additions and 103 deletions
--- a/bot_code/makefile
+++ b/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 $@
--- a/bot_code/maze_func.h
+++ b/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 pid_speed_xianfu(int Kpid_speed);  //对PID控制的输出部分限幅
+void eI_xianfu();         //对PID控制的积分部分I限幅
+void pid_speed_xianfu();  //对PID控制的输出部分限幅
 void PID_AL();                      //位置式PID算法
 void PIDStraight();                 //PID控制下的直线行驶
 void go_to(int dir);                //行驶到指定单元格
--- a/bot_code/maze_func_goto.cpp
+++ b/bot_code/maze_func_goto.cpp
@@ -68,16 +68,23 @@ 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;  //更新机器人移动到下一格之后的方向
  xneighbor(rob_x,rob_dir); //更新机器人移动到下一格之后的X坐标
  yneighbor(rob_y,rob_dir); //更新机器人移动到下一格之后的y坐标
-}
+}
--- a/bot_code/maze_func_pid.cpp
+++ b/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;   //积分累计误差值
-  eI_xianfu(eI_max);   //积分部分限幅
+  //~ err_sum += err;   //积分累计误差值
+  //~ eI_xianfu();   //积分部分限幅
  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 << " " ;
+    //~ cout << "old_err:" << 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;
+  //~ Kpid = Kp*err + Ki*err_sum + Kd*err_diff;   //PID控制算法输出的倍率
+  //~ 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,speed2);
+  MOTORDriveRaw(2,speed_r);
+  MOTORDriveRaw(1,speed_l);
+

  do 
  {
-    F_PSD = PSDGetRaw(1) - 5;    //读取前方和墙壁的距离
-    L_PSD = PSDGetRaw(2) - 8;   //读取左侧和墙壁的距离
+    F_PSD = PSDGetRaw(1) - 2;    //读取前方和墙壁的距离
+    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;
-      R_PSD = R_PSD % DIST_cell;
+      L_PSD = L_PSD % 300;
+      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));
-    cout << "move:" << DIST_move << endl;
+    bot_move = sqrt(pow(x_2-x_1,2) + pow(y_2-y_1,2));
+    //~ 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);
-  
 }
--- a/bot_code/maze_func_turn.cpp
+++ b/bot_code/maze_func_turn.cpp
@@ -8,38 +8,49 @@ using namespace std;

 void BOTturn(int turn)
 {
-    if (turn == 1)
+    if (turn == -1)  //右转
    {
-        MOTORDriveRaw(1,24);
-        MOTORDriveRaw(2,-22);
-
-        OSWait(1985);
-
-        MOTORDriveRaw(1,0);
-        MOTORDriveRaw(2,0);
+        ENCODERReset(1);
+        ENCODERReset(2);
+        do 
+		{
+			MOTORDriveRaw(1,21);
+			MOTORDriveRaw(2,-19);
+            
+		} while (abs(ENCODERRead(2)) < 218);
+        
+		MOTORDriveRaw(1,0);
+		MOTORDriveRaw(2,0);
    }
-    else if (turn == -1)
+    else if (turn == 1)    //左转
    {
-        MOTORDriveRaw(1,-24);
-        MOTORDriveRaw(2,22);
+        ENCODERReset(1);
+		ENCODERReset(2);
+		do 
+		{
+			MOTORDriveRaw(1,-21);
+			MOTORDriveRaw(2,19);

-        OSWait(1985);
-
-        MOTORDriveRaw(1,0);
-        MOTORDriveRaw(2,0);
-    }
-    else if (turn == 2)
+		} while (abs(ENCODERRead(2)) < 240);
+		
+		MOTORDriveRaw(1,0);
+		MOTORDriveRaw(2,0);
+	}
+    else if (turn == 2)     //掉头
    {
-        MOTORDriveRaw(1,24);
-        MOTORDriveRaw(2,-22);
-
-        OSWait(2985);
-
-        MOTORDriveRaw(1,0);
-        MOTORDriveRaw(2,0);
+        ENCODERReset(1);
+		ENCODERReset(2);
+		do 
+		{
+			MOTORDriveRaw(1,21);
+			MOTORDriveRaw(2,-19);
+		
+		} while (abs(ENCODERRead(2)) < 500);
+		MOTORDriveRaw(1,0);
+		MOTORDriveRaw(2,0);
    }
    else 
    {
        cout << "errer" << endl;
    }
-}
+}
--- a/bot_code/maze_parameter.cpp
+++ b/bot_code/maze_parameter.cpp
@@ -8,9 +8,10 @@ using namespace std;

 /*---定义数据---*/
 /*---定义全局常量---*/
-const int DIST_cell = 300 ;     //单元格长度
-const int DIST_wall_F = 90 ;     //与墙壁的距离
-extern const int DIST_wall_RL = 100;      //与左右墙壁的距离
+const int DIST_cell = 240;     	//单元格长度
+const int DIST_move = 292;		//移动的单元格距离
+const int DIST_wall_F = 90;     //与墙壁的距离
+const int DIST_wall_RL = 88;      //与左右墙壁的距离

 /*---定义全局变量---*/
 /*---容器类---*/
@@ -28,19 +29,21 @@ int rob_dir = 0;    //机器人当前的朝向（初始为0）


 /*---PID算法---*/
-const float Kp = 0.8, Ki = 0.015, Kd = 0.15;   //定义PID的参数
-float Kpid;                             //定义PID计算后输出的倍率
+const float Kp = 0.4, Ki = 0, Kd = 0.25;   //定义PID的参数17
+//~ 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 speed2 = 0.75*speed;          //快到目的地时减速
-const int speed_xianfu = 3;             //定义kpid限幅
-int speed1;
-int err, err_old, err_sum, err_diff;    //定义误差，上一次误差，积分误差，微分误差
-const int eI_max = 200;                  //定义积分限幅
+const float Kpid_base = 1;				//定义基本角速度和速度
+const int speed_l = 16;					//定义基本左轮速度
+//~ const int speed_r = speed_l - 2;		//定义基本右轮速度
+int speed_r = speed_l - 2;		//定义基本右轮速度
+const int speed_xianfu = 2;				//定义kpid限幅
+int err, err_old, err_sum, err_diff;	//定义误差，上一次误差，积分误差，微分误差
+const int eI_max = 200;                 //定义积分限幅

 /*---目标点的坐标---*/
 const int target_x = 4;   //声明目标点的X坐标
 const int target_y = 4;   //声明目标点的Y坐标

-/*---定义结束---*/
+/*---定义结束---*/
--- a/bot_code/maze_parameter.h
+++ b/bot_code/maze_parameter.h
@@ -4,7 +4,8 @@
 /*---声明数据---*/
 /*---声明全局常量---*/
 extern const int DIST_cell;         //单元格长度
-extern const int DIST_wall_F;       //与前面墙壁的距离
+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 speed2;                        //快到目的地时减速
+extern const float Kpid_base;					//定义速度基础倍数速度
+extern const int speed_l;						//定义左轮基本速度
+//~ 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;                        //定义积分限幅

@@ -40,4 +41,4 @@ extern const int eI_max;                        //定义积分限幅
 extern const int target_x;    //声明目标点的X坐标
 extern const int target_y;    //声明目标点的Y坐标

-/*---声明结束---*/
+/*---声明结束---*/
--- a/bot_code_turn/makefile
+++ b/bot_code_turn/makefile
@@ -0,0 +1,20 @@
+
+cxx := gccarm
+
+file_cpp := $(wildcard *.cpp)
+file_o := /home/pi/usr/maze.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
--- a/bot_code_turn/maze_func.h
+++ b/bot_code_turn/maze_func.h
@@ -0,0 +1,47 @@
+#pragma once
+
+/*---PART1---*/
+/*---以下为递归算法探索部分涉及到的函数声明---*/
+bool check_mark();                                  //检查单元格各方面是否已被确认
+void maze_entry(int x, int y, int dir, int open);   //maze_entry函数,用于写入到wall数组中，1：有墙 0：通路
+bool unmarked(int y, int x, int dir);               //检查该方向是否已被标记
+void explore();                                     //递归函数
+/*---以上为递归算法探索部分涉及到的函数声明---*/
+
+/*---PART2---*/
+/*---以下为pid控制和行驶部分涉及到的函数声明---*/
+void xneighbor(int x, int dir);     //X坐标更新
+void yneighbor(int y, int dir);     //Y坐标更新
+void eI_xianfu();         //对PID控制的积分部分I限幅
+void pid_speed_xianfu();  //对PID控制的输出部分限幅
+void PID_AL();                      //位置式PID算法
+void PIDStraight();                 //PID控制下的直线行驶
+void go_to(int dir);                //行驶到指定单元格
+void BOTturn(int turn);             //自定义转向函数
+/*---以上为pid控制和行驶部分涉及到的函数声明---*/
+
+/*---PART3---*/
+/*---以下为洪水填充算法部分涉及到的函数声明---*/
+void flood(int *map, int *copy_wall);  //洪水填充算法
+/*---以上为洪水填充算法部分涉及到的函数声明---*/
+
+/*---PART4---*/
+/*---以下为路径算法部分涉及到的函数声明---*/
+void build_path(int i, int j, int len, int *path, int *map, int *copy_wall);     //路径算法
+void drive_path(int len, int *path);    //移动到目的地
+/*---以上为路径算法部分涉及到的函数声明---*/
+
+/*---PART5---*/
+/*---以下为数组构建部分涉及到的函数声明---*/
+void array_negative_one(int size, int *arr);  //将数组初始化为-1
+void array_copy_mark(int size_x, int size_y, int *copy_mark);   //复制一份mark数组，便于后续频繁读取提升性能
+void array_copy_wall(int size_x, int size_y, int *copy_wall);   //复制一份wall数组，便于后续频繁读取提升性能
+/*---以上为数组构建部分涉及到的函数声明---*/
+
+/*---PART6---*/
+/*---以下为数组打印部分涉及到的函数声明---*/
+void output_arr2D(int size_x, int size_y, int *arr);        //打印所有二维数组数组
+void output_arrwall(int size_x, int size_y, int *arr);      //打印wall数组
+void output_arrpath(int size, int *arr);      //打印path数组
+/*---以上为数组打印部分涉及到的函数声明---*/
+
--- a/bot_code_turn/maze_func_array.cpp
+++ b/bot_code_turn/maze_func_array.cpp
@@ -0,0 +1,42 @@
+#include<iostream>
+#include<string>
+#include<vector>
+#include"eyebot++.h"
+#include"maze_parameter.h"
+#include"maze_func.h"
+using namespace std;
+
+/*---从vector容器复制mark信息到数组函数---*/
+void array_copy_mark(int size_x, int size_y, int *copy_mark)
+{
+  for (int i = 0; i < size_x; i++)
+  {
+    for (int j = 0; j < size_y; j++)
+    {                                               
+      copy_mark[i * size_y + j] = mark[i][j];
+    }
+  }
+}
+
+/*---从vector容器复制wall信息到数组函数---*/
+void array_copy_wall(int size_x, int size_y, int *copy_wall)
+{
+  for (int i = 0; i <= size_x; i++)
+  {
+    for (int j = 0; j <= size_y; j++)
+    {
+      for(int k = 0; k < 2; k++)
+      {
+        copy_wall[i * (size_y + 1) * 2 + j * 2 + k] = wall[i][j][k];  //注释：k = 0记录的是单元格下方的数据，1记录的是单元格左边的数据，所以转为一维数组后，i*...+j*...+k,k=1为左侧数据，k=0为下方墙壁。
+      }
+    }
+  }
+}
+
+void array_negative_one(int size, int *arr)  //将数组初始化为-1
+{
+  for (int i = 0; i < size; i++)
+  {
+    arr[i] = -1;
+  }
+}
--- a/bot_code_turn/maze_func_explore.cpp
+++ b/bot_code_turn/maze_func_explore.cpp
@@ -0,0 +1,257 @@
+#include<iostream>
+#include<string>
+#include<vector>
+#include<cstddef>
+#include"eyebot++.h"
+#include"maze_parameter.h"
+#include"maze_func.h"
+using namespace std;
+
+
+/*---检查单元格各方向是否已被到访---*/
+bool check_mark()
+{ bool check = false;
+  int x, y;
+  x = rob_x;  //机器人当前x坐标
+  y = rob_y;  //机器人当前y坐标
+  int size_x = mark.size();
+  int size_y = mark[0].size();
+
+  bool N_mark;
+  bool W_mark;
+  bool S_mark;
+  bool E_mark;
+
+  if (x > 0)
+  {
+    W_mark = mark[x-1][y];
+  }
+  else
+  {
+    W_mark = true;
+  }
+
+  if (y > 0)
+  {
+    S_mark = mark[x][y-1];
+  }
+  else
+  {
+    S_mark = true;
+  }
+
+  if (x < size_x - 1)
+  {
+    E_mark = mark[x+1][y];
+  }
+  else
+  {
+    E_mark = true;
+  }
+
+  if (y < size_y - 1)
+  {
+    N_mark = mark[x][y+1];
+  }
+  else
+  {
+    N_mark = true;
+  }
+
+  bool N_road = (!wall[x][y+1][0]  &&  N_mark);
+  bool W_road = (!wall[x][y][1]  &&  W_mark);
+  bool S_road = (!wall[x][y][0]  &&  S_mark);
+  bool E_road = (!wall[x+1][y][1]  &&  E_mark);
+  
+  if (N_road  &&  W_road  &&  S_road  &&  E_road)
+  {
+    check = true;
+  }
+  return check;
+}
+
+/*---maze_entry函数,用于写入到wall数组中，1：有墙 0：通路---*/
+void maze_entry(int x, int y, int dir, int open)
+{
+  int maze_dir = (dir + 4) % 4;
+  
+  switch (maze_dir)
+  {
+    case 0:  //0表示北(上)，表示(x，y+1)坐标单元格的下方墙壁信息
+    if (y == static_cast<int>(mark[0].size()) - 1  &&  open)  //检测是否超出容器范围，如果超出，则扩大容器：对每一个内层尾插数据
+    {
+      if (!mark.empty()  &&  !wall.empty()) //插入安全性检测，确保容器非空
+      {
+        for (size_t i = 0; i < mark.size(); i++)
+        {
+          mark[i].emplace_back(0);
+        }
+        for (size_t i = 0; i < wall.size(); i++)
+        {
+          wall[i].emplace_back(std::vector<int>(2,0));
+        }
+      }
+      else 
+      {
+        cout << "error: mark or wall empty" << endl;
+      }
+    }
+    wall[x][y+1][0] = !open;
+    break;
+    
+    case 1:  //1表示西(左)，表示(x，y)坐标单元格的左侧墙壁信息
+    if (x == 0  &&  open)  //检测是否超出容器范围，如果超出，则扩大容器：头插一个内层
+    {
+      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;
+    
+    case 2:  //2表示南(下)，表示(x，y)坐标单元格的下侧墙壁信息
+    if (y == 0  &&  open)  //检测是否超出容器范围，如果超出，则扩大容器：对每一个内层头插数据
+    {
+      if (!mark.empty()  &&  !wall.empty()) //插入安全性检测，确保容器非空
+      {
+        for (size_t i = 0; i < mark.size(); i++)
+        {
+          mark[i].emplace(mark[i].begin(), 0);
+        }
+        for (size_t i = 0; i < wall.size(); i++)
+        {
+          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 == static_cast<int>(mark.size()) - 1  &&  open)  //检测是否超出容器范围，如果超出，则扩大容器：尾插一个内层
+    {
+      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;
+  }
+}
+
+/*---检查该方向是否已被标记---*/
+bool unmarked(int x, int y, int dir)
+{
+  bool check = false;
+  int maze_dir = (dir + 4) % 4;
+  switch(maze_dir)
+  {
+    case 0:
+    if (y < (static_cast<int>(mark[0].size()) - 1))
+    {
+      if (mark[x][y+1] == 0)
+      {
+        check = true;
+      }
+    }
+    break;
+    
+    case 1:
+    if (x > 0)
+    {
+      if (mark[x-1][y] == 0)
+      {
+        check = true;
+      }
+    }
+    break;
+    
+    case 2:
+    if (y > 0)
+    {
+      if (mark[x][y-1] == 0)
+      {
+        check = true;
+      }
+    }
+    break;
+    
+    case 3:
+    if (x < (static_cast<int>(mark.size()) - 1))
+    {
+      if (mark[x+1][y] == 0)
+      {
+        check = true;
+      }
+    }
+    break;
+  }
+  return check;
+}
+
+/*---递归函数---*/
+void explore()
+{
+  int F_open,L_open,R_open,old_dir;
+  bool check;
+  
+  mark[rob_x][rob_y] = 1;   //将当前的单元格设置为已到访
+  F_open = PSDGetRaw(1) > DIST_cell;   //获取前侧空间状况
+  L_open = PSDGetRaw(2) > DIST_cell;   //获取左侧空间状况
+  R_open = PSDGetRaw(3) > DIST_cell;   //获取右侧空间状况
+  
+  maze_entry(rob_x, rob_y, rob_dir, F_open);   //写入前方墙壁信息
+  maze_entry(rob_x, rob_y, rob_dir + 1, L_open);   //写入左侧墙壁信息
+  maze_entry(rob_x, rob_y, rob_dir - 1, R_open);   //写入右侧墙壁信息
+  check = check_mark();   //检查3个方向是否到访
+  if (check)
+  {
+    goto end_explore; 
+  }
+  
+  old_dir = rob_dir;
+  if (F_open  &&  unmarked(rob_x, rob_y, old_dir))  //如果前方有通路且前方单元格未被探索
+  {
+    go_to(old_dir); //向前移动一格
+    explore();  //进行递归，在完成所有可到达单元格的访问前，一直循环在explore中
+    go_to(old_dir + 2); //返回到移动前的单元格
+  }
+  
+  if (L_open  &&  unmarked(rob_x, rob_y, old_dir + 1))  //如果左侧有通路且前方单元格未被探索
+  {
+    go_to(old_dir + 1); //向左移动一格
+    explore();  //进行递归，在完成所有可到达单元格的访问前，一直循环在explore中
+    go_to(old_dir - 1); //返回到移动前的单元格
+  }
+  
+  if (R_open  &&  unmarked(rob_x, rob_y, old_dir - 1))  //如果右侧有通路且前方单元格未被探索
+  {
+    go_to(old_dir - 1); //向右移动一格
+    explore();  //进行递归，在完成所有可到达单元格的访问前，一直循环在explore中
+    go_to(old_dir + 1); //返回到移动前的单元格
+  }
+  end_explore:  //check_mark函数确认三个方向都被探索的话，直接跳转到此处结束此次循环
+  VWWait();
+} //递归结束
--- a/bot_code_turn/maze_func_flood.cpp
+++ b/bot_code_turn/maze_func_flood.cpp
@@ -0,0 +1,63 @@
+#include<iostream>
+#include<string>
+#include<vector>
+#include"eyebot++.h"
+#include"maze_parameter.h"
+#include"maze_func.h"
+using namespace std;
+
+void flood(int *map, int *copy_wall)
+{
+  int num = 0;
+  int size_x = mark.size();
+  int size_y = mark[0].size();
+  map[0] = 0;
+
+  do
+  {
+    for (int i = 0; i < size_x; i++)
+    { 
+      for (int j = 0; j < size_y; j++)
+      {
+        if (map[(i * size_y) + j] != -1)
+        { 
+          if (i > 0)
+          {
+            if (!copy_wall[(i * (size_y + 1) * 2) + (j * 2) + 1]  &&  map[((i-1) * size_y) + j] == -1) //左边格子
+            {
+              map[((i-1) * size_y) + j] = map[(i * size_y) + j] + 1;
+              num++;
+            }
+          }
+
+          if (i < size_x - 1)
+          {
+            if (!copy_wall[((i+1) * (size_y + 1) * 2) + (j * 2) + 1]  &&  map[((i+1) * size_y) + j] == -1) //右边格子
+            {
+              map[((i+1) * size_y) + j] = map[(i * size_y) + j] + 1;
+              num++;
+            }
+          }
+          
+          if (j > 0)
+          {
+            if (!copy_wall[(i * (size_y + 1) * 2) + (j * 2) + 0]  &&  map[(i *size_y) + (j-1)] == -1)
+            {
+              map[(i * size_y) + (j-1)] = map[(i * size_y) + j] + 1;
+              num++;
+            }
+          }
+
+          if (j < size_y - 1)
+          {
+            if (!copy_wall[(i * (size_y + 1) * 2) + ((j+1) * 2) + 0]  &&  map[(i * size_y) + (j+1)] == -1)
+            {
+              map[(i * size_y) + (j+1)] = map[(i * size_y) + j] + 1;
+              num++;
+            }
+          }
+        }
+      }
+    }
+  } while (map[((target_x - 1) * size_y) + (target_y - 1)] == -1  &&  num < (size_x * size_y));
+}
--- a/bot_code_turn/maze_func_goto.cpp
+++ b/bot_code_turn/maze_func_goto.cpp
@@ -0,0 +1,90 @@
+#include<iostream>
+#include<string>
+#include"eyebot++.h"
+#include"maze_parameter.h"
+#include"maze_func.h"
+using namespace std;
+
+/*---X坐标更新---*/
+void xneighbor(int x, int dir)
+{
+  int maze_dir = (dir + 4) % 4;
+  switch (maze_dir)
+  {
+    case 0:
+    break;
+    
+    case 1:
+    rob_x = x-1;
+    break;
+    
+    case 2:
+    break;
+    
+    case 3:
+    rob_x = x+1;
+    break;
+  }
+}
+
+/*---Y坐标更新---*/
+void yneighbor(int y, int dir)
+{
+  int maze_dir = (dir + 4) % 4;
+  switch (maze_dir)
+  {
+    case 0:
+    rob_y = y+1;
+    break;
+    
+    case 1:
+    break;
+    
+    case 2:
+    rob_y = y-1;
+    break;
+    
+    case 3:
+    break;
+  }
+}
+
+/*---行驶到指定单元格---*/
+void go_to(int dir)
+{
+  int turn;   //定义转向倍率
+  int maze_dir;
+  //static int cur_x,cur_y,cur_p;
+  
+  maze_dir = (dir + 4) % 4;  //保证机器人dir方位在[0-3]之间
+  turn = maze_dir - rob_dir;   //旋转角度倍数
+  //turn = turn == +3 ? turn = -1 : turn = turn;  三目运算符？能用吗？
+  
+  if (turn == +3) //确保机器人是旋转90度而不是270度，减少时间
+  {
+    turn = -1;
+  }
+  if (turn == -3)
+  {
+    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;  //更新机器人移动到下一格之后的方向
+  xneighbor(rob_x,rob_dir); //更新机器人移动到下一格之后的X坐标
+  yneighbor(rob_y,rob_dir); //更新机器人移动到下一格之后的y坐标
+}
--- a/bot_code_turn/maze_func_output.cpp
+++ b/bot_code_turn/maze_func_output.cpp
@@ -0,0 +1,58 @@
+#include<iostream>
+#include<string>
+#include"eyebot++.h"
+#include"maze_func.h"
+using namespace std;
+
+
+/*---打印数组检查错误---*/
+/*---打印数组mark,map,copy_map等二维数组所用的函数---*/
+void output_arr2D(int size_x, int size_y, int *arr)
+{
+  for (int i = size_y - 1; i >= 0; i--)
+  {
+    for (int j = 0; j < size_x; j++)
+    {
+      cout << arr[(j * size_y)  +  i] << " ";
+    }
+    cout << endl;
+  }
+}
+
+/*---打印数组wall---*/
+void output_arrwall(int size_x, int size_y, int *arr)
+{
+  for (int i = size_y; i >= 0; i--)
+  {
+    for (int j = 0; j <= size_x; j++)
+    {
+      if (arr[(j * (size_y + 1) * 2)  +  (i * 2)  +  1] == 1)
+      {
+        cout << "|" ;
+      }
+      else
+      {
+        cout << " " ;
+      }
+      
+      if (arr[(j * (size_y + 1) * 2)  +  (i * 2)  +  0] == 1)
+      {
+        cout << "_" ;
+      }
+      else
+      {
+        cout << " " ;
+      }
+    }
+    cout << endl;
+  }
+}
+
+void output_arrpath(int size, int *arr)
+{
+  for (int i = 0; i < size; i++)
+  {
+    cout << arr[i] << " ";
+  }
+  cout << endl;
+}
--- a/bot_code_turn/maze_func_path.cpp
+++ b/bot_code_turn/maze_func_path.cpp
@@ -0,0 +1,51 @@
+#include<iostream>
+#include<string>
+#include<vector>
+#include"eyebot++.h"
+#include"maze_parameter.h"
+#include"maze_func.h"
+using namespace std;
+
+/*---解迷宫路径到path数组函数---*/
+void build_path(int i, int j, int len, int *path, int *map, int *copy_wall)
+{
+    int size_x = mark.size();
+    int size_y = mark[0].size();
+
+    for (int k = len -1; k >= 0; k--)
+    {
+        if (i > 0  &&  !copy_wall[(i * (size_y + 1) * 2)  +  (j * 2)  +  1]  &&  map[((i-1) * size_y)  +  j] == k)   //前一个单位的东面是当前格，即当前格子的左侧为上一个格子
+        {
+            i--;
+            path[k] = 3;
+        }
+        else if (i < size_x - 1  &&  !copy_wall[((i+1) * (size_y + 1) * 2)  +  (j * 2)  +  1]  &&  map[((i+1) * size_y)  +  j] == k)   //前一个单位的西面是当前格，即当前格子的右侧为上一个格子
+        {
+            i++;
+            path[k] = 1;
+        }
+        else if(j > 0  &&  !copy_wall[(i * (size_y + 1) * 2)  +  (j * 2) +  0]  &&  map[(i * size_y)  +  (j-1)] == k)   //前一个单位的北面是当前格，即当前格子的下侧为上一个格子
+        {
+            j--;
+            path[k] = 0;
+        }
+        else if (j < size_y - 1  &&  !copy_wall[(i * (size_y + 1) * 2)  +  ((j+1) * 2)  +  0]  &&  map[(i * size_y)  +  (j+1)] == k)   //前一个单位的南面是当前格，即当前格子的上侧为上一个格子
+        {
+            j++;
+            path[k] = 2;
+        }
+        else
+        {
+            LCDPrintf("ERROR");
+        }
+    }
+}
+
+/*---通过path数组移动到目标点函数---*/
+void drive_path(int len, int *path)
+{
+    for(int i = 0; i < len; i++)
+    {
+        go_to(path[i]);
+    }
+}
--- a/bot_code_turn/maze_func_pid.cpp
+++ b/bot_code_turn/maze_func_pid.cpp
@@ -0,0 +1,125 @@
+#include<iostream>
+#include<string>
+#include"eyebot++.h"
+#include"maze_parameter.h"
+#include"maze_func.h"
+using namespace std;
+
+
+/*---对PID控制的积分部分I限幅---*/
+void eI_xianfu()
+{
+  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
+  {
+    //此处为空;
+  }
+}
+
+void pid_speed_xianfu()
+{
+  if (Kpid_speed >= speed_xianfu)
+  {
+    Kpid_speed = speed_xianfu;
+  }
+  else if (Kpid_speed <= -speed_xianfu)
+  {
+    Kpid_speed = -speed_xianfu;
+  }
+  else 
+  {
+    //此处为空
+  }
+}
+
+/*---位置式PID算法---*/
+void PID_AL()
+{
+  //~ err_sum += err;   //积分累计误差值
+  //~ eI_xianfu();   //积分部分限幅
+  err_diff = err - err_old;   //微分部分误差值
+  /*---打印输出PID控制的各项参数1---*/
+    //~ cout << "err:" << err << " " ;
+    //~ cout << "old_err:" << 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 = (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控制的各项参数2---*/
+    cout << "PID_out:" << Kpid_speed << endl ;
+  /*---打印结束---*/
+  // VWSetSpeed(speed,Kpid*Kpid_base);   //控制小车的行驶速度，角速度(模拟器可以用，实物no)
+ 
+  MOTORDriveRaw(2,speed_r);
+  MOTORDriveRaw(1,speed_l);
+}
+
+/*---PID控制下的直线行驶---*/
+void PIDStraight()
+{
+  int bot_move;    //定义基础角速度和速度,和移动距离
+  int L_PSD, R_PSD, F_PSD;    //定义左侧，右侧，前方的距离值
+  int x_1,x_2, y_1,y_2, phi_1,phi_2;    //VWGetPosition的参数定义
+  VWGetPosition(&x_1, &y_1, &phi_1);    //获取机器人在移动前的x,y,phi值
+  
+  MOTORDriveRaw(2,speed_r);
+  MOTORDriveRaw(1,speed_l);
+
+
+  do 
+  {
+    F_PSD = PSDGetRaw(1) - 2;    //读取前方和墙壁的距离
+    L_PSD = PSDGetRaw(2) - 16;   //读取左侧和墙壁的距离
+    R_PSD = PSDGetRaw(3);    //读取右侧和墙壁的距离
+
+    /*---打印输出PSD数值---*/
+    cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << "\t" ;
+    /*---打印结束---*/
+
+    if (20<L_PSD && L_PSD<140  &&  20<R_PSD && R_PSD<140)   //如果两侧都有墙的情况
+    {
+      err = L_PSD - R_PSD;
+      PID_AL();
+    }
+    else if (20<L_PSD && L_PSD<140)    //只有左侧有墙的情况
+    {
+      err = L_PSD - DIST_wall_RL;
+      PID_AL();
+    }
+    else if (20<R_PSD && R_PSD<140)    //只有右侧有墙的情况
+    {
+      err = DIST_wall_RL - R_PSD;
+      PID_AL();
+    }
+    else
+    {
+      L_PSD = L_PSD % 300;
+      R_PSD = R_PSD % 300;
+      err = L_PSD - R_PSD;
+      PID_AL();
+    }
+    VWGetPosition(&x_2, &y_2, &phi_2);
+    bot_move = sqrt(pow(x_2-x_1,2) + pow(y_2-y_1,2));
+    //~ cout << "move:" << bot_move << endl;
+
+  }while (bot_move < DIST_move  &&  F_PSD > DIST_wall_F);    //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
+
+  MOTORDriveRaw(1,0);
+  MOTORDriveRaw(2,0);
+}
--- a/bot_code_turn/maze_func_turn.cpp
+++ b/bot_code_turn/maze_func_turn.cpp
@@ -0,0 +1,96 @@
+#include<iostream>
+#include<string>
+#include"eyebot++.h"
+#include"maze_parameter.h"
+#include"maze_func.h"
+using namespace std;
+
+
+void BOTturn(int turn)
+{
+	int f_wall, b_wall, r_wall, l_wall;
+    int old_f_wall, old_b_wall;
+    int turn_check;
+
+    if (turn == -1)  //右转
+    {
+		l_wall = PSDGetRaw(2) - 16;
+		if (l_wall < 145)
+		{
+			b_wall = PSDGetRaw(4) - 2;
+            do
+            {
+                old_b_wall = b_wall;
+                MOTORDriveRaw(1,19);
+                MOTORDriveRaw(2,-17);
+                OSWait(850);
+                MOTORDriveRaw(1,0);
+                MOTORDriveRaw(2,0);
+                b_wall = (PSDGetRaw(4) - 2) * 0.7  +  old_b_wall * 0.3;   //减小突变
+                turn_check = b_wall - old_b_wall;
+                OSWait(50);
+            } while ( turn_check < -3  ||  b_wall > 140);
+		}
+		else 
+		{
+			ENCODERReset(1);
+			ENCODERReset(2);
+        	do 
+			{
+				MOTORDriveRaw(1,21);
+				MOTORDriveRaw(2,-19);
+			} while (abs(ENCODERRead(2)) < 218);
+		}
+		MOTORDriveRaw(1,0);
+		MOTORDriveRaw(2,0);
+    }
+    else if (turn == 1)    //左转
+    {
+		r_wall = PSDGetRaw(3);
+		if (r_wall < 145)
+		{
+			b_wall = PSDGetRaw(4) - 2;
+            do 
+            {
+                old_b_wall = b_wall;
+                MOTORDriveRaw(1,-19);
+                MOTORDriveRaw(2,17);
+                OSWait(850);
+                MOTORDriveRaw(1,0);
+                MOTORDriveRaw(2,0);
+                b_wall = (PSDGetRaw(4) - 2) * 0.7  +  old_b_wall * 0.3;   //减小突变
+                turn_check = b_wall - old_b_wall;
+				OSWait(50);
+            } while (turn_check < -3  ||  b_wall > 130);
+		}
+		else
+		{
+        	ENCODERReset(1);
+			ENCODERReset(2);
+			do 
+			{
+				MOTORDriveRaw(1,-21);
+				MOTORDriveRaw(2,19);
+			} while (abs(ENCODERRead(2)) < 240);
+		}
+		MOTORDriveRaw(1,0);
+		MOTORDriveRaw(2,0);
+	}
+    else if (turn == 2)     //掉头
+    {
+        ENCODERReset(1);
+		ENCODERReset(2);
+		do 
+		{
+			MOTORDriveRaw(1,21);
+			MOTORDriveRaw(2,-19);
+		
+		} while (abs(ENCODERRead(2)) < 500);
+		MOTORDriveRaw(1,0);
+		MOTORDriveRaw(2,0);
+    }
+    else 
+    {
+        cout << "errer" << endl;
+    }
+}
--- a/bot_code_turn/maze_main.cpp
+++ b/bot_code_turn/maze_main.cpp
@@ -0,0 +1,42 @@
+#include<iostream>
+#include<string>
+#include<vector>
+#include"eyebot++.h"
+#include"maze_parameter.h"
+#include"maze_func.h"
+using namespace std;
+
+
+/*---下面是主函数---*/
+int main()
+{
+  explore();  //使用递归算法构建地图
+  wall[0][0][0] = 1;
+
+  const int mazesize_x = mark.size();       //获取探索结束之后的迷宫的X轴长度
+  const int mazesize_y = mark[0].size();    //获取探索结束之后的迷宫的Y轴长度
+
+  int copy_mark[mazesize_x * mazesize_y] = {};           //复制一份mark数组，并转换为一维数组，便于后续频繁读取提升性能并初始化为0
+  int copy_wall[(mazesize_x + 1) * (mazesize_y + 1) * 2] = {};   //复制一份wall数组，并转换为一维数组，便于后续频繁读取提升性能并初始化为0
+
+  array_copy_mark(mazesize_x, mazesize_y, copy_mark);   //复制一份mark数组，并转换为一维数组，便于后续频繁读取提升性能
+  array_copy_wall(mazesize_x, mazesize_y, copy_wall);   //复制一份wall数组，并转换为一维数组，便于后续频繁读取提升性能
+
+  output_arr2D(mazesize_x, mazesize_y, copy_mark);    //打印地图mark信息
+  output_arrwall(mazesize_x, mazesize_y, copy_wall);  //打印墙壁wall信息
+
+  int map[mazesize_x * mazesize_y] = {};       //创建最短路径求解地图并初始化为0
+  array_negative_one(mazesize_x * mazesize_y, map);        //将map数组初始化为-1
+  
+  flood(map, copy_wall);  //洪水填充算法
+  output_arr2D(mazesize_x, mazesize_y, map);  //打印map的数组信息
+
+  const int len = map[((target_x - 1) * mazesize_y) + (target_y -1)];  //迷宫的终点（人为确定）
+  int path[len] = {};   //创建最短路径结果地图并初始化为0
+
+  build_path(target_x - 1, target_y - 1, len, path, map, copy_wall);   //构建出最短路径path数组
+  drive_path(len, path);  //移动到指定目标点
+
+  output_arrpath(len, path);   //打印path数组信息，及小车每一步的行驶方向
+  
+}
--- a/bot_code_turn/maze_parameter.cpp
+++ b/bot_code_turn/maze_parameter.cpp
@@ -0,0 +1,49 @@
+#include<iostream>
+#include<string>
+#include<vector>
+#include"eyebot++.h"
+#include"maze_parameter.h"
+// #include"maze_func.h"
+using namespace std;
+
+/*---定义数据---*/
+/*---定义全局常量---*/
+const int DIST_cell = 240;     	//单元格长度
+const int DIST_move = 292;		//移动的单元格距离
+const int DIST_wall_F = 90;     //与墙壁的距离
+const int DIST_wall_RL = 88;      //与左右墙壁的距离
+
+/*---定义全局变量---*/
+/*---容器类---*/
+std::vector<std::vector<int>> mark(1,std::vector<int>(1,0));                                   //定义mark容器（二维容器），容器初始为1行1列    1=为已访问
+std::vector<std::vector<std::vector<int>>> wall(2,std::vector<std::vector<int>>(2,std::vector<int>(2,0)));    //定义wall容器（三维容器），容器初始为1行1列，且每个格子内带有2个墙壁信息   1=墙壁；0=通路
+
+/*---机器人坐标---*/
+int rob_x0 = 0;     //机器人起点X坐标
+int rob_y0 = 0;     //机器人起点Y坐标
+int rob_x = 0;      //机械人当前X坐标（初始为0）
+int rob_y = 0;      //机器人当前Y坐标（初始为0）
+int rob_dir = 0;    //机器人当前的朝向（初始为0）
+
+/*---递归算法---*/
+
+
+/*---PID算法---*/
+const float Kp = 0.4, Ki = 0, Kd = 0.25;   //定义PID的参数17
+//~ 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_l = 16;					//定义基本左轮速度
+//~ const int speed_r = speed_l - 2;		//定义基本右轮速度
+int speed_r = speed_l - 2;		//定义基本右轮速度
+const int speed_xianfu = 2;				//定义kpid限幅
+int err, err_old, err_sum, err_diff;	//定义误差，上一次误差，积分误差，微分误差
+const int eI_max = 200;                 //定义积分限幅
+
+/*---目标点的坐标---*/
+const int target_x = 4;   //声明目标点的X坐标
+const int target_y = 4;   //声明目标点的Y坐标
+
+/*---定义结束---*/
--- a/bot_code_turn/maze_parameter.h
+++ b/bot_code_turn/maze_parameter.h
@@ -0,0 +1,44 @@
+#pragma once
+#include<vector>
+
+/*---声明数据---*/
+/*---声明全局常量---*/
+extern const int DIST_cell;         //单元格长度
+extern const int DIST_move;			//移动的单元格距离
+extern const int DIST_wall_F;		//与前面墙壁的距离
+extern const int DIST_wall_RL;      //与左右墙壁的距离
+
+/*---声明全局变量---*/
+
+/*---声明全局容器---*/
+extern std::vector<std::vector<int>> mark;                  //声明mark容器（二维容器）
+extern std::vector<std::vector<std::vector<int>>> wall;     //声明wall容器（三维容器）
+
+/*---声明机器人坐标---*/
+extern int rob_x0;  //机器人起点X坐标
+extern int rob_y0;  //机器人起点Y坐标
+extern int rob_x;   //机械人当前X坐标
+extern int rob_y;   //机器人当前Y坐标
+extern int rob_dir; //机器人当前的朝向
+
+/*---各种全局量声明结束---*/
+
+/*---递归算法---*/
+
+/*---PID算法---*/
+extern const float Kp, Ki, Kd;                  //声明PID的参数
+//~ extern float Kpid;                              //定义PID计算后输出的倍率
+extern int Kpid_speed;                          //定义计算后的速度
+extern const float Kpid_base;					//定义速度基础倍数速度
+extern const int speed_l;						//定义左轮基本速度
+//~ extern const int speed_r;						//定义右轮基本速度
+extern int speed_r;						//定义右轮基本速度
+extern const int speed_xianfu;                  //kpid输出速度的限幅
+extern int err, err_old, err_sum, err_diff;     //定义误差，上一次误差，积分误差，微分误差
+extern const int eI_max;                        //定义积分限幅
+
+/*---目标点坐标---*/
+extern const int target_x;    //声明目标点的X坐标
+extern const int target_y;    //声明目标点的Y坐标
+
+/*---声明结束---*/
--- a/bot_code_turn/全局变量.txt
+++ b/bot_code_turn/全局变量.txt
@@ -0,0 +1,16 @@
+几个数组：
+wall，mark，maze....
+
+机器人坐标：
+rob_x   机器人X坐标
+rob_y   机器人Y坐标
+rob_dir   机器人朝向
+
+迷宫基础数据：
+DIST_cell   单个单元格的长度
+DIST_wall   PID直线的时候，检测与墙壁的距离值
+
+PID：
+err，err_old，err_sum，err_diff, eI_max   误差，上一次误差，积分累计误差，微分误差变化，积分限幅
+Kp, Ki, Kd, Kpid    PID控制的参数P,I,D以及算法输出的倍率参数
+
--- a/bot_code_turn/名称解释.txt
+++ b/bot_code_turn/名称解释.txt
@@ -0,0 +1,5 @@
+头文件:mazetool.h
+函数文件:mazetool.cpp
+全局:
+rob_y:机器人在迷宫地图中的Y坐标值
+rob_x:机器人在迷宫地图中的x坐标值
--- a/robot_test_code/PID/PID.cpp
+++ b/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();
--- a/robot_test_code/PID/PIDmain.cpp
+++ b/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_wall_f = 120;
+const int DIST_cell = 310;
+const int DIST_wall_f = 75;
 const int DIST_wall_rl = 90;
 /*---全局各种变量与常量定义完成---*/

--- a/robot_test_code/实机测试/makefile
+++ b/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/psd.x
+#~ file_o := /home/pi/usr/turn.x
+file_o := /home/pi/usr/psd.x

 $(file_o) : $(file_cpp)
 	@$(cxx) $^ -o $@
--- a/robot_test_code/实机测试/test_psd.cpp
+++ b/robot_test_code/实机测试/test_psd.cpp
@@ -11,8 +11,8 @@ int main()
    LCDMenu("","","","END");
    do
    {
-        Fwall = PSDGetRaw(1);   //检测前面侧是否有墙
-        Lwall = PSDGetRaw(2);   //检测左面
+        Fwall = PSDGetRaw(1)-2;   //检测前面侧是否有墙
+        Lwall = PSDGetRaw(2)-16;   //检测左面
        Rwall = PSDGetRaw(3);   //检测右面
        bwall = PSDGetRaw(4);   //
        cout << "PSD L:" << Lwall << " R:" << Rwall << " F:" << Fwall << " b:" << bwall << endl; ;
--- a/robot_test_code/实机测试/test_turn.cpp
+++ b/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 old_f_wall, old_r_wall, old_l_wall, old_b_wall;
-    int b_wall;
-    int old_b_wall;
+    int f_wall, b_wall;
+    int old_f_wall, 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);
--- a/robot_test_code/实机测试/test_turn2.cpp
+++ b/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);
+	
+}
--- a/文件/eyebot函数.cpp
+++ b/文件/eyebot函数.cpp
@@ -6,3 +6,5 @@ VWSetSpeed();
 VWGetPosition();
 VWSetPosition();
 VWWait();
+ENCODERReset();
+ENCODERRead();
Author	SHA1	Message	Date
Ñõԭ×Ó	1155ae6f6d	2025.5.18第二次提交，重新写一份复杂的，慢一些的turn函数	2025-05-18 11:55:43 +08:00
Ñõԭ×Ó	df42d6ce7f	2025.5.18第一次提交，包括17号和今天上午对实机的调试	2025-05-18 11:35:02 +08:00