File content as of revision 1:32c9f196607d:

/**
 * @author Aaron Berk
 *
 * @section LICENSE
 *
 * Copyright (c) 2010 ARM Limited
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 *
 * @section DESCRIPTION
 * 
 * A PID controller is a widely used feedback controller commonly found in
 * industry.
 *
 * This library is a port of Brett Beauregard's Arduino PID library:
 *
 *  http://www.arduino.cc/playground/Code/PIDLibrary
 *
 * The wikipedia article on PID controllers is a good place to start on
 * understanding how they work:
 *
 *  http://en.wikipedia.org/wiki/PID_controller
 *
 * For a clear and elegant explanation of how to implement and tune a
 * controller, the controlguru website by Douglas J. Cooper (who also happened
 * to be Brett's controls professor) is an excellent reference:
 *
 *  http://www.controlguru.com/
 */

/**
 * Includes
 */
#include "PID.h"
#include "mbed.h"

PID::PID(double* Input, double* Output, double* Setpoint,
        double Kp, double Ki, double Kd, int ControllerDirection, float* NOWTIME, float* LASTTIME)
{
  
    myOutput = Output;
    myInput = Input;
    mySetpoint = Setpoint;
  inAuto = false;
  
  PID::SetOutputLimits(0, 387);       //default output limit corresponds to 
                        //the arduino pwm limits

    SampleTime =0.01;             //default Controller Sample Time is 0.1 seconds

    PID::SetControllerDirection(ControllerDirection);
    PID::SetTunings(Kp, Ki, Kd);
    //float TIME=* NOWTIME
    lastTime =* NOWTIME-SampleTime;                                                 ///*******************
    //lastTime =0;                                                 ///*******************
}

void PID::SetSampleTime(int NewSampleTime)
{
   if (NewSampleTime > 0)
   {
      double ratio  = (double)NewSampleTime
                      / (double)SampleTime;
      ki *= ratio;
      kd /= ratio;
      SampleTime = (unsigned long)NewSampleTime;
   }
}

void PID::SetOutputLimits(double Min, double Max)
{
   if(Min >= Max) return;
   outMin = Min;
   outMax = Max;
 
   if(inAuto)
   {
     if( *myOutput > outMax ) *myOutput = outMax;
     else if( *myOutput < outMin ) *myOutput = outMin;
   
     if(ITerm > outMax) ITerm= outMax;
     else if(ITerm < outMin) ITerm= outMin;
   }
}

void PID::SetTunings(double Kp, double Ki, double Kd)
{
   if (Kp<0 || Ki<0 || Kd<0) return;
 
   dispKp = Kp; dispKi = Ki; dispKd = Kd;
   
   double SampleTimeInSec = ((double)SampleTime)/1000;  
   kp = Kp;
   ki = Ki * SampleTimeInSec;
   kd = Kd / SampleTimeInSec;
 
  if(controllerDirection ==REVERSE)
   {
      kp = (0 - kp);
      ki = (0 - ki);
      kd = (0 - kd);
   }
}

void PID::Initialize()
{
   ITerm = *myOutput;
   lastInput = *myInput;
   if(ITerm > outMax) ITerm = outMax;
   else if(ITerm < outMin) ITerm = outMin;
}

void PID::SetMode(int Mode)
{
    bool newAuto = (Mode == AUTOMATIC);
    if(newAuto == !inAuto)
    {  /*we just went from manual to auto*/
        PID::Initialize();
    }
    inAuto = newAuto;
}

void PID::SetControllerDirection(int Direction)
{
   if(inAuto && Direction !=controllerDirection)
   {
    kp = (0 - kp);
      ki = (0 - ki);
      kd = (0 - kd);
   }   
   controllerDirection = Direction;
}

double PID::Compute(float* now)
{

   float timeChange = (* now - lastTime);
   //Serial.print("  timeChange: ");
   //Serial.print(timeChange);
   if(timeChange>=SampleTime)
   {
      /*Compute all the working error variables*/
      double input = *myInput;
      double error = *mySetpoint - input;
      ITerm+= (dispKi * error);
      if(abs(ITerm/error) > outMax) ITerm= error*outMax;
      else if(abs(ITerm/error) < outMin) ITerm= error*outMin;
      double dInput = (input - lastInput);
 
      /*Compute PID Output*/
      //double output = dispKp * error + ITerm- dispKd * dInput;
      double output =(1+dispKd*dInput)*(dispKp*error + dispKi*ITerm);
      //if(abs(output/(*mySetpoint)) > outMax) output= (*mySetpoint)*outMax;
      //else if(abs(output/(*mySetpoint)) < outMin) output= (*mySetpoint)*outMin;
      *myOutput = output + input;
      //printf("**********************mySetpoint:%.3f\n",*mySetpoint);
      //printf("**********************input:%.3f\n",input);
      //printf("**********************myOutput:%.3f\n",*myOutput);
      if( *myOutput > outMax ) *myOutput = outMax;
        else if( *myOutput < outMin ) *myOutput = outMin;
      //printf("**********************myOutput:%.3f\n",*myOutput);
      //printf("********PID do**************\n");
      /*Remember some variables for next time*/
      lastInput = input;
      lastTime = * now;
    }
}