PID.cpp

00001 #include "PID.h"
00002 
00003 void
00004 PID::setP(float p)
00005 {
00006     NVIC_DisableIRQ(TIMER3_IRQn);   // Disable Ticker IRQ for atomicity
00007     k_p = p;
00008     NVIC_EnableIRQ(TIMER3_IRQn);    // Enable Ticker IRQ
00009 }
00010 
00011 void
00012 PID::setI(float i)
00013 {
00014     NVIC_DisableIRQ(TIMER3_IRQn);   // Disable Ticker IRQ for atomicity
00015     k_i = i;
00016     NVIC_EnableIRQ(TIMER3_IRQn);    // Enable Ticker IRQ
00017 }
00018 
00019 void 
00020 PID::setD(float d)
00021 {
00022     NVIC_DisableIRQ(TIMER3_IRQn);   // Disable Ticker IRQ for atomicity
00023     k_d = d;
00024     NVIC_EnableIRQ(TIMER3_IRQn);    // Enable Ticker IRQ
00025 }
00026 
00027 void 
00028 PID::setClip(float clip)
00029 {
00030     NVIC_DisableIRQ(TIMER3_IRQn);   // Disable Ticker IRQ for atomicity
00031     this->clip = clip;
00032     NVIC_EnableIRQ(TIMER3_IRQn);    // Enable Ticker IRQ
00033 }
00034 
00035 void
00036 PID::setPV(float pv) // Set process variable
00037 {
00038     NVIC_DisableIRQ(TIMER3_IRQn);   // Disable Ticker IRQ for atomicity
00039     error = pv;
00040     NVIC_EnableIRQ(TIMER3_IRQn);    // Enable Ticker IRQ
00041 
00042 }
00043 
00044 void
00045 PID::dumpDebug(Serial *debug)
00046 {
00047     if (debug != NULL)
00048     {
00049         debug->printf("PID Debug: v_p        = %3.2f\r\n", v_p);
00050         debug->printf("PID Debug: v_i        = %3.2f\r\n", v_i);
00051         debug->printf("PID Debug: v_d        = %3.2f\r\n", v_d);
00052         debug->printf("PID Debug: k_p        = %3.2f\r\n", k_p);
00053         debug->printf("PID Debug: k_i        = %3.2f\r\n", k_i);
00054         debug->printf("PID Debug: k_d        = %3.2f\r\n", k_d);
00055         debug->printf("PID Debug: clip       = %3.2f\r\n", clip);
00056         debug->printf("PID Debug: error      = %3.2f\r\n", error);
00057         debug->printf("PID Debug: prev_error = %3.2f\r\n", prev_error);
00058         debug->printf("PID Debug: output     = %3.2f\r\n", output);
00059     }
00060 }
00061 
00062 float
00063 PID::run(float in)
00064 {
00065     if (debug != NULL)
00066         debug->printf("PID Debug: in         = %3.2f\r\n", in);
00067     error = in;
00068     
00069     v_p  = error;
00070     
00071     if (k_i != 0)
00072         v_i += v_p;
00073     else
00074         v_i = 0;
00075         
00076     v_d  = error - prev_error;    
00077     
00078     if (v_i > clip)
00079         v_i = clip;
00080     if (v_i < -clip)
00081         v_i = -clip;
00082         
00083     if (v_i != v_i) // NAN check...
00084         v_i = 0;
00085         
00086     if (debug != NULL)
00087     {
00088         debug->printf("PID Debug: v_p        = %3.2f\r\n", v_p);
00089         debug->printf("PID Debug: v_i        = %3.2f\r\n", v_i);
00090         debug->printf("PID Debug: v_d        = %3.2f\r\n", v_d);
00091         debug->printf("PID Debug: k_p        = %3.2f\r\n", k_p);
00092         debug->printf("PID Debug: k_i        = %3.2f\r\n", k_i);
00093         debug->printf("PID Debug: k_d        = %3.2f\r\n", k_d);
00094         debug->printf("PID Debug: clip       = %3.2f\r\n", clip);
00095     }
00096     
00097     output = (v_p * k_p) + (v_i * k_i) + (v_d * k_d);
00098     
00099     if (debug != NULL)
00100     {
00101         debug->printf("PID Debug: error      = %3.2f\r\n", error);
00102         debug->printf("PID Debug: prev_error = %3.2f\r\n", prev_error);
00103         debug->printf("PID Debug: output     = %3.2f\r\n", output);
00104     }
00105     
00106     prev_error = error;
00107     
00108     return output;
00109 }