feedforward.h

#pragma once
 
#include "core/utils/math_util.h"
#include "core/utils/moving_average.h"
#include "vex.h"
#include <math.h>
#include <vector>

class FeedForward {
  public:
    typedef struct {
        double kS; 
        double kV; 
        double kA; 
        double kG; 
    } ff_config_t;

    FeedForward(ff_config_t &cfg) : cfg(cfg) {}

    double calculate(double v, double a, double pid_ref = 0.0) {
        double ks_sign = 0;
        if (v != 0)
            ks_sign = sign(v);
        else if (pid_ref != 0)
            ks_sign = sign(pid_ref);
 
        return (cfg.kS * ks_sign) + (cfg.kV * v) + (cfg.kA * a) + cfg.kG;
    }
 
  private:
    ff_config_t &cfg;
};

FeedForward::ff_config_t tune_feedforward(vex::motor_group &motor, double pct, double duration);