PID
Dependents: Nucleo_spi 2015_denziben_i2c_S2 Nucleo_Motor Nucleo_Motor
Fork of PID by
PID.cpp
- Committer:
- kikoaac
- Date:
- 2015-10-30
- Revision:
- 6:775c9421fe3b
- Parent:
- 5:3519920d064d
File content as of revision 6:775c9421fe3b:
/** * @author Aaron Berk * * @section LICENSE * * Copyright (c) 2010 ARM Limited * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. * * @section DESCRIPTION * * A PID controller is a widely used feedback controller commonly found in * industry. * * This library is a port of Brett Beauregard's Arduino PID library: * * http://www.arduino.cc/playground/Code/PIDLibrary * * The wikipedia article on PID controllers is a good place to start on * understanding how they work: * * http://en.wikipedia.org/wiki/PID_controller * * For a clear and elegant explanation of how to implement and tune a * controller, the controlguru website by Douglas J. Cooper (who also happened * to be Brett's controls professor) is an excellent reference: * * http://www.controlguru.com/ */ /** * Includes */ #include "PID.h" PID::PID(const PID& p) { timer = p.timer; data = 0; bias=0; GAIN_P = p.GAIN_P; GAIN_I = p.GAIN_I; GAIN_D = p.GAIN_D; setInterval(0.001); s_dErrIntg=0; dErr_prev=0;OutputLimits(1,0); } PID::PID(float tauKp, float tauKi, float tauKd,Timer *T) { timer=T; data = 0; bias=0; GAIN_P = tauKp; GAIN_I = tauKi; GAIN_D = tauKd; setInterval(0.001); s_dErrIntg=0; dErr_prev=0;OutputLimits(1,0); } void PID::InputLimits(float max,float min) { //Make sure we haven't been given impossible values. if (min >= max) { return; } InMin = min; InMax = max; InSpan = InMax - InMin; } void PID::OutputLimits(float max,float min) { if (min >= max) { return; } OutMin = min; OutMax = max; OutSpan = OutMax - OutMin; } void PID::setInterval(double inter) { interval = inter; //start(); } void PID::start() { timer->start(); //T.attach(this,&PID::PIDctrl,interval); //printf("PID statr\n"); //wait(0.1); //PIDctrl(); } void PID::stop() { timer->stop(); //T.detach(); } void PID::pid_reset() { dTarget=0; dPoint=0; // PI制御ゲイン data=0; s_dErrIntg=0 ,dErr_prev=0; } void PID::PIDctrl() { double dErr; double dRet; // 誤差 dErr = dTarget - dPoint; float T=gettime(); //printf("%f\t",T); double dErrDiff = (dErr-data)/T; // 誤差積分 if(data>OutMax)s_dErrIntg=OutMax; else if(data<OutMin)s_dErrIntg=OutMin; else s_dErrIntg += (dErr+dErr_prev )* T /2.0; // 制御入力 dRet = bias+GAIN_P * dErr + GAIN_I * s_dErrIntg + GAIN_D*dErrDiff; dErr_prev = dErr; if(dRet>OutMax)data=OutMax; else if(dRet<OutMin)data=OutMin; else data = dRet; //printf("PID %f,%f,%f,%f,%f\r\n",data ,dErr,s_dErrIntg,dErrDiff,timer->read()); }