Merge branch 'master' of https://codehub.devcloud.cn-east-3.huaweicloud.com/2e32552ddb1a4a45b5e8c3c0e5705b48/bk_bishe_pi

bot_code/maze_func_explore.cpp

@@ -104,14 +104,16 @@ void maze_entry(int x, int y, int dir, int open)
     {
       if (!mark.empty()  &&  !wall.empty()) //插入安全性检测，确保容器非空
       {
-        // std::vector<int> mark_insert(mark[0].size(), 0);
+        //~ std::vector<int> mark_insert(mark[0].size(), 0);
-        // mark.insert(mark.begin(), mark_insert);
+        //~ mark.insert(mark.begin(), mark_insert);
 
-        // std::vector<std::vector<int>> wall_insert(wall[0].size(), std::vector<int>(2,0));
+        //~ std::vector<std::vector<int>> wall_insert(wall[0].size(), std::vector<int>(2,0));
-        // wall.insert(wall.begin(), wall_insert);
+        //~ 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头部插入一个内层
+        rob_x++;
+        rob_x0++;
       }
       else 
       {
@@ -134,6 +136,8 @@ void maze_entry(int x, int y, int dir, int open)
         {
           wall[i].emplace(wall[i].begin(), std::vector<int>(2,0));
         }
+        rob_y++;
+        rob_y0++;
       }
       else 
       {

bot_code/maze_func_goto.cpp

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

bot_code/maze_func_pid.cpp

@@ -42,8 +42,8 @@ void pid_speed_xianfu()
 /*---位置式PID算法---*/
 void PID_AL()
 {
-  //~ err_sum += err;   //积分累计误差值
+  err_sum += err;   //积分累计误差值
-  //~ eI_xianfu();   //积分部分限幅
+  eI_xianfu();   //积分部分限幅
   err_diff = err - err_old;   //微分部分误差值
   /*---打印输出PID控制的各项参数1---*/
     //~ cout << "err:" << err << " " ;
@@ -54,19 +54,21 @@ void PID_AL()
   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 + Ki*err_sum + Kd*err_diff) * Kpid_base;
-  Kpid_speed = (Kp*err + Kd*err_diff) * Kpid_base;
+  //~ Kpid_speed = (Kp*err + Kd*err_diff) * Kpid_base;
   
   pid_speed_xianfu();
-  speed_r = speed_l + Kpid_speed;
+  //~ speed_r = speed_l + Kpid_speed;
 
 
   /*---打印输出PID控制的各项参数2---*/
-    cout << "PID_out:" << Kpid_speed << endl ;
+    //~ cout << "PID_out:" << Kpid_speed << endl ;
+        //~ cout << "PID_out:" << Kpid_speed << " " ;
   /*---打印结束---*/
   // VWSetSpeed(speed,Kpid*Kpid_base);   //控制小车的行驶速度，角速度(模拟器可以用，实物no)
  
-  MOTORDriveRaw(2,speed_r);
+  MOTORDriveRaw(2,speed_l - 2 + Kpid_speed);
+  //~ MOTORDriveRaw(2,speed_r);
   MOTORDriveRaw(1,speed_l);
 }
 
@@ -77,16 +79,26 @@ void PIDStraight()
   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值
+  //~ cout << x_1 <<  " " << y_1 << " " << phi_1 << endl;
   
-  MOTORDriveRaw(2,speed_r);
+  F_PSD = PSDGetRaw(1) - 2;    //读取前方和墙壁的距离
+  L_PSD = PSDGetRaw(2) - 16;   //读取左侧和墙壁的距离
+  R_PSD = PSDGetRaw(3);    //读取右侧和墙壁的距离
+  
+  //~ MOTORDriveRaw(2,speed_r);
+  MOTORDriveRaw(2,speed_l - 2);
   MOTORDriveRaw(1,speed_l);
 
 
   do 
   {
-    F_PSD = PSDGetRaw(1) - 2;    //读取前方和墙壁的距离
+    //~ F_PSD = PSDGetRaw(1) - 2;    //读取前方和墙壁的距离
-    L_PSD = PSDGetRaw(2) - 16;   //读取左侧和墙壁的距离
+    //~ L_PSD = PSDGetRaw(2) - 16;   //读取左侧和墙壁的距离
-    R_PSD = PSDGetRaw(3);    //读取右侧和墙壁的距离
+    //~ R_PSD = PSDGetRaw(3);    //读取右侧和墙壁的距离
+    
+    F_PSD = (PSDGetRaw(1) - 2) * 0.3  +  F_PSD * 0.7;    //读取前方和墙壁的距离
+    L_PSD = (PSDGetRaw(2) - 16) * 0.3  +  L_PSD * 0.7;   //读取左侧和墙壁的距离
+    R_PSD = (PSDGetRaw(3)) * 0.3  +  R_PSD * 0.7;    //读取右侧和墙壁的距离
 
     /*---打印输出PSD数值---*/
     cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << "\t" ;
@@ -116,7 +128,7 @@ void PIDStraight()
     }
     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;
+    cout << "move:" << bot_move << endl;
 
   }while (bot_move < DIST_move  &&  F_PSD > DIST_wall_F);    //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
 

