MHVLib  20111011
An efficiency oriented runtime library for AVR microcontrollers
A:/eclipse/mhvlib/MHV_PID.cpp
Go to the documentation of this file.
00001 /*
00002  * Based on Arduino PID Library - Version 1
00003  *              by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
00004  *
00005  * Original code is licensed under a Creative Commons Attribution-ShareAlike 3.0 Unported License.
00006  *
00007  * MHV version Copyright (c) 2011, Make, Hack, Void Inc
00008  * All rights reserved.
00009  *
00010  * Redistribution and use in source and binary forms, with or without
00011  * modification, are permitted provided that the following conditions are met:
00012  *  * Redistributions of source code must retain the above copyright
00013  *    notice, this list of conditions and the following disclaimer.
00014  *  * Redistributions in binary form must reproduce the above copyright
00015  *    notice, this list of conditions and the following disclaimer in the
00016  *    documentation and/or other materials provided with the distribution.
00017  *  * Neither the name of the Make, Hack, Void nor the
00018  *    names of its contributors may be used to endorse or promote products
00019  *    derived from this software without specific prior written permission.
00020  *
00021  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00022  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00023  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00024  * DISCLAIMED. IN NO EVENT SHALL MAKE, HACK, VOID BE LIABLE FOR ANY
00025  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00026  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00028  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00029  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00030  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00031  */
00032 
00033 #include <MHV_PID.h>
00034 
00046 MHV_PID::MHV_PID(float setpoint, float kP, float kI, float kD, uint16_t period,
00047                 bool reverse, uint16_t min, uint16_t max) {
00048         _setpoint = setpoint;
00049 
00050         setDirection(reverse);
00051     setTuning(kP, kI, kD, period);
00052 
00053     _enabled = false;
00054 }
00055 
00056 
00057 inline void MHV_PID::clampIntegral() {
00058         if (_integral > outMax) {
00059                 _integral = outMax;
00060         } else if (_integral < outMin) {
00061                 _integral = outMin;
00062         }
00063 }
00064 
00069 float MHV_PID::compute(float input) {
00070         float error = _setpoint - input;
00071 
00072         _integral += (_kI * error);
00073 
00074         clampIntegral();
00075 
00076         float output = _kP * error + _integral - _kD * (input - _lastInput);
00077 
00078         if (output > outMax)
00079                 output = outMax;
00080         else if (output < outMin)
00081                 output = outMin;
00082 
00083         _lastInput = input;
00084         _lastOutput = output;
00085 
00086         return output;
00087 }
00088 
00089 
00097 void MHV_PID::setTuning(float kP, float kI, float kD, uint16_t period) {
00098         float myPeriod = period / 1000;
00099 
00100         _kP = kP;
00101         _kI = kI * myPeriod;
00102         _kD = kD / myPeriod;
00103 
00104         if (_reverse) {
00105                 _kP = 0 - _kP;
00106                 _kI = 0 - _kI;
00107                 _kD = 0 - _kD;
00108         }
00109 }
00110 
00111 
00117 void MHV_PID::setOutputLimits(float min, float max) {
00118         outMin = min;
00119         outMax = max;
00120 
00121         clampIntegral();
00122 }
00123 
00124 
00129 void MHV_PID::enable(bool enable) {
00130         if (enable && !_enabled) {
00131                 _integral = _lastOutput;
00132 
00133                 clampIntegral();
00134         }
00135         _enabled = enable;
00136 }
00137 
00138 
00145 void MHV_PID::setDirection(bool reverse) {
00146    if (_enabled && reverse != _reverse) {
00147           _kP = 0 - _kP;
00148       _kI = 0 - _kI;
00149       _kD = 0 - _kD;
00150    }
00151 
00152    _reverse = reverse;
00153 }
00154