PID.cpp

00001 /**
00002  * @author Aaron Berk
00003  *
00004  * @section LICENSE
00005  *
00006  * Copyright (c) 2010 ARM Limited
00007  *
00008  * Permission is hereby granted, free of charge, to any person obtaining a copy
00009  * of this software and associated documentation files (the "Software"), to deal
00010  * in the Software without restriction, including without limitation the rights
00011  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00012  * copies of the Software, and to permit persons to whom the Software is
00013  * furnished to do so, subject to the following conditions:
00014  *
00015  * The above copyright notice and this permission notice shall be included in
00016  * all copies or substantial portions of the Software.
00017  *
00018  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00019  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00020  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00021  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00022  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00023  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00024  * THE SOFTWARE.
00025  *
00026  * @section DESCRIPTION
00027  * 
00028  * A PID controller is a widely used feedback controller commonly found in
00029  * industry.
00030  *
00031  * This library is a port of Brett Beauregard's Arduino PID library:
00032  *
00033  *  http://www.arduino.cc/playground/Code/PIDLibrary
00034  *
00035  * The wikipedia article on PID controllers is a good place to start on
00036  * understanding how they work:
00037  *
00038  *  http://en.wikipedia.org/wiki/PID_controller
00039  *
00040  * For a clear and elegant explanation of how to implement and tune a
00041  * controller, the controlguru website by Douglas J. Cooper (who also happened
00042  * to be Brett's controls professor) is an excellent reference:
00043  *
00044  *  http://www.controlguru.com/
00045  */
00046 
00047 /**
00048  * Includes
00049  */
00050 #include "PID.h"
00051 #include "mbed.h"
00052 
00053 PID::PID(double* Input, double* Output, double* Setpoint,
00054         double Kp, double Ki, double Kd, int ControllerDirection, float* NOWTIME, float* LASTTIME)
00055 {
00056   
00057     myOutput = Output;
00058     myInput = Input;
00059     mySetpoint = Setpoint;
00060   inAuto = false;
00061   
00062   PID::SetOutputLimits(0, 387);       //default output limit corresponds to 
00063                         //the arduino pwm limits
00064 
00065     SampleTime =0.01;             //default Controller Sample Time is 0.1 seconds
00066 
00067     PID::SetControllerDirection(ControllerDirection);
00068     PID::SetTunings(Kp, Ki, Kd);
00069     //float TIME=* NOWTIME
00070     lastTime =* NOWTIME-SampleTime;                                                 ///*******************
00071     //lastTime =0;                                                 ///*******************
00072 }
00073 
00074 void PID::SetSampleTime(int NewSampleTime)
00075 {
00076    if (NewSampleTime > 0)
00077    {
00078       double ratio  = (double)NewSampleTime
00079                       / (double)SampleTime;
00080       ki *= ratio;
00081       kd /= ratio;
00082       SampleTime = (unsigned long)NewSampleTime;
00083    }
00084 }
00085 
00086 void PID::SetOutputLimits(double Min, double Max)
00087 {
00088    if(Min >= Max) return;
00089    outMin = Min;
00090    outMax = Max;
00091  
00092    if(inAuto)
00093    {
00094      if( *myOutput > outMax ) *myOutput = outMax;
00095      else if( *myOutput < outMin ) *myOutput = outMin;
00096    
00097      if(ITerm > outMax) ITerm= outMax;
00098      else if(ITerm < outMin) ITerm= outMin;
00099    }
00100 }
00101 
00102 void PID::SetTunings(double Kp, double Ki, double Kd)
00103 {
00104    if (Kp<0 || Ki<0 || Kd<0) return;
00105  
00106    dispKp = Kp; dispKi = Ki; dispKd = Kd;
00107    
00108    double SampleTimeInSec = ((double)SampleTime)/1000;  
00109    kp = Kp;
00110    ki = Ki * SampleTimeInSec;
00111    kd = Kd / SampleTimeInSec;
00112  
00113   if(controllerDirection ==REVERSE)
00114    {
00115       kp = (0 - kp);
00116       ki = (0 - ki);
00117       kd = (0 - kd);
00118    }
00119 }
00120 
00121 void PID::Initialize()
00122 {
00123    ITerm = *myOutput;
00124    lastInput = *myInput;
00125    if(ITerm > outMax) ITerm = outMax;
00126    else if(ITerm < outMin) ITerm = outMin;
00127 }
00128 
00129 void PID::SetMode(int Mode)
00130 {
00131     bool newAuto = (Mode == AUTOMATIC);
00132     if(newAuto == !inAuto)
00133     {  /*we just went from manual to auto*/
00134         PID::Initialize();
00135     }
00136     inAuto = newAuto;
00137 }
00138 
00139 void PID::SetControllerDirection(int Direction)
00140 {
00141    if(inAuto && Direction !=controllerDirection)
00142    {
00143     kp = (0 - kp);
00144       ki = (0 - ki);
00145       kd = (0 - kd);
00146    }   
00147    controllerDirection = Direction;
00148 }
00149 
00150 double PID::Compute(float* now)
00151 {
00152 
00153    float timeChange = (* now - lastTime);
00154    //Serial.print("  timeChange: ");
00155    //Serial.print(timeChange);
00156    if(timeChange>=SampleTime)
00157    {
00158       /*Compute all the working error variables*/
00159       double input = *myInput;
00160       double error = *mySetpoint - input;
00161       ITerm+= (dispKi * error);
00162       if(abs(ITerm/error) > outMax) ITerm= error*outMax;
00163       else if(abs(ITerm/error) < outMin) ITerm= error*outMin;
00164       double dInput = (input - lastInput);
00165  
00166       /*Compute PID Output*/
00167       //double output = dispKp * error + ITerm- dispKd * dInput;
00168       double output =(1+dispKd*dInput)*(dispKp*error + dispKi*ITerm);
00169       //if(abs(output/(*mySetpoint)) > outMax) output= (*mySetpoint)*outMax;
00170       //else if(abs(output/(*mySetpoint)) < outMin) output= (*mySetpoint)*outMin;
00171       *myOutput = output + input;
00172       //printf("**********************mySetpoint:%.3f\n",*mySetpoint);
00173       //printf("**********************input:%.3f\n",input);
00174       //printf("**********************myOutput:%.3f\n",*myOutput);
00175       if( *myOutput > outMax ) *myOutput = outMax;
00176         else if( *myOutput < outMin ) *myOutput = outMin;
00177       //printf("**********************myOutput:%.3f\n",*myOutput);
00178       //printf("********PID do**************\n");
00179       /*Remember some variables for next time*/
00180       lastInput = input;
00181       lastTime = * now;
00182     }
00183 }