wrapped_float.hpp

Go to the documentation of this file.

/*****************************************************************************/
/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/
/*****************************************************************************/
 
/*
 * 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_