FRC - Hackathon / PID

Motion Control library for 2, 3 inputs Full Bridge + Quadrature Encoder, motor system (also called a 2 wheels robot)

Dependencies:   Encoder_Nucleo_16_bits

Dependents:   FRC_2018 FRC2018_Bis 0hackton_08_06_18 lib_FRC_2019 ... more

PID.h@8:4553677e8b99, 2018-06-05 (annotated)

Committer:
haarkon
Date:
Tue Jun 05 13:33:28 2018 +0000
Revision:
8:4553677e8b99
Parent:
7:78cdcb637927
Child:
9:e7c306f7e579
Tested valid program

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 8:4553677e8b99 35 #define PI 3.1415926535898
haarkon 8:4553677e8b99 36
haarkon 0:259a31d968a6 37 /**
haarkon 2:d6f14bb935da 38 * Includes : Mbed Library + Nucleo_Encoder_16_bits library
haarkon 0:259a31d968a6 39 */
haarkon 0:259a31d968a6 40 #include "mbed.h"
haarkon 0:259a31d968a6 41 #include "Nucleo_Encoder_16_bits.h"
haarkon 0:259a31d968a6 42
haarkon 0:259a31d968a6 43 /**
haarkon 2:d6f14bb935da 44 * PID Motion control system (speed control).
haarkon 2:d6f14bb935da 45 * refere to wikipedia for more explainations : https://en.wikipedia.org/wiki/PID_controller
haarkon 3:93fb84c4e9bc 46 *
haarkon 3:93fb84c4e9bc 47 * @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 48 *
haarkon 8:4553677e8b99 49 * @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 50 *
haarkon 8:4553677e8b99 51 * @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 52 *
haarkon 3:93fb84c4e9bc 53 * @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 8:4553677e8b99 54 *
haarkon 3:93fb84c4e9bc 55 * @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 56 *
haarkon 3:93fb84c4e9bc 57 * @note The PID is initialized with Ki = 0, Kd = 0 and Kp = 1
haarkon 3:93fb84c4e9bc 58 * - Increasing Kp will shorten your response time but will also create instability (at beggining overshoots, then instability).
haarkon 3:93fb84c4e9bc 59 * - 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 60 * - Adding a bit of Kd will stabilize the response (with almost no bad effect, as long as Kd remains small).
haarkon 8:4553677e8b99 61 * @note More info can be found here : https://en.wikipedia.org/wiki/PID_controller
haarkon 0:259a31d968a6 62 */
haarkon 8:4553677e8b99 63 class PID
haarkon 8:4553677e8b99 64 {
haarkon 0:259a31d968a6 65
haarkon 0:259a31d968a6 66 public :
haarkon 0:259a31d968a6 67
haarkon 0:259a31d968a6 68 /**
haarkon 2:d6f14bb935da 69 * Constructor (standard) with full bridge control
haarkon 0:259a31d968a6 70 *
haarkon 5:d01614d14cd1 71 * @param LTIM (TIM_TypeDef*) : the Mbed 16 bits timer used to connect the A&B output of left QE
haarkon 5:d01614d14cd1 72 * @param RTIM (TIM_TypeDef*) : the Mbed 16 bits timer used to connect the A&B output of right QE
haarkon 5:d01614d14cd1 73 * @param LPWM (PinName) : the Mbed pin used to send PWM for the left wheel's full bridge
haarkon 5:d01614d14cd1 74 * @param RPWM (PinName) : the Mbed pin used to send PWM for the right wheel's full bridge
haarkon 5:d01614d14cd1 75 * @param LDIRA (PinName) : the Mbed pin used to send direction to the Left wheel IN1 full bridge pin
haarkon 5:d01614d14cd1 76 * @param LDIRB (PinName) : the Mbed pin used to send direction to the Left wheel IN2 full bridge pin
haarkon 5:d01614d14cd1 77 * @param RDIRA (PinName) : the Mbed pin used to send direction to the Right wheel IN1 full bridge pin
haarkon 5:d01614d14cd1 78 * @param RDIRB (PinName) : the Mbed pin used to send direction to the Right wheel IN2 full bridge pin
haarkon 0:259a31d968a6 79 */
haarkon 5:d01614d14cd1 80 PID(TIM_TypeDef *LTIM, TIM_TypeDef *RTIM, PinName LPWM, PinName RPWM, PinName LDIRA, PinName LDIRB, PinName RDIRA, PinName RDIRB);
haarkon 0:259a31d968a6 81
haarkon 0:259a31d968a6 82 /**
haarkon 0:259a31d968a6 83 * Set the Kp value
haarkon 0:259a31d968a6 84 *
haarkon 3:93fb84c4e9bc 85 * @param Kp (float) : value of Kp
haarkon 3:93fb84c4e9bc 86 * @return The value of Kp (float)
haarkon 3:93fb84c4e9bc 87 * @note Can also be accessed using the global variable _Kp
haarkon 0:259a31d968a6 88 */
haarkon 0:259a31d968a6 89 float setProportionnalValue (float KpValue);
haarkon 8:4553677e8b99 90
haarkon 0:259a31d968a6 91 /**
haarkon 0:259a31d968a6 92 * Set the Ki value
haarkon 0:259a31d968a6 93 *
haarkon 3:93fb84c4e9bc 94 * @param Ki (float) : value of Ki
haarkon 3:93fb84c4e9bc 95 * @return The value of Ki (float)
haarkon 3:93fb84c4e9bc 96 * @note Can also be accessed using the global variable _Ki
haarkon 0:259a31d968a6 97 */
haarkon 0:259a31d968a6 98 float setintegralValue (float KiValue);
haarkon 8:4553677e8b99 99
haarkon 0:259a31d968a6 100 /**
haarkon 0:259a31d968a6 101 * Set the Kd value
haarkon 0:259a31d968a6 102 *
haarkon 3:93fb84c4e9bc 103 * @param Kd (float) : value of Kd
haarkon 3:93fb84c4e9bc 104 * @return The value of Kd (float)
haarkon 3:93fb84c4e9bc 105 * @note Can also be accessed using the global variable _Kd
haarkon 0:259a31d968a6 106 */
haarkon 0:259a31d968a6 107 float setDerivativeValue (float KdValue);
haarkon 8:4553677e8b99 108
haarkon 0:259a31d968a6 109 /**
haarkon 0:259a31d968a6 110 * Set the Set point value of the speed for integrated full bridge
haarkon 0:259a31d968a6 111 *
haarkon 5:d01614d14cd1 112 * @param left (double) : Set point value for left wheel speed (in mm/s)
haarkon 5:d01614d14cd1 113 * @param right (double) : Set point value for right wheel speed (in mm/s)
haarkon 5:d01614d14cd1 114 */
haarkon 5:d01614d14cd1 115 void setSpeed (double left, double right);
haarkon 8:4553677e8b99 116
haarkon 5:d01614d14cd1 117 /**
haarkon 5:d01614d14cd1 118 * Get position of the robot (in mm for X and Y and radian for Theta)
haarkon 5:d01614d14cd1 119 *
haarkon 5:d01614d14cd1 120 * @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)
haarkon 5:d01614d14cd1 121 * @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)
haarkon 5:d01614d14cd1 122 * @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
haarkon 5:d01614d14cd1 123 * @note the position is updated each time a motion computation take place (ie : each milliseconds)
haarkon 0:259a31d968a6 124 */
haarkon 5:d01614d14cd1 125 void getPosition (double *x, double *y, double *theta);
haarkon 5:d01614d14cd1 126
haarkon 5:d01614d14cd1 127 /**
haarkon 5:d01614d14cd1 128 * Reset position of the robot (in mm for X and Y and radian for Theta)
haarkon 5:d01614d14cd1 129 *
haarkon 5:d01614d14cd1 130 * @note the positionis equal to 0 on all 3 axis (cannot be undone)
haarkon 5:d01614d14cd1 131 */
haarkon 5:d01614d14cd1 132 void resetPosition (void);
haarkon 5:d01614d14cd1 133
haarkon 5:d01614d14cd1 134 /**
haarkon 5:d01614d14cd1 135 * Get speed of the two wheels of the robot
haarkon 5:d01614d14cd1 136 *
haarkon 5:d01614d14cd1 137 * @param vG (double - passed by reference) : actual speed in mm/s of the left wheel
haarkon 5:d01614d14cd1 138 * @param vD (double - passed by reference) : actual speed in mm/s of the right wheel
haarkon 5:d01614d14cd1 139 */
haarkon 5:d01614d14cd1 140 void getSpeed (double *vG, double *vD);
haarkon 8:4553677e8b99 141
haarkon 0:259a31d968a6 142 /**
haarkon 5:d01614d14cd1 143 * Global Variable of corrective values
haarkon 5:d01614d14cd1 144 * @note works well with Kp = 2.0, Ki = 0.4 and Kd = 1
haarkon 5:d01614d14cd1 145 */
haarkon 5:d01614d14cd1 146 double _Kp, _Ki, _Kd;
haarkon 5:d01614d14cd1 147 /**
haarkon 5:d01614d14cd1 148 * Global Variable of speed
haarkon 5:d01614d14cd1 149 */
haarkon 5:d01614d14cd1 150 double _SpeedG, _SpeedD;
haarkon 5:d01614d14cd1 151 /**
haarkon 5:d01614d14cd1 152 * Global Variable of measured speed
haarkon 5:d01614d14cd1 153 */
haarkon 5:d01614d14cd1 154 double _measSpeedG, _measSpeedD;
haarkon 5:d01614d14cd1 155 /**
haarkon 5:d01614d14cd1 156 * Global Variable of measured displacement
haarkon 0:259a31d968a6 157 */
haarkon 5:d01614d14cd1 158 double _measDistG, _measDistD;
haarkon 5:d01614d14cd1 159 /**
haarkon 5:d01614d14cd1 160 * Global Variable to indicate that required wheel speed is unreachable (set if speed is unreachable)
haarkon 5:d01614d14cd1 161 * @note Must be cleared by user
haarkon 5:d01614d14cd1 162 */
haarkon 5:d01614d14cd1 163 int _WheelStuckG, _WheelStuckD;
haarkon 5:d01614d14cd1 164 /**
haarkon 5:d01614d14cd1 165 * Global Variable of wheel PWM value
haarkon 5:d01614d14cd1 166 */
haarkon 5:d01614d14cd1 167 double _PwmValueG, _PwmValueD;
haarkon 5:d01614d14cd1 168 /**
haarkon 5:d01614d14cd1 169 * Global Variable of gravity center of robot position (odometric, error increase with time)
haarkon 5:d01614d14cd1 170 */
haarkon 5:d01614d14cd1 171 double _X, _Y, _THETA;
haarkon 5:d01614d14cd1 172
haarkon 2:d6f14bb935da 173 /**
haarkon 2:d6f14bb935da 174 * Global Variable to indicate that required speed is unreachable (=1 if speed is unreachable)
haarkon 3:93fb84c4e9bc 175 * @note Must be cleared by user
haarkon 2:d6f14bb935da 176 */
haarkon 8:4553677e8b99 177 int RobotIsStuck;
haarkon 0:259a31d968a6 178
haarkon 8:4553677e8b99 179 protected :
haarkon 0:259a31d968a6 180
haarkon 5:d01614d14cd1 181 Nucleo_Encoder_16_bits _Lencoder, _Rencoder;
haarkon 5:d01614d14cd1 182 PwmOut _Lpwm, _Rpwm;
haarkon 5:d01614d14cd1 183 DigitalOut _LdirA, _LdirB, _RdirA, _RdirB;
haarkon 8:4553677e8b99 184
haarkon 8:4553677e8b99 185 private :
haarkon 8:4553677e8b99 186
haarkon 8:4553677e8b99 187 Ticker _tick;
haarkon 8:4553677e8b99 188 void controlLoop();
haarkon 8:4553677e8b99 189
haarkon 0:259a31d968a6 190 };
haarkon 0:259a31d968a6 191 #endif //PID_H