Kiko Ishimoto / PID

ＰＩＤ

Dependents:   Nucleo_spi 2015_denziben_i2c_S2 Nucleo_Motor Nucleo_Motor

Fork of PID by Aaron Berk

PID.cpp@3:34f4f22b18e7, 2015-08-18 (annotated)

Committer:
kikoaac
Date:
Tue Aug 18 04:45:10 2015 +0000
Revision:
3:34f4f22b18e7
Parent:
2:14176355508a
Child:
4:a3c85727f0f6
Child:
5:3519920d064d
PID

;

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 2:14176355508a 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 2:14176355508a 51
kikoaac 2:14176355508a 52 PID::PID(float tauKp, float tauKi, float tauKd)
kikoaac 2:14176355508a 53 {
kikoaac 2:14176355508a 54
kikoaac 2:14176355508a 55 data = 0;
kikoaac 2:14176355508a 56 GAIN_P = tauKp;
kikoaac 2:14176355508a 57 GAIN_I = tauKi;
kikoaac 2:14176355508a 58 GAIN_D = tauKd;
kikoaac 2:14176355508a 59 setInterval(0.001);
kikoaac 2:14176355508a 60 s_dErrIntg=0; dErr_prev=0;
kikoaac 2:14176355508a 61 }
kikoaac 2:14176355508a 62 void PID::setInterval(double inter)
kikoaac 2:14176355508a 63 {
kikoaac 2:14176355508a 64 interval = inter;
kikoaac 2:14176355508a 65 //start();
kikoaac 2:14176355508a 66 }
kikoaac 2:14176355508a 67 void PID::Start()
kikoaac 2:14176355508a 68 {
kikoaac 2:14176355508a 69 timer.start();
kikoaac 2:14176355508a 70 T.attach(this,&PID::PIDctrl,interval);
kikoaac 2:14176355508a 71 //printf("PID statr\n");
kikoaac 2:14176355508a 72 //wait(0.1);
kikoaac 2:14176355508a 73 //PIDctrl();
kikoaac 2:14176355508a 74
kikoaac 2:14176355508a 75 }
kikoaac 2:14176355508a 76 void PID::stop()
kikoaac 2:14176355508a 77 {
kikoaac 2:14176355508a 78 timer.stop();
kikoaac 2:14176355508a 79 T.detach();
kikoaac 2:14176355508a 80 }
kikoaac 3:34f4f22b18e7 81
kikoaac 3:34f4f22b18e7 82 void PID::Reset()
kikoaac 3:34f4f22b18e7 83 {
kikoaac 3:34f4f22b18e7 84 dTarget=0;
kikoaac 3:34f4f22b18e7 85 dPoint=0;
kikoaac 3:34f4f22b18e7 86 // PI制御ゲイン
kikoaac 3:34f4f22b18e7 87 data=0;
kikoaac 3:34f4f22b18e7 88 s_dErrIntg=0 ,dErr_prev=0;
kikoaac 3:34f4f22b18e7 89 }
kikoaac 2:14176355508a 90 void PID::PIDctrl()
kikoaac 2:14176355508a 91 {
kikoaac 2:14176355508a 92 double dErr;
kikoaac 2:14176355508a 93 double dRet;
kikoaac 2:14176355508a 94
kikoaac 2:14176355508a 95 // 誤差
kikoaac 2:14176355508a 96 dErr = dTarget - dPoint;
kikoaac 2:14176355508a 97
kikoaac 2:14176355508a 98 double dErrDiff = (dErr-dErr_prev)/timer.read();
kikoaac 2:14176355508a 99 //printf("PID %f,%f,%f,%f\n",dErr,s_dErrIntg,dErrDiff,timer.read());
kikoaac 2:14176355508a 100 // 誤差積分
kikoaac 2:14176355508a 101 s_dErrIntg += (dErr+dErr_prev )* timer.read() /2.0;
kikoaac 2:14176355508a 102 // 制御入力
kikoaac 2:14176355508a 103 dRet = GAIN_P * dErr + GAIN_I * s_dErrIntg + GAIN_D*dErrDiff;
kikoaac 2:14176355508a 104 timer.reset();
kikoaac 2:14176355508a 105 dErr_prev = dErr;
kikoaac 2:14176355508a 106 data = dRet;
kikoaac 2:14176355508a 107 }
kikoaac 2:14176355508a 108
kikoaac 2:14176355508a 109