bot_code/maze_func_turn.cpp

@@ -14,10 +14,10 @@ void BOTturn(int turn)
         ENCODERReset(2);
         do 
 		{
-			MOTORDriveRaw(1,21);
+			MOTORDriveRaw(1,20);
-			MOTORDriveRaw(2,-19);
+			MOTORDriveRaw(2,-18);
             
-		} while (abs(ENCODERRead(2)) < 218);
+		} while (abs(ENCODERRead(2)) < 240);
         
 		MOTORDriveRaw(1,0);
 		MOTORDriveRaw(2,0);
@@ -28,8 +28,8 @@ void BOTturn(int turn)
 		ENCODERReset(2);
 		do 
 		{
-			MOTORDriveRaw(1,-21);
+			MOTORDriveRaw(1,-20);
-			MOTORDriveRaw(2,19);
+			MOTORDriveRaw(2,18);
 
 		} while (abs(ENCODERRead(2)) < 240);
 		
@@ -42,12 +42,13 @@ void BOTturn(int turn)
 		ENCODERReset(2);
 		do 
 		{
-			MOTORDriveRaw(1,21);
+			MOTORDriveRaw(1,20);
-			MOTORDriveRaw(2,-19);
+			MOTORDriveRaw(2,-18);
 		
-		} while (abs(ENCODERRead(2)) < 500);
+		} while (abs(ENCODERRead(2)) < 550);
 		MOTORDriveRaw(1,0);
 		MOTORDriveRaw(2,0);
