ChoreoTrajectoryState.h

// Copyright (c) Choreo contributors
 
#pragma once
 
#include <frc/geometry/Pose2d.h>
#include <frc/kinematics/ChassisSpeeds.h>
#include <wpi/json_fwd.h>
 
namespace choreolib {
class ChoreoTrajectoryState {
 public:
  ChoreoTrajectoryState() = default;
 
  ChoreoTrajectoryState(units::second_t t, units::meter_t x, units::meter_t y,
                        units::radian_t heading,
                        units::meters_per_second_t xVel,
                        units::meters_per_second_t yVel,
                        units::radians_per_second_t angularVel);
 
  frc::Pose2d GetPose() const;
 
  frc::ChassisSpeeds GetChassisSpeeds() const;
 
  ChoreoTrajectoryState Interpolate(const ChoreoTrajectoryState& endValue,
                                    double i) const;
 
  std::array<double, 7> AsArray() const;
 
  ChoreoTrajectoryState Flipped() const;
 
  units::second_t timestamp{0_s};
 
  units::meter_t x = 0_m;
 
  units::meter_t y = 0_m;
 
  units::radian_t heading = 0_rad;
 
  units::meters_per_second_t velocityX = 0_mps;
 
  units::meters_per_second_t velocityY = 0_mps;
 
  units::radians_per_second_t angularVelocity = 0_rad_per_s;
 
 private:
  static constexpr units::meter_t fieldLength = 16.5410515_m;
};
 
void to_json(wpi::json& json, const ChoreoTrajectoryState& trajState);
void from_json(const wpi::json& json, ChoreoTrajectoryState& trajState);
}  // namespace choreolib