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

1

Browse Source
This commit is contained in:
Ñõԭ×Ó
2025-05-27 14:57:30 +08:00
parent 00e0d9366d
commit 9d96af2ff3
6 changed files with 45 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
bot_code_turn/maze_func_pid.cpp
View File

@@ -75,6 +75,7 @@ void PID_AL()
MOTORDriveRaw(2,speed_r + Kpid_speed);
//~ MOTORDriveRaw(2,speed_r);
MOTORDriveRaw(1,speed_l - Kpid_speed);
//~ MOTORDriveRaw(1,speed_l);
}
/*---PID控制下的直线行驶---*/
@@ -85,9 +86,9 @@ void PIDStraight()
int x_1,x_2, y_1,y_2, phi_1,phi_2; //VWGetPosition的参数定义
VWGetPosition(&x_1, &y_1, &phi_1); //获取机器人在移动前的x,y,phi值
F_PSD = PSDGetRaw(1) - 2; //读取前方和墙壁的距离
L_PSD = PSDGetRaw(2) - 16; //读取左侧和墙壁的距离
R_PSD = PSDGetRaw(3); //读取右侧和墙壁的距离
//~ F_PSD = PSDGetRaw(1) - 2; //读取前方和墙壁的距离
//~ L_PSD = PSDGetRaw(2) - 16; //读取左侧和墙壁的距离
//~ R_PSD = PSDGetRaw(3); //读取右侧和墙壁的距离
MOTORDriveRaw(2,speed_r);
MOTORDriveRaw(1,speed_l);
@@ -95,13 +96,13 @@ void PIDStraight()
do
{
//~ F_PSD = PSDGetRaw(1) - 2; //读取前方和墙壁的距离
//~ L_PSD = PSDGetRaw(2) - 16; //读取左侧和墙壁的距离
//~ R_PSD = PSDGetRaw(3); //读取右侧和墙壁的距离
F_PSD = PSDGetRaw(1) - 2; //读取前方和墙壁的距离
L_PSD = PSDGetRaw(2) - 16; //读取左侧和墙壁的距离
R_PSD = PSDGetRaw(3); //读取右侧和墙壁的距离
F_PSD = (PSDGetRaw(1) - 2) * 0.6 + F_PSD * 0.4; //读取前方和墙壁的距离
L_PSD = (PSDGetRaw(2) - 16) * 0.6 + L_PSD * 0.4; //读取左侧和墙壁的距离
R_PSD = (PSDGetRaw(3)) * 0.6 + R_PSD * 0.4; //读取右侧和墙壁的距离
//~ F_PSD = (PSDGetRaw(1) - 2) * 0.6 + F_PSD * 0.4; //读取前方和墙壁的距离
//~ L_PSD = (PSDGetRaw(2) - 16) * 0.6 + L_PSD * 0.4; //读取左侧和墙壁的距离
//~ R_PSD = (PSDGetRaw(3)) * 0.6 + R_PSD * 0.4; //读取右侧和墙壁的距离
/*---打印输出PSD数值---*/
//~ cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << "\t" ;
@@ -124,9 +125,12 @@ void PIDStraight()
}
else
{
L_PSD = L_PSD % 100;
R_PSD = R_PSD % 100;
err = L_PSD - R_PSD;
//~ L_PSD = L_PSD % 200;
//~ R_PSD = R_PSD % 200;
//~ err = L_PSD - R_PSD;
err = 0;
err_old = 0;
err_sum = 0;
PID_AL();
}
VWGetPosition(&x_2, &y_2, &phi_2);