Diff: PID.cpp

Revision:

0:8a420ac6394e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.cpp	Mon Jun 19 15:55:51 2017 +0000
@@ -0,0 +1,72 @@
+
+#include "PID.h"
+
+const int SCALING_FACTOR = 10000;
+
+PID::PID(void)
+{
+    minOutput = 0;
+    maxOutput = 0;
+    kp = 0;
+    ki = 0;
+    kd = 0;
+    errorIntegral = 0;
+    lastError = 0;
+    msPeriod = 1;
+    target = 0;
+    currentOutput = 0;
+}
+
+
+void PID::Reset(void)
+{
+    errorIntegral = 0;
+    lastError = 0;
+    currentOutput = 0;
+}
+
+
+void PID::SetOutputRange(double minOutput, double maxOutput)
+{
+    this->minOutput = minOutput;
+    this->maxOutput = maxOutput;
+}
+
+
+void PID::SetPID(double kp, double ki, double kd)
+{
+    this->kp = (kp/SCALING_FACTOR);
+    this->ki = (ki/SCALING_FACTOR);
+    this->kd = (kd/SCALING_FACTOR);
+}
+
+
+void PID::AddSample(double sampleValue, double control)
+{
+    pidSampleValue = sampleValue;
+    // calculate error
+    double error = target - sampleValue;
+    
+    // update our integral; the I & D gain are assumed to use seconds as their units
+    errorIntegral += error * msPeriod / 1000;
+    
+    // calculate our differential
+    double errorDifferential = (error - lastError) / (msPeriod / 1000);
+    lastError = error;
+    
+    // that gives us what we need to calculate an output
+    double output = control+(
+        kp * error +
+        ki * errorIntegral +
+        kd * errorDifferential)
+        ;
+    if (output < minOutput)
+        output = minOutput;
+    if (output > maxOutput)
+        output = maxOutput;
+        
+    currentOutput = output;
+}
+
+
+