PID控制

版本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

PID控制的相关文章

Matlab高斯分布输入的PID控制

一.matlab的随机数组 s=1:1:500;in = 0.1*randn(1,500)+1;plot(s,in,'*'); hist(in,20); 二.PID控制 网上源码: clear all; close all; ts=0.001; sys=tf(5.235e005,[1,87.35,1.047e004,0]);%建立传递函数 dsys=c2d(sys,ts,'z');%将连续的时间模型转换成离散的时间模型,采样时间是ts=0.001 [num,den]=tfdata(dsys,'v

PID控制動手玩玩看

PID控制動手玩玩看 PID三個參數到底該怎麼調才好,真的是一門藝術. 雖然在Marlin韌體內有提供自動測量的功能,但是測得的結果,不見得能令人滿意,還是需要調整.可是到底該怎麼調整?從哪個參數開始動手?數值該增加還是該減小?會不會調整了這個,又需要回頭調整上一個?整個調整過程實在是亂七八糟,不容易理出頭緒.在加上實驗需要時間,可是有一直找不出最佳參數,實在有夠累人. 今天我不曉得是哪根筋不對,突然想說可以用excel模擬PID溫度控制,這樣就可以很快得得到實驗結果,增加調整PID參數的經驗.

两轮自平衡小车双闭环PID控制设计

                                                                                        两轮自平衡小车的研究意义 1.1两轮平衡车的研究意义 两轮平衡车是一种能够感知环境,并且能够进行分析判断然后进行行为控制的多功能的系统,是移动机器人的一种.在运动控制领域中,为了研究控制算法,建立两轮平衡车去验证控制算法也是非常有用的,这使得在研究自动控制领域理论时,两轮平衡车也被作为课题,被广泛研究.对于两轮平衡车模型的

四轴飞行器飞行原理与双闭环PID控制

四轴轴飞行器是微型飞行器的其中一种,相对于固定翼飞行器,它的方向控制灵活.抗干扰能力强.飞行稳定,能够携带一定的负载和有悬停功能,因此能够很好地进行空中拍摄.监视.侦查等功能,在军事和民用上具备广泛的运用前景. 四轴飞行器关键技术在于控制策略.由于智能控制算法在运行复杂的浮点型运算以及矩阵运算时,微处理器计算能力受限,难以达到飞行控制实时性的要求:而PID控制简单,易于实现,且技术成熟,因此目前主流的控制策略主要是围绕传统的PID控制展开. 1 四轴飞行器的结构与基本飞行原理 四轴飞行器结构主要

Codesys——AD_DA在PID控制中的作用

1. 摘要 PID控制中用到AD/DA的输入/输出,给出其大致实现思路. 2. 思路 3. 总结 无

PID控制最通俗的解释与PID参数的整定方法

转自->这里 PID是比例.积分.微分的简称,PID控制的难点不是编程,而是控制器的参数整定.参数整定的关键是正确地理解各参数的物理意义,PID控制的原理可以用人对炉温的手动控制来理解.阅读本文不需要高深的数学知识. 1.比例控制 有经验的操作人员手动控制电加热炉的炉温,可以获得非常好的控制品质,PID控制与人工控制的控制策略有很多相似的地方. 下面介绍操作人员怎样用比例控制的思想来手动控制电加热炉的炉温.假设用热电偶检测炉温,用数字仪表显示温度值.在控制过程中,操作人员用眼睛读取炉温,并与炉温

SLAM+语音机器人DIY系列:(四)差分底盘设计——5.底盘PID控制参数整定

摘要 运动底盘是移动机器人的重要组成部分,不像激光雷达.IMU.麦克风.音响.摄像头这些通用部件可以直接买到,很难买到通用的底盘.一方面是因为底盘的尺寸结构和参数是要与具体机器人匹配的:另一方面是因为底盘包含软硬件整套解决方案,是很多机器人公司的核心技术,一般不会随便公开.出于强烈的求知欲与学习热情,我想自己DIY一整套两轮差分底盘,并且将完整的设计过程公开出去供大家学习.说干就干,本章节主要内容: 1.stm32主控硬件设计 2.stm32主控软件设计 3.底盘通信协议 4.底盘ROS驱动开发

模糊控制——(3)模糊自适应整定PID控制

1.原理 这种控制必须精确地确定对象模型,首先将操作人员(专家)长期实践积累的经验知识用控制规则模型化,然后运用推理便可对PID参数实现最佳调整. 自适应模糊PID控制器以误差e和误差变化ec作为输入,可以满足不同时刻的e和ec对PID参数自整定的要求.利用模糊控制规则在线对PID参数进行修改,便构成了自适应模糊PID控制器,其结构如图4-17所示. 离散PID控制算法为: 式中, k为采样序号,T 为采样时间. PID参数模糊自整定是找出PID三个参数 Kp, Ki, Kd 与e和ec之间的模

神经网络及其PID控制

一.人工神经元模型 1.突触权值(连接权) 每一个突触都由其权值作为特征表征,各个神经元之间的连接强度由突触权值来表示.与神经元相连的突触上,连接的输入信号通过权值的加权进入神经元的求和单元. 2.求和单元 求和单元用于求取各输入信号的突触加权和,这个操作构成一个线性组合器. 3.激活函数 激活函数起非线性映射的作用,并用来限制神经元输出振幅.激活函数也称限制函数,或传输函数.通常一个神经元输出的正常范围在[0, 1]区间或[?1, 1]区间. 4.外部偏置 此外,神经元模型还包括一个外部偏置,