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@2:f78e083a20c4, 2015-01-19 (annotated)

Committer:
ms523
Date:
Mon Jan 19 14:20:30 2015 +0000
Revision:
2:f78e083a20c4
Parent:
1:f85ca5649d55
Child:
3:715c8245e671
19/01/15

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 1:f85ca5649d55 43 if(_output > _maxLimit)
ms523 1:f85ca5649d55 44 _output = _maxLimit;
ms523 1:f85ca5649d55 45 if(_output < _minLimit)
ms523 1:f85ca5649d55 46 _output = _minLimit;
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 1:f85ca5649d55 99 if(_sumError > 1000)
ms523 1:f85ca5649d55 100 _sumError = 1000;
ms523 0:3a0d53c1740f 101 float Intergral = _Ki * _sumError; // Calculate the Intergral part
ms523 0:3a0d53c1740f 102
ms523 0:3a0d53c1740f 103 // Calculate the output
ms523 2:f78e083a20c4 104 _output = ((1 / _setPoint) * _kvelff) + Proportional + Intergral;
ms523 0:3a0d53c1740f 105
ms523 0:3a0d53c1740f 106 // Check for wind-up
ms523 1:f85ca5649d55 107 if(_output > _maxLimit)
ms523 1:f85ca5649d55 108 _output = _maxLimit;
ms523 1:f85ca5649d55 109 if(_output < _minLimit)
ms523 1:f85ca5649d55 110 _output = _minLimit;
ms523 1:f85ca5649d55 111
ms523 0:3a0d53c1740f 112 // Return the new output
ms523 0:3a0d53c1740f 113 return(_output); // Return the PI loop output (duty cycle)
ms523 0:3a0d53c1740f 114 }
ms523 0:3a0d53c1740f 115
ms523 0:3a0d53c1740f 116 void Classic_PID::setKvelff(float Kvelff)
ms523 0:3a0d53c1740f 117 {
ms523 0:3a0d53c1740f 118
ms523 0:3a0d53c1740f 119 _kvelff = Kvelff / 500000;
ms523 0:3a0d53c1740f 120
ms523 1:f85ca5649d55 121 }
ms523 1:f85ca5649d55 122
ms523 1:f85ca5649d55 123 void Classic_PID::setProcessLimits(float maxLimit, float minLimit)
ms523 1:f85ca5649d55 124 {
ms523 1:f85ca5649d55 125
ms523 1:f85ca5649d55 126 _maxLimit = maxLimit;
ms523 1:f85ca5649d55 127 _minLimit = minLimit;
ms523 1:f85ca5649d55 128
ms523 1:f85ca5649d55 129 }
ms523 1:f85ca5649d55 130
ms523 1:f85ca5649d55 131 float Classic_PID::getSetPoint(void)
ms523 1:f85ca5649d55 132 {
ms523 1:f85ca5649d55 133
ms523 1:f85ca5649d55 134 return(_setPoint);
ms523 1:f85ca5649d55 135
ms523 0:3a0d53c1740f 136 }