Diff: PID.cpp

Revision:

0:873985df821c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.cpp	Sat Dec 14 12:23:58 2019 +0000
@@ -0,0 +1,72 @@
+#include "PID.h"
+
+PID::PID(Timer *T)
+{
+    integral         = 0;
+    last_error       = 0;
+    current_time     = 0;
+    last_time        = 0;
+    
+    timer = T;
+    timer -> start();
+}
+
+float PID::controlPID(float target, float current)
+{
+    current_time = timer -> read();
+    float error = target - current;
+    float dt = current_time - last_time;
+    integral += ( error + last_error ) / 2 * dt;
+    float differential = ( error - last_error ) / dt;
+    float control = Kp * error + Ki * integral + Kd * differential;
+    last_error = error;
+    last_time = current_time;
+    return control;
+}
+
+float PID::controlP(float target, float current, float new_Kp)
+{
+    float error = target - current;
+    float control = new_Kp * error;
+    return control;
+}
+float PID::controlPI(float target, float current)
+{
+    current_time = timer -> read();
+    float error = target - current;
+    float dt = current_time - last_time;
+    integral += (error + last_error) / 2 * dt;
+    float control = Kp * error + Ki * integral;
+    last_error = error;
+    last_time = timer->read();
+    return control;
+}
+void PID::setParameter(float new_Kp, float new_Ki, float new_Kd)
+{
+    Kp = new_Kp;
+    Ki = new_Ki;
+    Kd = new_Kd;
+}
+void PID::setParameter(float new_Ku, float new_Pu)
+{  
+    Ku = new_Ku;
+    Pu = new_Pu;
+    
+    Kp = 0.60 * Ku;
+    Ti = 0.50 * Pu;
+    Td = 0.125 * Pu;
+    Ki = (1 / Ti) * Kp;
+    Kd = Td * Kp;
+}
+void PID::setParameterPI(float new_Ku, float new_Pu)
+{
+    Kp = 0.45 * Ku;
+    Ti = 0.83 * Pu;
+    Ki = (1 / Ti) * Kp;
+}
+void PID::reset()
+{
+    integral    = 0;
+    last_error  = 0;
+    last_time = timer -> read();
+}