Carbon Fibre / Classic_PID

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.

Classic_PID.cpp@0:3a0d53c1740f, 2014-11-04 (annotated)

Committer:
ms523
Date:
Tue Nov 04 11:57:02 2014 +0000
Revision:
0:3a0d53c1740f
Child:
1:f85ca5649d55
Working with velocity lop only

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ms523 0:3a0d53c1740f 1 #include "mbed.h"
ms523 0:3a0d53c1740f 2 #include "Classic_PID.h"
ms523 0:3a0d53c1740f 3
ms523 0:3a0d53c1740f 4 // Create object
ms523 0:3a0d53c1740f 5 Classic_PID::Classic_PID(float Kp, float Ki, float Kd, float kvelff)
ms523 0:3a0d53c1740f 6 {
ms523 0:3a0d53c1740f 7
ms523 0:3a0d53c1740f 8 // Save paramaters
ms523 0:3a0d53c1740f 9 _Kp = Kp;
ms523 0:3a0d53c1740f 10 _Ki = Ki;
ms523 0:3a0d53c1740f 11 _Kd = Kd;
ms523 0:3a0d53c1740f 12 _kvelff = kvelff;
ms523 0:3a0d53c1740f 13 }
ms523 0:3a0d53c1740f 14
ms523 0:3a0d53c1740f 15 // Set Methods
ms523 0:3a0d53c1740f 16 void Classic_PID::setSetPoint(float sp)
ms523 0:3a0d53c1740f 17 {
ms523 0:3a0d53c1740f 18
ms523 0:3a0d53c1740f 19 _setPoint = sp;
ms523 0:3a0d53c1740f 20
ms523 0:3a0d53c1740f 21 }
ms523 0:3a0d53c1740f 22
ms523 0:3a0d53c1740f 23 void Classic_PID::setProcessValue(float pv)
ms523 0:3a0d53c1740f 24 {
ms523 0:3a0d53c1740f 25
ms523 0:3a0d53c1740f 26 _processVariable = pv;
ms523 0:3a0d53c1740f 27
ms523 0:3a0d53c1740f 28 }
ms523 0:3a0d53c1740f 29
ms523 0:3a0d53c1740f 30 float Classic_PID::compute(void)
ms523 0:3a0d53c1740f 31 {
ms523 0:3a0d53c1740f 32 int _error = _setPoint - _processVariable; // Work out the instantaneous error
ms523 0:3a0d53c1740f 33
ms523 0:3a0d53c1740f 34 // Calculate the individual parts of the PID algorithm
ms523 0:3a0d53c1740f 35 float Proportional = _Kp * (_error - _lastError); // Calculate the Proportional part
ms523 0:3a0d53c1740f 36 //_sumError += _error; // Add this error to the sum of errors
ms523 0:3a0d53c1740f 37 float Intergral = _Ki * _error; // Calculate the Intergral part
ms523 0:3a0d53c1740f 38
ms523 0:3a0d53c1740f 39 // Calculate the output
ms523 0:3a0d53c1740f 40 _output = _lastOutput + Proportional + Intergral;
ms523 0:3a0d53c1740f 41
ms523 0:3a0d53c1740f 42 // Check for wind-up
ms523 0:3a0d53c1740f 43 if(_output > 1.0)
ms523 0:3a0d53c1740f 44 _output = 1.0;
ms523 0:3a0d53c1740f 45 if(_output < -1.0)
ms523 0:3a0d53c1740f 46 _output = -1.0;
ms523 0:3a0d53c1740f 47
ms523 0:3a0d53c1740f 48 // Save the outputs for the next time round the loop
ms523 0:3a0d53c1740f 49 _lastOutput = _output; // Save the ouput
ms523 0:3a0d53c1740f 50 _lastError = _error;
ms523 0:3a0d53c1740f 51
ms523 0:3a0d53c1740f 52 // Return the new output
ms523 0:3a0d53c1740f 53 return(_output); // Return the PI loop output (duty cycle)
ms523 0:3a0d53c1740f 54 }
ms523 0:3a0d53c1740f 55
ms523 0:3a0d53c1740f 56 void Classic_PID::setKp(float Kp)
ms523 0:3a0d53c1740f 57 {
ms523 0:3a0d53c1740f 58
ms523 0:3a0d53c1740f 59 _Kp = Kp;
ms523 0:3a0d53c1740f 60
ms523 0:3a0d53c1740f 61 }
ms523 0:3a0d53c1740f 62
ms523 0:3a0d53c1740f 63 void Classic_PID::setKi(float Ki)
ms523 0:3a0d53c1740f 64 {
ms523 0:3a0d53c1740f 65
ms523 0:3a0d53c1740f 66 _Ki = Ki;
ms523 0:3a0d53c1740f 67 _sumError = 0;
ms523 0:3a0d53c1740f 68
ms523 0:3a0d53c1740f 69 }
ms523 0:3a0d53c1740f 70
ms523 0:3a0d53c1740f 71 float Classic_PID::getKp(void)
ms523 0:3a0d53c1740f 72 {
ms523 0:3a0d53c1740f 73
ms523 0:3a0d53c1740f 74 return(_Kp);
ms523 0:3a0d53c1740f 75
ms523 0:3a0d53c1740f 76 }
ms523 0:3a0d53c1740f 77
ms523 0:3a0d53c1740f 78 float Classic_PID::getKi(void)
ms523 0:3a0d53c1740f 79 {
ms523 0:3a0d53c1740f 80
ms523 0:3a0d53c1740f 81 return(_Ki);
ms523 0:3a0d53c1740f 82
ms523 0:3a0d53c1740f 83 }
ms523 0:3a0d53c1740f 84
ms523 0:3a0d53c1740f 85 float Classic_PID::getKvelff(void)
ms523 0:3a0d53c1740f 86 {
ms523 0:3a0d53c1740f 87
ms523 0:3a0d53c1740f 88 return(_kvelff);
ms523 0:3a0d53c1740f 89
ms523 0:3a0d53c1740f 90 }
ms523 0:3a0d53c1740f 91
ms523 0:3a0d53c1740f 92 float Classic_PID::compute_ff(void)
ms523 0:3a0d53c1740f 93 {
ms523 0:3a0d53c1740f 94 int _error = _setPoint - _processVariable; // Work out the instantaneous error
ms523 0:3a0d53c1740f 95
ms523 0:3a0d53c1740f 96 // Calculate the individual parts of the PID algorithm
ms523 0:3a0d53c1740f 97 float Proportional = _Kp * _error; // Calculate the Proportional part
ms523 0:3a0d53c1740f 98 _sumError += _error; // Add this error to the sum of errors
ms523 0:3a0d53c1740f 99 float Intergral = _Ki * _sumError; // Calculate the Intergral part
ms523 0:3a0d53c1740f 100
ms523 0:3a0d53c1740f 101 // Calculate the output
ms523 0:3a0d53c1740f 102 _output = (_setPoint * _kvelff) + Proportional + Intergral;
ms523 0:3a0d53c1740f 103
ms523 0:3a0d53c1740f 104 // Check for wind-up
ms523 0:3a0d53c1740f 105 if(_output > 1.0)
ms523 0:3a0d53c1740f 106 _output = 1.0;
ms523 0:3a0d53c1740f 107 if(_output < -1.0)
ms523 0:3a0d53c1740f 108 _output = -1.0;
ms523 0:3a0d53c1740f 109
ms523 0:3a0d53c1740f 110 // Return the new output
ms523 0:3a0d53c1740f 111 return(_output); // Return the PI loop output (duty cycle)
ms523 0:3a0d53c1740f 112 }
ms523 0:3a0d53c1740f 113
ms523 0:3a0d53c1740f 114 void Classic_PID::setKvelff(float Kvelff)
ms523 0:3a0d53c1740f 115 {
ms523 0:3a0d53c1740f 116
ms523 0:3a0d53c1740f 117 _kvelff = Kvelff / 500000;
ms523 0:3a0d53c1740f 118
ms523 0:3a0d53c1740f 119 }