Diff: PID.cpp

Revision:

0:9a6f7aafe531

Child:

1:c307cd559154

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.cpp	Mon Nov 23 02:42:53 2015 +0000
@@ -0,0 +1,71 @@
+#include "PID.h"
+/*
+    Bryce Williams 11/19/2015
+    See PID.h for references as well as method descriptions
+*/
+
+PID::PID(float* setpoint, float* feedback, float* output,
+         float output_lower, float output_upper,
+         float  kp, float ki,  float kd, float Ts){
+    _Ts = Ts;           // Init params
+    _kp = kp; 
+    _ki = ki*Ts;        // Roll sample time into gain  
+    _kd = kd / Ts;      // Roll sample time into gain
+    
+    _setpoint = setpoint; 
+    _feedback = feedback; 
+    _output = output;
+    
+    _output_lower = output_lower;
+    _output_upper = output_upper;
+}
+
+void PID::start(){
+    // Start up such we avoid bumps... (see "Initialization" section in
+    // the reference link found in the header file).
+    last_feedback = *_feedback;   // Eliminate derivative kick at start/restart
+    i_accumulator = clip(*_output, _output_lower,
+                     _output_upper);    // P and D terms are zero, thus 
+                                        // i term is used to keep output unchanged
+    
+    sample_timer.attach(this, &PID::sample, _Ts);
+}
+
+void PID::stop(){
+    sample_timer.detach();
+}
+
+float PID::getError(){
+    return error;
+}
+
+void PID::set_parameters(float kp, float ki, float kd, float Ts){
+    stop();         // Disable Sample Interrupt... stop()
+    _Ts = Ts;       // Set New Sample Time
+    _kp = kp;       // Seet New Kp
+    _ki = ki*Ts;    // Roll sample time into gain  
+    _kd = kd / Ts;  // Roll sample time into gain
+    start();        // Enable Sample Interrupt... start()
+}
+
+void PID::sample(){
+    error = *_setpoint - *_feedback;
+
+    // Accumulate Integral Term such ki is applied to current error
+    // before adding to pool; avoids bumps if ki gain value is changed.
+    i_accumulator += _ki * error;
+    // Avoid "Windup" by clamping intergral term to output limits;
+    // essentially we stop integrating when we reach an upper or 
+    // lower bound.
+    i_accumulator = clip(i_accumulator, _output_lower, _output_upper);
+    
+    // Run it!
+    *_output = _kp*error + i_accumulator - _kd*(*_feedback - last_feedback);
+    last_feedback = *_feedback;
+    // Clamp Output
+    *_output = clip(*_output, _output_lower, _output_upper);
+}
+
+float PID::clip(float value, float lower, float upper){
+    return std::max(lower, std::min(value, upper));
+}
\ No newline at end of file