pids.h

Go to the documentation of this file.

/*
 * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT)
 * Copyright (C) 2006-2010 RobotCub Consortium
 * All rights reserved.
 *
 * This software may be modified and distributed under the terms
 * of the BSD-3-Clause license. See the accompanying LICENSE file for
 * details.
*/
 
#ifndef __PIDS_H__
#define __PIDS_H__
 
#include <deque>
 
#include <yarp/os/Bottle.h>
#include <yarp/sig/Vector.h>
#include <yarp/sig/Matrix.h>
 
#include <iCub/ctrl/math.h>
#include <iCub/ctrl/filters.h>
 
 
namespace iCub
{
 
namespace ctrl
{
 
class Integrator
{
private:
    // Default constructor: not implemented.
    Integrator();
 
protected:
    unsigned int dim;
    yarp::sig::Vector y;
    yarp::sig::Vector x_old;
    yarp::sig::Matrix lim;
    double Ts;
    bool   applySat;
 
    void allocate(const Integrator &I);
    yarp::sig::Vector saturate(const yarp::sig::Vector &v);
 
public:
    Integrator(const double _Ts, const yarp::sig::Vector &y0, const yarp::sig::Matrix &_lim);
 
    Integrator(const double _Ts, const yarp::sig::Vector &y0);
 
    Integrator(const Integrator &I) { allocate(I); }
 
    Integrator &operator=(const Integrator &I) { allocate(I); return *this; }
 
    const yarp::sig::Vector& integrate(const yarp::sig::Vector &x);
 
    void setSaturation(bool _applySat);
 
    bool getSaturation() { return applySat; }
 
    void setTs(const double _Ts);
 
    void setLim(const yarp::sig::Matrix &_lim);
 
    double getTs() { return Ts; }
 
    const yarp::sig::Matrix& getLim() { return lim; }
 
    void reset(const yarp::sig::Vector &y0);
 
    const yarp::sig::Vector& get() const { return y; }
};
 
 
class helperPID
{
public:
    static void addVectorToOption(yarp::os::Bottle &option, const char *key, const yarp::sig::Vector &val);
 
    static bool getVectorFromOption(const yarp::os::Bottle &options, const char *key, yarp::sig::Vector &val, int &size);
};
 
 
class parallelPID : public helperPID
{
private:
    // Default constructor: not implemented.
    parallelPID();
 
protected:
    yarp::sig::Vector Kp;
    yarp::sig::Vector Ki;
    yarp::sig::Vector Kd;
 
    yarp::sig::Vector Wp;
    yarp::sig::Vector Wi;
    yarp::sig::Vector Wd;
 
    yarp::sig::Vector N;
    yarp::sig::Vector Tt;
    yarp::sig::Matrix satLim;
 
    yarp::sig::Vector P;
    yarp::sig::Vector I;
    yarp::sig::Vector D;
    yarp::sig::Vector u;
    yarp::sig::Vector uSat;
 
    unsigned int dim;
    double Ts;
 
    Integrator          *Int;
    std::deque<Filter*>  Der;
 
public:
    parallelPID(const double _Ts,
                const yarp::sig::Vector &_Kp, const yarp::sig::Vector &_Ki, const yarp::sig::Vector &_Kd,
                const yarp::sig::Vector &_Wp, const yarp::sig::Vector &_Wi, const yarp::sig::Vector &_Wd,
                const yarp::sig::Vector &_N,  const yarp::sig::Vector &_Tt, const yarp::sig::Matrix &_satLim);
 
    virtual const yarp::sig::Vector& compute(const yarp::sig::Vector &ref, const yarp::sig::Vector &fb);
 
    virtual void reset(const yarp::sig::Vector &u0);
 
    virtual void getOptions(yarp::os::Bottle &options);
 
    virtual void setOptions(const yarp::os::Bottle &options);
 
    ~parallelPID();
};
 
 
class seriesPID : public helperPID
{
private:
    // Default constructor: not implemented.
    seriesPID();
 
protected:
    yarp::sig::Vector Kp;
    yarp::sig::Vector Ti;
    yarp::sig::Vector Kd;
 
    yarp::sig::Vector N;
    yarp::sig::Matrix satLim;    
 
    yarp::sig::Vector e;
    yarp::sig::Vector P;
    yarp::sig::Vector I;
    yarp::sig::Vector D;
    yarp::sig::Vector u;
    yarp::sig::Vector uSat;
 
    unsigned int dim;
    double Ts;
 
    std::deque<Filter*> Int;
    std::deque<Filter*> Der;
 
public:
    seriesPID(const double _Ts,
              const yarp::sig::Vector &_Kp, const yarp::sig::Vector &_Ti, const yarp::sig::Vector &_Kd,
              const yarp::sig::Vector &_N,  const yarp::sig::Matrix &_satLim);
 
    virtual const yarp::sig::Vector& compute(const yarp::sig::Vector &ref, const yarp::sig::Vector &fb);
 
    virtual void reset();
 
    virtual void getOptions(yarp::os::Bottle &options);
 
    virtual void setOptions(const yarp::os::Bottle &options);
 
    ~seriesPID();
};
 
}
 
}
 
#endif