math/controls/feedforward.h

#pragma once
 
#include <cmath>
#include "math/math_util.h"
 
namespace core {

class Feedforward {
   public:
   typedef struct {
        double kS;  
        double kV;  
        double kA;  
        double kG;  
    } feedforward_config_t;
    Feedforward(double kS, double kV, double kA) : kS_(kS), kV_(kV), kA_(kA) {}
    Feedforward(Feedforward::feedforward_config_t config)
        : kS_(config.kS), kV_(config.kV), kA_(config.kA) {}
 
    Feedforward(const Feedforward&) = default;
    Feedforward(Feedforward&&) = default;
    Feedforward& operator=(const Feedforward&) = default;
    Feedforward& operator=(Feedforward&&) = default;

    double calculate(double v, double a) const {
        double ks_sign = sign(v);
        return kS_ * ks_sign + kV_ * v + kA_ * a;
    }

    double max_vel(double max_voltage) const {
        if (kV_ == 0) {
            return 0;  // don't divide by zero
        }
        return max_voltage / kV_;
    }

    double max_acc(double max_voltage) const {
        if (kA_ == 0) {
            return 0;  // don't divide by zero
        }
        return max_voltage / kA_;
    }
 
    double kS() const { return kS_; }
    double kV() const { return kV_; }
    double kA() const { return kA_; }
 
    void set_kS(double kS) { kS_ = kS; }
    void set_kV(double kV) { kV_ = kV; }
    void set_kA(double kA) { kA_ = kA; }
 
   private:
    double kS_;
    double kV_;
    double kA_;
};
 
}  // namespace core