dual_cascade_pid.hpp

Go to the documentation of this file.

#pragma once
 
#include "smooth_pid.hpp"
#include "util/chrono/simple_timer.hpp"
 
namespace trap::algorithms
{
    template
    <
        typename MainType,
        typename IntermediateType,
        typename OutputType,
        typename TimeType,
        typename MainError = MainType,
        typename IntermediateError = IntermediateType
    >
    class DualCascadePid
    {
    public:
        using MainPid = SmoothPid<MainType, IntermediateType, TimeType>;
        using IntermediatePid = SmoothPid<IntermediateType, OutputType, TimeType>; 
        struct Config
        {
            MainPid::Config mainPidConfig;
            IntermediatePid::Config intermediatePidConfig;
            MainType mainPidInitialValue;
            IntermediateType intermediatePidInitialValue;
        };
 
        DualCascadePid(const Config& config)
            :
            mainPid_{config.mainPidConfig, config.mainPidInitialValue},
            intermediatePid_{config.intermediatePidConfig, config.intermediatePidInitialValue}
        {
        }
 
        OutputType runController(const MainType& mainCurrent, const IntermediateType& intermediateCurrent)
        {
            const TimeType kDeltaTime{static_cast<TimeType>(runControllerTimer_.getDurationAndReset())};
            return runController(mainCurrent, intermediateCurrent, kDeltaTime);
        }
 
        OutputType  runController(
            const MainType& mainCurrent,
            const IntermediateType& intermediateCurrent,
            const TimeType& deltaTime
        )
        {
            const MainType kMainError{
                MainError{mainTarget_}
              - MainError{mainCurrent}
            };
 
            static MainType mainError{};
            mainError = kMainError;
 
            const IntermediateType kIntermediateTarget{
                mainPid_.runController(
                    kMainError,
                    deltaTime
                )
            };
 
            static IntermediateType intermediateTarget{};
            intermediateTarget = kIntermediateTarget;
 
            const IntermediateType kIntermediateError{
                IntermediateError{kIntermediateTarget}
              - IntermediateError{intermediateCurrent}
            };
 
            static IntermediateType intermediateError{};
            intermediateError = kIntermediateError;
 
            const OutputType kOutput{
                intermediatePid_.runController(
                    kIntermediateError,
                    deltaTime
                )
            };
 
            static OutputType output{};
            output = kOutput;
 
            return kOutput; 
        }
 
        void setTarget(const MainType& mainTarget)
        {
            mainTarget_ = mainTarget;
        }
    private:
        MainPid mainPid_;
        IntermediatePid intermediatePid_;
 
        MainType mainTarget_{}; 
        fang::chrono::SimpleTimer runControllerTimer_{};
    };
}