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     GAIN_P = p.GAIN_P;
00057     GAIN_I = p.GAIN_I;
00058     GAIN_D = p.GAIN_D;
00059     setInterval(0.001);
00060     s_dErrIntg=0;
00061     dErr_prev=0;OutputLimits(1,0);
00062 }
00063 PID::PID(float tauKp, float tauKi, float tauKd,Timer *T)
00064 {
00065     timer=T;
00066     data = 0;
00067     bias=0;
00068     GAIN_P = tauKp;
00069     GAIN_I = tauKi;
00070     GAIN_D = tauKd;
00071     setInterval(0.001);
00072     s_dErrIntg=0;
00073     dErr_prev=0;OutputLimits(1,0);
00074 }
00075 
00076 void PID::InputLimits(float max,float min)
00077 {
00078     //Make sure we haven't been given impossible values.
00079     if (min >= max) {
00080         return;
00081     }
00082 
00083 
00084     InMin  = min;
00085     InMax  = max;
00086     InSpan = InMax - InMin;
00087 }
00088 void PID::OutputLimits(float max,float min)
00089 {
00090     if (min >= max) {
00091         return;
00092     }
00093 
00094 
00095     OutMin  = min;
00096     OutMax  = max;
00097     OutSpan = OutMax - OutMin;
00098 }
00099 void PID::setInterval(double inter)
00100 {
00101     interval = inter;
00102     //start();
00103 }
00104 void PID::start()
00105 {
00106     timer->start();
00107     //T.attach(this,&PID::PIDctrl,interval);
00108     //printf("PID statr\n");
00109     //wait(0.1);
00110     //PIDctrl();
00111 
00112 }
00113 void PID::stop()
00114 {
00115     timer->stop();
00116     //T.detach();
00117 }
00118 
00119 void PID::pid_reset()
00120 {
00121     dTarget=0;
00122     dPoint=0;
00123     // PI制御ゲイン
00124     data=0;
00125     s_dErrIntg=0 ,dErr_prev=0;
00126 }
00127 void PID::PIDctrl()
00128 {
00129     double dErr;
00130     double dRet;
00131 
00132     // 誤差
00133     dErr = dTarget - dPoint;
00134     float T=gettime();
00135     //printf("%f\t",T);
00136     double dErrDiff = (dErr-data)/T;
00137     // 誤差積分
00138     if(data>OutMax)s_dErrIntg=OutMax;
00139     else if(data<OutMin)s_dErrIntg=OutMin;
00140     else s_dErrIntg += (dErr+dErr_prev )* T /2.0;
00141     // 制御入力
00142     dRet = bias+GAIN_P * dErr + GAIN_I * s_dErrIntg + GAIN_D*dErrDiff;
00143     
00144     dErr_prev = dErr;
00145     if(dRet>OutMax)data=OutMax;
00146     else if(dRet<OutMin)data=OutMin;
00147     else data = dRet;
00148     
00149     //printf("PID %f,%f,%f,%f,%f\r\n",data ,dErr,s_dErrIntg,dErrDiff,timer->read());
00150     
00151 }
00152 
00153 
00154 
00155