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

2025.5.18第一次提交，包括17号和今天上午对实机的调试

Browse Source
This commit is contained in:
Ñõԭ×Ó
2025-05-18 11:35:02 +08:00
parent 47a4473330
commit df42d6ce7f
14 changed files with 232 additions and 103 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
bot_code/makefile
View File

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

4
bot_code/maze_func.h
View File

@@ -12,8 +12,8 @@ void explore(); //递归函数
/*---以下为pid控制和行驶部分涉及到的函数声明---*/
void xneighbor(int x, int dir); //X坐标更新
void yneighbor(int y, int dir); //Y坐标更新
void eI_xianfu(int eI_max); //对PID控制的积分部分I限幅
void pid_speed_xianfu(int Kpid_speed); //对PID控制的输出部分限幅
void eI_xianfu(); //对PID控制的积分部分I限幅
void pid_speed_xianfu(); //对PID控制的输出部分限幅
void PID_AL(); //位置式PID算法
void PIDStraight(); //PID控制下的直线行驶
void go_to(int dir); //行驶到指定单元格

9
bot_code/maze_func_goto.cpp
View File

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

67
bot_code/maze_func_pid.cpp
View File

@@ -7,7 +7,7 @@ using namespace std;
/*---对PID控制的积分部分I限幅---*/
void eI_xianfu(int eI_max)
void eI_xianfu()
{
if (err_sum >= eI_max) //如果err_sum大于限幅，则直接让err_sum=限幅，防止err_sum过大，影响PID控制
{
@@ -23,13 +23,13 @@ void eI_xianfu(int eI_max)
}
}
void pid_speed_xianfu(int Kpid_speed)
void pid_speed_xianfu()
{
if (Kpid_speed > speed_xianfu)
if (Kpid_speed >= speed_xianfu)
{
Kpid_speed = speed_xianfu;
}
else if (Kpid_speed < -speed_xianfu)
else if (Kpid_speed <= -speed_xianfu)
{
Kpid_speed = -speed_xianfu;
}
@@ -42,81 +42,84 @@ void pid_speed_xianfu(int Kpid_speed)
/*---位置式PID算法---*/
void PID_AL()
{
err_sum += err; //积分累计误差值
eI_xianfu(eI_max); //积分部分限幅
//~ err_sum += err; //积分累计误差值
//~ eI_xianfu(); //积分部分限幅
err_diff = err - err_old; //微分部分误差值
/*---打印输出PID控制的各项参数1---*/
cout << "本次误差:" << err << "\t" ;
cout << "上次误差:" << err_old << "\t" ;
cout << "误差累计:" << err_sum << "\t" ;
cout << "误差变化:" << err_diff << "\t" ;
//~ cout << "err:" << err << " " ;
//~ cout << "old_err:" << err_old << " " ;
//~ cout << "err_sum:" << err_sum << " " ;
//~ cout << "err_diff:" << err_diff << " " ;
/*---打印结束---*/
err_old = err; //将误差幅值到上一次误差
Kpid = Kp*err + Ki*err_sum + Kd*err_diff; //PID控制算法输出的倍率
Kpid_speed = Kpid * Kpid_base;
//~ 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 + Kd*err_diff) * Kpid_base;
pid_speed_xianfu();
speed_r = speed_l + Kpid_speed;
pid_speed_xianfu(Kpid_speed);
/*---打印输出PID控制的各项参数2---*/
cout << "PID输出值:" << Kpid_speed << "\t" ;
cout << "PID_out:" << Kpid_speed << endl ;
/*---打印结束---*/
// VWSetSpeed(speed,Kpid*Kpid_base); //控制小车的行驶速度，角速度(模拟器可以用，实物no)
MOTORDriveRaw(1,speed1);
MOTORDriveRaw(2,speed2 + Kpid_speed);
MOTORDriveRaw(2,speed_r);
MOTORDriveRaw(1,speed_l);
}
/*---PID控制下的直线行驶---*/
void PIDStraight()
{
int DIST_move; //定义基础角速度和速度,和移动距离
int bot_move; //定义基础角速度和速度,和移动距离
int L_PSD, R_PSD, F_PSD; //定义左侧，右侧，前方的距离值
int x_1,x_2, y_1,y_2, phi_1,phi_2; //VWGetPosition的参数定义
speed1 = speed;
VWGetPosition(&x_1, &y_1, &phi_1); //获取机器人在移动前的x,y,phi值
MOTORDriveRaw(1,speed1);
MOTORDriveRaw(2,speed2);
MOTORDriveRaw(2,speed_r);
MOTORDriveRaw(1,speed_l);
do
{
F_PSD = PSDGetRaw(1) - 5; //读取前方和墙壁的距离
L_PSD = PSDGetRaw(2) - 8; //读取左侧和墙壁的距离
F_PSD = PSDGetRaw(1) - 2; //读取前方和墙壁的距离
L_PSD = PSDGetRaw(2) - 16; //读取左侧和墙壁的距离
R_PSD = PSDGetRaw(3); //读取右侧和墙壁的距离
/*---打印输出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 (30<L_PSD && L_PSD<180 && 30<R_PSD && R_PSD<180) //如果两侧都有墙的情况
if (20<L_PSD && L_PSD<140 && 20<R_PSD && R_PSD<140) //如果两侧都有墙的情况
{
err = L_PSD - R_PSD;
PID_AL();
}
else if (30<L_PSD && L_PSD<180) //只有左侧有墙的情况
else if (20<L_PSD && L_PSD<140) //只有左侧有墙的情况
{
err = L_PSD - DIST_wall_RL;
PID_AL();
}
else if (30<R_PSD && R_PSD<180) //只有右侧有墙的情况
else if (20<R_PSD && R_PSD<140) //只有右侧有墙的情况
{
err = DIST_wall_RL - R_PSD;
PID_AL();
}
else
{
L_PSD = L_PSD % DIST_cell;
R_PSD = R_PSD % DIST_cell;
L_PSD = L_PSD % 300;
R_PSD = R_PSD % 300;
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));
cout << "move:" << DIST_move << endl;
bot_move = sqrt(pow(x_2-x_1,2) + pow(y_2-y_1,2));
//~ cout << "move:" << bot_move << endl;
}while (DIST_move < DIST_cell && F_PSD > DIST_wall_F); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
}while (bot_move < DIST_move && F_PSD > DIST_wall_F); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
MOTORDriveRaw(1,0);
MOTORDriveRaw(2,0);
}

61
bot_code/maze_func_turn.cpp
View File

@@ -8,38 +8,49 @@ using namespace std;
void BOTturn(int turn)
{
if (turn == 1)
if (turn == -1) //右转
{
MOTORDriveRaw(1,24);
MOTORDriveRaw(2,-22);
OSWait(1985);
MOTORDriveRaw(1,0);
MOTORDriveRaw(2,0);
ENCODERReset(1);
ENCODERReset(2);
do
{
MOTORDriveRaw(1,21);
MOTORDriveRaw(2,-19);
} while (abs(ENCODERRead(2)) < 218);
MOTORDriveRaw(1,0);
MOTORDriveRaw(2,0);
}
else if (turn == -1)
else if (turn == 1) //左转
{
MOTORDriveRaw(1,-24);
MOTORDriveRaw(2,22);
ENCODERReset(1);
ENCODERReset(2);
do
{
MOTORDriveRaw(1,-21);
MOTORDriveRaw(2,19);
OSWait(1985);
MOTORDriveRaw(1,0);
MOTORDriveRaw(2,0);
}
else if (turn == 2)
} while (abs(ENCODERRead(2)) < 240);
MOTORDriveRaw(1,0);
MOTORDriveRaw(2,0);
}
else if (turn == 2) //掉头
{
MOTORDriveRaw(1,24);
MOTORDriveRaw(2,-22);
OSWait(2985);
MOTORDriveRaw(1,0);
MOTORDriveRaw(2,0);
ENCODERReset(1);
ENCODERReset(2);
do
{
MOTORDriveRaw(1,21);
MOTORDriveRaw(2,-19);
} while (abs(ENCODERRead(2)) < 500);
MOTORDriveRaw(1,0);
MOTORDriveRaw(2,0);
}
else
{
cout << "errer" << endl;
}
}
}

29
bot_code/maze_parameter.cpp
View File

@@ -8,9 +8,10 @@ using namespace std;
/*---定义数据---*/
/*---定义全局常量---*/
const int DIST_cell = 300 ; //单元格长度
const int DIST_wall_F = 90 ; //与墙壁的距离
extern const int DIST_wall_RL = 100; //与左右墙壁的距离
const int DIST_cell = 240; //单元格长度
const int DIST_move = 292; //移动的单元格距离
const int DIST_wall_F = 90; //与墙壁的距离
const int DIST_wall_RL = 88; //与左右墙壁的距离
/*---定义全局变量---*/
/*---容器类---*/
@@ -28,19 +29,21 @@ int rob_dir = 0; //机器人当前的朝向（初始为0）
/*---PID算法---*/
const float Kp = 0.8, Ki = 0.015, Kd = 0.15; //定义PID的参数
float Kpid; //定义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 = 0.80, Ki = 0.0076, Kd = 0.2; //定义PID的参数18
//~ float Kpid; //定义PID计算后输出的倍率
int Kpid_speed;
const float Kpid_base = 1; //定义基本角速度和速度
const int speed = 18; //定义基本角速度和速度
const int speed2 = 0.75*speed; //快到目的地时减速
const int speed_xianfu = 3; //定义kpid限幅
int speed1;
int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差
const int eI_max = 200; //定义积分限幅
const float Kpid_base = 1; //定义基本角速度和速度
const int speed_l = 16; //定义基本左轮速度
//~ const int speed_r = speed_l - 2; //定义基本右轮速度
int speed_r = speed_l - 2; //定义基本右轮速度
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_y = 4; //声明目标点的Y坐标
/*---定义结束---*/
/*---定义结束---*/

15
bot_code/maze_parameter.h
View File

@@ -4,7 +4,8 @@
/*---声明数据---*/
/*---声明全局常量---*/
extern const int DIST_cell; //单元格长度
extern const int DIST_wall_F; //与前面墙壁的距离
extern const int DIST_move; //移动的单元格距离
extern const int DIST_wall_F; //与前面墙壁的距离
extern const int DIST_wall_RL; //与左右墙壁的距离
/*---声明全局变量---*/
@@ -26,13 +27,13 @@ extern int rob_dir; //机器人当前的朝向
/*---PID算法---*/
extern const float Kp, Ki, Kd; //声明PID的参数
extern float Kpid; //定义PID计算后输出的倍率
//~ extern float Kpid; //定义PID计算后输出的倍率
extern int Kpid_speed; //定义计算后的速度
extern const float Kpid_base; //定义速度基础倍数速度
extern const int speed; //定义速度基础倍数速度
extern const int speed2; //快到目的地时减速
extern const float Kpid_base; //定义速度基础倍数速度
extern const int speed_l; //定义左轮基本速度
//~ extern const int speed_r; //定义右轮基本速度
extern int speed_r; //定义右轮基本速度
extern const int speed_xianfu; //kpid输出速度的限幅
extern int speed1;
extern int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差
extern const int eI_max; //定义积分限幅
@@ -40,4 +41,4 @@ extern const int eI_max; //定义积分限幅
extern const int target_x; //声明目标点的X坐标
extern const int target_y; //声明目标点的Y坐标
/*---声明结束---*/
/*---声明结束---*/

13
robot_test_code/PID/PID.cpp
View File

@@ -4,26 +4,21 @@
#include"PID.h"
using namespace std;
float Kp = 0.79, Ki = 0.009, Kd = 0.14; //定义PID的参数
float Kp = 0.815, Ki = 0.0083, Kd = 0.167; //定义PID的参数
float Kpid;
int Kpid_speed;
const int speed_l = 17;
const int speed_l = 18;
const int speed_r = speed_l - 2;
const int speed_err = 2;
const float Kpid_base = 1;
// const int speed2 = 0.75*speed;
// int speed1;
//int speed2 = speed;
/*---定义全局变量---*/
int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差
const int eI_max = 200;
/*---定义全局常量---*/
/*---定义全局常量---*/
// const int DIST_cell = 300 ;
// const int DIST_wall = 100 ;
const int DIST_cell = 310;
const int DIST_wall_f = 120;
const int DIST_wall_f = 75;
const int DIST_wall_rl = 90;
/*---全局各种变量与常量定义完成---*/
@@ -126,7 +121,7 @@ void PIDStraight()
/*---打印输出PSD数值---*/
cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << " " ;
/*---打印结束---*/
if (20<L_PSD && L_PSD<180 && 50<R_PSD && R_PSD<180) //如果两侧都有墙的情况
if (20<L_PSD && L_PSD<180 && 20<R_PSD && R_PSD<180) //如果两侧都有墙的情况
{
err = L_PSD - R_PSD;
PID_AL();

4
robot_test_code/PID/PIDmain.cpp
View File

@@ -7,8 +7,8 @@ using namespace std;
/*---定义全局常量---*/
// const int DIST_cell = 300 ;
// const int DIST_wall = 100 ;
const int DIST_cell = 300;
const int DIST_wall_f = 120;
const int DIST_cell = 310;
const int DIST_wall_f = 75;
const int DIST_wall_rl = 90;
/*---全局各种变量与常量定义完成---*/

10
robot_test_code/实机测试/makefile
View File

@@ -1,14 +1,16 @@
cxx := gccarm
#~ cxx := gpparm
#~ file_cpp := $(wildcard *.cpp)
file_cpp := ./test_turn.cpp
#~ file_cpp := ./test_turn.cpp
#~ file_cpp := ./test_turn2.cpp
#~ file_cpp := ./test_run.cpp
#~ file_cpp := ./test_psd.cpp
file_cpp := ./test_psd.cpp
#~ file_o := /home/pi/usr/move.x
file_o := /home/pi/usr/turn.x
#~ file_o := /home/pi/usr/psd.x
#~ file_o := /home/pi/usr/turn.x
file_o := /home/pi/usr/psd.x
$(file_o) : $(file_cpp)
@$(cxx) $^ -o $@

4
robot_test_code/实机测试/test_psd.cpp
View File

@@ -11,8 +11,8 @@ int main()
LCDMenu("","","","END");
do
{
Fwall = PSDGetRaw(1); //检测前面侧是否有墙
Lwall = PSDGetRaw(2); //检测左面
Fwall = PSDGetRaw(1)-2; //检测前面侧是否有墙
Lwall = PSDGetRaw(2)-16; //检测左面
Rwall = PSDGetRaw(3); //检测右面
bwall = PSDGetRaw(4); //
cout << "PSD L:" << Lwall << " R:" << Rwall << " F:" << Fwall << " b:" << bwall << endl; ;

34
robot_test_code/实机测试/test_turn.cpp
View File

@@ -7,10 +7,8 @@ using namespace std;
int main()
{
//~ int f_wall, r_wall, l_wall, b_wall;
//~ int old_f_wall, old_r_wall, old_l_wall, old_b_wall;
int b_wall;
int old_b_wall;
int f_wall, b_wall;
int old_f_wall, old_b_wall;
int turn_check;
LCDMenu("","","","END");
int num;
@@ -65,6 +63,34 @@ int main()
OSWait(50);
} while (turn_check < -3 || b_wall > 130);
}
else if (num == 2)
{
b_wall = PSDGetRaw(4) - 2;
f_wall = PSDGetRaw(1) - 2;
do
{
old_b_wall = b_wall;
old_f_wall = f_wall;
MOTORDriveRaw(1,19);
MOTORDriveRaw(2,-17);
//~ MOTORDriveRaw(1,24);
//~ MOTORDriveRaw(2,-22);
OSWait(1000);
//~ OSWait(1980);
MOTORDriveRaw(1,0);
MOTORDriveRaw(2,0);
b_wall = (PSDGetRaw(4) - 2) * 0.7 + old_b_wall * 0.3; //减小突变
f_wall = (PSDGetRaw(1) - 2) * 0.7 + old_f_wall * 0.3; //减小突变
//~ b_wall = PSDGetRaw(4) - 2;
//~ f_wall = PSDGetRaw(1) - 2;
turn_check = b_wall - old_b_wall;
/*---打印输出PSD数值---*/
//~ cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << " " ;
cout << "f_wall: " << f_wall << "\t" << "b_wall: " << b_wall << "\t" << "check: " << turn_check << endl;
/*---打印结束---*/
OSWait(50);
} while (f_wall < 200 || turn_check < -4 || b_wall > 130);
}
//~ if(num == 1)
//~ {
//~ MOTORDriveRaw(1,19);

79
robot_test_code/实机测试/test_turn2.cpp Normal file
View File

@@ -0,0 +1,79 @@
#include<iostream>
#include<string>
#include<cmath>
//~ #include"eyebot++.h"
#include"/home/pi/eyebot/include/eyebot++.h"
using namespace std;
int main()
{
LCDMenu("","","","END");
int num;
//~ int pwm1, pwm2;
do
{
cin>>num;
if (num == 1)
{
ENCODERReset(1);
ENCODERReset(2);
do
{
MOTORDriveRaw(1,21);
MOTORDriveRaw(2,-19);
} while (abs(ENCODERRead(2)) < 217);
MOTORDriveRaw(1,0);
MOTORDriveRaw(2,0);
}
else if (num == 2)
{
ENCODERReset(1);
ENCODERReset(2);
do
{
MOTORDriveRaw(1,21);
MOTORDriveRaw(2,-19);
} while (abs(ENCODERRead(2)) < 500);
MOTORDriveRaw(1,0);
MOTORDriveRaw(2,0);
}
else if (num == 3)
{
ENCODERReset(1);
ENCODERReset(2);
do
{
MOTORDriveRaw(1,-21);
MOTORDriveRaw(2,19);
} while (abs(ENCODERRead(2)) < 242);
MOTORDriveRaw(1,0);
MOTORDriveRaw(2,0);
}
else if (num == 4)
{
ENCODERReset(1);
ENCODERReset(2);
do
{
MOTORDriveRaw(1,-21);
} while (abs(ENCODERRead(1)) < 285);
MOTORDriveRaw(1,0);
do
{
MOTORDriveRaw(2,19);
} while (abs(ENCODERRead(2)) < 285);
MOTORDriveRaw(2,0);
}
else
{
cout << " num error " << endl;
}
} while (KEYRead() != KEY4);
}

2
文件/eyebot函数.cpp
View File

@@ -6,3 +6,5 @@ VWSetSpeed();
VWGetPosition();
VWSetPosition();
VWWait();
ENCODERReset();
ENCODERRead();