dual_modded_pid.hpp

Go to the documentation of this file.

#pragma once
 
#include "modded_pid.hpp"
#include "dual_cascade_pid.hpp"
 
namespace trap::algorithms
{
template
<
    typename MainType,
    typename IntermediateType,
    typename OutputType,
    typename TimeType,
    typename MainError = MainType,
    typename IntermediateError = IntermediateType
>
class DualModdedPid
{
public:
    using MainPid = ModdedPid<MainType, IntermediateType, TimeType>;
    using IntermediatePid = ModdedPid<IntermediateType, OutputType, TimeType>; 
    using MainModder = IPidModder<IntermediateType>;
    using IntermediateModder = IPidModder<OutputType>;
 
    struct Config
    {
        MainPid::Config mainPidConfig;
        IntermediatePid::Config intermediatePidConfig;
    };
 
    DualModdedPid(
        const Config& config,
        std::unique_ptr<MainModder> mainModder = std::make_unique<NullPidModder<IntermediateType>>(),
        std::unique_ptr<IntermediateModder> intermediateModder  = std::make_unique<NullPidModder<OutputType>>()
    )
        :
        mainPid_{config.mainPidConfig},
        intermediatePid_{config.intermediatePidConfig},
        mainModder_(std::move(mainModder)),
        intermediateModder_(std::move(intermediateModder))
    {
    }
 
    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}
        };
 
        const IntermediateType kIntermediateTarget{
            mainPid_.runController(
                kMainError,
                deltaTime
            )
        };
 
        const IntermediateType kModdedIntermediateTarget{mainModder_->getModdedOutput(kIntermediateTarget)};
 
        const IntermediateType kIntermediateError{
            IntermediateError{kModdedIntermediateTarget}
          - IntermediateError{intermediateCurrent}
        };
 
        const OutputType kOutput{
            intermediatePid_.runController(
                kIntermediateError,
                deltaTime
            )
        };
 
        const OutputType kModdedOutput{intermediateModder_->getModdedOutput(kOutput)};
 
        return kModdedOutput; 
    }
 
    void setTarget(const MainType& mainTarget)
    {
        mainTarget_ = mainTarget;
    }
private:
    MainPid mainPid_;
    IntermediatePid intermediatePid_;
 
    std::unique_ptr<MainModder> mainModder_;
    std::unique_ptr<IntermediateModder> intermediateModder_;
 
    MainType mainTarget_{}; 
    fang::chrono::SimpleTimer runControllerTimer_{};
};
}