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晚上提交，实机调试后的代码，有一次成功了，但是后面没复现了，很奇怪

Browse Source
This commit is contained in:
Ñõԭ×Ó
2025-05-18 22:47:24 +08:00
parent 1155ae6f6d
commit 00e0d9366d
13 changed files with 129 additions and 124 deletions
Download Patch File Download Diff File Expand all files Collapse all files

61
bot_code_turn/maze_func_pid.cpp
View File

@@ -1,6 +1,7 @@
#include<iostream>
#include<string>
#include"eyebot++.h"
#include<cmath>
#include"maze_parameter.h"
#include"maze_func.h"
using namespace std;
@@ -42,8 +43,8 @@ void pid_speed_xianfu()
/*---位置式PID算法---*/
void PID_AL()
{
//~ err_sum += err; //积分累计误差值
//~ eI_xianfu(); //积分部分限幅
err_sum += err; //积分累计误差值
eI_xianfu(); //积分部分限幅
err_diff = err - err_old; //微分部分误差值
/*---打印输出PID控制的各项参数1---*/
//~ cout << "err:" << err << " " ;
@@ -54,72 +55,84 @@ void PID_AL()
err_old = err; //将误差幅值到上一次误差
//~ 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;
//~ if (abs(err_sum) > 100 )
//~ {
Kpid_speed = (Kp*err + Ki*err_sum + Kd*err_diff) * Kpid_base;
//~ }
//~ else
//~ {
//~ Kpid_speed = (Kp*err + Kd*err_diff) * Kpid_base;
//~ }
pid_speed_xianfu();
speed_r = speed_l + Kpid_speed;
//~ speed_r = speed_l - 1+ Kpid_speed;
/*---打印输出PID控制的各项参数2---*/
cout << "PID_out:" << Kpid_speed << endl ;
//~ cout << "PID_out:" << Kpid_speed << endl ;
/*---打印结束---*/
// VWSetSpeed(speed,Kpid*Kpid_base); //控制小车的行驶速度，角速度(模拟器可以用，实物no)
MOTORDriveRaw(2,speed_r);
MOTORDriveRaw(1,speed_l);
MOTORDriveRaw(2,speed_r + Kpid_speed);
//~ MOTORDriveRaw(2,speed_r);
MOTORDriveRaw(1,speed_l - Kpid_speed);
}
/*---PID控制下的直线行驶---*/
void PIDStraight()
{
{
int bot_move; //定义基础角速度和速度,和移动距离
int L_PSD, R_PSD, F_PSD; //定义左侧，右侧，前方的距离值
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); //读取右侧和墙壁的距离
MOTORDriveRaw(2,speed_r);
MOTORDriveRaw(1,speed_l);
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; //读取右侧和墙壁的距离
/*---打印输出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 (20<L_PSD && L_PSD<140 && 20<R_PSD && R_PSD<140) //如果两侧都有墙的情况
if (20<L_PSD && L_PSD<125 && 20<R_PSD && R_PSD<125) //如果两侧都有墙的情况
{
err = L_PSD - R_PSD;
PID_AL();
}
else if (20<L_PSD && L_PSD<140) //只有左侧有墙的情况
else if (20<L_PSD && L_PSD<150) //只有左侧有墙的情况
{
err = L_PSD - DIST_wall_RL;
PID_AL();
}
else if (20<R_PSD && R_PSD<140) //只有右侧有墙的情况
else if (20<R_PSD && R_PSD<150) //只有右侧有墙的情况
{
err = DIST_wall_RL - R_PSD;
PID_AL();
}
else
{
L_PSD = L_PSD % 300;
R_PSD = R_PSD % 300;
L_PSD = L_PSD % 100;
R_PSD = R_PSD % 100;
err = L_PSD - R_PSD;
PID_AL();
}
VWGetPosition(&x_2, &y_2, &phi_2);
bot_move = sqrt(pow(x_2-x_1,2) + pow(y_2-y_1,2));
//~ cout << "move:" << bot_move << endl;
}while (bot_move < DIST_move && F_PSD > DIST_wall_F); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
MOTORDriveRaw(1,0);
MOTORDriveRaw(2,0);
//~ cout << "move:" << bot_move << endl;
//~ MOTORDriveRaw(1,0);
//~ MOTORDriveRaw(2,0);
}