PID控制

#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

#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;
//}