Amber Tang / PID01

123

Fork of PID by Aaron Berk

PID.cpp@1:6b78c8331ed9, 2017-08-30 (annotated)

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