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