9#include <frc/kinematics/ChassisSpeeds.h>
10#include <units/acceleration.h>
11#include <units/angle.h>
12#include <units/angular_acceleration.h>
13#include <units/angular_velocity.h>
14#include <units/force.h>
15#include <units/length.h>
16#include <units/time.h>
17#include <units/velocity.h>
18#include <wpi/MathExtras.h>
19#include <wpi/json_fwd.h>
21#include "choreo/util/AllianceFlipperUtil.h"
55 units::meter_t
y, units::radian_t
heading,
56 units::meters_per_second_t
vx,
57 units::meters_per_second_t
vy,
58 units::radians_per_second_t
omega,
59 units::meters_per_second_squared_t
ax,
60 units::meters_per_second_squared_t
ay,
61 units::radians_per_second_squared_t
alpha,
90 return frc::Pose2d{
x,
y, frc::Rotation2d{
heading}};
108 template <
int Year = util::kDefaultYear>
110 constexpr auto flipper = choreo::util::GetFlipperForYear<Year>();
111 if constexpr (flipper.isMirrored) {
179 units::second_t t)
const {
181 frc::Pose2d interpolatedPose =
184 std::array<units::newton_t, 4> interpolatedForcesX;
185 std::array<units::newton_t, 4> interpolatedForcesY;
186 for (
int i = 0; i < 4; i++) {
187 interpolatedForcesX[i] =
189 interpolatedForcesY[i] =
194 interpolatedPose.X(),
195 interpolatedPose.Y(),
196 interpolatedPose.Rotation().Radians(),
197 wpi::Lerp(
vx, endValue.
vx, scale),
198 wpi::Lerp(
vy, endValue.
vy, scale),
200 wpi::Lerp(
ax, endValue.
ax, scale),
201 wpi::Lerp(
ay, endValue.
ay, scale),
204 interpolatedForcesY};
214 constexpr double epsilon = 1e-6;
216 auto compare_units = [epsilon](
const auto& a,
const auto& b) {
218 std::remove_const_t<std::remove_reference_t<
decltype(a)>>;
219 return units::math::abs(a - b) < UnitType(epsilon);
222 auto compare_arrays = [&compare_units](
const auto& arr1,
const auto& arr2) {
223 return std::equal(arr1.begin(), arr1.end(), arr2.begin(), compare_units);
227 compare_units(
x, other.
x) && compare_units(
y, other.
y) &&
229 compare_units(
vx, other.
vx) && compare_units(
vy, other.
vy) &&
230 compare_units(
omega, other.
omega) && compare_units(
ax, other.
ax) &&
231 compare_units(
ay, other.
ay) && compare_units(
alpha, other.
alpha) &&
240 units::meter_t
x = 0_m;
243 units::meter_t
y = 0_m;
249 units::meters_per_second_t
vx = 0_mps;
252 units::meters_per_second_t
vy = 0_mps;
255 units::radians_per_second_t
omega = 0_rad_per_s;
258 units::meters_per_second_squared_t
ax = 0_mps_sq;
261 units::meters_per_second_squared_t
ay = 0_mps_sq;
264 units::radians_per_second_squared_t
alpha = 0_rad_per_s_sq;
275void to_json(wpi::json& json,
const SwerveSample& trajectorySample);
276void from_json(
const wpi::json& json, SwerveSample& trajectorySample);
280#include "choreo/trajectory/struct/SwerveSampleStruct.h"
Definition SwerveSample.h:28
units::meter_t y
The Y position of the sample relative to the blue alliance wall origin.
Definition SwerveSample.h:243
units::meters_per_second_t vy
The velocity of the sample in the Y direction.
Definition SwerveSample.h:252
constexpr SwerveSample Interpolate(const SwerveSample &endValue, units::second_t t) const
Definition SwerveSample.h:178
constexpr units::second_t GetTimestamp() const
Definition SwerveSample.h:82
units::meters_per_second_t vx
The velocity of the sample in the X direction.
Definition SwerveSample.h:249
units::radians_per_second_t omega
The angular velocity of the sample.
Definition SwerveSample.h:255
units::meters_per_second_squared_t ax
The acceleration of the in the X direction.
Definition SwerveSample.h:258
units::meter_t x
The X position of the sample relative to the blue alliance wall origin.
Definition SwerveSample.h:240
constexpr SwerveSample OffsetBy(units::second_t timeStampOffset) const
Definition SwerveSample.h:156
constexpr bool operator==(const SwerveSample &other) const
Definition SwerveSample.h:213
constexpr frc::ChassisSpeeds GetChassisSpeeds() const
Definition SwerveSample.h:98
constexpr frc::Pose2d GetPose() const
Definition SwerveSample.h:89
constexpr SwerveSample()=default
units::radian_t heading
The heading of the sample, with 0 being in the +X direction.
Definition SwerveSample.h:246
units::second_t timestamp
The timestamp of this sample relative to the beginning of the trajectory.
Definition SwerveSample.h:237
constexpr SwerveSample Flipped() const
Definition SwerveSample.h:109
units::meters_per_second_squared_t ay
The acceleration of the in the Y direction.
Definition SwerveSample.h:261
units::radians_per_second_squared_t alpha
The angular acceleration of the sample.
Definition SwerveSample.h:264
std::array< units::newton_t, 4 > moduleForcesY
Definition SwerveSample.h:272
std::array< units::newton_t, 4 > moduleForcesX
Definition SwerveSample.h:268
constexpr SwerveSample(units::second_t timestamp, units::meter_t x, units::meter_t y, units::radian_t heading, units::meters_per_second_t vx, units::meters_per_second_t vy, units::radians_per_second_t omega, units::meters_per_second_squared_t ax, units::meters_per_second_squared_t ay, units::radians_per_second_squared_t alpha, std::array< units::newton_t, 4 > moduleForcesX, std::array< units::newton_t, 4 > moduleForcesY)
Definition SwerveSample.h:54