PID
Dependents: Nucleo_spi 2015_denziben_i2c_S2 Nucleo_Motor Nucleo_Motor
Fork of PID by
PID.cpp@2:14176355508a, 2015-07-21 (annotated)
- Committer:
- kikoaac
- Date:
- Tue Jul 21 08:12:24 2015 +0000
- Revision:
- 2:14176355508a
- Parent:
- 1:aac6180820a4
- Child:
- 3:34f4f22b18e7
PID
;
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 | 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 | 2:14176355508a | 81 | void PID::PIDctrl() |
kikoaac | 2:14176355508a | 82 | { |
kikoaac | 2:14176355508a | 83 | double dErr; |
kikoaac | 2:14176355508a | 84 | double dRet; |
kikoaac | 2:14176355508a | 85 | |
kikoaac | 2:14176355508a | 86 | // 誤差 |
kikoaac | 2:14176355508a | 87 | dErr = dTarget - dPoint; |
kikoaac | 2:14176355508a | 88 | |
kikoaac | 2:14176355508a | 89 | double dErrDiff = (dErr-dErr_prev)/timer.read(); |
kikoaac | 2:14176355508a | 90 | //printf("PID %f,%f,%f,%f\n",dErr,s_dErrIntg,dErrDiff,timer.read()); |
kikoaac | 2:14176355508a | 91 | // 誤差積分 |
kikoaac | 2:14176355508a | 92 | s_dErrIntg += (dErr+dErr_prev )* timer.read() /2.0; |
kikoaac | 2:14176355508a | 93 | // 制御入力 |
kikoaac | 2:14176355508a | 94 | dRet = GAIN_P * dErr + GAIN_I * s_dErrIntg + GAIN_D*dErrDiff; |
kikoaac | 2:14176355508a | 95 | timer.reset(); |
kikoaac | 2:14176355508a | 96 | dErr_prev = dErr; |
kikoaac | 2:14176355508a | 97 | data = dRet; |
kikoaac | 2:14176355508a | 98 | } |
kikoaac | 2:14176355508a | 99 | |
kikoaac | 2:14176355508a | 100 |