File content as of revision 1:24c444f3716b:

/**
 * @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