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.15测试

Browse Source
This commit is contained in:
Ñõԭ×Ó
2025-05-15 20:21:34 +08:00
parent 9cd5540936
commit 69f91d3b52
2 changed files with 67 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

99
robot_test_code/PID/PID.cpp
View File

@@ -4,24 +4,24 @@
#include"PID.h" #include"PID.h"
using namespace std; using namespace std;
float Kp = 0.5, Ki = 0.3, Kd = 0.1; //定义PID的参数 float Kp = 0.2, Ki = 0.1, Kd = 0.1; //定义PID的参数
float Kpid; float Kpid;
float Kpid_speed; int Kpid_speed;
const int speed = 16; const int speed = 18;
const float Kpid_base = 0.1; const float Kpid_base = 0.4;
const int speed2 = 0.75*speed; // const int speed2 = 0.75*speed;
int speed1; // int speed1;
//int speed2 = speed; //int speed2 = speed;
/*---定义全局变量---*/ /*---定义全局变量---*/
int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差 int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差
const int eI_max = 30; const int eI_max = 20;
/*---定义全局常量---*/ /*---定义全局常量---*/
/*---定义全局常量---*/ /*---定义全局常量---*/
// const int DIST_cell = 300 ; // const int DIST_cell = 300 ;
// const int DIST_wall = 100 ; // const int DIST_wall = 100 ;
const int DIST_cell = 300; const int DIST_cell = 300;
const int DIST_wall_f = 68; const int DIST_wall_f = 80;
// const int DIST_wall_rl = 88; const int DIST_wall_rl = 87;
/*---全局各种变量与常量定义完成---*/ /*---全局各种变量与常量定义完成---*/
@@ -49,32 +49,33 @@ void PID_AL()
eI_xianfu(eI_max); //积分部分限幅 eI_xianfu(eI_max); //积分部分限幅
err_diff = err - err_old; //微分部分误差值 err_diff = err - err_old; //微分部分误差值
/*---打印输出PID控制的各项参数1---*/ /*---打印输出PID控制的各项参数1---*/
cout << "err:" << err << "\t" ; cout << "err:" << err << " " ;
cout << "err_old:" << err_old << "\t" ; cout << "err_old:" << err_old << " " ;
cout << "err_sum:" << err_sum << "\t" ; cout << "err_sum:" << err_sum << " " ;
cout << "err_diff:" << err_diff << "\t" ; cout << "err_diff:" << err_diff << " " ;
/*---打印结束---*/ /*---打印结束---*/
err_old = err; //将误差幅值到上一次误差 err_old = err; //将误差幅值到上一次误差
Kpid = Kp*err + Ki*err_sum + Kd*err_diff; //PID控制算法输出的倍率 Kpid = Kp*err + Ki*err_sum + Kd*err_diff; //PID控制算法输出的倍率
Kpid_speed = Kpid * Kpid_base; Kpid_speed = Kpid * Kpid_base;
if (Kpid_speed >= 8) //对pid输出值进行限制，防止过大电机过快，或者减的过小使电机不转动 if (Kpid_speed >= 6) //对pid输出值进行限制，防止过大电机过快，或者减的过小使电机不转动
{ {
Kpid_speed = 8; Kpid_speed = 6;
} }
else if (Kpid_speed <= -8) else if (Kpid_speed <= -6)
{ {
Kpid_speed = -8; Kpid_speed = -6;
} }
else else
{ {
//error //error
} }
/*---打印输出PID控制的各项参数2---*/ /*---打印输出PID控制的各项参数2---*/
cout << "PID_out:" << Kpid << endl ; cout << "PID_out:" << Kpid_speed << " " ;
/*---打印结束---*/ /*---打印结束---*/
MOTORDriveRaw(1,speed1);
MOTORDriveRaw(2,speed1 + Kpid_speed); //控制小车的行驶速度，角速度 MOTORDriveRaw(2,speed + Kpid_speed); //控制小车的行驶速度，角速度
MOTORDriveRaw(1,speed);
} }
/*---PID控制下的直线行驶---*/ /*---PID控制下的直线行驶---*/
@@ -82,35 +83,50 @@ void PIDStraight()
{ {
int DIST_move; //定义基础角速度和速度,和移动距离 int DIST_move; //定义基础角速度和速度,和移动距离
int L_PSD, R_PSD, F_PSD; //定义左侧，右侧，前方的距离值 int L_PSD, R_PSD, F_PSD; //定义左侧，右侧，前方的距离值
int x_1,x_2, y_1,y_2, phi_1,phi_2; //VWGetPosition的参数定义 int DIST_move_x1;
speed1 = speed; int DIST_move_x2;
// int L_PSD_old, R_PSD_old, F_PSD_old;
// int x_1,x_2, y_1,y_2, phi_1,phi_2; //VWGetPosition的参数定义
// speed1 = speed;
F_PSD = PSDGetRaw(1);
// F_PSD_old = F_PSD;
// L_PSD = PSDGetRaw(2) - 18;
// L_PSD_old = L_PSD;
// R_PSD = PSDGetRaw(3);
// R_PSD_old = R_PSD;
VWGetPosition(&x_1, &y_1, &phi_1); //获取机器人在移动前的x,y,phi值 // VWGetPosition(&x_1, &y_1, &phi_1); //获取机器人在移动前的x,y,phi值
DIST_move_x1 = F_PSD;
MOTORDriveRaw(1,speed1); MOTORDriveRaw(1,speed);
MOTORDriveRaw(2,speed1); MOTORDriveRaw(2,speed);
do do
{ {
F_PSD = PSDGetRaw(1); //读取前方和墙壁的距离 F_PSD = PSDGetRaw(1); //读取前方和墙壁的距离
L_PSD = PSDGetRaw(2) - 16; //读取左侧和墙壁的距离 // F_PSD = F_PSD * 0.4 + F_PSD_old * 0.6;
// F_PSD_old = F_PSD;
L_PSD = PSDGetRaw(2) - 18; //读取左侧和墙壁的距离
// L_PSD = L_PSD * 0.4 + L_PSD_old * 0.6;
// L_PSD_old = L_PSD;
R_PSD = PSDGetRaw(3); //读取右侧和墙壁的距离 R_PSD = PSDGetRaw(3); //读取右侧和墙壁的距离
// R_PSD = R_PSD * 0.4 + R_PSD_old * 0.6;
// R_PSD_old = R_PSD;
/*---打印输出PSD数值---*/ /*---打印输出PSD数值---*/
cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << "\t" ; cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << " " ;
/*---打印结束---*/ /*---打印结束---*/
if (50<L_PSD && L_PSD<180 && 50<R_PSD && R_PSD<180) //如果两侧都有墙的情况 if (25<L_PSD && L_PSD<180 && 50<R_PSD && R_PSD<180) //如果两侧都有墙的情况
{ {
err = L_PSD - R_PSD; err = L_PSD - R_PSD;
PID_AL(); PID_AL();
} }
else if (50<L_PSD && L_PSD<180) //只有左侧有墙的情况 else if (25<L_PSD && L_PSD<180) //只有左侧有墙的情况
{ {
err = L_PSD - DIST_wall_f; err = L_PSD - DIST_wall_rl;
PID_AL(); PID_AL();
} }
else if (50<R_PSD && R_PSD<180) //只有右侧有墙的情况 else if (25<R_PSD && R_PSD<180) //只有右侧有墙的情况
{ {
err = DIST_wall_f - R_PSD; err = DIST_wall_rl - R_PSD;
PID_AL(); PID_AL();
} }
else else
@@ -120,12 +136,17 @@ void PIDStraight()
err = L_PSD - R_PSD; err = L_PSD - R_PSD;
PID_AL(); PID_AL();
} }
VWGetPosition(&x_2, &y_2, &phi_2); // VWGetPosition(&x_2, &y_2, &phi_2);
DIST_move = sqrt(pow(x_2-x_1,2) + pow(y_2-y_1,2)); // DIST_move = sqrt(pow(x_2-x_1,2) + pow(y_2-y_1,2));
if (DIST_move > (DIST_cell -50)) //当移动距离只剩下最后的50mm的时候降点速度 DIST_move_x2 = F_PSD;
{ DIST_move = DIST_move_x1 - DIST_move_x2;
speed1 = speed2; cout << "move: " << DIST_move << endl;
} // if (DIST_move > (DIST_cell -50)) //当移动距离只剩下最后的50mm的时候降点速度
// {
// speed1 = speed2;
// }
}while (DIST_move < DIST_cell && F_PSD > DIST_wall_f); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行 }while (DIST_move < DIST_cell && F_PSD > DIST_wall_f); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
MOTORDriveRaw(1,0);
MOTORDriveRaw(2,0);
//VWSetSpeed(0,0); //VWSetSpeed(0,0);
} }

