Kiko Ishimoto / PID

ＰＩＤ

Dependents:   Nucleo_spi 2015_denziben_i2c_S2 Nucleo_Motor Nucleo_Motor

Fork of PID by Aaron Berk

PID.cpp@1:aac6180820a4, 2015-06-19 (annotated)

Committer:
kikoaac
Date:
Fri Jun 19 06:36:10 2015 +0000
Revision:
1:aac6180820a4
Parent:
0:6e12a3e5af19
Child:
2:14176355508a
PID

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aberk 0:6e12a3e5af19 1 /**
aberk 0:6e12a3e5af19 2 * @author Aaron Berk
aberk 0:6e12a3e5af19 3 *
aberk 0:6e12a3e5af19 4 * @section LICENSE
aberk 0:6e12a3e5af19 5 *
aberk 0:6e12a3e5af19 6 * Copyright (c) 2010 ARM Limited
aberk 0:6e12a3e5af19 7 *
aberk 0:6e12a3e5af19 8 * Permission is hereby granted, free of charge, to any person obtaining a copy
aberk 0:6e12a3e5af19 9 * of this software and associated documentation files (the "Software"), to deal
aberk 0:6e12a3e5af19 10 * in the Software without restriction, including without limitation the rights
aberk 0:6e12a3e5af19 11 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
aberk 0:6e12a3e5af19 12 * copies of the Software, and to permit persons to whom the Software is
aberk 0:6e12a3e5af19 13 * furnished to do so, subject to the following conditions:
aberk 0:6e12a3e5af19 14 *
aberk 0:6e12a3e5af19 15 * The above copyright notice and this permission notice shall be included in
aberk 0:6e12a3e5af19 16 * all copies or substantial portions of the Software.
aberk 0:6e12a3e5af19 17 *
aberk 0:6e12a3e5af19 18 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
aberk 0:6e12a3e5af19 19 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
aberk 0:6e12a3e5af19 20 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
aberk 0:6e12a3e5af19 21 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
aberk 0:6e12a3e5af19 22 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
aberk 0:6e12a3e5af19 23 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
aberk 0:6e12a3e5af19 24 * THE SOFTWARE.
aberk 0:6e12a3e5af19 25 *
aberk 0:6e12a3e5af19 26 * @section DESCRIPTION
aberk 0:6e12a3e5af19 27 *
aberk 0:6e12a3e5af19 28 * A PID controller is a widely used feedback controller commonly found in
aberk 0:6e12a3e5af19 29 * industry.
aberk 0:6e12a3e5af19 30 *
aberk 0:6e12a3e5af19 31 * This library is a port of Brett Beauregard's Arduino PID library:
aberk 0:6e12a3e5af19 32 *
aberk 0:6e12a3e5af19 33 * http://www.arduino.cc/playground/Code/PIDLibrary
aberk 0:6e12a3e5af19 34 *
aberk 0:6e12a3e5af19 35 * The wikipedia article on PID controllers is a good place to start on
aberk 0:6e12a3e5af19 36 * understanding how they work:
aberk 0:6e12a3e5af19 37 *
aberk 0:6e12a3e5af19 38 * http://en.wikipedia.org/wiki/PID_controller
aberk 0:6e12a3e5af19 39 *
aberk 0:6e12a3e5af19 40 * For a clear and elegant explanation of how to implement and tune a
aberk 0:6e12a3e5af19 41 * controller, the controlguru website by Douglas J. Cooper (who also happened
aberk 0:6e12a3e5af19 42 * to be Brett's controls professor) is an excellent reference:
aberk 0:6e12a3e5af19 43 *
aberk 0:6e12a3e5af19 44 * http://www.controlguru.com/
aberk 0:6e12a3e5af19 45 */
aberk 0:6e12a3e5af19 46
aberk 0:6e12a3e5af19 47 /**
aberk 0:6e12a3e5af19 48 * Includes
aberk 0:6e12a3e5af19 49 */
aberk 0:6e12a3e5af19 50 #include "PID.h"
aberk 0:6e12a3e5af19 51
kikoaac 1:aac6180820a4 52 PID::PID(float tauKp, float tauKi, float tauKd)
kikoaac 1:aac6180820a4 53 {
aberk 0:6e12a3e5af19 54
kikoaac 1:aac6180820a4 55 data = 0;
kikoaac 1:aac6180820a4 56 GAIN_P = tauKp;
kikoaac 1:aac6180820a4 57 GAIN_I = tauKi;
kikoaac 1:aac6180820a4 58 GAIN_D = tauKd;
kikoaac 1:aac6180820a4 59 setInterval(0.001);
aberk 0:6e12a3e5af19 60 }
kikoaac 1:aac6180820a4 61 void PID::setInterval(double inter)
kikoaac 1:aac6180820a4 62 {
kikoaac 1:aac6180820a4 63 interval = inter;
kikoaac 1:aac6180820a4 64 start();
aberk 0:6e12a3e5af19 65 }
kikoaac 1:aac6180820a4 66 void PID::start()
kikoaac 1:aac6180820a4 67 {
kikoaac 1:aac6180820a4 68 T.attach(this,&PID::PIDctrl,interval);
kikoaac 1:aac6180820a4 69 timer.start();
kikoaac 1:aac6180820a4 70 timer.reset();
aberk 0:6e12a3e5af19 71 }
kikoaac 1:aac6180820a4 72 void PID:: stop()
kikoaac 1:aac6180820a4 73 {
kikoaac 1:aac6180820a4 74 T.detach();
kikoaac 1:aac6180820a4 75 timer.stop();
aberk 0:6e12a3e5af19 76 }
kikoaac 1:aac6180820a4 77 void PID:: PIDctrl()
kikoaac 1:aac6180820a4 78 {
kikoaac 1:aac6180820a4 79 static double s_dErrIntg = 0 ,dErr_prev=0;
kikoaac 1:aac6180820a4 80 double dErr;
kikoaac 1:aac6180820a4 81 double dRet;
kikoaac 1:aac6180820a4 82
kikoaac 1:aac6180820a4 83 // 誤差
kikoaac 1:aac6180820a4 84 dErr = dTarget - dPoint;
kikoaac 1:aac6180820a4 85
kikoaac 1:aac6180820a4 86 // 誤差積分
kikoaac 1:aac6180820a4 87 s_dErrIntg += (dErr+dErr_prev )* timer.read() /2.0;
kikoaac 1:aac6180820a4 88
kikoaac 1:aac6180820a4 89 // 制御入力
kikoaac 1:aac6180820a4 90 dRet = GAIN_P * dErr + GAIN_I * s_dErrIntg + GAIN_D*(dErr-dErr_prev)/timer.read();
kikoaac 1:aac6180820a4 91 timer.reset();
kikoaac 1:aac6180820a4 92 dErr_prev = dErr;
kikoaac 1:aac6180820a4 93 data = dRet;
aberk 0:6e12a3e5af19 94 }
kikoaac 1:aac6180820a4 95