xerolyskinner/MicrochipFor32
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
61213192c538a3d28f52e3835a0e9e6d19507f9c
MicrochipFor32/bsp_Device/bsp_math/bsp_PID.cpp

208 lines
6.6 KiB
C++
Raw Normal View History

内容大更新 更加规范化 加入modbus类 优化帮助文档
2022-11-22 03:43:24 +08:00
/*----------------------------------------------------------------------------------------------------
#
# Copyright (c) 2022 Yuankang Liang(XerolySkinner)
#
# <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD><EFBFBD>ṩ,<2C><><EFBFBD>κ<EFBFBD><CEBA><EFBFBD>ʾ<EFBFBD><CABE><EFBFBD><EFBFBD>ʾ
# <09><><EFBFBD>κ<EFBFBD><CEBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD>߶<EFBFBD><DFB6><EFBFBD><EFBFBD>е<EFBFBD><D0B5>κ<EFBFBD><CEBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>⳥<EFBFBD><E2B3A5><EFBFBD><EFBFBD>
#
# ʹ<>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>:
# 1. <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Դ,<2C>㲻<EFBFBD><E3B2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>д<EFBFBD><D0B4>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD>.
# 2. <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>κ<EFBFBD>Ŀ<EFBFBD><C4BF>,ǰ<><C7B0><EFBFBD>ǰ<EFBFBD>Ȩ<EFBFBD><C8A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>и<EFBFBD><D0B8><EFBFBD><EFBFBD><EFBFBD>.
# <09><><EFBFBD>Ұ<EFBFBD>Ȩ<EFBFBD><C8A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͬʱ<CDAC><CAB1><EFBFBD><EFBFBD>.
# 3. <09><><EFBFBD><EFBFBD>ʹ<EFBFBD><CAB9>,<2C><><EFBFBD><EFBFBD>,<2C>޸<EFBFBD>,<2C>ַ<EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD>۱<EFBFBD><DBB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
# 4. <09><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڲ<EFBFBD>Ʒ<EFBFBD><C6B7>ʹ<EFBFBD><CAB9>,<2C><>Ʒ<EFBFBD>ĵ<EFBFBD><C4B5>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>͵ĵ<CDB5><C4B5><EFBFBD><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD><EFBFBD><EFBFBD>.
# 5. <09><>֪ͨ<CDA8><D6AA><EFBFBD>ô<EFBFBD><C3B4>κ<EFBFBD><CEBA><EFBFBD>Դɾ<D4B4><C9BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
#
# Yuankang Liang(XerolySkinner)
# E-mail:zabbcccbbaz@163.com
# QQ:2715099320
# Mobile Phone:13005636215
#
# All rights reserved.
*/
/**
* @file bsp_PID.cpp
* @brief ʵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>PID<EFBFBD>㷨
* @mainpage <EFBFBD><EFBFBD>Ҫ<EFBFBD><EFBFBD>Ϣ
* @author Yuankang Liang(XerolySkinner)
* @email zabbcccbbaz@163.com
* @version V1.0.0
* @date 2022-11-09 03:12
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////
//----------------------------------------------------------------------------------------------------
// ͷ<>ļ<EFBFBD>
//
#include "bsp_PID.h"
//////////////////////////////////////////////////////////////////////////////////////////////////////
//----------------------------------------------------------------------------------------------------
// <09>ຯ<EFBFBD><E0BAAF>
//
/**
* @brief PID<EFBFBD>㷨
* @param temp Ŀǰ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* @return <EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*/
float bsp_Position_PID::PID(float temp) {
float thisError;
thisError = target - temp; //<2F><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>趨ֵ<E8B6A8><D6B5>ȥ<EFBFBD><C8A5>ǰֵ
integral += thisError; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֣<EFBFBD><D6A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ۼ<EFBFBD><DBBC><EFBFBD><EFBFBD><EFBFBD>
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<EFBFBD>㷨
* @param temp Ŀǰ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* @return <EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*/
float bsp_Increment_PID::PID(float temp) {
float thisError=target - temp; //<2F><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>趨ֵ<E8B6A8><D6B5>ȥ<EFBFBD><C8A5>ǰֵ
float increment;
float pError, dError, iError;
pError = thisError - lasterror;
iError = thisError;
dError = thisError - 2 * lasterror + preerror;
increment =
Kp * pError +
Ki * iError +
Kd * dError; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
preerror = lasterror; //<2F><><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>´<EFBFBD><C2B4><EFBFBD><EFBFBD><EFBFBD>
lasterror = thisError; //<2F><><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>´<EFBFBD><C2B4><EFBFBD><EFBFBD><EFBFBD>
result += increment; //<2F>ϴν<CFB4><CEBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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 <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* @return <EFBFBD><EFBFBD><EFBFBD><EFBFBD>PID<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*/
void bsp_Position_PID::clean(void) {
integral = 0; //<2F><><EFBFBD><EFBFBD>ֵ
lasterror = 0; //ǰһ<C7B0><D2BB>ƫ<EFBFBD><C6AB>
result = 0; //<2F><><EFBFBD><EFBFBD>ֵ
}
//----------------------------------------------------------------------------------------------------
/**
* @brief <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* @return <EFBFBD><EFBFBD><EFBFBD><EFBFBD>PID<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*/
void bsp_Increment_PID::clean(void) {
lasterror = 0; //ǰһ<C7B0><D2BB>ƫ<EFBFBD><C6AB>
preerror = 0; //ǰ<><C7B0><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB>
result = 0; //<2F><><EFBFBD><EFBFBD>ֵ
}
//----------------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------
/**
* @brief PID<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* @param P P<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* @param I I<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* @param D D<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*/
void bsp_Position_PID::set(float P, float I, float D) {
Kp = P; Ki = I; Kd = D;}
//----------------------------------------------------------------------------------------------------
/**
* @brief PID<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* @param P P<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* @param I I<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* @param D D<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*/
void bsp_Increment_PID::set(float P, float I, float D) {
Kp = P; Ki = I; Kd = D;}
//////////////////////////////////////////////////////////////////////////////////////////////////////
//----------------------------------------------------------------------------------------------------
// <09><><EFBFBD>캯<EFBFBD><ECBAAF>
//
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<EFBFBD>㷨
* @param Kp P<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* @param Ki I<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* @param Kd D<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* @param target Ŀ<EFBFBD><EFBFBD>ֵ
* @param result_H <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
* @param result_L <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Сֵ
*/
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 = 0;
bsp_Position_PID::result_L = 0;
}
//----------------------------------------------------------------------------------------------------
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<EFBFBD>㷨
* @param Kp P<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* @param Ki I<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* @param Kd D<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* @param target Ŀ<EFBFBD><EFBFBD>ֵ
* @param result_H <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
* @param result_L <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Сֵ
*/
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 = 0;
bsp_Increment_PID::result_L = 0;
}
//----------------------------------------------------------------------------------------------------
bsp_Increment_PID::~bsp_Increment_PID(void) {}
//////////////////////////////////////////////////////////////////////////////////////////////////////
Reference in New Issue Copy Permalink