7#include <frc/geometry/Pose2d.h>
8#include <frc/kinematics/ChassisSpeeds.h>
9#include <units/acceleration.h>
10#include <units/angle.h>
11#include <units/angular_acceleration.h>
12#include <units/angular_velocity.h>
13#include <units/force.h>
14#include <units/length.h>
15#include <units/time.h>
16#include <units/velocity.h>
17#include <wpi/MathExtras.h>
18#include <wpi/json_fwd.h>
20#include "choreo/util/AllianceFlipperUtil.h"
51 units::meter_t
y, units::radian_t
heading,
52 units::meters_per_second_t
vl,
53 units::meters_per_second_t
vr,
54 units::radians_per_second_t
omega,
55 units::meters_per_second_squared_t
al,
56 units::meters_per_second_squared_t
ar,
57 units::newton_t
fl, units::newton_t
fr)
83 return frc::Pose2d{
x,
y, frc::Rotation2d{
heading}};
92 return frc::ChassisSpeeds{(
vl +
vr) / 2.0, 0_mps,
omega};
123 units::second_t t)
const {
125 frc::Pose2d interpolatedPose =
130 interpolatedPose.X(),
131 interpolatedPose.Y(),
132 interpolatedPose.Rotation().Radians(),
133 wpi::Lerp(
vl, endValue.
vl, scale),
134 wpi::Lerp(
vr, endValue.
vr, scale),
136 wpi::Lerp(
al, endValue.
al, scale),
137 wpi::Lerp(
ar, endValue.
ar, scale),
138 wpi::Lerp(
fl, endValue.
fl, scale),
139 wpi::Lerp(
fr, endValue.
fr, scale),
149 template <
int Year = util::kDefaultYear>
151 constexpr auto flipper = choreo::util::GetFlipperForYear<Year>();
152 if constexpr (flipper.isMirrored) {
170 constexpr double epsilon = 1e-6;
172 auto compare_units = [epsilon](
const auto& a,
const auto& b) {
174 std::remove_const_t<std::remove_reference_t<
decltype(a)>>;
175 return units::math::abs(a - b) < UnitType(epsilon);
179 compare_units(
x, other.
x) && compare_units(
y, other.
y) &&
181 compare_units(
vl, other.
vl) && compare_units(
vr, other.
vr) &&
182 compare_units(
omega, other.
omega) && compare_units(
al, other.
al) &&
183 compare_units(
ar, other.
ar) && compare_units(
fl, other.
fl) &&
184 compare_units(
fr, other.
fr);
191 units::meter_t
x = 0_m;
194 units::meter_t
y = 0_m;
200 units::meters_per_second_t
vl = 0_mps;
203 units::meters_per_second_t
vr = 0_mps;
206 units::radians_per_second_t
omega = 0_rad_per_s;
209 units::meters_per_second_squared_t
al = 0_mps_sq;
212 units::meters_per_second_squared_t
ar = 0_mps_sq;
215 units::newton_t
fl = 0_N;
218 units::newton_t
fr = 0_N;
226#include "choreo/trajectory/struct/DifferentialSampleStruct.h"
Definition DifferentialSample.h:27
units::newton_t fl
The force of the left wheels.
Definition DifferentialSample.h:215
units::meter_t y
The Y position of the sample relative to the blue alliance wall origin.
Definition DifferentialSample.h:194
constexpr bool operator==(const DifferentialSample &other) const
Definition DifferentialSample.h:169
units::second_t timestamp
The timestamp of this sample relative to the beginning of the trajectory.
Definition DifferentialSample.h:188
constexpr DifferentialSample Flipped() const
Definition DifferentialSample.h:150
units::radian_t heading
The heading of the sample, with 0 being in the +X direction.
Definition DifferentialSample.h:197
units::newton_t fr
The force of the right wheels.
Definition DifferentialSample.h:218
constexpr DifferentialSample Interpolate(const DifferentialSample &endValue, units::second_t t) const
Definition DifferentialSample.h:122
units::radians_per_second_t omega
The chassis angular velocity.
Definition DifferentialSample.h:206
units::second_t GetTimestamp() const
Definition DifferentialSample.h:75
units::meters_per_second_t vr
The velocity of the right wheels.
Definition DifferentialSample.h:203
constexpr DifferentialSample()=default
constexpr frc::Pose2d GetPose() const
Definition DifferentialSample.h:82
constexpr frc::ChassisSpeeds GetChassisSpeeds() const
Definition DifferentialSample.h:91
units::meters_per_second_t vl
The velocity of the left wheels.
Definition DifferentialSample.h:200
units::meters_per_second_squared_t al
The acceleration of the left wheels.
Definition DifferentialSample.h:209
constexpr DifferentialSample OffsetBy(units::second_t timeStampOffset) const
Definition DifferentialSample.h:101
constexpr DifferentialSample(units::second_t timestamp, units::meter_t x, units::meter_t y, units::radian_t heading, units::meters_per_second_t vl, units::meters_per_second_t vr, units::radians_per_second_t omega, units::meters_per_second_squared_t al, units::meters_per_second_squared_t ar, units::newton_t fl, units::newton_t fr)
Definition DifferentialSample.h:50
units::meter_t x
The X position of the sample relative to the blue alliance wall origin.
Definition DifferentialSample.h:191
units::meters_per_second_squared_t ar
The acceleration of the right wheels.
Definition DifferentialSample.h:212