fsb_trapezoidal_position.h

#ifndef FSB_TRAPEZOIDAL_POSITION_H
#define FSB_TRAPEZOIDAL_POSITION_H
 
#include "fsb_types.h"
#include "fsb_trajectory_types.h"
#include "fsb_trajectory_segment.h"
#include "fsb_trapezoidal_velocity.h"
 
namespace fsb
{
 
class TrapezoidalPosition final : public SegmentScalar
{
public:
    TrapezoidalPosition() = default;
 
    TrapezoidalStatus generate(
        Real start_time, Real initial_position, Real final_position,
        Real max_velocity, Real max_acceleration, Real max_jerk);
 
    TrapezoidalStatus generate(
        Real start_time, Real initial_position, Real final_position,
        Real initial_velocity, Real final_velocity,
        Real max_velocity, Real max_acceleration, Real max_jerk);
 
    [[nodiscard]] TrajState evaluate(Real t_eval) const override final;
 
    [[nodiscard]] TrajState get_final_state() const override final
    {
        return evaluate(m_start_time + m_total_duration);
    }
 
    [[nodiscard]] TrajState get_initial_state() const override final
    {
        return evaluate(m_start_time);
    }
 
    [[nodiscard]] Real get_start_time() const override final
    {
        return m_start_time;
    }
 
    [[nodiscard]] Real get_duration() const override final
    {
        return m_total_duration;
    }
 
    [[nodiscard]] Real get_final_time() const override final
    {
        return m_start_time + m_total_duration;
    }
 
private:
    Real m_start_time = 0.0;
    Real m_total_duration = 0.0;
 
    TrapezoidalVelocity m_accel_ramp;
    SegmentConstVel     m_cruise;
    TrapezoidalVelocity m_decel_ramp;
    SegmentConstVel     m_seg_extrapolate;
 
    TrajState m_initial_state = {};
    TrajState m_final_state = {};
};
 
} // namespace fsb
 
#endif