Kiko Ishimoto / PID

ＰＩＤ

Dependents:   Nucleo_spi 2015_denziben_i2c_S2 Nucleo_Motor Nucleo_Motor

Fork of PID by Aaron Berk

PID.cpp@6:775c9421fe3b, 2015-10-30 (annotated)

Committer:
kikoaac
Date:
Fri Oct 30 12:47:54 2015 +0000
Revision:
6:775c9421fe3b
Parent:
5:3519920d064d 
???

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kikoaac 2:14176355508a 1 /**
kikoaac 2:14176355508a 2 * @author Aaron Berk
kikoaac 2:14176355508a 3 *
kikoaac 2:14176355508a 4 * @section LICENSE
kikoaac 2:14176355508a 5 *
kikoaac 2:14176355508a 6 * Copyright (c) 2010 ARM Limited
kikoaac 2:14176355508a 7 *
kikoaac 2:14176355508a 8 * Permission is hereby granted, free of charge, to any person obtaining a copy
kikoaac 2:14176355508a 9 * of this software and associated documentation files (the "Software"), to deal
kikoaac 2:14176355508a 10 * in the Software without restriction, including without limitation the rights
kikoaac 2:14176355508a 11 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
kikoaac 2:14176355508a 12 * copies of the Software, and to permit persons to whom the Software is
kikoaac 2:14176355508a 13 * furnished to do so, subject to the following conditions:
kikoaac 2:14176355508a 14 *
kikoaac 2:14176355508a 15 * The above copyright notice and this permission notice shall be included in
kikoaac 2:14176355508a 16 * all copies or substantial portions of the Software.
kikoaac 2:14176355508a 17 *
kikoaac 2:14176355508a 18 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
kikoaac 2:14176355508a 19 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
kikoaac 2:14176355508a 20 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
kikoaac 2:14176355508a 21 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
kikoaac 2:14176355508a 22 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
kikoaac 2:14176355508a 23 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
kikoaac 2:14176355508a 24 * THE SOFTWARE.
kikoaac 2:14176355508a 25 *
kikoaac 2:14176355508a 26 * @section DESCRIPTION
kikoaac 5:3519920d064d 27 *
kikoaac 2:14176355508a 28 * A PID controller is a widely used feedback controller commonly found in
kikoaac 2:14176355508a 29 * industry.
kikoaac 2:14176355508a 30 *
kikoaac 2:14176355508a 31 * This library is a port of Brett Beauregard's Arduino PID library:
kikoaac 2:14176355508a 32 *
kikoaac 2:14176355508a 33 * http://www.arduino.cc/playground/Code/PIDLibrary
kikoaac 2:14176355508a 34 *
kikoaac 2:14176355508a 35 * The wikipedia article on PID controllers is a good place to start on
kikoaac 2:14176355508a 36 * understanding how they work:
kikoaac 2:14176355508a 37 *
kikoaac 2:14176355508a 38 * http://en.wikipedia.org/wiki/PID_controller
kikoaac 2:14176355508a 39 *
kikoaac 2:14176355508a 40 * For a clear and elegant explanation of how to implement and tune a
kikoaac 2:14176355508a 41 * controller, the controlguru website by Douglas J. Cooper (who also happened
kikoaac 2:14176355508a 42 * to be Brett's controls professor) is an excellent reference:
kikoaac 2:14176355508a 43 *
kikoaac 2:14176355508a 44 * http://www.controlguru.com/
kikoaac 2:14176355508a 45 */
kikoaac 1:aac6180820a4 46
kikoaac 2:14176355508a 47 /**
kikoaac 2:14176355508a 48 * Includes
kikoaac 2:14176355508a 49 */
kikoaac 2:14176355508a 50 #include "PID.h"
kikoaac 5:3519920d064d 51 PID::PID(const PID& p)
kikoaac 2:14176355508a 52 {
kikoaac 5:3519920d064d 53 timer = p.timer;
kikoaac 5:3519920d064d 54 data = 0;
kikoaac 6:775c9421fe3b 55 bias=0;
kikoaac 5:3519920d064d 56 GAIN_P = p.GAIN_P;
kikoaac 5:3519920d064d 57 GAIN_I = p.GAIN_I;
kikoaac 5:3519920d064d 58 GAIN_D = p.GAIN_D;
kikoaac 5:3519920d064d 59 setInterval(0.001);
kikoaac 5:3519920d064d 60 s_dErrIntg=0;
kikoaac 5:3519920d064d 61 dErr_prev=0;OutputLimits(1,0);
kikoaac 5:3519920d064d 62 }
kikoaac 5:3519920d064d 63 PID::PID(float tauKp, float tauKi, float tauKd,Timer *T)
kikoaac 5:3519920d064d 64 {
kikoaac 5:3519920d064d 65 timer=T;
kikoaac 2:14176355508a 66 data = 0;
kikoaac 6:775c9421fe3b 67 bias=0;
kikoaac 2:14176355508a 68 GAIN_P = tauKp;
kikoaac 2:14176355508a 69 GAIN_I = tauKi;
kikoaac 2:14176355508a 70 GAIN_D = tauKd;
kikoaac 2:14176355508a 71 setInterval(0.001);
kikoaac 5:3519920d064d 72 s_dErrIntg=0;
kikoaac 5:3519920d064d 73 dErr_prev=0;OutputLimits(1,0);
kikoaac 5:3519920d064d 74 }
kikoaac 5:3519920d064d 75
kikoaac 5:3519920d064d 76 void PID::InputLimits(float max,float min)
kikoaac 5:3519920d064d 77 {
kikoaac 5:3519920d064d 78 //Make sure we haven't been given impossible values.
kikoaac 5:3519920d064d 79 if (min >= max) {
kikoaac 5:3519920d064d 80 return;
kikoaac 5:3519920d064d 81 }
kikoaac 5:3519920d064d 82
kikoaac 5:3519920d064d 83
kikoaac 5:3519920d064d 84 InMin = min;
kikoaac 5:3519920d064d 85 InMax = max;
kikoaac 5:3519920d064d 86 InSpan = InMax - InMin;
kikoaac 5:3519920d064d 87 }
kikoaac 5:3519920d064d 88 void PID::OutputLimits(float max,float min)
kikoaac 5:3519920d064d 89 {
kikoaac 5:3519920d064d 90 if (min >= max) {
kikoaac 5:3519920d064d 91 return;
kikoaac 5:3519920d064d 92 }
kikoaac 5:3519920d064d 93
kikoaac 5:3519920d064d 94
kikoaac 5:3519920d064d 95 OutMin = min;
kikoaac 5:3519920d064d 96 OutMax = max;
kikoaac 5:3519920d064d 97 OutSpan = OutMax - OutMin;
kikoaac 2:14176355508a 98 }
kikoaac 2:14176355508a 99 void PID::setInterval(double inter)
kikoaac 2:14176355508a 100 {
kikoaac 2:14176355508a 101 interval = inter;
kikoaac 2:14176355508a 102 //start();
kikoaac 2:14176355508a 103 }
kikoaac 5:3519920d064d 104 void PID::start()
kikoaac 2:14176355508a 105 {
kikoaac 5:3519920d064d 106 timer->start();
kikoaac 5:3519920d064d 107 //T.attach(this,&PID::PIDctrl,interval);
kikoaac 2:14176355508a 108 //printf("PID statr\n");
kikoaac 2:14176355508a 109 //wait(0.1);
kikoaac 2:14176355508a 110 //PIDctrl();
kikoaac 5:3519920d064d 111
kikoaac 2:14176355508a 112 }
kikoaac 2:14176355508a 113 void PID::stop()
kikoaac 2:14176355508a 114 {
kikoaac 5:3519920d064d 115 timer->stop();
kikoaac 5:3519920d064d 116 //T.detach();
kikoaac 2:14176355508a 117 }
kikoaac 3:34f4f22b18e7 118
kikoaac 5:3519920d064d 119 void PID::pid_reset()
kikoaac 3:34f4f22b18e7 120 {
kikoaac 5:3519920d064d 121 dTarget=0;
kikoaac 5:3519920d064d 122 dPoint=0;
kikoaac 5:3519920d064d 123 // PI制御ゲイン
kikoaac 5:3519920d064d 124 data=0;
kikoaac 5:3519920d064d 125 s_dErrIntg=0 ,dErr_prev=0;
kikoaac 3:34f4f22b18e7 126 }
kikoaac 2:14176355508a 127 void PID::PIDctrl()
kikoaac 2:14176355508a 128 {
kikoaac 2:14176355508a 129 double dErr;
kikoaac 2:14176355508a 130 double dRet;
kikoaac 5:3519920d064d 131
kikoaac 2:14176355508a 132 // 誤差
kikoaac 2:14176355508a 133 dErr = dTarget - dPoint;
kikoaac 5:3519920d064d 134 float T=gettime();
kikoaac 5:3519920d064d 135 //printf("%f\t",T);
kikoaac 5:3519920d064d 136 double dErrDiff = (dErr-data)/T;
kikoaac 2:14176355508a 137 // 誤差積分
kikoaac 5:3519920d064d 138 if(data>OutMax)s_dErrIntg=OutMax;
kikoaac 5:3519920d064d 139 else if(data<OutMin)s_dErrIntg=OutMin;
kikoaac 5:3519920d064d 140 else s_dErrIntg += (dErr+dErr_prev )* T /2.0;
kikoaac 2:14176355508a 141 // 制御入力
kikoaac 6:775c9421fe3b 142 dRet = bias+GAIN_P * dErr + GAIN_I * s_dErrIntg + GAIN_D*dErrDiff;
kikoaac 5:3519920d064d 143
kikoaac 2:14176355508a 144 dErr_prev = dErr;
kikoaac 5:3519920d064d 145 if(dRet>OutMax)data=OutMax;
kikoaac 5:3519920d064d 146 else if(dRet<OutMin)data=OutMin;
kikoaac 5:3519920d064d 147 else data = dRet;
kikoaac 5:3519920d064d 148
kikoaac 5:3519920d064d 149 //printf("PID %f,%f,%f,%f,%f\r\n",data ,dErr,s_dErrIntg,dErrDiff,timer->read());
kikoaac 5:3519920d064d 150
kikoaac 2:14176355508a 151 }
kikoaac 2:14176355508a 152
kikoaac 2:14176355508a 153
kikoaac 5:3519920d064d 154
kikoaac 6:775c9421fe3b 155