Justin Rhodes / Mbed 2 deprecated RFVS

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?

UserRevisionLine numberNew 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