fsb_trapezoidal_velocity.h

#ifndef FSB_TRAPEZOIDAL_VELOCITY_H
#define FSB_TRAPEZOIDAL_VELOCITY_H
 
#include <cstdint>
#include "fsb_types.h"
#include "fsb_trajectory_types.h"
#include "fsb_trajectory_segment.h"
 
namespace fsb
{
 
enum class TrapezoidalStatus: uint8_t
{
    SUCCESS = 0,
    MAX_VALUE_BELOW_TOLERANCE = 1,
    FAILED_TRAJECTORY_GENERATION = 2
};
 
struct TrapezoidalDuration
{
    real_t start;
 
    real_t plateau;
 
    real_t end;
};
 
struct TrapezoidalConstraints
{
    real_t start_jerk;
 
    real_t plateau_acceleration;
 
    real_t end_jerk;
};
 
class TrapezoidalVelocity final : public Segment
{
public:
    TrapezoidalVelocity() = default;
 
    TrapezoidalStatus goto_velocity(
        real_t start_time, const TrajState& initial_state, real_t final_velocity,
        real_t final_acceleration, real_t max_acceleration, real_t max_jerk);
 
    [[nodiscard]] TrajState evaluate(real_t t_eval) const override;
 
    [[nodiscard]] TrajState get_final_state() const override
    {
        return evaluate(m_start_time + m_total_duration);
    }
 
    [[nodiscard]] real_t get_start_time() const override
    {
        return m_start_time;
    }
 
    [[nodiscard]] real_t get_duration() const override
    {
        return m_total_duration;
    }
 
    [[nodiscard]] real_t get_final_time() const override
    {
        return m_start_time + m_total_duration;
    }
 
private:
    real_t m_start_time = 0.0;
    real_t m_total_duration = 0.0;
 
    SegmentConstJerk m_seg1;
    SegmentConstAcc  m_seg2;
    SegmentConstJerk m_seg3;
    SegmentConstAcc  m_seg_extrapolate;
 
    TrajState m_initial_state = {};
    TrajState m_final_state = {};
};
 
} // namespace fsb
 
#endif