Robocon 2021 / Mbed 2 deprecated PID

Source: https://gist.github.com/bradley219/5373998 Include serial output

Dependencies:   mbed

Dependents:   DC_Stepper_Controller_Lib

pid.cpp@1:4ecc202a912a, 2021-05-27 (annotated)

Committer:
stivending
Date:
Thu May 27 05:43:33 2021 +0000
Revision:
1:4ecc202a912a
Parent:
0:845ccddeede5 
change min/max to absolute control;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ftppr 0:845ccddeede5 1 /**
ftppr 0:845ccddeede5 2 * Copyright 2019 Bradley J. Snyder <snyder.bradleyj@gmail.com>
ftppr 0:845ccddeede5 3 *
ftppr 0:845ccddeede5 4 * Permission is hereby granted, free of charge, to any person obtaining a copy
ftppr 0:845ccddeede5 5 * of this software and associated documentation files (the "Software"), to deal
ftppr 0:845ccddeede5 6 * in the Software without restriction, including without limitation the rights
ftppr 0:845ccddeede5 7 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
ftppr 0:845ccddeede5 8 * copies of the Software, and to permit persons to whom the Software is
ftppr 0:845ccddeede5 9 * furnished to do so, subject to the following conditions:
ftppr 0:845ccddeede5 10 *
ftppr 0:845ccddeede5 11 * The above copyright notice and this permission notice shall be included in
ftppr 0:845ccddeede5 12 * all copies or substantial portions of the Software.
ftppr 0:845ccddeede5 13 *
ftppr 0:845ccddeede5 14 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
ftppr 0:845ccddeede5 15 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
ftppr 0:845ccddeede5 16 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
ftppr 0:845ccddeede5 17 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
ftppr 0:845ccddeede5 18 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
ftppr 0:845ccddeede5 19 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
ftppr 0:845ccddeede5 20 * THE SOFTWARE.
ftppr 0:845ccddeede5 21 */
ftppr 0:845ccddeede5 22
ftppr 0:845ccddeede5 23 #ifndef _PID_SOURCE_
ftppr 0:845ccddeede5 24 #define _PID_SOURCE_
ftppr 0:845ccddeede5 25
ftppr 0:845ccddeede5 26 #include <iostream>
ftppr 0:845ccddeede5 27 #include <cmath>
ftppr 0:845ccddeede5 28 #include "pid.h"
ftppr 0:845ccddeede5 29
ftppr 0:845ccddeede5 30 using namespace std;
ftppr 0:845ccddeede5 31
ftppr 0:845ccddeede5 32 class PIDImpl
ftppr 0:845ccddeede5 33 {
ftppr 0:845ccddeede5 34 public:
ftppr 0:845ccddeede5 35 PIDImpl( double dt, double max, double min, double Kp, double Kd, double Ki );
ftppr 0:845ccddeede5 36 ~PIDImpl();
ftppr 0:845ccddeede5 37 double calculate( double setpoint, double pv );
ftppr 0:845ccddeede5 38
ftppr 0:845ccddeede5 39 private:
ftppr 0:845ccddeede5 40 double _dt;
ftppr 0:845ccddeede5 41 double _max;
ftppr 0:845ccddeede5 42 double _min;
ftppr 0:845ccddeede5 43 double _Kp;
ftppr 0:845ccddeede5 44 double _Kd;
ftppr 0:845ccddeede5 45 double _Ki;
ftppr 0:845ccddeede5 46 double _pre_error;
ftppr 0:845ccddeede5 47 double _integral;
ftppr 0:845ccddeede5 48 };
ftppr 0:845ccddeede5 49
ftppr 0:845ccddeede5 50
ftppr 0:845ccddeede5 51 PID::PID( double dt, double max, double min, double Kp, double Kd, double Ki )
ftppr 0:845ccddeede5 52 {
ftppr 0:845ccddeede5 53 pimpl = new PIDImpl(dt,max,min,Kp,Kd,Ki);
ftppr 0:845ccddeede5 54 }
ftppr 0:845ccddeede5 55 double PID::calculate( double setpoint, double pv )
ftppr 0:845ccddeede5 56 {
ftppr 0:845ccddeede5 57 return pimpl->calculate(setpoint,pv);
ftppr 0:845ccddeede5 58 }
ftppr 0:845ccddeede5 59 PID::~PID()
ftppr 0:845ccddeede5 60 {
ftppr 0:845ccddeede5 61 delete pimpl;
ftppr 0:845ccddeede5 62 }
ftppr 0:845ccddeede5 63
ftppr 0:845ccddeede5 64
ftppr 0:845ccddeede5 65 /**
ftppr 0:845ccddeede5 66 * Implementation
ftppr 0:845ccddeede5 67 */
ftppr 0:845ccddeede5 68 PIDImpl::PIDImpl( double dt, double max, double min, double Kp, double Kd, double Ki ) :
ftppr 0:845ccddeede5 69 _dt(dt),
ftppr 0:845ccddeede5 70 _max(max),
ftppr 0:845ccddeede5 71 _min(min),
ftppr 0:845ccddeede5 72 _Kp(Kp),
ftppr 0:845ccddeede5 73 _Kd(Kd),
ftppr 0:845ccddeede5 74 _Ki(Ki),
ftppr 0:845ccddeede5 75 _pre_error(0),
ftppr 0:845ccddeede5 76 _integral(0)
ftppr 0:845ccddeede5 77 {
ftppr 0:845ccddeede5 78 }
ftppr 0:845ccddeede5 79
ftppr 0:845ccddeede5 80 double PIDImpl::calculate( double setpoint, double pv )
ftppr 0:845ccddeede5 81 {
ftppr 0:845ccddeede5 82
ftppr 0:845ccddeede5 83 // Calculate error
ftppr 0:845ccddeede5 84 double error = setpoint - pv;
ftppr 0:845ccddeede5 85
ftppr 0:845ccddeede5 86 // Proportional term
ftppr 0:845ccddeede5 87 double Pout = _Kp * error;
ftppr 0:845ccddeede5 88
ftppr 0:845ccddeede5 89 // Integral term
ftppr 0:845ccddeede5 90 _integral += error * _dt;
ftppr 0:845ccddeede5 91 double Iout = _Ki * _integral;
ftppr 0:845ccddeede5 92
ftppr 0:845ccddeede5 93 // Derivative term
ftppr 0:845ccddeede5 94 double derivative = (error - _pre_error) / _dt;
ftppr 0:845ccddeede5 95 double Dout = _Kd * derivative;
ftppr 0:845ccddeede5 96
ftppr 0:845ccddeede5 97 // Calculate total output
ftppr 0:845ccddeede5 98 double output = Pout + Iout + Dout;
ftppr 0:845ccddeede5 99
ftppr 0:845ccddeede5 100 // Restrict to max/min
stivending 1:4ecc202a912a 101 if( abs(output) > _max )
stivending 1:4ecc202a912a 102 output = output > 0 ? _max : -_max;
stivending 1:4ecc202a912a 103 else if( abs(output) < _min )
stivending 1:4ecc202a912a 104 output = output > 0 ? _min : -_min;
ftppr 0:845ccddeede5 105
ftppr 0:845ccddeede5 106 // Save error to previous error
ftppr 0:845ccddeede5 107 _pre_error = error;
ftppr 0:845ccddeede5 108
ftppr 0:845ccddeede5 109 return output;
ftppr 0:845ccddeede5 110 }
ftppr 0:845ccddeede5 111
ftppr 0:845ccddeede5 112 PIDImpl::~PIDImpl()
ftppr 0:845ccddeede5 113 {
ftppr 0:845ccddeede5 114 }
ftppr 0:845ccddeede5 115
ftppr 0:845ccddeede5 116 #endif