Revision 0:9a6f7aafe531, committed 2015-11-23

Comitter:

electromotivated

Date:

Mon Nov 23 02:42:53 2015 +0000

Child:

1:c307cd559154

Commit message:

Fairly Robust PID controller library;

Changed in this revision

--- /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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.h	Mon Nov 23 02:42:53 2015 +0000
@@ -0,0 +1,100 @@
+#ifndef PID_H
+#define PID_H
+#include "mbed.h"
+#include <algorithm>
+
+/*
+    Bryce Williams 11/19/2015
+    
+    PID Controller Class based on Brett Beauregard's Arduino PID Library
+    and PID blog post. 
+    
+    Brett Beauregard's blog post explains the PID code implementation very well
+    and discusses why the actual equation is a bit different than the classical 
+    equation, i.e. he explains and implements how to overcome windup, dervative
+    kick, etc.  This class uses the same implementation, but adds interrupt 
+    driven computation. 
+    
+    Reference Links:
+    1. Arduion Library:
+        (http://playground.arduino.cc/Code/PIDLibrary)
+    2. Brett Beauregard's PID Blog:
+        (http://brettbeauregard.com/blog/2011/04/improving-the-beginners-
+         pid-introduction/)
+*/
+
+class PID{
+    public:
+        /*
+            Constructor for PID objects. 
+            
+            Note: PID objects use given pointers, ie setpoint, 
+            feedback, output inside interrupts. When reading/ modifying
+            these vars make sure we don't have possible read/write 
+            conflicts if the interrupt fires. Either ensure reads/writes
+            are atomic operations, or call the stop() method perform the 
+            read/write and then call the start() method.
+        */
+        PID(float* setpoint, float* feedback, float* output,
+            float output_lower, float output_upper,
+            float  kp, float ki,  float kd, float Ts);
+        /*
+            Starts PID Controller; Attaches sample() as callback to Ticker 
+            sample_timer and starts the interrupt
+        */
+        void start();
+        /*
+            Stops PID Contoller; detaches callback from Ticker sample_timer.
+            Allows manual setting of output. 
+        */
+        void stop();
+        /*
+            Increments/ decrements Gain values and Sample time 
+            by the given value. Gives a simple method to 
+            programatically step through different values; just put in a 
+            loop and go
+            @param delta_"name" The value that will be added to its currently set value         
+        */
+//        void adjust_parameters(float delta_kp, float delta_ki, float delta_kd, float delta Ts);
+        /*
+            Overwrite Gain and Sample Time parameters with new 
+            values
+            Note: sample_timer interrupt is disabled during update
+                  to avoid synch issues.
+            
+        */
+        void set_parameters(float kp, float ki, float kd, float Ts);
+        
+        /*
+            returns current error
+        */
+        float getError();
+   
+    private:
+        float _kp, _ki, _kd;            // PID Gain values
+        float _Ts;                      // Sample time is seconds
+        float* _setpoint;               // Pointer to setpoint value
+        float* _feedback;               // Pointer to sensor feedback value (sensor input)
+        float* _output;                 // Pointer to control output value
+        float  _output_lower;            // Ouput Lower Limit
+        float  _output_upper;            // Output Upper Limit
+        
+        float i_accumulator;            // Integral Term accumulator
+        float last_feedback;            // Previous feedback value
+        float error;                    // Feedback error term
+        Ticker sample_timer;            // Generates the sample time interrupt and calls sample()
+        /* 
+            sample() performs next sample calculationand updates command value
+        */
+        void sample();
+        /*
+            Clips value to lower/ uppper
+            @param value    The value to clip
+            @param lower    The mininum allowable value
+            @param upper    The maximum allowable value
+            @return         The resulting clipped value
+        */
+        float clip(float value, float lower, float upper);
+};
+
+#endif
\ No newline at end of file