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@15:5dfa92698884, 2019-10-25 (annotated)

Committer:
haarkon
Date:
Fri Oct 25 09:41:31 2019 +0000
Revision:
15:5dfa92698884
Parent:
10:a852385c6a34 
New upda

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 9:e7c306f7e579 62 *
haarkon 9:e7c306f7e579 63 * @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.
haarkon 9:e7c306f7e579 64 *
haarkon 9:e7c306f7e579 65 * @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
haarkon 0:259a31d968a6 66 */
haarkon 8:4553677e8b99 67 class PID
haarkon 8:4553677e8b99 68 {
haarkon 0:259a31d968a6 69
haarkon 0:259a31d968a6 70 public :
haarkon 0:259a31d968a6 71
haarkon 0:259a31d968a6 72 /**
haarkon 2:d6f14bb935da 73 * Constructor (standard) with full bridge control
haarkon 0:259a31d968a6 74 *
haarkon 5:d01614d14cd1 75 * @param LTIM (TIM_TypeDef*) : the Mbed 16 bits timer used to connect the A&B output of left QE
haarkon 5:d01614d14cd1 76 * @param RTIM (TIM_TypeDef*) : the Mbed 16 bits timer used to connect the A&B output of right QE
haarkon 5:d01614d14cd1 77 * @param LPWM (PinName) : the Mbed pin used to send PWM for the left wheel's full bridge
haarkon 5:d01614d14cd1 78 * @param RPWM (PinName) : the Mbed pin used to send PWM for the right wheel's full bridge
haarkon 5:d01614d14cd1 79 * @param LDIRA (PinName) : the Mbed pin used to send direction to the Left wheel IN1 full bridge pin
haarkon 5:d01614d14cd1 80 * @param LDIRB (PinName) : the Mbed pin used to send direction to the Left wheel IN2 full bridge pin
haarkon 5:d01614d14cd1 81 * @param RDIRA (PinName) : the Mbed pin used to send direction to the Right wheel IN1 full bridge pin
haarkon 5:d01614d14cd1 82 * @param RDIRB (PinName) : the Mbed pin used to send direction to the Right wheel IN2 full bridge pin
haarkon 0:259a31d968a6 83 */
haarkon 5:d01614d14cd1 84 PID(TIM_TypeDef *LTIM, TIM_TypeDef *RTIM, PinName LPWM, PinName RPWM, PinName LDIRA, PinName LDIRB, PinName RDIRA, PinName RDIRB);
haarkon 0:259a31d968a6 85
haarkon 0:259a31d968a6 86 /**
haarkon 0:259a31d968a6 87 * Set the Kp value
haarkon 0:259a31d968a6 88 *
haarkon 3:93fb84c4e9bc 89 * @param Kp (float) : value of Kp
haarkon 3:93fb84c4e9bc 90 * @return The value of Kp (float)
haarkon 3:93fb84c4e9bc 91 * @note Can also be accessed using the global variable _Kp
haarkon 0:259a31d968a6 92 */
haarkon 0:259a31d968a6 93 float setProportionnalValue (float KpValue);
haarkon 8:4553677e8b99 94
haarkon 0:259a31d968a6 95 /**
haarkon 0:259a31d968a6 96 * Set the Ki value
haarkon 0:259a31d968a6 97 *
haarkon 3:93fb84c4e9bc 98 * @param Ki (float) : value of Ki
haarkon 3:93fb84c4e9bc 99 * @return The value of Ki (float)
haarkon 3:93fb84c4e9bc 100 * @note Can also be accessed using the global variable _Ki
haarkon 0:259a31d968a6 101 */
haarkon 0:259a31d968a6 102 float setintegralValue (float KiValue);
haarkon 8:4553677e8b99 103
haarkon 0:259a31d968a6 104 /**
haarkon 0:259a31d968a6 105 * Set the Kd value
haarkon 0:259a31d968a6 106 *
haarkon 3:93fb84c4e9bc 107 * @param Kd (float) : value of Kd
haarkon 3:93fb84c4e9bc 108 * @return The value of Kd (float)
haarkon 3:93fb84c4e9bc 109 * @note Can also be accessed using the global variable _Kd
haarkon 0:259a31d968a6 110 */
haarkon 0:259a31d968a6 111 float setDerivativeValue (float KdValue);
haarkon 8:4553677e8b99 112
haarkon 0:259a31d968a6 113 /**
haarkon 0:259a31d968a6 114 * Set the Set point value of the speed for integrated full bridge
haarkon 0:259a31d968a6 115 *
haarkon 5:d01614d14cd1 116 * @param left (double) : Set point value for left wheel speed (in mm/s)
haarkon 5:d01614d14cd1 117 * @param right (double) : Set point value for right wheel speed (in mm/s)
haarkon 5:d01614d14cd1 118 */
haarkon 5:d01614d14cd1 119 void setSpeed (double left, double right);
haarkon 8:4553677e8b99 120
haarkon 5:d01614d14cd1 121 /**
haarkon 5:d01614d14cd1 122 * Get position of the robot (in mm for X and Y and radian for Theta)
haarkon 5:d01614d14cd1 123 *
haarkon 5:d01614d14cd1 124 * @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 125 * @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 126 * @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 127 * @note the position is updated each time a motion computation take place (ie : each milliseconds)
haarkon 0:259a31d968a6 128 */
haarkon 5:d01614d14cd1 129 void getPosition (double *x, double *y, double *theta);
haarkon 5:d01614d14cd1 130
haarkon 5:d01614d14cd1 131 /**
haarkon 5:d01614d14cd1 132 * Reset position of the robot (in mm for X and Y and radian for Theta)
haarkon 5:d01614d14cd1 133 *
haarkon 5:d01614d14cd1 134 * @note the positionis equal to 0 on all 3 axis (cannot be undone)
haarkon 5:d01614d14cd1 135 */
haarkon 5:d01614d14cd1 136 void resetPosition (void);
haarkon 5:d01614d14cd1 137
haarkon 5:d01614d14cd1 138 /**
haarkon 5:d01614d14cd1 139 * Get speed of the two wheels of the robot
haarkon 5:d01614d14cd1 140 *
haarkon 5:d01614d14cd1 141 * @param vG (double - passed by reference) : actual speed in mm/s of the left wheel
haarkon 5:d01614d14cd1 142 * @param vD (double - passed by reference) : actual speed in mm/s of the right wheel
haarkon 5:d01614d14cd1 143 */
haarkon 5:d01614d14cd1 144 void getSpeed (double *vG, double *vD);
haarkon 8:4553677e8b99 145
haarkon 0:259a31d968a6 146 /**
haarkon 9:e7c306f7e579 147 * Global Variable of corrective proportionnal value
haarkon 9:e7c306f7e579 148 * @note works well with Kp = 2.0, Ki = 0.4 and Kd = 1
haarkon 9:e7c306f7e579 149 */
haarkon 9:e7c306f7e579 150 double _Kp;
haarkon 9:e7c306f7e579 151 /**
haarkon 9:e7c306f7e579 152 * Global Variable of corrective integral value
haarkon 9:e7c306f7e579 153 * @note works well with Kp = 2.0, Ki = 0.4 and Kd = 1
haarkon 9:e7c306f7e579 154 */
haarkon 9:e7c306f7e579 155 double _Ki;
haarkon 9:e7c306f7e579 156 /**
haarkon 9:e7c306f7e579 157 * Global Variable of corrective derivative value
haarkon 5:d01614d14cd1 158 * @note works well with Kp = 2.0, Ki = 0.4 and Kd = 1
haarkon 5:d01614d14cd1 159 */
haarkon 9:e7c306f7e579 160 double _Kd;
haarkon 5:d01614d14cd1 161 /**
haarkon 9:e7c306f7e579 162 * Global Variable of Left speed
haarkon 5:d01614d14cd1 163 */
haarkon 10:a852385c6a34 164 double _SpeedG;
haarkon 5:d01614d14cd1 165 /**
haarkon 9:e7c306f7e579 166 * Global Variable of Right speed
haarkon 9:e7c306f7e579 167 */
haarkon 10:a852385c6a34 168 double _SpeedD;
haarkon 9:e7c306f7e579 169 /**
haarkon 9:e7c306f7e579 170 * Global Variable of measured left speed
haarkon 5:d01614d14cd1 171 */
haarkon 9:e7c306f7e579 172 double _measSpeedG;
haarkon 9:e7c306f7e579 173 /**
haarkon 9:e7c306f7e579 174 * Global Variable of measured right speed
haarkon 9:e7c306f7e579 175 */
haarkon 9:e7c306f7e579 176 double _measSpeedD;
haarkon 5:d01614d14cd1 177 /**
haarkon 9:e7c306f7e579 178 * Global Variable of measured Left wheel displacement
haarkon 0:259a31d968a6 179 */
haarkon 9:e7c306f7e579 180 double _measDistG;
haarkon 5:d01614d14cd1 181 /**
haarkon 9:e7c306f7e579 182 * Global Variable of measured Right wheel displacement
haarkon 9:e7c306f7e579 183 */
haarkon 9:e7c306f7e579 184 double _measDistD;
haarkon 9:e7c306f7e579 185 /**
haarkon 9:e7c306f7e579 186 * Global Variable to indicate that required Left wheel speed is unreachable (set if speed is unreachable)
haarkon 5:d01614d14cd1 187 * @note Must be cleared by user
haarkon 5:d01614d14cd1 188 */
haarkon 9:e7c306f7e579 189 int _WheelStuckG;
haarkon 5:d01614d14cd1 190 /**
haarkon 9:e7c306f7e579 191 * Global Variable to indicate that required Right wheel speed is unreachable (set if speed is unreachable)
haarkon 9:e7c306f7e579 192 * @note Must be cleared by user
haarkon 9:e7c306f7e579 193 */
haarkon 9:e7c306f7e579 194 int _WheelStuckD;
haarkon 9:e7c306f7e579 195 /**
haarkon 9:e7c306f7e579 196 * Global Variable of Left wheel PWM value
haarkon 9:e7c306f7e579 197 */
haarkon 9:e7c306f7e579 198 double _PwmValueG;
haarkon 9:e7c306f7e579 199 /**
haarkon 9:e7c306f7e579 200 * Global Variable of Right wheel PWM value
haarkon 5:d01614d14cd1 201 */
haarkon 9:e7c306f7e579 202 double _PwmValueD;
haarkon 9:e7c306f7e579 203 /**
haarkon 9:e7c306f7e579 204 * Global Variable of odometric measurement of X position of gravity center of robot.
haarkon 9:e7c306f7e579 205 * @note odometric error increase with time
haarkon 9:e7c306f7e579 206 */
haarkon 9:e7c306f7e579 207 double _X;
haarkon 5:d01614d14cd1 208 /**
haarkon 9:e7c306f7e579 209 * Global Variable of odometric measurement of Y position of gravity center of robot.
haarkon 9:e7c306f7e579 210 * @note odometric error increase with time
haarkon 5:d01614d14cd1 211 */
haarkon 9:e7c306f7e579 212 double _Y;
haarkon 9:e7c306f7e579 213 /**
haarkon 9:e7c306f7e579 214 * Global Variable of odometric measurement of THETA angle of gravity center of robot.
haarkon 9:e7c306f7e579 215 * @note odometric error increase with time
haarkon 9:e7c306f7e579 216 */
haarkon 9:e7c306f7e579 217 double _THETA;
haarkon 2:d6f14bb935da 218 /**
haarkon 2:d6f14bb935da 219 * Global Variable to indicate that required speed is unreachable (=1 if speed is unreachable)
haarkon 3:93fb84c4e9bc 220 * @note Must be cleared by user
haarkon 2:d6f14bb935da 221 */
haarkon 8:4553677e8b99 222 int RobotIsStuck;
haarkon 0:259a31d968a6 223
haarkon 8:4553677e8b99 224 protected :
haarkon 0:259a31d968a6 225
haarkon 5:d01614d14cd1 226 Nucleo_Encoder_16_bits _Lencoder, _Rencoder;
haarkon 5:d01614d14cd1 227 PwmOut _Lpwm, _Rpwm;
haarkon 5:d01614d14cd1 228 DigitalOut _LdirA, _LdirB, _RdirA, _RdirB;
haarkon 8:4553677e8b99 229
haarkon 8:4553677e8b99 230 private :
haarkon 8:4553677e8b99 231
haarkon 8:4553677e8b99 232 Ticker _tick;
haarkon 8:4553677e8b99 233 void controlLoop();
haarkon 8:4553677e8b99 234
haarkon 0:259a31d968a6 235 };
haarkon 0:259a31d968a6 236 #endif //PID_H