+		//~ speed_r = speed_l - Kpid_speed;
     }
     else 
     {

bot_code/maze_parameter.cpp

@@ -9,9 +9,9 @@ using namespace std;
 /*---定义数据---*/
 /*---定义全局常量---*/
 const int DIST_cell = 240;     	//单元格长度
-const int DIST_move = 292;		//移动的单元格距离
+const int DIST_move = 330;		//移动的单元格距离
 const int DIST_wall_F = 90;     //与墙壁的距离
-const int DIST_wall_RL = 88;      //与左右墙壁的距离
+const int DIST_wall_RL = 90;      //与左右墙壁的距离
 
 /*---定义全局变量---*/
 /*---容器类---*/
@@ -29,9 +29,10 @@ int rob_dir = 0;    //机器人当前的朝向（初始为0）
 
 
 /*---PID算法---*/
-const float Kp = 0.4, Ki = 0, Kd = 0.25;   //定义PID的参数17
+//~ 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
+const float Kp = 1.6, Ki = 0.008, Kd = 0.18;   //定义PID的参数18
 //~ float Kpid;                             //定义PID计算后输出的倍率
 int Kpid_speed;
 const float Kpid_base = 1;				//定义基本角速度和速度

bot_code_turn/makefile

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

bot_code_turn/maze_func.h

@@ -45,3 +45,6 @@ void output_arrwall(int size_x, int size_y, int *arr);      //打印wall数组
 void output_arrpath(int size, int *arr);      //打印path数组
 /*---以上为数组打印部分涉及到的函数声明---*/
 
+/*---以下为目标点输入函数声明---*/
+void cin_targey();      //输入目标点
+/*---以上为目标点输入函数声明---*/

bot_code_turn/maze_func_array.cpp

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

bot_code_turn/maze_func_cin.cpp

@@ -0,0 +1,26 @@
+#include<iostream>
+#include<string>
+#include<vector>
+#include"maze_parameter.h"
+#include"maze_func.h"
+using namespace std;
+
+void cin_targey()
+{
+    int size_x = mark.size();
+    int size_y = mark[0].size();
+
+    int num = 0;
+    do 
+    {
+        if (!num)
+        {
+            cout << "您输入的目标点超出迷宫范围，请重新输入" << endl;
+        }
+        cout << "请输入目标点的X,Y坐标值 用空格隔开并回车" << endl;
+        cin >> target_x >> target_y;
+        cout << endl;
+        num++;
+    } while (target_x <= size_x  &&  target_y <= size_y);
+    cout << endl;
+}

bot_code_turn/maze_func_explore.cpp

@@ -2,6 +2,7 @@
 #include<string>
 #include<vector>
 #include<cstddef>
+#include<cstdlib>
 #include"eyebot++.h"
 #include"maze_parameter.h"
 #include"maze_func.h"
@@ -22,7 +23,25 @@ bool check_mark()
   bool S_mark;
   bool E_mark;
 
+if (y < size_y - 1)
+  {
+    if (!wall[x][y+1][0])
+    {
+      N_mark = mark[x][y+1];
+    }
+    else
+    {
+      N_mark = true;
+    }
+  }
+  else
+  {
+    N_mark = true;
+  }
+  
   if (x > 0)
+  {
+    if (!wall[x][y][1])
     {
       W_mark = mark[x-1][y];
     }
@@ -30,8 +49,15 @@ bool check_mark()
     {
       W_mark = true;
     }
+  }
+  else
+  {
+    W_mark = true;
+  }
 
   if (y > 0)
+  {
+    if (!wall[x][y][0])
     {
       S_mark = mark[x][y-1];
     }
@@ -39,8 +65,15 @@ bool check_mark()
     {
       S_mark = true;
     }
+  }
+  else
+  {
+    S_mark = true;
+  }
 
   if (x < size_x - 1)
+  {
+    if (!wall[x+1][y][1])
     {
       E_mark = mark[x+1][y];
     }
@@ -48,22 +81,13 @@ bool check_mark()
     {
       E_mark = true;
     }
-
-  if (y < size_y - 1)
-  {
-    N_mark = mark[x][y+1];
   }
   else
   {
-    N_mark = true;
+    E_mark = true;
   }
   
-  bool N_road = (!wall[x][y+1][0]  &&  N_mark);
+  if (N_mark  &&  W_mark  &&  S_mark  &&  E_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;
   }
@@ -94,6 +118,7 @@ void maze_entry(int x, int y, int dir, int open)
       else 
       {
         cout << "error: mark or wall empty" << endl;
+        exit(1);
       }
     }
     wall[x][y+1][0] = !open;
@@ -112,10 +137,13 @@ void maze_entry(int x, int y, int dir, int open)
 
         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头部插入一个内层
+        rob_x++;
+        rob_x0++;
       }
       else 
       {
         cout << "error: mark or wall empty" << endl;
+        exit(1);
       }
     }
     wall[x][y][1] = !open;
@@ -134,10 +162,13 @@ void maze_entry(int x, int y, int dir, int open)
         {
           wall[i].emplace(wall[i].begin(), std::vector<int>(2,0));
         }
