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@23:5238b046119b, 2016-11-18 (annotated)

Committer:
nolan21
Date:
Fri Nov 18 11:55:33 2016 +0000
Revision:
23:5238b046119b
Parent:
22:c18f04d1dc49 
almost there boys

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
nolan21 23:5238b046119b 10 PID::PID( float dt, float max, float min, float kp, float ki, float kd )
dlweakley 22:c18f04d1dc49 11 {
nolan21 23:5238b046119b 12 dt_ = dt;
nolan21 23:5238b046119b 13 max_ = max;
nolan21 23:5238b046119b 14 min_ = min;
nolan21 23:5238b046119b 15 kp_ = kp;
nolan21 23:5238b046119b 16 ki_ = ki;
nolan21 23:5238b046119b 17 kd_ = kd;
nolan21 23:5238b046119b 18 pre_error_ = 0;
nolan21 23:5238b046119b 19 integral_ = 0;
nolan21 23:5238b046119b 20
nolan21 23:5238b046119b 21 }
nolan21 23:5238b046119b 22
nolan21 23:5238b046119b 23 float PID::calculate(float setpoint, float pv)
nolan21 23:5238b046119b 24 {
nolan21 23:5238b046119b 25 // Calculate time changed since last PID calculation
nolan21 23:5238b046119b 26 // dt_ = current_time - dt_;
nolan21 23:5238b046119b 27
nolan21 23:5238b046119b 28 // Calculate error
nolan21 23:5238b046119b 29 float error = setpoint - pv;
dlweakley 22:c18f04d1dc49 30
nolan21 23:5238b046119b 31 // Proportional term
nolan21 23:5238b046119b 32 float Pout = kp_ * error;
nolan21 23:5238b046119b 33
nolan21 23:5238b046119b 34 // Integral term
nolan21 23:5238b046119b 35 integral_ += error * dt_;
nolan21 23:5238b046119b 36 float Iout = ki_ * integral_;
nolan21 23:5238b046119b 37
nolan21 23:5238b046119b 38 // Derivative term
nolan21 23:5238b046119b 39 float derivative = (error - pre_error_) / dt_;
nolan21 23:5238b046119b 40 float Dout = kd_ * derivative;
nolan21 23:5238b046119b 41
nolan21 23:5238b046119b 42 // Calculate total output
nolan21 23:5238b046119b 43 float output = Pout + Iout + Dout;
nolan21 23:5238b046119b 44
nolan21 23:5238b046119b 45 // Restrict to max/min
nolan21 23:5238b046119b 46
nolan21 23:5238b046119b 47 if( output > max_ ){
nolan21 23:5238b046119b 48 output = max_;
nolan21 23:5238b046119b 49 }
nolan21 23:5238b046119b 50 else if( output < min_ ){
nolan21 23:5238b046119b 51 output = min_;
nolan21 23:5238b046119b 52 }
dlweakley 22:c18f04d1dc49 53
dlweakley 22:c18f04d1dc49 54
nolan21 23:5238b046119b 55 // Save error to previous error
nolan21 23:5238b046119b 56 pre_error_ = error;
nolan21 23:5238b046119b 57
nolan21 23:5238b046119b 58 return output;
dlweakley 22:c18f04d1dc49 59 }
nolan21 23:5238b046119b 60
nolan21 23:5238b046119b 61 void PID::testError(float setpoint, float pv){
nolan21 23:5238b046119b 62 float error = setpoint - pv;
nolan21 23:5238b046119b 63 printf("Error: %f\n", error);
nolan21 23:5238b046119b 64 printf("Change in Time: %f\n", dt_);
dlweakley 22:c18f04d1dc49 65 }
nolan21 23:5238b046119b 66
nolan21 23:5238b046119b 67 PID::~PID() {}
dlweakley 22:c18f04d1dc49 68
dlweakley 22:c18f04d1dc49 69
dlweakley 22:c18f04d1dc49 70 /**
dlweakley 22:c18f04d1dc49 71 * Implementation
dlweakley 22:c18f04d1dc49 72 */
nolan21 23:5238b046119b 73 /*
nolan21 23:5238b046119b 74 PIDImpl::PIDImpl( float dt, float max, float min, float Kp, float Kd, float Ki ) :
dlweakley 22:c18f04d1dc49 75 _dt(dt),
dlweakley 22:c18f04d1dc49 76 _max(max),
dlweakley 22:c18f04d1dc49 77 _min(min),
dlweakley 22:c18f04d1dc49 78 _Kp(Kp),
dlweakley 22:c18f04d1dc49 79 _Kd(Kd),
dlweakley 22:c18f04d1dc49 80 _Ki(Ki),
dlweakley 22:c18f04d1dc49 81 _pre_error(0),
dlweakley 22:c18f04d1dc49 82 _integral(0)
dlweakley 22:c18f04d1dc49 83 {
dlweakley 22:c18f04d1dc49 84 }
dlweakley 22:c18f04d1dc49 85
nolan21 23:5238b046119b 86 float PIDImpl::calculate( float setpoint, float pv )
dlweakley 22:c18f04d1dc49 87 {
dlweakley 22:c18f04d1dc49 88
dlweakley 22:c18f04d1dc49 89 // Calculate error
nolan21 23:5238b046119b 90 float error = setpoint - pv;
dlweakley 22:c18f04d1dc49 91
dlweakley 22:c18f04d1dc49 92 // Proportional term
nolan21 23:5238b046119b 93 float Pout = _Kp * error;
dlweakley 22:c18f04d1dc49 94
dlweakley 22:c18f04d1dc49 95 // Integral term
dlweakley 22:c18f04d1dc49 96 _integral += error * _dt;
nolan21 23:5238b046119b 97 float Iout = _Ki * _integral;
dlweakley 22:c18f04d1dc49 98
dlweakley 22:c18f04d1dc49 99 // Derivative term
nolan21 23:5238b046119b 100 float derivative = (error - _pre_error) / _dt;
nolan21 23:5238b046119b 101 float Dout = _Kd * derivative;
dlweakley 22:c18f04d1dc49 102
dlweakley 22:c18f04d1dc49 103 // Calculate total output
nolan21 23:5238b046119b 104 float output = Pout + Iout + Dout;
dlweakley 22:c18f04d1dc49 105
dlweakley 22:c18f04d1dc49 106 // Restrict to max/min
dlweakley 22:c18f04d1dc49 107 if( output > _max )
dlweakley 22:c18f04d1dc49 108 output = _max;
dlweakley 22:c18f04d1dc49 109 else if( output < _min )
dlweakley 22:c18f04d1dc49 110 output = _min;
dlweakley 22:c18f04d1dc49 111
dlweakley 22:c18f04d1dc49 112 // Save error to previous error
dlweakley 22:c18f04d1dc49 113 _pre_error = error;
dlweakley 22:c18f04d1dc49 114
dlweakley 22:c18f04d1dc49 115 return output;
dlweakley 22:c18f04d1dc49 116 }
dlweakley 22:c18f04d1dc49 117
dlweakley 22:c18f04d1dc49 118 PIDImpl::~PIDImpl()
dlweakley 22:c18f04d1dc49 119 {
dlweakley 22:c18f04d1dc49 120 }
nolan21 23:5238b046119b 121 */
dlweakley 22:c18f04d1dc49 122 #endif