Diff: PID.h

Revision:

0:259a31d968a6

Child:

1:24c444f3716b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.h	Sun May 20 08:34:23 2018 +0000
@@ -0,0 +1,173 @@
+/**
+ * @author Hugues Angelis
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * @section DESCRIPTION
+ *
+ * motion control PID speed controler library.
+ *
+ *  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.
+ *
+ * @note :
+ *      You must provide a 16 bits Timer definition that allow the use of a
+ *      quadrature encoder (pins A & B 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 
+ *
+ *      You must also provide the number of pulses generated by the QE for a
+ *      1mm displacement of the wheel.       
+ *
+ *      You may also add 2 or 3 outputs to drive a PWM full bridge :
+ *          - 2 outputs for DIR/PWM full bridge
+ *          - 3 outputs for IN+/IN-/PWM full bridge.
+ *      Or you may not define any outputs.
+ *
+ *      In this case the PID controler must use a specific function that will
+ *      output a PWM value that user must transform to match the full bridge.
+ *      Value evolve from 1 (full PWM fortward) to -1 (full PWM backward).
+ *      This value can also be found in the global variable : _PwmValue
+ * 
+ * @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 mean changing the
+ *      corrector's coefficients.
+ *
+ *      We 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.
+ *
+ *      Take care not to increase too mutch this time (or you will fail to 
+ *      to control the motion) and to always keep enought time for computation
+ *      to take place during this loop.
+ *
+ * @note :
+ *      The PID is initialized with Ki = 0, Kd = 0 and Kp = 1
+ *       -  Increasing Kp will shorten your response time but will also create
+ *          instability (at beggining overshoots, then instability).
+ *       -  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.
+ *       -  Adding a bit of Kd will stabilize the response (with almost no bad
+ *          effect, as long as Kd remains small).
+ *
+ * more info can be found here : https://en.wikipedia.org/wiki/PID_controller 
+ */
+
+#ifndef PID_H
+#define PID_H
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+#include "Nucleo_Encoder_16_bits.h"
+
+/**
+ * PID Motion control system
+ */
+class PID {
+
+public :
+
+    /**
+     * Constructor (standard) without full bridge control
+     *
+     * @param _TIM is the Mbed 16 bits timer used to connect the A&B output of QE
+     * @param scale is the number of pulses on the QE for 1mm displacement of the wheel
+     * @param outPWM is the Mbed pin used to send PWM on the full bridge
+     * @param outDir is the Mbed pin used to send direction to the full bridge
+     * @param invDir is the Mbed pin used to send direction (in case of 3 pins full bridge)
+     */
+    PID(TIM_TypeDef * _TIM, PinName outPWM, PinName outA, PinName outB, double scale = 31.8309886184, double loopTime = 0.001);
+
+    /**
+     * Set the Kp value
+     *
+     * @param : Kp value
+     * @return : the value of Kp
+     * @note : can also be accessed using the global variable _Kp
+     */
+    float setProportionnalValue (float KpValue);
+    
+    /**
+     * Set the Ki value
+     *
+     * @param : Ki value
+     * @return : the value of Ki
+     * @note : can also be accessed using the global variable _Ki
+     */
+    float setintegralValue (float KiValue);
+    
+    /**
+     * Set the Kd value
+     *
+     * @param : Kd value
+     * @return : the value of Kd
+     * @note : can also be accessed using the global variable _Kd
+     */
+    float setDerivativeValue (float KdValue);
+    
+    /**
+     * compute the PWM value of a motion controled system
+     *
+     * @param : Set point value (in mm/s)
+     * @return : PWM value (between -1 and 1)
+     */
+    float computePWM (float setPoint); 
+    
+    /**
+     * Set the Set point value of the speed for integrated full bridge
+     *
+     * @param : Set point value (in mm/s)
+     */
+    void setSpeed (float setPoint);
+    
+    /**
+     * Global Variable
+     */
+    float   _Kp, _Ki, _Kd, _PwmValue;
+    int     RobotIsStuck;  
+
+protected :
+
+    Nucleo_Encoder_16_bits  _encoder;
+    Ticker                  _tick;
+    DigitalOut              _outA, _outB;
+    PwmOut                  _pwm;
+
+private :    
+
+    void    controlLoop();
+    
+    float   _consigne;
+    double  _scale, _loopTime;
+};
+#endif //PID_H
\ No newline at end of file