2025.5.15第三次提交，继续修改

maze_code/maze_func_array.cpp

@@ -27,7 +27,7 @@ void array_copy_wall(int size_x, int size_y, int *copy_wall)
     {
       for(int k = 0; k < 2; k++)
       {
-        copy_wall[i * size_y * 2 + j * 2 + k] = wall[i][j][k];
+        copy_wall[i * (size_y + 1) * 2 + j * 2 + k] = wall[i][j][k];
       }
     }
   }

maze_code/maze_func_flood.cpp

@@ -25,36 +25,36 @@ void flood(int *map, int *copy_map, int *copy_wall)
         { 
           if (i > 0)
           {
-            if (!copy_wall[(i * size_y * 2) + (j * 2) + 1]  &&  map[((i-1) * size_y) + j] == -1) //左边格子
+            if (!copy_wall[(i * (size_y + 1) * 2) + (j * 2) + 1]  &&  map[((i-1) * size_y) + j] == -1) //左边格子
             {
-              copy_map[((i-1) * size_y) + j] = map[(i * 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 * 2) + (j * 2) + 1]  &&  map[((i+1) * size_y) + j] == -1) //右边格子
+            if (!copy_wall[((i+1) * (size_y + 1) * 2) + (j * 2) + 1]  &&  map[((i+1) * size_y) + j] == -1) //右边格子
             {
-              copy_map[((i+1) * size_y) + j] = map[(i * 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 * 2) + (j * 2) + 0]  &&  map[(i *size_y) + (j-1)] == -1)
+            if (!copy_wall[(i * (size_y + 1) * 2) + (j * 2) + 0]  &&  map[(i *size_y) + (j-1)] == -1)
             {
-              copy_map[(i * size_y) + (j-1)] = map[(i * size_y) + j] + 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 * 2) + ((j+1) * 2) + 0]  &&  map[(i * size_y) + (j+1)] == -1)
+            if (!copy_wall[(i * (size_y + 1) * 2) + ((j+1) * 2) + 0]  &&  map[(i * size_y) + (j+1)] == -1)
             {
-              copy_map[(i * size_y) + (j+1)] = map[(i * size_y) + j] + 1;
+              map[(i * size_y) + (j+1)] = map[(i * size_y) + j] + 1;
               num++;
             }
           }
@@ -62,12 +62,12 @@ void flood(int *map, int *copy_map, int *copy_wall)
       }
     }
 
-    for (int i = 0; i < size_x; i++)
+    // for (int i = 0; i < size_x; i++)
-    {
+    // {
-      for (int j = 0; j < size_y; j++)
+    //   for (int j = 0; j < size_y; j++)
-      {
+    //   {
-        map[(i * size_y) + j] = copy_map[(i * size_y) + j];
+    //     map[(i * size_y) + j] = copy_map[(i * size_y) + j];
-      }
+    //   }
-    }
+    // }
   } while (map[(target_x * size_y) + target_y] == -1  &&  num < (size_x * size_y));
 }

maze_code/maze_func_output.cpp

@@ -26,11 +26,11 @@ void output_arr2D(int size_x, int size_y, int *arr)
 void output_arrwall(int size_x, int size_y, int *arr)
 {
 
-  for (int i = size_y - 1; i >= 0; i--)
+  for (int i = size_y; i >= 0; i--)
   {
-    for (int j = 0; j < size_x; j++)
+    for (int j = 0; j <= size_x; j++)
     {
-      if (arr[(j * size_y * 2)  +  (i * 2)  +  1] == 1)
+      if (arr[(j * (size_y + 1) * 2)  +  (i * 2)  +  1] == 1)
       {
         cout << "|" ;
       }
@@ -39,7 +39,7 @@ void output_arrwall(int size_x, int size_y, int *arr)
         cout << " " ;
       }
       
-      if (arr[(j * size_y * 2)  +  (i * 2)  +  0] == 1)
+      if (arr[(j * (size_y + 1) * 2)  +  (i * 2)  +  0] == 1)
       {
         cout << "_" ;
       }

maze_code/maze_func_path.cpp

@@ -15,22 +15,22 @@ void build_path(int i, int j, int len, int *path, int *map, int *copy_wall)
 
     for (int k = len -1; k >= 0; k--)
     {
-        if (i > 0  &&  !copy_wall[(i * size_y * 2)  +  (j * 2)  +  1]  &&  map[((i-1) * size_y)  +  j] == 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 * 2)  +  (j * 2)  +  1]  &&  map[((i+1) * size_y)  +  j] == k)   //前一个单位的西面是当前格，即当前格子的右侧为上一个格子
+        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 * 2)  +  (j * 2) +  0]  &&  map[(i * size_y)  +  (j-1)] == k)   //前一个单位的北面是当前格，即当前格子的下侧为上一个格子
+        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 * 2)  +  ((j+1) * 2)  +  0]  &&  map[(i * size_y)  +  (j+1)] == k)   //前一个单位的南面是当前格，即当前格子的上侧为上一个格子
+        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;

