Tuk ZeroFour / PID

29/10/2019

Dependencies:   Encoder_Nucleo_16_bits

Dependents:   Kenya_2019

PID.h@3:93fb84c4e9bc, 2018-05-23 (annotated)

Committer:
haarkon
Date:
Wed May 23 15:08:15 2018 +0000
Revision:
3:93fb84c4e9bc
Parent:
2:d6f14bb935da
Child:
5:d01614d14cd1
Last release before test

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 3:93fb84c4e9bc 28 * motion control.
haarkon 0:259a31d968a6 29 *
haarkon 0:259a31d968a6 30 */
haarkon 0:259a31d968a6 31
haarkon 0:259a31d968a6 32 #ifndef PID_H
haarkon 0:259a31d968a6 33 #define PID_H
haarkon 0:259a31d968a6 34
haarkon 0:259a31d968a6 35 /**
haarkon 2:d6f14bb935da 36 * Includes : Mbed Library + Nucleo_Encoder_16_bits library
haarkon 0:259a31d968a6 37 */
haarkon 0:259a31d968a6 38 #include "mbed.h"
haarkon 0:259a31d968a6 39 #include "Nucleo_Encoder_16_bits.h"
haarkon 0:259a31d968a6 40
haarkon 0:259a31d968a6 41 /**
haarkon 2:d6f14bb935da 42 * PID Motion control system (speed control).
haarkon 2:d6f14bb935da 43 * refere to wikipedia for more explainations : https://en.wikipedia.org/wiki/PID_controller
haarkon 3:93fb84c4e9bc 44 *
haarkon 3:93fb84c4e9bc 45 * @note This library require a tuning (mostly base on try/modify) and will not give a correct performance until you have tuned all 3 parameters Kp, Ki and Kd for each wheels of the robot
haarkon 3:93fb84c4e9bc 46 *
haarkon 3:93fb84c4e9bc 47 * @note You must use a QE (quadrature Encoder) connected to a 16 bits timer to get proper motion control. Pins A & B of the QE must be connected respectively to pin 1 and 2 of the timer. Typicaly on Nucleo F446RE TIM3 and TIM4 are perfect to do this job. In this case simply use TIM3 or TIM4 as timer parameter.
haarkon 3:93fb84c4e9bc 48 *
haarkon 3:93fb84c4e9bc 49 * @note You must also specify the number of pulses generated by the QE for a 1mm displacement of the wheel. This is the scale parameter
haarkon 3:93fb84c4e9bc 50 *
haarkon 3:93fb84c4e9bc 51 * @note Outputed PWM value evolve from 1 (full PWM fortward) to -1 (full PWM backward). This value can also be found in the global variable : _PwmValue. The PWM value is based on a 1.3 m/s maximum speed.
haarkon 3:93fb84c4e9bc 52 *
haarkon 3:93fb84c4e9bc 53 * @note 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. Kp, Ki and Kd are dependent of the loop time. Changing loop time means changing all the corrector's coefficients. Library use a Ticker to control loop time. 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 3:93fb84c4e9bc 54 *
haarkon 3:93fb84c4e9bc 55 * @note The PID is initialized with Ki = 0, Kd = 0 and Kp = 1
haarkon 3:93fb84c4e9bc 56 * - Increasing Kp will shorten your response time but will also create instability (at beggining overshoots, then instability).
haarkon 3:93fb84c4e9bc 57 * - Adding a bit of Ki will allow your system to bring to 0 the static error (ie : will make the error, between the set point and your mesurment, tend to 0) but might create instability and increase setting time.
haarkon 3:93fb84c4e9bc 58 * - Adding a bit of Kd will stabilize the response (with almost no bad effect, as long as Kd remains small).
haarkon 3:93fb84c4e9bc 59 * @note More info can be found here : https://en.wikipedia.org/wiki/PID_controller
haarkon 0:259a31d968a6 60 */
haarkon 0:259a31d968a6 61 class PID {
haarkon 0:259a31d968a6 62
haarkon 0:259a31d968a6 63 public :
haarkon 0:259a31d968a6 64
haarkon 0:259a31d968a6 65 /**
haarkon 2:d6f14bb935da 66 * Constructor (standard) with full bridge control
haarkon 0:259a31d968a6 67 *
haarkon 3:93fb84c4e9bc 68 * @param _TIM (TIM_TypeDef*) : the Mbed 16 bits timer used to connect the A&B output of QE
haarkon 3:93fb84c4e9bc 69 * @param outPWM (PinName) : the Mbed pin used to send PWM on the full bridge
haarkon 3:93fb84c4e9bc 70 * @param outA (PinName) : the Mbed pin used to send direction to the full bridge
haarkon 3:93fb84c4e9bc 71 * @param outB (PinName) : the Mbed pin used to send direction (in case of 3 pins full bridge)
haarkon 3:93fb84c4e9bc 72 * @param scale (double) : the number of pulses on the QE for 1mm displacement of the wheel
haarkon 3:93fb84c4e9bc 73 * @param loopTime (double) : the time between 2 computations
haarkon 0:259a31d968a6 74 */
haarkon 3:93fb84c4e9bc 75 PID(TIM_TypeDef * TIM, PinName outPWM, PinName outA, PinName outB, double scale = 63.661977236758, double loopTime = 0.001);
haarkon 0:259a31d968a6 76
haarkon 0:259a31d968a6 77 /**
haarkon 0:259a31d968a6 78 * Set the Kp value
haarkon 0:259a31d968a6 79 *
haarkon 3:93fb84c4e9bc 80 * @param Kp (float) : value of Kp
haarkon 3:93fb84c4e9bc 81 * @return The value of Kp (float)
haarkon 3:93fb84c4e9bc 82 * @note Can also be accessed using the global variable _Kp
haarkon 0:259a31d968a6 83 */
haarkon 0:259a31d968a6 84 float setProportionnalValue (float KpValue);
haarkon 0:259a31d968a6 85
haarkon 0:259a31d968a6 86 /**
haarkon 0:259a31d968a6 87 * Set the Ki value
haarkon 0:259a31d968a6 88 *
haarkon 3:93fb84c4e9bc 89 * @param Ki (float) : value of Ki
haarkon 3:93fb84c4e9bc 90 * @return The value of Ki (float)
haarkon 3:93fb84c4e9bc 91 * @note Can also be accessed using the global variable _Ki
haarkon 0:259a31d968a6 92 */
haarkon 0:259a31d968a6 93 float setintegralValue (float KiValue);
haarkon 0:259a31d968a6 94
haarkon 0:259a31d968a6 95 /**
haarkon 0:259a31d968a6 96 * Set the Kd value
haarkon 0:259a31d968a6 97 *
haarkon 3:93fb84c4e9bc 98 * @param Kd (float) : value of Kd
haarkon 3:93fb84c4e9bc 99 * @return The value of Kd (float)
haarkon 3:93fb84c4e9bc 100 * @note Can also be accessed using the global variable _Kd
haarkon 0:259a31d968a6 101 */
haarkon 0:259a31d968a6 102 float setDerivativeValue (float KdValue);
haarkon 0:259a31d968a6 103
haarkon 0:259a31d968a6 104 /**
haarkon 0:259a31d968a6 105 * compute the PWM value of a motion controled system
haarkon 0:259a31d968a6 106 *
haarkon 3:93fb84c4e9bc 107 * @param Setpoint value (in mm/s)
haarkon 2:d6f14bb935da 108 * @return PWM value (between -1 and 1)
haarkon 3:93fb84c4e9bc 109 * @note This function is provided for users that don't use a 3 wires full bridge
haarkon 0:259a31d968a6 110 */
haarkon 0:259a31d968a6 111 float computePWM (float setPoint);
haarkon 0:259a31d968a6 112
haarkon 0:259a31d968a6 113 /**
haarkon 0:259a31d968a6 114 * Set the Set point value of the speed for integrated full bridge
haarkon 0:259a31d968a6 115 *
haarkon 0:259a31d968a6 116 * @param : Set point value (in mm/s)
haarkon 0:259a31d968a6 117 */
haarkon 0:259a31d968a6 118 void setSpeed (float setPoint);
haarkon 0:259a31d968a6 119
haarkon 0:259a31d968a6 120 /**
haarkon 2:d6f14bb935da 121 * Get position of the wheel (in step of the Quadrature Encoder)
haarkon 2:d6f14bb935da 122 *
haarkon 2:d6f14bb935da 123 * @note One step is a fouth of a period of A or B signal
haarkon 3:93fb84c4e9bc 124 * @note Position is updated each time a motion computation take place
haarkon 3:93fb84c4e9bc 125 * @return The number of step from beggining
haarkon 0:259a31d968a6 126 */
haarkon 2:d6f14bb935da 127 long getPosition (void);
haarkon 2:d6f14bb935da 128
haarkon 0:259a31d968a6 129 float _Kp, _Ki, _Kd, _PwmValue;
haarkon 2:d6f14bb935da 130 long _Position;
haarkon 2:d6f14bb935da 131 /**
haarkon 2:d6f14bb935da 132 * Global Variable to indicate that required speed is unreachable (=1 if speed is unreachable)
haarkon 3:93fb84c4e9bc 133 * @note Must be cleared by user
haarkon 2:d6f14bb935da 134 */
haarkon 0:259a31d968a6 135 int RobotIsStuck;
haarkon 0:259a31d968a6 136
haarkon 0:259a31d968a6 137 protected :
haarkon 0:259a31d968a6 138
haarkon 3:93fb84c4e9bc 139 Ticker _tick;
haarkon 0:259a31d968a6 140 Nucleo_Encoder_16_bits _encoder;
haarkon 3:93fb84c4e9bc 141 PwmOut _pwm;
haarkon 0:259a31d968a6 142 DigitalOut _outA, _outB;
haarkon 0:259a31d968a6 143
haarkon 0:259a31d968a6 144 private :
haarkon 0:259a31d968a6 145
haarkon 0:259a31d968a6 146 void controlLoop();
haarkon 0:259a31d968a6 147
haarkon 0:259a31d968a6 148 float _consigne;
haarkon 0:259a31d968a6 149 double _scale, _loopTime;
haarkon 0:259a31d968a6 150 };
haarkon 0:259a31d968a6 151 #endif //PID_H