multi_encoder.hpp

Go to the documentation of this file.

/*****************************************************************************/
/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/
/*****************************************************************************/
 
/*
 * Copyright (c) 2024 Advanced Robotics at the University of Washington <robomstr@uw.edu>
 *
 * This file is part of Taproot.
 *
 * Taproot is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Taproot is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
 */
 
#ifndef TAPROOT_MULTI_ENCODER_HPP_
#define TAPROOT_MULTI_ENCODER_HPP_
 
#include <array>
 
#include "tap/util_macros.hpp"
 
#include "modm/architecture/interface/assert.hpp"
 
#include "encoder_interface.hpp"
 
namespace tap::encoder
{
template <uint32_t COUNT>
class MultiEncoder : public EncoderInterface
{
public:
    MultiEncoder(std::array<EncoderInterface*, COUNT> encoders)
        : encoders(encoders),
          seenEncoders(0)
    {
        modm_assert(this->encoders[0] != nullptr, "MultiEncoder", "nullptr first encoder");
    }
 
    void initialize() override
    {
        for (EncoderInterface*& encoder : this->encoders)
        {
            if (encoder != nullptr)
            {
                encoder->initialize();
            }
        }
    }
 
    bool isOnline() const override
    {
        const_cast<MultiEncoder<COUNT>*>(this)->syncEncoders();
 
        for (uint32_t i = 0; i < COUNT; i++)
        {
            if (this->validEncoder(i))
            {
                return true;
            }
        }
 
        return false;
    }
 
    tap::algorithms::WrappedFloat getPosition() const override
    {
        const_cast<MultiEncoder<COUNT>*>(this)->syncEncoders();
        int onlineEncoders = 0;
        float position = 0;
 
        for (uint32_t i = 0; i < COUNT; i++)
        {
            if (this->validEncoder(i))
            {
                position += this->encoders[i]->getPosition().getUnwrappedValue();
                onlineEncoders += 1;
            }
        }
 
        return tap::algorithms::WrappedFloat(
            onlineEncoders == 0 ? 0 : position / onlineEncoders,
            0,
            static_cast<float>(M_TWOPI));
    }
 
    float getVelocity() const override
    {
        const_cast<MultiEncoder<COUNT>*>(this)->syncEncoders();
        int onlineEncoders = 0;
        float velocity = 0;
 
        for (uint32_t i = 0; i < COUNT; i++)
        {
            if (this->validEncoder(i))
            {
                velocity += this->encoders[i]->getVelocity();
                onlineEncoders += 1;
            }
        }
 
        return onlineEncoders == 0 ? 0 : velocity / onlineEncoders;
    };
 
    void resetEncoderValue() override
    {
        this->syncEncoders();
 
        for (EncoderInterface*& encoder : this->encoders)
        {
            if (encoder != nullptr)
            {
                encoder->resetEncoderValue();
            }
        }
    }
 
    void alignWith(EncoderInterface* other) override
    {
        this->syncEncoders();
 
        for (EncoderInterface*& encoder : this->encoders)
        {
            if (encoder != nullptr)
            {
                encoder->alignWith(other);
            }
        }
    }
 
    DISALLOW_COPY_AND_ASSIGN(MultiEncoder)
 
private:
    std::array<EncoderInterface*, COUNT> encoders;
    uint32_t seenEncoders;
 
    void syncEncoders()
    {
        // The primary encoder *must* be online before syncing anything to it
        if (this->encoders[0]->isOnline())
        {
            for (uint32_t i = 1; i < COUNT; i++)
            {
                bool online = this->encoders[i] != nullptr && this->encoders[i]->isOnline();
                if (online && !this->seenEncoder(i))
                {
                    // Sync the newly discovered encoder to the primary encoder
                    this->seenEncoders |= 1 << i;
                    encoders[i]->alignWith(encoders[0]);
                }
                else if (validEncoder(i) && !seenEncoder(0))
                {
                    // Primary encoder disappeared but came back, resync it.
                    this->seenEncoders |= 1;
                    encoders[0]->alignWith(encoders[i]);
                }
                else if (!online)
                {
                    // This encoder disappeared, remove it
                    this->seenEncoders &= ~(1 << i);
                }
            }
            this->seenEncoders |= 1;
        }
        else
        {
            this->seenEncoders &= ~(1);
        }
    }
 
    inline bool validEncoder(uint32_t index) const
    {
        return this->seenEncoder(index) && this->encoders[index] != nullptr &&
               this->encoders[index]->isOnline();
    }
 
    inline bool seenEncoder(uint32_t index) const
    {
        return ((this->seenEncoders) & (1 << index)) != 0;
    }
};
 
}  // namespace tap::encoder
 
#endif  // TAPROOT_MULTI_ENCODER_HPP_