maze_code/maze_func_pid.cpp

@@ -31,21 +31,21 @@ void PID_AL()
   eI_xianfu(eI_max);   //积分部分限幅
   err_diff = err - err_old;   //微分部分误差值
   /*---打印输出PID控制的各项参数1---*/
-    // cout << "本次误差:" << err << "\t" ;
+    cout << "本次误差:" << err << "\t" ;
-    // cout << "上次误差:" << err_old << "\t" ;
+    cout << "上次误差:" << err_old << "\t" ;
-    // cout << "误差累计:" << err_sum << "\t" ;
+    cout << "误差累计:" << err_sum << "\t" ;
-    // cout << "误差变化:" << err_diff << "\t" ;
+    cout << "误差变化:" << err_diff << "\t" ;
   /*---打印结束---*/
   err_old = err;    //将误差幅值到上一次误差
   
   Kpid = Kp*err + Ki*err_sum + Kd*err_diff;   //PID控制算法输出的倍率
   Kpid_speed = Kpid * Kpid_base;
   /*---打印输出PID控制的各项参数2---*/
-    // cout << "PID输出值:" << Kpid << endl ;
+    cout << "PID输出值:" << Kpid_speed << "\t" ;
   /*---打印结束---*/
   // VWSetSpeed(speed,Kpid*Kpid_base);   //控制小车的行驶速度，角速度(模拟器可以用，实物no)
   MOTORDriveRaw(1,speed1);
-  MOTORDriveRaw(2,speed1+Kpid_speed);
+  MOTORDriveRaw(2,speed1 + Kpid_speed);
 }
 
 /*---PID控制下的直线行驶---*/
@@ -70,19 +70,19 @@ void PIDStraight()
       // 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)   //如果两侧都有墙的情况
+    if (30<L_PSD && L_PSD<180  &&  30<R_PSD && R_PSD<180)   //如果两侧都有墙的情况
     {
       err = L_PSD - R_PSD;
       PID_AL();
     }
-    else if (100<L_PSD && L_PSD<180)    //只有左侧有墙的情况
+    else if (30<L_PSD && L_PSD<180)    //只有左侧有墙的情况
     {
-      err = L_PSD - DIST_wall;
+      err = L_PSD - DIST_wall_RL;
       PID_AL();
     }
-    else if (100<R_PSD && R_PSD<180)    //只有右侧有墙的情况
+    else if (30<R_PSD && R_PSD<180)    //只有右侧有墙的情况
     {
-      err = DIST_wall - R_PSD;
+      err = DIST_wall_RL - R_PSD;
       PID_AL();
     }
     else
@@ -94,9 +94,12 @@ void PIDStraight()
     }
     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的时候降点速度
+    cout << "move:" << DIST_move << endl;
-    {
+    // if (DIST_move > (DIST_cell -50))    //当移动距离只剩下最后的50mm的时候降点速度
-      speed1 = speed2;
+    // {
-    }
+    //   speed1 = speed2;
-  }while (DIST_move < DIST_cell && F_PSD > DIST_wall);    //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
+    // }
+  }while (DIST_move < DIST_cell && F_PSD > DIST_wall_F);    //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
+  MOTORDriveRaw(1,0);
+  MOTORDriveRaw(2,0);
 }

