Inverted Pendulum / Mbed 2 deprecated IP-Interface

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed QEI

PID/pid.cpp@22:c18f04d1dc49, 2016-11-18 (annotated)

Committer:
dlweakley
Date:
Fri Nov 18 02:47:59 2016 +0000
Revision:
22:c18f04d1dc49
Child:
23:5238b046119b
PID attempt

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dlweakley 22:c18f04d1dc49 1 #ifndef _PID_SOURCE_
dlweakley 22:c18f04d1dc49 2 #define _PID_SOURCE_
dlweakley 22:c18f04d1dc49 3
dlweakley 22:c18f04d1dc49 4 #include <iostream>
dlweakley 22:c18f04d1dc49 5 #include <cmath>
dlweakley 22:c18f04d1dc49 6 #include "pid.h"
dlweakley 22:c18f04d1dc49 7
dlweakley 22:c18f04d1dc49 8 using namespace std;
dlweakley 22:c18f04d1dc49 9
dlweakley 22:c18f04d1dc49 10 class PIDImpl
dlweakley 22:c18f04d1dc49 11 {
dlweakley 22:c18f04d1dc49 12 public:
dlweakley 22:c18f04d1dc49 13 PIDImpl( double dt, double max, double min, double Kp, double Kd, double Ki );
dlweakley 22:c18f04d1dc49 14 ~PIDImpl();
dlweakley 22:c18f04d1dc49 15 double calculate( double setpoint, double pv );
dlweakley 22:c18f04d1dc49 16
dlweakley 22:c18f04d1dc49 17 private:
dlweakley 22:c18f04d1dc49 18 double _dt;
dlweakley 22:c18f04d1dc49 19 double _max;
dlweakley 22:c18f04d1dc49 20 double _min;
dlweakley 22:c18f04d1dc49 21 double _Kp;
dlweakley 22:c18f04d1dc49 22 double _Kd;
dlweakley 22:c18f04d1dc49 23 double _Ki;
dlweakley 22:c18f04d1dc49 24 double _pre_error;
dlweakley 22:c18f04d1dc49 25 double _integral;
dlweakley 22:c18f04d1dc49 26 };
dlweakley 22:c18f04d1dc49 27
dlweakley 22:c18f04d1dc49 28
dlweakley 22:c18f04d1dc49 29 PID::PID( double dt, double max, double min, double Kp, double Kd, double Ki )
dlweakley 22:c18f04d1dc49 30 {
dlweakley 22:c18f04d1dc49 31 pimpl = new PIDImpl(dt,max,min,Kp,Kd,Ki);
dlweakley 22:c18f04d1dc49 32 }
dlweakley 22:c18f04d1dc49 33 double PID::calculate( double setpoint, double pv )
dlweakley 22:c18f04d1dc49 34 {
dlweakley 22:c18f04d1dc49 35 return pimpl->calculate(setpoint,pv);
dlweakley 22:c18f04d1dc49 36 }
dlweakley 22:c18f04d1dc49 37 PID::~PID()
dlweakley 22:c18f04d1dc49 38 {
dlweakley 22:c18f04d1dc49 39 delete pimpl;
dlweakley 22:c18f04d1dc49 40 }
dlweakley 22:c18f04d1dc49 41
dlweakley 22:c18f04d1dc49 42
dlweakley 22:c18f04d1dc49 43 /**
dlweakley 22:c18f04d1dc49 44 * Implementation
dlweakley 22:c18f04d1dc49 45 */
dlweakley 22:c18f04d1dc49 46 PIDImpl::PIDImpl( double dt, double max, double min, double Kp, double Kd, double Ki ) :
dlweakley 22:c18f04d1dc49 47 _dt(dt),
dlweakley 22:c18f04d1dc49 48 _max(max),
dlweakley 22:c18f04d1dc49 49 _min(min),
dlweakley 22:c18f04d1dc49 50 _Kp(Kp),
dlweakley 22:c18f04d1dc49 51 _Kd(Kd),
dlweakley 22:c18f04d1dc49 52 _Ki(Ki),
dlweakley 22:c18f04d1dc49 53 _pre_error(0),
dlweakley 22:c18f04d1dc49 54 _integral(0)
dlweakley 22:c18f04d1dc49 55 {
dlweakley 22:c18f04d1dc49 56 }
dlweakley 22:c18f04d1dc49 57
dlweakley 22:c18f04d1dc49 58 double PIDImpl::calculate( double setpoint, double pv )
dlweakley 22:c18f04d1dc49 59 {
dlweakley 22:c18f04d1dc49 60
dlweakley 22:c18f04d1dc49 61 // Calculate error
dlweakley 22:c18f04d1dc49 62 double error = setpoint - pv;
dlweakley 22:c18f04d1dc49 63
dlweakley 22:c18f04d1dc49 64 // Proportional term
dlweakley 22:c18f04d1dc49 65 double Pout = _Kp * error;
dlweakley 22:c18f04d1dc49 66
dlweakley 22:c18f04d1dc49 67 // Integral term
dlweakley 22:c18f04d1dc49 68 _integral += error * _dt;
dlweakley 22:c18f04d1dc49 69 double Iout = _Ki * _integral;
dlweakley 22:c18f04d1dc49 70
dlweakley 22:c18f04d1dc49 71 // Derivative term
dlweakley 22:c18f04d1dc49 72 double derivative = (error - _pre_error) / _dt;
dlweakley 22:c18f04d1dc49 73 double Dout = _Kd * derivative;
dlweakley 22:c18f04d1dc49 74
dlweakley 22:c18f04d1dc49 75 // Calculate total output
dlweakley 22:c18f04d1dc49 76 double output = Pout + Iout + Dout;
dlweakley 22:c18f04d1dc49 77
dlweakley 22:c18f04d1dc49 78 // Restrict to max/min
dlweakley 22:c18f04d1dc49 79 if( output > _max )
dlweakley 22:c18f04d1dc49 80 output = _max;
dlweakley 22:c18f04d1dc49 81 else if( output < _min )
dlweakley 22:c18f04d1dc49 82 output = _min;
dlweakley 22:c18f04d1dc49 83
dlweakley 22:c18f04d1dc49 84 // Save error to previous error
dlweakley 22:c18f04d1dc49 85 _pre_error = error;
dlweakley 22:c18f04d1dc49 86
dlweakley 22:c18f04d1dc49 87 return output;
dlweakley 22:c18f04d1dc49 88 }
dlweakley 22:c18f04d1dc49 89
dlweakley 22:c18f04d1dc49 90 PIDImpl::~PIDImpl()
dlweakley 22:c18f04d1dc49 91 {
dlweakley 22:c18f04d1dc49 92 }
dlweakley 22:c18f04d1dc49 93
dlweakley 22:c18f04d1dc49 94 #endif