Skip to content

# File evamInertiaDecor.h

File List > src > evamInertiaDecor.h

Go to the documentation of this file

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

    using namespace eva;

    namespace evam
    {
        constexpr unsigned short kInertiaMass = 5;
        struct InertiaConfig
        {
            signed short inertiaMass;

            InertiaConfig(signed short inertiaMass) : inertiaMass(inertiaMass) {}
        };

        template <class TMotor, unsigned short tInertiaMass = kInertiaMass>
        class InertiaDecor : public Heartbeat, public TMotor
        {
            static_assert(0 < tInertiaMass, "tInertiaMass must be > 0");

        private:
            static constexpr unsigned long kHeartbeatPeriodMs = 100;

            InertiaConfig mConfig;
            signed short mDesiredSpeed = 0;
            signed short mSpeed = 0;

            signed short calcSpeed() const
            {
                if ((mDesiredSpeed != 0) && ((mDesiredSpeed > 0) ^ (mSpeed > 0)))
                    return mDesiredSpeed;

                if (abs(mDesiredSpeed) > abs(mSpeed))
                    return mDesiredSpeed;

                signed short delta = mDesiredSpeed - mSpeed;
                signed short step = delta / mConfig.inertiaMass;
                if (abs(step) < 2)
                    return mDesiredSpeed;
                return mSpeed + step;
            }

        protected:
            void onHeartbeat() override
            {
                mSpeed = calcSpeed();
                TMotor::Go(mSpeed);
            }

        public:
            InertiaDecor() : Heartbeat(kHeartbeatPeriodMs), mConfig(tInertiaMass), mDesiredSpeed(0), mSpeed(0) {}

            template <typename... Args>
            InertiaDecor(InertiaConfig config, Args... args)
                : Heartbeat(kHeartbeatPeriodMs), TMotor(args...), mConfig(config), mDesiredSpeed(0), mSpeed(0) {}

            void SetInertiaMass(unsigned short aValue)
            {
                mConfig.inertiaMass = constrain(aValue, 1, 200);
            }

            unsigned short GetInertiaMass() const
            {
                return mConfig.inertiaMass;
            }

            void Go(int aSpeed)
            {
                mDesiredSpeed = aSpeed;
                if (aSpeed == calcSpeed())
                {
                    mSpeed = aSpeed;
                    TMotor::Go(mSpeed);
                }
            }
        };
    }