core/utils/controls/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);