+        rob_y++;
+        rob_y0++;
       }
       else 
       {
         cout << "error: mark or wall empty" << endl;
+        exit(1);
       }
     }
     wall[x][y][0] = !open;
@@ -154,6 +185,7 @@ void maze_entry(int x, int y, int dir, int open)
       else 
       {
         cout << "error: mark or wall empty" << endl;
+        exit(1);
       }
     }
     wall[x+1][y][1] = !open;

bot_code_turn/maze_func_flood.cpp

@@ -1,7 +1,6 @@
 #include<iostream>
 #include<string>
 #include<vector>
-#include"eyebot++.h"
 #include"maze_parameter.h"
 #include"maze_func.h"
 using namespace std;
@@ -11,10 +10,11 @@ void flood(int *map, int *copy_wall)
   int num = 0;
   int size_x = mark.size();
   int size_y = mark[0].size();
-  map[0] = 0;
+  map[rob_x0 * (size_y)  +  rob_y0] = 0;
 
   do
   {
+    num++;
     for (int i = 0; i < size_x; i++)
     { 
       for (int j = 0; j < size_y; j++)
@@ -26,7 +26,7 @@ void flood(int *map, int *copy_wall)
             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++;
+              //~ num++;
             }
           }
 
@@ -35,7 +35,7 @@ void flood(int *map, int *copy_wall)
             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++;
+              //~ num++;
             }
           }
           
@@ -44,7 +44,7 @@ void flood(int *map, int *copy_wall)
             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++;
+              //~ num++;
             }
           }
 
@@ -53,7 +53,7 @@ void flood(int *map, int *copy_wall)
             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++;
+              //~ num++;
             }
           }
         }

bot_code_turn/maze_func_goto.cpp

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

bot_code_turn/maze_func_output.cpp

@@ -1,6 +1,5 @@
 #include<iostream>
 #include<string>
-#include"eyebot++.h"
 #include"maze_func.h"
 using namespace std;
 
@@ -13,10 +12,11 @@ void output_arr2D(int size_x, int size_y, int *arr)
   {
     for (int j = 0; j < size_x; j++)
     {
-      cout << arr[(j * size_y)  +  i] << " ";
+      cout << arr[(j * size_y)  +  i] << "\t";
     }
     cout << endl;
   }
+   cout << endl;
 }
 
 /*---打印数组wall---*/
@@ -46,6 +46,7 @@ void output_arrwall(int size_x, int size_y, int *arr)
     }
     cout << endl;
   }
+   cout << endl;
 }
 
 void output_arrpath(int size, int *arr)
@@ -55,4 +56,5 @@ void output_arrpath(int size, int *arr)
     cout << arr[i] << " ";
   }
   cout << endl;
+  cout << endl;
 }

bot_code_turn/maze_func_path.cpp

@@ -1,7 +1,7 @@
 #include<iostream>
 #include<string>
 #include<vector>
-#include"eyebot++.h"
+#include<cstdlib>
 #include"maze_parameter.h"
 #include"maze_func.h"
 using namespace std;
@@ -36,7 +36,8 @@ void build_path(int i, int j, int len, int *path, int *map, int *copy_wall)
         }
         else
         {
-            LCDPrintf("ERROR");
+            cout << "error" << endl;
+            exit(1);
         }
     }
 }

bot_code_turn/maze_func_pid.cpp

@@ -1,6 +1,7 @@
 #include<iostream>
 #include<string>
 #include"eyebot++.h"
+#include<cmath>
 #include"maze_parameter.h"
 #include"maze_func.h"
 using namespace std;
