baseline build
Dependencies: FastPWM mbed-os mbed
PID.cpp@0:8a420ac6394e, 2017-06-19 (annotated)
- Committer:
- jrhodes5150
- Date:
- Mon Jun 19 15:55:51 2017 +0000
- Revision:
- 0:8a420ac6394e
initial build - baseline;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jrhodes5150 | 0:8a420ac6394e | 1 | |
jrhodes5150 | 0:8a420ac6394e | 2 | #include "PID.h" |
jrhodes5150 | 0:8a420ac6394e | 3 | |
jrhodes5150 | 0:8a420ac6394e | 4 | const int SCALING_FACTOR = 10000; |
jrhodes5150 | 0:8a420ac6394e | 5 | |
jrhodes5150 | 0:8a420ac6394e | 6 | PID::PID(void) |
jrhodes5150 | 0:8a420ac6394e | 7 | { |
jrhodes5150 | 0:8a420ac6394e | 8 | minOutput = 0; |
jrhodes5150 | 0:8a420ac6394e | 9 | maxOutput = 0; |
jrhodes5150 | 0:8a420ac6394e | 10 | kp = 0; |
jrhodes5150 | 0:8a420ac6394e | 11 | ki = 0; |
jrhodes5150 | 0:8a420ac6394e | 12 | kd = 0; |
jrhodes5150 | 0:8a420ac6394e | 13 | errorIntegral = 0; |
jrhodes5150 | 0:8a420ac6394e | 14 | lastError = 0; |
jrhodes5150 | 0:8a420ac6394e | 15 | msPeriod = 1; |
jrhodes5150 | 0:8a420ac6394e | 16 | target = 0; |
jrhodes5150 | 0:8a420ac6394e | 17 | currentOutput = 0; |
jrhodes5150 | 0:8a420ac6394e | 18 | } |
jrhodes5150 | 0:8a420ac6394e | 19 | |
jrhodes5150 | 0:8a420ac6394e | 20 | |
jrhodes5150 | 0:8a420ac6394e | 21 | void PID::Reset(void) |
jrhodes5150 | 0:8a420ac6394e | 22 | { |
jrhodes5150 | 0:8a420ac6394e | 23 | errorIntegral = 0; |
jrhodes5150 | 0:8a420ac6394e | 24 | lastError = 0; |
jrhodes5150 | 0:8a420ac6394e | 25 | currentOutput = 0; |
jrhodes5150 | 0:8a420ac6394e | 26 | } |
jrhodes5150 | 0:8a420ac6394e | 27 | |
jrhodes5150 | 0:8a420ac6394e | 28 | |
jrhodes5150 | 0:8a420ac6394e | 29 | void PID::SetOutputRange(double minOutput, double maxOutput) |
jrhodes5150 | 0:8a420ac6394e | 30 | { |
jrhodes5150 | 0:8a420ac6394e | 31 | this->minOutput = minOutput; |
jrhodes5150 | 0:8a420ac6394e | 32 | this->maxOutput = maxOutput; |
jrhodes5150 | 0:8a420ac6394e | 33 | } |
jrhodes5150 | 0:8a420ac6394e | 34 | |
jrhodes5150 | 0:8a420ac6394e | 35 | |
jrhodes5150 | 0:8a420ac6394e | 36 | void PID::SetPID(double kp, double ki, double kd) |
jrhodes5150 | 0:8a420ac6394e | 37 | { |
jrhodes5150 | 0:8a420ac6394e | 38 | this->kp = (kp/SCALING_FACTOR); |
jrhodes5150 | 0:8a420ac6394e | 39 | this->ki = (ki/SCALING_FACTOR); |
jrhodes5150 | 0:8a420ac6394e | 40 | this->kd = (kd/SCALING_FACTOR); |
jrhodes5150 | 0:8a420ac6394e | 41 | } |
jrhodes5150 | 0:8a420ac6394e | 42 | |
jrhodes5150 | 0:8a420ac6394e | 43 | |
jrhodes5150 | 0:8a420ac6394e | 44 | void PID::AddSample(double sampleValue, double control) |
jrhodes5150 | 0:8a420ac6394e | 45 | { |
jrhodes5150 | 0:8a420ac6394e | 46 | pidSampleValue = sampleValue; |
jrhodes5150 | 0:8a420ac6394e | 47 | // calculate error |
jrhodes5150 | 0:8a420ac6394e | 48 | double error = target - sampleValue; |
jrhodes5150 | 0:8a420ac6394e | 49 | |
jrhodes5150 | 0:8a420ac6394e | 50 | // update our integral; the I & D gain are assumed to use seconds as their units |
jrhodes5150 | 0:8a420ac6394e | 51 | errorIntegral += error * msPeriod / 1000; |
jrhodes5150 | 0:8a420ac6394e | 52 | |
jrhodes5150 | 0:8a420ac6394e | 53 | // calculate our differential |
jrhodes5150 | 0:8a420ac6394e | 54 | double errorDifferential = (error - lastError) / (msPeriod / 1000); |
jrhodes5150 | 0:8a420ac6394e | 55 | lastError = error; |
jrhodes5150 | 0:8a420ac6394e | 56 | |
jrhodes5150 | 0:8a420ac6394e | 57 | // that gives us what we need to calculate an output |
jrhodes5150 | 0:8a420ac6394e | 58 | double output = control+( |
jrhodes5150 | 0:8a420ac6394e | 59 | kp * error + |
jrhodes5150 | 0:8a420ac6394e | 60 | ki * errorIntegral + |
jrhodes5150 | 0:8a420ac6394e | 61 | kd * errorDifferential) |
jrhodes5150 | 0:8a420ac6394e | 62 | ; |
jrhodes5150 | 0:8a420ac6394e | 63 | if (output < minOutput) |
jrhodes5150 | 0:8a420ac6394e | 64 | output = minOutput; |
jrhodes5150 | 0:8a420ac6394e | 65 | if (output > maxOutput) |
jrhodes5150 | 0:8a420ac6394e | 66 | output = maxOutput; |
jrhodes5150 | 0:8a420ac6394e | 67 | |
jrhodes5150 | 0:8a420ac6394e | 68 | currentOutput = output; |
jrhodes5150 | 0:8a420ac6394e | 69 | } |
jrhodes5150 | 0:8a420ac6394e | 70 | |
jrhodes5150 | 0:8a420ac6394e | 71 | |
jrhodes5150 | 0:8a420ac6394e | 72 |