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@2:d6f14bb935da, 2018-05-22 (annotated)

Committer:
haarkon
Date:
Tue May 22 17:15:51 2018 +0000
Revision:
2:d6f14bb935da
Parent:
1:24c444f3716b
Child:
3:93fb84c4e9bc
added callback treatment of interrupt

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 2:d6f14bb935da 34 * You must provide a 16 bits Timer definition that allow the use of a quadrature encoder (pins A & B must be connected respectively to pin 1 and 2 of the timer.
haarkon 0:259a31d968a6 35 *
haarkon 0:259a31d968a6 36 * Typicaly on Nucleo F446RE TIM3 and TIM4 are perfect to do this job.
haarkon 0:259a31d968a6 37 * In this case simply use TIM3 or TIM4 as timer parameter
haarkon 0:259a31d968a6 38 *
haarkon 2:d6f14bb935da 39 * You must also provide the number of pulses generated by the QE for a 1mm displacement of the wheel.
haarkon 0:259a31d968a6 40 *
haarkon 0:259a31d968a6 41 * You may also add 2 or 3 outputs to drive a PWM full bridge :
haarkon 0:259a31d968a6 42 * - 2 outputs for DIR/PWM full bridge
haarkon 0:259a31d968a6 43 * - 3 outputs for IN+/IN-/PWM full bridge.
haarkon 0:259a31d968a6 44 * Or you may not define any outputs.
haarkon 0:259a31d968a6 45 *
haarkon 2:d6f14bb935da 46 * In this case the PID controler must use a specific function that will output a PWM value that user must transform to match the full bridge.
haarkon 0:259a31d968a6 47 * Value evolve from 1 (full PWM fortward) to -1 (full PWM backward).
haarkon 0:259a31d968a6 48 * This value can also be found in the global variable : _PwmValue
haarkon 0:259a31d968a6 49 *
haarkon 0:259a31d968a6 50 * @note :
haarkon 2:d6f14bb935da 51 * As this motion control system is implemented in a microcontroler it is important to understand that there is a loop time for the motion control system and that this loop time MUST be constant.
haarkon 2:d6f14bb935da 52 * Kp, Ki and Kd are dependent of the loop time. changing loop time mean changing the corrector's coefficients.
haarkon 0:259a31d968a6 53 * We use a Ticker to control loop time.
haarkon 0:259a31d968a6 54 *
haarkon 2:d6f14bb935da 55 * The looptime can be set by software as long as it remain constant during the whole use of PID controler. Loop time is set to 1ms by default.
haarkon 0:259a31d968a6 56 *
haarkon 2:d6f14bb935da 57 * Take care not to increase too mutch this time (or you will fail to to control the motion) and to always keep enought time for computation to take place during this loop.
haarkon 0:259a31d968a6 58 *
haarkon 0:259a31d968a6 59 * @note :
haarkon 0:259a31d968a6 60 * The PID is initialized with Ki = 0, Kd = 0 and Kp = 1
haarkon 0:259a31d968a6 61 * - Increasing Kp will shorten your response time but will also create
haarkon 0:259a31d968a6 62 * instability (at beggining overshoots, then instability).
haarkon 0:259a31d968a6 63 * - Adding a bit of Ki will allow your system to bring to 0 the static
haarkon 0:259a31d968a6 64 * error (ie : will make the error, between the set point and your
haarkon 0:259a31d968a6 65 * mesurment, tend to 0) but might create instability and increase
haarkon 0:259a31d968a6 66 * setting time.
haarkon 0:259a31d968a6 67 * - Adding a bit of Kd will stabilize the response (with almost no bad
haarkon 0:259a31d968a6 68 * effect, as long as Kd remains small).
haarkon 0:259a31d968a6 69 *
haarkon 0:259a31d968a6 70 * more info can be found here : https://en.wikipedia.org/wiki/PID_controller
haarkon 0:259a31d968a6 71 */
haarkon 0:259a31d968a6 72
haarkon 0:259a31d968a6 73 #ifndef PID_H
haarkon 0:259a31d968a6 74 #define PID_H
haarkon 0:259a31d968a6 75
haarkon 0:259a31d968a6 76 /**
haarkon 2:d6f14bb935da 77 * Includes : Mbed Library + Nucleo_Encoder_16_bits library
haarkon 0:259a31d968a6 78 */
haarkon 0:259a31d968a6 79 #include "mbed.h"
haarkon 0:259a31d968a6 80 #include "Nucleo_Encoder_16_bits.h"
haarkon 0:259a31d968a6 81
haarkon 0:259a31d968a6 82 /**
haarkon 2:d6f14bb935da 83 * PID Motion control system (speed control).
haarkon 2:d6f14bb935da 84 * refere to wikipedia for more explainations : https://en.wikipedia.org/wiki/PID_controller
haarkon 0:259a31d968a6 85 */
haarkon 0:259a31d968a6 86 class PID {
haarkon 0:259a31d968a6 87
haarkon 0:259a31d968a6 88 public :
haarkon 0:259a31d968a6 89
haarkon 0:259a31d968a6 90 /**
haarkon 2:d6f14bb935da 91 * Constructor (standard) with full bridge control
haarkon 0:259a31d968a6 92 *
haarkon 2:d6f14bb935da 93 * @param _TIM : the Mbed 16 bits timer used to connect the A&B output of QE
haarkon 2:d6f14bb935da 94 * @param outPWM : the Mbed pin used to send PWM on the full bridge
haarkon 2:d6f14bb935da 95 * @param outA : the Mbed pin used to send direction to the full bridge
haarkon 2:d6f14bb935da 96 * @param outB : the Mbed pin used to send direction (in case of 3 pins full bridge)
haarkon 2:d6f14bb935da 97 * @param scale : the number of pulses on the QE for 1mm displacement of the wheel
haarkon 2:d6f14bb935da 98 * @param loopTime : the time between 2 computations
haarkon 0:259a31d968a6 99 */
haarkon 1:24c444f3716b 100 PID(TIM_TypeDef * TIM, PinName outPWM, PinName outA, PinName outB, double scale = 31.8309886184, double loopTime = 0.001);
haarkon 0:259a31d968a6 101
haarkon 0:259a31d968a6 102 /**
haarkon 0:259a31d968a6 103 * Set the Kp value
haarkon 0:259a31d968a6 104 *
haarkon 2:d6f14bb935da 105 * @param Kp value
haarkon 2:d6f14bb935da 106 * @return the value of Kp
haarkon 2:d6f14bb935da 107 * @note can also be accessed using the global variable _Kp
haarkon 0:259a31d968a6 108 */
haarkon 0:259a31d968a6 109 float setProportionnalValue (float KpValue);
haarkon 0:259a31d968a6 110
haarkon 0:259a31d968a6 111 /**
haarkon 0:259a31d968a6 112 * Set the Ki value
haarkon 0:259a31d968a6 113 *
haarkon 2:d6f14bb935da 114 * @param Ki value
haarkon 2:d6f14bb935da 115 * @return the value of Ki
haarkon 2:d6f14bb935da 116 * @note can also be accessed using the global variable _Ki
haarkon 0:259a31d968a6 117 */
haarkon 0:259a31d968a6 118 float setintegralValue (float KiValue);
haarkon 0:259a31d968a6 119
haarkon 0:259a31d968a6 120 /**
haarkon 0:259a31d968a6 121 * Set the Kd value
haarkon 0:259a31d968a6 122 *
haarkon 2:d6f14bb935da 123 * @param Kd value
haarkon 2:d6f14bb935da 124 * @return the value of Kd
haarkon 2:d6f14bb935da 125 * @note can also be accessed using the global variable _Kd
haarkon 0:259a31d968a6 126 */
haarkon 0:259a31d968a6 127 float setDerivativeValue (float KdValue);
haarkon 0:259a31d968a6 128
haarkon 0:259a31d968a6 129 /**
haarkon 0:259a31d968a6 130 * compute the PWM value of a motion controled system
haarkon 0:259a31d968a6 131 *
haarkon 2:d6f14bb935da 132 * @param Set point value (in mm/s)
haarkon 2:d6f14bb935da 133 * @return PWM value (between -1 and 1)
haarkon 2:d6f14bb935da 134 * @note this function is provided for users that don't use a 3 wires full bridge
haarkon 0:259a31d968a6 135 */
haarkon 0:259a31d968a6 136 float computePWM (float setPoint);
haarkon 0:259a31d968a6 137
haarkon 0:259a31d968a6 138 /**
haarkon 0:259a31d968a6 139 * Set the Set point value of the speed for integrated full bridge
haarkon 0:259a31d968a6 140 *
haarkon 0:259a31d968a6 141 * @param : Set point value (in mm/s)
haarkon 0:259a31d968a6 142 */
haarkon 0:259a31d968a6 143 void setSpeed (float setPoint);
haarkon 0:259a31d968a6 144
haarkon 0:259a31d968a6 145 /**
haarkon 2:d6f14bb935da 146 * Get position of the wheel (in step of the Quadrature Encoder)
haarkon 2:d6f14bb935da 147 *
haarkon 2:d6f14bb935da 148 * @note One step is a fouth of a period of A or B signal
haarkon 2:d6f14bb935da 149 * @note position is updated each time a motion computation take place
haarkon 2:d6f14bb935da 150 * @return the number of step from beggining
haarkon 0:259a31d968a6 151 */
haarkon 2:d6f14bb935da 152 long getPosition (void);
haarkon 2:d6f14bb935da 153
haarkon 0:259a31d968a6 154 float _Kp, _Ki, _Kd, _PwmValue;
haarkon 2:d6f14bb935da 155 long _Position;
haarkon 2:d6f14bb935da 156 /**
haarkon 2:d6f14bb935da 157 * Global Variable to indicate that required speed is unreachable (=1 if speed is unreachable)
haarkon 2:d6f14bb935da 158 * @note must be cleared by user
haarkon 2:d6f14bb935da 159 */
haarkon 0:259a31d968a6 160 int RobotIsStuck;
haarkon 0:259a31d968a6 161
haarkon 0:259a31d968a6 162 protected :
haarkon 0:259a31d968a6 163
haarkon 0:259a31d968a6 164 Nucleo_Encoder_16_bits _encoder;
haarkon 0:259a31d968a6 165 Ticker _tick;
haarkon 0:259a31d968a6 166 DigitalOut _outA, _outB;
haarkon 0:259a31d968a6 167 PwmOut _pwm;
haarkon 0:259a31d968a6 168
haarkon 0:259a31d968a6 169 private :
haarkon 0:259a31d968a6 170
haarkon 0:259a31d968a6 171 void controlLoop();
haarkon 0:259a31d968a6 172
haarkon 0:259a31d968a6 173 float _consigne;
haarkon 0:259a31d968a6 174 double _scale, _loopTime;
haarkon 0:259a31d968a6 175 };
haarkon 0:259a31d968a6 176 #endif //PID_H