Kiko Ishimoto / PIDonerobot

pid

Dependents:   OneCircleRobot

Fork of PID by Kiko Ishimoto

PID.cpp@7:66e3f3fd08d8, 2016-08-07 (annotated)

Committer:
kikoaac
Date:
Sun Aug 07 12:47:39 2016 +0000
Revision:
7:66e3f3fd08d8
Parent:
6:775c9421fe3b 
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 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 7:66e3f3fd08d8 56
kikoaac 7:66e3f3fd08d8 57 intF = false;
kikoaac 7:66e3f3fd08d8 58 diffF = false;
kikoaac 7:66e3f3fd08d8 59 propF = false;
kikoaac 5:3519920d064d 60 GAIN_P = p.GAIN_P;
kikoaac 5:3519920d064d 61 GAIN_I = p.GAIN_I;
kikoaac 5:3519920d064d 62 GAIN_D = p.GAIN_D;
kikoaac 7:66e3f3fd08d8 63 setInterval(0.01);
kikoaac 5:3519920d064d 64 s_dErrIntg=0;
kikoaac 5:3519920d064d 65 dErr_prev=0;OutputLimits(1,0);
kikoaac 5:3519920d064d 66 }
kikoaac 5:3519920d064d 67 PID::PID(float tauKp, float tauKi, float tauKd,Timer *T)
kikoaac 5:3519920d064d 68 {
kikoaac 5:3519920d064d 69 timer=T;
kikoaac 2:14176355508a 70 data = 0;
kikoaac 6:775c9421fe3b 71 bias=0;
kikoaac 2:14176355508a 72 GAIN_P = tauKp;
kikoaac 2:14176355508a 73 GAIN_I = tauKi;
kikoaac 2:14176355508a 74 GAIN_D = tauKd;
kikoaac 7:66e3f3fd08d8 75 setInterval(0.01);
kikoaac 5:3519920d064d 76 s_dErrIntg=0;
kikoaac 5:3519920d064d 77 dErr_prev=0;OutputLimits(1,0);
kikoaac 5:3519920d064d 78 }
kikoaac 5:3519920d064d 79
kikoaac 5:3519920d064d 80 void PID::InputLimits(float max,float min)
kikoaac 5:3519920d064d 81 {
kikoaac 5:3519920d064d 82 //Make sure we haven't been given impossible values.
kikoaac 5:3519920d064d 83 if (min >= max) {
kikoaac 5:3519920d064d 84 return;
kikoaac 5:3519920d064d 85 }
kikoaac 5:3519920d064d 86
kikoaac 5:3519920d064d 87
kikoaac 5:3519920d064d 88 InMin = min;
kikoaac 5:3519920d064d 89 InMax = max;
kikoaac 5:3519920d064d 90 InSpan = InMax - InMin;
kikoaac 5:3519920d064d 91 }
kikoaac 5:3519920d064d 92 void PID::OutputLimits(float max,float min)
kikoaac 5:3519920d064d 93 {
kikoaac 5:3519920d064d 94 if (min >= max) {
kikoaac 5:3519920d064d 95 return;
kikoaac 5:3519920d064d 96 }
kikoaac 5:3519920d064d 97
kikoaac 5:3519920d064d 98
kikoaac 5:3519920d064d 99 OutMin = min;
kikoaac 5:3519920d064d 100 OutMax = max;
kikoaac 5:3519920d064d 101 OutSpan = OutMax - OutMin;
kikoaac 2:14176355508a 102 }
kikoaac 7:66e3f3fd08d8 103 void PID::setInterval(float inter)
kikoaac 2:14176355508a 104 {
kikoaac 2:14176355508a 105 interval = inter;
kikoaac 2:14176355508a 106 //start();
kikoaac 2:14176355508a 107 }
kikoaac 5:3519920d064d 108 void PID::start()
kikoaac 2:14176355508a 109 {
kikoaac 5:3519920d064d 110 timer->start();
kikoaac 7:66e3f3fd08d8 111 T.attach(this,&PID::PIDctrl,interval);
kikoaac 2:14176355508a 112 //printf("PID statr\n");
kikoaac 2:14176355508a 113 //wait(0.1);
kikoaac 2:14176355508a 114 //PIDctrl();
kikoaac 5:3519920d064d 115
kikoaac 2:14176355508a 116 }
kikoaac 2:14176355508a 117 void PID::stop()
kikoaac 2:14176355508a 118 {
kikoaac 5:3519920d064d 119 timer->stop();
kikoaac 5:3519920d064d 120 //T.detach();
kikoaac 2:14176355508a 121 }
kikoaac 3:34f4f22b18e7 122
kikoaac 5:3519920d064d 123 void PID::pid_reset()
kikoaac 3:34f4f22b18e7 124 {
kikoaac 5:3519920d064d 125 dTarget=0;
kikoaac 5:3519920d064d 126 dPoint=0;
kikoaac 5:3519920d064d 127 // PI制御ゲイン
kikoaac 5:3519920d064d 128 data=0;
kikoaac 5:3519920d064d 129 s_dErrIntg=0 ,dErr_prev=0;
kikoaac 3:34f4f22b18e7 130 }
kikoaac 2:14176355508a 131 void PID::PIDctrl()
kikoaac 2:14176355508a 132 {
kikoaac 2:14176355508a 133 double dErr;
kikoaac 2:14176355508a 134 double dRet;
kikoaac 5:3519920d064d 135
kikoaac 2:14176355508a 136 // 誤差
kikoaac 2:14176355508a 137 dErr = dTarget - dPoint;
kikoaac 5:3519920d064d 138 float T=gettime();
kikoaac 5:3519920d064d 139 //printf("%f\t",T);
kikoaac 7:66e3f3fd08d8 140 double dErrDiff;
kikoaac 2:14176355508a 141 // 誤差積分
kikoaac 7:66e3f3fd08d8 142 /*if(GAIN_I*s_dErrIntg>OutMax)s_dErrIntg=GAIN_I*OutMax;
kikoaac 7:66e3f3fd08d8 143 else if(GAIN_I*s_dErrIntg<OutMin)s_dErrIntg=GAIN_I*OutMin;
kikoaac 7:66e3f3fd08d8 144 else */
kikoaac 7:66e3f3fd08d8 145 s_dErrIntg += (dErr+dErr_prev )* T /2.0;
kikoaac 2:14176355508a 146 // 制御入力
kikoaac 7:66e3f3fd08d8 147 dRet = bias + GAIN_P * (propF == true ? userProp : dErr) +
kikoaac 7:66e3f3fd08d8 148 GAIN_I * (intF == true ? userInt : s_dErrIntg) -
kikoaac 7:66e3f3fd08d8 149 GAIN_D * (diffF == true ? userDiff : dErrDiff);
kikoaac 5:3519920d064d 150
kikoaac 2:14176355508a 151 dErr_prev = dErr;
kikoaac 7:66e3f3fd08d8 152 dErrDiff = (dErr-data)/T;
kikoaac 5:3519920d064d 153 if(dRet>OutMax)data=OutMax;
kikoaac 5:3519920d064d 154 else if(dRet<OutMin)data=OutMin;
kikoaac 5:3519920d064d 155 else data = dRet;
kikoaac 5:3519920d064d 156
kikoaac 7:66e3f3fd08d8 157 //printf("PID %3.3f %3.3f %3.3f %3.3f %3.3f\r\n",data ,GAIN_P*dErr,GAIN_I*s_dErrIntg,GAIN_D*dErrDiff,T);
kikoaac 5:3519920d064d 158
kikoaac 2:14176355508a 159 }
kikoaac 2:14176355508a 160
kikoaac 2:14176355508a 161
kikoaac 5:3519920d064d 162
kikoaac 6:775c9421fe3b 163