@@ -42,8 +43,8 @@ void pid_speed_xianfu()
 /*---位置式PID算法---*/
 void PID_AL()
 {
-  //~ err_sum += err;   //积分累计误差值
+  err_sum += err;   //积分累计误差值
-  //~ eI_xianfu();   //积分部分限幅
+  eI_xianfu();   //积分部分限幅
   err_diff = err - err_old;   //微分部分误差值
   /*---打印输出PID控制的各项参数1---*/
     //~ cout << "err:" << err << " " ;
@@ -54,20 +55,27 @@ void PID_AL()
   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;
+  //~ if (abs(err_sum) > 100 )
-  Kpid_speed = (Kp*err + Kd*err_diff) * Kpid_base;
+  //~ {
-  
+    Kpid_speed = (Kp*err + Ki*err_sum + Kd*err_diff) * Kpid_base;
+  //~ }
+  //~ else
+  //~ {
+    //~ Kpid_speed = (Kp*err + Kd*err_diff) * Kpid_base;
+  //~ }
   pid_speed_xianfu();
-  speed_r = speed_l + Kpid_speed;
+  //~ speed_r = speed_l - 1+ Kpid_speed;
 
 
   /*---打印输出PID控制的各项参数2---*/
-    cout << "PID_out:" << Kpid_speed << endl ;
+    //~ cout << "PID_out:" << Kpid_speed << endl ;
   /*---打印结束---*/
   // VWSetSpeed(speed,Kpid*Kpid_base);   //控制小车的行驶速度，角速度(模拟器可以用，实物no)
  
-  MOTORDriveRaw(2,speed_r);
+  MOTORDriveRaw(2,speed_r + Kpid_speed);
-  MOTORDriveRaw(1,speed_l);
+  //~ MOTORDriveRaw(2,speed_r);
+  MOTORDriveRaw(1,speed_l - Kpid_speed);
+  //~ MOTORDriveRaw(1,speed_l);
 }
 
 /*---PID控制下的直线行驶---*/
@@ -78,6 +86,10 @@ void PIDStraight()
   int x_1,x_2, y_1,y_2, phi_1,phi_2;    //VWGetPosition的参数定义
   VWGetPosition(&x_1, &y_1, &phi_1);    //获取机器人在移动前的x,y,phi值
   
+  //~ F_PSD = PSDGetRaw(1) - 2;    //读取前方和墙壁的距离
+  //~ L_PSD = PSDGetRaw(2) - 16;   //读取左侧和墙壁的距离
+  //~ R_PSD = PSDGetRaw(3);    //读取右侧和墙壁的距离
+  
   MOTORDriveRaw(2,speed_r);
   MOTORDriveRaw(1,speed_l);
 
@@ -88,38 +100,43 @@ void PIDStraight()
     L_PSD = PSDGetRaw(2) - 16;   //读取左侧和墙壁的距离
     R_PSD = PSDGetRaw(3);    //读取右侧和墙壁的距离
     
