FRC - Hackathon / Mbed 2 deprecated TestPID

programme de test de la bibliothèque d'asservissement PID

Dependencies:   Encoder_Nucleo_16_bits mbed

PID.h@0:86c5a1f6e21d, 2018-06-05 (annotated)

Committer:
haarkon
Date:
Tue Jun 05 08:39:37 2018 +0000
Revision:
0:86c5a1f6e21d
testlib (not functionning)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
haarkon 0:86c5a1f6e21d 1 /**
haarkon 0:86c5a1f6e21d 2 * @author Hugues Angelis
haarkon 0:86c5a1f6e21d 3 *
haarkon 0:86c5a1f6e21d 4 * @section LICENSE
haarkon 0:86c5a1f6e21d 5 *
haarkon 0:86c5a1f6e21d 6 * Copyright (c) 2010 ARM Limited
haarkon 0:86c5a1f6e21d 7 *
haarkon 0:86c5a1f6e21d 8 * Permission is hereby granted, free of charge, to any person obtaining a copy
haarkon 0:86c5a1f6e21d 9 * of this software and associated documentation files (the "Software"), to deal
haarkon 0:86c5a1f6e21d 10 * in the Software without restriction, including without limitation the rights
haarkon 0:86c5a1f6e21d 11 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
haarkon 0:86c5a1f6e21d 12 * copies of the Software, and to permit persons to whom the Software is
haarkon 0:86c5a1f6e21d 13 * furnished to do so, subject to the following conditions:
haarkon 0:86c5a1f6e21d 14 *
haarkon 0:86c5a1f6e21d 15 * The above copyright notice and this permission notice shall be included in
haarkon 0:86c5a1f6e21d 16 * all copies or substantial portions of the Software.
haarkon 0:86c5a1f6e21d 17 *
haarkon 0:86c5a1f6e21d 18 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
haarkon 0:86c5a1f6e21d 19 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
haarkon 0:86c5a1f6e21d 20 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
haarkon 0:86c5a1f6e21d 21 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
haarkon 0:86c5a1f6e21d 22 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
haarkon 0:86c5a1f6e21d 23 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
haarkon 0:86c5a1f6e21d 24 * THE SOFTWARE.
haarkon 0:86c5a1f6e21d 25 *
haarkon 0:86c5a1f6e21d 26 * @section DESCRIPTION
haarkon 0:86c5a1f6e21d 27 *
haarkon 0:86c5a1f6e21d 28 * motion control.
haarkon 0:86c5a1f6e21d 29 *
haarkon 0:86c5a1f6e21d 30 */
haarkon 0:86c5a1f6e21d 31
haarkon 0:86c5a1f6e21d 32 #ifndef PID_H
haarkon 0:86c5a1f6e21d 33 #define PID_H
haarkon 0:86c5a1f6e21d 34
haarkon 0:86c5a1f6e21d 35 #define PI 3.1415926535898
haarkon 0:86c5a1f6e21d 36
haarkon 0:86c5a1f6e21d 37 /**
haarkon 0:86c5a1f6e21d 38 * Includes : Mbed Library + Nucleo_Encoder_16_bits library
haarkon 0:86c5a1f6e21d 39 */
haarkon 0:86c5a1f6e21d 40 #include "mbed.h"
haarkon 0:86c5a1f6e21d 41 #include "Nucleo_Encoder_16_bits.h"
haarkon 0:86c5a1f6e21d 42
haarkon 0:86c5a1f6e21d 43 /**
haarkon 0:86c5a1f6e21d 44 * PID Motion control system (speed control).
haarkon 0:86c5a1f6e21d 45 * refere to wikipedia for more explainations : https://en.wikipedia.org/wiki/PID_controller
haarkon 0:86c5a1f6e21d 46 *
haarkon 0:86c5a1f6e21d 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 0:86c5a1f6e21d 48 *
haarkon 0:86c5a1f6e21d 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 0:86c5a1f6e21d 50 *
haarkon 0:86c5a1f6e21d 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 0:86c5a1f6e21d 52 *
haarkon 0:86c5a1f6e21d 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 0:86c5a1f6e21d 54 *
haarkon 0:86c5a1f6e21d 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 0:86c5a1f6e21d 56 *
haarkon 0:86c5a1f6e21d 57 * @note The PID is initialized with Ki = 0, Kd = 0 and Kp = 1
haarkon 0:86c5a1f6e21d 58 * - Increasing Kp will shorten your response time but will also create instability (at beggining overshoots, then instability).
haarkon 0:86c5a1f6e21d 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 0:86c5a1f6e21d 60 * - Adding a bit of Kd will stabilize the response (with almost no bad effect, as long as Kd remains small).
haarkon 0:86c5a1f6e21d 61 * @note More info can be found here : https://en.wikipedia.org/wiki/PID_controller
haarkon 0:86c5a1f6e21d 62 */
haarkon 0:86c5a1f6e21d 63 class PID
haarkon 0:86c5a1f6e21d 64 {
haarkon 0:86c5a1f6e21d 65
haarkon 0:86c5a1f6e21d 66 public :
haarkon 0:86c5a1f6e21d 67
haarkon 0:86c5a1f6e21d 68 /**
haarkon 0:86c5a1f6e21d 69 * Constructor (standard) with full bridge control
haarkon 0:86c5a1f6e21d 70 *
haarkon 0:86c5a1f6e21d 71 * @param LTIM (TIM_TypeDef*) : the Mbed 16 bits timer used to connect the A&B output of left QE
haarkon 0:86c5a1f6e21d 72 * @param RTIM (TIM_TypeDef*) : the Mbed 16 bits timer used to connect the A&B output of right QE
haarkon 0:86c5a1f6e21d 73 * @param LPWM (PinName) : the Mbed pin used to send PWM for the left wheel's full bridge
haarkon 0:86c5a1f6e21d 74 * @param RPWM (PinName) : the Mbed pin used to send PWM for the right wheel's full bridge
haarkon 0:86c5a1f6e21d 75 * @param LDIRA (PinName) : the Mbed pin used to send direction to the Left wheel IN1 full bridge pin
haarkon 0:86c5a1f6e21d 76 * @param LDIRB (PinName) : the Mbed pin used to send direction to the Left wheel IN2 full bridge pin
haarkon 0:86c5a1f6e21d 77 * @param RDIRA (PinName) : the Mbed pin used to send direction to the Right wheel IN1 full bridge pin
haarkon 0:86c5a1f6e21d 78 * @param RDIRB (PinName) : the Mbed pin used to send direction to the Right wheel IN2 full bridge pin
haarkon 0:86c5a1f6e21d 79 */
haarkon 0:86c5a1f6e21d 80 PID(TIM_TypeDef *LTIM, TIM_TypeDef *RTIM, PinName LPWM, PinName RPWM, PinName LDIRA, PinName LDIRB, PinName RDIRA, PinName RDIRB, PinName LED1);
haarkon 0:86c5a1f6e21d 81
haarkon 0:86c5a1f6e21d 82 /**
haarkon 0:86c5a1f6e21d 83 * Set the Kp value
haarkon 0:86c5a1f6e21d 84 *
haarkon 0:86c5a1f6e21d 85 * @param Kp (float) : value of Kp
haarkon 0:86c5a1f6e21d 86 * @return The value of Kp (float)
haarkon 0:86c5a1f6e21d 87 * @note Can also be accessed using the global variable _Kp
haarkon 0:86c5a1f6e21d 88 */
haarkon 0:86c5a1f6e21d 89 float setProportionnalValue (float KpValue);
haarkon 0:86c5a1f6e21d 90
haarkon 0:86c5a1f6e21d 91 /**
haarkon 0:86c5a1f6e21d 92 * Set the Ki value
haarkon 0:86c5a1f6e21d 93 *
haarkon 0:86c5a1f6e21d 94 * @param Ki (float) : value of Ki
haarkon 0:86c5a1f6e21d 95 * @return The value of Ki (float)
haarkon 0:86c5a1f6e21d 96 * @note Can also be accessed using the global variable _Ki
haarkon 0:86c5a1f6e21d 97 */
haarkon 0:86c5a1f6e21d 98 float setintegralValue (float KiValue);
haarkon 0:86c5a1f6e21d 99
haarkon 0:86c5a1f6e21d 100 /**
haarkon 0:86c5a1f6e21d 101 * Set the Kd value
haarkon 0:86c5a1f6e21d 102 *
haarkon 0:86c5a1f6e21d 103 * @param Kd (float) : value of Kd
haarkon 0:86c5a1f6e21d 104 * @return The value of Kd (float)
haarkon 0:86c5a1f6e21d 105 * @note Can also be accessed using the global variable _Kd
haarkon 0:86c5a1f6e21d 106 */
haarkon 0:86c5a1f6e21d 107 float setDerivativeValue (float KdValue);
haarkon 0:86c5a1f6e21d 108
haarkon 0:86c5a1f6e21d 109 /**
haarkon 0:86c5a1f6e21d 110 * Set the Set point value of the speed for integrated full bridge
haarkon 0:86c5a1f6e21d 111 *
haarkon 0:86c5a1f6e21d 112 * @param left (double) : Set point value for left wheel speed (in mm/s)
haarkon 0:86c5a1f6e21d 113 * @param right (double) : Set point value for right wheel speed (in mm/s)
haarkon 0:86c5a1f6e21d 114 */
haarkon 0:86c5a1f6e21d 115 void setSpeed (double left, double right);
haarkon 0:86c5a1f6e21d 116
haarkon 0:86c5a1f6e21d 117 /**
haarkon 0:86c5a1f6e21d 118 * Get position of the robot (in mm for X and Y and radian for Theta)
haarkon 0:86c5a1f6e21d 119 *
haarkon 0:86c5a1f6e21d 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 0:86c5a1f6e21d 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 0:86c5a1f6e21d 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 0:86c5a1f6e21d 123 * @note the position is updated each time a motion computation take place (ie : each milliseconds)
haarkon 0:86c5a1f6e21d 124 */
haarkon 0:86c5a1f6e21d 125 void getPosition (double *x, double *y, double *theta);
haarkon 0:86c5a1f6e21d 126
haarkon 0:86c5a1f6e21d 127 /**
haarkon 0:86c5a1f6e21d 128 * Reset position of the robot (in mm for X and Y and radian for Theta)
haarkon 0:86c5a1f6e21d 129 *
haarkon 0:86c5a1f6e21d 130 * @note the positionis equal to 0 on all 3 axis (cannot be undone)
haarkon 0:86c5a1f6e21d 131 */
haarkon 0:86c5a1f6e21d 132 void resetPosition (void);
haarkon 0:86c5a1f6e21d 133
haarkon 0:86c5a1f6e21d 134 /**
haarkon 0:86c5a1f6e21d 135 * Get speed of the two wheels of the robot
haarkon 0:86c5a1f6e21d 136 *
haarkon 0:86c5a1f6e21d 137 * @param vG (double - passed by reference) : actual speed in mm/s of the left wheel
haarkon 0:86c5a1f6e21d 138 * @param vD (double - passed by reference) : actual speed in mm/s of the right wheel
haarkon 0:86c5a1f6e21d 139 */
haarkon 0:86c5a1f6e21d 140 void getSpeed (double *vG, double *vD);
haarkon 0:86c5a1f6e21d 141
haarkon 0:86c5a1f6e21d 142 /**
haarkon 0:86c5a1f6e21d 143 * Global Variable of corrective values
haarkon 0:86c5a1f6e21d 144 * @note works well with Kp = 2.0, Ki = 0.4 and Kd = 1
haarkon 0:86c5a1f6e21d 145 */
haarkon 0:86c5a1f6e21d 146 double _Kp, _Ki, _Kd;
haarkon 0:86c5a1f6e21d 147 /**
haarkon 0:86c5a1f6e21d 148 * Global Variable of speed
haarkon 0:86c5a1f6e21d 149 */
haarkon 0:86c5a1f6e21d 150 double _SpeedG, _SpeedD;
haarkon 0:86c5a1f6e21d 151 /**
haarkon 0:86c5a1f6e21d 152 * Global Variable of measured speed
haarkon 0:86c5a1f6e21d 153 */
haarkon 0:86c5a1f6e21d 154 double _measSpeedG, _measSpeedD;
haarkon 0:86c5a1f6e21d 155 /**
haarkon 0:86c5a1f6e21d 156 * Global Variable of measured displacement
haarkon 0:86c5a1f6e21d 157 */
haarkon 0:86c5a1f6e21d 158 double _measDistG, _measDistD;
haarkon 0:86c5a1f6e21d 159 /**
haarkon 0:86c5a1f6e21d 160 * Global Variable to indicate that required wheel speed is unreachable (set if speed is unreachable)
haarkon 0:86c5a1f6e21d 161 * @note Must be cleared by user
haarkon 0:86c5a1f6e21d 162 */
haarkon 0:86c5a1f6e21d 163 int _WheelStuckG, _WheelStuckD;
haarkon 0:86c5a1f6e21d 164 /**
haarkon 0:86c5a1f6e21d 165 * Global Variable of wheel PWM value
haarkon 0:86c5a1f6e21d 166 */
haarkon 0:86c5a1f6e21d 167 double _PwmValueG, _PwmValueD;
haarkon 0:86c5a1f6e21d 168 /**
haarkon 0:86c5a1f6e21d 169 * Global Variable of gravity center of robot position (odometric, error increase with time)
haarkon 0:86c5a1f6e21d 170 */
haarkon 0:86c5a1f6e21d 171 double _X, _Y, _THETA;
haarkon 0:86c5a1f6e21d 172
haarkon 0:86c5a1f6e21d 173 /**
haarkon 0:86c5a1f6e21d 174 * Global Variable to indicate that required speed is unreachable (=1 if speed is unreachable)
haarkon 0:86c5a1f6e21d 175 * @note Must be cleared by user
haarkon 0:86c5a1f6e21d 176 */
haarkon 0:86c5a1f6e21d 177 int RobotIsStuck;
haarkon 0:86c5a1f6e21d 178
haarkon 0:86c5a1f6e21d 179 protected :
haarkon 0:86c5a1f6e21d 180
haarkon 0:86c5a1f6e21d 181 Nucleo_Encoder_16_bits _Lencoder, _Rencoder;
haarkon 0:86c5a1f6e21d 182 PwmOut _Lpwm, _Rpwm;
haarkon 0:86c5a1f6e21d 183 DigitalOut _LdirA, _LdirB, _RdirA, _RdirB;
haarkon 0:86c5a1f6e21d 184 DigitalOut _Led1;
haarkon 0:86c5a1f6e21d 185
haarkon 0:86c5a1f6e21d 186 private :
haarkon 0:86c5a1f6e21d 187
haarkon 0:86c5a1f6e21d 188 void controlLoop();
haarkon 0:86c5a1f6e21d 189
haarkon 0:86c5a1f6e21d 190 };
haarkon 0:86c5a1f6e21d 191 #endif //PID_H