1

编写代码/maze_func.h

@@ -0,0 +1,23 @@
+#pragma once
+
+/*---PART1---*/
+/*---以下为递归算法探索部分涉及到的函数声明---*/
+bool check_mark();    //检查单元格各方面是否已被确认
+void maze_entry(int x, int y, int dir, int open);   //maze_entry函数,用于写入到wall数组中，1：有墙 0：通路
+void xneighbor(int x, int dir);   //X坐标更新
+void yneighbor(int y, int dir);   //Y坐标更新
+void eI_xianfu(int eI_max);    //对PID控制的积分部分I限幅
+void PID_AL();    //位置式PID算法
+void PIDStraight();   //PID控制下的直线行驶
+void go_to(int dir);    //行驶到指定单元格
+bool unmarked(int y, int x, int dir);   //检查该方向是否已被标记
+void explore();   //递归函数
+void output_mark();    //打印mark数组检查错误
+void output_wall();    //打印wall数组检查错误
+/*---以上为递归算法探索部分涉及到的函数声明---*/
+
+/*---PART2---*/
+/*---以下为洪水填充算法设涉及到的函数声明---*/
+
+
+

编写代码/maze_func_explore.cpp

@@ -0,0 +1,327 @@
+#include<iostream>
+#include<string>
+#include"eyebot++.h"
+#include"maze_func.h"
+#include"maze_parameter.h"
+using namespace std;
+
+
+/*---检查单元格各方向是否已被到访---*/
+bool check_mark()
+{ bool check = false;
+  int x, y;
+  x = rob_x;  //机器人当前x坐标
+  y = rob_y;  //机器人当前y坐标
+  
+  bool N_road = ((wall[x][y+1][0] + 1) % 2 && mark[x][y+1]);
+  bool W_road = ((wall[x][y][1] + 1) % 2 && mark[x-1][y]);
+  bool S_road = ((wall[x][y][0] + 1) % 2 && mark[x][y-1]);
+  bool E_road = ((wall[x+1][y][1] + 1) % 2 && mark[x+1][y]);
+  
+  if (x>0 && y>0) //情况1
+  {
+    if (N_road && W_road && S_road && E_road)
+    {
+      check = true;
+    }
+  }
+  else if (x==0 && y>0)  //情况2
+  {
+    if (N_road && S_road && E_road)
+    {
+      check = true;
+    }
+  }
+  else if (x>0 && y==0)  //情况3
+  {
+    if (N_road && W_road && E_road)
+    {
+      check = true;
+    }
+  }
+  else  //情况4
+  {
+    if (N_road && E_road)
+    {
+      check = true;
+    }
+  }
+  // if (check)
+  // {
+  //   goto end_explore; 
+  // }
+  return check;
+}
+
+/*---maze_entry函数,用于写入到wall数组中，1：有墙 0：通路---*/
+void maze_entry(int x, int y, int dir, int open)
+{
+  int maze_dir = (dir + 4) % 4;
+  
+  if (open) //获取到的open信息为是否有路径信息，转换为墙壁信息
+  {
+    open = 0;
+  }
+  else
+  {
+    open = 1;
+  }
+  
+  switch (maze_dir)
+  {
+    case 0:  //0表示北(上)，表示(x，y+1)坐标单元格的下方墙壁信息
+    wall[x][y+1][0] = open;
+    break;
+    
+    case 1:  //1表示西(左)，表示(x，y)坐标单元格的左侧墙壁信息
+    wall[x][y][1] = open;
+    break;
+    
+    case 2:  //2表示南(下)，表示(x，y)坐标单元格的下侧墙壁信息
+    wall[x][y][0] = open;
+    break;
+    
+    case 3:  //3表示东(右)，表示(x+1，y)坐标单元格的左侧墙壁信息
+    wall[x+1][y][1] = open;
+    break;
+  }
+}
+
+/*---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;
+  }
+}
+
+/*---对PID控制的积分部分I限幅---*/
+void eI_xianfu(int eI_max)
+{
+  if (err_sum >= eI_max)    //如果err_sum大于限幅，则直接让err_sum=限幅，防止err_sum过大，影响PID控制
+  {
+    err_sum = eI_max;
+  }
+}
+
+/*---位置式PID算法---*/
+void PID_AL()
+{
+  err_sum += err;   //积分累计误差值
+  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" ;
+  /*---打印结束---*/
+  err_old = err;    //将误差幅值到上一次误差
+  
+  Kpid = Kp*err + Ki*err_sum + Kd*err_diff;   //PID控制算法输出的倍率
+  /*---打印输出PID控制的各项参数2---*/
+    // cout << "PID输出值:" << Kpid << endl ;
+  /*---打印结束---*/
+  VWSetSpeed(SPEED,Kpid*Alpha);   //控制小车的行驶速度，角速度
+}
+
+/*---PID控制下的直线行驶---*/
+void PIDStraight()
+{
+  int DIST_move;    //定义基础角速度和速度,和移动距离
+  int L_PSD, R_PSD, F_PSD;    //定义左侧，右侧，前方的距离值
+  int x_1,x_2, y_1,y_2, phi_1,phi_2;    //VWGetPosition的参数定义
+  SPEED = 200;
+  VWGetPosition(&x_1, &y_1, &phi_1);    //获取机器人在移动前的x,y,phi值
+  
+  do 
+  {
+    L_PSD = PSDGet(PSD_LEFT);   //读取左侧和墙壁的距离
+    R_PSD = PSDGet(PSD_RIGHT);    //读取右侧和墙壁的距离
+    F_PSD = PSDGet(PSD_FRONT);    //读取前方和墙壁的距离
+    /*---打印输出PSD数值---*/
+      // 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)   //如果两侧都有墙的情况
+    {
+      err = L_PSD - R_PSD;
+      PID_AL();
+    }
+    else if (100<L_PSD && L_PSD<180)    //只有左侧有墙的情况
+    {
+      err = L_PSD - DIST_wall;
+      PID_AL();
+    }
+    else if (100<R_PSD && R_PSD<180)    //只有右侧有墙的情况
+    {
+      err = DIST_wall - 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));
+    if (DIST_move > (DIST_cell -50))    //当移动距离只剩下最后的50mm的时候降点速度
+    {
+      SPEED = SPEED2;
+    }
+  }while (DIST_move < DIST_cell && F_PSD > DIST_wall);    //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
+}
+
+/*---行驶到指定单元格---*/
+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) //如果turn不是0，则执行旋转命令
+  {
+    VWTurn(turn*90,45);
+    VWWait();
+  }
+  PIDStraight();   //使用PID算法算法，向指定方向直线行驶一格
+  VWWait();   //等待下一条指令
+  
+  rob_dir = maze_dir;  //更新机器人移动到下一格之后的方向
+  xneighbor(rob_x,rob_dir); //更新机器人移动到下一格之后的X坐标
+  yneighbor(rob_y,rob_dir); //更新机器人移动到下一格之后的y坐标
+}
+
+/*---检查该方向是否已被标记---*/
+bool unmarked(int x, int y, int dir)
+{
+  bool check = false;
+  int maze_dir = (dir + 4) % 4;
+  switch(maze_dir)
+  {
+    case 0:
+    if (mark[x][y+1] == 0)
+    {
+      check = true;
+    }
+    break;
+    
+    case 1:
+    if (mark[x-1][y] == 0)
+    {
+      check = true;
+    }
+    break;
+    
+    case 2:
+    if (mark[x][y-1] == 0)
+    {
+      check = true;
+    }
+    break;
+    
+    case 3:
+    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;   //将当前的单元格设置为已到访
+  L_open = PSDGet(PSD_LEFT) > DIST_cell;    //获取左侧空间状况
+  F_open = PSDGet(PSD_FRONT) > DIST_cell;   //获取前侧空间状况
+  R_open = PSDGet(PSD_RIGHT) > 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();
+} //递归结束

编写代码/maze_func_flood.cpp

@@ -0,0 +1,64 @@
+#include<iostream>
+#include<string>
+#include"eyebot++.h"
+#include"maze_func.h"
+#include"maze_parameter.h"
+using namespace std;
+
+void flood()
+{
+  int num = 0;
+  
+  do
+  {
+    num ++;
+    for (i = 1; i < mazesize_x - 1; i++)
+    { 
+      for (j = 1; j < mazesize_y - 1; j++)
+      { 
+        if (map[i][j] == -1)
+        { 
+          if (i > 1)
+          {
+            if (! wall[i][j][1] && map[i-1][j] != -1)
+            {
+              copy_map[i][j] = map[i-1][j] + 1;
+            }
+          }
+
+          if (i < mazesize_x - 2)
+          {
+            if (! wall[i+1][j][1] && map[i+1][j] != -1)
+            {
+              copy_map[i][j] = map[i+1][j] = 1;
+            }
+          }
+          
+          if (j > 1)
+          {
+            if (! wall[i][j][0] && map[i][j-1] != -1)
+            {
+              copy_map[i][j] = map[i][j-1] + 1;
+            }
+          }
+
+          if (j < mazesize_y - 2)
+          {
+            if (! wall[i][j+1][0] && map[i][j+1] != -1)
+            {
+              copy_map[i][j] = map[i][j+1] + 1;
+            }
+          }
+        }
+      }
+    }
+
+    for (i = 0; i < mazesize_x; i++)
+    {
+      for (j = 0; j < mazesize_y; j++)
+      {
+        map[i][j] = copy_map[i][j];
+      }
+    }
+  } while (map[target_x][target_y] == -1 && num < ((mazesize_x - 2) * (mazesize_y - 2)))
+}

编写代码/maze_func_output.cpp

@@ -0,0 +1,52 @@
+#include<iostream>
+#include<string>
+#include"eyebot++.h"
+#include"maze_func.h"
+#include"maze_parameter.h"
+using namespace std;
+
+
+/*---打印数组检查错误---*/
+/*---打印数组mark---*/
+void output_mark()
+{
+  for (int i = 5; i >= 0; i--)
+  {
+    for (int j = 0; j < 6; j++)
+    {
+      cout << mark[j][i] ;
+    }
+    cout << endl;
+  }
+}
+
+/*---打印数组wall---*/
+void output_wall()
+{
+  for (int i = 6; i >= 0; i--)
+  {
+    for (int j = 0; j < 7; j++)
+    {
+      if (wall[j][i][1] == 1)
+      {
+        cout << "|" ;
+      }
+      else
+      {
+        cout << " " ;
+      }
+      
+      if (wall[j][i][0] == 1)
+      {
+        cout << "_" ;
+      }
+      else
+      {
+        cout << " " ;
+      }
+    }
+    cout << endl;
+  }
+}
+
+/*---打印数组map---*/

编写代码/maze_func_path.cpp

@@ -0,0 +1,6 @@
+#include<iostream>
+#include<string>
+#include"eyebot++.h"
+#include"maze_func.h"
+#include"maze_parameter.h"
+using namespace std;

编写代码/maze_main.cpp

@@ -0,0 +1,13 @@
+#include<iostream>
+#include"maze_func.h"
+#include"maze_parameter.h"
+using namespace std;
+
+
+
+/*---下面是主函数---*/
+int main()
+{
+  explore();
+  output();
+}

编写代码/maze_parameter.cpp

@@ -0,0 +1,42 @@
+#include<iostream>
+#include<string>
+#include"eyebot++.h"
+#include"maze_func.h"
+#include"maze_parameter.h"
+using namespace std;
+
+
+/*---定义全局常量---*/
+const int DIST_cell = 360 ;
+const int DIST_wall = 130 ;
+// const int mazesize_x = 6 ;
+// const int mazesize_y = 6 ;
+/*---全局各种常量定义完成---*/
+
+/*---定义全局变量---*/
+/*---数组类---*/
+int mark[6][6] = {}; //1=为已访问
+int wall[7][7][2] = {}; //1=墙壁；0=通路
+int map[6][6] = {};   //最短路径求解地图
+int copy_map[6][6] = {};    //copy复制地图
+int path[6][6] = {};    //最短路径结果地图
+// int mark[mazesize_x][mazesize_y] = {}; //1=为已访问
+// int wall[mazesize_x + 1][mazesize_y + 1][2] = {}; //1=墙壁；0=通路
+//wall[1][1][0] = 1;    //定义迷宫的初始小车背后为墙
+// int map[mazesize_x][mazesize_y];  //最短路径求解地图
+// int copy_map[mazesize_x][mazesize_y]; //copy复制地图
+// int path[mazesize_x][mazesize_y]; //最短路径结果地图
+/*---机器人坐标---*/
+int rob_x = 1 ;   //机械人初始的X坐标
+int rob_y = 1 ;   //机器人初始的Y坐标
+int rob_dir = 0 ; //机器人初始的朝向
+
+/*---递归算法---*/
+/*---PID算法---*/
+const float Kp = 1, Ki = 0, Kd = 0.5;   //定义PID的参数
+float Kpid;   //定义PID计算后输出的倍率
+const int Alpha = 2, SPEED = 200;   //定义基本角速度和速度
+const int SPEED2 = 0.25*SPEED;    //快到目的地时减速
+int err, err_old, err_sum, err_diff;    //定义误差，上一次误差，积分误差，微分误差
+const int eI_max = 20;    //定义积分限幅
+/*---全局各种变量定义完成---*/

编写代码/maze_parameter.h

@@ -0,0 +1,34 @@
+#pragma once
+
+/*---声明数据---*/
+/*---声明全局常量---*/
+extern const int DIST_cell;   //单元格长度
+extern const int DIST_wall;   //与墙壁的距离
+extern const int mazesize_x;  //迷宫X轴长度
+extern const int mazesize_y;  //迷宫Y轴高度
+/*---声明全局变量---*/
+/*---声明数组类---*/
+extern int mark[6][6];    //迷宫单元格是否访问数组
+extern int wall[7][7][2];   //迷宫墙壁信息数组
+extern int map[6][6];   //迷宫地图数组
+extern int copy_map[6][6];    //迷宫地图复制数组
+extern int path[6][6];
+/*---声明机器人坐标---*/
+extern int rob_x;
+extern int rob_y;
+extern int rob_dir;
+/*---各种全局量声明结束---*/
+
+/*---递归算法---*/
+/*---PID算法---*/
+extern const float Kp, Ki, Kd;   //声明PID的参数
+extern float Kpid;   //定义PID计算后输出的倍率
+extern const int Alpha, SPEED;   //定义基本角速度和速度
+extern const int SPEED2;    //快到目的地时减速
+extern int err, err_old, err_sum, err_diff;    //定义误差，上一次误差，积分误差，微分误差
+extern const int eI_max = 20;    //定义积分限幅
+
+
+
+
+/*---全局各种变量声明完成---*/

编写代码/全局变量.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以及算法输出的倍率参数
+

编写代码/名称解释.txt

@@ -0,0 +1,5 @@
+头文件:mazetool.h
+函数文件:mazetool.cpp
+全局:
+rob_y:机器人在迷宫地图中的Y坐标值
+rob_x:机器人在迷宫地图中的x坐标值