File content as of revision 3:93fb84c4e9bc:

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

#ifndef PID_H
#define PID_H

/**
 * Includes : Mbed Library + Nucleo_Encoder_16_bits library
 */
#include "mbed.h"
#include "Nucleo_Encoder_16_bits.h"

/**
 * PID Motion control system (speed control).
 *  refere to wikipedia for more explainations : https://en.wikipedia.org/wiki/PID_controller
 *
 * @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
 *
 * @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. 
 *
 * @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       
 *
 * @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.
 * 
 * @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.
 *
 * @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).
 * @note More info can be found here : https://en.wikipedia.org/wiki/PID_controller 
 */
class PID {

public :

    /**
     * Constructor (standard) with full bridge control
     *
     * @param _TIM (TIM_TypeDef*) : the Mbed 16 bits timer used to connect the A&B output of QE
     * @param outPWM (PinName) : the Mbed pin used to send PWM on the full bridge
     * @param outA (PinName) : the Mbed pin used to send direction to the full bridge
     * @param outB (PinName) : the Mbed pin used to send direction (in case of 3 pins full bridge)
     * @param scale (double) : the number of pulses on the QE for 1mm displacement of the wheel
     * @param loopTime (double) : the time between 2 computations
     */
    PID(TIM_TypeDef * TIM, PinName outPWM, PinName outA, PinName outB, double scale = 63.661977236758, double loopTime = 0.001);

    /**
     * Set the Kp value
     *
     * @param  Kp (float) : value of Kp
     * @return The value of Kp (float)
     * @note  Can also be accessed using the global variable _Kp
     */
    float setProportionnalValue (float KpValue);
    
    /**
     * Set the Ki value
     *
     * @param  Ki (float) : value of Ki
     * @return The value of Ki (float)
     * @note  Can also be accessed using the global variable _Ki
     */
    float setintegralValue (float KiValue);
    
    /**
     * Set the Kd value
     *
     * @param  Kd (float) : value of Kd
     * @return The value of Kd (float)
     * @note  Can also be accessed using the global variable _Kd
     */
    float setDerivativeValue (float KdValue);
    
    /**
     * compute the PWM value of a motion controled system
     *
     * @param  Setpoint value (in mm/s)
     * @return  PWM value (between -1 and 1)
     * @note    This function is provided for users that don't use a 3 wires full bridge
     */
    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);
    
    /**
     * Get position of the wheel (in step of the Quadrature Encoder)
     *
     * @note One step is a fouth of a period of A or B signal
     * @note Position is updated each time a motion computation take place
     * @return The number of step from beggining
     */
    long getPosition (void);
    
    float   _Kp, _Ki, _Kd, _PwmValue;
    long    _Position;
    /**
     * Global Variable to indicate that required speed is unreachable (=1 if speed is unreachable)
     * @note    Must be cleared by user
     */
    int     RobotIsStuck;  

protected :

    Ticker                  _tick;
    Nucleo_Encoder_16_bits  _encoder;
    PwmOut                  _pwm;
    DigitalOut              _outA, _outB;

private :    

    void    controlLoop();
    
    float   _consigne;
    double  _scale, _loopTime;
};
#endif //PID_H