+    //~ F_PSD = (PSDGetRaw(1) - 2) * 0.6  +  F_PSD * 0.4;    //读取前方和墙壁的距离
+    //~ L_PSD = (PSDGetRaw(2) - 16) * 0.6  +  L_PSD * 0.4;   //读取左侧和墙壁的距离
+    //~ R_PSD = (PSDGetRaw(3)) * 0.6  +  R_PSD * 0.4;    //读取右侧和墙壁的距离
+
     /*---打印输出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 (20<L_PSD && L_PSD<140  &&  20<R_PSD && R_PSD<140)   //如果两侧都有墙的情况
+    if (20<L_PSD && L_PSD<125  &&  20<R_PSD && R_PSD<125)   //如果两侧都有墙的情况
     {
       err = L_PSD - R_PSD;
       PID_AL();
     }
-    else if (20<L_PSD && L_PSD<140)    //只有左侧有墙的情况
+    else if (20<L_PSD && L_PSD<150)    //只有左侧有墙的情况
     {
       err = L_PSD - DIST_wall_RL;
       PID_AL();
     }
-    else if (20<R_PSD && R_PSD<140)    //只有右侧有墙的情况
+    else if (20<R_PSD && R_PSD<150)    //只有右侧有墙的情况
     {
       err = DIST_wall_RL - R_PSD;
       PID_AL();
     }
     else
     {
-      L_PSD = L_PSD % 300;
+      //~ L_PSD = L_PSD % 200;
-      R_PSD = R_PSD % 300;
+      //~ R_PSD = R_PSD % 200;
-      err = L_PSD - R_PSD;
+      //~ err = L_PSD - R_PSD;
+      err = 0;
+      err_old = 0;
+      err_sum = 0;
       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);    //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
-
+  //~ cout << "move:" << bot_move << endl;
-  MOTORDriveRaw(1,0);
+  //~ MOTORDriveRaw(1,0);
-  MOTORDriveRaw(2,0);
+  //~ MOTORDriveRaw(2,0);
 }

bot_code_turn/maze_func_turn.cpp

@@ -8,71 +8,27 @@ 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(1,20);
-				MOTORDriveRaw(2,-19);
+				MOTORDriveRaw(2,-18);
-			} while (abs(ENCODERRead(2)) < 218);
+			} while (abs(ENCODERRead(2)) < 240);
-		}
 		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(1,-20);
-				MOTORDriveRaw(2,19);
+				MOTORDriveRaw(2,18);
 			} while (abs(ENCODERRead(2)) < 240);
-		}
 		MOTORDriveRaw(1,0);
 		MOTORDriveRaw(2,0);
 	}
@@ -82,10 +38,10 @@ void BOTturn(int turn)
 		ENCODERReset(2);
 		do 
 		{
-			MOTORDriveRaw(1,21);
+			MOTORDriveRaw(1,20);
-			MOTORDriveRaw(2,-19);
+			MOTORDriveRaw(2,-18);
 		
-		} while (abs(ENCODERRead(2)) < 500);
+		} while (abs(ENCODERRead(2)) < 570);
 		MOTORDriveRaw(1,0);
 		MOTORDriveRaw(2,0);
     }
@@ -93,4 +49,10 @@ void BOTturn(int turn)
     {
         cout << "errer" << endl;
     }
+	err_sum = 0;
+	err_old = 0;
+	err = 0;
+	err_diff = 0;
+    
+    
 }

bot_code_turn/maze_main.cpp

@@ -1,6 +1,7 @@
 #include<iostream>
 #include<string>
 #include<vector>
+#include<cstdlib>
 #include"eyebot++.h"
 #include"maze_parameter.h"
 #include"maze_func.h"
@@ -11,7 +12,10 @@ using namespace std;
 int main()
 {
   explore();  //使用递归算法构建地图
-  wall[0][0][0] = 1;
+  BOTturn(2 - rob_dir);
+  VWWait();
+  rob_dir = 2;
+  explore();
 
   const int mazesize_x = mark.size();       //获取探索结束之后的迷宫的X轴长度
   const int mazesize_y = mark[0].size();    //获取探索结束之后的迷宫的Y轴长度
@@ -22,21 +26,31 @@ int main()
   array_copy_mark(mazesize_x, mazesize_y, copy_mark);   //复制一份mark数组，并转换为一维数组，便于后续频繁读取提升性能
   array_copy_wall(mazesize_x, mazesize_y, copy_wall);   //复制一份wall数组，并转换为一维数组，便于后续频繁读取提升性能
 
+  cout << "访问数组打印：" << endl; 
+  cout << endl;
   output_arr2D(mazesize_x, mazesize_y, copy_mark);    //打印地图mark信息
+  cout << "地图打印：" << endl;
+  cout << endl;
   output_arrwall(mazesize_x, mazesize_y, copy_wall);  //打印墙壁wall信息
 
+  cin_targey();   //输入目标点，带超界检测
+
   int map[mazesize_x * mazesize_y] = {};       //创建最短路径求解地图并初始化为0
   array_negative_one(mazesize_x * mazesize_y, map);        //将map数组初始化为-1
   
   flood(map, copy_wall);  //洪水填充算法
+  cout << "路径打印：" << endl;
+  cout << endl;
   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);  //移动到指定目标点
 
+  cout << "移动指令打印：" << endl;
+  cout << endl;
   output_arrpath(len, path);   //打印path数组信息，及小车每一步的行驶方向
 
+  drive_path(len, path);  //移动到指定目标点
 }

bot_code_turn/maze_parameter.cpp

@@ -1,7 +1,6 @@
 #include<iostream>
 #include<string>
 #include<vector>
-#include"eyebot++.h"
 #include"maze_parameter.h"
 // #include"maze_func.h"
 using namespace std;
@@ -9,9 +8,12 @@ using namespace std;
 /*---定义数据---*/
 /*---定义全局常量---*/
 const int DIST_cell = 240;     	//单元格长度
