Gain.hpp

Go to the documentation of this file.

#ifndef ORG_EEROS_CONTROL_GAIN_HPP_
#define ORG_EEROS_CONTROL_GAIN_HPP_

#include <eeros/control/Block1i1o.hpp>
#include <type_traits>


namespace eeros {
    namespace control {

        template <typename Tout = double, typename Tgain = double, bool elementWise = false>
        class Gain : public Block1i1o<Tout> {

        public:
            Gain() : Gain(1.0) {}


            Gain(Tgain c) : Gain(c, 1.0, -1.0) { // 1.0 and -1.0 are temp values only.
              resetMinMaxGain<Tgain>(); // set limits to smallest/largest value.
            }


            Gain(Tgain c, Tgain maxGain, Tgain minGain) {
                gain = c;
                this->maxGain = maxGain;
                this->minGain = minGain;
                targetGain = gain;
                gainDiff = 0;
            }


            virtual void run() {
                if(smoothChange) {
                    if(gain < targetGain) {
                    gain += gainDiff;
                    if(gain > targetGain) { // overshoot case.
                      gain = targetGain;
                    }
                    }

                    if(gain > targetGain) {
                    gain -= gainDiff;
                    if(gain < targetGain) {
                      gain = targetGain;
                    }
                    }
                }  

                if(gain > maxGain) { // if diff will cause gain to be too large.
                    gain = maxGain;
                }

                if(gain < minGain) {
                    gain = minGain;
                }

                if(enabled) {
                    this->out.getSignal().setValue(calculateResults<Tout>(this->in.getSignal().getValue()));
                } else {
                    this->out.getSignal().setValue(this->in.getSignal().getValue());
                }
                this->out.getSignal().setTimestamp(this->in.getSignal().getTimestamp());
            }


            virtual void enable() {
                enabled = true;
            }


            virtual void disable() {
                enabled = false;
            }


            virtual void enableSmoothChange(bool enable) {
                smoothChange = enable;
            }


            virtual void setGain(Tgain c) {
                if(c <= maxGain && c >= minGain) {
                    if(smoothChange) {
                    targetGain = c;
                    }else {
                    gain = c;
                    }
                }
            }


            virtual void setMaxGain(Tgain maxGain) {
                this->maxGain = maxGain;
            }


            virtual void setMinGain(Tgain minGain) {
                this->minGain = minGain;
            }


            virtual void setGainDiff(Tgain gainDiff) {
                    this->gainDiff = gainDiff;
            }


            /*
             * Friend operator overload to give the operator overload outside 
             * the class access to the private fields.
             */
            template <typename Xout, typename Xgain>
            friend std::ostream& operator<<(std::ostream& os, Gain<Xout,Xgain>& gain);


        protected:
            Tgain gain;
            Tgain maxGain;
            Tgain minGain;
            Tgain targetGain;
            Tgain gainDiff;
            bool enabled{true};
            bool smoothChange{false};


        private:
            template <typename S>
            typename std::enable_if<!elementWise,S>::type calculateResults(S value) {
                return gain * value;
            }


            template <typename S>
            typename std::enable_if<elementWise,S>::type calculateResults(S value) {
                return value.multiplyElementWise(gain);
            }


            template <typename S>
            typename std::enable_if<std::is_integral<S>::value>::type resetMinMaxGain() {
                minGain = std::numeric_limits<int32_t>::min();
                maxGain = std::numeric_limits<int32_t>::max();
            }


            template <typename S>
            typename std::enable_if<std::is_floating_point<S>::value>::type resetMinMaxGain() {
                minGain = std::numeric_limits<double>::lowest();
                maxGain = std::numeric_limits<double>::max();
            }


            template <typename S>
            typename std::enable_if<!std::is_arithmetic<S>::value && std::is_integral<typename S::value_type>::value>::type resetMinMaxGain() {
                minGain.fill(std::numeric_limits<int32_t>::min());
                maxGain.fill(std::numeric_limits<int32_t>::max());
            }


            template <typename S>
            typename std::enable_if<!std::is_arithmetic<S>::value && std::is_floating_point<typename S::value_type>::value>::type resetMinMaxGain() {
                minGain.fill(std::numeric_limits<double>::lowest());
                maxGain.fill(std::numeric_limits<double>::max());
            }
        };


        template <typename Tout, typename Tgain>
        std::ostream& operator<<(std::ostream& os, Gain<Tout,Tgain>& gain) {
            os << "Block Gain: '" << gain.getName() << "' is enabled=" << gain.enabled << ", gain=" << gain.gain << ", ";
            os << "smoothChange=" << gain.smoothChange << ", minGain=" << gain.minGain << ", maxGain=" << gain.maxGain << ", ";
            os << "targetGain=" << gain.targetGain << ", gainDiff=" << gain.gainDiff;
        }
    };
};

#endif /* ORG_EEROS_CONTROL_GAIN_HPP_ */