yyz/bk_bishe_pi
1
0
Fork 0
mirror of https://gitee.com/yyz_o/bk_bishe_pi.git synced 2025-09-07 23:21:26 +00:00
Code Issues Packages Projects Releases Wiki Activity

Compare commits

base: yyz:69f91d3b52b37eaf47b31881d4b79cc71db1f7bd
Branches Tags
yyz:master
..
compare: yyz:a81594bf68d6b963d29b706242b55b68d54f4a81
Branches Tags
yyz:master

2 Commits
69f91d3b52 ... a81594bf68

Author SHA1 Message Date
氧原子
a81594bf68 2025.5.15第三次提交，继续修改 2025-05-15 22:40:18 +08:00
氧原子
75f7336601 1 2025-05-15 20:30:35 +08:00
10 changed files with 307 additions and 55 deletions
Expand all files Collapse all files

2
maze_code/maze_func_array.cpp
View File

@@ -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];
}
}
}

30
maze_code/maze_func_flood.cpp
View File

@@ -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 j = 0; j < size_y; j++)
{
map[(i * size_y) + j] = copy_map[(i * size_y) + j];
}
}
// for (int i = 0; i < size_x; i++)
// {
// for (int j = 0; j < 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));
}

8
maze_code/maze_func_output.cpp
View File

@@ -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 << "_" ;
}

8
maze_code/maze_func_path.cpp
View File

@@ -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;

35
maze_code/maze_func_pid.cpp
View File

@@ -31,21 +31,21 @@ void PID_AL()
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" ;
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控制算法输出的倍率
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的时候降点速度
{
speed1 = speed2;
}
}while (DIST_move < DIST_cell && F_PSD > DIST_wall); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
cout << "move:" << DIST_move << endl;
// if (DIST_move > (DIST_cell -50)) //当移动距离只剩下最后的50mm的时候降点速度
// {
// speed1 = speed2;
// }
}while (DIST_move < DIST_cell && F_PSD > DIST_wall_F); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
MOTORDriveRaw(1,0);
MOTORDriveRaw(2,0);
}

3
maze_code/maze_main.cpp
View File

@@ -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

16
maze_code/maze_parameter.cpp
View File

@@ -9,13 +9,14 @@ using namespace std;
/*---定义数据---*/
/*---定义全局常量---*/
const int DIST_cell = 360 ; //单元格长度
const int DIST_wall = 130 ; //与墙壁的距离
const int DIST_cell = 300 ; //单元格长度
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=为已访问
vector<vector<vector<int>>> wall(2,vector<vector<int>>(2,vector<int>(2,0))); //定义wall容器（三维容器），容器初始为1行1列，且每个格子内带有2个墙壁信息 1=墙壁；0=通路
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坐标
@@ -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;
const int Kpid_base = 2, speed = 24; //定义基本角速度和速度
int Kpid_speed;
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; //定义误差，上一次误差，积分误差，微分误差

8
maze_code/maze_parameter.h
View File

@@ -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 const int Kpid_base, speed; //定义基本角速度和速度
extern int Kpid_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; //定义误差，上一次误差，积分误差，微分误差

97
文件/hdt.txt Normal file
View File

@@ -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

147
文件/新建 文本文档.txt Normal file
View File

@@ -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);
}