Trajectory.hpp

#pragma once
 
#include <frc/kinematics/ChassisSpeeds.h>
#include <frc/geometry/Pose2d.h>
#include <frc/geometry/Rotation2d.h>
#include <units/time.h>
#include <units/velocity.h>
#include <units/length.h>
 
namespace thunder {
 
enum FieldSymmetry {
  NONE,
  ROTATIONAL,    // Rotated 180 degrees, like 2022, 2025, and 2026.
  REFLECTIONAL,  // Mirrored across the center line, like 2023 and 2024.
};
 
struct FieldDimensions {
  units::meter_t width = 0_m;   // X
  units::meter_t length = 0_m;  // Y
};
 
struct TrajectoryState {
  units::second_t time = 0_s;
  frc::Pose2d pose;
  frc::ChassisSpeeds chassisSpeeds; // Field-centric
  units::meters_per_second_t linearVelocity = 0.0_mps;
  frc::Rotation2d heading;
};
 
class Trajectory {
 public:
  virtual ~Trajectory() = default;
 
  virtual bool isValid() const noexcept = 0;
 
  operator bool() const noexcept { return isValid(); }
 
  virtual TrajectoryState sample(units::second_t time) const noexcept = 0;
 
  virtual units::second_t getDuration() const noexcept = 0;
 
  virtual TrajectoryState getInitialState() const noexcept = 0;
 
  virtual TrajectoryState getFinalState() const noexcept = 0;
 
 protected:
  Trajectory() = default;
};
 
}  // namespace thunder