PID
Dependents: Nucleo_spi 2015_denziben_i2c_S2 Nucleo_Motor Nucleo_Motor
Fork of PID by
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?
User | Revision | Line number | New 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 |