Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of PID by
PID.cpp
00001 /** 00002 * @author Aaron Berk 00003 * 00004 * @section LICENSE 00005 * 00006 * Copyright (c) 2010 ARM Limited 00007 * 00008 * Permission is hereby granted, free of charge, to any person obtaining a copy 00009 * of this software and associated documentation files (the "Software"), to deal 00010 * in the Software without restriction, including without limitation the rights 00011 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 00012 * copies of the Software, and to permit persons to whom the Software is 00013 * furnished to do so, subject to the following conditions: 00014 * 00015 * The above copyright notice and this permission notice shall be included in 00016 * all copies or substantial portions of the Software. 00017 * 00018 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 00019 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 00020 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 00021 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 00022 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 00023 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 00024 * THE SOFTWARE. 00025 * 00026 * @section DESCRIPTION 00027 * 00028 * A PID controller is a widely used feedback controller commonly found in 00029 * industry. 00030 * 00031 * This library is a port of Brett Beauregard's Arduino PID library: 00032 * 00033 * http://www.arduino.cc/playground/Code/PIDLibrary 00034 * 00035 * The wikipedia article on PID controllers is a good place to start on 00036 * understanding how they work: 00037 * 00038 * http://en.wikipedia.org/wiki/PID_controller 00039 * 00040 * For a clear and elegant explanation of how to implement and tune a 00041 * controller, the controlguru website by Douglas J. Cooper (who also happened 00042 * to be Brett's controls professor) is an excellent reference: 00043 * 00044 * http://www.controlguru.com/ 00045 */ 00046 00047 /** 00048 * Includes 00049 */ 00050 #include "PID.h" 00051 PID::PID(const PID& p) 00052 { 00053 timer = p.timer; 00054 data = 0; 00055 bias=0; 00056 00057 intF = false; 00058 diffF = false; 00059 propF = false; 00060 GAIN_P = p.GAIN_P; 00061 GAIN_I = p.GAIN_I; 00062 GAIN_D = p.GAIN_D; 00063 setInterval(0.01); 00064 s_dErrIntg=0; 00065 dErr_prev=0;OutputLimits(1,0); 00066 } 00067 PID::PID(float tauKp, float tauKi, float tauKd,Timer *T) 00068 { 00069 timer=T; 00070 data = 0; 00071 bias=0; 00072 GAIN_P = tauKp; 00073 GAIN_I = tauKi; 00074 GAIN_D = tauKd; 00075 setInterval(0.01); 00076 s_dErrIntg=0; 00077 dErr_prev=0;OutputLimits(1,0); 00078 } 00079 00080 void PID::InputLimits(float max,float min) 00081 { 00082 //Make sure we haven't been given impossible values. 00083 if (min >= max) { 00084 return; 00085 } 00086 00087 00088 InMin = min; 00089 InMax = max; 00090 InSpan = InMax - InMin; 00091 } 00092 void PID::OutputLimits(float max,float min) 00093 { 00094 if (min >= max) { 00095 return; 00096 } 00097 00098 00099 OutMin = min; 00100 OutMax = max; 00101 OutSpan = OutMax - OutMin; 00102 } 00103 void PID::setInterval(float inter) 00104 { 00105 interval = inter; 00106 //start(); 00107 } 00108 void PID::start() 00109 { 00110 timer->start(); 00111 T.attach(this,&PID::PIDctrl,interval); 00112 //printf("PID statr\n"); 00113 //wait(0.1); 00114 //PIDctrl(); 00115 00116 } 00117 void PID::stop() 00118 { 00119 timer->stop(); 00120 //T.detach(); 00121 } 00122 00123 void PID::pid_reset() 00124 { 00125 dTarget=0; 00126 dPoint=0; 00127 // PI制御ゲイン 00128 data=0; 00129 s_dErrIntg=0 ,dErr_prev=0; 00130 } 00131 void PID::PIDctrl() 00132 { 00133 double dErr; 00134 double dRet; 00135 00136 // 誤差 00137 dErr = dTarget - dPoint; 00138 float T=gettime(); 00139 //printf("%f\t",T); 00140 double dErrDiff; 00141 // 誤差積分 00142 /*if(GAIN_I*s_dErrIntg>OutMax)s_dErrIntg=GAIN_I*OutMax; 00143 else if(GAIN_I*s_dErrIntg<OutMin)s_dErrIntg=GAIN_I*OutMin; 00144 else */ 00145 s_dErrIntg += (dErr+dErr_prev )* T /2.0; 00146 // 制御入力 00147 dRet = bias + GAIN_P * (propF == true ? userProp : dErr) + 00148 GAIN_I * (intF == true ? userInt : s_dErrIntg) - 00149 GAIN_D * (diffF == true ? userDiff : dErrDiff); 00150 00151 dErr_prev = dErr; 00152 dErrDiff = (dErr-data)/T; 00153 if(dRet>OutMax)data=OutMax; 00154 else if(dRet<OutMin)data=OutMin; 00155 else data = dRet; 00156 00157 //printf("PID %3.3f %3.3f %3.3f %3.3f %3.3f\r\n",data ,GAIN_P*dErr,GAIN_I*s_dErrIntg,GAIN_D*dErrDiff,T); 00158 00159 } 00160 00161 00162 00163
Generated on Sat Jul 16 2022 09:23:56 by
1.7.2
