![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Source: https://gist.github.com/bradley219/5373998 Include serial output
Dependencies: mbed
Dependents: DC_Stepper_Controller_Lib
pid.cpp
- Committer:
- stivending
- Date:
- 2021-05-27
- Revision:
- 1:4ecc202a912a
- Parent:
- 0:845ccddeede5
File content as of revision 1:4ecc202a912a:
/** * Copyright 2019 Bradley J. Snyder <snyder.bradleyj@gmail.com> * * 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. */ #ifndef _PID_SOURCE_ #define _PID_SOURCE_ #include <iostream> #include <cmath> #include "pid.h" using namespace std; class PIDImpl { public: PIDImpl( double dt, double max, double min, double Kp, double Kd, double Ki ); ~PIDImpl(); double calculate( double setpoint, double pv ); private: double _dt; double _max; double _min; double _Kp; double _Kd; double _Ki; double _pre_error; double _integral; }; PID::PID( double dt, double max, double min, double Kp, double Kd, double Ki ) { pimpl = new PIDImpl(dt,max,min,Kp,Kd,Ki); } double PID::calculate( double setpoint, double pv ) { return pimpl->calculate(setpoint,pv); } PID::~PID() { delete pimpl; } /** * Implementation */ PIDImpl::PIDImpl( double dt, double max, double min, double Kp, double Kd, double Ki ) : _dt(dt), _max(max), _min(min), _Kp(Kp), _Kd(Kd), _Ki(Ki), _pre_error(0), _integral(0) { } double PIDImpl::calculate( double setpoint, double pv ) { // Calculate error double error = setpoint - pv; // Proportional term double Pout = _Kp * error; // Integral term _integral += error * _dt; double Iout = _Ki * _integral; // Derivative term double derivative = (error - _pre_error) / _dt; double Dout = _Kd * derivative; // Calculate total output double output = Pout + Iout + Dout; // Restrict to max/min if( abs(output) > _max ) output = output > 0 ? _max : -_max; else if( abs(output) < _min ) output = output > 0 ? _min : -_min; // Save error to previous error _pre_error = error; return output; } PIDImpl::~PIDImpl() { } #endif