Samuel Lin / PID

1

Fork of PID by Aaron Berk

PID.cpp@1:28542afc96d0, 2017-02-15 (annotated)

Committer:
d15321854
Date:
Wed Feb 15 12:33:10 2017 +0000
Revision:
1:28542afc96d0
Parent:
0:6e12a3e5af19 
123

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"
d15321854 1:28542afc96d0 51 #include "mbed.h"
aberk 0:6e12a3e5af19 52
d15321854 1:28542afc96d0 53 PID::PID(double* Input, double* Output, double* Setpoint,
d15321854 1:28542afc96d0 54 double Kp, double Ki, double Kd, int ControllerDirection, float* NOWTIME, float* LASTTIME)
d15321854 1:28542afc96d0 55 {
d15321854 1:28542afc96d0 56
d15321854 1:28542afc96d0 57 myOutput = Output;
d15321854 1:28542afc96d0 58 myInput = Input;
d15321854 1:28542afc96d0 59 mySetpoint = Setpoint;
d15321854 1:28542afc96d0 60 inAuto = false;
d15321854 1:28542afc96d0 61
d15321854 1:28542afc96d0 62 PID::SetOutputLimits(0, 387); //default output limit corresponds to
d15321854 1:28542afc96d0 63 //the arduino pwm limits
aberk 0:6e12a3e5af19 64
d15321854 1:28542afc96d0 65 SampleTime =0.01; //default Controller Sample Time is 0.1 seconds
aberk 0:6e12a3e5af19 66
d15321854 1:28542afc96d0 67 PID::SetControllerDirection(ControllerDirection);
d15321854 1:28542afc96d0 68 PID::SetTunings(Kp, Ki, Kd);
d15321854 1:28542afc96d0 69 //float TIME=* NOWTIME
d15321854 1:28542afc96d0 70 lastTime =* NOWTIME-SampleTime; ///*******************
d15321854 1:28542afc96d0 71 //lastTime =0; ///*******************
aberk 0:6e12a3e5af19 72 }
aberk 0:6e12a3e5af19 73
d15321854 1:28542afc96d0 74 void PID::SetSampleTime(int NewSampleTime)
d15321854 1:28542afc96d0 75 {
d15321854 1:28542afc96d0 76 if (NewSampleTime > 0)
d15321854 1:28542afc96d0 77 {
d15321854 1:28542afc96d0 78 double ratio = (double)NewSampleTime
d15321854 1:28542afc96d0 79 / (double)SampleTime;
d15321854 1:28542afc96d0 80 ki *= ratio;
d15321854 1:28542afc96d0 81 kd /= ratio;
d15321854 1:28542afc96d0 82 SampleTime = (unsigned long)NewSampleTime;
d15321854 1:28542afc96d0 83 }
aberk 0:6e12a3e5af19 84 }
aberk 0:6e12a3e5af19 85
d15321854 1:28542afc96d0 86 void PID::SetOutputLimits(double Min, double Max)
d15321854 1:28542afc96d0 87 {
d15321854 1:28542afc96d0 88 if(Min >= Max) return;
d15321854 1:28542afc96d0 89 outMin = Min;
d15321854 1:28542afc96d0 90 outMax = Max;
d15321854 1:28542afc96d0 91
d15321854 1:28542afc96d0 92 if(inAuto)
d15321854 1:28542afc96d0 93 {
d15321854 1:28542afc96d0 94 if( *myOutput > outMax ) *myOutput = outMax;
d15321854 1:28542afc96d0 95 else if( *myOutput < outMin ) *myOutput = outMin;
d15321854 1:28542afc96d0 96
d15321854 1:28542afc96d0 97 if(ITerm > outMax) ITerm= outMax;
d15321854 1:28542afc96d0 98 else if(ITerm < outMin) ITerm= outMin;
d15321854 1:28542afc96d0 99 }
aberk 0:6e12a3e5af19 100 }
aberk 0:6e12a3e5af19 101
d15321854 1:28542afc96d0 102 void PID::SetTunings(double Kp, double Ki, double Kd)
d15321854 1:28542afc96d0 103 {
d15321854 1:28542afc96d0 104 if (Kp<0 || Ki<0 || Kd<0) return;
d15321854 1:28542afc96d0 105
d15321854 1:28542afc96d0 106 dispKp = Kp; dispKi = Ki; dispKd = Kd;
d15321854 1:28542afc96d0 107
d15321854 1:28542afc96d0 108 double SampleTimeInSec = ((double)SampleTime)/1000;
d15321854 1:28542afc96d0 109 kp = Kp;
d15321854 1:28542afc96d0 110 ki = Ki * SampleTimeInSec;
d15321854 1:28542afc96d0 111 kd = Kd / SampleTimeInSec;
d15321854 1:28542afc96d0 112
d15321854 1:28542afc96d0 113 if(controllerDirection ==REVERSE)
d15321854 1:28542afc96d0 114 {
d15321854 1:28542afc96d0 115 kp = (0 - kp);
d15321854 1:28542afc96d0 116 ki = (0 - ki);
d15321854 1:28542afc96d0 117 kd = (0 - kd);
d15321854 1:28542afc96d0 118 }
aberk 0:6e12a3e5af19 119 }
aberk 0:6e12a3e5af19 120
d15321854 1:28542afc96d0 121 void PID::Initialize()
d15321854 1:28542afc96d0 122 {
d15321854 1:28542afc96d0 123 ITerm = *myOutput;
d15321854 1:28542afc96d0 124 lastInput = *myInput;
d15321854 1:28542afc96d0 125 if(ITerm > outMax) ITerm = outMax;
d15321854 1:28542afc96d0 126 else if(ITerm < outMin) ITerm = outMin;
aberk 0:6e12a3e5af19 127 }
aberk 0:6e12a3e5af19 128
d15321854 1:28542afc96d0 129 void PID::SetMode(int Mode)
d15321854 1:28542afc96d0 130 {
d15321854 1:28542afc96d0 131 bool newAuto = (Mode == AUTOMATIC);
d15321854 1:28542afc96d0 132 if(newAuto == !inAuto)
d15321854 1:28542afc96d0 133 { /*we just went from manual to auto*/
d15321854 1:28542afc96d0 134 PID::Initialize();
d15321854 1:28542afc96d0 135 }
d15321854 1:28542afc96d0 136 inAuto = newAuto;
aberk 0:6e12a3e5af19 137 }
aberk 0:6e12a3e5af19 138
d15321854 1:28542afc96d0 139 void PID::SetControllerDirection(int Direction)
d15321854 1:28542afc96d0 140 {
d15321854 1:28542afc96d0 141 if(inAuto && Direction !=controllerDirection)
d15321854 1:28542afc96d0 142 {
d15321854 1:28542afc96d0 143 kp = (0 - kp);
d15321854 1:28542afc96d0 144 ki = (0 - ki);
d15321854 1:28542afc96d0 145 kd = (0 - kd);
d15321854 1:28542afc96d0 146 }
d15321854 1:28542afc96d0 147 controllerDirection = Direction;
aberk 0:6e12a3e5af19 148 }
aberk 0:6e12a3e5af19 149
d15321854 1:28542afc96d0 150 double PID::Compute(float* now)
d15321854 1:28542afc96d0 151 {
aberk 0:6e12a3e5af19 152
d15321854 1:28542afc96d0 153 float timeChange = (* now - lastTime);
d15321854 1:28542afc96d0 154 //Serial.print(" timeChange: ");
d15321854 1:28542afc96d0 155 //Serial.print(timeChange);
d15321854 1:28542afc96d0 156 if(timeChange>=SampleTime)
d15321854 1:28542afc96d0 157 {
d15321854 1:28542afc96d0 158 /*Compute all the working error variables*/
d15321854 1:28542afc96d0 159 double input = *myInput;
d15321854 1:28542afc96d0 160 double error = *mySetpoint - input;
d15321854 1:28542afc96d0 161 ITerm+= (dispKi * error);
d15321854 1:28542afc96d0 162 if(abs(ITerm/error) > outMax) ITerm= error*outMax;
d15321854 1:28542afc96d0 163 else if(abs(ITerm/error) < outMin) ITerm= error*outMin;
d15321854 1:28542afc96d0 164 double dInput = (input - lastInput);
d15321854 1:28542afc96d0 165
d15321854 1:28542afc96d0 166 /*Compute PID Output*/
d15321854 1:28542afc96d0 167 //double output = dispKp * error + ITerm- dispKd * dInput;
d15321854 1:28542afc96d0 168 double output =(1+dispKd*dInput)*(dispKp*error + dispKi*ITerm);
d15321854 1:28542afc96d0 169 //if(abs(output/(*mySetpoint)) > outMax) output= (*mySetpoint)*outMax;
d15321854 1:28542afc96d0 170 //else if(abs(output/(*mySetpoint)) < outMin) output= (*mySetpoint)*outMin;
d15321854 1:28542afc96d0 171 *myOutput = output + input;
d15321854 1:28542afc96d0 172 //printf("**********************mySetpoint:%.3f\n",*mySetpoint);
d15321854 1:28542afc96d0 173 //printf("**********************input:%.3f\n",input);
d15321854 1:28542afc96d0 174 //printf("**********************myOutput:%.3f\n",*myOutput);
d15321854 1:28542afc96d0 175 if( *myOutput > outMax ) *myOutput = outMax;
d15321854 1:28542afc96d0 176 else if( *myOutput < outMin ) *myOutput = outMin;
d15321854 1:28542afc96d0 177 //printf("**********************myOutput:%.3f\n",*myOutput);
d15321854 1:28542afc96d0 178 //printf("********PID do**************\n");
d15321854 1:28542afc96d0 179 /*Remember some variables for next time*/
d15321854 1:28542afc96d0 180 lastInput = input;
d15321854 1:28542afc96d0 181 lastTime = * now;
d15321854 1:28542afc96d0 182 }
d15321854 1:28542afc96d0 183 }