-const int DIST_move = 292;		//移动的单元格距离
+const int DIST_move = 320;		//移动的单元格距离
-const int DIST_wall_F = 90;     //与墙壁的距离
+//~ const int DIST_move = 330;		//移动的单元格距离
-const int DIST_wall_RL = 88;      //与左右墙壁的距离
+const int DIST_wall_F = 100;     //与墙壁的距离
+//~ const int DIST_wall_F = 95;     //与墙壁的距离
+//~ const int DIST_wall_RL = 90;      //与左右墙壁的距离 1
+const int DIST_wall_RL = 87;      //与左右墙壁的距离
 
 /*---定义全局变量---*/
 /*---容器类---*/
@@ -29,21 +31,29 @@ 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 = 2.8, Ki = 0.006, Kd = 0.08;   //定义PD的参数16-2此方案成功过，就是有点摇摇晃晃 墙壁为1
-//~ const float Kp = 0.80, Ki = 0.0076, Kd = 0.2;   //定义PID的参数18
+//~ const float Kp = 0.2, Ki =0, Kd = 1;   //定义PD的参数16-2
+/*--在尝试的方案--*/
+
+const float Kp = 0.22, Ki =0.004, Kd = 1;   //定义PD的参数16-2
+
+
 //~ float Kpid;                             //定义PID计算后输出的倍率
 int Kpid_speed;
 const float Kpid_base = 1;				//定义基本角速度和速度
 const int speed_l = 16;					//定义基本左轮速度
-//~ const int speed_r = speed_l - 2;		//定义基本右轮速度
+const int speed_r = speed_l - 1;		//定义基本右轮速度
-int speed_r = speed_l - 2;		//定义基本右轮速度
+//~ int speed_r = speed_l - 1;		//定义基本右轮速度
 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_x = 3;   //声明目标点的X坐标
-const int target_y = 4;   //声明目标点的Y坐标
+// const int target_y = 4;   //声明目标点的Y坐标
+
+int target_x;   //声明目标点的X坐标
+int target_y;   //声明目标点的Y坐标
 
 /*---定义结束---*/

bot_code_turn/maze_parameter.h

@@ -31,14 +31,17 @@ extern const float Kp, Ki, Kd;                  //声明PID的参数
 extern int Kpid_speed;                          //定义计算后的速度
 extern const float Kpid_base;					//定义速度基础倍数速度
 extern const int speed_l;						//定义左轮基本速度
-//~ extern const int speed_r;						//定义右轮基本速度
+extern const int speed_r;						//定义右轮基本速度
-extern 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_x;    //声明目标点的X坐标
-extern const int target_y;    //声明目标点的Y坐标
+// extern const int target_y;    //声明目标点的Y坐标
+
+extern int target_x;    //声明目标点的X坐标
+extern int target_y;    //声明目标点的Y坐标
 
 /*---声明结束---*/

文件/老师的函数.txt