maze_code/maze_main.cpp

@@ -12,6 +12,7 @@ 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轴长度
@@ -23,7 +24,7 @@ int main()
   array_copy_wall(mazesize_x, mazesize_y, copy_wall);   //复制一份wall数组，并转换为一维数组，便于后续频繁读取提升性能
 
   output_arr2D(mazesize_x, mazesize_y, copy_mark);    //打印地图mark信息
-  output_arrwall(mazesize_x + 1, mazesize_y + 1, copy_wall);  //打印墙壁wall信息
+  output_arrwall(mazesize_x, mazesize_y, copy_wall);  //打印墙壁wall信息
 
   int map[mazesize_x * mazesize_y] = {};       //创建最短路径求解地图并初始化为0
   int copy_map[mazesize_x * mazesize_y] = {};  //创建copy复制地图并初始化为0

maze_code/maze_parameter.cpp

@@ -9,13 +9,14 @@ using namespace std;
 
 /*---定义数据---*/
 /*---定义全局常量---*/
-const int DIST_cell = 360 ;     //单元格长度
+const int DIST_cell = 300 ;     //单元格长度
-const int DIST_wall = 130 ;     //与墙壁的距离
+const int DIST_wall_F = 90 ;     //与墙壁的距离
+extern const int DIST_wall_RL = 100;      //与左右墙壁的距离
 
 /*---定义全局变量---*/
 /*---容器类---*/
-vector<vector<int>> mark(1,vector<int>(1,0));                                   //定义mark容器（二维容器），容器初始为1行1列    1=为已访问
+std::vector<std::vector<int>> mark(1,std::vector<int>(1,0));                                   //定义mark容器（二维容器），容器初始为1行1列    1=为已访问
-vector<vector<vector<int>>> wall(2,vector<vector<int>>(2,vector<int>(2,0)));    //定义wall容器（三维容器），容器初始为1行1列，且每个格子内带有2个墙壁信息   1=墙壁；0=通路
+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坐标
@@ -28,10 +29,11 @@ int rob_dir = 0;    //机器人当前的朝向（初始为0）
 
 
 /*---PID算法---*/
-const float Kp = 1, Ki = 0, Kd = 0.5;   //定义PID的参数
+const float Kp = 0.5, Ki = 0.2, Kd = 0.5;   //定义PID的参数
 float Kpid;                             //定义PID计算后输出的倍率
-float Kpid_speed;
+int Kpid_speed;
-const int Kpid_base = 2, speed = 24;    //定义基本角速度和速度
+const float Kpid_base = 0.1;            //定义基本角速度和速度
+const int speed = 50;                   //定义基本角速度和速度
 const int speed2 = 0.75*speed;          //快到目的地时减速
 int speed1;
 int err, err_old, err_sum, err_diff;    //定义误差，上一次误差，积分误差，微分误差

maze_code/maze_parameter.h

@@ -4,7 +4,8 @@
 /*---声明数据---*/
 /*---声明全局常量---*/
 extern const int DIST_cell;         //单元格长度
-extern const int DIST_wall;         //与墙壁的距离
+extern const int DIST_wall_F;       //与前面墙壁的距离
+extern const int DIST_wall_RL;      //与左右墙壁的距离
 
 /*---声明全局变量---*/
 
@@ -26,8 +27,9 @@ extern int rob_dir; //机器人当前的朝向
 /*---PID算法---*/
 extern const float Kp, Ki, Kd;                  //声明PID的参数
 extern float Kpid;                              //定义PID计算后输出的倍率
-extern float Kpid_speed;                        //定义计算后的速度
+extern int Kpid_speed;                          //定义计算后的速度
-extern const int Kpid_base, speed;              //定义基本角速度和速度
+extern const float Kpid_base;                   //定义速度基础倍数速度
+extern const int speed;                         //定义速度基础倍数速度
 extern const int speed2;                        //快到目的地时减速
 extern int speed1;
 extern int err, err_old, err_sum, err_diff;     //定义误差，上一次误差，积分误差，微分误差

