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 22:40:18 +08:00
parent 75f7336601
commit a81594bf68
8 changed files with 63 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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