Program Listing for File PID.cc

↰ Return to documentation for file (doxygen_docs/GeneralKModels/PID.cc)

#include "Karana/GeneralKModels/PID.h"
#include "Karana/KCore/Allocator.h"

/**
 * @file
 * @brief PID implementation.
 */

namespace Karana::Models {

    namespace kd = Karana::Dynamics;

    PID::PID(std::string_view name,
             const kc::ks_ptr<kd::ModelManager> &mm,
             const kc::ks_ptr<kd::PhysicalSubhinge> &sh,
             const kc::ks_ptr<ProfileGenerator<km::Vec>> &q_traj,
             const kc::ks_ptr<ProfileGenerator<km::Vec>> &u_traj)
        : KModel<PID, PIDParams>(name, mm)
        , q_traj(q_traj)
        , u_traj(u_traj)
        , _sh(sh) {
        params = std::allocate_shared<PIDParams>(kc::Allocator<PIDParams>{},
                                                 std::format("{}_params", name));

        // Start the internal state at zero so the integral gain can be calculated appropriately
        _e_prev.resize(sh->nQ());
        _e_prev.setZero();

        _integral_contrib.resize(sh->nQ());
        _integral_contrib.setZero();

        _prev_t = km::Ktime{0};
    };

    kc::ks_ptr<PID> PID::create(std::string_view name,
                                const kc::ks_ptr<kd::ModelManager> &mm,
                                const kc::ks_ptr<kd::PhysicalSubhinge> &sh,
                                const kc::ks_ptr<ProfileGenerator<km::Vec>> &q_traj,
                                const kc::ks_ptr<ProfileGenerator<km::Vec>> &u_traj) {
        kc::ks_ptr<PID> pid =
            std::allocate_shared<PID>(kc::Allocator<PID>{}, name, mm, sh, q_traj, u_traj);
        mm->registerModel(pid);
        return pid;
    }

    void PID::preDeriv(const km::Ktime &t, const km::Vec &) {
        km::Vec T = params->kp * (q_traj->getValue(t) - _sh->getQ()) +
                    params->kd * (u_traj->getValue(t) - _sh->getU()) + _integral_contrib;
        _sh->setT(T);

        if (debug_model) [[unlikely]] {
            stdDebugMsg(std::format("output torque {}", km::dumpString(T)));
        }
    }

    void PID::preModelStep(const km::Ktime &t, const km::Vec &) {
        // Calculate the integral contribution for this step
        _integral_contrib += params->ki * (_e_prev * km::ktimeToSeconds(t - _prev_t));

        // Calculate new error for next time
        _e_prev = q_traj->getValue(t) - _sh->getQ();

        // Set _prev_t to t for next time
        _prev_t = t;

        if (debug_model) [[unlikely]] {
            stdDebugMsg(std::format("preModelStep integral error {}", km::dumpString(_e_prev)));
        }
    };

    PIDParams::PIDParams(std::string_view name)
        : KModelParams(name) {
        kp = km::notReadyNaN;
        kd = km::notReadyNaN;
        ki = km::notReadyNaN;
    }

    bool PIDParams::isReady() const {
        bool flag = true;
        if (km::isNotReadyNaN(kp)) {
            kc::warn("Parameter kp is not ready.");
            flag = false;
        }
        if (km::isNotReadyNaN(kd)) {
            kc::warn("Parameter kd is not ready.");
            flag = false;
        }
        if (km::isNotReadyNaN(ki)) {
            kc::warn("Parameter ki is not ready.");
            flag = false;
        }
        return flag;
    }

    // Destructor included for MacOS builds. Must have a key-function out-of-line to avoid dulpicate
    // symbols.
    PID::~PID(){};
} // namespace Karana::Models