Diff: PID.cpp

Revision:

0:9ac4a91b71fa

Child:

3:aa1a77b080eb

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.cpp	Wed Apr 04 17:07:49 2012 +0000
@@ -0,0 +1,103 @@
+#include "PID.h"
+
+void
+PID::setP(float p)
+{
+    NVIC_DisableIRQ(TIMER3_IRQn);   // Disable Ticker IRQ for atomicity
+    k_p = p;
+    NVIC_EnableIRQ(TIMER3_IRQn);    // Enable Ticker IRQ
+}
+
+void
+PID::setI(float i)
+{
+    NVIC_DisableIRQ(TIMER3_IRQn);   // Disable Ticker IRQ for atomicity
+    k_i = i;
+    NVIC_EnableIRQ(TIMER3_IRQn);    // Enable Ticker IRQ
+}
+
+void 
+PID::setD(float d)
+{
+    NVIC_DisableIRQ(TIMER3_IRQn);   // Disable Ticker IRQ for atomicity
+    k_d = d;
+    NVIC_EnableIRQ(TIMER3_IRQn);    // Enable Ticker IRQ
+}
+
+void 
+PID::setClip(float clip)
+{
+    NVIC_DisableIRQ(TIMER3_IRQn);   // Disable Ticker IRQ for atomicity
+    this->clip = clip;
+    NVIC_EnableIRQ(TIMER3_IRQn);    // Enable Ticker IRQ
+}
+
+void
+PID::setPV(float pv) // Set process variable
+{
+    NVIC_DisableIRQ(TIMER3_IRQn);   // Disable Ticker IRQ for atomicity
+    error = pv;
+    NVIC_EnableIRQ(TIMER3_IRQn);    // Enable Ticker IRQ
+
+}
+
+void
+PID::dumpDebug(Serial *debug)
+{
+    if (debug != NULL)
+    {
+        debug->printf("PID Debug: v_p        = %3.2f\r\n", v_p);
+        debug->printf("PID Debug: v_i        = %3.2f\r\n", v_i);
+        debug->printf("PID Debug: v_d        = %3.2f\r\n", v_d);
+        debug->printf("PID Debug: k_p        = %3.2f\r\n", k_p);
+        debug->printf("PID Debug: k_i        = %3.2f\r\n", k_i);
+        debug->printf("PID Debug: k_d        = %3.2f\r\n", k_d);
+        debug->printf("PID Debug: clip       = %3.2f\r\n", clip);
+        debug->printf("PID Debug: error      = %3.2f\r\n", error);
+        debug->printf("PID Debug: prev_error = %3.2f\r\n", prev_error);
+        debug->printf("PID Debug: output     = %3.2f\r\n", output);
+    }
+}
+
+float
+PID::run(float in)
+{
+    if (debug != NULL)
+        debug->printf("PID Debug: in         = %3.2f\r\n", in);
+    error = in;
+    
+    v_p  = error;
+    v_i += v_p;
+    v_d  = error - prev_error;    
+    
+    if (v_i > clip)
+        v_i = clip;
+    if (v_i < clip)
+        v_i = -clip;
+    if (v_i != v_i) // NAN check...
+        v_i = 0;
+        
+    if (debug != NULL)
+    {
+        debug->printf("PID Debug: v_p        = %3.2f\r\n", v_p);
+        debug->printf("PID Debug: v_i        = %3.2f\r\n", v_i);
+        debug->printf("PID Debug: v_d        = %3.2f\r\n", v_d);
+        debug->printf("PID Debug: k_p        = %3.2f\r\n", k_p);
+        debug->printf("PID Debug: k_i        = %3.2f\r\n", k_i);
+        debug->printf("PID Debug: k_d        = %3.2f\r\n", k_d);
+        debug->printf("PID Debug: clip       = %3.2f\r\n", clip);
+    }
+    
+    output = (v_p * k_p) + (v_i * k_i) + (v_d * k_d);
+    
+    if (debug != NULL)
+    {
+        debug->printf("PID Debug: error      = %3.2f\r\n", error);
+        debug->printf("PID Debug: prev_error = %3.2f\r\n", prev_error);
+        debug->printf("PID Debug: output     = %3.2f\r\n", output);
+    }
+    
+    prev_error = error;
+    
+    return output;
+}
\ No newline at end of file