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

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"
@@ -117,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;
@@ -141,6 +143,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] = !open;
@@ -165,6 +168,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][0] = !open;
@@ -181,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;

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;
 
@@ -57,5 +56,5 @@ void output_arrpath(int size, int *arr)
     cout << arr[i] << " ";
   }
   cout << endl;
-   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_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"
@@ -32,10 +33,7 @@ int main()
   cout << endl;
   output_arrwall(mazesize_x, mazesize_y, copy_wall);  //打印墙壁wall信息
 
-  cout << "请输入目标点的X,Y坐标值 用空格隔开并回车" << endl;
-  cout << endl;
-  cin >> target_x >> target_y;
-  cout << endl;
+  cin_targey();   //输入目标点，带超界检测
 
   int map[mazesize_x * mazesize_y] = {};       //创建最短路径求解地图并初始化为0
   array_negative_one(mazesize_x * mazesize_y, map);        //将map数组初始化为-1

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;

文件/老师的函数.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);
+ }
+ 
+   
+