PID.hpp

Go to the documentation of this file.

#ifndef CONTROLLERPIDCONTROLLER_H
#define CONTROLLERPIDCONTROLLER_H

#include <math.h>
#include <iostream>
#include <iomanip>
#include <vector>
#include <base/Time.hpp>

bool zeroCrossing(double currValue, double prevValue, double refValue = 0);

bool positiveZeroCrossing(double currValue, double prevValue, double refValue = 0);

bool negativeZeroCrossing(double currValue, double prevValue, double refValue = 0);

namespace motor_controller
{

    struct PIDSettings
    {   
            double Ts;

            double K; 

            double Ti;

            double Td;


            double N;
                     

            double B;


            double Tt;                 


            double YMin;
            double YMax;

        PIDSettings():Ts(0),K(0),Ti(0),Td(0),N(0),B(1),Tt(-1),YMin(0),YMax(0){};

        bool operator==(const PIDSettings& other) const{
            return 
                Ts == other.Ts &&
                K == other.K &&
                Ti == other.Ti &&
                Td == other.Td &&
                N == other.N &&
                B == other.B &&
                Tt == other.Tt &&
                YMin == other.YMin &&
                YMax == other.YMax;
        }

        bool operator!=(const PIDSettings& other) const{
            return !(*this == other);
        }

            void setParallelCoefficients(
                    double _Kp = 0,
                    double _Ki = 0,
                    double _Kd = 0);
    };
    
    struct ParallelPIDSettings
    {   
            double Ts;

            double Kp; 

            double Ki;

            double Kd;


            double N;
                     

            double B;


            double Tt;                 


            double YMin;
            double YMax;

        ParallelPIDSettings():Ts(0),Kp(0),Ki(0),Kd(0),N(0),B(1),Tt(-1),YMin(0),YMax(0){};

        bool operator==(const ParallelPIDSettings& other) const{
            return 
                Ts == other.Ts &&
                Kp == other.Kp &&
                Ki == other.Ki &&
                Kd == other.Kd &&
                N == other.N &&
                B == other.B &&
                Tt == other.Tt &&
                YMin == other.YMin &&
                YMax == other.YMax;
        }

        bool operator!=(const ParallelPIDSettings& other) const{
            return !(*this == other);
        }

            void setIdealCoefficients(
                    double _K = 0,
                    double _Ti = 0,
                    double _Td = 0);
    };

    struct PIDState
    {
        base::Time time;

        bool initialized;
        double P, I, D;
        double input;
        double reference;
        double rawOutput;
        double saturatedOutput;
    };

    enum DerivativeMode
    {
        Error,
                Output
    };

    class PID
    {
        public:
            PID();


            void setParallelCoefficients(double _Ts,
                    double _Kp = 0,
                    double _Ki = 0,
                    double _Kd = 0, 
                    double _N = 0,
                    double _B = 1, 
                    double _Tt = -1,
                    double _YMin = 0, 
                    double _YMax = 0);


            void setIdealCoefficients (double _Ts,
                    double _K = 0 , 
                    double _Ti = 0, 
                    double _Td = 0, 
                    double _N = 0,
                    double _B = 1, 
                    double _Tt = -1,
                    double _YMin = 0, 
                    double _YMax = 0);
        void setParallelPIDSettings(const ParallelPIDSettings &_settings);
            
        void setPIDSettings(const PIDSettings &_settings);

            double saturate ( double _val );

            double update ( double _measuredValue, double _referenceValue, double time = 0.0  );

            void reset();

            void printCoefficients();


                void disableIntegral(); 

            void enableIntegral(); 

            void enableIntegral(double _Ti); 

            bool isIntegralEnabled() const { return bIntegral; }


            void disableDerivative(); 

            void enableDerivative(); 

            void enableDerivative(double _Td); 

            bool isDerivativeEnabled() const { return bDerivative; }

            void disableDerivativeFiltering(); 

            void enableDerivativeFiltering(); 

            void enableDerivativeFiltering(double _N);

            bool isDerivativeFilteringEnabled() const { return bDerivativeFiltering; }


            void setDerivativeMode(DerivativeMode mode);

            DerivativeMode getDerivativeMode() const;

        bool isSaturated();

        PIDState getState() const;

        private:

                struct PIDSettings gains;

            bool initialized;

            double prevValue;

            double prevError;

            double prevReferenceValue;

            double Bi, Ad, Bd, Ao; 
            double P, I, D, rawCommand, saturatedCommand;

            bool bIntegral;
            bool bDerivative;
            bool bDerivativeFiltering;
            DerivativeMode derivativeMode;

            double Kold;
            double Bold;

            bool firstRun;

            bool bSaturated;

            void computeCoefficients();

        // whether the corresponding gains are set
        void autoEnableModes();
    };

    class PIDAutoTuning
    {
        public:
            PIDAutoTuning();
            PIDAutoTuning(double _Ts,
                    double _stepRef = 1.0,
                    double _inputAmplitude = 0.1,
                    double _testTimeSec = 10.0);

            void setCoefficients(double _Ts,
                    double _stepRef = 1.0,
                    double _inputAmplitude = 0.1,
                    double _testTimeSec = 10.0);

            double update(double _measuredValue);

            void getTunedP(double &_Kp); // Tuned value for P controller
            void getTunedPI(double &_Kp, double &_Ti); // Tuned values for interacting PI controller
            void getTunedPID(double &_Kp, double &_Ti, double &_Td); // Tuned values for interacting PID controller

        private:
            double Ts; // Sampling time 
            double testTimeSec; // Total time period of the test

            double stepRef; // Step reference input
            double inputAmplitude; // Amplitude of input signal

            double outputTimePeriodSec; // Time period of output signal
            double outputAmplitude; // Amplitude of output signal

            double initialValue, // Start value of the output
                   prevError; // Measured value in the last step

            double error;

            bool firstRun; // true if first run
            bool firstZeroCrossing;

            double currTime; // Current time
            double deltaOutputTime; // Delta time for output wave

            double maxAmplitude, minAmplitude; // Minimum and maximum amplitude

            double ultimateGain;  // Gain for computation of parameters
    };

    class PIDStepResponseProperties
    {
        public:
            PIDStepResponseProperties() { reset(); };
            PIDStepResponseProperties(double _Ts,
                    double _riseTimeFractionReference = 1.0,
                    double _settlingTimeFractionReference = 0.05);

            void setCoefficients(double _Ts,
                    double _riseTimeFractionReference = 1.0,
                    double _settlingTimeFractionReference = 0.05);

            void reset();

            void update(double _actualOutput,
                    double _refInput);

            void getProperties(double &_riseTimeSec,
                    double &_settlingTimeSec,
                    double &_percentOvershoot,
                    double &_steadyStateError);

            void printProperties();

        private:
            double Ts;

            double riseTimeSec;  // Rise time is seconds
            double settlingTimeSec; // Settling time in seconds
            double percentOvershoot; // Percentage Overshoot
            double steadyStateError; // Steady state error 
            double steadyStateErrorTimeSec;
            double squaredError;
            std::vector<double> maxAmplitudes;
            double maxAmplitude;

            double riseTimeFractionReference; // Measures the rise time to the fraction of reference value 
            double settlingTimeFractionReference; // Measure the settling time limit within the fraction of reference value

            bool firstRun;
            double prevOutput;
            double currTime; // Current time in seconds

            bool riseTimeDetected;
            bool firstZeroCrossing;
    };
}

#endif