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 start;
 
    Real plateau;
 
    Real end;
};
 
struct TrapezoidalConstraints
{
    Real start_jerk;
 
    Real plateau_acceleration;
 
    Real end_jerk;
};
 
class TrapezoidalVelocity final : public Segment
{
public:
    TrapezoidalVelocity() = default;
 
    TrapezoidalStatus goto_velocity(
        Real start_time, const TrajState& initial_state, Real final_velocity,
        Real final_acceleration, Real max_acceleration, Real max_jerk);
 
    [[nodiscard]] TrajState evaluate(Real t_eval) const override;
 
    [[nodiscard]] TrajState get_final_state() const override
    {
        return evaluate(m_start_time + m_total_duration);
    }
 
    [[nodiscard]] Real get_start_time() const override
    {
        return m_start_time;
    }
 
    [[nodiscard]] Real get_duration() const override
    {
        return m_total_duration;
    }
 
    [[nodiscard]] Real get_final_time() const override
    {
        return m_start_time + m_total_duration;
    }
 
private:
    Real m_start_time = 0.0;
    Real 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