core/utils/controls/trapezoid_profile.h

#pragma once
 
#include <math.h>
 
// Struct representing a state along the motion profile containing position, velocity and acceleration.
typedef struct {
  double pos; // 1D position
  double vel; // 1D velocity
  double acc; // 1D acceleration
} motion_t;

class TrapezoidProfile {
public:
  TrapezoidProfile(const double &x_initial, const double &x_target, const double &v_max, const double &accel, const double &decel);

  motion_t calculate(double t);

  double total_time();
 
private:
  double x_initial_;
  double x_target_;
  double v_max_;
  double v_peak_;
  double accel_;
  double decel_;
  double distance_;
  double dist_accel_;
  double dist_decel_;
  double dist_cruise_;
  double dist_full_;
 
  double time_accel_;
  double time_decel_;
  double time_cruise_;
  double time_total_;
 
  bool triangular_;
 
  // 1 if positive, -1 if negative
  int direction_;
};