PathConstraints.h

#pragma once
 
#include <units/velocity.h>
#include <units/acceleration.h>
#include <units/angular_velocity.h>
#include <units/angular_acceleration.h>
#include <units/voltage.h>
#include <wpi/json.h>
#include <limits>
 
namespace pathplanner {
class PathConstraints {
public:
    constexpr PathConstraints(units::meters_per_second_t maxVel,
            units::meters_per_second_squared_t maxAccel,
            units::radians_per_second_t maxAngularVel,
            units::radians_per_second_squared_t maxAngularAccel,
            units::volt_t nominalVoltage = 12_V, bool unlimited = false) : m_maxVelocity(
            maxVel), m_maxAcceleration(maxAccel), m_maxAngularVelocity(
            maxAngularVel), m_maxAngularAcceleration(maxAngularAccel), m_nominalVoltage(
            nominalVoltage), m_unlimited(unlimited) {
    }
 
    static PathConstraints fromJson(const wpi::json &json);
 
    static constexpr PathConstraints unlimitedConstraints(
            units::volt_t nominalVoltage) {
        double inf = std::numeric_limits<double>::infinity();
        return PathConstraints(units::meters_per_second_t { inf },
                units::meters_per_second_squared_t { inf },
                units::radians_per_second_t { inf },
                units::radians_per_second_squared_t { inf }, nominalVoltage,
                true);
    }
 
    constexpr units::meters_per_second_t getMaxVelocity() const {
        return m_maxVelocity;
    }
 
    constexpr units::meters_per_second_squared_t getMaxAcceleration() const {
        return m_maxAcceleration;
    }
 
    constexpr units::radians_per_second_t getMaxAngularVelocity() const {
        return m_maxAngularVelocity;
    }
 
    constexpr units::radians_per_second_squared_t getMaxAngularAcceleration() const {
        return m_maxAngularAcceleration;
    }
 
    constexpr units::volt_t getNominalVoltage() const {
        return m_nominalVoltage;
    }
 
    constexpr bool isUnlimited() const {
        return m_unlimited;
    }
 
    bool operator==(const PathConstraints &other) const {
        return std::abs(m_maxVelocity() - other.m_maxVelocity()) < 1E-9
                && std::abs(m_maxAcceleration() - other.m_maxAcceleration())
                        < 1E-9
                && std::abs(
                        m_maxAngularVelocity() - other.m_maxAngularVelocity())
                        < 1E-9
                && std::abs(
                        m_maxAngularAcceleration()
                                - other.m_maxAngularAcceleration()) < 1E-9
                && std::abs(m_nominalVoltage() - other.m_nominalVoltage())
                        < 1E-9 && m_unlimited == other.m_unlimited;
    }
 
private:
    units::meters_per_second_t m_maxVelocity;
    units::meters_per_second_squared_t m_maxAcceleration;
    units::radians_per_second_t m_maxAngularVelocity;
    units::radians_per_second_squared_t m_maxAngularAcceleration;
    units::volt_t m_nominalVoltage;
    bool m_unlimited;
};
}