208 lines
6.7 KiB
C++
208 lines
6.7 KiB
C++
/*----------------------------------------------------------------------------------------------------
|
|
#
|
|
# Copyright (c) 2022 Yuankang Liang(XerolySkinner)
|
|
#
|
|
# 本软件按原样提供,无任何明示或暗示
|
|
# 在任何情况下,作者都不承担任何损害赔偿责任
|
|
#
|
|
# 使用的许可声明:
|
|
# 1. 不得歪曲本软件的来源,你不能声称你编写了原始软件.
|
|
# 2. 免费授予以任何目的,前提是版权声明出现在所有副本中.
|
|
# 并且版权声明和许可声明同时出现.
|
|
# 3. 你有使用,复制,修改,分发,和销售本软件的许可.
|
|
# 4. 如果你在产品中使用,产品文档中的声明是赞赏的但不是必须的.
|
|
# 5. 本通知不得从任何来源删除或更改.
|
|
#
|
|
# Yuankang Liang(XerolySkinner)
|
|
# E-mail:zabbcccbbaz@163.com
|
|
# QQ:2715099320
|
|
# Mobile Phone:13005636215
|
|
#
|
|
# All rights reserved.
|
|
*/
|
|
|
|
/**
|
|
* @file bsp_PID.cpp
|
|
* @brief 实现了PID算法
|
|
* @mainpage 主要信息
|
|
* @author Yuankang Liang(XerolySkinner)
|
|
* @email zabbcccbbaz@163.com
|
|
* @version V1.0.0
|
|
* @date 2022-11-09 03:12
|
|
*/
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
//----------------------------------------------------------------------------------------------------
|
|
// 头文件
|
|
//
|
|
#include "bsp_PID.h"
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
//----------------------------------------------------------------------------------------------------
|
|
// 类函数
|
|
//
|
|
/**
|
|
* @brief PID算法
|
|
* @param temp 目前变量
|
|
* @return 返回调整量
|
|
*/
|
|
float bsp_Position_PID::PID(float temp) {
|
|
float thisError;
|
|
thisError = target - temp; //当前误差等于设定值减去当前值
|
|
integral += thisError; //误差积分，把所有误差累加起来
|
|
result =
|
|
Kp * thisError +
|
|
Ki * integral +
|
|
Kd * (thisError - lasterror);
|
|
lasterror = thisError;
|
|
|
|
if (result_H == result_L)return result;
|
|
if (result >= result_H)result = result_H;
|
|
else if (result <= result_L)result = result_L;
|
|
return result;
|
|
}
|
|
//----------------------------------------------------------------------------------------------------
|
|
/**
|
|
* @brief PID算法
|
|
* @param temp 目前变量
|
|
* @return 返回调整量
|
|
*/
|
|
float bsp_Increment_PID::PID(float temp) {
|
|
float thisError=target - temp; //当前误差等于设定值减去当前值
|
|
float increment;
|
|
float pError, dError, iError;
|
|
|
|
pError = thisError - lasterror;
|
|
iError = thisError;
|
|
dError = thisError - 2 * lasterror + preerror;
|
|
|
|
increment =
|
|
Kp * pError +
|
|
Ki * iError +
|
|
Kd * dError; //增量计算
|
|
preerror = lasterror; //存放偏差用于下次运算
|
|
lasterror = thisError; //存放偏差用于下次运算
|
|
result += increment; //上次结果叠加增量
|
|
|
|
if (result_H == result_L)return result;
|
|
if (result >= result_H)result = result_H;
|
|
else if (result <= result_L)result = result_L;
|
|
return result;}
|
|
//----------------------------------------------------------------------------------------------------
|
|
//----------------------------------------------------------------------------------------------------
|
|
/**
|
|
* @brief 清理历史数据
|
|
* @return 清除PID运算数据
|
|
*/
|
|
void bsp_Position_PID::clean(void) {
|
|
integral = 0; //积分值
|
|
lasterror = 0; //前一拍偏差
|
|
result = 0; //输出值
|
|
}
|
|
//----------------------------------------------------------------------------------------------------
|
|
/**
|
|
* @brief 清理历史数据
|
|
* @return 清除PID运算数据
|
|
*/
|
|
void bsp_Increment_PID::clean(void) {
|
|
lasterror = 0; //前一拍偏差
|
|
preerror = 0; //前两拍偏差
|
|
result = 0; //输出值
|
|
}
|
|
//----------------------------------------------------------------------------------------------------
|
|
//----------------------------------------------------------------------------------------------------
|
|
/**
|
|
* @brief PID参数设置
|
|
* @param P P参数
|
|
* @param I I参数
|
|
* @param D D参数
|
|
*/
|
|
void bsp_Position_PID::set(float P, float I, float D) {
|
|
Kp = P; Ki = I; Kd = D;}
|
|
//----------------------------------------------------------------------------------------------------
|
|
/**
|
|
* @brief PID参数设置
|
|
* @param P P参数
|
|
* @param I I参数
|
|
* @param D D参数
|
|
*/
|
|
void bsp_Increment_PID::set(float P, float I, float D) {
|
|
Kp = P; Ki = I; Kd = D;}
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
//----------------------------------------------------------------------------------------------------
|
|
// 构造函数
|
|
//
|
|
bsp_Position_PID::bsp_Position_PID(void) {
|
|
bsp_Position_PID::target = 0;
|
|
bsp_Position_PID::lasterror = 0;
|
|
bsp_Position_PID::Kp = 0;
|
|
bsp_Position_PID::Ki = 0;
|
|
bsp_Position_PID::Kd = 0;
|
|
bsp_Position_PID::integral = 0;
|
|
bsp_Position_PID::result = 0;
|
|
bsp_Position_PID::result_H = 0;
|
|
bsp_Position_PID::result_L = 0;
|
|
}
|
|
//----------------------------------------------------------------------------------------------------
|
|
/**
|
|
* @brief PID算法
|
|
* @param Kp P参数
|
|
* @param Ki I参数
|
|
* @param Kd D参数
|
|
* @param target 目标值
|
|
* @param result_H 输出最大值
|
|
* @param result_L 输出最小值
|
|
*/
|
|
bsp_Position_PID::bsp_Position_PID(
|
|
float Kp, float Ki, float Kd, float target,
|
|
float result_H, float result_L) {
|
|
bsp_Position_PID::target = target;
|
|
bsp_Position_PID::lasterror = 0;
|
|
bsp_Position_PID::Kp = Kp;
|
|
bsp_Position_PID::Ki = Ki;
|
|
bsp_Position_PID::Kd = Kd;
|
|
bsp_Position_PID::integral = 0;
|
|
bsp_Position_PID::result = 0;
|
|
bsp_Position_PID::result_H = result_H;
|
|
bsp_Position_PID::result_L = result_L;
|
|
}
|
|
//----------------------------------------------------------------------------------------------------
|
|
bsp_Position_PID::~bsp_Position_PID(void) {}
|
|
//----------------------------------------------------------------------------------------------------
|
|
bsp_Increment_PID::bsp_Increment_PID(void) {
|
|
bsp_Increment_PID::target = 0;
|
|
bsp_Increment_PID::lasterror = 0;
|
|
bsp_Increment_PID::preerror = 0;
|
|
bsp_Increment_PID::Kp = 0;
|
|
bsp_Increment_PID::Ki = 0;
|
|
bsp_Increment_PID::Kd = 0;
|
|
bsp_Increment_PID::result = 0;
|
|
bsp_Increment_PID::result_H = 0;
|
|
bsp_Increment_PID::result_L = 0;
|
|
}
|
|
//----------------------------------------------------------------------------------------------------
|
|
/**
|
|
* @brief PID算法
|
|
* @param Kp P参数
|
|
* @param Ki I参数
|
|
* @param Kd D参数
|
|
* @param target 目标值
|
|
* @param result_H 输出最大值
|
|
* @param result_L 输出最小值
|
|
*/
|
|
bsp_Increment_PID::bsp_Increment_PID(
|
|
float Kp, float Ki, float Kd, float target,
|
|
float result_H, float result_L) {
|
|
bsp_Increment_PID::target = target;
|
|
bsp_Increment_PID::lasterror = 0;
|
|
bsp_Increment_PID::preerror = 0;
|
|
bsp_Increment_PID::Kp = Kp;
|
|
bsp_Increment_PID::Ki = Ki;
|
|
bsp_Increment_PID::Kd = Kd;
|
|
bsp_Increment_PID::result = 0;
|
|
bsp_Increment_PID::result_H = result_H;
|
|
bsp_Increment_PID::result_L = result_L;
|
|
}
|
|
//----------------------------------------------------------------------------------------------------
|
|
bsp_Increment_PID::~bsp_Increment_PID(void) {}
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////////
|