版本1 云台+无人机
https://en.wikipedia.org/wiki/PID_controller
https://github.com/tekdemo/MiniPID
详细讲解
PIDController.h
#ifndef _POSITION_CONTROLLER_H
#define _POSITION_CONTROLLER_H
//#include "RefPoint.h"
#include <iostream>
#include <vector>
#include <stdint.h>
class MoSLAM;
class PIDController{
public:
/* coefficients for P, I, D*/
double kP, kI, kD;
/* coefficient for filtering the derivative values */
double kN;
double _P, _I, _D;
bool _bFirstFrame;
//protected:
double old_err, cur_err;
//
double errs[20];
/* error integration*/
double cur_I;
/* error derivative*/
double old_D;
public:
PIDController():kP(0.0),kI(0.0),kD(0.0){reset();}
void loadParam(const char* filePath);
void setParam(double _kP, double _kI, double _kD, double _kN);
void reset();
double getOutput(double curerr, double dt);
};
/*
class PositionController{
protected:
uint32_t _oldts;
///* reference point
RefPoint _refPt;
///* error to the reference point
const MoSLAM* _moSLAM;
PIDController _pidX, _pidY, _pidZ;
public:
PositionController():_oldts(0),_moSLAM(0){}
void loadParameters();
void setMoSLAM(const MoSLAM* moSLAM){
_moSLAM = moSLAM;
}
void reset(){
_oldts = -1;
_pidX.reset();
_pidY.reset();
_pidZ.reset();
_refPt.setFinished();
//std::cout <<" reset is called!" << std::endl;
}
void moveTo(RefPoint refPt){
reset();
_refPt = refPt;
_refPt.setRunning();
}
void getXYZErrors(double scale, const double* R, const double* t, double err[3]);
void correct(double dt, double scale, const double* R, const double* t, double roll, double pitch, double yaw);
void update();
};
void sendMove(double req_phi, double req_theta, double req_speed);
*/
#endif
PIDController.cpp
#include "PIDController.h"
#include <fstream>
using namespace std;
void PIDController::loadParam(const char* filePath)
{
std::ifstream file(filePath);
file >> kP >> kI >> kD >> kN;
cout <<" kP:" << kP << " kI:" << kI << " kD:" << kD << " kN:" << kN << endl;
file.close();
}
void PIDController::setParam(double _kP, double _kI, double _kD, double _kN)
{
kP = _kP;
kI = _kI;
kD = _kD;
kN = _kN;
}
void PIDController::reset(){
old_err = 0;
cur_err = 0;
old_D = 0;
cur_I = 0;
old_D = 0;
for (int i = 0; i < 20; ++i)
{
errs[i] = 0;
}
_bFirstFrame = true;
}
double PIDController::getOutput(double curerr, double dt)
{
old_err = cur_err;
cur_err = curerr;
///
for (int i = 0; i < 10; ++i)
{
errs[i + 1] = errs[i];
}
errs[0]=curerr;
///
double s = 0;
if( !_bFirstFrame)
{
//assert(dt > 0);
cur_I += cur_err*dt;
_P = cur_err;
_I = cur_I;
double Derr = (cur_err - old_err)/dt;
_D = (kN*dt*Derr + old_D)/(kN*dt + 1);
old_D = _D;
}else
{
_P = cur_err;
_I = 0;
_D = 0;
_bFirstFrame = false;
}
//
double aveerr = (errs[0]+errs[1])/2;
double pasterr =0;
for (int i=1; i<=3; ++i) pasterr+=errs[i];
pasterr/=3;
_D = aveerr - pasterr;
//cout << " _P" << _P << " _D" << _D << endl; //<< " _I" << _I
return kP*_P + kI*_I + kD*_D;
}
使用
#define IMAGECETREX 960
int main(int argc, char* argv)
{
PIDController pid;
pid.reset(); //初始化PID
pid.setParam(0.32,0.009,0.0028,0); //基本参数
int x = 10;
int diffX = (x - IMAGECETREX);
while (1)
{
x = x - pid.getOutput(diffX, 0.04);
diffX = (x - IMAGECETREX);
//cout << "diffX = " << diffX << endl;
if (diffX == 0)
system("pause");
cout << "x = " << x << "diffX = " << diffX << "id.getOutput(diffX, 0.04) = "<< pid.getOutput(diffX, 0.04) << endl;
}
return 0;
}