Thomas Lew / Mbed 2 deprecated FreeFlyerROS

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed ros_lib_kinetic

PID.h@1:40bdbe1a93b7, 2018-06-22 (annotated)

Committer:
Knillinux
Date:
Fri Jun 22 02:09:50 2018 +0000
Revision:
1:40bdbe1a93b7
Parent:
0:dd126a1080d3 
Updates

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Knillinux 0:dd126a1080d3 1 /**
Knillinux 0:dd126a1080d3 2 * @author Aaron Berk
Knillinux 0:dd126a1080d3 3 *
Knillinux 0:dd126a1080d3 4 * @section LICENSE
Knillinux 0:dd126a1080d3 5 *
Knillinux 0:dd126a1080d3 6 * Copyright (c) 2010 ARM Limited
Knillinux 0:dd126a1080d3 7 *
Knillinux 0:dd126a1080d3 8 * Permission is hereby granted, free of charge, to any person obtaining a copy
Knillinux 0:dd126a1080d3 9 * of this software and associated documentation files (the "Software"), to deal
Knillinux 0:dd126a1080d3 10 * in the Software without restriction, including without limitation the rights
Knillinux 0:dd126a1080d3 11 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
Knillinux 0:dd126a1080d3 12 * copies of the Software, and to permit persons to whom the Software is
Knillinux 0:dd126a1080d3 13 * furnished to do so, subject to the following conditions:
Knillinux 0:dd126a1080d3 14 *
Knillinux 0:dd126a1080d3 15 * The above copyright notice and this permission notice shall be included in
Knillinux 0:dd126a1080d3 16 * all copies or substantial portions of the Software.
Knillinux 0:dd126a1080d3 17 *
Knillinux 0:dd126a1080d3 18 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
Knillinux 0:dd126a1080d3 19 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
Knillinux 0:dd126a1080d3 20 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
Knillinux 0:dd126a1080d3 21 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
Knillinux 0:dd126a1080d3 22 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
Knillinux 0:dd126a1080d3 23 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
Knillinux 0:dd126a1080d3 24 * THE SOFTWARE.
Knillinux 0:dd126a1080d3 25 *
Knillinux 0:dd126a1080d3 26 * @section DESCRIPTION
Knillinux 0:dd126a1080d3 27 *
Knillinux 0:dd126a1080d3 28 * A PID controller is a widely used feedback controller commonly found in
Knillinux 0:dd126a1080d3 29 * industry.
Knillinux 0:dd126a1080d3 30 *
Knillinux 0:dd126a1080d3 31 * This library is a port of Brett Beauregard's Arduino PID library:
Knillinux 0:dd126a1080d3 32 *
Knillinux 0:dd126a1080d3 33 * http://www.arduino.cc/playground/Code/PIDLibrary
Knillinux 0:dd126a1080d3 34 *
Knillinux 0:dd126a1080d3 35 * The wikipedia article on PID controllers is a good place to start on
Knillinux 0:dd126a1080d3 36 * understanding how they work:
Knillinux 0:dd126a1080d3 37 *
Knillinux 0:dd126a1080d3 38 * http://en.wikipedia.org/wiki/PID_controller
Knillinux 0:dd126a1080d3 39 *
Knillinux 0:dd126a1080d3 40 * For a clear and elegant explanation of how to implement and tune a
Knillinux 0:dd126a1080d3 41 * controller, the controlguru website by Douglas J. Cooper (who also happened
Knillinux 0:dd126a1080d3 42 * to be Brett's controls professor) is an excellent reference:
Knillinux 0:dd126a1080d3 43 *
Knillinux 0:dd126a1080d3 44 * http://www.controlguru.com/
Knillinux 0:dd126a1080d3 45 */
Knillinux 0:dd126a1080d3 46
Knillinux 0:dd126a1080d3 47 #ifndef PID_H
Knillinux 0:dd126a1080d3 48 #define PID_H
Knillinux 0:dd126a1080d3 49
Knillinux 0:dd126a1080d3 50 /**
Knillinux 0:dd126a1080d3 51 * Includes
Knillinux 0:dd126a1080d3 52 */
Knillinux 0:dd126a1080d3 53 #include "mbed.h"
Knillinux 0:dd126a1080d3 54
Knillinux 0:dd126a1080d3 55 /**
Knillinux 0:dd126a1080d3 56 * Defines
Knillinux 0:dd126a1080d3 57 */
Knillinux 0:dd126a1080d3 58 #define MANUAL_MODE 0
Knillinux 0:dd126a1080d3 59 #define AUTO_MODE 1
Knillinux 0:dd126a1080d3 60
Knillinux 0:dd126a1080d3 61 /**
Knillinux 0:dd126a1080d3 62 * Proportional-integral-derivative controller.
Knillinux 0:dd126a1080d3 63 */
Knillinux 0:dd126a1080d3 64 class PID {
Knillinux 0:dd126a1080d3 65
Knillinux 0:dd126a1080d3 66 public:
Knillinux 0:dd126a1080d3 67
Knillinux 0:dd126a1080d3 68 /**
Knillinux 0:dd126a1080d3 69 * Constructor.
Knillinux 0:dd126a1080d3 70 *
Knillinux 0:dd126a1080d3 71 * Sets default limits [0-3.3V], calculates tuning parameters, and sets
Knillinux 0:dd126a1080d3 72 * manual mode with no bias.
Knillinux 0:dd126a1080d3 73 *
Knillinux 0:dd126a1080d3 74 * @param Kc - Tuning parameter
Knillinux 0:dd126a1080d3 75 * @param tauI - Tuning parameter
Knillinux 0:dd126a1080d3 76 * @param tauD - Tuning parameter
Knillinux 0:dd126a1080d3 77 * @param interval PID calculation performed every interval seconds.
Knillinux 0:dd126a1080d3 78 */
Knillinux 0:dd126a1080d3 79 PID(float Kc, float tauI, float tauD, float interval);
Knillinux 0:dd126a1080d3 80
Knillinux 0:dd126a1080d3 81 /**
Knillinux 0:dd126a1080d3 82 * Scale from inputs to 0-100%.
Knillinux 0:dd126a1080d3 83 *
Knillinux 0:dd126a1080d3 84 * @param InMin The real world value corresponding to 0%.
Knillinux 0:dd126a1080d3 85 * @param InMax The real world value corresponding to 100%.
Knillinux 0:dd126a1080d3 86 */
Knillinux 0:dd126a1080d3 87 void setInputLimits(float inMin , float inMax);
Knillinux 0:dd126a1080d3 88
Knillinux 0:dd126a1080d3 89 /**
Knillinux 0:dd126a1080d3 90 * Scale from outputs to 0-100%.
Knillinux 0:dd126a1080d3 91 *
Knillinux 0:dd126a1080d3 92 * @param outMin The real world value corresponding to 0%.
Knillinux 0:dd126a1080d3 93 * @param outMax The real world value corresponding to 100%.
Knillinux 0:dd126a1080d3 94 */
Knillinux 0:dd126a1080d3 95 void setOutputLimits(float outMin, float outMax);
Knillinux 0:dd126a1080d3 96
Knillinux 0:dd126a1080d3 97 /**
Knillinux 0:dd126a1080d3 98 * Set hard controller input limits
Knillinux 0:dd126a1080d3 99 *
Knillinux 0:dd126a1080d3 100 * @param coutMin The real world value placing a lower bound within the input span.
Knillinux 0:dd126a1080d3 101 * @param coutMax The real world value placing an upper bound within the input span.
Knillinux 0:dd126a1080d3 102 *
Knillinux 0:dd126a1080d3 103 void setConInputLimits(float cinMin, float cinMax);
Knillinux 0:dd126a1080d3 104 */
Knillinux 0:dd126a1080d3 105
Knillinux 0:dd126a1080d3 106 /**
Knillinux 0:dd126a1080d3 107 * Calculate PID constants.
Knillinux 0:dd126a1080d3 108 *
Knillinux 0:dd126a1080d3 109 * Allows parameters to be changed on the fly without ruining calculations.
Knillinux 0:dd126a1080d3 110 *
Knillinux 0:dd126a1080d3 111 * @param Kc - Tuning parameter
Knillinux 0:dd126a1080d3 112 * @param tauI - Tuning parameter
Knillinux 0:dd126a1080d3 113 * @param tauD - Tuning parameter
Knillinux 0:dd126a1080d3 114 */
Knillinux 0:dd126a1080d3 115 void setTunings(float Kc, float tauI, float tauD);
Knillinux 0:dd126a1080d3 116
Knillinux 0:dd126a1080d3 117 /**
Knillinux 0:dd126a1080d3 118 * Reinitializes controller internals. Automatically
Knillinux 0:dd126a1080d3 119 * called on a manual to auto transition.
Knillinux 0:dd126a1080d3 120 */
Knillinux 0:dd126a1080d3 121 void reset(void);
Knillinux 0:dd126a1080d3 122
Knillinux 0:dd126a1080d3 123 /**
Knillinux 0:dd126a1080d3 124 * Set PID to manual or auto mode.
Knillinux 0:dd126a1080d3 125 *
Knillinux 0:dd126a1080d3 126 * @param mode 0 -> Manual
Knillinux 0:dd126a1080d3 127 * Non-zero -> Auto
Knillinux 0:dd126a1080d3 128 */
Knillinux 0:dd126a1080d3 129 void setMode(int mode);
Knillinux 0:dd126a1080d3 130
Knillinux 0:dd126a1080d3 131 /**
Knillinux 0:dd126a1080d3 132 * Set how fast the PID loop is run.
Knillinux 0:dd126a1080d3 133 *
Knillinux 0:dd126a1080d3 134 * @param interval PID calculation peformed every interval seconds.
Knillinux 0:dd126a1080d3 135 */
Knillinux 0:dd126a1080d3 136 void setInterval(float interval);
Knillinux 0:dd126a1080d3 137
Knillinux 0:dd126a1080d3 138 /**
Knillinux 0:dd126a1080d3 139 * Set the set point.
Knillinux 0:dd126a1080d3 140 *
Knillinux 0:dd126a1080d3 141 * @param sp The set point as a real world value.
Knillinux 0:dd126a1080d3 142 */
Knillinux 0:dd126a1080d3 143 void setSetPoint(float sp);
Knillinux 0:dd126a1080d3 144
Knillinux 0:dd126a1080d3 145 /**
Knillinux 0:dd126a1080d3 146 * Set the process value.
Knillinux 0:dd126a1080d3 147 *
Knillinux 0:dd126a1080d3 148 * @param pv The process value as a real world value.
Knillinux 0:dd126a1080d3 149 */
Knillinux 0:dd126a1080d3 150 void setProcessValue(float pv);
Knillinux 0:dd126a1080d3 151
Knillinux 0:dd126a1080d3 152 /**
Knillinux 0:dd126a1080d3 153 * Set the bias.
Knillinux 0:dd126a1080d3 154 *
Knillinux 0:dd126a1080d3 155 * @param bias The bias for the controller output.
Knillinux 0:dd126a1080d3 156 */
Knillinux 0:dd126a1080d3 157 void setBias(float bias);
Knillinux 0:dd126a1080d3 158
Knillinux 0:dd126a1080d3 159 void setAccLimit(float accLimit);
Knillinux 0:dd126a1080d3 160
Knillinux 0:dd126a1080d3 161 /**
Knillinux 0:dd126a1080d3 162 * PID calculation.
Knillinux 0:dd126a1080d3 163 *
Knillinux 0:dd126a1080d3 164 * @return The controller output as a float between outMin and outMax.
Knillinux 0:dd126a1080d3 165 */
Knillinux 0:dd126a1080d3 166 float compute(void);
Knillinux 0:dd126a1080d3 167
Knillinux 0:dd126a1080d3 168 //Getters.
Knillinux 0:dd126a1080d3 169 float getInMin();
Knillinux 0:dd126a1080d3 170 float getInMax();
Knillinux 0:dd126a1080d3 171 float getOutMin();
Knillinux 0:dd126a1080d3 172 float getOutMax();
Knillinux 0:dd126a1080d3 173 float getInterval();
Knillinux 0:dd126a1080d3 174 float getPParam();
Knillinux 0:dd126a1080d3 175 float getIParam();
Knillinux 0:dd126a1080d3 176 float getDParam();
Knillinux 1:40bdbe1a93b7 177 float getAccLimit();
Knillinux 1:40bdbe1a93b7 178 float getAccError();
Knillinux 1:40bdbe1a93b7 179 float getSetPoint();
Knillinux 0:dd126a1080d3 180
Knillinux 1:40bdbe1a93b7 181 private:
Knillinux 0:dd126a1080d3 182
Knillinux 0:dd126a1080d3 183 bool usingFeedForward;
Knillinux 0:dd126a1080d3 184 bool inAuto;
Knillinux 0:dd126a1080d3 185
Knillinux 0:dd126a1080d3 186 //Actual tuning parameters used in PID calculation.
Knillinux 0:dd126a1080d3 187 float Kc_;
Knillinux 0:dd126a1080d3 188 float tauR_;
Knillinux 0:dd126a1080d3 189 float tauD_;
Knillinux 0:dd126a1080d3 190
Knillinux 0:dd126a1080d3 191 //Raw tuning parameters.
Knillinux 0:dd126a1080d3 192 float pParam_;
Knillinux 0:dd126a1080d3 193 float iParam_;
Knillinux 0:dd126a1080d3 194 float dParam_;
Knillinux 0:dd126a1080d3 195
Knillinux 1:40bdbe1a93b7 196 // The point we want to reach.
Knillinux 0:dd126a1080d3 197 float setPoint_;
Knillinux 0:dd126a1080d3 198 //The thing we measure.
Knillinux 0:dd126a1080d3 199 float processVariable_;
Knillinux 0:dd126a1080d3 200 float prevProcessVariable_;
Knillinux 0:dd126a1080d3 201 //The output that affects the process variable.
Knillinux 0:dd126a1080d3 202 float controllerOutput_;
Knillinux 0:dd126a1080d3 203 float prevControllerOutput_;
Knillinux 0:dd126a1080d3 204
Knillinux 0:dd126a1080d3 205 //We work in % for calculations so these will scale from
Knillinux 0:dd126a1080d3 206 //real world values to 0-100% and back again.
Knillinux 0:dd126a1080d3 207 float inMin_;
Knillinux 0:dd126a1080d3 208 float inMax_;
Knillinux 0:dd126a1080d3 209 float inSpan_;
Knillinux 0:dd126a1080d3 210 float outMin_;
Knillinux 0:dd126a1080d3 211 float outMax_;
Knillinux 0:dd126a1080d3 212 float outSpan_;
Knillinux 0:dd126a1080d3 213
Knillinux 0:dd126a1080d3 214 //The accumulated error, i.e. integral.
Knillinux 0:dd126a1080d3 215 float accError_;
Knillinux 0:dd126a1080d3 216 float accLimit_;
Knillinux 0:dd126a1080d3 217 float error_;
Knillinux 0:dd126a1080d3 218 //The controller output bias.
Knillinux 0:dd126a1080d3 219 float bias_;
Knillinux 0:dd126a1080d3 220
Knillinux 0:dd126a1080d3 221 //The interval between samples.
Knillinux 0:dd126a1080d3 222 float tSample_;
Knillinux 0:dd126a1080d3 223
Knillinux 0:dd126a1080d3 224 //Controller output as a real world value.
Knillinux 0:dd126a1080d3 225 volatile float realOutput_;
Knillinux 0:dd126a1080d3 226
Knillinux 0:dd126a1080d3 227 };
Knillinux 0:dd126a1080d3 228
Knillinux 0:dd126a1080d3 229 #endif /* PID_H */