Program Listing for File wrapped_float.hpp

↰ Return to documentation for file (src/tap/algorithms/wrapped_float.hpp)

/*
 * Copyright (c) 2020-2023 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_WRAPPED_FLOAT_HPP_
#define TAPROOT_WRAPPED_FLOAT_HPP_

#include <assert.h>

#include <cmath>

#include <modm/architecture/interface/assert.hpp>
#include <modm/math/utils.hpp>

#include "math_user_utils.hpp"

namespace tap
{
namespace algorithms
{
class WrappedFloat
{
public:
    WrappedFloat(float value, float lowerBound, float upperBound);

    inline WrappedFloat withSameBounds(const float value) const
    {
        return WrappedFloat(value, this->lowerBound, this->upperBound);
    }

    // Overloaded Operators ----------------

    bool operator==(const WrappedFloat& other) const;

    void operator+=(const WrappedFloat& other);

    void operator-=(const WrappedFloat& other);

    WrappedFloat operator+(const WrappedFloat& other) const;

    WrappedFloat operator-(const WrappedFloat& other) const;

    void operator+=(float other);

    void operator-=(float other);

    WrappedFloat operator+(float other) const;

    WrappedFloat operator-(float other) const;

    float minDifference(const WrappedFloat& other) const;

    float minDifference(const float& unwrappedValue) const;

    WrappedFloat minInterpolate(const WrappedFloat& other, float alpha) const;

    void shiftBounds(float shiftMagnitude);

    static float limitValue(
        const WrappedFloat& valueToLimit,
        const WrappedFloat& min,
        const WrappedFloat& max,
        int* status);

    static float limitValue(
        const WrappedFloat& valueToLimit,
        const float min,
        const float max,
        int* status);

    bool withinRange(const WrappedFloat& lowerBound, const WrappedFloat& upperBound) const;

    static float rangeOverlap(
        const WrappedFloat& lowerA,
        const WrappedFloat& upperA,
        const WrappedFloat& lowerB,
        const WrappedFloat& upperB);

    // Getters/Setters ----------------

    inline float getUnwrappedValue() const
    {
        return wrapped + (upperBound - lowerBound) * revolutions;
    };

    inline float getWrappedValue() const { return wrapped; };

    inline void setWrappedValue(float newWrappedValue)
    {
        this->wrapped = newWrappedValue;
        wrapValue();
    };

    inline void setUnwrappedValue(float newUnwrappedValue)
    {
        this->wrapped = newUnwrappedValue;
        this->revolutions = 0;
        wrapValue();
    };

    inline WrappedFloat getNormalized() const
    {
        WrappedFloat out(*this);
        out.revolutions = 0;
        return out;
    }

    inline int getRevolutions() const { return revolutions; };

    inline float getUpperBound() const { return upperBound; };

    inline float getLowerBound() const { return lowerBound; };

    static constexpr float EPSILON = 1E-8;

private:
    float wrapped;

    int revolutions{0};

    float lowerBound;

    float upperBound;

    void wrapValue();

    inline static void assertBoundsEqual(const WrappedFloat& a, const WrappedFloat& b)
    {
        modm_assert(
            compareFloatClose(a.getLowerBound(), b.getLowerBound(), EPSILON),
            "WrappedFloat::assertBoundsEqual",
            "Lower bounds do not match");
        modm_assert(
            compareFloatClose(a.getUpperBound(), b.getUpperBound(), EPSILON),
            "WrappedFloat::assertBoundsEqual",
            "Upper bounds do not match");
    }

    inline void assertBoundsEqual(const WrappedFloat& other) const
    {
        assertBoundsEqual(*this, other);
    }

};  // class WrappedFloat

class Angle : public WrappedFloat
{
public:
    inline Angle(const float value) : WrappedFloat(value, 0, M_TWOPI){};

    static inline WrappedFloat fromDegrees(const float degrees)
    {
        return Angle(modm::toRadian(degrees));
    }
};

}  // namespace algorithms

}  // namespace tap

#endif  // TAPROOT_WRAPPED_FLOAT_HPP_