版本1 云台+无人机
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; //}
使用
原文地址:https://www.cnblogs.com/kekeoutlook/p/8353531.html
时间: 2024-10-14 00:52:16