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@0:dd126a1080d3, 2017-02-14 (annotated)

Committer:
Knillinux
Date:
Tue Feb 14 05:12:54 2017 +0000
Revision:
0:dd126a1080d3
Child:
1:40bdbe1a93b7
test

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 0:dd126a1080d3 177
Knillinux 0:dd126a1080d3 178 //private:
Knillinux 0:dd126a1080d3 179
Knillinux 0:dd126a1080d3 180 bool usingFeedForward;
Knillinux 0:dd126a1080d3 181 bool inAuto;
Knillinux 0:dd126a1080d3 182
Knillinux 0:dd126a1080d3 183 //Actual tuning parameters used in PID calculation.
Knillinux 0:dd126a1080d3 184 float Kc_;
Knillinux 0:dd126a1080d3 185 float tauR_;
Knillinux 0:dd126a1080d3 186 float tauD_;
Knillinux 0:dd126a1080d3 187
Knillinux 0:dd126a1080d3 188 //Raw tuning parameters.
Knillinux 0:dd126a1080d3 189 float pParam_;
Knillinux 0:dd126a1080d3 190 float iParam_;
Knillinux 0:dd126a1080d3 191 float dParam_;
Knillinux 0:dd126a1080d3 192
Knillinux 0:dd126a1080d3 193 //The point we want to reach.
Knillinux 0:dd126a1080d3 194 float setPoint_;
Knillinux 0:dd126a1080d3 195 //The thing we measure.
Knillinux 0:dd126a1080d3 196 float processVariable_;
Knillinux 0:dd126a1080d3 197 float prevProcessVariable_;
Knillinux 0:dd126a1080d3 198 //The output that affects the process variable.
Knillinux 0:dd126a1080d3 199 float controllerOutput_;
Knillinux 0:dd126a1080d3 200 float prevControllerOutput_;
Knillinux 0:dd126a1080d3 201
Knillinux 0:dd126a1080d3 202 //We work in % for calculations so these will scale from
Knillinux 0:dd126a1080d3 203 //real world values to 0-100% and back again.
Knillinux 0:dd126a1080d3 204 float inMin_;
Knillinux 0:dd126a1080d3 205 float inMax_;
Knillinux 0:dd126a1080d3 206 float inSpan_;
Knillinux 0:dd126a1080d3 207 float outMin_;
Knillinux 0:dd126a1080d3 208 float outMax_;
Knillinux 0:dd126a1080d3 209 float outSpan_;
Knillinux 0:dd126a1080d3 210 /*float coutSMin_; // Imposed scaled controller output min, max - can be adjusted to apply hard limits without re-tuning controller
Knillinux 0:dd126a1080d3 211 float coutSMax_;*/
Knillinux 0:dd126a1080d3 212
Knillinux 0:dd126a1080d3 213 //The accumulated error, i.e. integral.
Knillinux 0:dd126a1080d3 214 float accError_;
Knillinux 0:dd126a1080d3 215 float accLimit_;
Knillinux 0:dd126a1080d3 216 float error_;
Knillinux 0:dd126a1080d3 217 //The controller output bias.
Knillinux 0:dd126a1080d3 218 float bias_;
Knillinux 0:dd126a1080d3 219
Knillinux 0:dd126a1080d3 220 //The interval between samples.
Knillinux 0:dd126a1080d3 221 float tSample_;
Knillinux 0:dd126a1080d3 222
Knillinux 0:dd126a1080d3 223 //Controller output as a real world value.
Knillinux 0:dd126a1080d3 224 volatile float realOutput_;
Knillinux 0:dd126a1080d3 225
Knillinux 0:dd126a1080d3 226 };
Knillinux 0:dd126a1080d3 227
Knillinux 0:dd126a1080d3 228 #endif /* PID_H */