/**************************************************************************************************
---------------------------------------------------------------------------------------------------
	Copyright (c) 2004-2008, Jonathan Bagg
	All rights reserved.

	 Redistribution and use in source and binary forms, with or without modification, are permitted 
	 provided that the following conditions are met:

    * Redistributions of source code must retain the above copyright notice, this list of 
	  conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright notice, this list of 
	  conditions and the following disclaimer in the documentation and/or other materials provided 
	  with the distribution.
    * Neither the name of Jonathan Bagg nor the names of its contributors may be used to 
	  endorse or promote products derived from this software without specific prior written permission.

  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR 
  IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY 
  AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR 
  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 
  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 
  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR 
  OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
  POSSIBILITY OF SUCH DAMAGE.
---------------------------------------------------------------------------------------------------
   Project name : Infidigm AVR Drivers
   Processor	: ATMega64, ATMega128
   File name    : pid.h
---------------------------------------------------------------------------------------------------
   Modifications: 

   Revision 1.0  2004/07/23 	Bagg
   - Finished for release
   
   Revision 1.1  2008/04/26 	Bagg
   - Added variable to cap maximum accumulated error
---------------------------------------------------------------------------------------------------
   Created      : 10 April 2004                Author(s) : Jonathan Bagg
---------------------------------------------------------------------------------------------------
   Pid Controller 
---------------------------------------------------------------------------------------------------
**************************************************************************************************/
#include "pid.h"

/**************************************************************************************************
*   pid(); - See 'pid.h' Header file for Description
**************************************************************************************************/
signed int pid(struct pidvars *piddata)
{
 signed int error;
 signed long pterm, iterm, dterm;
  
//Calculate error
 error = piddata->setpoint - piddata->input;
 
//Check if outside of deadband
 if ((error < piddata->deadband) && (error > -piddata->deadband)) 
	error = 0 ;

//Limit error
 if (error > piddata->limit) 
	error = piddata->limit;
 if (error < -piddata->limit) 
	error = -piddata->limit;

//Store Accumulated error (and cap at max_accum)
 if ((piddata->accumerror + error) < piddata->max_accum 
	&& (piddata->accumerror + error) > -piddata->max_accum) {
 	piddata->accumerror += error;
 }
 else {
	if (piddata->accumerror >= 0)
		piddata->accumerror = piddata->max_accum;
	else
		piddata->accumerror = -piddata->max_accum;
 }
 
//Calculate terms & output
 pterm = (long)error * piddata->pgain;
 iterm = (long)piddata->accumerror * piddata->igain;
 dterm = (long)(piddata->lasterror - error) * piddata->dgain;
 #ifdef PIDFLOAT
  piddata->output = (pterm + iterm + dterm);
 #else
  piddata->output = (pterm + iterm + dterm) >> 4;
 #endif
 
//Limit output
 if (piddata->output > piddata->maxout) 
	piddata->output = piddata->maxout;
 if (piddata->output < -piddata->maxout) 
	piddata->output = -piddata->maxout;
  
 piddata->lasterror = error;
 return piddata->output;
}