test
Dependencies: Encoder_Nucleo_16_bits
PID.h@0:259a31d968a6, 2018-05-20 (annotated)
- Committer:
- haarkon
- Date:
- Sun May 20 08:34:23 2018 +0000
- Revision:
- 0:259a31d968a6
- Child:
- 1:24c444f3716b
Untested motion control library for Nucl?o + Quadrature Encoder
Who changed what in which revision?
User | Revision | Line number | New 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 | 0:259a31d968a6 | 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 |