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@3:715c8245e671, 2015-02-04 (annotated)

Committer:
ms523
Date:
Wed Feb 04 14:19:03 2015 +0000
Revision:
3:715c8245e671
Parent:
2:f78e083a20c4
Child:
4:7b42de70b65f
kinda working

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 float Intergral = _Ki * _error; // Calculate the Intergral part
ms523 0:3a0d53c1740f 37
ms523 0:3a0d53c1740f 38 // Calculate the output
ms523 0:3a0d53c1740f 39 _output = _lastOutput + Proportional + Intergral;
ms523 0:3a0d53c1740f 40
ms523 0:3a0d53c1740f 41 // Check for wind-up
ms523 1:f85ca5649d55 42 if(_output > _maxLimit)
ms523 1:f85ca5649d55 43 _output = _maxLimit;
ms523 1:f85ca5649d55 44 if(_output < _minLimit)
ms523 1:f85ca5649d55 45 _output = _minLimit;
ms523 0:3a0d53c1740f 46
ms523 0:3a0d53c1740f 47 // Save the outputs for the next time round the loop
ms523 0:3a0d53c1740f 48 _lastOutput = _output; // Save the ouput
ms523 0:3a0d53c1740f 49 _lastError = _error;
ms523 0:3a0d53c1740f 50
ms523 0:3a0d53c1740f 51 // Return the new output
ms523 0:3a0d53c1740f 52 return(_output); // Return the PI loop output (duty cycle)
ms523 0:3a0d53c1740f 53 }
ms523 0:3a0d53c1740f 54
ms523 0:3a0d53c1740f 55 void Classic_PID::setKp(float Kp)
ms523 0:3a0d53c1740f 56 {
ms523 0:3a0d53c1740f 57
ms523 0:3a0d53c1740f 58 _Kp = Kp;
ms523 0:3a0d53c1740f 59
ms523 0:3a0d53c1740f 60 }
ms523 0:3a0d53c1740f 61
ms523 0:3a0d53c1740f 62 void Classic_PID::setKi(float Ki)
ms523 0:3a0d53c1740f 63 {
ms523 0:3a0d53c1740f 64
ms523 0:3a0d53c1740f 65 _Ki = Ki;
ms523 0:3a0d53c1740f 66 _sumError = 0;
ms523 0:3a0d53c1740f 67
ms523 0:3a0d53c1740f 68 }
ms523 0:3a0d53c1740f 69
ms523 0:3a0d53c1740f 70 float Classic_PID::getKp(void)
ms523 0:3a0d53c1740f 71 {
ms523 0:3a0d53c1740f 72
ms523 0:3a0d53c1740f 73 return(_Kp);
ms523 0:3a0d53c1740f 74
ms523 0:3a0d53c1740f 75 }
ms523 0:3a0d53c1740f 76
ms523 0:3a0d53c1740f 77 float Classic_PID::getKi(void)
ms523 0:3a0d53c1740f 78 {
ms523 0:3a0d53c1740f 79
ms523 0:3a0d53c1740f 80 return(_Ki);
ms523 0:3a0d53c1740f 81
ms523 0:3a0d53c1740f 82 }
ms523 0:3a0d53c1740f 83
ms523 0:3a0d53c1740f 84 float Classic_PID::getKvelff(void)
ms523 0:3a0d53c1740f 85 {
ms523 0:3a0d53c1740f 86
ms523 0:3a0d53c1740f 87 return(_kvelff);
ms523 0:3a0d53c1740f 88
ms523 0:3a0d53c1740f 89 }
ms523 0:3a0d53c1740f 90
ms523 0:3a0d53c1740f 91 float Classic_PID::compute_ff(void)
ms523 0:3a0d53c1740f 92 {
ms523 0:3a0d53c1740f 93 int _error = _setPoint - _processVariable; // Work out the instantaneous error
ms523 0:3a0d53c1740f 94
ms523 0:3a0d53c1740f 95 // Calculate the individual parts of the PID algorithm
ms523 0:3a0d53c1740f 96 float Proportional = _Kp * _error; // Calculate the Proportional part
ms523 0:3a0d53c1740f 97 _sumError += _error; // Add this error to the sum of errors
ms523 1:f85ca5649d55 98 if(_sumError > 1000)
ms523 1:f85ca5649d55 99 _sumError = 1000;
ms523 0:3a0d53c1740f 100 float Intergral = _Ki * _sumError; // Calculate the Intergral part
ms523 0:3a0d53c1740f 101
ms523 0:3a0d53c1740f 102 // Calculate the output
ms523 3:715c8245e671 103 _output = (_setPoint * _kvelff) + Proportional + Intergral + 0.005;
ms523 0:3a0d53c1740f 104
ms523 0:3a0d53c1740f 105 // Check for wind-up
ms523 1:f85ca5649d55 106 if(_output > _maxLimit)
ms523 1:f85ca5649d55 107 _output = _maxLimit;
ms523 1:f85ca5649d55 108 if(_output < _minLimit)
ms523 1:f85ca5649d55 109 _output = _minLimit;
ms523 1:f85ca5649d55 110
ms523 0:3a0d53c1740f 111 // Return the new output
ms523 0:3a0d53c1740f 112 return(_output); // Return the PI loop output (duty cycle)
ms523 0:3a0d53c1740f 113 }
ms523 0:3a0d53c1740f 114
ms523 0:3a0d53c1740f 115 void Classic_PID::setKvelff(float Kvelff)
ms523 0:3a0d53c1740f 116 {
ms523 0:3a0d53c1740f 117
ms523 0:3a0d53c1740f 118 _kvelff = Kvelff / 500000;
ms523 0:3a0d53c1740f 119
ms523 1:f85ca5649d55 120 }
ms523 1:f85ca5649d55 121
ms523 1:f85ca5649d55 122 void Classic_PID::setProcessLimits(float maxLimit, float minLimit)
ms523 1:f85ca5649d55 123 {
ms523 1:f85ca5649d55 124
ms523 1:f85ca5649d55 125 _maxLimit = maxLimit;
ms523 1:f85ca5649d55 126 _minLimit = minLimit;
ms523 1:f85ca5649d55 127
ms523 1:f85ca5649d55 128 }
ms523 1:f85ca5649d55 129
ms523 1:f85ca5649d55 130 float Classic_PID::getSetPoint(void)
ms523 1:f85ca5649d55 131 {
ms523 1:f85ca5649d55 132
ms523 1:f85ca5649d55 133 return(_setPoint);
ms523 1:f85ca5649d55 134
ms523 0:3a0d53c1740f 135 }