11
robot_test_code/PID/PIDmain.cpp
View File

@@ -8,8 +8,8 @@ using namespace std;
// const int DIST_cell = 300 ; // const int DIST_cell = 300 ;
// const int DIST_wall = 100 ; // const int DIST_wall = 100 ;
const int DIST_cell = 300; const int DIST_cell = 300;
const int DIST_wall_f = 68; const int DIST_wall_f = 80;
// const int DIST_wall_rl = 88; const int DIST_wall_rl = 87;
/*---全局各种变量与常量定义完成---*/ /*---全局各种变量与常量定义完成---*/
@@ -28,7 +28,7 @@ int main()
//判断旋转方向 //判断旋转方向
if (Lwall) if (Lwall)
{ {
VWTurn(+90,10); //向左旋转 VWTurn(+90,15); //向左旋转
VWWait(); VWWait();
} }
else if (Fwall) else if (Fwall)
@@ -37,15 +37,16 @@ int main()
} }
else if (Rwall) else if (Rwall)
{ {
VWTurn(-90,10); //向右旋转 VWTurn(-90,15); //向右旋转
VWWait(); VWWait();
} }
else else
{ {
VWTurn(180,10); VWTurn(180,15);
VWWait(); VWWait();
} }
//直走 //直走
PIDStraight(); PIDStraight();
VWWait();
} while (KEYRead() != KEY4); } while (KEYRead() != KEY4);
} }