PIDF.h

 
#pragma once
 
#include <optional>
#include <numbers>
 
#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/velocity.h>
 
namespace valor {
 
enum FeedForwardType { LINEAR, CIRCULAR };
 
struct PIDF {
    void setMaxVelocity(units::turns_per_second_t _maxVelocity) { maxVelocity = _maxVelocity; }
 
    void setMaxVelocity(units::meters_per_second_t _maxVelocityMPS, units::meter_t wheelDiameter) {
        units::turns_per_second_t _maxVelocity = _maxVelocityMPS * 1_tr / (std::numbers::pi * wheelDiameter);
        maxVelocity = _maxVelocity;
    }
 
    void setMaxAcceleration(units::turns_per_second_squared_t _maxAcceleration) { maxAcceleration = _maxAcceleration; }
 
    void setMaxAcceleration(units::meters_per_second_squared_t _maxAccelerationMPS, units::meter_t wheelDiameter) {
        units::turns_per_second_squared_t _maxAcceleration = _maxAccelerationMPS * 1_tr / (std::numbers::pi * wheelDiameter);
        maxAcceleration = _maxAcceleration;
    }
 
    void setMaxJerk(units::turns_per_second_cubed_t _maxJerk) { maxJerk = _maxJerk; }
 
    double P = 0.0;
    double I = 0.0;
    double D = 0.0;
 
    std::optional<double> kV = std::nullopt;
    units::turns_per_second_t maxVelocity = 0_tps;
    units::turns_per_second_squared_t maxAcceleration = 0_tr_per_s_sq;
    units::turns_per_second_cubed_t maxJerk = 0_tr_per_s_cu;
    units::turn_t error = 0_tr;
 
    double S = 0.19;
 
    double aFF = 0;
    units::turn_t aFFTarget = 0_deg;
    FeedForwardType aFFType = FeedForwardType::LINEAR;
};
}  // namespace valor