arm_feedforward.h

#pragma once
 
#include "math/controls/feedforward.h"
#include "math/geometry/rotation2d.h"
 
namespace core {

class ArmFeedforward : public Feedforward {
   public:
    ArmFeedforward(double kS, double kV, double kA, double kG)
        : kG_(kG), Feedforward(kS, kV, kA) {}
    ArmFeedforward(Feedforward::feedforward_config_t config)
        : kG_(config.kG), Feedforward(config.kS, config.kV, config.kA) {}
 
    ArmFeedforward(const ArmFeedforward&) = default;
    ArmFeedforward(ArmFeedforward&&) = default;
    ArmFeedforward& operator=(const ArmFeedforward&) = default;
    ArmFeedforward& operator=(ArmFeedforward&&) = default;

    double calculate(double v, double a, Rotation2d angle) const {
        double out = Feedforward::calculate(v, a);
        return out + kG_ * angle.f_cos();
    }
 
    double kG() const { return kG_; }
    void set_kG(double kG) { kG_ = kG; }
 
   private:
    double kG_;
};
 
}  // namespace core