test
Dependencies: Encoder_Nucleo_16_bits
Diff: PID.h
- Revision:
- 0:259a31d968a6
- Child:
- 1:24c444f3716b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.h Sun May 20 08:34:23 2018 +0000 @@ -0,0 +1,173 @@ +/** + * @author Hugues Angelis + * + * @section LICENSE + * + * Copyright (c) 2010 ARM Limited + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * @section DESCRIPTION + * + * motion control PID speed controler library. + * + * 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. + * + * @note : + * 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. + * + * Typicaly on Nucleo F446RE TIM3 and TIM4 are perfect to do this job. + * In this case simply use TIM3 or TIM4 as timer parameter + * + * You must also provide the number of pulses generated by the QE for a + * 1mm displacement of the wheel. + * + * You may also add 2 or 3 outputs to drive a PWM full bridge : + * - 2 outputs for DIR/PWM full bridge + * - 3 outputs for IN+/IN-/PWM full bridge. + * Or you may not define any outputs. + * + * 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. + * Value evolve from 1 (full PWM fortward) to -1 (full PWM backward). + * This value can also be found in the global variable : _PwmValue + * + * @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 mean changing the + * corrector's coefficients. + * + * We 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. + * + * 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. + * + * @note : + * The PID is initialized with Ki = 0, Kd = 0 and Kp = 1 + * - Increasing Kp will shorten your response time but will also create + * instability (at beggining overshoots, then instability). + * - 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. + * - Adding a bit of Kd will stabilize the response (with almost no bad + * effect, as long as Kd remains small). + * + * more info can be found here : https://en.wikipedia.org/wiki/PID_controller + */ + +#ifndef PID_H +#define PID_H + +/** + * Includes + */ +#include "mbed.h" +#include "Nucleo_Encoder_16_bits.h" + +/** + * PID Motion control system + */ +class PID { + +public : + + /** + * Constructor (standard) without full bridge control + * + * @param _TIM is the Mbed 16 bits timer used to connect the A&B output of QE + * @param scale is the number of pulses on the QE for 1mm displacement of the wheel + * @param outPWM is the Mbed pin used to send PWM on the full bridge + * @param outDir is the Mbed pin used to send direction to the full bridge + * @param invDir is the Mbed pin used to send direction (in case of 3 pins full bridge) + */ + PID(TIM_TypeDef * _TIM, PinName outPWM, PinName outA, PinName outB, double scale = 31.8309886184, double loopTime = 0.001); + + /** + * Set the Kp value + * + * @param : Kp value + * @return : the value of Kp + * @note : can also be accessed using the global variable _Kp + */ + float setProportionnalValue (float KpValue); + + /** + * Set the Ki value + * + * @param : Ki value + * @return : the value of Ki + * @note : can also be accessed using the global variable _Ki + */ + float setintegralValue (float KiValue); + + /** + * Set the Kd value + * + * @param : Kd value + * @return : the value of Kd + * @note : can also be accessed using the global variable _Kd + */ + float setDerivativeValue (float KdValue); + + /** + * compute the PWM value of a motion controled system + * + * @param : Set point value (in mm/s) + * @return : PWM value (between -1 and 1) + */ + float computePWM (float setPoint); + + /** + * Set the Set point value of the speed for integrated full bridge + * + * @param : Set point value (in mm/s) + */ + void setSpeed (float setPoint); + + /** + * Global Variable + */ + float _Kp, _Ki, _Kd, _PwmValue; + int RobotIsStuck; + +protected : + + Nucleo_Encoder_16_bits _encoder; + Ticker _tick; + DigitalOut _outA, _outB; + PwmOut _pwm; + +private : + + void controlLoop(); + + float _consigne; + double _scale, _loopTime; +}; +#endif //PID_H \ No newline at end of file