@@ -0,0 +1,147 @@
+#include "eyebot.h"
+#define SAFE 200
+
+static int v_des1,v_des2;
+
+float Kp=0.25;
+float Ki=0.06;
+float Klink=0.05;
+
+TIMER t1;
+
+void picontroller()
+{
+    int enc_new1,v_act1,r_mot1,e_func1;
+    int enc_new2,v_act2,r_mot2,e_func2;
+    static int enc_old1,enc_old2;
+    static int r_old1=0,e_old1=0;
+    static int r_old2=0,e_old2=0;
+    
+    enc_new1=ENCODERRead(1);
+    enc_new2=ENCODERRead(2);
+    
+    v_act1=enc_new1-enc_old1;
+    v_act2=enc_new2-enc_old2;
+    
+    e_func1=v_des1-v_act1;
+    e_func2=v_des2-v_act2;
+    
+
+    r_mot1=r_old1+Kp*(e_func1-e_old1)+Ki*(e_func1+e_old1)/2+Klink*(abs(enc_new1)-abs(enc_new2));
+    r_mot2=r_old2+Kp*(e_func2-e_old2)+Ki*(e_func2+e_old2)/2-Klink*(abs(enc_new1)-abs(enc_new2));
+    
+    if(v_des1>0&&v_des2>0)
+    {
+        if(r_mot1>35)r_mot1=35;
+        else if(r_mot1<0) r_mot1=0;
+    
+        if(r_mot2>35)r_mot2=35;
+        else if(r_mot2<0) r_mot2=0;
+    }
+    else if(v_des1>0&&v_des2<0)
+    {
+        if(r_mot1>35)r_mot1=35;
+        else if(r_mot1<0) r_mot1=0;
+        if(r_mot2>0)r_mot2=0;
+        else if(r_mot2<-35) r_mot2=-35;
+    }
+    else if(v_des1<0&&v_des2>0)
+    {
+        if(r_mot1>0)r_mot1=0;
+        else if(r_mot1<-35) r_mot1=-35;
+        if(r_mot2>35)r_mot2=35;
+        else if(r_mot2<0) r_mot2=0;
+    }
+    else if(v_des1==0&&v_des2==0)
+    {
+        r_mot1=0;
+        r_mot2=0;
+        v_act1=0;
+        v_act2=0;
+        enc_old1=0;
+        r_old1=0;
+        enc_old2=0;
+        r_old2=0;
+        e_func1=0;
+        e_func2=0;
+        ENCODERReset(1);
+        ENCODERReset(2);
+    }
+    
+    MOTORDrive(1,r_mot1);
+    MOTORDrive(2,r_mot2);    
+    
+    enc_old1=enc_new1;
+    r_old1=r_mot1;
+    e_old1=e_func1;
+    
+    enc_old2=enc_new2;
+    r_old2=r_mot2;
+    e_old2=e_func2;
+    
+    printf("%d    %d    %d     %d\n",r_mot1,r_mot2,enc_old1,enc_old2);
+    
+}
+
+void VWSpeed(int v1,int v2)
+{
+    v_des1=v1;
+    v_des2=v2;
+}
+
+
+void drive()
+{
+   do
+   {
+        if(PSDGetRaw(PSD_LEFT)<SAFE)
+            VWSpeed(100,90);
+        else VWSpeed(90,100);
+        
+        OSWait(80);
+        
+    }while(PSDGetRaw(PSD_FRONT)>SAFE);
+    VWSpeed(0,0);
+    OSWait(1000);
+}
+
+void turn()
+{
+    VWSpeed(50,-50);
+    while(PSDGetRaw(PSD_FRONT)<400) OSWait(80);
+    VWSpeed(0,0);
+    OSWait(1000);
+}
+
+int main() 
+{
+    LCDMenu("STOP","2","3","END");
+    ENCODERReset(1);
+    ENCODERReset(2);
+    
+    PSDGetRaw(PSD_FRONT);
+    PSDGetRaw(PSD_LEFT);
+    
+    
+    t1=OSAttachTimer(100,picontroller);
+
+    while(KEYRead()!=KEY1)
+    { drive();
+      turn();
+    }
+    
+    OSDetachTimer(t1);
+    
+    AUBeep();
+
+    while(KEYRead()!=KEY4)
+    {
+    MOTORDrive(1,0);
+    MOTORDrive(2,0);
+    }
+
+    return(0);
+ }
+ 
+   
+