文件/hdt.txt

@@ -0,0 +1,97 @@
+# EYEBOT Name
+EYEBOT IBot-D
+
+# MOTOR Number | TableName
+MOTOR 1 Motor_Table
+MOTOR 2 Motor_Table
+
+# Servo Number | Low | High |TableName
+SERVO 1 0 255 Servo_Table
+SERVO 2 0 255 Servo_Table
+SERVO 3 0 255 Servo_Table
+SERVO 4 0 255 Servo_Table
+
+# ENCODER Number | Clicks per meter
+ENCODER 1 6900
+ENCODER 2 6900
+
+# PSD NUMER | TableName
+PSD 1 PSD_TableA
+PSD 2 PSD_TableA
+PSD 3 PSD_TableA
+PSD 4 PSD_TableA
+
+# WIFI Default_Hotspot(0)/Slave(1)/Custom_Hotspot(2)
+WIFI 0
+
+# HOTSPOT Network_Name | Password 
+# HOTSPOT rob rasp
+
+# SLAVE Network_Name | Password 
+SLAVE SSID PASS
+
+# RoBIOS On(1)/Off(0) | FontSize
+ROBIOS 1 10
+
+# USB Number | DeviceName
+USB 1 EyeBot
+USB 2 GPS
+
+# DISPLAY LCD(0)/HDMI(1) | Fullscreen Off/On (0/1) | Rotation (0/1) | Autorefresh (0/1)
+# Note: Change of rotation requires 2 reboots
+LCD 0 0 0 1
+
+# RPI RaspPi_Ver(1/2/3/4)
+RPI 4
+
+# DRIVE Wheel distance (90 or 140mm) | Max Motor Speed | Motor1/left dir. (0 = c/w) | Motor2/right dir. (1=clockwise)
+DRIVE 110 200 1 2
+
+#DEMOPATH | Path
+DEMOPATH /home/pi/eyebot/demo
+
+#SOFTWAREPATH | Path
+SOFTWAREPATH /home/pi/usr
+
+# VOMEGA | Kp | Ki | Kd | Klink (Note if 0,0,0,0 will turn off)
+# ... PID frequency, Dead-Rockoning min. change for update, Debug-Flag
+VOMEGA 400 40 10 50  10 20 0
+
+# -------------------- TABLES (Optional) ------------------
+# Motor Linearisation Table 101 values: 0 .. 100
+TABLE Motor_Table
+  0  1  2  3  4  5  6  7  8  9 10 11 12 13 14 15 16 17 18 19
+ 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39
+ 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59
+ 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79
+ 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99
+100
+END TABLE
+
+# Servo Linearisation Table 256 values: 0 .. 255
+TABLE Servo_Table
+  0  1  2  3  4  5  6  7  8  9 10 11 12 13 14 15 16 17 18 19
+ 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39
+ 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59
+ 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79
+ 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99
+100 101 102 103 104 105 106 107 108 109 110 111	112 113 114 115 116 117 118 119
+120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139
+140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159
+160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179
+180 181 182 183 184 185 186 187 188 189 190 191	192 193 194 195 196 197 198 199
+200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219
+220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239
+240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255
+END TABLE
+
+# PSD Sensor Linearisation Table 128 values in [mm]: 
+TABLE PSD_TableA
+800 800 800 800 800 800 800 800
+800 800 800 800 800 800 800 800 800 800 800 770 750 720 700 680 650 610 590 560
+540 530 480 470 460 450 440 430 420 410 400 390 380 370 360 350 340 330 330 320
+300 290 280 280 280 270 270 260 250 250 250 240 240 230 230 220 220 210 210 210
+200 200 190 190 190 190 180 180 170 170 170 160 160 160 160 150 150 150 150 150
+140 140 140 140 140 130 130 130 130 130 130 120 120 120 120 110 110 110 110 110
+110 110 110 110 110 110 110 110 110 100 100 100 100 100 100 100  90  90  90  90
+END TABLE

文件/新建 文本文档.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);
+ }
+ 
+   
+