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@5:d01614d14cd1, 2018-06-05 (annotated)

Committer:
haarkon
Date:
Tue Jun 05 07:24:40 2018 +0000
Revision:
5:d01614d14cd1
Parent:
3:93fb84c4e9bc
Child:
7:78cdcb637927
not working configuration to test;

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