Diff: Robot/src/pid.cpp

Revision:

0:fb4269aa5fb4

diff -r 000000000000 -r fb4269aa5fb4 Robot/src/pid.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Robot/src/pid.cpp	Fri May 26 03:51:19 2017 +0000
@@ -0,0 +1,60 @@
+#include "pid.h"
+
+
+void PID_Struct_Init(PID_Struct *pid, float Kp, float Kd, float Ki)
+{
+    pid->kp = Kp;
+    pid->kd = Kd;
+    pid->ki = Ki;
+    pid->diff[0] = 0;
+    pid->diff[1] = 0;
+    pid->i_term = 0;
+    
+    pid->is_initialized = 1;
+    pid->is_enabled = PID_CONTROL_DISABLE;
+}
+
+void PID_Enable_Control(PID_Struct *pid)
+{
+    pid->is_enabled = PID_CONTROL_ENABLE;
+}
+
+void PID_Disable_Control(PID_Struct *pid)
+{
+    pid->is_enabled = PID_CONTROL_DISABLE;
+}
+
+uint32_t PID_Check_Status(PID_Struct *pid)
+{
+    return pid->is_enabled;
+}
+
+float PID_Control(PID_Struct *pid, float new_input, float target)
+{
+    pid->diff[1] = -pid->diff[0];               //diff[1] = -e(k-1)
+    pid->diff[0] = target - new_input;      //e(k) = target - input
+    pid->diff[1] += pid->diff[0];             //diff[1] = diff[1] + e(k)
+                                                        //Eventually, diff[1] = e(k) - e(k-1)
+    pid->i_term += pid->ki * pid->diff[0];
+    
+    return (pid->kp * pid->diff[0] + pid->kd * pid->diff[1] + pid->i_term);
+}
+
+/*---------------These functions below are faster than PID_Control()----------------*/
+float PD_Control(PID_Struct *pid, float new_input, float target)
+{
+    pid->diff[1] = -pid->diff[0];               //diff[1] = -e(k-1)
+    pid->diff[0] = target - new_input;      //e(k) = target - input
+    pid->diff[1] += pid->diff[0];             //diff[1] = diff[1] + e(k)
+                                                        //Eventually, diff[1] = e(k) - e(k-1)
+    return (pid->kp * pid->diff[0] + pid->kd * pid->diff[1]);
+}
+
+float PI_Control(PID_Struct *pid, float new_input, float target)
+{
+    pid->diff[0] = target - new_input;      //e(k) = target - input
+    pid->i_term += pid->ki * pid->diff[0];
+    
+    return (pid->kp * pid->diff[0] + pid->i_term);
+}
+/*---------------------------------------------------------------------------*/