FRC - Hackathon / PID

Motion Control library for 2, 3 inputs Full Bridge + Quadrature Encoder, motor system (also called a 2 wheels robot)

Dependencies:   Encoder_Nucleo_16_bits

Dependents:   FRC_2018 FRC2018_Bis 0hackton_08_06_18 lib_FRC_2019 ... more

PID.h@1:24c444f3716b, 2018-05-21 (annotated)

Committer:
haarkon
Date:
Mon May 21 16:16:13 2018 +0000
Revision:
1:24c444f3716b
Parent:
0:259a31d968a6
Child:
2:d6f14bb935da
cosmetic modifications, still untested

Who changed what in which revision?

UserRevisionLine numberNew contents of line
haarkon 0:259a31d968a6 1 /**
haarkon 0:259a31d968a6 2 * @author Hugues Angelis
haarkon 0:259a31d968a6 3 *
haarkon 0:259a31d968a6 4 * @section LICENSE
haarkon 0:259a31d968a6 5 *
haarkon 0:259a31d968a6 6 * Copyright (c) 2010 ARM Limited
haarkon 0:259a31d968a6 7 *
haarkon 0:259a31d968a6 8 * Permission is hereby granted, free of charge, to any person obtaining a copy
haarkon 0:259a31d968a6 9 * of this software and associated documentation files (the "Software"), to deal
haarkon 0:259a31d968a6 10 * in the Software without restriction, including without limitation the rights
haarkon 0:259a31d968a6 11 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
haarkon 0:259a31d968a6 12 * copies of the Software, and to permit persons to whom the Software is
haarkon 0:259a31d968a6 13 * furnished to do so, subject to the following conditions:
haarkon 0:259a31d968a6 14 *
haarkon 0:259a31d968a6 15 * The above copyright notice and this permission notice shall be included in
haarkon 0:259a31d968a6 16 * all copies or substantial portions of the Software.
haarkon 0:259a31d968a6 17 *
haarkon 0:259a31d968a6 18 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
haarkon 0:259a31d968a6 19 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
haarkon 0:259a31d968a6 20 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
haarkon 0:259a31d968a6 21 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
haarkon 0:259a31d968a6 22 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
haarkon 0:259a31d968a6 23 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
haarkon 0:259a31d968a6 24 * THE SOFTWARE.
haarkon 0:259a31d968a6 25 *
haarkon 0:259a31d968a6 26 * @section DESCRIPTION
haarkon 0:259a31d968a6 27 *
haarkon 0:259a31d968a6 28 * motion control PID speed controler library.
haarkon 0:259a31d968a6 29 *
haarkon 0:259a31d968a6 30 * This library require a tuning (mostly base on try/modify) and will not give
haarkon 0:259a31d968a6 31 * a correct performance until you have tuned all 3 parameters Kp, Ki and Kd.
haarkon 0:259a31d968a6 32 *
haarkon 0:259a31d968a6 33 * @note :
haarkon 0:259a31d968a6 34 * You must provide a 16 bits Timer definition that allow the use of a
haarkon 0:259a31d968a6 35 * quadrature encoder (pins A & B must be connected respectively to
haarkon 0:259a31d968a6 36 * pin 1 and 2 of the timer.
haarkon 0:259a31d968a6 37 *
haarkon 0:259a31d968a6 38 * Typicaly on Nucleo F446RE TIM3 and TIM4 are perfect to do this job.
haarkon 0:259a31d968a6 39 * In this case simply use TIM3 or TIM4 as timer parameter
haarkon 0:259a31d968a6 40 *
haarkon 0:259a31d968a6 41 * You must also provide the number of pulses generated by the QE for a
haarkon 0:259a31d968a6 42 * 1mm displacement of the wheel.
haarkon 0:259a31d968a6 43 *
haarkon 0:259a31d968a6 44 * You may also add 2 or 3 outputs to drive a PWM full bridge :
haarkon 0:259a31d968a6 45 * - 2 outputs for DIR/PWM full bridge
haarkon 0:259a31d968a6 46 * - 3 outputs for IN+/IN-/PWM full bridge.
haarkon 0:259a31d968a6 47 * Or you may not define any outputs.
haarkon 0:259a31d968a6 48 *
haarkon 0:259a31d968a6 49 * In this case the PID controler must use a specific function that will
haarkon 0:259a31d968a6 50 * output a PWM value that user must transform to match the full bridge.
haarkon 0:259a31d968a6 51 * Value evolve from 1 (full PWM fortward) to -1 (full PWM backward).
haarkon 0:259a31d968a6 52 * This value can also be found in the global variable : _PwmValue
haarkon 0:259a31d968a6 53 *
haarkon 0:259a31d968a6 54 * @note :
haarkon 0:259a31d968a6 55 * As this motion control system is implemented in a microcontroler it is
haarkon 0:259a31d968a6 56 * important to understand that there is a loop time for the motion control
haarkon 0:259a31d968a6 57 * system and that this loop time MUST be constant. Kp, Ki and Kd are
haarkon 0:259a31d968a6 58 * dependent of the loop time. changing loop time mean changing the
haarkon 0:259a31d968a6 59 * corrector's coefficients.
haarkon 0:259a31d968a6 60 *
haarkon 0:259a31d968a6 61 * We use a Ticker to control loop time.
haarkon 0:259a31d968a6 62 *
haarkon 0:259a31d968a6 63 * The looptime can be set by software as long as it remain constant during
haarkon 0:259a31d968a6 64 * the whole use of PID controler. Loop time is set to 1ms by default.
haarkon 0:259a31d968a6 65 *
haarkon 0:259a31d968a6 66 * Take care not to increase too mutch this time (or you will fail to
haarkon 0:259a31d968a6 67 * to control the motion) and to always keep enought time for computation
haarkon 0:259a31d968a6 68 * to take place during this loop.
haarkon 0:259a31d968a6 69 *
haarkon 0:259a31d968a6 70 * @note :
haarkon 0:259a31d968a6 71 * The PID is initialized with Ki = 0, Kd = 0 and Kp = 1
haarkon 0:259a31d968a6 72 * - Increasing Kp will shorten your response time but will also create
haarkon 0:259a31d968a6 73 * instability (at beggining overshoots, then instability).
haarkon 0:259a31d968a6 74 * - Adding a bit of Ki will allow your system to bring to 0 the static
haarkon 0:259a31d968a6 75 * error (ie : will make the error, between the set point and your
haarkon 0:259a31d968a6 76 * mesurment, tend to 0) but might create instability and increase
haarkon 0:259a31d968a6 77 * setting time.
haarkon 0:259a31d968a6 78 * - Adding a bit of Kd will stabilize the response (with almost no bad
haarkon 0:259a31d968a6 79 * effect, as long as Kd remains small).
haarkon 0:259a31d968a6 80 *
haarkon 0:259a31d968a6 81 * more info can be found here : https://en.wikipedia.org/wiki/PID_controller
haarkon 0:259a31d968a6 82 */
haarkon 0:259a31d968a6 83
haarkon 0:259a31d968a6 84 #ifndef PID_H
haarkon 0:259a31d968a6 85 #define PID_H
haarkon 0:259a31d968a6 86
haarkon 0:259a31d968a6 87 /**
haarkon 0:259a31d968a6 88 * Includes
haarkon 0:259a31d968a6 89 */
haarkon 0:259a31d968a6 90 #include "mbed.h"
haarkon 0:259a31d968a6 91 #include "Nucleo_Encoder_16_bits.h"
haarkon 0:259a31d968a6 92
haarkon 0:259a31d968a6 93 /**
haarkon 0:259a31d968a6 94 * PID Motion control system
haarkon 0:259a31d968a6 95 */
haarkon 0:259a31d968a6 96 class PID {
haarkon 0:259a31d968a6 97
haarkon 0:259a31d968a6 98 public :
haarkon 0:259a31d968a6 99
haarkon 0:259a31d968a6 100 /**
haarkon 0:259a31d968a6 101 * Constructor (standard) without full bridge control
haarkon 0:259a31d968a6 102 *
haarkon 0:259a31d968a6 103 * @param _TIM is the Mbed 16 bits timer used to connect the A&B output of QE
haarkon 0:259a31d968a6 104 * @param scale is the number of pulses on the QE for 1mm displacement of the wheel
haarkon 0:259a31d968a6 105 * @param outPWM is the Mbed pin used to send PWM on the full bridge
haarkon 0:259a31d968a6 106 * @param outDir is the Mbed pin used to send direction to the full bridge
haarkon 0:259a31d968a6 107 * @param invDir is the Mbed pin used to send direction (in case of 3 pins full bridge)
haarkon 0:259a31d968a6 108 */
haarkon 1:24c444f3716b 109 PID(TIM_TypeDef * TIM, PinName outPWM, PinName outA, PinName outB, double scale = 31.8309886184, double loopTime = 0.001);
haarkon 0:259a31d968a6 110
haarkon 0:259a31d968a6 111 /**
haarkon 0:259a31d968a6 112 * Set the Kp value
haarkon 0:259a31d968a6 113 *
haarkon 0:259a31d968a6 114 * @param : Kp value
haarkon 0:259a31d968a6 115 * @return : the value of Kp
haarkon 0:259a31d968a6 116 * @note : can also be accessed using the global variable _Kp
haarkon 0:259a31d968a6 117 */
haarkon 0:259a31d968a6 118 float setProportionnalValue (float KpValue);
haarkon 0:259a31d968a6 119
haarkon 0:259a31d968a6 120 /**
haarkon 0:259a31d968a6 121 * Set the Ki value
haarkon 0:259a31d968a6 122 *
haarkon 0:259a31d968a6 123 * @param : Ki value
haarkon 0:259a31d968a6 124 * @return : the value of Ki
haarkon 0:259a31d968a6 125 * @note : can also be accessed using the global variable _Ki
haarkon 0:259a31d968a6 126 */
haarkon 0:259a31d968a6 127 float setintegralValue (float KiValue);
haarkon 0:259a31d968a6 128
haarkon 0:259a31d968a6 129 /**
haarkon 0:259a31d968a6 130 * Set the Kd value
haarkon 0:259a31d968a6 131 *
haarkon 0:259a31d968a6 132 * @param : Kd value
haarkon 0:259a31d968a6 133 * @return : the value of Kd
haarkon 0:259a31d968a6 134 * @note : can also be accessed using the global variable _Kd
haarkon 0:259a31d968a6 135 */
haarkon 0:259a31d968a6 136 float setDerivativeValue (float KdValue);
haarkon 0:259a31d968a6 137
haarkon 0:259a31d968a6 138 /**
haarkon 0:259a31d968a6 139 * compute the PWM value of a motion controled system
haarkon 0:259a31d968a6 140 *
haarkon 0:259a31d968a6 141 * @param : Set point value (in mm/s)
haarkon 0:259a31d968a6 142 * @return : PWM value (between -1 and 1)
haarkon 0:259a31d968a6 143 */
haarkon 0:259a31d968a6 144 float computePWM (float setPoint);
haarkon 0:259a31d968a6 145
haarkon 0:259a31d968a6 146 /**
haarkon 0:259a31d968a6 147 * Set the Set point value of the speed for integrated full bridge
haarkon 0:259a31d968a6 148 *
haarkon 0:259a31d968a6 149 * @param : Set point value (in mm/s)
haarkon 0:259a31d968a6 150 */
haarkon 0:259a31d968a6 151 void setSpeed (float setPoint);
haarkon 0:259a31d968a6 152
haarkon 0:259a31d968a6 153 /**
haarkon 0:259a31d968a6 154 * Global Variable
haarkon 0:259a31d968a6 155 */
haarkon 0:259a31d968a6 156 float _Kp, _Ki, _Kd, _PwmValue;
haarkon 0:259a31d968a6 157 int RobotIsStuck;
haarkon 0:259a31d968a6 158
haarkon 0:259a31d968a6 159 protected :
haarkon 0:259a31d968a6 160
haarkon 0:259a31d968a6 161 Nucleo_Encoder_16_bits _encoder;
haarkon 0:259a31d968a6 162 Ticker _tick;
haarkon 0:259a31d968a6 163 DigitalOut _outA, _outB;
haarkon 0:259a31d968a6 164 PwmOut _pwm;
haarkon 0:259a31d968a6 165
haarkon 0:259a31d968a6 166 private :
haarkon 0:259a31d968a6 167
haarkon 0:259a31d968a6 168 void controlLoop();
haarkon 0:259a31d968a6 169
haarkon 0:259a31d968a6 170 float _consigne;
haarkon 0:259a31d968a6 171 double _scale, _loopTime;
haarkon 0:259a31d968a6 172 };
haarkon 0:259a31d968a6 173 #endif //PID_H