File evamAdaptiveSmoothDecor.h

File List > src > evamAdaptiveSmoothDecor.h

Go to the documentation of this file

#pragma once

#include <Arduino.h>
#include <evaHeartbeat.h>

using namespace eva;

namespace evam
{
    constexpr unsigned short kDefaultMinTimeConstantMs = 10;
    constexpr unsigned short kDefaultMaxTimeConstantMs = 150;
    constexpr unsigned short kMinTimeConstantLimit = 5;
    constexpr unsigned short kMaxTimeConstantLimit = 500;

    struct AdaptiveSmoothConfig {
        unsigned short minTimeConstantMs;
        unsigned short maxTimeConstantMs;

        AdaptiveSmoothConfig(unsigned short minTimeConstantMs, unsigned short maxTimeConstantMs)
            : minTimeConstantMs(constrain(minTimeConstantMs, kMinTimeConstantLimit, kMaxTimeConstantLimit)),
              maxTimeConstantMs(constrain(maxTimeConstantMs, minTimeConstantMs, kMaxTimeConstantLimit)) {}
    };

    template <class TMotor,
              unsigned short tMinTimeConstantMs = kDefaultMinTimeConstantMs,
              unsigned short tMaxTimeConstantMs = kDefaultMaxTimeConstantMs>
    class AdaptiveSmoothDecor : public Heartbeat, public TMotor
    {
        static_assert(tMinTimeConstantMs >= kMinTimeConstantLimit && tMinTimeConstantMs <= kMaxTimeConstantLimit,
                      "tMinTimeConstantMs out of range");
        static_assert(tMaxTimeConstantMs >= tMinTimeConstantMs && tMaxTimeConstantMs <= kMaxTimeConstantLimit,
                      "tMaxTimeConstantMs must be >= tMinTimeConstantMs");

    private:
        static constexpr unsigned long kHeartbeatPeriodMs = 10;
        static constexpr signed short kDeadzone = 3;

        AdaptiveSmoothConfig mConfig;

        signed short mTargetValue = 0;
        signed short mCurrentValue = 0;
        signed short mLastTargetValue = 0;
        unsigned short mCurrentTimeConstantMs = tMaxTimeConstantMs;

        unsigned short calculateTimeConstant()
        {
            signed short change = abs(mTargetValue - mLastTargetValue);

            if (change >= 200)
                return mConfig.minTimeConstantMs;
            else if (change <= 5)
                return mConfig.maxTimeConstantMs;
            else
            {
                return mConfig.maxTimeConstantMs - ((change - 5) * (mConfig.maxTimeConstantMs - mConfig.minTimeConstantMs) / 195);
            }
        }

    protected:
        void onHeartbeat() override
        {
            mCurrentTimeConstantMs = calculateTimeConstant();
            mLastTargetValue = mTargetValue;

            if (abs(mTargetValue) <= kDeadzone && abs(mCurrentValue) <= kDeadzone)
            {
                if (mCurrentValue != 0)
                    mCurrentValue = 0;
            }
            else
            {
                signed long step = (signed long)(mTargetValue - mCurrentValue) * kHeartbeatPeriodMs * 1000 / mCurrentTimeConstantMs;
                mCurrentValue += step / 1000;
                mCurrentValue = constrain(mCurrentValue, -1000, 1000);
            }

            TMotor::Go(mCurrentValue);
        }

    public:
        AdaptiveSmoothDecor() : mConfig(tMinTimeConstantMs, tMaxTimeConstantMs), Heartbeat(kHeartbeatPeriodMs) {}

        template<typename... Args>
        AdaptiveSmoothDecor(AdaptiveSmoothConfig config, Args... args) 
            : mConfig(config), Heartbeat(kHeartbeatPeriodMs), TMotor(args...) {}

        void Go(signed short aValue)
        {
            mTargetValue = constrain(aValue, -1000, 1000);
        }

        void SetMinTimeConstantMs(unsigned short value)
        {
            mConfig.minTimeConstantMs = constrain(value, kMinTimeConstantLimit, kMaxTimeConstantLimit);
        }

        unsigned short GetMinTimeConstantMs() const
        {
            return mConfig.minTimeConstantMs;
        }

        void SetMaxTimeConstantMs(unsigned short value)
        {
            mConfig.maxTimeConstantMs = constrain(value, mConfig.minTimeConstantMs, kMaxTimeConstantLimit);
        }

        unsigned short GetMaxTimeConstantMs() const
        {
            return mConfig.maxTimeConstantMs;
        }

        void SetupRange(unsigned short minTimeConstantMs, unsigned short maxTimeConstantMs)
        {
            SetMinTimeConstantMs(minTimeConstantMs);
            SetMaxTimeConstantMs(maxTimeConstantMs);
        }
    };
}