ChoreoLib
Choreo support library.
Loading...
Searching...
No Matches
DifferentialSample.h
1// Copyright (c) Choreo contributors
2
3#pragma once
4
5#include <type_traits>
6
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>
19
20#include "choreo/util/AllianceFlipperUtil.h"
21
22namespace choreo {
23
28 public:
32 constexpr DifferentialSample() = default;
33
50 constexpr DifferentialSample(units::second_t timestamp, units::meter_t x,
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)
59 x{x},
60 y{y},
62 vl{vl},
63 vr{vr},
64 omega{omega},
65 al{al},
66 ar{ar},
67 fl{fl},
68 fr{fr} {}
69
75 units::second_t GetTimestamp() const { return timestamp; }
76
82 constexpr frc::Pose2d GetPose() const {
83 return frc::Pose2d{x, y, frc::Rotation2d{heading}};
84 }
85
91 constexpr frc::ChassisSpeeds GetChassisSpeeds() const {
92 return frc::ChassisSpeeds{(vl + vr) / 2.0, 0_mps, omega};
93 }
94
101 constexpr DifferentialSample OffsetBy(units::second_t timeStampOffset) const {
102 return DifferentialSample{timestamp + timeStampOffset,
103 x,
104 y,
105 heading,
106 vl,
107 vr,
108 omega,
109 al,
110 ar,
111 fl,
112 fr};
113 }
114
123 units::second_t t) const {
124 units::scalar_t scale = (t - timestamp) / (endValue.timestamp - timestamp);
125 frc::Pose2d interpolatedPose =
126 GetPose().Exp(GetPose().Log(endValue.GetPose()) * scale.value());
127
128 return DifferentialSample{
129 wpi::Lerp(timestamp, endValue.timestamp, scale),
130 interpolatedPose.X(),
131 interpolatedPose.Y(),
132 interpolatedPose.Rotation().Radians(),
133 wpi::Lerp(vl, endValue.vl, scale),
134 wpi::Lerp(vr, endValue.vr, scale),
135 wpi::Lerp(omega, endValue.omega, 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),
140 };
141 }
142
149 template <int Year = util::kDefaultYear>
150 constexpr DifferentialSample Flipped() const {
151 constexpr auto flipper = choreo::util::GetFlipperForYear<Year>();
152 if constexpr (flipper.isMirrored) {
153 return DifferentialSample(timestamp, flipper.FlipX(x), flipper.FlipY(y),
154 flipper.FlipHeading(heading), vr, vl, -omega,
155 ar, al, fr, fl);
156 } else {
157 return DifferentialSample(timestamp, flipper.FlipX(x), flipper.FlipY(y),
158 flipper.FlipHeading(heading), vl, vr, omega, al,
159 ar, fl, fr);
160 }
161 }
162
169 constexpr bool operator==(const DifferentialSample& other) const {
170 constexpr double epsilon = 1e-6;
171
172 auto compare_units = [epsilon](const auto& a, const auto& b) {
173 using UnitType =
174 std::remove_const_t<std::remove_reference_t<decltype(a)>>;
175 return units::math::abs(a - b) < UnitType(epsilon);
176 };
177
178 return compare_units(timestamp, other.timestamp) &&
179 compare_units(x, other.x) && compare_units(y, other.y) &&
180 compare_units(heading, other.heading) &&
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);
185 }
186
188 units::second_t timestamp = 0_s;
189
191 units::meter_t x = 0_m;
192
194 units::meter_t y = 0_m;
195
197 units::radian_t heading = 0_rad;
198
200 units::meters_per_second_t vl = 0_mps;
201
203 units::meters_per_second_t vr = 0_mps;
204
206 units::radians_per_second_t omega = 0_rad_per_s;
207
209 units::meters_per_second_squared_t al = 0_mps_sq;
210
212 units::meters_per_second_squared_t ar = 0_mps_sq;
213
215 units::newton_t fl = 0_N;
216
218 units::newton_t fr = 0_N;
219};
220
221void to_json(wpi::json& json, const DifferentialSample& trajectorySample);
222void from_json(const wpi::json& json, DifferentialSample& trajectorySample);
223
224} // namespace choreo
225
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