Diff: Sources/PID_Control.cpp

Revision:

0:15a8480061e8

diff -r 000000000000 -r 15a8480061e8 Sources/PID_Control.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sources/PID_Control.cpp	Mon May 22 11:24:46 2017 +0000
@@ -0,0 +1,80 @@
+/*
+ * PIDControl.cpp
+ *
+ *  Created on: 16.04.2017
+ *      Author: chris
+ */
+
+#include "PID_Control.h"
+#include "Robot.h"
+#include "Declarations.h"
+
+/**
+ * Constructor
+ */
+PID_Control::PID_Control() :
+    kp(0), ki(0), kd(0)
+{
+    eOld = 0.0f;
+    iSum = 0.0f;
+}
+
+/**
+ * Destructor
+ */
+PID_Control::~PID_Control()
+{
+}
+
+/**
+ * Sets the PID values
+ * @param p proportional gain
+ * @param i integral gain
+ * @param d differencial gain
+ */
+void PID_Control::setPIDValues(float p, float i, float d, float _max, float _min, float _iMax)
+{
+    kp = p;
+    ki = i;
+    kd = d;
+
+    max = _max;
+    min = _min;
+    iMax = _iMax;
+}
+
+/**
+ * Calculate and returns the next value from PID control
+ * @param e the error
+ * @param period the period
+ * @return
+ */
+float PID_Control::calc(float e, float period)
+{
+    static float dpart = 0.0f;
+    float out(0.0f);
+
+    iSum += e;
+
+    //Saturate i part
+    if (iSum > iMax)
+        iSum = iMax;
+    if (iSum < -iMax)
+        iSum = -iMax;
+
+    out = kp * e;
+    out += ki * iSum * period;
+    
+    dpart = 0.7f * dpart + 0.3f * (e - eOld) * 1.0f / period;
+    out += kd * dpart;
+
+   // out += kd * (e - eOld) * 1.0f / period;
+
+    eOld = e;
+
+    if( out > max ) out = max;
+    else if( out < min) out = min;
+
+    return out;
+}
+