Kiko Ishimoto / PIDonerobot

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   OneCircleRobot

Fork of PID by Kiko Ishimoto

PID.cpp@4:a3c85727f0f6, 2015-09-14 (annotated)

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