test
Dependencies: Encoder_Nucleo_16_bits
PID.h
- Committer:
- haarkon
- Date:
- 2018-05-21
- Revision:
- 1:24c444f3716b
- Parent:
- 0:259a31d968a6
- Child:
- 2:d6f14bb935da
File content as of revision 1:24c444f3716b:
/** * @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