PIDF.h

#pragma once
 
#include <optional>
#include <ratio>
#include <type_traits>
 
#include <units/acceleration.h>
#include <units/angle.h>
#include <units/angular_acceleration.h>
#include <units/angular_jerk.h>
#include <units/angular_velocity.h>
#include <units/base.h>
#include <units/time.h>
#include <units/velocity.h>
 
namespace valor {
 
enum class GravityFFType { LINEAR, CIRCULAR };

 
template <typename INPUT = units::dimensionless::dimensionless, typename OUTPUT = units::dimensionless::dimensionless>
    requires units::traits::is_unit_v<INPUT> && units::traits::is_unit_v<OUTPUT>
struct PIDF {
    using TagTraits = units::traits::unit_traits<INPUT>;
    using BaseUnits = typename TagTraits::base_unit_type;
    using TimeRatio = typename BaseUnits::second_ratio;
    using TimeCorrectionUnit =
        units::unit<std::ratio<1>,
                    units::base_unit<std::ratio<0>, std::ratio<0>, std::ratio<-TimeRatio::num, TimeRatio::den>, std::ratio<0>>>;
 
    using Position = units::compound_unit<INPUT, TimeCorrectionUnit>;
    using Velocity = units::compound_unit<units::inverse<units::second>, Position>;
    using Acceleration = units::compound_unit<units::inverse<units::second>, Velocity>;
    using Jerk = units::compound_unit<units::inverse<units::second>, Acceleration>;
 
    using Velocity_t = units::unit_t<Velocity>;
    using Acceleration_t = units::unit_t<Acceleration>;
    using Jerk_t = units::unit_t<Jerk>;
 
    using ProportionalGain = units::compound_unit<OUTPUT, units::inverse<INPUT>>;
    using IntegralGain = units::compound_unit<ProportionalGain, units::inverse<units::second>>;
    using DerivativeGain = units::compound_unit<ProportionalGain, units::second>;
 
    using ProportionalGain_t = units::unit_t<ProportionalGain>;
    using IntegralGain_t = units::unit_t<IntegralGain>;
    using DerivativeGain_t = units::unit_t<DerivativeGain>;
 
    using Input_t = units::unit_t<INPUT>;
    using Output_t = units::unit_t<OUTPUT>;
 
    using kV_t = units::unit_t<units::compound_unit<OUTPUT, units::inverse<Velocity>>>;
    using kA_t = units::unit_t<units::compound_unit<OUTPUT, units::inverse<Acceleration>>>;
 
    ProportionalGain_t P{0};
    IntegralGain_t I{0};
    DerivativeGain_t D{0};
 
    Velocity_t maxVelocity{0};
    Acceleration_t maxAcceleration{0};
    Jerk_t maxJerk{0};
 
    Input_t error{0};
    Input_t errorThreshold{0};
 
    Output_t kS{0};
    Output_t kG{0};
 
    std::optional<kV_t> kV = std::nullopt;
    std::optional<kA_t> kA = std::nullopt;
 
    units::turn_t GTarget = 0_deg;
    GravityFFType GType = GravityFFType::LINEAR;
 
    using DimensionlessPIDF_t = PIDF<units::dimensionless::dimensionless, units::dimensionless::dimensionless>;
 
    operator DimensionlessPIDF_t() const
        requires(!std::is_same_v<PIDF, DimensionlessPIDF_t>)
    {
        return DimensionlessPIDF_t{
            .P = typename DimensionlessPIDF_t::ProportionalGain_t{P.value()},
            .I = typename DimensionlessPIDF_t::IntegralGain_t{I.value()},
            .D = typename DimensionlessPIDF_t::DerivativeGain_t{D.value()},
            .maxVelocity = typename DimensionlessPIDF_t::Velocity_t{maxVelocity.value()},
            .maxAcceleration = typename DimensionlessPIDF_t::Acceleration_t{maxAcceleration.value()},
            .maxJerk = typename DimensionlessPIDF_t::Jerk_t{maxJerk.value()},
            .error = typename DimensionlessPIDF_t::Input_t{error.value()},
            .errorThreshold = typename DimensionlessPIDF_t::Input_t{errorThreshold.value()},
            .kS = typename DimensionlessPIDF_t::Output_t{kS.value()},
            .kG = typename DimensionlessPIDF_t::Output_t{kG.value()},
            .kV = kV.has_value() ? std::make_optional(typename DimensionlessPIDF_t::kV_t{kV->value()}) : std::nullopt,
            .kA = kA.has_value() ? std::make_optional(typename DimensionlessPIDF_t::kA_t{kA->value()}) : std::nullopt,
            .GTarget = GTarget,
            .GType = GType,
        };
    };
};
}  // namespace valor