PID.h

00001 /**
00002  * @author Hugues Angelis
00003  *
00004  * @section LICENSE
00005  *
00006  * Copyright (c) 2010 ARM Limited
00007  *
00008  * Permission is hereby granted, free of charge, to any person obtaining a copy
00009  * of this software and associated documentation files (the "Software"), to deal
00010  * in the Software without restriction, including without limitation the rights
00011  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00012  * copies of the Software, and to permit persons to whom the Software is
00013  * furnished to do so, subject to the following conditions:
00014  *
00015  * The above copyright notice and this permission notice shall be included in
00016  * all copies or substantial portions of the Software.
00017  *
00018  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00019  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00020  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00021  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00022  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00023  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00024  * THE SOFTWARE.
00025  *
00026  * @section DESCRIPTION
00027  *
00028  * motion control.
00029  *
00030  */
00031 
00032 #ifndef PID_H
00033 #define PID_H
00034 
00035 #define PI  3.1415926535898
00036 
00037 /**
00038  * Includes : Mbed Library + Nucleo_Encoder_16_bits library
00039  */
00040 #include "mbed.h"
00041 #include "Nucleo_Encoder_16_bits.h"
00042 
00043 /**
00044  * PID Motion control system (speed control).
00045  *  refere to wikipedia for more explainations : https://en.wikipedia.org/wiki/PID_controller
00046  *
00047  * @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
00048  *
00049  * @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.
00050  *
00051  * @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
00052  *
00053  * @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.
00054  *
00055  * @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.
00056  *
00057  * @note The PID is initialized with Ki = 0, Kd = 0 and Kp = 1
00058  *       -  Increasing Kp will shorten your response time but will also create instability (at beggining overshoots, then instability).
00059  *       -  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.
00060  *       -  Adding a bit of Kd will stabilize the response (with almost no bad effect, as long as Kd remains small).
00061  * @note More info can be found here : https://en.wikipedia.org/wiki/PID_controller
00062  *
00063  * @note The odometric position is computed by the trapezoidal integration method (approximation of a part of a circle by a trapezoid) after each program loop. Hence, odometric error increase with time.
00064  *
00065  * @note The odometric X coordinate is the direction in front of the robot at startup. Y coordinate is the orthogonal vector in trigonometric direction (counterclockwise) refered to X, and THETA is the angle of the robot refered to X at startup 
00066  */
00067 class PID
00068 {
00069 
00070 public :
00071 
00072     /**
00073      * Constructor (standard) with full bridge control
00074      *
00075      * @param LTIM (TIM_TypeDef*) : the Mbed 16 bits timer used to connect the A&B output of left QE
00076      * @param RTIM (TIM_TypeDef*) : the Mbed 16 bits timer used to connect the A&B output of right QE
00077      * @param LPWM (PinName) : the Mbed pin used to send PWM for the left wheel's full bridge
00078      * @param RPWM (PinName) : the Mbed pin used to send PWM for the right wheel's full bridge
00079      * @param LDIRA (PinName) : the Mbed pin used to send direction to the Left wheel IN1 full bridge pin
00080      * @param LDIRB (PinName) : the Mbed pin used to send direction to the Left wheel IN2 full bridge pin
00081      * @param RDIRA (PinName) : the Mbed pin used to send direction to the Right wheel IN1 full bridge pin
00082      * @param RDIRB (PinName) : the Mbed pin used to send direction to the Right wheel IN2 full bridge pin
00083      */
00084     PID(TIM_TypeDef *LTIM, TIM_TypeDef *RTIM, PinName LPWM, PinName RPWM, PinName LDIRA, PinName LDIRB, PinName RDIRA, PinName RDIRB);
00085 
00086     /**
00087      * Set the Kp value
00088      *
00089      * @param  Kp (float) : value of Kp
00090      * @return The value of Kp (float)
00091      * @note  Can also be accessed using the global variable _Kp
00092      */
00093     float setProportionnalValue (float KpValue);
00094 
00095     /**
00096      * Set the Ki value
00097      *
00098      * @param  Ki (float) : value of Ki
00099      * @return The value of Ki (float)
00100      * @note  Can also be accessed using the global variable _Ki
00101      */
00102     float setintegralValue (float KiValue);
00103 
00104     /**
00105      * Set the Kd value
00106      *
00107      * @param  Kd (float) : value of Kd
00108      * @return The value of Kd (float)
00109      * @note  Can also be accessed using the global variable _Kd
00110      */
00111     float setDerivativeValue (float KdValue);
00112 
00113     /**
00114      * Set the Set point value of the speed for integrated full bridge
00115      *
00116      * @param left  (double) : Set point value for left  wheel speed (in mm/s)
00117      * @param right (double) : Set point value for right wheel speed (in mm/s)
00118      */
00119     void setSpeed (double left, double right);
00120 
00121     /**
00122      * Get position of the robot (in mm for X and Y and radian for Theta)
00123      *
00124      * @param x (double - passed by reference) : actual position of the center of the robot, in mm, along X axis (front of the robot at startup)
00125      * @param y (double - passed by reference) : actual position of the center of the robot, in mm, along Y axis (left of the robot at startup)
00126      * @param theta (double - passed by reference) : radian angle between the normal to the line passing through the 2 wheels and the angle of the robot at startup
00127      * @note the position is updated each time a motion computation take place (ie : each milliseconds)
00128      */
00129     void getPosition (double *x, double *y, double *theta);
00130 
00131     /**
00132      * Reset position of the robot (in mm for X and Y and radian for Theta)
00133      *
00134      * @note the positionis equal to 0 on all 3 axis (cannot be undone)
00135      */
00136     void resetPosition (void);
00137 
00138     /**
00139      * Get speed of the two wheels of the robot
00140      *
00141      * @param vG (double - passed by reference) : actual speed in mm/s of the left wheel
00142      * @param vD (double - passed by reference) : actual speed in mm/s of the right wheel
00143      */
00144     void getSpeed (double *vG, double *vD);
00145 
00146     /**
00147      * Global Variable of corrective proportionnal value
00148      * @note  works well with Kp = 2.0, Ki = 0.4 and Kd = 1
00149      */
00150     double                  _Kp;
00151     /**
00152      * Global Variable of corrective integral value
00153      * @note  works well with Kp = 2.0, Ki = 0.4 and Kd = 1
00154      */
00155     double                  _Ki;
00156     /**
00157      * Global Variable of corrective derivative value
00158      * @note  works well with Kp = 2.0, Ki = 0.4 and Kd = 1
00159      */
00160     double                  _Kd;
00161     /**
00162      * Global Variable of Left speed
00163      */
00164     double                  _SpeedG;
00165     /**
00166      * Global Variable of Right speed
00167      */
00168     double                  _SpeedD;
00169     /**
00170      * Global Variable of measured left speed
00171      */
00172     double                  _measSpeedG;
00173      /**
00174      * Global Variable of measured right speed
00175      */
00176     double                  _measSpeedD;
00177     /**
00178      * Global Variable of measured Left wheel displacement
00179      */
00180     double                  _measDistG;
00181     /**
00182      * Global Variable of measured Right wheel displacement
00183      */
00184     double                  _measDistD;
00185     /**
00186      * Global Variable to indicate that required Left wheel speed is unreachable (set if speed is unreachable)
00187      * @note    Must be cleared by user
00188      */
00189     int                     _WheelStuckG;
00190     /**
00191      * Global Variable to indicate that required Right wheel speed is unreachable (set if speed is unreachable)
00192      * @note    Must be cleared by user
00193      */
00194     int                     _WheelStuckD;
00195     /**
00196      * Global Variable of Left wheel PWM value
00197      */
00198     double                  _PwmValueG;
00199     /**
00200      * Global Variable of Right wheel PWM value
00201      */
00202     double                  _PwmValueD;
00203     /**
00204      * Global Variable of odometric measurement of X position of gravity center of robot.
00205      * @note    odometric error increase with time
00206      */
00207     double                  _X;
00208     /**
00209      * Global Variable of odometric measurement of Y position of gravity center of robot.
00210      * @note    odometric error increase with time
00211      */
00212     double                  _Y;
00213     /**
00214      * Global Variable of odometric measurement of THETA angle of gravity center of robot.
00215      * @note    odometric error increase with time
00216      */
00217     double                  _THETA;
00218     /**
00219      * Global Variable to indicate that required speed is unreachable (=1 if speed is unreachable)
00220      * @note    Must be cleared by user
00221      */
00222     int                     RobotIsStuck;
00223 
00224 protected :
00225 
00226     Nucleo_Encoder_16_bits  _Lencoder, _Rencoder;
00227     PwmOut                  _Lpwm, _Rpwm;
00228     DigitalOut              _LdirA, _LdirB, _RdirA, _RdirB;
00229 
00230 private :
00231 
00232     Ticker      _tick;
00233     void    controlLoop();
00234 
00235 };
00236 #endif //PID_H