mirror of
https://gitee.com/yyz_o/bk_bishe_pi.git
synced 2025-09-07 23:21:26 +00:00
132 lines
3.7 KiB
C++
132 lines
3.7 KiB
C++
#include<iostream>
|
||
#include<string>
|
||
#include"eyebot++.h"
|
||
#include"PID.h"
|
||
using namespace std;
|
||
|
||
float Kp = 0.5, Ki = 0.3, Kd = 0.1; //定义PID的参数
|
||
float Kpid;
|
||
float Kpid_speed;
|
||
const int speed = 16;
|
||
const float Kpid_base = 0.1;
|
||
const int speed2 = 0.75*speed;
|
||
int speed1;
|
||
//int speed2 = speed;
|
||
/*---定义全局变量---*/
|
||
int err, err_old, err_sum, err_diff; //定义误差，上一次误差，积分误差，微分误差
|
||
const int eI_max = 30;
|
||
/*---定义全局常量---*/
|
||
/*---定义全局常量---*/
|
||
// const int DIST_cell = 300 ;
|
||
// const int DIST_wall = 100 ;
|
||
const int DIST_cell = 300;
|
||
const int DIST_wall_f = 68;
|
||
// const int DIST_wall_rl = 88;
|
||
/*---全局各种变量与常量定义完成---*/
|
||
|
||
|
||
/*---对PID控制的积分部分I限幅---*/
|
||
void eI_xianfu(int eI_max)
|
||
{
|
||
if (err_sum >= eI_max) //如果err_sum大于限幅，则直接让err_sum=限幅，防止err_sum过大，影响PID控制
|
||
{
|
||
err_sum = eI_max;
|
||
}
|
||
else if (err_sum <= -eI_max)
|
||
{
|
||
err_sum = -eI_max;
|
||
}
|
||
else
|
||
{
|
||
//此处为空;
|
||
}
|
||
}
|
||
|
||
/*---位置式PID算法---*/
|
||
void PID_AL()
|
||
{
|
||
err_sum += err; //积分累计误差值
|
||
eI_xianfu(eI_max); //积分部分限幅
|
||
err_diff = err - err_old; //微分部分误差值
|
||
/*---打印输出PID控制的各项参数1---*/
|
||
cout << "err:" << err << "\t" ;
|
||
cout << "err_old:" << err_old << "\t" ;
|
||
cout << "err_sum:" << err_sum << "\t" ;
|
||
cout << "err_diff:" << err_diff << "\t" ;
|
||
/*---打印结束---*/
|
||
err_old = err; //将误差幅值到上一次误差
|
||
|
||
Kpid = Kp*err + Ki*err_sum + Kd*err_diff; //PID控制算法输出的倍率
|
||
Kpid_speed = Kpid * Kpid_base;
|
||
if (Kpid_speed >= 8) //对pid输出值进行限制，防止过大电机过快，或者减的过小使电机不转动
|
||
{
|
||
Kpid_speed = 8;
|
||
}
|
||
else if (Kpid_speed <= -8)
|
||
{
|
||
Kpid_speed = -8;
|
||
}
|
||
else
|
||
{
|
||
//error
|
||
}
|
||
/*---打印输出PID控制的各项参数2---*/
|
||
cout << "PID_out:" << Kpid << endl ;
|
||
/*---打印结束---*/
|
||
MOTORDriveRaw(1,speed1);
|
||
MOTORDriveRaw(2,speed1 + Kpid_speed); //控制小车的行驶速度，角速度
|
||
}
|
||
|
||
/*---PID控制下的直线行驶---*/
|
||
void PIDStraight()
|
||
{
|
||
int DIST_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,speed1);
|
||
|
||
do
|
||
{
|
||
F_PSD = PSDGetRaw(1); //读取前方和墙壁的距离
|
||
L_PSD = PSDGetRaw(2) - 16; //读取左侧和墙壁的距离
|
||
R_PSD = PSDGetRaw(3); //读取右侧和墙壁的距离
|
||
/*---打印输出PSD数值---*/
|
||
cout << "PSD L:" << L_PSD << " R:" << R_PSD << " F:" << F_PSD << "\t" ;
|
||
/*---打印结束---*/
|
||
if (50<L_PSD && L_PSD<180 && 50<R_PSD && R_PSD<180) //如果两侧都有墙的情况
|
||
{
|
||
err = L_PSD - R_PSD;
|
||
PID_AL();
|
||
}
|
||
else if (50<L_PSD && L_PSD<180) //只有左侧有墙的情况
|
||
{
|
||
err = L_PSD - DIST_wall_f;
|
||
PID_AL();
|
||
}
|
||
else if (50<R_PSD && R_PSD<180) //只有右侧有墙的情况
|
||
{
|
||
err = DIST_wall_f - R_PSD;
|
||
PID_AL();
|
||
}
|
||
else
|
||
{
|
||
L_PSD = L_PSD % DIST_cell;
|
||
R_PSD = R_PSD % DIST_cell;
|
||
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));
|
||
if (DIST_move > (DIST_cell -50)) //当移动距离只剩下最后的50mm的时候降点速度
|
||
{
|
||
speed1 = speed2;
|
||
}
|
||
}while (DIST_move < DIST_cell && F_PSD > DIST_wall_f); //当移动到指定距离，或者检测到小车前方的距离小于检测距离的时候，停止运行
|
||
//VWSetSpeed(0,0);
|
||
}
|